一种基于场景自适应的多模态融合SLAM方法

本发明涉及多模态融合slam方法，尤其是一种基于场景自适应的多模态融合slam方法，属于移动机器人自主定位和导航。

背景技术：

1、同步定位与建图(simultaneous localization andmapping，slam)指机器人通过自身所搭载的传感器对周围环境进行感知，定位自身的位置并绘制周围的环境地图，是机器人实现自主移动的关键技术，通过将机器人所搭载的各个模态传感器进行数据融合，能够实现在复杂环境下的机器人定位和建图任务。因此，多模态融合的slam方法在移动机器人自主定位和导航领域内被广泛使用，其常用的传感器有惯性测量单元(inertialmeasurement unit，imu)、激光雷达(light detectionand ranging，lidar)和相机。

2、目前多模态融合的slam方法主要分为松耦合和紧耦合两种类型，其中紧耦合的方法考虑了来自不同传感器的观测误差，如视觉的特征点重投影误差和激光雷达的“点-点”、“点-线”、“点-面”的匹配误差以及imu的预积分误差。在一般情况下，紧耦合的方法有着较高的准确性和鲁棒性，但是对于极端条件，因为只使用单个位姿估计器，紧耦合方法更容易受到传感器故障的影响。再比如黑暗但是几何特征丰富的情况下，由于视觉的退化，在多传感器融合时会给估计器注入更多的噪声，因此定位精度会低于仅仅使用激光雷达的单一模态slam方法。

3、相比之下，松耦合的方法在这种具有感知挑战性的条件下更有具鲁棒性，其通常是将激光雷达里程计作为主要的位姿估计器，将其它里程计的估计结果视为帧间匹配的初值，但是环境退化引起的初值误差过大会导致算法的精度降低或收敛速度变慢。

4、针对上述问题，亟需研发一种新的多模态融合slam方法，以克服传感器故障或环境退化对多模态融合的slam系统造成的不利影响，提高多模态融合slam算法的可靠性。

技术实现思路

1、为解决背景技术存在的不足，本发明提供一种基于场景自适应的多模态融合slam方法，它对各个模态传感器实时进行可靠性评估，动态确定主/次模态传感器，将次模态传感器的里程计估计结果作为主模态传感器里程计的约束条件，采用因子图优化的方法进行融合，能够提高鲁棒性、可靠性以及精度。

2、为实现上述目的，本发明采取下述技术方案：一种基于场景自适应的多模态融合slam方法，包括以下步骤：

3、步骤一：输入传感器数据

4、输入激光雷达、相机和imu三个模态的传感器数据；

5、步骤二：模态可靠性评估

6、对于激光雷达传感器，设在相邻两帧之间存在对应的平面特征fpl,1:κ，κ表示平面特征数目，平面特征fpl,1:κ的单位法向量为n1:κ，计算二阶矩：

7、

8、将其进行特征值分解：

9、

10、式中，v表示列为特征向量v1,v2,v3的正交方阵，λ表示元素为对应的特征值λ1,λ2,λ3的对角矩阵；

11、特征值降序排序λ1≥λ2≥λ3，定义激光雷达传感器的环境退化系数当ρ<1.0×10-3时，认为环境发生退化导致激光雷达传感器的可靠性降低；

12、对于相机传感器，通过跟踪相邻两帧之间的视觉特征点来估计机器人运动，当跟踪到的视觉特征点的数目小于50对时，认为环境发生退化导致相机传感器的可靠性降低；

13、除了环境退化导致的传感器可靠性降低以外，对于激光雷达、相机和imu的传感器，分别检测三种传感器的输出频率，当输出频率小于1hz时，认为对应的传感器故障导致可靠性降低；

14、步骤三：确定主/次模态传感器

15、基于模态可靠性评估结果，主/次模态传感器的确定方法如下：

16、(1)当激光雷达正常工作时，无论相机或者imu的传感器可靠性降低与否，选择激光雷达传感器作为主模态传感器，其余传感器作为次模态传感器；

17、(2)当激光雷达可靠性降低但相机正常工作时，无论imu可靠性降低与否，选择相机传感器作为主模态传感器，其余传感器作为次模态传感器；

18、(3)当激光雷达和相机的可靠性均降低，但imu正常工作时，选择imu传感器作为主模态传感器，其余传感器作为次模态传感器；

19、步骤四：构建各个模态的里程计系统

20、(1)激光雷达里程计系统：提取当前帧激光雷达点云中的点特征、线特征和面特征，在上一帧激光雷达点云中搜索和当前帧对应的点特征、线特征和面特征，计算这三种特征的匹配误差：

21、

22、式中，i1、i2、i3分别表示当前帧的点特征、线特征和面特征，分别表示当前帧和上一帧之间点特征到点特征的误差、点特征到线特征的误差和点特征到面特征的误差，分别表示和的权重值，通过最小化匹配误差得到当前时刻机器人的位姿估计值；

23、(2)相机里程计系统：在当前k时刻的视觉关键帧mk以及上一时刻的视觉关键帧mk-1中提取视觉特征点，设mk和mk-1之间跟踪成功到的视觉特征点的数目为m，利用imu提供的初始位姿参数tk和相机与imu的外参将mk-1中观测到的视觉特征点pl投影到mk的图像坐标系下，计算视觉特征点的重投影误差：

24、

25、式中，l∈m表示视觉特征点索引，表示mk中与pl对应的视觉特征点的像素坐标，πc表示相机投影模型，表示李群中对旋转向量的变换操作符，通过最小化重投影误差得到当前时刻机器人的位姿估计值；

26、(3)imu里程计系统：对于t时刻的imu数据，定义imu的积分误差如下：

27、

28、式中，iωm、iam分别表示t时刻角速度和线加速度的测量值，和分别表示t-1时刻和t时刻imu的角速度随机游走和加速度随机游走，iω(t)和ia(t)通过以下公式计算：

29、

30、式中，表示t时刻imu相对于机器人初始位置坐标系的姿态，∨表示旋转矩阵从李群到李代数的变换，ga(t)和gg分别表示地球坐标系下的加速度测量值和地球重力加速度值，通过最小化积分误差得到当前时刻机器人的位姿估计值；

31、步骤五：采用因子图优化方法计算机器人当前时刻的最终位姿

32、将由主模态里程计系统得到的当前时刻机器人的位姿估计值作为因子图优化的节点xi，i＝1,2,...,n，n表示窗口中的节点数目，其余两个次模态里程计系统得到的当前时刻机器人的位姿估计值作为节点之间的相对变换因子，构建因子图优化模型，计算机器人当前时刻的最终位姿x*：

33、

34、式中，r0表示主模态里程计的先验因子，和分别表示主模态里程计相对变换因子和次模态里程计相对变换因子，l＝1,2表示次模态传感器的序号，∑λi和分别表示和的协方差，i表示在整个滑动窗口中的索引值；

35、步骤六：计算机器人下一时刻的最终位姿

36、重复执行步骤一至步骤五，即可得到后续机器人下一时刻的最终位姿。

37、与现有技术相比，本发明的有益效果是：本发明通过对机器人所搭载的各个模态传感器进行实时的可靠性评估，动态确定主模态传感器和次模态传感器，并将次模态传感器的里程计估计结果作为主模态传感器里程计的约束条件，采用因子图优化的方法进行融合，克服了因传感器故障或环境退化对多模态融合的slam系统造成的不利影响，提高了多模态融合slam算法的鲁棒性、可靠性以及精度。