CN111522043B - 一种无人车激光雷达快速重新匹配定位方法 - Google Patents

Abstract

本发明涉及一种无人车激光雷达快速重新匹配定位方法，属于无人驾驶领域。本发明利用多传感器融合的思想，解决了无人车在园区任意环境下SLAM的持续定位问题和激光雷达扫描匹配失败后的快速重新匹配问题。本发明一共分为多传感器标定、融合位姿和融合定位三个模块。通过多传感器联合标定，获得对目标的一致性描述；将GPS传感器和激光雷达传感器对无人车解算的位姿信息进行数据融合，获得GPS传感器和激光雷达传感器融合的车辆位姿信息；当激光雷达SLAM定位模块点云匹配失败时，使用融合位姿来替代点云匹配算法的定位预测矩阵，实现激光雷达SLAM算法的快速重新匹配，实现无人车SLAM算法的持续定位。

Description

一种无人车激光雷达快速重新匹配定位方法

技术领域

本发明属于无人驾驶领域，特别涉及一种无人车激光雷达快速重新匹配定位方法。

背景技术

无人驾驶汽车是一种智能汽车，通过车载传感系统感知道路环境并自行规划行驶路线抵达预设目的地。无人驾驶汽车整体一共分为三大模块，分别为感知模块、决策模块以及控制模块。在感知模块中，无人车对其自身的定位具有十分重要的作用，对于车辆后续行为具有极其重要的作用。随着人工智能的发展，无人车技术得到了极大的提升，但是受到目前的法律限制以及技术壁垒的限制，无人车商用化在短时间内是无法实现的。然而对于低速车，自动驾驶有很多的应用场景。

在现有技术下，当无人车在利用激光雷达SLAM定位过程中因为外界干扰(如临时大型车辆前方长期遮挡雷达视线)而发生点云匹配失败的问题，激光雷达SLAM定位算法需要将当前激光雷达扫描到的局部点云数据与事先已经获得的全局地图点云数据进行重新搜索匹配，由于点云数量巨大，重新全部搜索时间开销会很大，从而造成无人车原定停止。

与本发明最接近的技术如下：

专利名：一种低速无人车园区内组合导航方法及系统

申请号：CN201910538148.3

基本内容：本发明提供一种低速无人车园区内组合导航方法及系统,其中方法包括使用雷达SLAM模块生成路径信息1,还包括以下步骤：进行联合地图采集,生成路径地图；使用RTK导航模块生成路径信息2；融合模块接受所述路径信息1和路径信息2,加权融合后生成目标轨迹。本发明将移动站定位天线改为前天线,同时将雷达坐标原点平移到导航前天线位置,保证了采集地图轨迹的一致性；其次联合采集地图,地图中同一个路点既包括雷达SLAM定位坐标也包括RTK-导航经纬度坐标；然后雷达SLAM模块与RTK-导航模块加载同一个地图进行一次路径规划,所得路径点均以车辆坐标系为原点；最后通过融合模块处理两个定位模块的局部路径,最终生成无人车的目标路径。

与本专利的差异点：

上述专利与本专利的差异点主要体现在以下两点，

目的

上述专利的目标是利用RTK-导航与SLAM导航模块融合生成无人车的目标路径，本专利的目标是将GPS传感器与激光雷达传感器融合解决无人车激光雷达SLAM算法点云失配时无人车的快速重新匹配定位问题，实现无人车在SLAM过程中具有持续定位能力。

技术路线

上述专利在采集地图的过程中，需要使用GPS-RTK来获取无人车在全局地图的精确位置信息，并没有考虑到在GPS信号较差的路段如何解决精确定位的问题，也没有考虑在激光雷达SLAM失配的情况下快速重新匹配的问题。本专利考虑了园区内任意环境下的GPS的信号状态，并且解决了在GPS信号较差的路段配合激光雷达实现快速精确定位问题。

专利名：一种结合GPS和雷达里程计的SLAM方法

申请号：201811306455.0

基本内容：此专利是一种结合GPS和雷达里程计的SLAM方法，包括如下步骤：1)采集差分GPS数据和来自激光雷达的点云数据；2)处理GPS数据获得位移(X,Y,Z)和姿态RPY角；3)匹配GPS数据和LiDAR的点云数据，通过时间戳对齐的方式实现数据匹配；4)结合步骤2)处理GPS得到的位姿数据和LiDAR的点云数据检验GPS数据的可靠性；5)使用雷达里程计算法LOAM获取(X,Y,Z)和RPY角；6)在GPS数据可靠的地方，使用GPS获取的位姿作为最终的位姿；在GPS数据不可靠的路段，利用该路段起点和终点的GPS位姿优化LOAM算法的位姿来获取最终的位姿；7)使用步骤6)输出的位姿转换激光雷达的点云数据到世界坐标系下，获取最终的全局地图。本发明适用于大范围城市三维地图的构建。

与本发明的差异点：

本发明与上述专利的不同之处主要有两个方面，

目的

上述专利注重于利用GPS与LOAM算法的融合实现无人车全局一致性地图的生成。本发明的主要目标是利用GPS、激光雷达多传感器融合解决无人车在园区内激光雷达的快速精确定位的问题，使无人车可以在园区内实现SLAM持续精确定位。

技术层面不同

上述专利主要步骤是：上述专利在生成全局一致性地图的过程中，根据GPS的信号状态来选择此路段的最终位姿是选择GPS的位姿还是采用LOAM算法的位姿作为最终位姿，并没有考虑到GPS信号状态较差且LOAM算法点云失配时的情况。

本发明考虑了GPS信号以及激光雷达点云匹配的全部状态，当无人车在SLAM定位过程中发生点云失配的情况时，即使在GPS信号较差的路段，也可以利用GPS与激光雷达的融合位姿信息对SLAM定位的匹配算法NDT(Normal Distributions Transform)提供补偿，缩小NDT点云匹配算法的点云搜索范围，从而解决SLAM导航过程中匹配丢失无法再匹配的问题。

专利名：一种基于SINS/GPS和视觉里程计辅助的视觉SLAM方法

申请号：CN201710012804.7

基本内容：本发明公开了一种基于SINS/GPS和里程计辅助的视觉SLAM方法,包括如下步骤：当全球卫星定位系统信号可用时,将GPS与捷联惯性导航系统输出信息进行数据融合,得到姿态、速度、位置等信息；当GPS信号不可用时,将里程计与SINS输出信息进行数据融合,得到姿态、速度、位置等信息；利用双目摄像机拍摄得到环境图片,对其进行特征提取和特征匹配；利用上述得到的先验姿态、速度、位置信息和环境特征实现定位与地图构建,完成视觉SLAM算法。本发明利用SINS、GPS和里程计辅助视觉SLAM,能够实现室外和室内两种环境下的定位与地图构建,应用范围广泛,且能够提高定位的精度和鲁棒性。

与本发明的差异点：

上述专利与本发明主要有三方面不同

目的不同

上述专利注重于利用传感器融合数据实现室外和室内两种环境下的定位与地图构建。本发明的主要目标是利用GPS传感器与激光雷达多传感器融合实现无人车激光雷达SLAM快速重新匹配定位，使无人车在园区的全场景内实现激光雷达SLAM的持续精确定位。

传感器种类不同

上述专利使用到了GPS传感器、视觉里程计以及惯性导航，本发明只需要用到GPS传感器以及激光雷达，并且视觉里程计容易受到光照等因素的影响，鲁棒性不及激光雷达。

技术层面不同

上述专利为了实现建图效果，根据不同路段GPS的信号状态，选择GPS的位姿还是惯导与视觉里程计的融合位姿作为最终的位姿，并没有考虑到在GPS信号极差的路段，由于惯性导航误差的不断积累会导致无人车的定位不够精确。而本发明考虑了GPS信号的全部状态，针对园区低速场景，采用多传感器融合的方式，实现了无人车在园区内任意场景下的精确定位。

发明内容

本发明依据低速园区无人车运行环境的特点(半开放、路程短、楼宇树木遮挡严重等)，提出了激光雷达SLAM(simultaneous localization and mapping)与GPS(GlobalPositioning System)传感器融合定位方法，解决了无人车在园区任意环境下SLAM的持续定位问题和激光雷达扫描匹配失败后的快速重新匹配问题。

激光雷达可以生成三维的位置信息，快速确定物体的位置、大小，比摄像头等传感器，探测距离更远、精确度更高，还有更灵敏的响应速度，还不受环境光照的影响，但是激光雷达最大的缺陷，就是不能在雪雨等极端天气下工作，在这种天气下会导致无人车的“位置晕眩”。GPS传感器导航灵活性高，但需要接受卫星信号，会有信号失真及丢失的情况，影响定位精度。单一传感器在无人车的定位过程中都存在不足。

本发明采用GPS传感器和激光雷达传感器融合定位的思路，获得GPS传感器和激光雷达传感器融合的车辆位姿信息，当无人车在使用激光雷达SLAM定位算法NDT点云匹配失败时，根据当前的GPS信号状态替代点云匹配算法NDT算法的定位预测矩阵，实现无人车SLAM算法的持续定位，提高无人车在低速园区环境下的定位精度。本发明整体系统框架图如图4所示：

有益效果

本发明提出的多传感器融合的定位方法，在激光雷达SLAM点云失配时，根据GPS当前的状态情况，使用GPS传感器与激光雷达的融合位姿来替换激光雷达SLAM算法中的预测矩阵，可将重新匹配的搜索范围大大缩小，实现重新的快速匹配，实现了园区无人车激光雷达SLAM算法的持续定位。本方法在园区低速无人驾驶车中得到了实验验证，在匹配精度以及匹配速度上可以取得很好的效果。

附图说明

图1激光雷达与GPS接收器安装示意图

图2多传感器联合标定算法流程图

图3原始点云以及NDT算法表示下的点云分布

图4系统框架图

图5GPS与激光雷达传感器位姿融合流程图

图6(a)GPS信号接收正常6(b)GPS信号接收异常

图7GPS与激光雷达SLAM融合定位算法流程图

图8(a)GPS信号较好的路段8(b)GPS信号较差的路段

图9(a)GPS较好的路段点云失配场景(b)GPS较差的路段点云失配场景

图10(a)NDT算法车辆位姿预测坐标(b)GPS与激光雷达的融合位姿坐标信息

图11NDT算法预测矩阵与多传感器融合的位姿矩阵对比图

具体实施方式

准备工作以及背景知识：

低速无人车传感器标定

本发明基于激光雷达与GPS传感器融合实现激光雷达快速重新匹配的方法，为了保证激光雷达与GPS传感器坐标一致性。由于激光雷达以及GPS传感器与车辆是刚性连接，二者的安装位置已固定，对激光雷达以及GPS传感器的安装位置进行标定，使激光雷达坐标系、GPS坐标系以及车辆坐标系三者统一。本发明以车辆坐标系为基准，其中车辆坐标系坐标原点为车顶GPS传感器前天线位置，X轴方向指向车头正前方，Y轴正方向为X轴正方向逆时针旋转90°，Z轴正方向指向天空。采用旋转平移矩阵的方法将GPS传感器以及激光雷达传感器的坐标原点转换到车辆坐标原点。本发明采用的GPS导航装置为双天线导航，一般的双天线导航后天线为定位天线，前天线为定向天线，本发明为了将雷达坐标原点与导航定位点统一故将前天线连接到导航接收机的后天线位置，后天线连接到导航接收机的前天线位置，并且本发明只用到前天线。前天线位置即为车辆坐标系原点。

激光雷达与GPS传感器的安装位置关系如图1所示。其中，激光雷达坐标系，X轴正方向为雷达数据输出线缆的相反方向，Y轴正方向为X轴正方向逆时针旋转90°，Z轴正方向指向天空；假设雷达的安装位置距地面高度为0.5米，距车辆坐标系原点的前向位移为1.5米，水平方向无偏差，那么激光雷达坐标系以及GPS坐标系向车辆坐标系的转化过程，以激光雷达为例，设定激光雷达三维坐标系中的一个向量其中点P在XY平面、XZ平面、YZ平面的投影分别为点M、点P、点N，/>绕Z转旋转θ角，旋转矩阵为/> 绕X转旋转θ角，旋转矩阵为：/> 绕Y转旋转θ角，旋转矩阵为：/>在Z、X、Y轴的平移矩阵为：/>

其中，激光雷达、GPS传感器与车辆坐标系联合标定算法流程图如图2所示：

NDT(Normal Distributions Transform)匹配算法介绍

在SLAM导航过程中，传统的点云配准算法一般为ICP迭代的方法，这种方法一般需要提供一个较好的初值，也就是需要粗配准，同时由于算法本身缺陷，最终迭代结果可能会陷入局部最优，导致配准失败，效果往往不够理想。本发明介绍的是另一种比较好的配准算法，NDT配准算法。所谓NDT就是正态分布变换，作用与ICP一样用来估计两个点云之间的刚体变换。用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快。这个配准算法耗时稳定，跟初值相关不大，初值误差大时，也能很好的纠正过来。

NDT配准算法步骤如下：

将参考点云(reference scan)所占的空间划分成指定大小(CellSize)的网格或体素(Voxel)，并计算每个网格的多维正态分布参数，激光雷达的原始点云以及点云在NDT算法下的空间划分效果如图三所示。

(2)初始化变换参数初始化变换参数(赋予零值或者使用里程计数据赋值)。

(3)对于要配准的点云(second scan)，通过变换T将其转换到参考点云的网格中x_i′＝T(x_i,p)

根据正态分布参数计算每个转换点的概率密度

NDT配准得分(score)通过对每个网格计算出的概率密度相加得到

根据牛顿优化算法对目标函数-score-行优化，即寻找变换参数p使得score的值最大。

跳转到第3步继续执行，直到达到收敛条件为止。

GPS、激光雷达传感器融合位姿

本发明在实验过程中，首先要对GPS、激光雷达接收到的数据进行数据预处理，滤波处理以及剔除噪点，无人车的状态可以表示为

其中P_X,P_y代表无人车当前的位置，代表无人车的航向角信息。GPS传感器可以检测无人车的当前位置，没有速度信息。其测量值Z＝(G_x,G_y)，激光雷达传感器可以检测到距离、航向角、速度信息，其测量值表示为/>GPS与激光雷达位姿融合流程图如图5所示：

首先，对数据进行初始化，在收到传感器返回的测量值时，对无人车的状态进行初始化，初始化默认车辆当前处于原点而且速度值为零，同时加上对时间的更新。

GPS传感器处理过程如公式4所示：

在GPS的测量值Z＝(G_x,G_y)左乘一个系数矩阵，GPS传感器处理之后的结果为无人车此时的位置坐标。

激光雷达传感器处理过程如公式5所示，通过将激光雷达的测量值解算，可以得到当前无人车的位置坐标，以及无人车在X、Y轴的速度大小。

将两种传感器处理后的结果进行融合，无人车的位姿表示为

经过GPS以及激光雷达传感器数据解算后，得到来自激光雷达和GPS传感器解算的无人车位置坐标，以及来自激光雷达表示的无人车航向角信息，当激光雷达SLAM定位算法失配时，若GPS传感器接受信号状态良好，则将GPS传感器解析的位置坐标以及激光雷达解析的航向角作为最终的融合位姿对NDT算法的预测矩阵进行替换。若GPS传感器接受信号状态差，则将激光雷达传感器解析出来的位置坐标以及航向角作为最终的融合位姿对NDT算法的预测矩阵进行替换。

由于无人车处于持续运动中，故还可以使用卡尔曼滤波算法预测下一位置，利用预测后的信息计算位姿矩阵，达到矫正位姿的作用。两种传感器在预测无人车下一时刻的状态时，使用卡尔曼滤波算法预测下一位置，具体如下

位姿预测状态方程：

其中，x为无人车的状态，F是系统的控制函数，表示系统的改变，在这里，只考虑简单的线性问题，P代表无人车当前的位姿，Q代表系统误差。

GPS和激光雷达传感器在使用卡尔曼滤波算法预测无人车下一时刻的方法是完全相同的。GPS和激光雷达的参数更新部分是不同的，不同的原因是不同传感器收到的测量值是不同的。当收到GPS或激光雷达的测量值，依次执行预测、更新步骤；当同时收到GPS和激光雷达的测量值，依次执行预测校正、更新位姿参数步骤。

融合位姿替代SLAM定位算法预测矩阵

本发明中，激光雷达SLAM定位算法采用NDT(Normal Distributions Transform)点云配准算法，NDT算法配准实质上是在高精度的地图中利用激光雷达实时扫描到的激光雷达点云数据进行点云匹配来实现高精度定位的方法，NDT算法的定位指标评价表如表5-1所示。

表5-1 NDT算法点云匹配定位指标评价表

无人车在高精度地图中利用激光雷达NDT点云匹配算法定位的过程中，当点云匹配的匹配率低于80％时，会导致激光雷达NDT算法定位失败，导致NDT算法必须将此时激光雷达的局部环境点云与整个高精度点云地图进行搜索配准来确定车辆当前的位置，这个过程会耗费巨大的时间，例如当整个地图的点云数量达到40万以上时，当发生点云失配时，耗费时间大约在20分钟以上，而且匹配成功率具有不确定性。由于耗时巨大且无法确定最终的匹配结果，无人车不得不回到地图的起点位置，重新初始化预测矩阵来获得在全局地图的位置。本发明针对激光雷达NDT点云匹配算法失配时无法快速匹配的情况，提出了激光雷达快速重新匹配定位方法，其中GPS定位评价指标如图6所示,当GPS信号较强时，即当前GPS搜星数大于10颗时，无人车接收到的GPS坐标信息无异常的坐标点，如图6(a)所示，当GPS信号较差时，即当前GPS搜星数小于10颗。无人车接收到的GPS坐标信息有异常的坐标值(坐标突变)，如图6(b)所示。

当NDT算法点云失配时，根据当前的GPS信号状态分情况的对NDT算法的预测矩阵进行替代，其中NDT算法的定位策略如表5-2所示。

表5-2 NDT算法点云匹配策略

当NDT算法在定位过程中，如果点云的匹配率小于80％时会导致NDT点云匹配算法将会定位失败，此时利用GPS传感器与激光雷达传感器融合后的位姿矩阵来替换NDT算法的预测矩阵利用当前的融合位姿信息帮助激光雷达NDT定位算法再次获得在全局地图中的位置，GPS与激光雷达SLAM融合定位的算法流程图如图7所示。

当NDT算法失配时，若此时的GPS信号状态较好时，即GPS传感器表示的无人车位置坐标无异常值的存在，则利用GPS与激光雷达的融合位姿矩阵对NDT算法的预测矩阵进行精确替换，由图10所示，在GPS信号较强时，此时融合位姿矩阵与NDT点云匹配算法在正常定位下的预测矩阵几乎一致，在GPS信号较差的地方，即GPS代表的无人车位置坐标有异常值的存在，则利用融合位姿缩小NDT预测矩阵点云搜索匹配的范围，而不需要将局部点云地图与整个点云地图进行搜索匹配，本发明在某学校校区内分别选取了两个场景，分别在GPS信号较好的路段以及GPS较差的路段分别进行了若干此实验，实验场景如图8(a),8(b)所示：

实验过程中，当无人车在利用SLAM持续定位导航时，分别在GPS较好的路段以及GPS信号较差的路段对前进的无人车进行人为的长时间的车辆遮挡，当无人车当前的点云匹配失败时，我们将遮挡车辆移除，如图9(a),9(b)所示，计算无人车激光雷达SLAM再次快速重新匹配定位的时间。

实验结果如表5-3所示，在无人车定位过程中，当NDT点云匹配失败时，在GPS信号较好的地方，利用融合位姿一替换NDT算法的预测矩阵，NDT在断点处再次匹配的时间大约在14.3ms左右；在GPS信号较差的地方，利用融合位姿二替换NDT算法的预测矩阵，NDT在断点处再次匹配的时间大约在85ms左右。经过实验数据可得，当NDT算法失配时，利用本文算法可以使NDT算法再次快速定位。

表5-3 NDT算法点云失配再匹配时间

实现无人车激光雷达快速再匹配的过程中，需要用到激光雷达与GPS传感器融合后的位姿矩阵，在实验分析中，为了说明当NDT算法失配时，融合位姿矩阵可以代替NDT算法的预测矩阵。图10中描述了无人车当前位置坐标的横纵坐标信息。当NDT算法点云匹配率不足80％时，即NDT算法的预测矩阵代表的无人车横纵坐标会出现异常值，如图10(b)所示，在GPS信号较好的路段，采用GPS与激光雷达融合的位姿坐标曲线相对平滑，如图10(a)所示。图11是NDT算法预测矩阵与多传感器融合的位姿矩阵对比图，其中“---”状曲线代表NDT预测矩阵中的(x,y)横纵坐标信息，“…”曲线代表GPS传感器与激光雷达传感器融合后的位姿矩阵(x,y)横纵坐标信息。通过图10以及图11证明了在NDT算法的失配处，可以利用融合位姿矩阵近似代替NDT的预测矩阵，使NDT算法不需要进行全局扫描，可以通过融合位姿矩阵再次快速匹配。

总结：本发明针对低速园区的复杂环境，提出了无人车激光雷达快速重新匹配定位的方法，从而解决无人车在SLAM定位过程中，点云匹配失败无法再次快速匹配的问题，大大提高了无人车在低速园区内任意场景下SLAM的定位精度，保障无人车安全性的同时，也为无人车后续的决策控制提供了可靠的依据。

Claims (4)

1.一种无人车激光雷达快速重新匹配定位方法，本方法利用激光雷达SLAM定位方法NDT对无人车进行实时点云配准，利用激光雷达当前的局部点云地图与整个全局地图进行匹配定位，其特征在于：根据点云匹配率和GPS信号状态改进了激光雷达SLAM定位方法NDT，具体如下：

当NDT算法失配且此时的GPS传感器无异常值存在，则将NDT算法的预测矩阵替换为第一融合位姿矩阵，快速完成再匹配定位；

当NDT算法失配且此时的GPS传感器有异常值存在，则将NDT算法的预测矩阵替换为第二融合位姿矩阵，快速完成再匹配定位；

当NDT算法未失配且不论此时的GPS传感器是否有异常值存在，则使用NDT完成点云匹配定位；

所述的第一融合位姿矩阵指：当激光雷达SLAM定位模块发生失配且当前GPS无异常值存在时，由GPS解算的位置坐标和激光雷达的航向角信息构成的融合位姿矩阵；

所述的第二融合位姿矩阵指：当激光雷达SLAM定位模块发生失配且当前GPS有异常值存在时，由激光雷达解算的位置坐标和激光雷达的航向角信息构成的融合位姿矩阵。

2.根据权利要求1所述的一种无人车激光雷达快速重新匹配定位方法，其特征在于，所述的所有坐标要进行坐标标定，用于保证激光雷达传感器、GPS传感器与车辆坐标系的一致性，具体包括，

确定车辆坐标系；

坐标系转换：以车辆坐标系为基准，将激光雷达坐标系以及GPS传感器的坐标系采用矩阵的旋转平移方法将两种传感器的坐标系向车辆坐标系转化；判断三者的坐标标定是否完成，如果标定完成，则进行后续的位姿融合以及融合定位；否则，通过重新调整旋转平移矩阵使三者的坐标系统一。

3.根据权利要求2所述的一种无人车激光雷达快速重新匹配定位方法，其特征在于：所述的车辆坐标系的原点为车顶GPS传感器前天线的位置，车辆坐标系X轴方向指向车辆正前方，Y轴为X轴方向逆时针旋转90°的方向；Z轴方向垂直向上。

4.根据权利要求2所述的一种无人车激光雷达快速重新匹配定位方法，其特征在于：所述判断坐标系是否标定完成的标准为：车辆坐标系、激光雷达坐标系以及GPS传感器三者得到对目标的一致性描述。