[0001] 本发明涉及汽车定位技术领域，尤其是涉及一种基于实时滑移率估计与补偿的组合定位方法及其系统。

[0002] 智能车辆的运动状态，包括位置、速度、姿态，是实现智能驾驶至关重要的信息。轮速传感器(Wheel Speed Sensor，WSS)是提高全球导航卫星系统(Global Navigation Satellite System，GNSS)/惯性导航系统(Inertial Navigation System，INS)集成系统在GNSS中断期间定位精度的有吸引力的方案。然而，车辆高动态行驶条件下，车轮的纵向滑移导致定位效果不佳。

[0003] 大多数研究将测量的轮速信息直接当做量测信息，而没有考虑车辆滑移时轮速vWSS与实际车速vvehicle之间存在的误差。尤其是在强驱动/强制动等大动态工况下，存在较大的误差。

[0004] 公开号为CN113985466A的发明专利中公开了一种基于模式识别的组合导航方法及系统，该方法根据捷联惯导更新算法对车辆运动状态进行更新；实时检测车辆的全球导航卫星系统信号，若检测到车辆的全球导航卫星系统信号，则根据所述车辆的全球导航卫星系统信号、控制器局域网络车速数据和惯性测量单元输出的三轴角速度选择组合导航模型和观测量，并根据所述选择的组合导航模型和观测量采用卡尔曼滤波对所述车辆运动状态进行预测和更新。但该方法并未考虑车辆滑移时轮速与实际车速之间存在的误差，未考虑滑移率对定位的影响，使得定位仍不够精准。

[0060] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。

[0061] 实施例

[0062] 本实施例中，为了获得高精度的车辆状态估计，尤其是在全球导航卫星系统(Global Navigation Satellite System，GNSS)中断期间，应用如图1所示的方法，该方法以惯性导航系统(Inertial Navigation System，INS)、轮速传感器(Wheel Speed Sensor，WSS)、GNSS的测量为输入，基于误差状态卡尔曼滤波实现状态的最优估计。首先，利用惯性测量单元(Inertial Measurement Unit，IMU)的测量，基于INS算法实现状态的递推估计。随后，在GNSS可用期间，使用高精度的GNSS测量进行状态更新。而在更关键的GNSS中断期间，使用车辆的轮速信息进行状态更新；为了提高在强驱动/制动工况时轮速量测的精度，创新性的提出了滑移率估计和补偿方法。显著的提升了轮速信息的准确性，进而提升了车辆的定位精度。

[0063] 本实施例中，所应用的坐标系如下：(·)n代表导航坐标系，为n系，也称为ENU(东‑b北‑上)。原点与IMU传感器帧的中心重合。车身坐标系用(·) 表示，为b系，其原点也与IMUv
的中心重合。(·) 表示车辆坐标系，其原点位于车辆后轴中心。

[0064] 本实施例中，应用基于实时滑移率估计与补偿的组合定位方法对车辆进行定位。

[0065] 本实施例中，在时刻k，本文估计的车辆状态Xk包括：位置误差δpk(经度、纬度和高度)，速度误差 姿态误差 加速度计零偏 和陀螺仪的零偏

[0066]

[0067] 本实施例中，通过误差状态卡尔曼滤波器融合了INS预测状态和测量结果。卡尔曼滤波器的状态方程可表示为：

[0068]

[0069] 其中，F是状态转换矩阵；wk代表噪声矩阵； 为k‑1时刻预测k时刻的状态；为k‑1时刻最优状态。

[0070] 本实施例中，在GNSS可用期间，以GNSS的位置信息和速度信息作为量测信息；在GNSS中断期间，进行轮速的滑移率修正补偿，得到补偿后的轮速信息作为量测信息；

[0071] 大多数研究将测量的轮速信息直接当做量测，而没有考虑车辆滑移时轮速vWSS与实际车速vvehicle之间存在的误差。尤其是在强驱动/强制动等大动态工况下，存在较大的误差。

[0072] 本实施例中，提出了实时滑移率估计及补偿算法，以提供更准确的轮速观测。

[0073] 本实施例中，利用轮速传感器提供车辆的纵向速度量测，因此在车辆坐标系下得T到轮速信息为vWSS＝[0vWSS0]。

[0074] 本实施例中，设计卡尔曼滤波器对其进行估计。在t时刻对横摆角速度进行多项式泰勒展开，忽略二阶以上的高阶动态，则有：

[0075]

[0076] 其中，ωbz为横摆角速度； 为横摆角速度的一阶导数； 为横摆角速度的二阶导数；Δt为k时刻与k‑1时刻之间的时间间隔；ξ1，ξ2和ξ3为各阶项的随机噪声。

[0077] 本实施例中，基于上式构建的状态方程如下：

[0078]

[0079] 本实施例中，量测信息为ωbz。因此，观测方程定义如下：

[0080]

[0081] 其中，ωbz为横摆角速度； 为横摆角速度的一阶导数； 为横摆角速度的二阶导数。

[0082] 本实施例中，带入已知量，即可求取得到横摆角速度的一阶导数

[0083] 本实施例中，进行滑移率估计时，需要该车轮处的纵向加速度ay,k‑1信息。在车辆直线行驶时，由于横摆角速度较小，车身整体可认为是刚体，各个部分的纵向速度相同。此时左右车轮处的纵向加速度均使用IMU处的纵向加速度信息ab,y。但是，在转弯工况下，由于左右车轮的速度显著不同，需要估计各车轮处的纵向加速度。

[0084] 本实施例中，IMU处纵向速度与左车轮处纵向速度存在如下的转换关系：

[0085] vRL,y＝vb,y‑BLωbz；

[0086] 本实施例中，IMU处纵向速度与右车轮处纵向速度存在如下的转换关系：

[0087] vRR,y＝vb,y+BRωbz；

[0088] 本实施例中，对上式求导，可以分别得到左右车轮处纵向加速度估计公式：

[0089]

[0090] 其中，BL和BR分别为IMU到左右车轮的轴距；ab,y为IMU测量的纵向加速度；ωbz为IMU测量的横摆角速度； 为IMU处的横摆角速度的一阶导数。

[0091] 本实施例中，车辆驱动时，某一车轮的滑移率为：

[0092]

[0093] 本实施例中，车辆制动时，某一车轮的滑移率为：

[0094]

[0095] 其中，vvehicle为实际车速；vWSS为轮速信息。

[0096] 结合运动学公式vvehicle,k＝vvehicle,k‑1+ay,kΔt,我们可以推导得到驱动加速情况下的滑移率递推公式：

[0097]

[0098] 本实施例中，制动情况下的滑移率递推公式：

[0099]

[0100] 其中，vWSS,k‑1为k‑1时刻轮速；vWSS,k为k时刻轮速；ay,k‑1为k‑1时刻纵向加速度；Δt为k时刻与k‑1时刻之间的时间间隔。

[0101] 本实施例中，基于估计的滑移率对轮速信息进行补偿修正：

[0102] 本实施例中，车辆驱动加速时，

[0103]

[0104] 本实施例中，车辆制动时，

[0105]

[0106] 其中， 是经过滑移率补偿后的轮速信息；sa为车辆驱动时车轮滑移率；sb为车辆制动时车轮滑移率。

[0107] 本实施例中，利用量测信息，采用误差状态卡尔曼滤波器进行量测更新，以实现对车辆的定位。

[0108] 本实施例中，卡尔曼滤波的测量方程根据量测信息改变。

[0109] 本实施例中，针对在GNSS可用期间的量测信息，卡尔曼滤波器处理量测信息的测量方程具体为：

[0110]

[0111] 其中，ηk,GNSS为GNSS的测量噪声，Zk,GNSS为k时刻GNSS的观测信息；PINS为来自INS的预测位置状态；PGNSS为来自GNSS的位置测量信息； 为来自INS的预测速度状态； 为来自GNSS的速度测量信息；Hk,GNSS为k时刻GNSS信息的测量矩阵。

[0112] 本实施例中，针对在GNSS中断期间的量测信息，卡尔曼滤波器处理量测信息的测量方程具体为：

[0113]

[0114] Hk,WSS＝[03×3I3×303×9]；

[0115]

[0116] 其中，ηk,WSS为WSS的测量噪声；Zk,WSS为k时刻WSS的观测信息； 为来自WSS的速度测量信息；Hk,WSS为k时刻WSS信息的测量矩阵； 表示b系到n系的旋转矩阵，这是由INS系统得到的。 是v系到b系的旋转矩阵，是预先标定得到的外参矩阵。 是经过滑移率补偿后的轮速信息，

[0117] 本实施例中，通过公式分析不难看出，在强驱动或强制动等大动态工况下，由于滑移率较大，导致轮速信息与实际车速差值较大。若不考虑这一问题，会导致较大的定位误差。而通过本实施例所应用的滑移率估计及补偿方法，可以很好的解决这一问题。

[0118] 在本实施例中，在实际情况下对应用的一种基于实时滑移率估计与补偿的组合定位方法进行验证。

[0119] 本实施例中，验证实验利用如图2所示的后驱车进行。车辆配备的传感器包括：MEMS‑IMU ASM330LHH提供三个轴的加速度和角速度测量值；GNSS接收器Novatel 718D提供RTK GNSS测量信息；GNSS信号良好精度可达厘米级。

[0120] 本实施例中，车轮速度数据是从车辆的控制器区域网络总线接口获取。此外，高精度的INS/GNSS组合定位系统Novatel SPAN KVH 1750可提供高精度车辆状态作为真值。

[0121] 本实施例中，选取同济大学嘉定校区作为实验地点，实验包括直线行驶时的制动工况，转弯行驶时的驱动和制动工况。均在开阔场景进行，GNSS一直处于实时动态测量技术(Real Time Kinematic，RTK)状态。

[0122] 本实施例中，通过人为切断GNSS信号来模拟GNSS中断情况，以验证考虑滑移率补偿的定位算法的有效性。

[0123] 本实施例中，直线行驶工况实验时的车辆轨迹如图4所示，蓝色为真值的实验轨迹图，红色为未补偿时的实验轨迹图，橙色为补偿滑移率后的实验轨迹图。实验经历了静止，加速和制动的过程。我们从50s开始切断GNSS信号来模拟GNSS中断。此时车辆处于制动过2
程，最大制动加速度可达5m/s，这意味着车轮会出现滑移状态。

[0124] 本实施例中，直线行驶工况时的车辆速度如图3所示；纵向加速度估计结果如图5所示；蓝色为IMU所采集得到的纵向加速度，红色为Novatel SPAN KVH 1750所采集得到的纵向加速度，橙色为通过本实施例中方法所得到的纵向加速度估计结果；直线行驶工况时的左后、右后轮滑移率估计结果如图6a、6b所示，蓝色线条为参考真值，红色线条为本方法所递推计算得到的值，可以看出，滑移率估计精度高；水平位置误差如图7所示，蓝色线条为未补偿滑移率时水平位置误差，红色线条为补偿滑移率后水平位置误差，不难看出，加入滑移率补偿后，定位精度提升显著。

[0125] 本实施例中，转弯工况实验时的车辆轨迹如图9所示，蓝色为真值的实验轨迹图，红色为未补偿时的实验轨迹图，橙色为补偿滑移率后的实验轨迹图。实验从车辆转弯处80s开始切断GNSS信号来模拟GNSS中断。此时车辆先后经历了激烈的驱动和制动过程，这意味着车轮会出现滑移状态。

[0126] 本实施例中，转弯工况时的车辆速度如图8所示；纵向加速度估计结果如图10所示；蓝色为IMU所采集得到的纵向加速度，红色为Novatel SPAN KVH 1750所采集得到的纵向加速度，橙色为通过本实施例中方法所得到的纵向加速度估计结果；直线行驶工况时的左后、右后轮滑移率估计结果如图11a、11b所示，蓝色线条为参考真值，红色线条为本方法所递推计算得到的值，可以看出，滑移率估计精度高；水平位置误差如图12所示，蓝色线条为未补偿滑移率时水平位置误差，红色线条为补偿滑移率后水平位置误差，不难看出，加入滑移率补偿后，定位精度提升显著。

[0127] 以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。