一种基于IMU和激光里程计融合的地图更新方法及系统

本发明涉及移动机器人环境建图与地图维护，尤其涉及一种基于imu和激光里程计融合的地图更新方法、系统和移动机器人。

背景技术：

1、传统的基于先验栅格地图的同时定位与地图构建（slam）技术是移动机器人自主导航的核心基础。在长期运行场景中，环境中的半静态物体（如设备、工位、堆放物）可能发生位置变化，导致先验栅格地图与真实环境状态不再一致。当机器人继续依赖旧地图进行定位与导航时，观测信息将与地图模型产生几何不一致，从而引发定位误差甚至任务失败。因此，需要构建一种能够实时、准确地识别环境结构变化并更新地图的机制。

2、现有基于激光雷达的地图更新方法通常通过将当前激光观测与先验栅格地图进行对比来检测变化。然而，这类方法在实际应用中存在以下问题：

3、（1）对位姿精度依赖较强：激光观测与地图的一致性分析高度依赖于机器人当前位姿估计的准确性。位姿误差会导致射线投影偏差，从而产生虚假的变化检测；

4、（2）异常观测易引发误更新：传感器噪声、动态物体短时遮挡等异常观测若被直接用于地图更新，极易引入错误约束，导致地图结构震荡；

5、（3）对剧烈运动敏感：当机器人执行急转弯或原地旋转等动作时，即使环境未发生改变，射线投影误差仍可能因角度误差放大而显著增大，导致大量虚假异常被误判为环境变化；

6、（4）单一传感器存在固有局限：激光雷达更新频率低，在特征退化环境中易失稳；imu（惯性测量单元）存在零偏漂移，积分误差随时间累积发散。

7、因此，亟需一种能够在复杂运动条件下稳定、准确地检测真实环境变化并更新地图的方法和系统，以克服现有技术的不足。

技术实现思路

1、本发明旨在提供一种基于imu和激光里程计融合的地图更新方法及系统，以解决现有地图更新方法在机器人剧烈运动时易产生虚假更新、难以区分真实环境变化与运动干扰的技术问题。

2、为实现上述目的，本技术提出一种基于imu和激光里程计融合的地图更新方法，包括以下步骤：

3、步骤s1：获取机器人的当前时刻的激光雷达扫描数据、imu测量数据以及先验栅格地图；

4、步骤s2：基于扩展卡尔曼滤波器ekf，融合所述imu测量数据和由激光雷达扫描数据得到的激光里程计数据，预估所述机器人在当前时刻的位姿和运动状态，所述运动状态至少包括角速度；

5、步骤s3：根据所述步骤s2预估的角速度，计算一个用于控制地图更新强度的运动门控因子；

6、步骤s4：基于所述步骤s2预估的位姿，将所述激光雷达扫描数据与所述先验栅格地图进行观测一致性分析，生成候选变化栅格集合；

7、步骤s5：利用所述步骤s3计算的运动门控因子，对所述候选变化栅格集合中的栅格执行加权累积更新，并对未进入所述候选变化栅格集合的栅格执行衰减更新，以获得各栅格对应的变化累积值；

8、步骤s6：当任一栅格的变化累积值达到预设阈值时，判定该栅格对应环境发生真实变化，并据此更新先验栅格地图中对应栅格的占用状态。

9、进一步地，所述步骤s3中根据预估的角速度计算运动门控因子，具体包括：设定角速度阈值，当估计得到的角速度的绝对值大于设定角速度阈值时，将运动门控因子设置为0，此时不执行对候选变化栅格的累积计数、计数衰减及地图更新操作。

10、进一步地，所述步骤s3中采用如下公式计算运动门控因子：g(k)=exp(-|ω(k)|/ω_0)，其中ω(k)为当前时刻的角速度，ω_0为预设的角速度衰减常数。

11、进一步地，所述步骤s2中预估的机器人的运动状态还包括线速度；所述步骤s3中还基于所述线速度复合计算运动门控因子，复合计算公式为：g(k)=exp(-|ω(k)|/ω_0)·exp(-|v(k)|/v_0)，其中v(k)为线速度模长，v_0为预设的线速度衰减常数。

12、进一步地，所述步骤s4进一步包括：

13、步骤s41：根据所述步骤s2预估的位姿，对所述激光雷达扫描数据中的每一条激光束在所述先验栅格地图中进行射线投影，得到理论期望距离；

14、步骤s42：计算每条激光束的实际测距值与所述理论期望距离之间的残差，并将残差超过预设一致性阈值的激光束标记为异常束；

15、步骤s43：将所有异常束的终点位置投影到所述先验栅格地图的栅格坐标系中，并将这些终点位置所在栅格及其邻域栅格作为所述候选变化栅格集合。

16、进一步地，所述步骤s5中进行加权的时间累积计数和计数衰减操作的具体方法为：

17、为所述先验栅格地图中的每个栅格维护一个变化计数器；

18、对于当前时刻生成的候选变化栅格集合中的任一栅格，按如下公式更新其计数器：c_i(k)=c_i(k -1)+g(k)，其中g(k)为所述运动门控因子；

19、对于不在所述候选变化栅格集合中的任一栅格，按如下公式更新其计数器：c_i(k) =max(0,c_i(k-1)-η)，其中η为预设的衰减系数，max函数用于保证计数器非负。

20、本发明还提供一种基于imu和激光里程计融合的地图更新系统，其包括：

21、传感器数据获取模块：用于获取机器人的激光雷达扫描数据、imu测量数据及先验栅格地图；

22、融合定位与运动状态预估模块：用于基于扩展卡尔曼滤波器ekf，融合所述imu测量数据和由激光雷达扫描数据得到的激光里程计数据，以预估机器人的实时位姿和运动状态，所述运动状态至少包括角速度；

23、运动门控因子计算模块：用于根据预估得到的角速度，计算一个动态调节地图更新强度的运动门控因子；

24、候选变化栅格生成模块：用于基于预估的位姿，将激光雷达扫描数据与先验栅格地图进行一致性分析，生成候选变化栅格集合；

25、时空联合确认与地图更新模块：用于利用所述运动门控因子对所述候选变化栅格集合中的栅格进行加权的时间累积计数，同时对未被标记为候选变化栅格的栅格执行计数衰减操作，并在累积计数超过预设阈值时判定为真实环境变化并执行地图更新。

26、进一步地，所述传感器数据获取模块包括：

27、激光雷达数据接口，用于接收激光雷达采集的扫描数据；

28、imu数据接口，用于接收imu采集的角速度和线加速度数据；

29、地图加载接口，用于读取外部存储中的先验栅格地图。

30、进一步地，所述时空联合确认与地图更新模块包括：

31、计数器存储单元，用于为所述先验栅格地图中的每个栅格维护一个变化计数器；

32、加权累积单元，用于根据所述运动门控因子对候选变化栅格集合中的栅格的计数器进行累加；

33、衰减单元，用于按照预设的衰减系数对非候选栅格的计数器进行衰减；

34、比较输出单元，用于将每个栅格的计数器与预设阈值进行比较，当变化计数器大于等于预设阈值时输出变化确认信号，触发地图更新。

35、本发明还提供一种移动机器人，包括：激光雷达、imu、存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序；所述处理器执行所述计算机程序时实现上述任一项所述的基于imu和激光里程计融合的地图更新方法。移动机器人中具有非易失性计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述任一项所述的基于imu和激光里程计融合的地图更新方法。

36、与现有技术相比，本发明具有以下有益效果：

37、（1）引入运动感知的自适应门控机制，有效抑制旋转运动引起的虚假更新。

38、通过ekf融合imu与激光里程计数据，本发明能够获得精确的角速度预估值。在此基础上，根据角速度大小动态计算运动门控因子，使地图更新强度与机器人的运动平稳程度负相关。当机器人发生剧烈旋转（角速度超过阈值）时，运动门控因子自动降至0或极小值，地图更新过程被冻结或大幅抑制；当机器人运动平稳时，门控因子接近1，允许正常更新。实验结果表明，通过引入基于运动状态的门控机制，在机器人发生原地旋转等剧烈运动时，能够降低地图更新强度，从而抑制由姿态变化放大引起的虚假更新，该机制能够在旋转场景下有效降低无效地图更新。

39、（2）实现时空域联合的稳健更新决策，兼顾响应速度与抗干扰能力。

40、通过候选栅格生成将单帧观测异常转化为空间候选区域，再通过加权时间累积计数与衰减机制在时间维度上对变化信号进行确认。其中，计数器的增长幅度受运动门控因子调节：平稳运动时增长快，剧烈运动时增长慢或停滞。未被标记的栅格则通过衰减系数逐步回退。该机制实现了“单帧异常不更新、运动剧烈不更新、持续出现才更新”的三重约束，既避免了单帧噪声的干扰，又保证了对真实环境变化的及时响应。

41、（3）多传感器融合定位提升变化检测的可靠性。

42、基于ekf的imu与激光里程计融合定位，为后续的观测一致性分析提供了比单一传感器更连续、更平滑、更低噪声的位姿输入。相比纯激光里程计，融合定位在快速运动和特征退化场景下具有更高的鲁棒性，从根本上降低了因位姿预估误差而导致虚假变化检测的概率。

43、（4）计算效率高，满足实时运行需求。

44、本发明中，射线投影和候选栅格更新过程的计算量随激光束数量线性增长，因而具有较好的计算效率，能够满足移动机器人在线地图更新的实时处理需求。