摘要：马楠, 曹姗姗, 白涛, 孔繁涛, 孙伟. 农业复杂场景下多机器人协同SLAM研究进展与展望[J]. 智慧农业(中英文), 2024, 6(6): 23-43. DOI: 10.12133/j.smartag.SA202406005

本文节选自

马楠, 曹姗姗, 白涛, 孔繁涛, 孙伟. 农业复杂场景下多机器人协同SLAM研究进展与展望[J]. 智慧农业(中英文), 2024, 6(6): 23-43. DOI: 10.12133/j.smartag.SA202406005

MA Nan, CAO Shanshan, BAI Tao, KONG Fantao, SUN Wei. Research Progress and Prospect of Multi-robot Collaborative SLAM in Complex Agricultural Scenarios[J]. Smart Agriculture, 2024, 6(6): 23-43. DOI: 10.12133/j.smartag.SA202406005

官网全文免费阅读

传感器多源数据融合与多机器人协同定位关键技术

1 传感器多源数据融合

在农业复杂环境下，多传感器数据融合是SLAM重要的组成部分。SLAM主要根据所使用的传感器的类型来分类，目前常见的SLAM传感器有激光雷达（LightLaser Detection and Ranging, LiDAR）、视觉传感器（单目、双目、深度相机）和惯性测量单元（Inertial Measurement Unit, IMU），这些传感器为机器人提供了感知世界的能力。不同传感器有各自的特点，LiDAR可以测量环境准确的3D点云，但比较稀疏，并且不能获得较好的数据关联；视觉传感器可以获取较好的数据关联，但是没有深度信息，且对环境纹理比较敏感，易受到光照影响；IMU可以通过预积分对状态进行短时估计，但是噪声和零偏使得积分值发生漂移。由于农业领域的SLAM应用场景受到动态可变、地形环境复杂、气候阴晴多变、通信约束受限等多重因素叠加影响，结合各类传感器的特点进行取长补短，采用多种传感器数据融合的方法是在农业复杂环境下提高SLAM建图精度的重要方法。传感器之间数据融合的前提条件是不同传感器对同一目标在同一时刻的描述，目前，不同传感器数据之间外参的标定分为三类，分别是基于目标的标定方法、无目标的标定方法和基于学习的标定方法。以下介绍目前多传感器融合的主流策略，分别是视觉+IMU融合、LiDAR+IMU融合，以及LiDAR+视觉+IMU融合。

1.1 视觉+IMU

将视觉传感器数据和IMU数据进行融合的方法称为VIO-SLAM（Visual-Inertial Odometry Simultaneous Localization and Mapping） 。通过结合两种传感器数据的优势，IMU可以在视觉传感器短时间失效时为视觉提供短期的精确定位，与此同时，当IMU由于零偏而发散并积累误差时，视觉定位信息可用于估计IMU的零偏。两者的融合可以解决视觉姿态估计中输出频率低的问题。VIO-SLAM工作流程首先是采用视觉传感器获取数据，采用特征提取的方法，对特征进行跟踪与匹配；IMU对获取的数据进行预积分计算、向前传播计算等预处理；将两种模态的数据采用重力对齐、尺度对齐的方法进行初始化；其次，融合视觉投影的约束，以及IMU预积分的约束进行状态的求解，由于不断增量式运行会产生累积误差，借助回环检测对里程计得到的增量进行约束；最后，对机器人的位姿图进行优化，修正机器人在运行过程中的累积误差（图1）。

图1 视觉+IMU处理流程图

Fig. 1 Visual+IMU processing flow chart

目前，VIO-SLAM有两种融合类型，分别是松耦合和紧耦合。松耦合是指IMU和视觉传感器分别进行运动估计，再融合它们的位姿估计结果。由于松耦合下运动估计和位姿估计的更新频率不一致，所以模块之间需要信息交换。松耦合方法通常以惯性测量数据为核心，视觉传感器数据用来修正惯性测量数据产生的累积误差。紧耦合是指将IMU的状态和视觉传感器的状态合并，共同构建用于状态估计的运动和观测方程。将IMU中的尺度度量信息用于辅助视觉中的尺度估计。紧耦合算法复杂度高，但充分利用了传感器数据，不仅可以减少传感器测量的不准确性，而且减少多传感器融合关联之间的模糊性。代表性开源VIO-SLAM融合算法如多状态约束卡尔曼滤波器（Multi-State Constraint Kalman Filter, MSCKF）、基于关键帧的开源视觉-惯性SLAM（Open Keyframe-based Visual-Inertial SLAM, OKVIS）、视觉-惯性导航系统融合（Visual-Inertial Navigation System Fusion, VINS-Fusion）、鲁棒视觉惯性里程计（Robust Visual Inertial Odometry, ROVIO）和直接稀疏多状态约束视觉惯性里程计（Direct Sparse Multi-State Constraint Visual-Inertial Odometry, DM-VIO）等方法，其详细信息见表1。

表1 VIO-SLAM代表性融合算法

Table 1 Representative fusion algorithms of VIO-SLAM

在农业复杂场景下，视觉与IMU的融合方法在定位与建图任务中表现出一定的优势。然而，这种融合方法通常更适用于光照条件相对稳定、环境纹理丰富的室内场景。例如，在温室环境中进行草莓采摘时，由于光线控制较为稳定，视觉传感器能够准确捕捉到周围的细节纹理，结合IMU提供的运动信息，系统能够实现较高精度的定位与导航。同样，在养殖场中进行动物行为监测时，由于场地相对封闭且光线条件可控，视觉传感器可以清晰地识别动物的动作和位置，而IMU则提供了额外的运动轨迹信息，两者的融合能够有效提高SLAM系统的鲁棒性和准确性。

1.2 LiDAR+IMU

LiDAR数据和IMU数据的融合的方法称为LIO-SLAM（LiDAR-Inertial Odometry Simultaneous Localization and Mapping）。借助两种传感器的优势进行融合，IMU数据可以作为先验信息，提高算法的收敛速度和稳定性，而LiDAR提供的精确环境信息可以提高地图构建的准确性。LIO-SLAM的工作流程是首先去除LiDAR数据的运动畸变，对点云数据进行滤波及特征提取的预处理；IMU对获取的数据进行IMU预积分计算、向前传播计算等预处理；其次，通过两帧之间的点云进行快速匹配，求解出机器人的运动增量为后期提供初值，将当前的帧和地图匹配进行位置更新；最后通过回环检测对全局进行优化（图2）。

图2 激光雷达+IMU处理流程图

Fig. 2 LiDAR+IMU processing flowchart

目前，LIO-SLAM的松耦合是指将IMU和LiDAR数据分别独立处理，借助IMU数据进行位置、速度和姿态的预测，通过LiDAR点云进行独立的SLAM或点云配准，在滤波器或优化器中将IMU预测结果与LiDAR的定位结果进行融合；紧耦合是指通过IMU的姿态信息初始化系统状态，在联合滤波器或优化器中同时处理IMU和LiDAR数据，利用IMU数据进行运动预测，LiDAR数据进行地图构建和定位校正。代表性开源LIO-SLAM融合算法如激光雷达里程计与建图（LiDAR Odometry and Mapping, LOAM）、轻量级且针对地面优化的激光雷达里程计与建图（Lightweight and Ground-Optimized LiDAR Odometry and Mapping, LeGO-LOAM）、激光雷达-惯性里程计与建图（LiDAR-Inertial Odometry and Mapping, LIO-Mapping）、带平滑和映射的激光雷达-惯性里程计与建图（LiDAR-Inertial Odometry and Mapping with Smoothing and Mapping, LIO-SAM）和标记激光雷达-惯性里程计（Marked LiDAR-Inertial Odometry, Marked-LIEO）等方法，其详细信息见表2。

表2 LIO-SLAM代表性融合算法

Table 2 Representative fusion algorithms of LIO-SLAM

在农业复杂场景中，LiDAR与IMU的融合方法在实现动态室外环境下的SLAM任务时具有明显优势。适用于对数据关联要求较低的应用场景，在无人农机大田作业时，由于外部条件如风沙、天气多变等因素的影响，传统的视觉传感器可能难以获得清晰可靠的数据，而LiDAR则能够在这些复杂条件下提供稳定的距离测量信息。结合IMU提供的姿态和运动信息能够有效应对室外环境的动态变化，确保机器人在多尘或低能见度的条件下仍能准确定位和导航。

1.3 雷达+视觉+IMU

LVI-SLAM（LiDAR-Visual-Inertial SLAM）是通过LiDAR、视觉传感器和IMU传感器数据融合获得的SLAM。该方法能够实现高精度、鲁棒的状态估计和映射。融合三种传感器的优势在于提高数据的可靠性，即使出现1种或者2种传感器退化，系统仍能正常工作。LiDAR可直接测得环境的几何信息，与IMU融合完成快速初始化；借助激光惯性子系统辅助视觉IMU进行初始化；激光里程计建立的点云通过外参投影到图像，并辅助视觉特征的深度提取；联合激光+视觉+IMU优化，详细流程见图3。

图3 雷达+视觉+IMU处理流程图

Fig. 3 Processing flowchart of LiDAR +Vision+IMU

目前，大多数LVI-SLAM都是由VIO-SLAM和LIO-SLAM的耦合生成。代表性开源LVI-SLAM融合算法如视觉激光雷达里程计与建图（Visual LiDAR Odometry and Mapping, V-LOAM）、视觉-惯性激光雷达同步定位与建图（Visual-Inertial LiDAR SLAM, VIL-SLAM）、激光雷达-惯性映射与里程计（LiDAR-Inertial Mapping and Odometry, LIMO）、统一的激光雷达视觉-惯性里程计（Unified LiDAR Visual-Inertial Odometry, ULVIO ）和激光雷达-视觉-惯性平滑与建图（LiDAR-Visual Inertial Smoothing and Mapping, LVI-SAM ）等方法，见表3。在农业领域，LVI-SLAM融合方法多适用于复杂动态环境，如果园采收机器人，在充满不规则物体和快速变化的环境下，LVI-SLAM能够精确地感知周围果树的位置，避开障碍物并高效完成自动采摘任务；自然环境下放牧机器人在广阔、开放的农场环境中行走时，面临着地形复杂、多变的挑战，如草地的高度变化、障碍物的随机分布，以及环境中存在的动态物体（如牲畜的移动）。通过LVI-SLAM的高精度定位和建图能力，放牧机器人能够准确掌握自身在环境中的位置并动态避障，从而提高作业效率和适应能力。

表3 LVI-SLAM代表性融合算法

Table 3 Representative fusion algorithms of LVI-SLAM

2 多机器人协同定位

多机器人协同定位（Cooperative Localization, CL）是多机器人协同SLAM的重要组成部分，因此，准确、简便、高效的协作定位方法具有重要的意义。多机器人协同定位的关键问题是每个机器人都需要知道自己的位置，从而在地图上标记自己的位置信息，并将其传输给其他机器人。近年来，多机器人协作定位方法在农业领域的应用也日益广泛。在批量无人农机的田间作业中，协同定位技术在保证田间精确作业的同时，最大程度地减少了重复或遗漏任务的情况。在野外放牧机器人领域，协同定位技术使得多个放牧机器人在获取各自位置的基础上，能够协调并互相补偿位置，显著提高了准确度。协同定位技术展现出了在农业复杂场景中的显著潜力和益处。本文根据不同的需求目标将多机器人协作定位算法分为三类，贝叶斯滤波定位、分布式定位和容错补偿定位，并对多机器人协同定位的主要算法及应用场景进行了对比分析，如表4所示。

表 4 多机器人协同定位主要算法

Table 4 The main algorithms of multi-robot cooperative localization

贝叶斯滤波定位包括鲁棒扩展H∞滤波方法（A Robust Extended H∞ Filtering Approach, REHF）、随机扩展卡尔曼滤波器（Stochastic Extended Kalman Filter, STOC-EKF）、多过程卡尔曼滤波器（Multi-Process Kalman Filter, MP-KF）和改进的平方根自适应混合扩展卡尔曼滤波器（Improved Square-Root Adaptive Hybrid Extended Kalman Filter, Improved SHAEKF）等，多关注于如何使多个机器人在复杂环境中提高定位准确性和性能，优点是借助递归更新的方法来估计动态系统状态，有效处理不确定性和噪声，缺点是它们的计算成本很高；分布式定位方法包括分裂协方差信息滤波器（Split Covariance Information Filter, Split-CIF）、批处理双通道卡尔曼滤波器（Batch Processing Dual-Channel Kalman Filter, BP-DCKF）等方法，其优势是每个机器人可以利用自身的传感器数据自主地进行位置估计，机器人之间通过共享位置信息、传感器数据和其他有助于定位的信息，提高整体系统的定位精度和鲁棒性，缺点是需要稳定的通信环境。容错与补偿定位包括全分布式扩展信息滤波器（Fully Distributed Extended Information Filter, FDEIF）、双扩展卡尔曼滤波器（Dual-Extended Kalman Filter, DEKF ）、逆透视变换（Inverse Perspective Mapping, IPM）和具有通信不确定性的分布式协同定位（Distributed Collaborative Localization with Communication Uncertainty, DCL-CU）等方法，多聚焦于在传感器误差或通信延迟的情况下，多个机器人仍能准确地定位自己和彼此的位置，该方法的优点是保证了多机器人协同定位的可靠性，缺点是额外增加了控制策略，计算成本较高。

在农业领域，贝叶斯滤波定位计算适用于可以接受高成本计算且要求高精度定位作业的农业复杂场景。但是，在面向户外大规模田间作业等需要批量机器人作业的场景下，随着机器人数量的增加，致使计算成本呈指数级增长。此外，在自然环境放牧等大范围场景下，机器人之间需要进行频繁的通信以传递状态信息，会存在通信负载增加等问题。分布式定位方法可有效解决该问题，其适用于计算资源要求较低的轻量化定位，且能支持大规模高效作业的场景，如果园采摘机器人、大田无人机喷洒农药等，但对通信能力要求较高。容错与补偿定位方法针对通信受限问题，多适用于通信受限的大范围多机器人定位作业的应用场景，如在天然草场下协同放牧机器人、批量无人农机协同大田作业等。

发布征集

来源：智慧农业资讯一点号