[发明专利]一种基于轮速脉冲插值平滑的自动泊车定位方法

说明书

本发明属于自动驾驶技术领域，特别涉及一种自动驾驶泊车系统的实时定位方法。一种基于轮速脉冲插值平滑的自动泊车定位方法，包括以下步骤：S1.采集车身速度，脉冲转速，IMU传感器横摆角速度信号；S2.IMU零漂值计算；S3.实时监测是轮速传感器否已完成第一次脉冲更新，并基于滑窗算法对脉冲轮速传感器数据进行采样；脉冲更新后进入S4.脉冲轮速传感器滑窗平均；S5.基于零漂补偿的IMU横摆角速度计算；S6.基于选择机制的横摆角速度更新；S7.基于横摆角速度的航迹推算；本发明通过对轮速脉冲的插值平滑化及对输出值滤波使得航迹推算得到车辆实时位姿，避免了直接计算所产生的抖动性。同时结合零漂处理后的IMU输出的横摆角速度值使得车辆实时位姿更加精准。

技术领域

本发明属于自动驾驶技术领域，特别涉及一种自动驾驶泊车系统的实时定位方法。

背景技术

自动泊车系统作为自动驾驶的重要组成部分越来越多的受到科研机构及汽车厂商的关注。相比于正常道路驾驶工况，泊车工况往往存在泊车周围场景复杂，车辆运行姿态变化大，车辆行驶速度低的特点。而这些场景特点无一不对车辆的实时精准定位带来了巨大挑战。目前现有的自动泊车系统定位方法中有以下两种：

一种是基于超声波传感器和轮速传感器的航迹推算定位方法。该方法利用超声波传感器确定泊车空间及泊车空间坐标系，使用轮速传感器的数据采用航迹推算算法，定位车辆在泊车空间坐标系中的初始位姿和泊车过程中的车辆位姿。由于受现有轮速传感器齿数的限制，轮速脉冲信号更新往往不够平滑，因此该单纯依靠轮速传感器的轮速计算方法精度较低，实际应用中会导致估计获得的车辆航向和位姿抖动严重。此外，由于泊车工况下车辆多转弯特性且车速较低，进一步放大了轮速传感器齿数不足的痛点，从而导致该方法很难满足该场景下航迹推算精度的要求。

另一种是基于车辆四轮轮速脉冲和IMU惯导数据迭代计算的车辆位姿计算方法。该方法通过计算泊车第一时刻车辆航向定位值，结合车辆运动信息计算得到泊车第二时刻车辆航向定位值的系统矩阵，测量矩阵以及观察量；通过线性卡尔曼滤波算法对其分别进行滤波估算以得到泊车第二时刻车辆航向定位值；将泊车第二时刻车辆航向定位值的车辆航向角与计算得到的泊车第二时刻车辆零漂误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。该方法没有对轮速脉冲进行处理，而是直接使用四轮的轮速脉冲和IMU的输出数据直接迭代计算出车辆的实时位姿。这就会导致航迹推算的车辆位姿产生计算误差，同时再加上积分计算的累计误差会严重影响到车辆的实时定位。

发明内容

本发明的目的是：针对现有自动泊车定位方法的不足，提出一种基于轮速脉冲插值平滑的自动泊车定位方法。

本发明的技术方案是：一种基于轮速脉冲插值平滑的自动泊车定位方法，包括以下步骤：

S1.采集车辆左后轮脉冲转速信号ω_L(i)、右后轮脉冲转速信号ω_R(i)，车身速度信号v(i)，IMU传感器横摆角速度信号ω_IMU(i)；其中，i为信号采集的帧数；

S2.对IMU传感器横摆角速度零漂值初始化计算：

其中，b为IMU传感器横摆角速度初始化时序长度，此时车速v(i)为0；

S3.实时监测是轮速传感器否已完成第一次脉冲更新，并基于滑窗算法对脉冲轮速传感器数据进行采样：

其中，Ω_R和Ω_L分别为右侧和左侧脉冲轮速传感器滑窗采样数据，V为车身速度滑窗采样数据，w为滑窗宽度；当轮速传感器完成第一次脉冲更新后进入S4；

S4根据滑窗机制采集的车身速度信号对脉冲转速信号插值平滑处理，