基于Karto算法的割草机器人建图及其运动畸变校正*

侯可馨 ， 李霁翔 ， 邹腾跃 ， 黄浩军

（1.福建农林大学机电工程学院，福建 福州 350002；2.福建农林大学金山学院，福建 福州 350002）

1 割草机器人概况

随着农业信息化的发展，无论是智能物联网设备的部署还是机器人平台的投放，都为农业现代化的普及打下了坚实的基础，成为了农业领域的研究热点。SLAM技术可以非常有效地构建农产品区域的栅格地图，作为功能型机器人的行动基础，无论是机器人割草、运输还是自动化采摘都必须以有一幅精确的地图作为前提。

李小敏等[1]提出了以机器视觉为基础的割草机器人避障、自动返航等路径规划方法。徐晨[2]分析了机器人作业的环境特点，运用计算机视觉算法对目标进行准确识别。方言海等[3]利用数据融合与实时地图构建技术构建三维信息，为大棚内无人化作业提供了技术支持。李盛辉等[4]对IMU的姿态进行了解分析。这些方法实现的前提是要有一个精确构建的地图和里程计信息，但目前仍存在建图不准和运动畸变问题。对此，课题组提出了一种基于Karto算法的建图方法，引入G2O后端优化库，同时进行IMU及里程计标定，进行运动畸变校正，从而优化激光雷达的帧间匹配，提升回环效果。

传统的割草机器人采用的建图算法在前端里程计部分有所欠缺，很少通过回环约束匹配相似帧。若采用视觉与3D激光融合则又提高了成本，不具备经济效益，而且算法的精确度其实很大程度上依赖于先验数据的预处理[5]。

2 传感器标定

2.1 IMU的时间同步与角度积分

IMU（Inertial Measurement Unit）即惯性测量单元，是用来检测机器人相应坐标上的加速度、角速度等的多轴传感器。本文的激光数据均是采用一款低成本、高性能的思岚激光雷达RPLIDAR-A1测出的激光点数据。该雷达频率为10 Hz，而IMU的频率是100 Hz，首先要对两者进行时间戳的配准，也就是进行时间同步。在程序设计中使用双端队列进行IMU数据的预存储，所以队列中的IMU时间戳也会是连续的。需要修剪IMU的数据队列，直到接近当前帧的雷达点云数据。将单线雷达扫描一圈时间内的IMU数据进行角度积分，并存储三个方向上的角度值，从而进行数据对齐和姿态外参的联合调优。IMU模型如下：

式中，bωt是陀螺仪的偏置，bαt是加速度计的偏置，nω和nα都是IMU模型的附加噪声。在时间同步时需要计算IMU在指定时间范围内的角度积分，时间同步的示意图如图1所示。

图1 IMU与激光雷达当前帧时间同步

2.2 轮式里程计的标定

设底盘当前位姿为（x,y,θ），运动学解算增量为（dx,dy,dθ）[6]，于是本平台割草机器人的航迹推算位姿变换为：

通常采用线性最小二乘方法[7]进行里程计标定，让误差函数趋近于0，从线性空间的角度进行位姿投影。

在里程计标定过程中，由于帧间匹配算法PL-ICP计算得到的里程计数据更加准确，所以用激光雷达的scan-match数据作为真值里程计测量得到的数据为在模型中假设两者成线性关系于是对于每一组数据，可得：

合并为最小二乘的线性方程组形式：

将编码器测得的轮式里程计数据乘上这个标定矩阵，就可以得到一个校正后的里程计数据。同时，里程计数据与scan数据的时间戳同步，即可得到最终的畸变校正结果。

2.3 运动畸变校正结果

本文的实现平台基于ROS kinetic版本进行开发，畸变校正测试时，割草机器人处于园区花坛的直角转弯处。因为当机器人主要平移运动时，产生的畸变较小，所以畸变校正主要是对拐弯处的雷达数据进行姿态校正。通过ROS系统下的Rviz插件观测花坛90°拐角处机器人车身以0.8 rad/s的角速度旋转时激光点的运动畸变校正情况，如图2所示。左图是原始激光点集合与畸变校正后的激光点集合对比，右图为校正后的雷达数据点分布。畸变校正后的激光点，在数据集播放过程中旋转的雷达数据明显更加整齐。

图2 IMU与里程计联合校正激光点畸变

3 Karto建图与G2O后端优化

3.1 图优化策略

传统的SLAM算法都是基于滤波器实现的，缺点就是滤波器只估计当前的机器人位姿，而之前的累计误差不会被消除[8-9]，后期的回环检测和优化只会将错误的位姿进行优化与回环，导致与真实地图大相径庭。

本文采用的Karto建图算法是基于图优化的，在基于图优化的SLAM中，机器人的位姿是一个节点（Node）或者顶点，是待优化的变量；而位姿之间的约束关系构成边（Edge），通常表示误差项。后端采用非线性最小二乘来优化建图过程中的累计误差，最终得到一个一致性地图。所以，SLAM的图优化问题基本可以分解为两大步：首先是构建图，假设前一个位姿矩阵为H1，当前位姿矩阵为H2，那么可以认为其边即两点之间的相对位置关系为H1-1H2；其次是优化图，调整机器人的位姿，使其尽量满足边的约束。

设机器人的每个位姿为x0~i，用其相对位姿来构建残差的平方和函数：

在图优化过程中，需要使得回环位姿的残差平方和最小，最终得到机器人的最优位姿预测。回环约束是个权重占比很高的图优化约束，将这个约束放入图结构中同时进行优化，以达到消除误差的目的。

3.2 采用G2O进行后端优化

G2O（General Graphic Optimization）是基于图优化的库，契合Karto的位姿图的接口。不同于视觉中的光束平差法BA（Bundle Adjustment），G2O优化库不需要相机进行重投影来将相机姿态和特征点空间位置作出最优调整后，最后收束到相机光心。在实际应用过程中，即使图优化BA是稀疏的，也常常会出现实时性差、能耗高的问题[10]。引入G2O优化库后，只需要将Karto算法中对应的顶点和约束分别赋值给G2O对应接口即可。

相比于Ceres库而言，虽然G2O的代码实现更为复杂，但通用面更广，可以实现重定位，Ceres把优化问题定义为最小二乘问题，而G2O把优化问题表现为图的一种方式。G2O需要根据平台SLAM算法来适配参数，并分配顶点与边，其代码实现构造流程如图3所示。

图3 G2O适配Karto构造流程

程序实现步骤如下：

1）创建线性求解器LinearSolver。在图优化中，增量方程的形式为HΔX=-b，当机器人节点位姿观测数量增多时，H矩阵维数很大，求逆和求解过程很复杂，可以用C++内置的库来解决高维矩阵求逆问题。

2）创建BlockSolver，用线性求解器来初始化。割草机器人行进过程中需要姿态pose的维数和观测点，这些在程序开始运行时都无法确定，设置可变尺寸的Solver来创建块求解器。

3）创建总求解器Solver，用BlockSolver来初始化，选择LM（Levenberg-Marquardt）下降法为迭代的优化策略。

4）创建整个图优化的核心，即SparseOptimizer稀疏优化器，设置求解器为上一步定义的总求解器，并且打开调试输出。

5）自定义图优化的顶点和边，并将其添加到SparseOptimizer中。设置SparseOptimizer的初始化，并输入迭代次数最终保存结果。

4 试验与最终建图分析

在本实验平台下，将割草机器人置于一个大致100 m*30 m的花坛数据集环境进行测试，选取特征较为明显的一处花坛圆弧边缘区域进行建图特征对比分析。测试结果如图4所示。

图4 测试结果

从图4中观察到，图4（c）花坛圆弧边缘不再出现叠图现象，且构建的二维栅格地图边界重合，不存在分层现象，对割草机器人的稳定运行起到了至关重要的作用。因此，运动畸变校正、里程计标定及高效的后端优化策略，对于SLAM建图来说缺一不可。即使有算法效率再高的后端优化库，若前端帧间匹配有扰动，激光点数据畸变不予校正，所传入后端优化的粗糙原始数据也会被不断迭代，导致最终建图失败，进而导致部署的机器人运行不稳定，降低作业效率。

5 结束语

本文在Karto算法的基础上，增加实时运动畸变校正和里程计标定功能，并引入了G2O后端图优化库，实现了割草机器人在工作过程中的稳定高效，而不会出现建图失败或者定位失败等问题。利用IMU和轮式里程计的结合校正激光点位姿，使得割草机器人可以以一定速率行进，而且行进过程中不会产生建图失败或者叠图的现象。同时由于农产品园区通常是回环的，所以当引入G2O优化时，有一个优化权重占比很大的起始和终止相对位姿，为割草机器人后续要实现的各种功能打下了稳定的基础。该方案的设计架构还可以进一步改进，可以通过深度相机来得到三维的环境地图，同时将相机的像素点引入算法的激光雷达帧间匹配，进而获得更好的后端优化效果。