[发明专利]一种自动泊车定位系统的航迹推算方法

说明书

本发明涉及一种自动泊车定位系统的航迹推算方法，包括步骤：计算泊车第一时刻车辆航向定位值；通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量；通过线性卡尔曼滤波算法对其分别进行滤波估算以得到泊车第二时刻车辆航向定位值；将泊车第二时刻车辆航向定位值的车辆航向角与计算得到的泊车第二时刻车辆零飘误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。本发明以航迹推算的结果作为测量量结合车辆的横摆角速度及车辆后轴中心的速度等参数采用线性卡尔曼滤波算法对其进行滤波估算以得到车辆实时定位的航迹推算值，然后消除零飘误差，得到精准实时航向定位值。

技术领域

本发明涉及泊车技术领域，特别是涉及一种自动泊车定位系统的航迹推算方法。

背景技术

随着汽车行业的蓬勃发展，自动泊车系统目前已经应用到各大汽车厂商的高端配置车辆上，自动泊车过程分四步进行，首先，对车位进行检测，车位检测过程中需要定位跟踪车辆位置，航迹推算系统触发，当得到可以停靠的车位后，车辆继续往前走，车位需要根据之前的位置信息跟当前车辆的位置信息跟踪车位坐标；第二步，将车位坐标信息存储到控制器ECU中，控制器ECU获取车辆信息并建立车辆坐标系；第三步，控制器ECU根据车辆的位置信息、坐标信息、雷达信息等条件规划目标泊车轨迹；最后，泊车控制系统通过实时的航迹跟踪推算，根据目标挡位、目标转角、目标速度、目标加速度以及目标位置输出控制车身自动泊车，ECU控制单元完成整个自动泊车过程。

在自动泊车过程中，泊车航迹推算主要通过轮速脉冲信号，轮速，以及车身参数来匹配运算。采用车轮脉冲信号进行航迹推算来定位，其通过单位时间内左右轮运动行程来计算车身方位角增量和后轴中心位置的增量，分别对车身方位角和位置的增量进行累加，可得出车辆相对于起始位置的姿态。

航迹推算算法适用于短行程车辆位姿定位，但该方法存在一些误差来源。例如：航迹推算定位误差和非系统误差，航迹推算定位误差包括，车轮有效滚动半径的测量误差、两侧车轮的滚动半径不等、轮距测量值与真实值存在误差及码盘的分辨率不高等；非系统误差包括，路面不平、车轮打滑、溜车现象、轮胎载荷变化等，从而导致车辆位姿定位存在一定误差，进而航迹推算精度不高。

发明内容

本发明为克服上述现有技术所述的不足，提供一种自动泊车定位系统的航迹推算方法。

为解决上述技术问题，本发明的技术方案如下：

一种自动泊车定位系统的航迹推算方法，包括如下步骤：

计算泊车第一时刻车辆航向定位值；

通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量；

通过线性卡尔曼滤波算法对泊车第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量分别进行滤波估算以得到泊车第二时刻车辆航向定位值；

通过车身偏航率传感器值计算泊车第二时刻车辆零飘误差值；

将泊车第二时刻车辆航向定位值的车辆航向角与泊车第二时刻车辆零飘误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。

进一步的，作为优选技术方案，第二时刻车辆航向定位值的系统矩阵、测量矩阵以及观察量的计算具体包括：

通过泊车第一时刻车辆航向定位值结合车辆运动信息计算得到泊车第一时刻车辆航向定位值的系统状态向量和测量向量；

通过泊车第一时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统状态向量，通过第二时刻车辆航向定位值的系统状态向量计算得到泊车第二时刻车辆航向定位值的系统矩阵；