本文以自主研发的智能割草机作为实验平台，整体结构与传感器布置如图 1a所示。该智能割草机底盘采用后轮驱动，前轮(惰轮)转向，最大行走速度为2 m/s。根据实验需求，在智能割草机上安装内部集成IMU的ZED 2双目相机传感器和RTK接收机，搭载了英特尔i7-10875H型CPU和英伟达RTX 2060型GPU的笔记本电脑作为运算单元。传感器主要参数如表 1所示。

图 1b展示了实验中的坐标系及相关符号定义。W代表世界坐标系；B代表割草机机身坐标系，以IMU安装位置确定；C代表相机坐标系；D代表RTK测量坐标系；ENU代表(东北天)站心坐标系；下标B代表某实体在B系中的位置；B系到D系的刚性变换T_B^D∈SE(3)由R_B^D和t_B^D组合得到；R_B^D∈SO(3)代表D系相对于B系的旋转；$ \boldsymbol{t}_B^D \in \mathbb{R}^3$代表D系相对于B系的位移。

智能割草机RTK-VIO融合定位算法架构如图 2所示，该算法基于ORB-SLAM3^[16]框架实现。系统主要包括跟踪线程、局部建图线程、闭环和地图融合线程。跟踪线程接收割草机双目视觉帧和IMU数据帧，实时进行ORB特征提取和IMU积分，计算当前帧相对于活动地图的姿态，并进行关键帧的选取。关键帧和RTK测量帧在局部建图线程中加入活动地图，通过局部窗口进行视觉惯性局部BA(Bundle Adjustment)优化。在闭环和地图融合线程中，系统以关键帧频率进行共同区域检测，根据共同区域是否属于当前活动地图进行闭环矫正或地图融合，同时以独立线程完成全局BA工作。

软件系统基于ROS(Robot Operating System)实现。系统输入主要包含3个节点，分别是ZED 2传感器节点、RTK接收机节点和SLAM节点。ZED 2传感器节点发布消息类型为sensor_msgs/Image的双目相机视频信号话题，以及消息类型为sensor_msgs/Imu的IMU信号话题；RTK接收机节点发布消息类型为sensor_msgs/NavSatFix的RTK定位信号话题；SLAM节点订阅上述3个话题并完成SLAM工作；系统输出TUM格式的定位结果。

系统运行前需要进行RTK-VIO初始化，该初始化问题可以描述为一个MAP估计问题，系统通过初始化来获得割草机速度、重力方向和IMU偏差初始值。在ORB-SLAM3的框架中，系统需要进行纯视觉优化、纯IMU优化，以及视觉和IMU联合优化3个步骤^[22]。

首先，需要将RTK测量信息加入第一步纯视觉优化中。在系统运行的前2 s内，使用纯视觉BA进行优化，此时将RTK与纯视觉优化位姿进行对齐。系统以4~10 Hz频率插入关键帧，通过视觉里程计得到k_m=10个关键帧相机位姿和由数百个地图点组成的地图。此时的地图不在真实尺度下，它和现实大小之间存在一个比例因子s，用符号“-”表示尺度未知的参数。这些相机位姿表示为B系下的$ \boldsymbol{T}_{B_k}^{B_0}=[\boldsymbol{R}_{B_k}^{B_0}, s \overline{\boldsymbol{t}}_{B_k}^{B_0}]$。通过式(1)将相机位姿与RTK位置对齐到同一坐标系：

式中：t_D^B由RTK接收机安装位置决定；$ \boldsymbol{R}_{B_k}^{B_0} $、$ \overline{\boldsymbol{t}}_{B_k}^{B_0}$来自相机位姿$ \boldsymbol{T}_{B_k}^{B_0}=\left[\boldsymbol{R}_{B_k}^{B_0}, s \overline{\boldsymbol{t}}_{B_k}^{B_0}\right]$。

通过RTK测量得到ENU坐标系下的位置，然后通过插值获得与相机位姿对应的$ \boldsymbol{t}_{D_k}^{E N U}$。此时，通过Umeyama方法^[23]解决最小二乘估计问题，用于求解$ \boldsymbol{T}_{B_0}^{E N U}=\left[\boldsymbol{R}_{B_0}^{E N U}, \boldsymbol{t}_{B_0}^{E N U}\right]$和比例因子s：

用包含RTK测量的T_B0^ENU来代替VIO初始化中纯视觉优化的结果，接着再使用T_B0^ENU和这些关键帧之间的IMU测量值，继续完成包含IMU数据的初始化。

对于智能割草机的定位问题，用于优化分析的状态变量包括：智能割草机机身姿态T_B^W=[R_B^W，t_B^W]，割草机速度ν，陀螺仪偏差b^g和加速度计偏差b^a，得到i时刻的状态向量为：

采用基于关键帧的非线性优化状态估计问题来描述RTK-VIO的融合过程。这个过程发生在局部建图线程的局部BA优化中，每当一个新的关键帧插入，就进行一次局部BA优化，用这个新的关键帧和最后k个关键帧，以及这些关键帧所观察到的l个3D地图点进行优化。该过程的因子图如图 3所示，当关键帧插入时，最新的k个关键帧构成蓝色虚线框所示的局部窗口，系统将优化这些关键帧以及它们所能观察到的所有特征点。而与这k个关键帧具有共视关系但不在局部窗口中的关键帧构成红色虚线框所示的固定窗口，固定窗口中的关键帧提供残差用于优化，但关键帧本身不参与优化。特别的是，第k+1个关键帧总是被添加到固定窗口中，提供IMU约束。设这一组k+1个关键帧对应的状态为$ \overline{\boldsymbol{S}}_k=\left[\boldsymbol{S}_0, \cdots, \boldsymbol{S}_k\right]$，l个3D地图点对应的状态为X=[x₀，…，x_l-1]，那么优化问题可以表示为：

式中：K_j为能够观察到3D点j的关键帧集合；K^*为与RTK测量关联的关键帧集合；r_Gi为在RTK-VIO方法中加入的RTK残差项；r_Ii-1，i和r_νij分别为IMU的惯性残差和视觉的重投影残差，这2项根据ORB-SLAM3的建议按照Forster等^[24]提出的方法给出。

给定一个时间间隔[t_i-1，t_i]，惯性残差r_Ii-1，i由式(5)计算：

式中：ΔR_i-1，i、Δν_i-1，i、Δp_i-1，i由i-1和i帧之间的IMU预积分得到。

在i帧和3D点j的位置x_j之间，重投影残差由式(6)计算得到：

式中：u_ij为图像i中空间3D点j的观测位置；Π：$ \mathbb{R}^3 \rightarrow \mathbb{R}^n$为相机的投影函数，将3D点投影到图像平面中；T_B^C由双目相机在智能割草机的安装位置决定；T_B^W为B系到W系的刚性变换；⊕为SE(3)群对$ \mathbb{R}^3 $元素的变换。

当智能割草机通过接收机接收到RTK定位信息时，首先进行RTK帧与关键帧的关联，具体方法是在每个关键帧所在的一个时间邻域内寻找RTK帧。如果时间差小于规定的阈值，则认为RTK帧与关键帧关联。对于关联到关键帧时刻t_i的RTK帧，将测量值转换为ENU坐标系表示，记作$ {\mathop {{{\boldsymbol{t}}}}\limits^ \wedge }_i^{E N U}$。在局部BA滑动窗口中的第1个与RTK测量关联的关键帧时刻记为t₀。RTK残差项表示为：

式中：R_W^ENU由滑动窗口中的前20个关联的RTK测量值通过Umeyama方法与相机位姿对齐确定；t_D^B由RTK接收机在智能割草机的安装位置确定；T_Bi^W=[R_Bi^W，t_Bi^W]为i时刻由B系到W系的刚性变换；t_B0^W和ν_B0^W为第1个与RTK测量关联的关键帧时刻，B系与W系之间的平移和速度，且在滑动窗口的优化过程中保持固定；g^W为当地重力加速度。协方差矩阵Σ_Gi可根据RTK接收机技术文档给出，或根据返回的HDOP值确定：

关于位姿误差状态的雅可比行列式定义为：

式中：δ为扰动模型，本文选择对B系进行扰动，即右扰动；[t_D^B]^×表示t_D^B的反对称矩阵。

本文实验在2个真实的割草作业环境中开展。图 4a所示的场地1为一处校园绿化草地区域，草地区域大小约为28 m×40 m，草地中有景观灌木、乔木若干。场地1局部特征如图 4c所示，草地已进行了一定程度的修剪，整体比较平整。场地1中间有一条宽约2.5 m的人行道将草地一分为二，实验将在图 4a中左侧区域进行。由于场地1存在形态各异的景观灌木、乔木，能够为视觉特征点的识别提供更多有效信息，并且地面相对平整，振动噪声较小，可作为定位难度较小的作业环境。图 4b所示的场地2为一处人工草场，草场大小约为40 m×40 m，草场四周有零星分布的乔木。场地2局部特征如图 4d所示，草高约为10~15 cm。由于场地2比较空旷，能够提供有限的视觉信息，并且地面相对于场地1更加起伏不平，割草机作业时会产生更多振动噪声，因而场地2作为定位难度较大的作业环境。

在2个实验场地中设计3条不同的作业路径，用于模拟简单、一般、困难3种不同难度的工况来验证算法，如图 5所示。路径1的实验在场地1中进行，而路径2和路径3的实验在场地2中进行。路径1和路径2均为环形路径，在路径的始端和末端采用方向错位或位置错位的方法避免闭环，以模拟真实作业路径中缺少闭环信息的情况。路径3采用折返路线，更加符合智能割草机实际工作中遍历草地的工作路径。同时，场景中存在多个转向，而转向操作对SLAM效果影响最大，因此路径3代表了最为困难的工况。

为获得运动路径真值，在实验过程中使用1台无人机在实验场地上空定点悬停，并以90°俯视角同步拍摄视频。然后抽取视频帧，通过与RTK定位点的时间戳进行帧对齐和多帧合成得到路径真值，用于与实验结果进行对照。使用基于Python的SLAM评估工具EVO^[25]，对本文方法与对照方法的定位轨迹进行定量分析，计算本文方法与对照方法的估计位姿相对于真值位姿的绝对位姿误差(Absolute Pose Error，APE)，并计算误差均值和均方根误差。

图 6展示了简单工况下的实验结果。蓝色轨迹是利用本文提出的RTK融合VIO方法得到的结果，橙色轨迹是使用ORB-SLAM3 VIO方法得到的结果。本文方法的估计位姿相对于真值位姿的绝对位姿误差均值为0.931 m，均方根误差为1.016 m；对照方法的绝对位姿误差均值为1.219 m，均方根误差为1.237 m。可以看到，由于简单工况下环境视觉信息充足，作业过程振动较弱，2种定位方法都体现了较高的定位精度。然而，缺少闭环检测会导致误差累积产生一定的漂移，在轨迹末端(图 6中放大区域)仍然可以观察到ORB-SLAM3 VIO方法得到的定位结果偏离了实际轨迹，而本文方法避免了这种漂移。

图 7展示了一般工况下的实验结果。可以看到，在场地2开展的一般工况下的实验，由于严重缺乏环境视觉信息，且作业过程振动较强，相比简单工况，2种方法的定位效果都有一定程度降低。本文方法的估计位姿相对于真值位姿的绝对位姿误差均值为1.403 m，均方根误差为1.691 m；对照方法的绝对位姿误差均值为2.418 m，均方根误差为2.617 m。在开始的直线运动中，2种方法都能保持较高的精度，而从第1个转弯(图 7中右上方放大区域)开始，定位轨迹就产生了漂移。ORB-SLAM3 VIO方法在漂移的同时还发生了明显的抖动，而本文方法虽然也发生了一定程度的漂移，但避免了这种抖动，获得了和真实轨迹一致的光滑曲线。在轨迹末端(图 7中左下方放大区域)，由于误差累积，2种方法都在一定程度上偏移了真实轨迹，本文定位方法更加接近真实轨迹。

图 8展示了困难工况下的实验结果。由于困难工况下包含较多的转弯，基于视觉的SLAM方法在转弯处会发生明显的漂移。从路径的第1个转弯开始，2种方法都发生了不同程度的漂移，导致定位计算的转角和实际转角不一致。随着经过的转弯增多，该误差不断累积。本文方法的估计位姿相对于真值位姿的绝对位姿误差均值为0.857 m，均方根误差为0.932 m；对照方法的绝对位姿误差均值为0.985 m，均方根误差为1.069 m。因此，本文提出的RTK-VIO融合定位方法更加接近真实轨迹。

2种方法的误差分布对比如图 9所示，统计结果如表 2所示。本文方法与对照方法相比，简单、一般、困难3种工况下的绝对位姿误差均值分别降低了0.288 m、1.015 m、0.128 m，均方根误差分别降低了0.221 m、0.926 m、0.137 m。综上可知，本文提出的RTK-VIO融合定位方法相对于ORB-SLAM3 VIO方法具有明显提升。

在实时性方面，将本文方法与对照方法实验的局部BA过程耗时和局部建图总耗时平均值和标准差进行对比，如表 3所示。由表 3可知，本文方法在加入RTK耦合后，虽然耗时增加在毫秒级，但标准差表现更加稳定，可以认为基本不影响算法的实时性能。

1) 提出一种智能割草机的RTK-VIO融合定位算法，以缓解割草机场景下定位漂移和丢失的情况。将RTK定位测量通过紧耦合方法融合进ORB-SLAM3的VIO框架中，提出了该问题下RTK残差的计算方法和RTK-VIO初始化方法，并通过设计简单、一般、困难3种难度工况的实验验证其效果。

2) RTK-VIO融合定位算法与对照方法ORB-SLAM3 VIO相比，在简单、一般、困难3种难度工况的实验中，绝对位姿误差均值和均方根误差均有明显降低，且通过实时性分析得出本文方法相比对照方法无明显的实时性负面影响。

3) 本文方法得到的定位结果在困难场景中的表现可能受限于RTK接收机信号频率仅为1 Hz的条件，尽管相对于对照方法有明显提升，但误差绝对值仍然偏大，不能很好地满足智能割草机自动化作业需求。未来将考虑针对更新频率更高的RTK接收机进行紧耦合优化方法设计，以期对定位精度和鲁棒性有进一步提升。