一种基于轮速脉冲插值平滑的自动泊车定位方法与流程

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


背景技术：

2.自动泊车系统作为自动驾驶的重要组成部分越来越多的受到科研机构及汽车厂商的关注。相比于正常道路驾驶工况，泊车工况往往存在泊车周围场景复杂，车辆运行姿态变化大，车辆行驶速度低的特点。而这些场景特点无一不对车辆的实时精准定位带来了巨大挑战。目前现有的自动泊车系统定位方法中有以下两种：
3.一种是基于超声波传感器和轮速传感器的航迹推算定位方法。该方法利用超声波传感器确定泊车空间及泊车空间坐标系，使用轮速传感器的数据采用航迹推算算法，定位车辆在泊车空间坐标系中的初始位姿和泊车过程中的车辆位姿。由于受现有轮速传感器齿数的限制，轮速脉冲信号更新往往不够平滑，因此该单纯依靠轮速传感器的轮速计算方法精度较低，实际应用中会导致估计获得的车辆航向和位姿抖动严重。此外，由于泊车工况下车辆多转弯特性且车速较低，进一步放大了轮速传感器齿数不足的痛点，从而导致该方法很难满足该场景下航迹推算精度的要求。
4.另一种是基于车辆四轮轮速脉冲和imu惯导数据迭代计算的车辆位姿计算方法。该方法通过计算泊车第一时刻车辆航向定位值，结合车辆运动信息计算得到泊车第二时刻车辆航向定位值的系统矩阵，测量矩阵以及观察量；通过线性卡尔曼滤波算法对其分别进行滤波估算以得到泊车第二时刻车辆航向定位值；将泊车第二时刻车辆航向定位值的车辆航向角与计算得到的泊车第二时刻车辆零漂误差值进行相减运算，以得到泊车第二时刻确定车辆航向定位值。该方法没有对轮速脉冲进行处理，而是直接使用四轮的轮速脉冲和imu的输出数据直接迭代计算出车辆的实时位姿。这就会导致航迹推算的车辆位姿产生计算误差，同时再加上积分计算的累计误差会严重影响到车辆的实时定位。


技术实现要素：

5.本发明的目的是：针对现有自动泊车定位方法的不足，提出一种基于轮速脉冲插值平滑的自动泊车定位方法。
6.本发明的技术方案是：一种基于轮速脉冲插值平滑的自动泊车定位方法，包括以下步骤：
7.s1.采集车辆左后轮脉冲转速信号ω
l
(i)、右后轮脉冲转速信号ωr(i)，车身速度信号v(i)，imu传感器横摆角速度信号ω
imu
(i)；其中，i为信号采集的帧数；
8.s2.对imu传感器横摆角速度零漂值初始化计算：
9.10.其中，b为imu传感器横摆角速度初始化时序长度，此时车速v(i)为0；
11.s3.实时监测是轮速传感器否已完成第一次脉冲更新，并基于滑窗算法对脉冲轮速传感器数据进行采样：
[0012][0013][0014][0015]
其中，ωr和ω
l
分别为右侧和左侧脉冲轮速传感器滑窗采样数据，v为车身速度滑窗采样数据，w为滑窗宽度；当轮速传感器完成第一次脉冲更新后进入s4；
[0016]
s4根据滑窗机制采集的车身速度信号对脉冲转速信号插值平滑处理，
[0017][0018][0019]
s5.基于零漂值，实时对imu传感器横摆角速度进行补偿获得基于imu的车辆横摆角速度估计值
[0020][0021]
s6.对左后和右后轮脉冲转速信号状态进行观测，从而对横摆角速度估计值进行选择更新，最终确定实时车身横摆角速度值θ
′
(i)，
[0022][0023]
其中，l为后轴轮距；
[0024]
s7.基于横摆角速度θ
′
(i)进行航迹推算。
[0025]
有益效果：本发明通过对轮速脉冲的插值平滑化及对输出值滤波使得航迹推算得到车辆实时位姿，避免了直接计算所产生的抖动性。同时结合零漂处理后的imu输出的横摆角速度值使得车辆实时位姿更加精准。
附图说明
[0026]
图1为本发明的流程图。
具体实施方式
[0027]
实施例1，参见附图1，一种基于轮速脉冲插值平滑的自动泊车定位方法，包括以下步骤：
[0028]
s1.采集车辆左后轮脉冲转速信号ω
l
(i)、右后轮脉冲转速信号ωr(i)，车身速度信号v(i)，imu传感器横摆角速度信号ω
imu
(i)；其中，i为信号采集的帧数；
[0029]
s2.对imu传感器横摆角速度零漂值初始化计算：
[0030][0031]
其中，b为imu传感器横摆角速度初始化时序长度，此时车速v(i)为0；本例中，imu传感器横摆角速度初始化时序长度b＝100帧；在车辆速度为0时，取100帧输入的imu横摆角速度，计算取平均值，作为该次测试时的零漂值；
[0032]
s3.实时监测是轮速传感器否已完成第一次脉冲更新，并基于滑窗算法对脉冲轮速传感器数据进行采样：
[0033][0034][0035][0036]
其中，ωr和ω
l
分别为右侧和左侧脉冲轮速传感器滑窗采样数据，v为车身速度滑窗采样数据，w为滑窗宽度；本例中，滑窗宽度w＝20帧，前20帧数据用初始值，20帧以后采用滑窗的方式更新值；当轮速传感器完成第一次脉冲更新后进入s4；
[0037]
s4根据滑窗机制采集的车身速度信号对脉冲转速信号插值平滑处理，
[0038][0039][0040]
s5.基于零漂值，实时对imu传感器横摆角速度进行补偿获得基于imu的车辆横摆角速度估计值
[0041][0042]
s6.对左后和右后轮脉冲转速信号状态进行观测，从而对横摆角速度估计值进行选择更新，最终确定实时车身横摆角速度值θ
′
(i)，
[0043][0044]
其中，l为后轴轮距；
[0045]
s7.基于横摆角速度θ
′
(i)进行航迹推算：
[0046][0047][0048]
其中，x
′
(i)，y
′
(i)分别为车辆的当前实时横纵坐标的更新值，r为车轮滚动半径，θ为车身横摆角。
[0049]
虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。


技术特征：
1.一种基于轮速脉冲插值平滑的自动泊车定位方法，其特征在于，包括以下步骤：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根据滑窗机制采集的车身速度信号对脉冲转速信号插值平滑处理，s4根据滑窗机制采集的车身速度信号对脉冲转速信号插值平滑处理，s5.基于零漂值，实时对imu传感器横摆角速度进行补偿获得基于imu的车辆横摆角速度估计值度估计值s6.对左后和右后轮脉冲转速信号状态进行观测，从而对横摆角速度估计值进行选择更新，最终确定实时车身横摆角速度值θ
′
(i)，其中，l为后轴轮距；s7.基于横摆角速度θ
′
(i)进行航迹推算。
2.如权利要求1所述的一种基于轮速脉冲插值平滑的自动泊车定位方法，其特征在于，s2中，imu传感器横摆角速度初始化时序长度b＝100帧。3.如权利要求1所述的一种基于轮速脉冲插值平滑的自动泊车定位方法，其特征在于，s3中，滑窗宽度w＝20帧。4.如权利要求1所述的一种基于轮速脉冲插值平滑的自动泊车定位方法，其特征在于，s7中，航迹推算的方法为：s7中，航迹推算的方法为：其中，x
′
(i)，y
′
(i)分别为车辆的当前实时横纵坐标的更新值，r为车轮滚动半径，θ为车身横摆角。

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


技术研发人员：秦明玉 于宏啸 夏天
受保护的技术使用者：杭州流马锐驰科技有限公司
技术研发日：2022.02.25
技术公布日：2022/5/6