一种视觉-红外-惯性融合定位与建图方法

本发明属于即时定位与地图构建，具体涉及一种视觉-红外-惯性融合定位与建图方法。

背景技术：

1、随着机器人技术的迅速发展，自主机器人在工业制造、军事行动、民用生活中得到了广泛的应用，尤其在危险环境中具有广泛的用途和巨大的潜力。自主机器人系统面临的主要挑战之一是需要准确可靠的感知能力。在机器人感知领域中，同步定位与建图(slam)的能力尤为关键，稳定、精确的定位能力是机器人实现规划、决策等上层任务的重要基础，其中视觉惯性slam系统因结构紧凑、成本较低、精度较高而备受关注。

2、传统的作业型机器人技术主要应用于工业生产和简单场景下的移动等，其任务场景相对结构化或半结构化，环境相对稳定。然而在实际应用中，机器人面临着各种复杂和具有挑战性的环境条件，例如在地下环境、工业检测和搜救等复杂和危险的任务中，往往存在缺乏特征、照明不良、光照变化剧烈以及传感器噪声较大等场景，在这些视觉退化环境下，传统的视觉slam系统可能会出现性能明显下降甚至无法正常工作的情况，例如机器人定位误差增大、地图建立不准确乃至定位系统崩溃等问题，从而影响机器人的导航、路径规划和任务执行等功能。综上所述，视觉退化环境下的定位和建图仍然是一个具有挑战性的问题。

3、在光照变化、视觉遮挡(烟雾、灰尘等)等视觉退化环境下，现有的视觉slam系统的定位精度和稳定性往往会大幅下降，但热红外相机对光照变化不敏感，能够接受穿透烟雾的红外辐射，在视觉退化环境下较视觉相机有明显的优势。针对上述现象，一些工作将热红外相机获取温度辐射图像(以下称为红外图像)应用于传统视觉slam系统中，以克服视觉退化环境带来的挑战。此外，近年来许多基于多外传感器融合的slam框架被相继提出，多传感器融合slam系统根据不同外传感器组合以及惯性测量单元(imu)的协助，设计不同的多传感器数据融合框架。相较于单一外传感器的方法，这些框架不仅能够提升定位的精度，还能够提升系统在各种视觉退化环境中的稳定性，拓宽slam系统的应用场景。

4、尽管红外slam系统与多传感器融合slam系统能一定程度上克服视觉退化环境的影响，但在精度与稳定性的平衡、系统结构等方面仍不够完善。红外相机虽然在光照动态变化乃至无照明的环境下成像稳定，具有较高的一致性，但存在低信噪比、高温飘的问题，导致红外slam系统的定位精度较低。在目前一些多传感器融合的slam系统中，紧耦合的方案能够最大限度利用所有传感器的测量数据，能够实现更高的定位精度，但在面对复杂的感知退化环境时很容易引入额外的观测误差，进而直接传播到整个系统中，导致系统整体性能下降甚至崩溃。与紧耦合方案对应的是松耦合方案，由于各模态之间完全隔离或并不完全整合，在复杂的感知退化场景中具备更强的鲁棒性，但融合的结果在良好的环境中无法达到紧耦合系统的精度。一些配备了多种传感器的冗余slam系统能够克服各种复杂的感知退化场景，但其系统过于庞大，成本高昂，且需要经过细致的参数调整，可复现性不高。综上所述，目前的红外slam系统和多传感器slam系统在精度和鲁棒性的平衡上并不完善，部分系统过于冗余、庞大，难以调试，本发明正是根据这些需求提出了面向视觉退化环境的视觉-红外-惯性融合定位与建图技术。

5、热红外相机获取的是14或16位温度辐射图像，对比度较低，且存在噪声累计的问题，常见的解决方案是将14或16位热红外图像缩放为8位，以增强热红外图像的对比度，提高特征点提取和匹配的性能，再沿用视觉slam系统进行状态估计，发表于2016年《ieeetransactions on intelligent transportation systems》的文章“practical infraredvisual odometry”即采用了该方案。由于缩放热红外图像数据会导致信息丢失，放大噪声和图像饱和的效果，发表于2020年《journal of field robotics》的文章“keyframe-basedthermal–inertial odometry”对传统视觉slam系统进行改进，选择直接使用原始的红外图像进行运动估计，以避免由于缩放操作导致图像纹理的显著变化。《一种单目热成像同时定位与建图方法和系统》(公开号：cn117036404a)使用了采集器和处理器两个模块，采集器采集不受非均匀性校正中断的红外图像序列，处理器模块对采集到的图片进行实时去噪，能有效提高红外图像的信噪比，再进行图像中点特征和线特征的提取和跟踪，构建并求解非线性最小二乘问题，得到位姿估计结果，拓宽了传统视觉slam在红外图像上的使用边界。尽管红外slam系统能有效应对视觉退化环境，但在正常环境中的精度仍然难以赶超传统视觉slam系统，限制了其应用范围。

6、发表于2022年《ieee international conference on intelligent robots andsystems》的文章“mimosa:a multi-modal slam framework for resilient autonomyagainst sensor degradation”是目前最先进的松耦合多传感器融合slam系统之一，mimosa使用非线性因子图作为底层表示，支持激光雷达点云、视觉里程计、红外里程计和imu4种模态的输入，通过健康检测自动决定是否将该模态信息纳入因子图，以实现在感知退化的环境中的鲁棒定位。mimosa是一个高灵活度、强鲁棒性的多传感器融合slam系统，但其健康检测方法局限于协方差矩阵和特征值分析，这些方法对环境变化的响应不够迅速，定位和建图的精度比较有限。

7、《基于红外视觉、毫米波雷达和imu融合的定位与建图方法及系统》(公开号：cn117330052a)。该发明先通过融合红外相机和imu的传感器数据，构造非线性优化问题得到第一里程计，再通过4d毫米波雷达的自我运动估计和位姿解算模块得到第二里程计和三维体速度，最后基于图优化模型融合第一里程计、第二里程计和三维体速度，优化求解出最优里程计的信息，可以在光照条件不足等视觉退化环境中表现出良好的鲁棒性。红外相机和4d毫米波雷达在视觉退化环境中具有良好的鲁棒性，但其传感器数据具有低信噪比的缺点，精度上难以与视觉相机和激光雷达匹配，且该方法在基于因子图融合多种数据时，没有考虑到环境对传感器性能的影响，难以合理地分配各种测量的权重，可能导致系统的精度下降。

技术实现思路

1、本发明的目的在于提供一种视觉-红外-惯性融合定位与建图方法，以解决上述技术问题，在视觉退化环境中的精度和鲁棒性上都有较好的表现，同时能够兼顾系统在视觉退化环境中的稳定性和在正常环境中的精度。

2、为实现上述发明目的，本发明采取的技术方案如下：

3、一种视觉-红外-惯性融合定位与建图方法，包括如下步骤：

4、s100、获取rgb图像和红外图像以及imu数据，得到vio的估计结果以及tio的估计结果，且对所获取的rgb图像以及红外图像分别进行图像质量评估；

5、s200、以图像质量评估结果分别对vio的估计结果、tio的估计结果添加置信度并输出；

6、s300、设置融合vio的估计结果、tio的估计结果以及imu数据的因子图管理模块，带置信度的vio的估计结果和tio的估计结果以及imu数据输入到因子图管理模块中以松耦合方式进行融合，得到融合后的位姿估计。

7、优选的，所述图像质量评估根据梯度指标以及噪声指标综合评估。

8、优选的，所述梯度指标的定义为：

9、

10、其中，g代表图像的梯度指标，nt为图像的总像素点数，ne为边缘像素点数，λ是调整指标变化速率的参数。

11、优选的，所述噪声指标中，以噪声的模型是加性零均值的高斯噪声为基础，图像中每一个像素的噪声具有标准差δn；使用掩模m作为噪声评估算子，以判断均匀区域和不饱和区域的算子h(i)和子u(i)结合掩模m得到噪声指标，其中噪声标准差δn的估计公式如下所示：

12、

13、其中，ns为经过后的有效像素数量；|i*m|(i)为图像经掩模卷积后取绝对值后的结果。

14、优选的，判断均匀区域和不饱和区域的算子h(i)和子u(i)定义为：

15、

16、

17、其中，gi为梯度图像第i个像素的梯度值，σ为梯度强度阈值；i(i)为原始图像的第i个像素值，εl和εu分别为饱和度上界和下界。

18、优选的，所述掩模m为：

19、

20、优选的，所述s300中，

21、还包括s301、所述vio的估计结果以及tio的估计结果、imu数据均包括时间戳，对vio、tio、imu的时间戳进行时间同步处理；

22、当新的一帧tio估计twt，k+1到来时，以相邻两帧tio的时间戳为基准对数据进行打包，打包得到的一组测量包括以下数据：

23、

24、其中vio对应的位姿twv、tio对应的位姿twt、imu测量值mi对应时间戳分别为tv，tt，ti，twv，k，twv，k+1的时间戳满足为vio同步到的插值结果；的时间戳满足用于imu预积分计算；包打包完成后被传入到因子图管理模块。

25、优选的，所述因子图管理模块包括vio中间因子、tio中间因子、imu预积分因子；

26、且根据置信度调整vio和tio在因子图融合中的权重，其中系统状态x的定义为：

27、x＝[r，p，v，b]t，

28、其中，r∈so(3)为旋转矩阵，p∈r3为机器人的坐标，v∈r3为机器人的速度，b∈r6为imu的偏置；

29、分别计算vio中间因子的残差、tio中间因子的残差、imu预积分因子的残差以及对应的协方差矩阵；根据vio中间因子的残差、tio中间因子的残差、imu预积分因子的残差值以及协方差矩阵创建最小二乘问题：

30、

31、imu预积分因子残差vio中间因子残差tio中间因子残差对应的协方差矩阵分别为∑i，∑v，∑t，

32、并基于gtsam库实现因子图的构建，并使用isam2对上述最小二乘问题进行优化求解，得到融合后的位姿估计。

33、优选的，其中vio中间因子和tio中间因子，通过梯度指标g和噪声指标δn检测环境的变化，自适应地设置协方差矩阵的大小，从而调整其在优化问题中的权重，其中协方差矩阵的计算公式为：

34、

35、∑＝ω∑base；

36、其中，ω为协方差权重参数；δn、g分别为噪声指标和梯度指标；λn用于调节ω的数量级，以适配不同的imu噪声大小；λg用于平衡噪声指标和梯度指标对ω的影响，λg越大，梯度指标对ω的影响越大；∑为残差对应的协方差矩阵；∑base为基础协方差矩阵，设定为对角常数矩阵；

37、通过误差传播计算出imu预积分因子的协方差矩阵。

38、优选的，其中因子图的频率与tio同步，

39、vio中间因子构造：

40、使用插值函数finter和中twv，k，twv，k+1计算时刻的vio估计值

41、

42、插值方式选择的是四元数的球面线性插值；vio中间因子通过计算和之间的相对位姿变换得到：

43、

44、进而，vio中间因子的残差可被计算得：

45、

46、其中，tk，tk+1∈se(3)为因子图节点状态xk，xk+1中对应的分量；

47、tio中间因子构造：

48、tio中间因子以twt，k和twt，k+1之间相对变换的形式得到：

49、

50、进而，tio中间因子的残差可被计算得：

51、

52、本技术取得了有益的技术效果：

53、本发明中，vio以及tio均引入基于图像质量评估的置信度，在传统基于因子图的松耦合融合框架中添加图像质量评估和根据置信度以实现自适应权重调整因子图管理模块，在视觉退化环境中的精度和鲁棒性上都有较好的表现，同时能够兼顾系统在视觉退化环境中的稳定性和在正常环境中的精度。