一种基于多传感器数据融合的GPS数据修正算法

摘要

本发明涉及一种基于多传感器数据融合的GPS数据修正算法。所述方法包括：使用惯性测量单元Z轴数据对轮式里程计发生上下抖动时的数据进行处理。对处理后的轮式里程计数据与激光雷达数据进行扩展卡尔曼滤波，得到激光轮式里程计数据。之后对激光轮式里程计数据与GPS数据进行求平滑度，以判断是否用激光轮式里程计数据代替失效的GPS数据。本发明能够在GPS信号失效或者出现波动时，保证无人车定位精度并且减小无人车定位所用时间。

说明书

技术领域

本发明涉及人工智能与无人系统领域，具体涉及自动驾驶定位领域，尤其涉及一种基于多传感器数据融合的GPS数据修正算法。

背景技术

在无人系统领域无人车的自动驾驶中，定位是其关键技术之一，针对这一技术目前已经开展了大量的研究工作。无人车定位方案主要分为：

GNSS(Global Navigation Satellite System)：全球导航卫星系统。比如常见的GPS(Global Positioning System)、中国的北斗(BDS)。传统的定位精度在大约为米级别的。

IMU(Inertial Measurement Unit):惯性测量单元。利用加速度计和角速度计(陀螺仪)，根据上一时刻的位置和方位推断现在的位置和方位，即航迹推测.惯性导航和GPS可以结合起来使用,惯性导航可以弥补GPS更新频率低的缺陷，GPS可以纠正惯性导航的运动误差。但是，如果是在地下隧道或者其他信号不好的地方，GPS可能无法及时纠正惯性导航的运动误差。

传感器融合：利用激光雷达或者相机，记录道路外观，测量车辆到静态物体如建筑物，电线杆，路缘的距离，将传感器的点云数据与高精地图储存的特征进行匹配，并实现车辆坐标系与世界坐标系之间的转换，确定最有可能位于的位置。

针对GPS受环境影响较大而导致定位波动或者失效的问题，

文献《LIO-SAM:Tightly-coupled Lidar Inertial Odometry via Smoothingand Mapping》提出激光雷达、IMU、GPS紧耦合定位建图算法，存在如下问题：

(1)没有充分运用无人车上所搭载的传感器，如轮式里程计。

(2)没有对GPS失效情况下做出数据的纠正。

文献《LOAM:Lidar Odometry and Mapping in Real-time》提出使用平滑度来判断激光雷达点云数据中的点线关系与点面关系，存在如下问题：

(1)没有进行多传感器的融合，仅使用激光雷达来进行SLAM建图。

(2)没有对无人车姿态进行平滑度处理，尤其是在草坪与树叶较多的场景，激光雷达的姿态会出现偏差，无法做出姿态数据的纠正。

发明内容

本发明提供一种基于多传感器融合对GPS数据修正算法。将轮式里程计与激光雷达的数据进行扩展卡尔曼滤波，得到激光轮式里程计数据。之后对激光轮式里程计数据与GPS数据求平滑度，判断平滑度是否在其阈值区域内。最后比较平滑度与阈值的大小，判断是否将激光轮式里程计数据代替GPS数据加入图优化。本发明能够在GPS信号失效或者出现波动时，保证无人车定位精度并且减小无人车定位所用时间。

为达到上述目的，本发明提供一种基于多传感器数据融合的GPS数据修正算法，包括以下步骤：

S1、根据惯性测量单元IMU的Z轴加速度增量的数值对轮式里程计数据进行处理；

所述的S1中，根据惯性测量单元IMU的Z轴加速度增加量的数值对轮式里程计数据进行处理，具体包括以下子步骤：

S1.1、记录下无人车在当前环境平坦路段运行时Z轴加速度的最大值a

S1.2、根据所述S11中求得最大加速度值a

S1.3、在每帧轮式里程计数据到来时，对IMU的Z轴加速度值进行判断，若在加速度变化区间内，则视为轮式里程计数据为观测值，否则舍去此帧的轮式里程计数据。

S2、对轮式里程计数据与激光雷达里程计数据进行扩展卡尔曼滤波，得到激光轮式里程计数据；

S3、当收到GPS数据时，对GPS当前帧数据与其周围10帧激光轮式里程计数据求解平滑度，平滑度求解公式：

所述的S3中，当收到GPS数据时，对GPS当前帧数据与其周围10帧激光轮式里程计数据求解平滑度，平滑度求解公式：

S31、储存当前帧GPS数据X

S32、求得10帧激光轮式里程计与GPS数据集合S＝X

S33、求得平滑度

S4、根据初始帧激光轮式里程计数据的后5帧GPS数据与50帧激光轮式里程计数据的平滑度C

S4.1、求得小车刚启动后，连续5帧GPS的数据与50帧激光轮式里程计数据的平滑度；

S4.2、求上述5个平滑度的平均值，并设置为阈值，阈值公式为：

S5、判断当前平滑度是否小于阈值，若小于阈值则将激光轮式里程计作为GPS预测值加入图优化，否则使用原始GPS数据。

本发明具有如下有益效果：

1.通过IMU的Z轴加速度值对轮式里程计数据进行判断去除，避免使用无人车发生较强震动时带有偏差轮式里程计数据。使得通过相关里程计得出错误的无人车位姿。

2.通过激光轮式里程计数据与GPS数据进行平滑度求解，来判断GPS数据是否正常并且修正。可以实时的判断GPS数据的状态，可以实时地修正无人车的定位。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本专利的其它特征、目的和优点将会变得更明显：

图1一种基于多传感器数据融合的GPS数据修正算法流程图；

图2扩展卡尔曼滤波后激光轮式里程计轨迹图；

图3正常的GPS数据与激光轮式里程计数据平滑度图

图4波动的GPS数据与激光轮式里程计数据平滑度图

图5出现波动GPS数据图

图6修正后的GPS数据图

图7GPS数据发生波动的轨迹图；

图8修正后GPS数据的轨迹图；

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图，对本发明进行进一步详细说明。

如图1所示，本发明提供了一种基于多传感器数据融合的GPS数据修正算法，具体包括以下步骤：

S1、根据惯性测量单元IMU的Z轴加速度增加量的数值对轮式里程计数据进行处理；

所述的S1中，根据惯性测量单元IMU的Z轴加速度增加量的数值对轮式里程计数据进行处理，具体包括以下子步骤：

S11、记录下小车在当前环境平坦路段运行时Z轴加速度的最大值a

S12、根据所述S11中求得最大加速度值a

S13、在每帧轮式里程计数据到来时，对IMU的Z轴加速度值进行判断，若在加速度变化区间内，则将轮式里程计数据作为观测值，否则舍去此帧的轮式里程计数据。

S2、对轮式里程计数据与激光雷达里程计数据进行扩展卡尔曼滤波，得到激光轮式里程计数据；

所述的S2中，对轮式里程计数据与激光雷达里程计数据进行扩展卡尔曼滤波，得到激光轮式里程计数据，具体包括以下子步骤：

S21、位姿初始化：在点云地图中实现初始定位，记录初始时刻位置

S22、状态预测：环境中地标总数为N，地标状态变量为m，系统状态变量X＝[x,y,θ,m

S23、状态更新：根据激光雷达的数据得到系统的观测方程

V(k)＝Z(k)-H(k)X(k|k-1)

S(k)＝H(k)P(k|k-1)H(k)

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

X(k|k)＝X(k|k-1)+K(k)V(k)

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

其中V为残差，H为观测方程对系统状态变量求导得到的雅克比矩阵，S为残差方差矩阵，R为测量噪声的协方差矩阵，K为卡尔曼增益。如图2所示，为轮式里程计与激光里程计的轨迹，以及二者经过卡尔曼滤波所得的激光轮式里程计数据的轨迹。

S3、当收到GPS数据时，对GPS当前帧数据与其周围10帧激光轮式里程计数据求解平滑度，平滑度求解公式：

所述的S3中，收到GPS数据时，对GPS当前帧数据与其周围10帧激光轮式里程计数据求解平滑度，平滑度求解公式：

S31、储存当前帧周围10帧激光轮式里程计数据X

S32、求得10帧激光轮式里程计与GPS数据集合S＝X

S33、求得平滑度

S4、根据初始帧激光轮式里程计数据的后5帧GPS数据与50帧激光轮式里程计数据的平滑度C

所述的S4中，根据初始帧激光轮式里程计数据的后5帧GPS数据与50帧激光轮式里程计数据的平滑度C

S4.1、求得小车刚启动时，5帧GPS的数据与50帧激光轮式里程计数据的平滑度C

S4.2、求其平均值，并设置为阈值

S5、判断当前平滑度是否小于阈值，如图5所示，为出现失效GPS数据图。其中圆形点为GPS数据位置，并且个别GPS数据出现较大波动，以及叉形点为激光轮式里程计位置。如图6所示，为修正后的GPS数据图。其中个别GPS数据是图5中失效的GPS数据经过算法修正后的位置，其中叉形点为算法使用的激光轮式里程计位置。如图7所示，因为GPS姿态数据出现波动，导致无人车姿态出现误差，其平滑度大于阈值g若小于阈值则将激光轮式里程计作为GPS预测值加入图优化中，否则使用原始GPS数据。如图8所示，为修正GPS数据后的小车姿态轨迹。