一种基于LiDAR/GPS/IMU融合的无人车重定位方法

技术领域

本发明主要涉及一种基于LiDAR/GPS/IMU融合的无人车重定位方法，属于多传感器融合定位技术领域。

背景技术

基于匹配技术的定位方案在自动驾驶领域得到广泛应用，以其高匹配精度和精准的姿态估计结果著称，然而，该方案对于初始值的要求相当严格，仅当初始位置和姿态良好时，算法才能确保准确性和时效性。在没有初始值的情况下，直接在当前帧数据和全局地图之间进行匹配会导致搜索范围过大，平台计算负荷过大，从而使算法失效。

在开放环境中，纯卫星定位表现出色。然而，在复杂环境中，如建筑物密集区域和高大树木区域，容易出现多径效应。同样，在隧道和地下等封闭区域，信号丢失成为问题。此外，当GNSS信号发生跳变时，姿态解算也可能产生误差。因此，出于上述原因，纯GNSS方案目前逐渐被放弃，取而代之的是多传感器融合的定位方案。

用三维激光雷达数据进行大范围环境地图构建的时候，不可避免的会遇到一个问题，就是机器人是否运动到之前已经构图的地方。由于累积误差的存在，机器人两次经过同一地方构建的地图往往不会是封闭形式，因此为了保证地图的准确性和精度，必须检测机器人是否已经在某环境中运行过，所以重定位的研究意义重大。

发明内容

本发明所要解决的技术问题在于，现有技术中定位鲁棒性有待提高和大场景点云地图中的重定位技术无法在计算能力受限的平台上运行，提供一种基于LiDAR/GPS/IMU融合的无人车重定位方法，首先重将定位问题分成两部分分别为全局初始位姿确定和局部位姿确定；全局初始位姿是利用GPS辅助确定一个粗略的位置，再结合传统的匹配方法DNT进行精匹配。再设计基于LiDAR/IMU/Prior Map的融合定位方案，通过高频的IMU信息对原始点云进行预处理，对处理过后的点云观测作边缘特征和平面特征的提取，与先验地图(Prior Map)相匹配获得激光里程计结果。采用基于滤波的方式对系统状态和协方差矩阵进行更新，从而估计车辆实时位姿，实现位置定位。与传统定位方法相比较，在定位鲁棒性和准确性上均有大幅度提高。为解决上述技术问题，本发明采取的技术方案是一种基于LiDAR/GPS/IMU融合的无人车重定位方法，该方法包含以下步骤：

S1：全局初始位姿确定：首先采用GPS模块进行粗定位，确定无人车在已建好的先验地图中的大致位置；在采集激光雷达点云数据时，同步接收GPS数据，根据激光雷达和GPS的时间戳，在点云轨迹上进行GPS的经纬度标注,当无人车初始状态处于先验地图某一随机位置时，在先验地图中查询标注的经纬度信息并进行对比，计算偏差，将距离该位置最近的标注位置作为无人车在先验地图中的初步位置，完成车辆的粗定位，

S2：基于NDT的激光雷达点云与局部地图匹配：直通滤波器截取GPS粗定位附近的点云，将直通滤波截取出的点云图作为被匹配点云P＝{p

S3：匹配优化：确定全局初始位姿之后，利用高频的IMU信息对后续的雷达帧点云先进行预处理去畸变，提高点云质量；接着进行特征提取，此处引用曲率这一特征指标，根据各点的曲率将所有点分为边缘点、平面点、一般点；将第k帧扫描的特征点与先验地图进行配准，将会得到当前帧点云相对于先验地图的位姿变换关系，提供里程计的输出，因此构造出残差方程并进行优化，

S4：滤波融合方式确定局部位姿：IEKF滤波器将IMU预积分模块获得的位姿作为预测，将基于特征的激光里程计获得的位姿估计结果作为观测，融合预测与观测数据更新系统状态，输出最终的局部定位结果，构建基于迭代扩展卡尔曼滤波的IMU与激光雷达紧耦合的局部定位方法。

其中，所述步骤S1中利用GPS信息来确定机器人相对于先验地图的粗略初始位姿的方式如下：

S1.1当无人车处于运动状态时，对采集到的3D激光雷达数据应用SLAM系统构建出先验点云地图，保留地图中无人车的轨迹；根据激光雷达和GPS的时间戳，在点云轨迹上进行GPS的经纬度标注；GPS模块数据更新频率为1Hz，则每隔1s标注一次经纬度信息；经过上述操作，使得已有先验地图中对应点的坐标与经纬度一一对应，

S1.2当无人车初始状态处于先验地图某一随机位置时，通过GPS模块获取该位置经纬度信息，在先验地图中顺序查询标注的经纬度信息并进行对比，计算偏差，将距离该位置欧式直线距离最短的标注位置作为无人车在先验地图中的初步位置，完成车辆的粗定位。

其中，步骤S2中基于NDT的激光雷达点云与局部地图匹配方式如下：

S2.1将实时点云作为源点云

其中，

S2.2进行NDT匹配时构建的目标函数为：

其中，N

S2.3将实时点云与先验地图进行关联的残差函数为：

f

其中，f(·)表示概率密度函数；

计算残差函数关于待求参数s的雅可比矩阵J(p)，同时求解增量方程H△x

S3.1引用曲率这一指标，对雷达帧点云提取特征点，将其投影到先验地图坐标系中，并在全局地图内寻找与目标点对应的n个点，计算点到线与点到面的距离，对角点残差方程定义如下：

其中，

其中x＝[x,y,z]

S3.4通过最小化点到直线与点到面的距离求取最优的全局位姿估计

其中，ε为角点，p为面点，f(*)为鲁棒核函数，N(ε)、N(p)表示角点和面点的个数。

与现有的技术相比，本发明的有益效果是：提出了一种基于LiDAR/IMU/GPS融合定位方法。首先，全球定位系统提供绝对定位数据来源，误差小，可提供相对准确的初始位姿估计以减少在地图上的搜索时间，减少资源消耗；接着，在GPS粗定位的基础上，通过NDT算法进一步匹配实现在先验地图上的精确定位；利用高频IMU信息对点云进行处理后进行帧到先验地图的匹配，最后利用滤波器融合观测数据和预测数据估计局部位姿信息，进一步提高在已知地图下定位的精确性和稳定性。本发明解决了传统重定位技术的资源消耗问题和精度粗糙问题，其效果要比直接匹配更加鲁棒。

附图说明

图1为本发明的步骤流程图；

图2为本发明全局初始位姿确定流程图；

图3为本发明的滤波融合算法框架图；

图4为IEKF算法流程图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

实施例1：一种基于LiDAR/GPS/IMU融合的无人车重定位方法，如图1所示，具体步骤如下：步骤S1：全局初始位姿的确定，该步骤的主要思路是先利用GPS进行粗略定位，在进行重定位时，能够加快搜索效率。在电脑端通过可视化界面软件实时查看并保存无人车当前所处位置的经纬度信息。根据激光雷达和GPS的时间戳，在点云轨迹上进行GPS的经纬度标注，使得已有先验地图中对应点的坐标与经纬度一一对应。

当无人车初始状态处于先验地图某一随机位置时，通过GPS模块获取该位置经纬度信息，在先验地图中顺序查询标注的经纬度信息并进行对比，计算偏差。将距离该位置欧式直线距离最短的标注位置作为无人车在先验地图中的初步位置，完成车辆的粗定位。

步骤S2：基于NDT的激光雷达点云与局部地图匹配:

截取上述初始位置附近的15*15*15m规模的点云，将其作为被匹配点云P，旋转激光雷达实时测量数据作为待匹配点云Q，如式(1):

使用NDT算法在被截取点云中进一步定位，其大致思路如附图2所示，获取无人车在先验地图中的精确位置，同时如式(2)所示将联合概率最大作为目标函数：

f(·)表示概率密度函数，

其中，μ表示均值，Σ表示协方差，

式中

此时所有点都转至世界坐标系。可以得到联合概率为：

通过取对数等方式简化问题，得到目标函数与残差函数：

f

最后，计算残差函数关于待求参数s的雅可比矩阵J(p)，同时求解增量方程H△x

确定全局初始位姿之后，利用高频的IMU信息对后续的雷达帧点云先进行预处理去畸变，提高点云质量。再进行特征提取，此处引用曲率这一特征指标,如式(9)所示，

其中，

其中，

其中x＝[x,y,z]

其中，f(*)为鲁棒核函数，N(ε)、N(p)表示角点和面点的个数，通过高斯牛顿法求解非线性方程得到激光里程计位姿估计。步骤S4，滤波融合方式确定局部位姿：其滤波融合的思路如图3所示，IEKF对运动和观测模型方程为，对工作点x

此时，运动方程不变，观测模型和其雅可比矩阵均在x

y

使用以上的线性化模型，可以得到IEKF递归更新的五个基本方程：

预测的状态方程为：

预测的协方差矩阵为：

增益矩阵为：

量测更新的状态方程为：

量测更新的协方差方程为：

IEKF在第一次迭代中，令

当

虽然在本文中参照了特定的实施方式来描述本发明，但是应该理解的是，这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是，可以对示例性的实施例进行许多修改，并且可以设计出其他的布置，只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是，可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是，结合单独实施例所描述的特征可以使用在其他所述实施例中。