一种基于激光与视觉异步融合的SLAM算法

一种基于激光与视觉异步融合的slam算法
技术领域
1.本发明涉及机器人定位导航技术领域，具体涉及一种基于激光与视觉异步融合的slam算法。


背景技术：

2.移动机器人自主定位与导航能力是助行机器人的关键，而准确的地图是助行机器人自主定位与导航的前提。
3.在过去的几十年里，移动机器人slam得到了快速的发展。激光传感器与视觉传感器是slam应用中的两种主流传感器。其中以激光传感器为代表的slam算法，主要有gmapping算法、hector算法以及cartographer算法等。虽然仅依靠2d激光传感器就可以实现建图，但是由于2d激光传感器只具有深度信息，并且扫描的是一个平面，所以当机器人在长廊的运动过程中传感器返回的深度信息不随时间变化时，算法将会失效。而相机作为另一个廉价的传感器，在这种情况下却具有明显的优势，一方面可以获取一些立体空间上显著的角点或边缘信息。另一方面，相机也因具有更高的采样速率而被广泛应用。
4.由于每种传感器都有自身的一些限制，近年来，研究者们也开发了许多基于多传感器融合的slam建图算法。胡立鹤等人公开了一种多传感器融合slam技术的回收机器人，通过设计包含控制器和slam系统的机器人控制系统，并融合超声波避障传感器、红外传感器和雷达传感器采集的外界环境数据，实现回收机器人的自动避障功能。通过摄像组件对定位标识实时扫描，实现回收机器人的精准定位。吴叶兰等人提供了一种基于多传感器融合的slam定位导航方法及系统，通过融合深度相机、激光雷达和全向轮里程计信息用于导航，并通过改进贝叶斯数据算法的融合方法，最终可以实现移动机器人的多传感器辅助自动定位、全局二维栅格地图构建和避障功能，从而实现机器人的自主导航。尽管如此，视觉和激光相融合的算法却非常少，如何将两种具有很强互补优势的传感器融合实现高效精确的建图和导航是一件非常有意义的工作。


技术实现要素：

5.为了克服现有技术的不足，本发明基于2d激光和双目相机，提出了一种加权异步融合算法。传统的融合方式是在得到两个传感器的最优估计后对两者进行融合，但是两个传感器速率通常并不相同。如果采用时间戳对齐方式，则会导致较快的传感器中很多数据不能得到很好的利用。因此本发明采用了一种异步融合方式对来自两个传感器的数据进行融合。本算法考虑了不同传感器的采集频率特性，并使不同传感器在不同运行状态下具有不同的置信度。同时，将视觉传感器姿态估计作为激光传感器姿态估计的先验，减少迭代次数，提高算法的效率。最后，也通过仿真和实验证明了算法的有效性，实验表明，本发明提出的算法具有很好的精度和鲁棒性。
6.本发明的技术方案如下：
7.一种基于激光与视觉异步融合的slam算法，包括以下步骤：
8.步骤1)对于由视觉传感器反馈得到的位姿增量，根据相机小孔成像原理以及刚体变换公式，假设空间点p在世界坐标系下的齐次坐标为p＝(xw,yw,zw,1)
t
。则其在相机的像素坐标系下的坐标为：
[0009][0010]
其中(u,v)为点p在像素坐标系下的坐标值；s为点p在相机坐标系下的深度值；k为相机的内参矩阵；r为旋转矩阵；t为平移矩阵。
[0011]
步骤2)通过orb特征提取算法可以得到当前帧中图像的特征点，将rbrief描述符得到的当前帧中特征点的描述符与上一时刻图像中的特征点进行描述符之间的匹配，建立对应的匹配关系。将上一时刻的匹配点通过相机的内参矩阵以及初始位姿投影到当前帧图像中。由于相机的位姿通常存在一定偏差，所以投影点与当前地图上的匹配点之间通常会存在一定偏差。假设特征点p在当前帧图像中的坐标为p＝(u1,v1)
t
，对应上一帧中的匹配点在当前帧中的匹配点坐标为p’＝(u2,v2)
t
。通常由于相机位姿的偏差导致这两个点的坐标并不相同，因此定义误差函数：
[0012][0013]
其中e是误差值，(u,v)是当前帧中特征点的像素坐标，p是当前帧特征点对应的匹配点。将其改写为李代数形式：
[0014][0015]
其中exp(δ)为相机坐标系的李代数表达式，e为空间点p投影到当前图像中与匹配点之间的投影误差。
[0016]
通常一帧图像中会存在若干个匹配点，根据所有的匹配点可以得到投影误差函数：
[0017][0018]
其中ui表示当前帧中第i个特征点；pi表示其对应的匹配点的世界坐标；si表示该点的深度；k表示相机的内参矩阵；exp(δ)表示相机坐标系的李代数表达式。
[0019]
步骤3)利用高斯牛顿法或者列文伯格-马夸尔特方法求解该优化方程，得到位姿估计为：
[0020][0021]
其中z1(k1)为视觉传感器在k1时刻的最优估计，r为旋转矩阵，t为平移矩阵。v1(k1)为相机估计噪声且假设服从高斯分布。
[0022]
步骤4)假设k2时刻得到的激光传感器数据为令si(δ2)为世界坐标，m
(si(δ2))为坐标点si(δ2)的占用概率，为m(si(δ2))的导数。m(si(δ2))与的值可通过对世界坐标si(δ2)进行双线性插值求得。那么机器人从上一时刻到当前时刻的位姿增量可由下式给出：
[0023][0024]
其中
[0025][0026]
步骤5)根据位姿增量可以得到机器人在激光传感器位姿估计下的坐标为:
[0027]
z2(k2)＝z2(k
2-1) δδ2 v2(k2)
ꢀꢀꢀ
(8)
[0028]
其中z2(k2)为激光传感器在k2时刻的最优位姿估计；z2(k
2-1)为其在上一时刻的最优位姿估计；v2(k2)为观测噪声且假设服从高斯分布，满足如下关系：
[0029][0030]
步骤6)传统的融合方式是在得到两个传感器的最优估计后对两者进行融合，但是两个传感器速率通常并不相同，如果采用时间戳对齐方式，则会导致较快的传感器中很多数据不能得到很好的利用。因此本发明采用了一种异步融合方式对来自两个传感器的数据进行融合。
[0031]
首先将传感器的数据按较快的传感器(仿真相机传感器)采样频率划分为m个采样区间，每个采样区间内会存在两种情况：
[0032]
1)只拥有相机的位姿估计；
[0033]
2)同时拥有来自激光传感器和视觉传感器的位姿估计。
[0034]
结合kalman滤波器及多传感器异步融合的方法，得到了改进后的基于激光与视觉异步融合位姿估计算法如下：
[0035][0036]
其中为根据上一时刻机器人位姿的最优估计得到的预测位姿；p0(k|k)为预测位姿的协方差矩阵；zi(ki)为k时刻来自视觉或者激光传感器的最优位姿估计。
[0037]
当采样区间内只有相机的数据时，i＝1，此时式(10)变为：
[0038][0039]
此时估计模型转变为以较快的传感器采样频率下对单个传感器数据进行位姿估计的kalman滤波器。
[0040]
当采样区间内同时存在相机的数据以及激光传感器的位姿估计结果时，此时式(10)变为式(12)、(13)的形式：
[0041][0042][0043]
此时估计模型变为以较慢的传感器(激光雷达)采样速率下对激光传感器与视觉传感器进行融合估计的kalman滤波算法。算法以上一时刻的最优估计作为预测量，首先融合当前时刻传感器1(相机)估计的位姿，然后再融合传感器2(激光传感器)估计的位姿结果，最终以两次融合后的传感器估计值作为最终位姿估计的结果。
[0044]
步骤7)为了有效利用每个传感器不同的特性，根据传感器的这种特性引入置信度概念。为了降低机器人在长廊行走时激光传感器的数据偏差带来的影响，同时减少机器人在转向时由于相机漂移导致的位姿漂移问题。
[0045]
使用置信度w表示对两个传感器位姿估计的置信度，将式(10)改为：
[0046][0047]
其中wi代表第i个传感器的位姿估计置信度。
[0048]
步骤8)传感器的特性通常与机器人运动过程相关，当机器人直线行走时激光传感器往往会出现定位问题，当转弯时相机往往会发生漂移现象，因此wi应与机器人的运动状态相关。当机器人运动沿直线行驶时，算法增大对视觉的置信度；当机器人出现旋转时，算法增大对激光的置信度。
[0049]
每次融合时算法会根据激光传感器最新的位姿估计判断机器人当前运动过程中是否发生旋转。其由式(6)得到，这里记为δα2。
[0050]
将角度变化量代入wi得到：
[0051][0052]
其中δα2为上一时刻到当前时刻估计的机器人旋转角度。根据当前机器人运行效果，每个采样周期内机器人旋转角度不超过0.25rad，因此式(15)修改为：
[0053][0054]
将式(16)代入式(14)得到：
[0055][0056]
[0057]
本发明的有益效果如下：
[0058]
该方法首先提高了视觉传感器的数据利用率，其次利用了不同传感器的不同特性，算法在不同条件下对不同传感器具有更好的利用率，使系统跟踪效果更准确。
附图说明
[0059]
图1为本发明gazebo仿真环境示意图；
[0060]
图2为本发明异步kalman滤波及加权算法得到的建图结果示意图；
[0061]
图3为本发明加权异步融合kalman滤波算法得到的融合位姿估计与真实机器人运动轨迹对比示意图；
[0062]
图4为本发明真实运动场景示意图；
[0063]
图5为本发明加权异步融合算法得到的建图结果示意图；
[0064]
图6为本发明加权异步融合算法得到的轨迹结果示意图；
[0065]
图7为本发明的流程图。
具体实施方式
[0066]
下面结合附图对本发明进一步详细说明。
[0067]
参照图1-7，一种基于激光与视觉异步融合的slam算法，本专利的算法实施流程步骤如下：
[0068]
步骤1)为了验证算法的有效性，分别在gazebo仿真环境以及真实环境中进行实验。如图1所示为随机搭建的gazebo仿真环境。其中仿真环境为一个16m
×
16m大小的回形空间，该空间大部分环境为正常的可建图环境，但最下方存在的一段长走廊为退化环境。在该段走廊环境中，激光将得到较少的特征信息。机器人模型上方安装了一个仿真lidar和一个双目相机，其中lidar调用的是libgazebo_ros_laser.so插件：扫描最大距离为6m，最大扫描角度为4rad(约230
°
)，模拟噪声是均值为0，方差是0.01的高斯噪声。相机模型调用的是libgazebo_ros_multicamera.so插件：水平视场角1.39rad，图像大小为800
×
800正方形，模拟噪声是均值为0，方差是0.007的高斯噪声。
[0069]
步骤2)使用视觉位姿估计与激光位姿估计结果采用异步kalman滤波以及加权算法在该仿真地图中进行2d slam建图，得到建图结果如图2。
[0070]
步骤3)图3为视觉位姿估计与激光位姿估计采用加权异步融合kalman滤波算法得到的融合位姿估计与真实机器人运动轨迹对比。由图3可知，该算法的位姿估计曲线与实际的机器人运动曲线拟合度极高，机器人对自身的定位精度更加准确。
[0071]
步骤4)图4所示的场景为建图使用的真实场景，结构为一个梯形场景的办公大楼的普通一层。中间存在部分区域为退化场景，机器人在该场景中运动并建图。该机器人控制器使用的是jetson tx2，上面安装了ubuntu16.04系统以及kinetic版本的ros系统。小车上方安装的是hokuyo公司生产的urg-04lx-ug012d激光扫描测距仪。其测量距离为5.6米，测量角度240度，更新频率10hz。激光传感器正上方安装了kinect_v1 rgb_d相机，相机颜色分辨率为640
×
480，深度图分辨率为320
×
240，更新频率为30hz。水平视场角与垂直视场角分别为57
°
和43
°
。机器人的底盘安装了一对差速驱动轮组。使用ros的socket_can节点与电机驱动器通讯，通过键盘控制节点控制机器人运动。
[0072]
步骤5)采用加权异步kalman滤波算法下的地图如图5所示，机器人运动轨迹如图6所示。建图结果表明该算法效果极好。从位姿估计轨迹可以看出加权异步kalman滤波算法的轨迹估计起点与终点基本一致，说明本发明提出的算法的定位精度误差极小。证明本发明提出的加权异步多速率融合算法的有效性。
[0073]
上述实施例仅代表了本发明的一种实施方式，并不能理解为对本发明范围的限制。应当指出的是在本领域的普通技术人员，在不脱离本发明构思的前提下，还可以做出若干种变形设计，这些都属于本发明的保护范围。