一种基于环境感知的机器人高精度定位方法及系统与流程

本发明涉及基于环境感知的机器人高精度定位领域，尤其涉及一种基于环境感知的机器人高精度定位方法及系统。

背景技术：

1、在工业巡检领域，移动机器人通常采用视觉传感器、惯性测量单元等环境感知手段进行实时位姿估计，以实现复杂厂房内的自主导航。正常情况下，视觉惯性紧耦合滤波器通过联合优化位姿来维持稳定的定位结果。然而，当机器人行驶至高对比度光照区域，如厂房顶部排烟天窗下方时，阳光直射与阴影交替会导致图像序列出现大面积过曝与欠曝。此时，视觉前端特征匹配内点数量骤降，系统通常会判定视觉观测质量低于阈值，并自动切换至纯惯性递推的降级鲁棒模式。

2、在纯惯性递推降级期间，机器人会不可避免地累积位置和航向误差。当机器人驶离天窗区域，光照恢复后，视觉特征点重新稳定出现，系统检测到视觉观测有效便会切换回正常视觉惯性融合模式。此时，视觉里程计首次给出的绝对位姿估计与当前惯性递推预测值之间会存在显著差异，导致残差超过滤波器设置的检验门限。现有常规切换逻辑往往仅关注视觉观测是否连续稳定，而直接更改滤波器协方差矩阵与状态向量结构，将视觉残差以全权重注入滤波器，并未考虑退化期间累积误差对状态过渡的影响。

3、这种处理方式会将视觉残差解释为一次大幅度测量更新，导致融合滤波器直接输出一个阶跃性的定位跳变，将机器人位姿强行拉向视觉给出的位置。该跳变会使导航模块感知到机器人位姿发生瞬间突变，进而计算出大幅度的纠偏指令，造成机器人在平滑地面上突然转向，最终导致巡检定点停靠判断失准。因此，在移动机器人驶出光照剧烈变化区域后恢复视觉惯性紧耦合融合定位时，如何避免降级惯性递推累积的位姿漂移与恢复瞬间视觉观测给出的绝对位姿在融合滤波器残差检验门限处产生阶跃冲突，防止滤波器直接输出定位跳变干扰导航控制精度和巡检定点停靠任务，是该场景下亟待解决的具体技术问题。

技术实现思路

1、本技术公开了一种基于环境感知的机器人高精度定位方法及系统，旨在解决移动机器人在光照剧烈变化区域后恢复视觉惯性紧耦合融合定位时，降级惯性递推累积的位姿漂移与恢复瞬间视觉观测给出的绝对位姿在融合滤波器残差检验门限处产生阶跃冲突，导致滤波器直接输出定位跳变干扰导航控制精度和巡检定点停靠任务的问题。

2、第一方面，本技术公开了一种基于环境感知的机器人高精度定位方法，包括以下步骤：

3、获取机器人的视觉观测模块提供的第一位姿信息，以及惯性递推模块提供的第二位姿信息；

4、计算第一位姿信息与第二位姿信息之间的差异信息；

5、获取惯性递推模块状态的第一不确定度信息，以及视觉观测模块的第二不确定度信息，并基于第一不确定度信息和第二不确定度信息，生成差异信息的不确定度信息；第一不确定度信息反映了惯性递推模块在无校正下累积的误差范围；第二不确定度信息反映了视觉观测模块自身在当前环境下的测量噪声水平；

6、基于差异信息、差异信息的不确定度信息，以及系统处于纯惯性递推模式的持续时间，判断差异信息的来源类型是否为惯性漂移累积类型；

7、当差异信息的来源类型为惯性漂移累积类型时，确定机器人底盘输出的运动学参数的承接能力参数；

8、基于差异信息和承接能力参数，确定机器人的运动状态的调整参数；

9、根据调整参数，形成渐进式的状态校正过程。

10、可选的，计算第一位姿信息与第二位姿信息之间的差异信息，包括：

11、计算第一位姿信息与第二位姿信息之间的三维平移向量差以及通过四元数运算得到的旋转向量差；

12、将三维平移向量差和旋转向量差组合形成残差向量，并将残差向量作为差异信息。

13、可选的，基于差异信息、差异信息的不确定度信息，以及系统处于纯惯性递推模式的持续时间，判断差异信息的来源类型是否为惯性漂移累积类型，包括：

14、基于差异信息、差异信息的不确定度信息，确定马氏距离；

15、在满足第一预设条件的情况下，判断差异信息的来源类型为惯性漂移累积类型；第一预设条件为马氏距离处于预设距离区间内，且残差向量与系统处于纯惯性递推模式的持续时间的比值处于预设比值范围内；

16、在不满足第一预设条件的情况下，判断差异信息的来源类型不为惯性漂移累积类型。

17、可选的，基于差异信息、差异信息的不确定度信息，确定马氏距离，包括：

18、基于差异信息、差异信息的不确定度信息，确定残差向量的预测协方差矩阵；

19、计算残差向量的转置矩阵、预测协方差矩阵的逆矩阵、残差向量三者的乘积，得到马氏距离。

20、可选的，根据调整参数，形成渐进式的状态校正过程，包括：

21、基于第二不确定度信息和调整参数，生成调整后的第二不确定度信息；

22、基于第一不确定度信息、以及调整后的第二不确定度信息，计算更新的增益参数；

23、基于更新的增益参数和差异信息，生成运动状态的修正量，并更新机器人的运动状态；

24、在多个处理周期内，重复执行机器人的运动状态更新，以形成渐进式的状态校正过程。

25、可选的，方法还包括：

26、获取渐进式的状态校正过程中的更新状态序列，并基于更新状态序列生成运动学参数，校验运动学参数是否符合预设的物理约束；

27、当运动学参数不符合物理约束时，限制运动状态更新的幅度；

28、当差异信息和第二不确定度信息均满足预设的收敛条件时，结束渐进式的状态校正过程。

29、可选的，基于第二不确定度信息和调整参数，生成调整后的第二不确定度信息，包括：

30、对视觉观测模块输出的图像的局部区域进行光照变化分析，识别局部光照变化区域；

31、评估局部光照变化区域对视觉观测模块位姿解算精度的影响；

32、根据评估结果和调整参数，生成调整后的第二不确定度信息。

33、可选的，确定机器人底盘输出的运动学参数的承接能力参数，包括：

34、获取机器人底盘的负载信息、机器人底盘的车轮与地面之间的摩擦力信息、机器人底盘的电池的能量状态信息；

35、根据负载信息、摩擦力信息以及能量状态信息，确定承接能力参数。

36、可选的，根据负载信息、摩擦力信息以及能量状态信息，确定承接能力参数，包括：

37、获取第一映射关系；第一映射关系包括不同负载信息、不同摩擦力信息、不同能量状态信息，与不同承接能力参数之间的对应关系；

38、根据第一映射关系、负载信息、摩擦力信息以及能量状态信息，确定承接能力参数。

39、第二方面，本技术还公开了一种基于环境感知的机器人高精度定位系统，该系统包括：

40、信息获取模块，用于获取机器人的视觉观测模块提供的第一位姿信息，以及惯性递推模块提供的第二位姿信息；

41、差异计算模块，用于计算第一位姿信息与第二位姿信息之间的差异信息；

42、不确定度生成模块，用于获取惯性递推模块状态的第一不确定度信息，以及视觉观测模块的第二不确定度信息，并基于第一不确定度信息和第二不确定度信息，生成差异信息的不确定度信息；第一不确定度信息反映了惯性递推模块在无校正下累积的误差范围；第二不确定度信息反映了视觉观测模块自身在当前环境下的测量噪声水平；

43、来源判断模块，用于基于差异信息、差异信息的不确定度信息，以及系统处于纯惯性递推模式的持续时间，判断差异信息的来源类型是否为惯性漂移累积类型；

44、确定模块，用于当差异信息的来源类型为惯性漂移累积类型时，确定机器人底盘输出的运动学参数的承接能力参数；

45、确定模块，还用于基于差异信息和承接能力参数，确定机器人的运动状态的调整参数；

46、渐进校正模块，用于根据调整参数，形成渐进式的状态校正过程。

47、有益效果：本技术公开的一种基于环境感知的机器人高精度定位方法，通过获取机器人的视觉观测模块提供的第一位姿信息和惯性递推模块提供的第二位姿信息，并计算两者之间的差异信息。同时，获取惯性递推模块和视觉观测模块的不确定度信息，并基于这些不确定度信息生成差异信息的不确定度信息。在此基础上，结合差异信息、差异信息的不确定度信息以及系统处于纯惯性递推模式的持续时间，判断差异信息的来源类型是否为惯性漂移累积类型。当判断为惯性漂移累积类型时，进一步确定机器人底盘输出的运动学参数的承接能力参数，并基于差异信息和承接能力参数确定机器人的运动状态的调整参数，最终根据调整参数形成渐进式的状态校正过程。

48、通过上述技术方案，本技术能够有效解决现有技术中在机器人驶出光照剧烈变化区域后，恢复视觉惯性紧耦合融合定位时，降级惯性递推累积的位姿漂移与恢复瞬间视觉观测给出的绝对位姿在融合滤波器残差检验门限处产生阶跃冲突，导致滤波器直接输出定位跳变干扰导航控制精度和巡检定点停靠任务的问题。本技术通过引入对差异信息来源类型的判断，并结合机器人底盘的承接能力参数，实现了对机器人运动状态的渐进式校正，避免了传统方法中直接将视觉残差以全权重注入滤波器导致的阶跃性定位跳变。这种渐进式的校正过程使得机器人位姿调整更加平滑和稳定，有效防止了对导航控制的干扰，从而提高了机器人的定位精度和导航稳定性，尤其是在复杂光照环境下，能够保证巡检定点停靠任务的准确性，具有显著的优异技术效果。