一种自动泊车定位系统的航迹推算方法

摘要

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

说明书

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

通过泊车第一时刻车辆航向定位值的测量向量计算得到泊车第二时刻车辆航向定位值的观察量，通过第二时刻车辆航向定位值的观察量计算得到泊车第二时刻车辆航向定位值的测量矩阵。

进一步的，作为优选技术方案，所述车辆运动信息包括车辆的横摆角速度和车辆后轴中心的速度；

所述泊车第一时刻车辆航向定位值的系统状态向量和测量向量的计算具体包括：

泊车第一时刻车辆航向定位值分别结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量和测量向量。

进一步的，作为优选技术方案，所述车辆运动信息包括车辆的横摆角速度、车辆后轴中心的速度、车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角；

所述泊车第一时刻车辆航向定位值的系统状态向量和测量向量的计算具体包括：

泊车第一时刻车辆航向定位值结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量；

泊车第一时刻车辆航向定位值结合车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角计算得到泊车第一时刻车辆航向定位值的测量向量。

进一步的，作为优选技术方案，泊车第二时刻车辆航向定位值的系统矩阵通过以下公式计算：

所述车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角；

假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量为[x，y，θ，w，v]T ，通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量 ；

；

对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的系统矩阵；

；

其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；

泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：

假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量向量为[w，v]T，则通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量：

；

以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为 ：

；

其中，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻。

进一步的，作为优选技术方案，泊车第二时刻车辆航向定位值的系统矩阵通过以下公式计算：

所述车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角；

假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量为[x，y，θ，w，v]T ，通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量 ；

；

对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的系统矩阵；

；

其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；

泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：

假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量向量为[w，v]T，则通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量：

；

以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为：

；

其中，和为车辆两个后轮的轮速， 为后轴中心点转弯半径，为方向盘转角，t为泊车第一时刻，t+1为泊车第二时刻。

进一步的，作为优选技术方案，泊车第二时刻车辆航向定位值的计算具体包括：

分别采用线性卡尔曼滤波算法通过以下公式对泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角的系统矩阵、测量矩阵以及观察量进行滤波计算，以得到泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角；

线性卡尔曼滤波算法计算方程为：

；

+；

其中，为t+1时刻的系统状态向量值估计值，为t+1时刻协方差矩阵的估计值，为t+1时刻的系统噪声协方差矩阵，是对X求一阶导数后的雅克比矩阵，即t+1时刻的系统矩阵，为系统噪声。

进一步的，作为优选技术方案，线性卡尔曼滤波算法计算方程为：

；

；

；

其中，是h对X求一阶偏导数后的雅克比矩阵，即t+1时刻的测量矩阵，为t+1时刻的观察量。

进一步的，作为优选技术方案，泊车第二时刻车辆零飘误差值的计算具体包括：

以固定频率采集固定时间内的车身偏航率传感器值；

对采集的多个车身偏航率传感器值进行均值滤波计算，以得到第二时刻车辆零飘误差值。

一种自动泊车定位系统，包括航迹推算模块，所述航迹推算模块采用航迹推算方法进行车辆的航迹推算。

与现有技术相比，本发明技术方案的有益效果是：

本发明以航迹推算的结果作为测量量结合车辆的横摆角速度及车辆后轴中心的速度等参数采用线性卡尔曼滤波算法对其进行滤波估算以得到车辆实时定位的航迹推算值，大幅度提高了航迹定位精准度；

同时，由于本发明中所采用各车辆运动信息基于传感器采集，而传感器的信号中包含噪声，对算法计算结果带来很大影响，因此通过线性卡尔曼滤波算法进行滤波，以消弱噪声对估算结果的影响，进而提高自动泊车定位的精确度；

最后，增加对车辆自动泊车系统零飘误差值的计算，以消除车辆固有误差，以提高车辆航向角精度，从而得到精准实时航向定位值。

附图说明

图1为本发明方法步骤流程图。

图2为本发明结构框图。

图3为本发明坐标示意图。

附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的；相同或相似的标号对应相同或相似的部件；附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制。

具体实施方式

下面结合附图对本发明的较佳实施例进行详细阐述，以使本发明的优点和特征更易被本领域技术人员理解，从而对本发明的保护范围作出更为清楚的界定。

实施例1

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

S10. 计算泊车第一时刻车辆航向定位值。

在本步骤中，泊车第一时刻车辆航向定位值包括泊车第一时刻车辆横坐标、纵坐标以及车辆航向角。

在本发明中，泊车第一时刻车辆航向定位值通过车轮脉冲信号结合车辆自身参数推到计算，而通过车轮脉冲信号结合车辆自身参数对车辆的运动姿态进行推到计算属于现有技术，在此不在进行赘述。

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

本步骤具体包括：

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

在本步骤中，当车辆运动信息包括车辆的横摆角速度和车辆后轴中心的速度时，泊车第一时刻车辆航向定位值的系统状态向量和测量向量的计算具体包括：

泊车第一时刻车辆航向定位值分别结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量和测量向量。

当车辆运动信息还包括车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角时，泊车第一时刻车辆航向定位值的系统状态向量和测量向量的计算具体包括：

泊车第一时刻车辆航向定位值结合车辆的横摆角速度及车辆后轴中心的速度计算得到泊车第一时刻车辆航向定位值的系统状态向量；泊车第一时刻车辆航向定位值结合车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角计算得到泊车第一时刻车辆航向定位值的测量向量。

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

S203. 通过泊车第一时刻车辆航向定位值的测量向量计算得到泊车第二时刻车辆航向定位值的观察量，通过第二时刻车辆航向定位值的观察量计算得到泊车第二时刻车辆航向定位值的测量矩阵。

由于车辆航向定位值包括车辆横坐标、纵坐标以及车辆航向角。因此，在本步骤中，泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的系统矩阵均通过以下公式计算：

假设车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量为[x，y，θ，w，v]T，采用第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量通过以下公式计算泊车第二时刻车辆航向定位值的系统状态向量；

；

对泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的系统状态向量进行增量及线性化计算得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的系统矩阵；

即，对时刻系统状态进行增量计算，如下公式：

；

由车辆运动学模型公式可知，上式微分方程写成差分形式：

；

对上式线性化，得到泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的的系统矩阵：

；

其中，x，y，θ为泊车第一时刻车辆航向定位值，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻；

泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标以及车辆航向角的观察量和测量矩阵通过以下公式计算：

假设车辆航向定位值的测量向量为[w，v]T，采用第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量 ：

；

以泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量为基础通过以下公式计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的测量矩阵为：

；

其中，w为车辆的横摆角速度，v为车辆后轴中心的速度，t为泊车第一时刻，t+1为泊车第二时刻。

在上述计算过程中，计算泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量和测量矩阵车辆的横摆角速度和车辆后轴中心的速度。

而当车辆运动信息包括车辆两个后轮轮速、后轴中心点转弯半径及方向盘转角时，如图2-3所示：

；

；

因此，泊车第一时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量通过以下公式计算：

；

泊车第二时刻车辆航向定位值中的车辆横坐标、纵坐标或车辆航向角的观察量通过以下公式计算：

；

其中，和为车辆两个后轮的轮速，为后轴中心点转弯半径，为方向盘转角，t为泊车第一时刻，t+1为泊车第二时刻。

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

本步骤具体包括：

分别采用线性卡尔曼滤波算法通过以下公式对泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角的系统矩阵、测量矩阵以及观察量进行滤波计算，以得到泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角；

线性卡尔曼滤波算法计算方程为：

；

+；

其中，为t+1时刻的系统状态向量值估计值，为t+1时刻协方差矩阵的估计值，为t+1时刻的系统噪声协方差矩阵，是对X求一阶导数后的雅克比矩阵，即t+1时刻的系统矩阵，为系统噪声。

进一步的，线性卡尔曼滤波算法计算方程为：

；

；

；

其中，是h对X求一阶偏导数后的雅克比矩阵，即t+1时刻的测量矩阵，为t+1时刻的观察量。

通过本步骤的滤波计算，得到t+1时刻的系统状态向量值，即分别得到泊车第二时刻车辆航向定位值的车辆横坐标、纵坐标以及车辆航向角。

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

本步骤具体为：

以固定频率采集固定时间内的车身偏航率传感器值；

对采集的多个车身偏航率传感器值进行均值滤波计算，以得到第二时刻车辆零飘误差值。

在本步骤中，采集频率为20 ms每次，固定时间为1s，因此，本步骤具体为：气动泊车系统后，在车辆静止过程中重复以20 ms的频率采集1s以内的车身偏航率传感器值，共采样50个值，对采集的50个值进行均值滤波计算，以得到第二时刻车辆零飘误差值。

计算公式如下：

；

；

其中，YawRate 为车身偏航率传感器值，累计叠加50次，Yaw 为车身均值滤波后整车偏航率传感器值，即零飘误差值。

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

即，得到泊车第二时刻确定车辆位置信息（X，Y，θ）。

本步骤增加对车辆自动泊车系统零飘误差值的计算，以消除车辆固有误差，以提高车辆航向角精度，从而得到精准实时航向定位值。

实施例2

一种自动泊车定位系统，包括航迹推算模块，所述航迹推算模块采用实施例1所述的航迹推算方法进行车辆的航迹推算。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。