一种多传感器松耦合的室内外同步定位与建图方法及系统

1.本发明属于无人驾驶车辆领域，具体涉及一种多传感器松耦合的室内外同步定位与建图方法及系统。


背景技术：

2.激光雷达(light detection and ranging,lidar)是一种有源传感器，是以发射激光束探测目标的位置、速度等特征量的雷达系统。其工作原理是向目标发射探测信号(激光束),然后将接收到的从目标反射回来的信号(目标回波)与发射信号进行比较,作适当处理后,就可获得激光雷达与待测目标的距离、高度等信息。惯性测量单元(inertial measurement unit,imu)是测量物体三轴姿态角(或角速率)以及加速度的装置。一般地，一个imu包含三轴的陀螺仪和三个方向的加速度计，来测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。同步定位于建图(simultaneous localization and mapping,slam)同步定位与建图(simultaneous localization and mapping,slam)技术可以使载体在未知环境中根据传感器采集的数据实时估计自身位姿并绘制环境地图，为载体提供定位信息，用于自主规划与决策。slam技术使用的传感器种类多样，包括lidar、相机、imu、全球导航卫星系统(global navigation satellite system,gnss)等。多种传感器同时安装在一个载体上时，通过信息流耦合，能充分发挥各种传感器的优点。例如lidar虽然测距精准，但频率较低，一般为5-20hz。imu采集频率高达100-300hz，适合安装在动态平台上。将lidar与imu耦合，可以用高频imu对lidar数据进行修正，获得更高的精度。根据耦合的紧密程度，信息流可以进行松耦合与紧耦合。松耦合中各传感器系统是独立的模块，分别收集数据，再将每个模块输出的结果进行融合。与松耦合相比，紧耦合信息融合得更早，使用多个传感器的原始数据共同输出一种结果，可以更充分地利用传感器数据。松耦合与紧耦合示意图如图1所示。
3.专利cn201910297985.1提供了一种用于视觉-惯性-激光融合的同步定位与建图方法，主要涉及多传感器融合、slam等技术领域。为了解决单传感器slam在定位和建图中精度低、易丢失的问题，该技术提出了一种鲁棒的高精度的视觉、惯性、激光雷达融合的slam系统。
4.现有技术一没有根据场景的特点对方法进行细化。具体体现在，其对环境特征点的分类为边缘特征点与平面特征点，在不同的环境下可能会出现分类不准确的情况。
5.现有技术一使用的高斯牛顿优化方法，在计算效率上还存在可以改进的空间。


技术实现要素：

6.本发明所为了解决背景技术中存在的技术问题，目的在于提供了一种多传感器松耦合的室内外同步定位与建图方法及系统。
7.为了解决技术问题，本发明的技术方案是：
8.一种多传感器松耦合的室内外同步定位与建图方法，所述方法包括：
9.对点云数据与惯性数据，进行数据预处理，得到点云的图像视图数据和自动驾驶车辆的粗略位姿数据；
10.对所述点云的图像视图数据，进行场景识别、特征提取和拼接处理，得到周围环境未经优化的点云地图；
11.基于场景识别的结果，利用后端优化算法和回环检测算法，对自动驾驶车辆的粗略位姿数据和所述周围环境未经优化的点云地图进行优化处理，得到优化后的自动驾驶车辆的位姿数据和周围环境点云地图，即实现了自动驾驶车辆的同步定位与建图。
12.进一步，通过激光雷达采集点云数据，通过惯性测量单元采集惯性数据。
13.进一步，所述对点云数据进行预处理，具体包括：
14.对所述点云数据进行线束分割，得到点云数据的垂直分割结果；
15.对所述点云数据进行元区域水平分割，得到点云数据的水平分割结果；
16.利用预设的激光雷达的坐标系与车辆质心的位置关系对点云数据的垂直分割结果和水平分割结果进行重投影，得到点云的图像视图数据。
17.进一步，所述对惯性数据进行预处理，具体包括：
18.对所述惯性数据的重力分量进行消除，得到自动驾驶车辆的粗略位姿数据；
[0019][0020]
其中a'
t
是惯性测量单元数据真值,a
t
是惯性测量单元数据测量值,r
zyx
是惯性测量单元数据的旋转矩阵,g是重力加速度。通过对a'
t
的积分，得到自动驾驶车辆的速度与位置。
[0021]
进一步，所述进行场景识别、特征提取和拼接处理，具体包括：
[0022]
利用点云的图像视图数据的平均点云深度da是否超过阈值作为判断条件，对自动驾驶车辆处于室内、室外环境进行区分，得到场景识别结果；
[0023][0024]
其中n是一帧点云中点的总数，di是第i个点相对于激光雷达几何中心的欧氏距离；
[0025]
基于场景识别结果，利用点云的图像视图数据中的点云每点附近曲率c将场景特征分为角特征与平面特征，使用迭代最近点算法配准点云的图像视图数据(多个数据帧的点云)，得到周围环境未经优化的点云地图；
[0026][0027]
定义点云的图像视图数据中同一条水平线上第i个点pi附近的曲率为c，其中a为点pi单侧相邻的点数，pj为与pi相邻的点。
[0028]
进一步，基于场景识别的结果中的室外环境，利用后端优化算法进行处理，具体包括：
[0029]
基于室外环境，利用后端优化算法中的高斯牛顿算法，对自动驾驶车辆的粗略位姿数据和所述周围环境未经优化的点云地图，进行优化处理，得到室外环境优化后的自动驾驶车辆的位姿数据和周围环境点云地图。
[0030]
进一步，基于场景识别的结果中的室内环境，利用后端优化算法中的两步lm算法
进行处理。
[0031]
进一步，利用后端优化算法中的两步lm算法进行处理，具体包括：
[0032]
对自动驾驶车辆的粗略位姿数据和所述周围环境未经优化的点云地图利用两步lm优化；
[0033]
第一步lm优化，首先使用提取的所述平面特征计算出六自由度位姿中受旋转影响较大的俯仰角θ
pitch
和高程位移z，以第一步lm优化的计算出结果作为先验信息，即精确的自动驾驶车辆旋转位姿信息；
[0034]
第二步lm优化，计算其余的六自由度位姿的信息偏航角θ
yaw
、纵向位移x、横向位移y和桶滚角θ
roll
，得到精确的自动驾驶车辆平移位姿信息；
[0035]
所述精确的自动驾驶车辆旋转位姿信息和精确的自动驾驶车辆平移位姿信息，即为优化后的自动驾驶车辆的位姿数据，利用优化后的自动驾驶车辆的位姿数据得到优化后的周围环境点云地图。
[0036]
进一步，利用回环检测算法，对回到相同场景的周围环境点云地图进行优化处理，具体包括：
[0037]
利用两帧的所述点云图像视图数据的余弦距离计算两帧点云的相似程度s(pr,pc)；即得到优化后的自动驾驶车辆周围环境点云地图；
[0038][0039]
其中，ns为元区域数目，为参考点云图像视图数据中提取的向量，为待检测点云图像视图数据中提取的向量。
[0040]
使用余弦距离来描述某一元区域下高度值的近似程度，s(pr,pc)的值越小，说明两帧点云越相似。
[0041]
一种多传感器松耦合的室内外同步定位与建图系统，所述系统包括：
[0042]
所述系统包括：
[0043]
一个或多个处理器；
[0044]
存储器，用于存储一个或多个程序；
[0045]
当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器执行如上述任一所述的一种多传感器松耦合的室内外同步定位与建图方法。
[0046]
与现有技术相比，本发明的优点在于：
[0047]
利用激光雷达与惯性测量单元多传感器数据的松耦合，使自动驾驶车辆能够进行高精度的同步定位与建图，室外实验平均轨迹误差为0.06m。
[0048]
利用激光雷达数据帧的平均点云深度对室内、室外环境进行区分，在自动驾驶车辆的行驶路线途径室内、室外环境时，本发明平均轨迹误差为0.95m。
[0049]
利用两步lm优化方法减少计算量，使算法在保持上述精度时，也保证了实时性。
附图说明
[0050]
图1、本发明松耦合与紧耦合示意图；
[0051]
图2、本发明一种多传感器松耦合的室内外同步定位与建图方法的总流程图；
[0052]
图3、本发明建立的室外开阔环境地图；
[0053]
图4、室外开阔环境实验的轨迹；
[0054]
图5、室外开阔环境实验的平均轨迹误差；
[0055]
图6、室内外切换驾驶实验效果图。
具体实施方式
[0056]
下面结合实施例描述本发明具体实施方式：
[0057]
需要说明的是，本说明书所示意的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明可实施的限定条件，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容能涵盖的范围内。
[0058]
同时，本说明书中所引用的如“上”、“下”、“左”、“右”、“中间”及“一”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。
[0059]
实施例1
[0060]
如图1所示，一种多传感器松耦合的室内外同步定位与建图方法，所述方法包括：
[0061]
对点云数据与惯性数据，进行数据预处理，得到点云的图像视图数据和自动驾驶车辆的粗略位姿数据；
[0062]
对所述点云的图像视图数据，进行场景识别、特征提取和拼接处理，得到周围环境未经优化的点云地图；
[0063]
基于场景识别的结果，利用后端优化算法和回环检测算法，对自动驾驶车辆的粗略位姿数据和所述周围环境未经优化的点云地图进行优化处理，得到优化后的自动驾驶车辆的位姿数据和周围环境点云地图，即实现了自动驾驶车辆的同步定位与建图。
[0064]
进一步，通过激光雷达采集点云数据，通过惯性测量单元采集惯性数据。
[0065]
进一步，所述对点云数据进行预处理，具体包括：
[0066]
对所述点云数据进行线束分割，得到点云数据的垂直分割结果；
[0067]
对所述点云数据进行元区域水平分割，得到点云数据的水平分割结果；
[0068]
利用预设的激光雷达的坐标系与车辆质心的位置关系对点云数据的垂直分割结果和水平分割结果进行重投影，得到点云的图像视图数据。
[0069]
进一步，所述对惯性数据进行预处理，具体包括：
[0070]
对所述惯性数据的重力分量进行消除，得到自动驾驶车辆的粗略位姿数据；
[0071][0072]
其中a'
t
是惯性测量单元数据真值,a
t
是惯性测量单元数据测量值,r
zyx
是惯性测量单元数据的旋转矩阵,g是重力加速度。通过对a'
t
的积分，得到自动驾驶车辆的速度与位置。
[0073]
进一步，所述进行场景识别、特征提取和拼接处理，具体包括：
[0074]
利用点云的图像视图数据的平均点云深度da是否超过阈值作为判断条件，对自动驾驶车辆处于室内、室外环境进行区分，得到场景识别结果；
[0075][0076]
其中n是一帧点云中点的总数，di是第i个点相对于激光雷达几何中心的欧氏距离；
[0077]
基于场景识别结果，利用点云的图像视图数据中的点云每点附近曲率c将场景特征分为角特征与平面特征，使用迭代最近点算法配准点云的图像视图数据(多个数据帧的点云)，得到周围环境未经优化的点云地图；
[0078][0079]
定义点云的图像视图数据中同一条水平线上第i个点pi附近的曲率为c，其中a为点pi单侧相邻的点数，pj为与pi相邻的点。
[0080]
进一步，基于场景识别的结果中的室外环境，利用后端优化算法进行处理，具体包括：
[0081]
基于室外环境，利用后端优化算法中的高斯牛顿算法，对自动驾驶车辆的粗略位姿数据和所述周围环境未经优化的点云地图，进行优化处理，得到室外环境优化后的自动驾驶车辆的位姿数据和周围环境点云地图。
[0082]
进一步，基于场景识别的结果中的室内环境，利用后端优化算法中的两步lm算法进行处理。
[0083]
进一步，利用后端优化算法中的两步lm算法进行处理，具体包括：
[0084]
对自动驾驶车辆的粗略位姿数据和所述周围环境未经优化的点云地图利用两步lm优化；
[0085]
第一步lm优化，首先使用提取的所述平面特征计算出六自由度位姿中受旋转影响较大的俯仰角θ
pitch
和高程位移z，以第一步lm优化的计算出结果作为先验信息，即精确的自动驾驶车辆旋转位姿信息；
[0086]
第二步lm优化，计算其余的六自由度位姿的信息偏航角θ
yaw
、纵向位移x、横向位移y和桶滚角θ
roll
，得到精确的自动驾驶车辆平移位姿信息；
[0087]
所述精确的自动驾驶车辆旋转位姿信息和精确的自动驾驶车辆平移位姿信息，即为优化后的自动驾驶车辆的位姿数据，利用优化后的自动驾驶车辆的位姿数据得到优化后的周围环境点云地图。
[0088]
进一步，利用回环检测算法，对回到相同场景的周围环境点云地图进行优化处理，具体包括：
[0089]
利用两帧的所述点云图像视图数据的余弦距离计算两帧点云的相似程度s(pr,pc)；即得到优化后的自动驾驶车辆周围环境点云地图；
[0090][0091]
其中，ns为元区域数目，为参考点云图像视图数据中提取的向量，为待检测点云图像视图数据中提取的向量。
[0092]
使用余弦距离来描述某一元区域下高度值的近似程度，s(pr,pc)的值越小，说明两帧点云越相似。
[0093]
一种多传感器松耦合的室内外同步定位与建图系统，所述系统包括：
[0094]
所述系统包括：
[0095]
一个或多个处理器；
[0096]
存储器，用于存储一个或多个程序；
[0097]
当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器执行如上述任一所述的一种多传感器松耦合的室内外同步定位与建图方法。
[0098]
实施例2
[0099]
本实施例提出一种激光雷达/imu松耦合的同步定位与建图系统；如图2所示。
[0100]
该系统以激光雷达与imu信号为输入，以自动驾驶车辆在未知环境中的位姿与地图为输出。具体包含数据预处理、场景识别、激光里程计、后端优化、回环检测等多个模块。下面对不同模块依次进行介绍。
[0101]
1)数据预处理。
[0102]
该模块主要完成对激光雷达数据的重投影、线束分割以及去畸变操作。由于激光雷达在三维空间中具有稀疏性，随着距离的增加，同一水平线束间的激光点距被拉大。因此使用图像视图将激光雷达点云投影在二维平面上，对数据进行降维，提高数据的储存效率。重投影后的横坐标表示激光雷达同一水平线12
°
范围内的点，将该范围定义为元区域s，则每一帧点云的横坐标有30个元素s0～s
29
；纵坐标表示激光雷达的线数，根据激光雷达型号的不同，线数存在差异，纵坐标元素数量也不同。以velodyne vlp-16激光雷达为例，该产品在垂直方向有16个通道，因此在完成重投影后，纵坐标就有16个元素。部分激光雷达的线数信息是跟随原始数据一起发布的，通常被称为ring通道。对于没有ring通道的激光雷达，可以根据数据点的仰角近似计算线数归属。
[0103]
在重投影后的图像矩阵中，每个元素的数值为当前元区域s范围内激光雷达点的平均高度。经过重投影后，点云的三维信息被保留，同时简化了数据的存储。
[0104]
定义p＝{p1,p2,
…
,pn}为激光雷达在i时刻获取的一帧点云。由于激光雷达频率不高，点云不能在同一时刻被获取。假设车辆以10m/s的速度运动，虽然在激光雷达坐标系中，一帧点云的起始点与结束点是相接的，但是在世界坐标系中，这两点之间会有1m的距离，因此有必要使用高频imu对点云进行去畸变操作。在处理之前，需要先将imu数据的额重力分量消除。
[0105][0106]
其中a'
t
是imu真值,a
t
是imu测量值,r
zyx
是imu的旋转矩阵,g是重力加速度。通过对a'
t
的积分，可以得到自动驾驶车辆的速度与位置。点云去畸变的具体流程为：记录一帧点云的起始时刻ti,t
i+1
，起始、终止时刻位姿ti,t
i+1
，假设自动驾驶车辆在一帧点云采集时间内做匀速运动，使用线性插值将将该时间内所有的点云转换到起始时刻，完成去畸变操作。
[0107]
2)带有场景识别的激光里程计
[0108]
完成数据预处理，激光里程计模块开始执行。该模块是同步定位与建图的核心，主要流程为：存储激光雷达每帧的点云，提取每帧点云的特征点，在帧间相同的特征点间解算位姿变换矩阵，通过该矩阵将多帧点云拼接到一起，形成环境地图。输出车辆的实时位姿与
地图。
[0109]
定义激光雷达平均点云深度da对室内、室外环境进行区分，
[0110][0111]
其中n是一帧点云中点的总数，di是第i个点相对于激光雷达几何中心的欧氏距离。识别出当前环境后，启动特征提取线程，提取用于帧间点云配准的特征点。受启发于文献[2]，定义点云的图像视图数据中同一条水平线上第i个点pi附近的曲率为c。
[0112][0113]
其中a为点pi单侧相邻的点数，pj为与pi相邻的点。通过计算c，可以将点云划分为角特征与平面特征。
[0114]
由于室内环境的地面使用瓷砖铺成，相比室外平整，因此使用两步lm方法进行里程计的优化。与文献[3]类似，第一步lm优化先使用提取的平面特征计算出六自由度位姿中受旋转影响较大的俯仰角θ
pitch
、高程位移z。以第一步lm优化的结果为先验信息，进一步计算其余的六自由度位姿信息偏航角θ
yaw
、纵向位移x、横向位移y、桶滚角θ
roll
。
[0115]
对于室外环境，不能假设地面是水平的，因此当da大于阈值，判断出自动驾驶车辆运行到室外环境后，采用高斯牛顿优化方法进行优化。
[0116]
3)回环检测
[0117]
回环检测的目的是保证在自动驾驶车辆行驶过相同的场景时，能够以该场景作为路标优化已经建立的地图，减小因时间累计而产生的地图漂移误差。
[0118]
图像视图将三维点云数据投影到二维平面上，方面了回环检测。对于参考点云pr、当前点云pc，定义两帧点云的相似程度s(pr,pc)：
[0119][0120]
其中ns为元区域数目，使用余弦距离来描述某一元区域下高度值的近似程度，s(pr,pc)的值越小，说明两帧点云越相似。
[0121]
上述发明在室外开阔环境、室内外切换驾驶环境下均进行了测试，取得了不低于已有同步定位与建图方法的结果。
[0122]
室外开阔环境的实验结果如图3-图5所示。该场景为田径运动场，本次实验全长475m，与gnss的参考数据相比，平均轨迹误差为6.7cm。
[0123]
室内外切换驾驶的实验结果如图6所示。该实验起始于室内房间，以匀速1m/s的速度经过斜坡行驶至室外，围绕建筑行驶一圈后经由斜坡重新回到室内，完成线路闭环。与gnss的参考数据相比，平均轨迹误差为95cm。
[0124]
实施例3
[0125]
在优化方法上，可以使用拟牛顿法代替高斯牛顿法，达到类似效果。
[0126]
在回环检测方法上，可以使用点云虹膜识别方法替代。文献[4]提出了一个应用于激光雷达点云的名为lidar iris的全局描述子，用它来进行快速且精确的闭环检测。在对由lidar-iris表示的图像进行了几次log-gabor滤波和阈值运算之后，可以获得每个点云
的二进制特征图像。给定两帧点云，可以通过从这两帧点云提取的二进制特征图像对应的汉明距离(hamming-distance)计算它们之间的相似度。
[0127]
参考文献(如专利/论文/标准)
[0128]
[1][中国发明,中国发明授权]cn201910297985.1一种用于视觉-惯性-激光融合的同步定位与建图方法
[0129]
[2]zhang j,singh s.loam:lidar odometry and mapping in real-time[r].2014.
[0130]
[3]shan t,englot b.lego-loam:lightweight and ground-optimized lidar odometry and mapping on variable terrain[c/ol]//2018ieee/rsj international conference on intelligent robots and systems(iros).madrid:ieee,2018:4758-4765.
[0131]
[4]wang y,sun z,xu c z,等.lidar iris for loop-closure detection[c/ol]//2020ieee/rsj international conference on intelligent robots and systems(iros).las vegas,nv,usa:ieee,2020:5769-5775.
[0132]
上面对本发明优选实施方式作了详细说明，但是本发明不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。
[0133]
不脱离本发明的构思和范围可以做出许多其他改变和改型。应当理解，本发明不限于特定的实施方式，本发明的范围由所附权利要求限定。