CN110095793A - 一种基于轮胎半径自适应的自动驾驶低速清扫车定位方法 - Google Patents

Abstract

本发明涉及一种基于轮胎半径自适应的自动驾驶低速清扫车定位方法，包括以下步骤：1)根据GNSS状态判断子模块的GNSS状态的标志位，车辆航向角估计模块计算获得当前航向角ψ_k；2)轮胎半径估计模块采用卡尔曼滤波对车轮半径进行估计，得到轮胎半径估计值r；3)位置估计模块根据轮胎半径估计值r、轮速传感器测量得到的轮速信号ω以及传感器信号处理模块得到的当前航向角ψ_k对车辆位置进行在线实时估计，最终得到车辆位置估计值。与现有技术相比，本发明具有成本低廉易实现、位置估计准确、鲁棒性强等优点。

Description

一种基于轮胎半径自适应的自动驾驶低速清扫车定位方法

技术领域

本发明涉及自动驾驶汽车定位领域，尤其是涉及一种基于轮胎半径自适应的自动驾驶低速清扫车定位方法。

背景技术

车辆定位信息作为自动驾驶车辆环境感知技术的重要组成部分，是底层车辆运动控制的输入，因此是自动驾驶车辆稳定行驶的基础。城市自动驾驶清扫车的工作区域多为路边树木密集区域或楼宇集中区域，GNSS信号质量较差，测量值往往会出现错误或伴随有很大的噪声，GNSS测量得到的信息不可直接应用于自动驾驶车辆的控制，因此如何在GNSS信号较差或无GNSS信号区域估计车辆的位置成为近年来的研究热点和难点。

目前国内外对于无GNSS信号情况下的车辆位置估计方法主要有以下几种：

1、基于IMU信息进行惯性导航，其对于IMU的精度要求很高，且随着时间的累积速度和位置误差都会很快发散。

2、基于轮速测量计算车速反馈进行惯性导航，此种方法对于轮胎半径的精度要求较高，而轮胎半径受车速、温度、胎压、载荷、磨损等影响较大，固定的轮胎半径参数会使得车速计算产生偏差从而影响定位精度。

3、基于激光雷达、摄像头等传感器和地图信息进行匹配估计车辆位置，此方法所需传感器价格昂贵且需要地图信息，不便于应用。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于轮胎半径自适应的自动驾驶低速清扫车定位方法。

本发明的目的可以通过以下技术方案来实现：

一种基于轮胎半径自适应的自动驾驶低速清扫车定位方法，包括以下步骤：

1)根据GNSS状态判断子模块的GNSS状态的标志位，车辆航向角估计模块计算获得当前航向角ψ_k；

2)轮胎半径估计模块采用卡尔曼滤波对车轮半径进行估计，得到轮胎半径估计值r；

3)位置估计模块根据轮胎半径估计值r、轮速传感器测量得到的轮速信号ω以及传感器信号处理模块得到的当前航向角ψ_k对车辆位置进行在线实时估计，最终得到车辆位置估计值。

所述的步骤1)中，当前航向角ψ_k的计算式为：

其中，ψ_k为车辆航向角估计模块计算得到的当前航向角，ψ_k-1为上一采样时刻的车辆航向角，ψ_G为由GNSS测量得到的车辆航向角，γ为由IMU测量得到的当前车辆横摆角速度，ΔT为采样时间间隔，flag为GNSS状态判断子模块表征GNSS状态的标志位，当GNSS状态良好时取1，GNSS状态不好时取0。

GNSS接收机能够直接输出当前的GNSS状态，GNSS状态判断子模块直接根据输出的状态即可判断状态好差，此状态信息相当于传感器信息，可以直接获取。

所述的步骤2)中，轮胎半径估计值r的估计式为：

X(k)＝X(kk-1)+K(k)[Y(k)-H^T(k)X(kk-1)]

X(kk-1)＝A(k-1)X(k-1)

X(k)＝X(kk-1)+K(k)[Y(k)-H^T(k)X(kk-1)]

P(kk-1)＝A(k-1)P(k-1)A^T(k-1)+Q

P(k)＝(I-K(k)H(k))P(k|k-1)

其中，X(kk-1)为状态预测值，A(k-1)为上一时刻系统的状态转移矩阵，且ω为轮速传感器测量得到的轮速信号，X(k)、X(k-1)为当前时刻和上一时刻系统状态，且v为车速，P(k)为协方差矩阵，H(k)为当前时刻系统测量矩阵，且H＝[10]，P(k|k-1)为协方差矩阵的预测值，Q为状态方程过程噪声方差，K(k)为卡尔曼滤波增益，R为测量过程噪声方差，Y(k)为通过传感器测量得到的当前观测量，I为二阶单位阵。

所述的步骤3)中，位置估计模块估计出的车辆位置估计值包括经度估计值和纬度估计值λ_k。

所述的经度估计值的估计式为：

其中，为当前时刻由GNSS测量得到的车辆位置经度，为前一时刻车辆位置经度估计值，ΔT为采样时间间隔，R_N为当地地球卯酉圈的曲率半径，λ_k为纬度估计值。

所述的纬度估计值λ_k的估计式为：

其中，λ_Gk为当前时刻由GNSS测量得到的车辆位置纬度，λ_k-1为前一时刻车辆位置经度估计值，R_M为当地地球子午圈的曲率半径。

实现该基于轮胎半径自适应的自动驾驶低速清扫车定位方法的定位系统包括：

传感器信号处理模块：用以根据GNSS状态判断当前测量车速是否准确，确定是否启用轮胎半径估计模块，并根据GNSS航向测量并融合IMU传感器测量信息得到当前车辆航向角，并以此作为位置估计模块的输入；

轮胎半径估计模块：当GNSS状态良好时，基于卡尔曼滤波根据速度测量反馈对轮胎半径进行在线实时估计，从而在GNSS状态不好时通过轮胎半径和车轮转速得到准确车速；

位置估计模块：利用轮胎半径估计模块在线估计出的轮胎半径、轮速传感器测量得到的轮速信号以及传感器信号处理模块得到的车辆航向角信息对车辆位置进行在线实时估计。

所述的传感器信号处理模块包括：

GNSS状态判断子模块：用以根据GNSS状态判断当前测量车速是否准确，确定是否启用轮胎半径估计模块；

车辆航向角计算子模块：根据GNSS航向测量并融合IMU传感器测量信息得到当前车辆航向角，并以此作为位置估计模块的输入。

与现有技术相比，本发明具有以下优点：

(1)本发明仅利用了车辆IMU传感器、GNSS传感器及轮速传感器进行车辆定位，未增加如摄像头、激光雷达等昂贵传感器，成本低廉易于实现，环境适应性好，鲁棒性强；

(2)现有的融合轮速信号的车辆定位方案大多忽略了轮胎半径对车速计算的影响，而轮胎半径为缓变量，易受温度、胎压、载荷等外界因素影响，因此本发明基于卡尔曼滤波对轮胎半径进行自适应在线估计，保证了在GNSS信号丢失或较差情况下仍能获取准确速度，从而得到准确的位置估计值，精度高，实用性强。

附图说明

图1为本发明系统结构原理框图。

图中标记说明：

1为传感器信号处理模块，2为轮胎半径估计模块，3为位置估计模块，4为GNSS信号质量判断模块，5为车辆航向角计算子模块，6为经度估计模块，7为纬度估计模块。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

如图1所示，本发明提供一种基于轮胎半径自适应的自动驾驶低速清扫车定位系统，该系统包括传感器信号处理模块1，其中包括GNSS信号质量判断模块4和车辆航向角计算子模块5；轮胎半径估计模块2，位置估计模块3，其中包括经度估计模块6和纬度估计模块7。

传感器信号处理模块1的输入为GNSS定位状态、GNSS测量得到的车辆航向角，IMU测量得到的车辆横摆角速度，输出为GNSS质量标志位及车辆航向角；轮胎半径估计模块2的输入为GNSS质量标志位，GNSS测量得到的车辆速度，轮速传感器测量得到的轮速，输出为轮胎半径实时估计值；位置估计模块3的输入为车辆航向角、轮速及轮胎半径。

其中，传感器信号处理模块1中的GNSS信号质量判断模块4具体为：

其中，flag为表征GNSS状态的标志位，当GNSS状态良好时取1，GNSS状态不好时取0。

传感器信号处理模块1中的车辆航向角估计模块5具体为：

其中，ψ_k表示车辆航向角估计模块计算得到的当前航向角，ψ_k-1表示上一采样时刻的车辆航向角，ψ_G表示由GNSS测量得到的车辆航向角，γ表示由IMU测量得到的当前车辆横摆角速度，ΔT为采样时间间隔。

当GNSS状态良好，即flag＝1时，轮胎半径估计模块满足计算条件，可以实时估计轮胎半径，轮胎半径估计模块2具体的计算步骤如下：

系统状态向量：

系统状态转移矩阵：

系统测量矩阵：

H＝[10]

计算状态预测：

X(k|k-1)＝A(k-1)X(k-1)

计算协方差矩阵预测：

P(k|k-1)＝A(k-1)P(k-1)A^T(k-1)+Q

计算卡尔曼滤波增益：

K(k)＝P(k|k-1)H^T(k)[H(k)P(k|k-1)H^T(k)+R]^-1

计算状态更新：

X(k)＝X(k|k-1)+K(k)[Y(k)-H^T(k)X(k|k-1)]

计算协方差矩阵更新：

P(k)＝(I-K(k)H(k))P(k|k-1)

其中，v为车速，r为车轮半径，ω为车轮转速。X(k|k-1)表示状态预测值，A(k-1)为上一时刻系统状态转移矩阵，X(k-1)为上一时刻系统状态，P(k|k-1)表示协方差矩阵的预测值，Q表示状态方程过程噪声方差，K(k)为卡尔曼滤波增益，R表示测量过程噪声方差，Y(k)表示通过传感器测量得到的当前观测量。I表示二阶单位阵。

当GNSS状态较差，即flag＝0时，采用GNSS状态良好时估计出的轮胎半径值。

位置估计模块3，其中的经度估计模块6具体为：

其中，为当前时刻车辆位置经度估计值，为当前时刻由GNSS测量得到的车辆位置经度，为前一时刻车辆位置经度估计值，ω为轮速，r为轮胎半径估计模块估计出的轮胎半径，ψ_k为传感器数据处理模块得到的车辆当前时刻航向角，ΔT为采样时间间隔，R_N为当地地球卯酉圈的曲率半径，λ_k为当地纬度，flag为传感器信号处理模块得到的GNSS状态标志位。

位置估计模块3，其中的纬度估计模块7具体为：

其中，λ_k为当前时刻车辆位置经度估计值，λ_Gk为当前时刻由GNSS测量得到的车辆位置纬度，λ_k-1为前一时刻车辆位置经度估计值，ω为轮速，r为轮胎半径估计模块估计出的轮胎半径，ψ_k为传感器数据处理模块得到的车辆当前时刻航向角，ΔT为采样时间间隔，R_M为当地地球子午圈的曲率半径，flag为传感器信号处理模块得到的GNSS状态标志位。

Claims (8)

1.一种基于轮胎半径自适应的自动驾驶低速清扫车定位方法，其特征在于，包括以下步骤：

1)根据GNSS状态判断子模块的GNSS状态的标志位，车辆航向角估计模块计算获得当前航向角ψ_k；

2)轮胎半径估计模块采用卡尔曼滤波对车轮半径进行估计，得到轮胎半径估计值r；

3)位置估计模块根据轮胎半径估计值r、轮速传感器测量得到的轮速信号ω以及传感器信号处理模块得到的当前航向角ψ_k对车辆位置进行在线实时估计，最终得到车辆位置估计值。

2.根据权利要求1所述的一种基于轮胎半径自适应的自动驾驶低速清扫车定位方法，其特征在于，所述的步骤1)中，当前航向角ψ_k的计算式为：

其中，ψ_k为车辆航向角估计模块计算得到的当前航向角，ψ_k-1为上一采样时刻的车辆航向角，ψ_G为由GNSS测量得到的车辆航向角，γ为由IMU测量得到的当前车辆横摆角速度，ΔT为采样时间间隔，flag为GNSS状态判断子模块表征GNSS状态的标志位，当GNSS状态良好时取1，GNSS状态不好时取0。

3.根据权利要求2所述的一种基于轮胎半径自适应的自动驾驶低速清扫车定位方法，其特征在于，所述的步骤2)中，轮胎半径估计值r的估计式为：

X(k)＝X(k|k-1)+K(k)[Y(k)-H^T(k)X(k|k-1)]

X(k|k-1)＝A(k-1)X(k-1)

X(k)＝X(k|k-1)+K(k)[Y(k)-H^T(k)X(k|k-1)]

P(k|k-1)＝A(k-1)P(k-1)A^T(k-1)+Q

P(k)＝(I-K(k)H(k))P(k|k-1)

其中，X(k|k-1)为状态预测值，A(k-1)为上一时刻系统的状态转移矩阵，且ω为轮速传感器测量得到的轮速信号，X(k)、X(k-1)为当前时刻和上一时刻系统状态，且v为车速，H(k)为当前时刻系统测量矩阵，且H＝[1 0]，P(k|k-1)为协方差矩阵的预测值，P(k)为协方差矩阵，Q为状态方程过程噪声方差，K(k)为卡尔曼滤波增益，R为测量过程噪声方差，Y(k)为通过传感器测量得到的当前观测量，I为二阶单位阵。

4.根据权利要求3所述的一种基于轮胎半径自适应的自动驾驶低速清扫车定位方法，其特征在于，所述的步骤3)中，位置估计模块估计出的车辆位置估计值包括经度估计值和纬度估计值λ_k。

5.根据权利要求4所述的一种基于轮胎半径自适应的自动驾驶低速清扫车定位方法，其特征在于，所述的经度估计值的估计式为：

其中，为当前时刻由GNSS测量得到的车辆位置经度，为前一时刻车辆位置经度估计值，ΔT为采样时间间隔，R_N为当地地球卯酉圈的曲率半径，λ_k为纬度估计值。

6.根据权利要求4所述的一种基于轮胎半径自适应的自动驾驶低速清扫车定位方法，其特征在于，所述的纬度估计值λ_k的估计式为：

其中，λ_Gk为当前时刻由GNSS测量得到的车辆位置纬度，λ_k-1为前一时刻车辆位置经度估计值，R_M为当地地球子午圈的曲率半径。

7.根据权利要求1所述的一种基于轮胎半径自适应的自动驾驶低速清扫车定位方法，其特征在于，实现该基于轮胎半径自适应的自动驾驶低速清扫车定位方法的定位系统包括：

传感器信号处理模块(1)：用以根据GNSS状态判断当前测量车速是否准确，确定是否启用轮胎半径估计模块，并根据GNSS航向测量并融合IMU传感器测量信息得到当前车辆航向角，并以此作为位置估计模块的输入；

轮胎半径估计模块(2)：当GNSS状态良好时，基于卡尔曼滤波根据速度测量反馈对轮胎半径进行在线实时估计，从而在GNSS状态不好时通过轮胎半径和车轮转速得到准确车速；

位置估计模块(3)：利用轮胎半径估计模块在线估计出的轮胎半径、轮速传感器测量得到的轮速信号以及传感器信号处理模块得到的车辆航向角信息对车辆位置进行在线实时估计。

8.根据权利要求7所述的一种基于轮胎半径自适应的自动驾驶低速清扫车定位方法，其特征在于，所述的传感器信号处理模块(1)包括：

GNSS状态判断子模块：用以根据GNSS状态判断当前测量车速是否准确，确定是否启用轮胎半径估计模块；

车辆航向角计算子模块：根据GNSS航向测量并融合IMU传感器测量信息得到当前车辆航向角，并以此作为位置估计模块的输入。