当SLAM学会动态权衡:深度强化学习让农业机器人在复杂果园中定位更精准

「3D视觉从入门到精通」知识星球(点开有惊喜) ！星球内有20多门3D视觉系统课程、3DGS独家系列视频教程、顶会论文最新解读、海量3D视觉行业源码、项目承接、求职招聘等。想要入门3D视觉、做项目、搞科研，欢迎加入！

传统的农业机器人定位与建图通常依赖固定的多传感器融合策略，然而传感器易受场景状态改变而精度退化，导致定位和建图失稳失准。针对此问题，该研究提出基于深度强化学习和动态融合感知的农业机器人同步定位与建图（simultaneous localization and mapping, SLAM）算法。其前端包括3个部分：视觉-惯性模块融合视觉里程计和惯性测量单元（inertial measurement unit，IMU）数据，优化视觉重投影误差和IMU测量误差；激光雷达-惯性模块提取点云特征并与IMU数据联合优化，提供稳定的位姿估计；自适应权重调整模块将视觉特征统计量与点云几何特征作为状态输入，基于双延迟深度确定性策略梯度（twin delayed deep deterministic policy gradient，TD3）算法在线学习并输出传感器融合权重，直接作用于因子图残差加权项，实现传感器贡献的自适应分配。后端构建了因子图优化模型以联合优化观测数据，实现最大后验位姿估计和地图构建。最后，分别在公开数据集和复杂果园场景中开展试验验证。试验结果表明，所提方法在复杂果园中定位的最大误差与均值误差分别为158.3与  ，位姿估计精度优于主流对比算法，证明了所提算法在复杂农业应用场景中的有效性。

0 引言

机器人作为提升生产效率与灵活性的关键工具，已经在温室[1-2]、种植区[3-4]以及露天作业[5]等多种环境中得到广泛应用。现有的机器人同步定位与建图方法多采用固定的多传感器融合策略[6-7]，但传感器具有明显的场景依赖性：在室内环境中GNSS信号失效；在长廊等几何特征单一的区域激光雷达易发生退化漂移；进出温室时光照剧变导致视觉失效；种植区还面临植被纹理相似与枝叶动态遮挡等挑战。传统固定权重融合策略无法在线感知传感器退化，当某一模态失效时仍按预设比例融合其观测数据，导致错误约束被引入因子图优化，进而引发前端估计不稳与后端优化误差积累，造成定位与建图失稳失准。因此，能否在线感知环境变化与传感器退化并在复杂果园等场景中持续获得可靠的位姿估计与地图构建，成为当前研究的关键问题[8]。

关于多模态协同与自适应融合的研究可概括为两类：规则驱动的质量评估与加权以及学习驱动的自适应权重调整。在规则驱动的质量评估与加权方面，ZHANG等[9]面向果园环境提出基于扩展卡尔曼滤波的激光雷达-视觉惯性松耦合融合框架，以激光雷达里程计与陀螺仪预积分作为预测模型，以视觉惯性里程计作为观测模型，通过卡尔曼增益自动计算融合权重，实现实时定位与彩色三维点云建图。针对GPS信号受阻的温室环境，YAN等[10]提出Visual-IMU-Wheel里程计方法，将轮式里程计与IMU预积分作为预测步骤，将视觉惯性里程计作为观测更新，在无土栽培温室与廊架结构等设施农业场景下实现了厘米级定位精度。针对多作物场景中遮挡频繁与纹理重复的问题，HONG等[11]采用裁剪滤波与统计滤波进行点云质量评估，通过改进的体素化点云配准结合自适应概率分布实现前端位姿估计，并在后端引入因子图与几何观测器对关键帧与普通帧进行分层优化。面向狭窄通道、遮挡严重的温室环境，CHENG等[12]通过预设距离阈值判断非视距状态并计算基站置信度进行加权，结合扩展卡尔曼滤波融合多传感器数据。此外，JIA等[13]针对棚架式猕猴桃果园弱信号环境提出超宽带、激光雷达与轮式里程计集成定位方法，以预设距离阈值与角度阈值判断超宽带异常值并采用卡尔曼滤波校正动态误差，通过粒子滤波融合多源测量值后经扩展卡尔曼滤波输出最终位姿估计。上述规则驱动方法在多场景中取得成效，但其权重函数、阈值或不确定性建模通常需人工设定或依赖固定统计假设。在设施农业等场景中，季节变化、作业工况与传感器遮挡导致观测分布长期非平稳且并发退化现象常见，固定规则难以持续适配，因而亟需能够在线感知退化并自适应调整融合权重的学习式方法。

在学习驱动的自适应权重调整方面，ASLAM-FD（a multi-sensor adaptive collaborative fusion SLAM framework based on degradation detection and deep reinforcement learning）框架[14]通过外部与内部两类退化量化模型对各传感器的实时退化状态进行连续表征，并引入带有长短期记忆的深度强化学习网络对多模态融合权重进行自适应调整，具备学习能力但性能受退化检测准确性与训练分布影响，在实际环境中的泛化性验证不足；Lvio-Fusion框架[15]在固化优中融合多种传感器，并以自适应强化学习更新各模态权重，但其强化学习模型的输入仅依赖视觉观测矩阵，根据平衡策略间接作用于因子固化优中雷达因子的权重，但权重的贡献尚未得到充分量化；YANG等[16]提出Adaptive-Visual-Selection VIO框架，训练策略基于惯性测量单元（inertial measurement unit，IMU）特征与历史隐状态，借助Gumbel-Softmax实现端到端可微的视觉模态启闭决策，运行时按伯努利策略自适应地启用和关闭视觉分支；WONG等[17]将多模型自适应估计与近端策略优化结合以实现激光雷达与视觉的动态调权，策略优化优势明显，但尚未在真实机器人上验证。现有学习驱动方法多在室内或城市道路环境验证，在农业环境中的泛化性验证有限，亟需开展面向果园、温室等设施农业场景的学习驱动自适应融合同步定位与建图（simultaneous localization and mapping，SLAM）研究。

为此，本文基于平滑与建图的紧耦合激光雷达-视觉惯性里程计（tightly-coupled lidar-visual-inertial odometry via smoothing and mapping，LVI-SAM）算法框架，提出一种基于因子图优化的动态自适应SLAM（dynamic SLAM，DYN-SLAM）算法，算法由视觉-惯性模块、激光雷达-惯性模块和自适应权重调整模块组成，其中视觉-惯性模块融合视觉里程计与IMU数据，通过最小化视觉重投影误差和IMU测量误差实现高精度状态估计，并利用激光雷达深度信息提升初始化鲁棒性；激光雷达-惯性模块提取激光边缘和平面特征，与IMU数据联合构建约束因子并进行因子图优化，提供稳定的位姿估计；自适应权重调整模块基于TD3深度强化学习模型，将视觉特征与激光几何特征作为输入，以实时感知环境变化并动态调整两者在因子图中的融合权重，使系统在视觉退化时增强雷达贡献、在视觉良好时保持平衡融合，通过多源信息的自适应调整，提升复杂环境下的定位精度与鲁棒性。

1 DYN-SLAM算法概述

DYN-SLAM系统架构如图1所示。视觉-惯性模块利用图像特征提取与光流跟踪获取特征匹配关系，结合IMU预积分约束，在滑动窗口内最小化视觉重投影误差与惯性测量误差，以实现连续的视觉-惯性状态估计。为进一步提高系统在弱纹理、低光照环境下的稳健性，该模块在必要时引入激光雷达的深度信息对特征点进行深度补偿与筛选，从而提升尺度可观性和初始化鲁棒性，同时在系统初始化阶段可为激光雷达-惯性模块提供初始位姿估计，并在回环检测阶段辅助特征匹配与全局优化。激光雷达-惯性模块负责处理经过IMU去畸变的点云数据，提取边缘特征和平面特征，并与滑动窗口内维护的局部地图进行特征匹配以构建激光约束因子。IMU数据不仅用于点云去畸变，还为扫描匹配提供初值估计，以提高匹配收敛速度与精度。当视觉信息退化或失败时，激光雷达-惯性模块可独立运行以维持系统的位姿跟踪能力。最后，来自激光雷达-惯性模块的激光约束、视觉-惯性模块的视觉与惯性约束，以及回环检测因子共同输入至因子图进行全局优化，实现多源信息的紧耦合融合。自适应权重调整模块基于TD3算法构建Actor-Critic框架，用于实现视觉与激光因子在因子图中的动态加权。该模块以视觉特征观测矩阵和激光几何特征统计量作为状态输入，采用相对位姿误差（relative pose error，RPE）的倒数作为奖励信号，通过策略网络与价值网络的交替更新实现强化学习过程。策略网络根据输入状态输出相机因子与雷达因子的连续权重，并将其作为动作直接作用于因子图残差的加权项，从而完成视觉与雷达约束的自适应融合。

2 多传感器动态融合定位与建图算法设计

2.1 视觉-惯性模块

视觉-惯性模块面向高频与高精度的前端位姿估计，在滑动窗口内将图像特征观测与IMU预积分进行紧耦合非线性优化，联合估计相机和IMU的位姿、速度与偏置，并在系统初始化与回环检测阶段提供关键先验，保障后端全局优化的稳定性与收敛性。

2.1.1 视觉特征处理流程

为了在相邻帧之间建立稳定的特征对应关系，本文以角点检测获得初始特征集合，并采用KLT（kanade-lucas-tomasi）光流算法估计像素位移  。给定两帧图像  及在  中的特征点  ，光流残差  定义为

式中  为特征点在第二帧图像  中的位置坐标，  。

在回环检测阶段，系统采用二值鲁棒独立基本特征（binary robust independent elementary features，BRIEF）描述子构建DBoW2词袋模型进行检索。关键帧  和候选帧  的相似度用汉明距离衡量，当汉明距离低于阈值时记为回环候选并记录时间戳，进而由激光雷达-惯性模块执行几何一致性验证，汉明距离  定义为

式中  为描述符  在第  位的二进制值；  为描述符  在第  位的二进制值；  为指示函数，当括号内条件成立时取1，否则取0。

在初始化完成后引入激光雷达深度先验：首先将多帧激光数据配准到相机坐标系，融合形成相对稠密的深度图；再在单位球面上为每个特征点  检索最近三点  拟合局部平面（仅当任意两点的最大距离不超过  时执行深度估计），特征点  的深度通过射线与平面交点计算，表示为

式中  为特征点  的深度，  ；  为单位化的特征射线方向向量；  为射线原点到平面的距离，  。

考虑到视觉在剧烈运动、强光照变化或无纹理场景中易退化，模块设置两类安全触发：有效跟踪特征数低于阈值，或估计的IMU偏置超限。任一触发时，视觉-惯性模块将重新初始化并向激光雷达-惯性模块发送通知，以防止错误状态传播到因子图优化中。

2.1.2 状态建模与优化目标

视觉-惯性模块的核心任务是在滑动窗口内联合估计系统状态，并通过非线性优化最小化多源测量残差。系统状态量由位姿、速度以及惯性传感器偏置构成，表示为

式中  为第  个时间戳的系统状态向量；  为世界坐标系到机体坐标系的旋转矩阵；  为IMU在世界坐标系的位置向量，  ；  为IMU在世界坐标系的速度向量，  ；  为加速度计的偏置，  ；  为陀螺仪的偏置，  。

IMU预积分将相邻关键帧间的惯性测量转化为相对运动约束，第  个时间戳的IMU预积分残差向量  形式为

式中  为IMU预积分的速度残差，  ；  为IMU预积分的位置残差，  ；  为IMU预积分的旋转残差，  ；  为加速度计的偏置残差，  ；  为陀螺仪的偏置残差，  。

对于第  个关键帧观测到的第  个空间点  ，其像素观测值为  。视觉重投影残差  定义为

式中  为第  个关键帧对应点的像素观测值，像素；  为相机投影函数；  为相机内参矩阵；  为世界坐标系到机体坐标系的旋转矩阵；  为第  个空间点在世界坐标系下的坐标向量，  ；  为第  帧相机从世界坐标系到机体坐标系的平移向量，  。

在因子图优化框架下，视觉残差与IMU预积分残差共同构成优化目标函数，表示为

式中  为滑动窗口内所有帧的系统状态量集合；  为IMU预积分残差的协方差矩阵；  为视觉重投影残差的协方差矩阵；  为参与第  个时间戳优化的视觉特征集合；  为鲁棒核函数。

通过最小化式（7）中的IMU与视觉残差，系统能够在滑动窗口内同时利用惯性测量的短时约束和视觉观测的几何一致性，从而获得鲁棒且精确的状态估计结果。

2.2 激光雷达-惯性模块

激光雷达-惯性模块为系统提供高精度且鲁棒的几何约束，并在滑动窗口因子图中与惯性信息实现紧耦合优化。其主要思路是先依据IMU进行运动补偿以消除逐线扫描畸变，再从校正点云中提取代表性的边缘与平面特征，并与局部地图匹配以形成点-线与点-面约束；这些约束与IMU预积分共同纳入非线性优化，输出稳定且精确的位姿估计。在此基础上，本文设计了4类反映激光观测质量的统计特征，作为状态量引入深度强化学习策略中，用于自适应地调整多传感器融合的权重。

2.2.1 点云特征处理流程

激光雷达特征处理的目标是从原始逐线扫描点云中提取能稳定参与约束的几何特征，并为后端优化提供可靠输入。由于同一帧点云的各采样点并非同时获取，直接使用会引入几何畸变。为此，系统利用IMU与里程计估计的相对运动对点云进行去畸变补偿，将所有采样点统一到参考时刻  的坐标系中，得到时空一致的数据。设采样点  在  时刻获得，其时刻对齐后的坐标表示为

式中  为时刻对齐后的坐标向量；  为IMU在两时刻间的刚体变换矩阵；  为采样点坐标向量。

在完成点云去畸变后，系统按局部曲率将点云划分为角点与平面点，作为互补的几何约束来源；在此基础上对特征进行筛选与均匀下采样，在大幅压缩数据规模的同时保留足以表达环境结构的要素；随后将筛选后的特征与局部地图历史特征建立对应，形成点-线与点-面残差因子，并与IMU预积分残差一并纳入滑动窗口内的非线性优化，从而获得精确且稳定的运动估计。

2.2.2 状态建模与优化目标

激光雷达-惯性模块在状态建模上与视觉-惯性模块保持一致，系统状态由位姿、速度及IMU零偏组成，具体形式同式（4）。不同之处在于，该模块的观测残差来源于点云几何约束，而非像素重投影。

对于角点特征，采用点到直线的最小距离作为残差，该残差表征角点  到由  与  定义直线的最小距离，表示为

式中  为第  个角点特征残差，  ；  为当前帧点云中的角点坐标向量，  ；  ，  为局部地图中与当前角点对应的2个边缘特征点的坐标向量，  。

对于平面特征，采用点到平面的距离作为残差，该残差表征平面点  相对参考平面的几何一致性，定义为

式中  为第  个平面特征残差，  ；  为当前时间戳点云中的平面点坐标向量，  ；  为通过邻域拟合得到的参考平面上一点的坐标向量，  ；  为该平面的单位法向量。

在因子图优化框架下，激光残差与IMU预积分残差共同构成优化目标函数，表述为

式中  为第  个时间戳的第  个边约束残差，  ；  为第  个时间戳的第  个平面点约束残差，  ；  为参与第  个时间戳优化的边特征集合；  为参与第  个时间戳优化的平面特征集合；  为IMU残差的协方差矩阵；  为角点残差的协方差矩阵；  为平面点残差的协方差矩阵。

通过最小化上述目标函数，系统能够在保证运动动力学一致性的同时，充分利用点云提供的空间几何约束，实现鲁棒且精确的状态估计。

2.3 自适应权重调整模块

在多传感器融合的图优化框架中，视觉与激光因子的可靠性受到场景条件的显著影响：在光照突变或纹理缺乏的环境中，视觉约束往往退化；而在稀疏或动态遮挡场景下，激光几何特征的约束同样会失效。若采用固定权重对不同模态赋值，难以保证在复杂多变环境下的鲁棒性。为解决这一问题，本模块引入基于强化学习的自适应权重调整机制，将图优化过程建模为强化学习环境，通过学习动态调整因子权重，实现传感器贡献的自适应分配。

2.3.1 面向TD3的视觉特征信息构造

在视觉-惯性模块完成特征点提取与光流跟踪后，进一步对原始观测进行统计化处理。每个特征点由图像坐标  及帧间位移量  组成。直接将所有特征点输入强化学习模型不仅会导致维度过高，且易受局部噪声干扰。为解决这一问题，参考Lvio-Fusion框架[15]，采用基于图像网格划分的特征统计方法。如图2所示，将图像平面划分为  个规则单元，分别计算每个单元内的特征点数量  、平均水平位移  、平均垂直位移  。该处理方式使局部特征信息被压缩为结构化统计量，能够在降低输入维度的同时保留运动模式。

第  行、第  列的单元格为  ，其中包含  个有效特征点。单元格  内第  个特征点的水平方向光流位移为  ，垂直方向光流位移为  ，光流位移记为  ，则其统计量定义为

式中  为单元格  内的有效特征点数量；  为单元格  内的特征点在水平方向的平均光流位移，像素；  为单元格  内的特征点在垂直方向的平均光流位移，像素。

若某单元格无有效特征点，则该单元格统计量设为零向量  。最终，整幅图像的视觉特征向量  可表示为

在机器人操作系统（robot operating system，ROS）的服务端将  经环境封装模块传递至TD3的观测空间。与传统视觉-惯性里程计中直接构造残差不同，本方法通过对观测信息的显式统计化建模，有效缓解了特征点数量波动与局部遮挡导致的不稳定性，使得强化学习模型能够接收稳定且结构化的输入。

2.3.2 面向TD3的激光特征信息构造

为了使TD3策略能够根据不同环境下的几何约束质量自适应地调整激光雷达的权重，本文在激光特征提取的基础上进一步设计了一组统计量，并将其构造成策略网络的输入，如图3所示。

首先统计角点匹配成功率与平面点匹配成功率。这两个量分别描述角点和面点在构建点-线与点-面残差中的利用程度，能够直接反映几何约束的有效性，角点匹配成功率  与平面点匹配成功率  定义为

式中  为当前时间戳提取的角点总数；  为当前时间戳提取的平面点总数；  为角点成功匹配并参与优化的数量；  为平面点成功匹配并参与优化的数量。

然后，引入点云投影成功比例，表征点云在二维深度图投影中的覆盖度。该指标越高，说明当前激光观测对场景的覆盖越充分；反之则表明观测信息存在一定缺失。点云投影成功比例  表示为

式中  为投影后有效填充的像素数。

最后，计算距离分布的标准差  ，衡量场景的几何复杂性。  较小，说明点云集中在某一深度范围，几何约束可能退化；  较大，表明场景具有明显的远近层次，约束更为丰富。  定义为

式中  为第  个点的深度，  ；  为平均深度，  。

完成上述统计后，将其组合为一个特征向量  ：

将该向量与视觉特征向量一同拼接后输入至TD3模型的观测空间。通过这种方式，策略网络能够在不同环境条件下感知激光与视觉观测的可靠性，并自适应地调整它们在多模态融合中的权重分配。

2.3.3 深度强化学习算法设计

为处理连续动作空间下的最优控制问题，本文采用基于Actor-Critic框架的TD3算法。TD3算法在深度确定性策略梯度（deep deterministic policy gradient，DDPG）算法[18]的基础上通过双Q网络裁剪、延迟策略更新和目标策略平滑三项改进，表现出更低的Q值偏差、更稳定的收敛以及更好的泛化能力[19]。在本文中，交互环境定义为采用滑动窗口机制的后端因子图优化器，强化学习智能体通过与环境的交互学习合适的权重策略。状态输入由前文构造的视觉特征统计矩阵与激光几何特征统计量共同组成，能够全面反映当前环境下的观测特征，根据式（14）和式（19），状态输入  定义为

动作输出  为各模态因子的连续权重，定义为

式中  为第  个时间戳视觉的权重；  为第  个时间戳激光雷达的权重。

 直接作为多模态联合优化目标函数中的加权系数，动态调整视觉因子与激光雷达因子在因子图优化中的相对贡献，合并式（7）与式（11）得到多模态联合优化目标函数为

在给定  下，通过对多模态联合优化目标函数进行非线性最小二乘优化得到当前滑窗的轨迹估计，再用这条轨迹与真值轨迹计算RPE，本文将RPE在时间跨度  上的大小作为惩罚量，并取其倒数作为奖励信号，给定时间戳  与  的位姿  ，  ，先构造相对旋转与相对平移，然后计算其合成量的范数并取倒数，第  个时间戳的奖励函数  定义为

式中  和  分别为第  个时间戳和第  个时间戳的旋转矩阵；  和  分别为第  个时间戳和第  个时间戳的位置向量，  。

以上过程定义了第  个时间步的一次交互：在状态  下，策略输出动作  ，后端在该动作条件下完成优化并产生奖励  ，随后获得下一状态  。由此得到的样本  被写入回放缓冲区，用于TD3的目标构造与参数更新。

为提高训练稳定性并缓解Q值过估计，本文采用TD3的“双Q裁剪、延迟策略更新、目标策略平滑”机制。首先，TD3维护两套独立的Critic以及相应的目标网络，对下一状态  与目标策略  计算两路目标Q值，并以较小者构造TD目标，TD目标定义为

式中  为当前的即时奖励；  为折扣因子；  为第  路目标价值网络输出的Q值；  为下一状态的目标动作。

在此目标下，两个Critic分别最小化均方误差损失，均方误差损失定义为

式中  为小批量样本数；  为批内样本索引；  为第  路价值网络输出的Q值。

其次，TD3采用延迟策略更新以降低目标漂移：在每一步都更新两路Critic的同时，Actor与各目标网络仅每隔  步（  为策略延迟超参数）才更新一次。这样可令价值函数先得到更稳定的近似，再驱动策略改进，从而减少训练震荡与方差。Actor的更新仍使用确定性策略梯度，策略梯度固定用第一路，按式（26）计算。

式中  为当前动作处价值网络对动作的梯度；  为策略网络的参数；  为当前策略网络在状态  输出的动作；  为策略输出对参数  的雅可比矩阵。

在计算目标值时仅对目标动作施加策略平滑正则化：先在目标路径上加入逐分量截断高斯噪声，再用softmax投到动作可行域，目标动作与噪声分布分别定义为

式中  为目标策略网络的参数；  为经区间裁剪的高斯噪声；  为裁剪阈值；  为标准差。

所有目标网络采用软更新以保证目标缓慢、平滑地演进，其更新规律定义为

式中  为软更新系数。

以上构成TD3的完整训练流程：从回放缓冲区采样小批量  ，用带平滑与裁剪的目标更新两路Critic；每经过  步再更新一次Actor，并按软更新同步目标网络。在如图4所示的架构上，TD3由Actor与Critic网络及其对应的目标网络组成，共6个多层感知机（multilayer perceptron，MLP）：Actor网络基于输入状态输出连续的权重动作，Critic网络估计状态-动作对的Q值以评估动作质量，“双Q网络和延迟策略更新”机制有效缓解了过估计问题并提高了训练稳定性。

3 试验验证与分析

为了评估DYN-SLAM算法的性能，本文在公开数据集和实际的果园环境中使用DYN-SLAM与其他主流SLAM算法进行定位精度试验。试验使用的计算平台配备Intel Ultra 7 165H处理器和32G内存。为避免硬件性能差异对试验结果的干扰，所有算法均在统一的硬件环境下进行测试。

3.1 公开数据集试验

3.1.1 试验设置

M2DGR数据集[20]在地面机器人上进行采集，机器人装备多种传感器，包括Velodyne VLP-32C激光雷达、FLIR Pointgrey RGB相机、RealSense D435i V-I传感器、iniVation DVXplorer事件相机、Ublox M8T GNSS、Ublox MINS 660 GNSS-IMU系统、HandSense九轴IMU、Leica MS60激光扫描仪以及Vicon Vero动作捕捉系统。数据集中包含由高精度GNSS-IMU和动作捕捉系统提供的地面真值位姿数据。从M2DGR数据集中选择street_02、street_04、street_08、walk_01、gate_01、gate_03六个序列：street_02序列采集自城市街道，包含夜间街道、建筑物等场景；street_04和street_08序列采集自校园，包含校内建筑和校外花坛等场景；walk_01序列包含校内和校外切换的场景，为在相对封闭且动态的环境中评估算法性能提供了良好的基础；gate_01序列包含多种户外场景，如开阔空间和建筑物，环境复杂多变；gate_03序列采集自城市，包含白天街道、人行道等动态场景。

以LVI-SAM算法作为对比，采用绝对位姿误差（absolute pose error, APE）作为主要评估指标，并进一步计算各数据集上各算法的最大误差、最小误差、均值误差、均方根误差和标准差等指标。

3.1.2 强化学习算法训练

以street_04序列为验证集，street_02、street_08、walk_01、gate_01和gate_03序列为测试集。从训练序列中提取视觉特征和激光雷达点云信息，通过IMU数据进行预积分，智能体通过与环境的交互优化各传感器权重。在训练阶段，数据集被划分为多个场景部分，每部分包含10个关键帧。每轮训练中，TD3代理随机选择场景子集，根据当前状态输出视觉和激光雷达的权重，将其注入因子图进行残差加权和图优化，并基于RPE获得奖励以更新策略。通过多轮训练，系统逐渐学会根据环境变化动态调整权重，在视觉信息退化或激光雷达数据不稳定时提高整体性能。

强化学习训练结果如图5所示，在训练初始阶段，需要充分探索未知状态以获得更好的训练结果，故奖励值较低。随着不断的训练，TD3代理所获得的奖励逐渐升高，在接近1500回合数时达到收敛状态，此时在大部分情况下估计轨迹和真值轨迹能紧密对齐，说明其选择的动作能够获得更大的奖励值，达到更好的规划效果。

3.1.3 试验结果与分析

试验结果如表1所示，在street_04序列上，本文算法最大误差为  ，最小误差为  ，均值误差为  ，均方根误差为  ，标准差为  ，和LVI-SAM相比表现最佳。在训练阶段，DYN-SLAM算法在关键指标上也明显优于LVI-SAM，在street_02序列上的最大误差为  ，远低于LVI-SAM的  ，在walk_01、gate_01和gate_03序列上也最优，在walk_01序列上最小误差为  ，明显低于LVI-SAM的  ，在street_08序列上为  ，同样低于LVI-SAM的  ，在street_02、gate_01、gate_03序列上也明显低于LVI-SAM。

在street_08序列上的均值误差为  ，优于LVI-SAM的  ，在street_02、walk_01和gate_03序列上也取得了最低的均值误差；在street_08序列上的均方根误差为  ，明显优于LVI-SAM的  ，在street_02、walk_01和gate_01序列上也远优于对比算法；标准差方面，所提算法在多个训练序列上表现稳定，尤其是在street_02序列上，标准差为  ，明显低于LVI-SAM的  ，体现了所提算法在多种环境下的鲁棒性。

图6为DYN-SLAM和LVI-SAM算法在M2DGR数据集上的定位轨迹。在所有对比序列中，所提算法偏差明显小于LVI-SAM，特别是在walk_01序列中，LVI-SAM中存在明显错位。在street_02、street_04、street_08、gate_01和gate_03序列中，所提算法始终与真值保持对齐，而LVI-SAM出现更大的误差和离散的轨迹。所提算法精度高、轨迹紧凑且漂移极小，具有更优的定位和建图精度，在俯视视角下，所提方法也表现出最佳的地图一致性，进一步验证了其优越性能。

3.2 复杂果园环境试验

3.2.1 试验设置

试验果园及试验平台如图7所示。该机器人集成了多种传感器，包括深度相机（D435i，Intel RealSense）、双目相机（ZED2，StereoLabs）、激光雷达（VLP-16，Velodyne）、惯性测量单元（SI3200G MEMS，MeiTai）以及GNSS接收机（UM482，Unicorecomm）。车载计算与处理单元为NUC9 VXQNX工作站（Ubuntu 20.04.6 LTS，Intel E-2286M CPU@2.4-5.0 GHz，32 GB RAM）。定位真值由NovAtel SPAN-CPT系统提供，该系统为增强型RTK+INS导航方案。系统开发主要基于Robot Operating System（ROS）Noetic和C++。

试验选取LVI-SAM、LIO-SAM[21]和Lego-LOAM[22]为对比算法。试验地点位于上海市宝山区的一处大型苹果果园，涵盖露天果园与温室等多样化场景。机器人自室外区域进入温室果园，遍历果园全域，最终返回起始位置。该试验场景对多传感器融合定位构成多重挑战：对于相机，机器人进出温室时经历光照突变，且行间植被外观高度相似、枝叶动态遮挡，导致视觉特征提取与匹配易失效；对于激光雷达，果园呈对称长直结构，几何约束不足易引发纵向漂移，温室内部点云特征稀疏进一步加剧退化风险；对于GNSS，温室框架结构会导致卫星信号中断和多径效应，而室外树冠的遮挡则会导致定位不稳定。

3.2.2 试验结果与分析

果园试验的机器人绝对轨迹误差和预测轨迹如表2和图8所示。如图8所示，所提算法始终与真值紧密跟随，偏差最小；而LIO-SAM和LVI-SAM算法轨迹则出现较大的偏移。LIO-SAM的最大误差为  ，最小误差  ，均值误差为  ，均方根误差为  ，标准差为  ，定位轨迹与地面真实轨迹严重偏离。

尽管LVI-SAM在LIO-SAM的基础上有所改进，但仍表现出较大的波动，最大误差为  ，均值误差为  ，均方根误差为  ，标准差为  。Lego-LOAM表现稍好，最大误差为  ，均方根误差为  ，标准差为  ，但仍落后于所提方法，所提方法的最大误差仅为  ，均方根误差为  ，标准差为  ，在所有方法中也保持最优，进一步突显了其优越的定位精度和稳定性。

本算法仍存在一定局限性：在大雾、暴雨等极端天气下，视觉与激光可能同时退化，现有自适应机制难以应对并发失效；训练阶段依赖高精度RTK-INS系统提供真值轨迹，增加了数据采集成本；若目标场景的环境特征分布与训练数据差异较大，可能需要微调训练。未来可从引入毫米波雷达等天气鲁棒传感器、开发无需真值的自监督训练机制以及基于域自适应的跨场景迁移学习等方向进行改进。

4 结论

本文针对农业机器人复杂场景定位和建图问题，研究了可根据环境条件动态调整视觉与激光雷达可信度的自适应权重调整机制，将其嵌入到多传感器融合的图优化框架中使其能够实时运行。将图优化过程建模为强化学习环境，通过学习环境中的状态输入（包括视觉特征和激光几何特征）输出连续的权重动作，直接调整图优化中各因子的贡献。在公开数据集和真实环境中的试验结果表明，本文方法能够在复杂场景中稳定运行，机器人运动轨迹与真值轨迹保持一致，在复杂果园场景定位的最大误差与均值误差分别为  与  。本文研究为复杂农业场景下多传感器融合定位与建图中的动态权重分配提供了一种可行思路，对提升系统在环境变化和传感器性能波动条件下的定位精度与鲁棒性具有一定参考意义。

本文仅做学术分享，如有侵权，请联系删文。

3D视觉方向论文辅导来啦！可辅导SCI期刊、CCF会议、本硕博毕设、核心期刊等。

添加微信：cv3d001，备注：姓名+方向+单位，邀请入群。