¹重庆科技大学电气工程学院，重庆

²上海易咖智车科技有限公司，上海

³重庆海云捷讯科技有限公司，重庆

⁴重庆科创职业学院，重庆

收稿日期：2024年4月8日；录用日期：2024年5月24日；发布日期：2024年5月31日

摘要

本文针对移动机器人在复杂环境中的定位精度低和环境感知性能差的问题，基于IMU、GPS检测机器人自身位姿信息，基于激光雷达和RGB相机感知环境信息，开展机器人位姿信息与环境感知信息分层级融合的关键技术研究。在位姿信息融合层，通过粒子群优化算法优化BP神经网络，在基于优化后的BP神经网络改进无迹卡尔曼滤波实现INS与GPS松耦合的组合导航，减小INS元件IMU的偏置和噪声，且利用训练好的神经网络在GPS信号失锁的情况下输出预测信息对惯导系统进行误差校正，输出更精准的速度信息与坐标信息作为绝对位置约束；在环境感知融合层，将补偿后的IMU预积分测量值、加速度和角速度分别与视觉里程计和激光里程计进行二级融合，实现了机器人实时精准定位和更精细的环境地图构建。最后使用真实采集的轨迹对多传感信息两级融合的算法进行验证，实验结果表明该算法提升了机器人定位的准确性与环境感知性能，机器人运动轨迹与原真实轨迹的最大误差为1.46 m、最小误差为0.04 m，平均误差为0.60 m。

关键词

环境感知，移动机器人，多传感器融合，激光里程计，视觉里程计

Mobile Robot Localization and Environment Perception Technology Based on Multi-Sensor Information Fusion

Li Li¹, Minghao Gao¹, Chunxi Feng¹, Fei Li², Chao Huang³, Tingting He⁴, Junjie Bai^1*

¹School of Electrical Engineering, Chongqing University of Science and Technology, Chongqing

²Shanghai ECAR Technology Co., Ltd., Chongqing

³Chongqing AW Cloud Technology Co., Ltd., Chongqing

⁴Chongqing College of Science and Creation, Chongqing

Received: Apr. 8^th, 2024; accepted: May 24^th, 2024; published: May 31^st, 2024

ABSTRACT

This article focuses on the problem of low positioning accuracy and poor environmental perception performance of mobile robots in complex environments. It conducts key technical research on hierarchical fusion of robot pose information and environment perception information based on IMU and GPS for detecting the robot’s own pose information, as well as laser radar and 3D camera for sensing environmental information. In the pose information fusion layer, particle swarm optimization algorithm is used to optimize BP neural network. Unbiased Kalman filtering with improved unscented Kalman filter is implemented to achieve INS-GPS loosely coupled navigation, reducing biases and noise from INS component IMU. Additionally, a trained neural network is utilized to output prediction information for error correction of inertial navigation system when GPS signal loss occurs, providing more accurate velocity and coordinate information as absolute position constraints. In the environment perception fusion layer, compensated IMU pre-integration measurements are fused with visual odometry and lidar odometry separately at a secondary level. This enables real-time precise localization of the robot and finer construction of environmental maps. Finally, real collected trajectories are used to validate the algorithm for two-level fusion of multi-sensor information. Experimental results show that this algorithm improves both positioning accuracy and environmental perception performance of the robot. The maximum error between robot motion trajectory and original true trajectory is 1.46 m units, while the minimum error is 0.04 m units with an average error of 0.60 m units.

Keywords:Environment Sensing, Mobile Robotics, Multi-Sensor Fusion, Laser Odometer, Visual Odometer

Copyright © 2024 by author(s) and Hans Publishers Inc.

This work is licensed under the Creative Commons Attribution International License (CC BY 4.0).

http://creativecommons.org/licenses/by/4.0/

1. 引言

随着智能机器人技术的不断发展，移动机器人在各个领域中的应用越来越广泛，如无人驾驶、智能交通、服务机器人等。在这些应用中，导航定位是移动机器人的一项关键技术，它直接影响着机器人的运动控制和任务执行的精度和效率 [1] [2] 。其中用于同时建图和定位的关键技术SLAM (Simultaneous Localization and Mapping)，仍然存在定位误差和地图构建精度问题 [3] 。大量分析研究表明，在室内、城市峡谷或地下空间，因为导航和定位不够精准、导航系统的鲁棒性不够好以及实时性能滞后延迟的问题可能导致机器人导航到错误的位置或与障碍物发生碰撞在，在实际情况中导致了诸多困扰和事故 [4] 。

移动机器人能够自主运动主要在于对外部环境的感知和对自身姿态的感知，在目前的研究中，对于外部感知通常使用激光雷达(LIDAR)、视觉传感器、GPS (Global Position System)进行定位与环境感知，对于自身姿态感知基本是使用IMU (Inertial Measurement Unit) [5] 。其中激光雷达虽然分辨率高、速度快、抗有源干扰能力强，但是二维激光雷达仅能提供平面内测距信息、三维环境下难实现独立依赖于点云数据的实时帧间配准等问题 [6] [7] ；实时视觉同步定位与建图方法环境感知能力强、成本较低，探测范围宽、特征丰富且易提取，但视觉传感器(单目、立体、RGB-D相机等)容易受到光照条件、环境纹理特征等因素影响，容易导致位姿跟踪失败 [8] [9] ；GPS系统具有全天候服务、全球覆盖范围广、定位精度高且误差不随时间累积等优势，通常情况下，可以为移动机器人提供令人满意的定位精度，但在树木茂密或建筑物密集的城市峡谷等卫星信号受阻的场景下，定位精度将大幅度下降甚至完全失效 [10] ；IMU包括加速度计和陀螺仪，用于测量机器人的线性加速度和角速度，这些数据对机器人的自主导航、姿态控制和运动感知至关重要。虽然IMU短时间精度高、更新频率高，但IMU在长时间测量中由于IMU的bias和noise容易出现积分漂移，而且误差会随时间累积，导致定位和导航的不准确性 [11] 。

2017年，Raul Mur-Arta等人 [12] 在单目ORB-SLAM系统基础上，提出了一个开源的基于关键帧的视觉SLAM系统ORB-SLAM2，该系统支持单目、立体和RGB-D三种传感器输入，利用立体和RGB-D传感器提供的深度信息，实现了系统的初始化和建图能力。但是ORB-SLAM2仅支持单个传感器，在复杂的应用环境下，多传感器信息融合成为视觉SLAM的一个重要研究方向。

2017年，秦通等 [13] 创造了一种通用性高，且鲁棒性强的单目视觉惯性状态估计器(VINS-Mono)，融合了惯导信息与视觉信息。采取非线性优化的方法将特征观测值与预积分后的惯导测量值进行融合，实现高精度工作的同时还兼具四自由度的全局位姿优化。

2018年，贺一家等 [14] 设计了一种点线特征融合视觉惯性里程计(Point Line-Visual Inertial Odometry, PL-VIO)，将点、线重投影误差组合与惯导(IMU)预积分误差结合在一起，应用在滑动窗口优化框架上，通过最小化误差函数对状态变量进行改进优化，实验结果证明该方法相较于仅有点特征的VIO系统更加优越。

2018年，Sofia Yousuf等人 [15] 提出了一种利用多传感器数据融合技术的定位方法。当GPS信号丢失时，利用之前训练好的多层感知网络预测位置，并与编码器位置通过加权融合得到最终输出，该方法在一定程度上解决了传统方法在GPS中断后无法继续定位的问题。但是对卡尔曼滤波器和网络模型设计理论依据不足。

2020年，Andrii V. Rudyk等人 [16] 研究了MEMS陀螺仪在定位移动机器人中的应用。他们使用了Allan偏差法分析了MEMS陀螺仪随机误差的各个成分，这项研究通过实验测试得出MEMS陀螺仪主要误差成分，为其在定位应用提供参考。但是，研究没有对平均时间进行优化，没有检测到陀螺仪随机速率漂移，没有完全分析出马尔可夫噪声，说明对陀螺仪误差结构识别还需要进一步研究，以提高精度。

2020年，程和等人 [17] 提出设计了一个基于ROS的多传感器融合定位导航系统。该系统使用激光传感器作为核心传感器获取环境二维信息，同时也集成IMU和编码器来获取机器人的姿态和位移信息，虽然该系统相比单传感器导航在无障碍环境下导航时间更稳定，但是在复杂实际环境中，系统的定位精度和路径规划能力还需要进一步测试。

2021年，张逵等人 [18] 提出了一种基于惯性导航系统分别与3D激光雷达和单目视觉构成联邦滤波的室内多传感器融合导航定位方法，通过联邦滤波将两个子滤波器的最优估计结果传送到主滤波器进行融合，从而获得系统的全局最优估计结果。但是文章没有给出联邦滤波算法的具体实现过程，算法的细节描述不够清楚。

2021年，Wisth D, Camurri M, Das S等 [19] 讨论了一种针对里程计目的的紧密耦合激光雷达、视觉和惯性传感器的方法。作者提出了一种统一的多模式地标跟踪方法，旨在提高里程计估计的准确性和鲁棒性。

2022年，Muhammad Husnain Haider等人 [20] 提出了一种基于自适应神经模糊推理系统(ANFIS)的移动机器人导航方法，将ANFIS用于本地运动控制，同时使用GPS和航向传感器进行全局运动控制。但是，该研究环境比较简单，障碍物形状和大小变化较小，在更复杂环境下方法的性能需要进一步验证。

2023年，邵明志等人 [21] 研究了基于陀螺仪、里程计和电子罗盘等多种传感信息融合的移动机器人导航定位技术，使用扩展卡尔曼滤波算法对这些传感数据进行融合，再利用模糊神经网络对融合后的数据进行训练处理。但是模糊神经网络的训练样本量和迭代次数设置可能不够，网络的泛化能力需要进一步验证。

综上所述，多传感器融合技术是当前机器人在复杂环境下定位与感知领域的研究热点，国内外研究人员还在不断探索新的算法提升移动机器人定位的精准性和鲁棒性，因此本文融合IMU、GPS、激光雷达、3D相机的感知信息，开展机器人位姿信息与环境感知信息分层级融合的关键技术研究，实现机器人实时精准定位和更精细的环境地图构建。

2. 移动机器人的定位与环境感知系统设计

为了解决移动机器人在复杂环境中定位不准确和环境感知能力较弱的问题，本文设计了一种基于IMU、GPS、激光雷达和相机的多层融合定位和环境感知系统，系统结构图如图1所示。

图1. 系统结构图

在第一层融合中，采用了基于PSO算法优化的BP神经网络来改进无迹卡尔曼滤波，实现了INS与GPS的松耦合组合导航系统，利用训练好的神经网络在GPS信号丢失的情况下输出预测信息，对惯导系统进行误差校正，得到更精确的速度和坐标信息作为全局的绝对位置约束；有效地减小了INS元件IMU的偏置和噪声，补偿了IMU预积分测量值辅助视觉里程计获得更准确的位姿信息。然后，将补偿后的IMU预积分测量值、加速度和角速度与激光里程计和视觉里程计进行二级融合，在第二级融合中采用子图匹配技术实现闭环探测，这种方法把多帧雷达点云数据与相机数据拼接成子图进行闭环检测，弥补了单一激光雷达在低频环境下闭环探测的不足，增强移动机器人的位置和环境感知能力，使其具有更高的适应性能力。

3. 基于PSO-BP改进UKF的组合导航系统

本文的一级融合主要是针对INS与GPS构建了一个松耦合组合导航系统，其结构图如图2所示，通过采用PSO算法优化的BP神经网络对无迹卡尔曼滤波进行改进，改进后主要优势在于，当GPS信号丢失时，可以利用已经训练好的神经网络输出预测信息，对惯导系统进行误差校正，从而得到更为精确的速度和坐标信息，将其作为全局的绝对位置约束，此外改进后的算法还能有效减少INS元件IMU的偏置和噪声，通过补偿IMU预积分测量值，辅助视觉里程计获得更准确的位姿信息。

图2. 组合定位导航结构图

组合导航系统模型为： $X_K=\phi X_{K-1}+W_k$ ， $Z_K=H{X}_{k-1}+V_k$ 。其中，状态向量 $X={\left[\varphi ,\Delta v,\Delta p,\epsilon ,\sigma \right]}^{\text{T}}$ 包含三个失准角、x、y、z这3向速度差、经纬度位置座标的差值、陀螺随机行走和加速计的随机行走。

测量的是GPS和IMU的输出速度和位置的误差。该测量输入为： $Z_k=\left[\Delta v_x,\Delta v_y,\Delta v_z,\Delta p_x,\Delta p_y,\Delta p_z\right]$ 。

3.1. PSO算法优化BP神经网络

粒子群优化算法(PSO)是一种基于群体信息分享和历史定位的方法，它具有参数简单、易于实现的特点。该方法采用微粒群算法对BP网络进行权重和阈值的优化，并将其引入变异算子，并对其进行动态调节，从而改善了标准微粒群的缺陷，如图3所示。

图3. 改进粒子群优化的神经网络算法流程图

PSO算法在搜索最优权重、最大阈值时，其搜索的范围和深度都较宽，可以有效地避免BP神经网络中的错误逆向传播，从而容易陷入局部最优。PSO算法是实数编码，其运算速度比传统的二进制编码快得多。BP神经网络在后期的迭代率很低，可以对学习因素进行动态调整，增加变异算子，以避免局部最优，确保群体的多样性。改进粒子群优化算法实施步骤，设定大小并位置和速度进行初始化。评估各颗粒的适合性，对它们所经过的最佳位置进行比较，如果是最佳的，就把它当作目前最佳的位置；并将其与最佳群进行对比，如果是最优的，则以其为最优，反之不变。调整颗粒的速度和位置，按照公式(1)

$\{\begin{array}{l}{V}_i^{\left(k+1\right)}=\omega V_i^k+c_1\left(j\right)\cdot r_1\cdot \left(P_{best}-X_i^k\right)+c_2\left(j\right)\cdot r_2\cdot \left(g_{best}-X_i^k\right)\\ X_i^{\left(k+1\right)}=X_i^k+V_i^{\left(k+1\right)}\\ C_2\left(j\right)=C_{\max }-\left(C_{\max }-C_{\min }\right)\cdot \frac{i_{t\max }-j}{i_{t\max }}\\ C_1\left(j\right)=4-C_2\left(j\right)\end{array}$ (1)

式中 $C_1\left(j\right)$ 、 $C_2\left(j\right)$ 是j次迭代时的学习因子；i是迭代次数； $\omega$ 是权值系数；r₁、r₂是随机函数。生成(0, 1)区间随机数，若比变化系数小，则对群体进行随机初始化，反之则进行下一步骤。判断是否满足终止条件。

3.2. 基于PSO-BP神经网络训练GPS

基于PSO的BP神经网络其主要是分别利用3轴加速度和3轴角速度作为训练输入值，其为 $\left[a_E,a_N,a_U,\Delta \sigma ,\Delta \gamma ,\Delta \phi \right]$ 将东向、北向、天向速度、方位等作为训练对象，设定4s一周期为训练窗口。图4展示了神经网络的学习与预测流程。

图4. 神经网络的学习与预测流程

3.3. 改进的无迹卡尔曼滤波器

针对系统噪声统计特性不明确时，常规UKF算法存在着滤波精度降低，甚至滤波发散的问题，本文提出了一种利用协方差估计法估算出系统测量噪声和系统噪声的协方差矩阵法，从而改善UKF算法对系统噪声的适应性，其估算公式如(2)和(3)。

${\hat{R}}_k=\frac{1}{N}{\displaystyle {\sum }_{i=0}^{N-1}{\epsilon }_{k-i}{\epsilon }_{k-i}^{\text{T}}}-{\displaystyle {\sum }_{i=0}^{2n}{\omega }_i}\left[Z_{i,k/\left(k-1\right)}-{\hat{Z}}_{k/\left(k-1\right)}\right]\text{·}{\left[Z_{i,k/\left(k-1\right)}-{\hat{Z}}_{k/\left(k-1\right)}\right]}^{\text{T}}$ (2)

$\begin{array}{l}{\hat{Q}}_k=\frac{1}{N}\left[{\displaystyle {\sum }_{j=0}^{N-1}{\epsilon }_{k-j}{\epsilon }_{k-j}^{\text{T}}}+{\displaystyle {\sum }_{j=0}^{N-1}{\eta }_{k-j}{\eta }_{k-j}^{\text{T}}}\right]\\ -{\displaystyle {\sum }_{i=0}^{2n}{\omega }_i}\left[{\xi }_{i,k/\left(k-1\right)}-{\hat{X}}_{k/\left(k-1\right)}\right]\cdot {\left[{\xi }_{i,k/\left(k-1\right)}-{\hat{X}}_{k/\left(k-1\right)}\right]}^{\text{T}}\end{array}$ (3)

在这里，N是估计窗宽， ${\epsilon }_k$ 是残差， ${\eta }_k$ 是新息。使用这一方法，可以通过调节窗口宽度，从而使新息方差阵和残差方差阵的取样特性不同，在噪声较少的情况下，可以通过调节窗宽来降低测量结果和噪声，从而提高估计的响应速度；在噪声对信号造成很大影响的情况下，通过增加噪声的更新频率，可以提高估计精度。用(4)公式得出N。

$N_{i+1}=\left[N_i-\left({\displaystyle {\sum }_{j=1}^L\frac{{\eta }_k\left(j\right)-{\hat{\eta }}_{k-1}\left(j\right)}{{\hat{\eta }}_{k-1}\left(j\right)}}\right)/L\right]$ (4)

在这里， $[·]$ 是一个整数， ${\hat{\eta }}_{k-1}\left(j\right)$ 是最近k − 1个新的新息的平均值， ${\eta }_k$ 是第k个新的利息，L是一个度量的维数，它是一个新的利率，它是一个新的利率和一个k − 1的新的利率的差，并且用这个比率的平均值作为一个调整的参数。将(4)式中的N值带入到式(2)、(3)中去，由此能够自适应地调整UKF系统的噪音和量测噪声，由此减小由于不准确的噪声统计特性所导致的误差发散。

4. “视觉 + 激光”融合定位与环境感知系统

在第二级融合中，主要设计了由视觉惯性里程计(Visual Inertial Odometry, VIO)、雷达惯性里程计(Lidar Inertial Odometry, LIO)和闭环优化三大模块组成的“视觉 + 激光”融合定位与环境感知系统，如图5所示。

图5. “视觉 + 激光”融合定位与环境感知系统框图

采用多线激光雷达、视觉相机的原始数据和第一级融合中出输出补偿后的IMU预积分测量值、加速度和角速度作为“视觉 + 激光”算法的输入，其中VIO利用激光雷达测量数据为视觉特征提供深度信息以提高VIO精度，而LIO采用VIO提供的位姿预测信息快速完成点云配准，从而实现高精度和高鲁棒性的实时定位；当VIO和LIO两个子系统检测到故障时或者当检测到足够的特性时，可以独立工作，也可以相互耦合，避免了单一子系统故障导致系统不能使用的情况，提高了整个系统的鲁棒性；在闭环优化模块中，本文采用子图匹配技术实现闭环探测，这种方法把多帧雷达点云数据与相机数据拼接成子图进行闭环检测，弥补了单一激光雷达在低频环境下闭环探测的不足。

闭环检测与优化算法

通过闭环探测，可以确定机器人有没有返回到之前的位置，所以在进行图的优化时，可以根据机器人的精确位置信息，将激光雷达闭环探测信息结合起来，大大提高了机器人的定位精度为环境感知以及地图构建的精准度打下基础。传统的ICP算法在利用短程激光雷达来构造低特征的环境下，会出现一些问题。针对此问题，提出了利用局部子图构造方法来克服传统ICP方法在闭环探测时所存在的缺陷。在机器人运动距离低于一定阈值的情况下，利用激光点云数据构造子地图，进行闭环探测。利用多帧激光点云进行子图匹配，可以极大地克服单帧激光匹配的缺陷，并在完成了前端图的绘制后，对其进行了后端的优化，并将其输出到最优位置。通过对激光雷达的分析，得到了最优路径，得到了更高精度的目标。图6显示了多级融合的流程图。

图6. 多级融合的流程图

5. 实验与结果分析

5.1. 多传感器融合构建的地图效果

从图7中可以直观的看出，通过使用视觉 + 激光构建地图与相使用单一的激光雷达构建地图的区别，融入了视觉后的能够更精确的识别出道路旁的房屋建筑，激光 + 视觉的地图元素更加丰富，细节更加精细，感知的周伟物体更具有辨识度。

图7. 地图构建对比

5.2. evo评估优化后算法得出的轨迹

基于kitti数据集中的07对本文的slam算法与单一激光雷达slam算法分别使用evo进行评估，从图8可以看出，图中07是kitti数据集中的07的原轨迹，07_old是单一激光雷达slam算法得到的轨迹，07_change是使用本文slam算法得到的轨迹。使用本文的方法优化后的轨迹明显比原算法更加贴合原轨迹，且误差控制到较小范围内。

图8. 轨迹对比图

根据表1数据可以看到改进前后绝对姿态误差数据的对比，表格中列出了误差的最大值(max)、平均值(mean)等统计指标。改进后的最大误差从10.360967 m降低到1.455266 m，平均误差从1.968058 m降低到0.600112 m，中位数误差从1.565123 m降低到0.483278 m，最小误差从0.370440 m降低到0.040534 m，均方误差从2.801205 m降低到0.689174 m，误差平方和从2879.756613 m降低到261.703415 m，标准差从1.993363 m降低到0.338860 m。这些数据的减少说明了系统在处理姿态误差方面的改进效果，反映了算法优化或系统调整的有效性。

表1. 改进前后的绝对姿态误差数据对比

6. 结束语

本文针对移动机器人在复杂环境中定位精度低和环境感知性能差的问题，提出了一种基于IMU、GPS、激光雷达、相机的分层级融合定位和环境感知技术。在一级融合中采用了基于粒子群优化算法优化的BP神经网络来改进无迹卡尔曼滤波，实现了INS与GPS的松耦合组合导航系统。这种方法有效地减小了INS元件IMU的偏置和噪声，并利用训练好的神经网络在GPS信号失锁的情况下输出预测信息，对惯导系统进行误差校正，输出更精准的速度信息和坐标信息作为绝对位置约束。然后，将补偿后的IMU预积分测量值、加速度和角速度分别与激光里程计和视觉里程计进行二级融合。

通过使用真实采集的轨迹对改进后的算法进行验证，实验结果表明该算法提升了机器人定位的准确性与环境感知性能，机器人运动轨迹与原真实轨迹的最大误差为1.46、最小误差为0.04，平均误差为0.60。

总的来说，本文的研究成果为移动机器人在复杂环境中的定位和环境感知提供了一种有效的解决方案，不仅提高了移动机器人的定位精度，也增强了其环境感知能力，为未来移动机器人的研究和应用提供了技术支持。

基金项目

重庆科技大学硕士研究生创新计划项目，项目号“YKJCX2220406”。

重庆市雏鹰计划项目“可燃性气体泄露源追踪定位技术的研究”，项目编号“CY221001”。

文章引用

李 力,高明浩,封淳曦,李 飞,黄 超,何婷婷,柏俊杰. 基于多传感信息融合的移动机器人定位与环境感知技术
Mobile Robot Localization and Environment Perception Technology Based on Multi-Sensor Information Fusion[J]. 人工智能与机器人研究, 2024, 13(02): 408-418. https://doi.org/10.12677/airr.2024.132042

参考文献