孙宏昌，何婉凌，蒋永翔，邓三鹏
（1.天津职业技术师范大学 机器人及智能装备研究院，天津 300222；
2.天津职业技术师范大学 天津市智能机器人技术及应用企业重点实验室，天津 300222）
随着农业机械化的发展，深松土壤的机械种类也日渐增多，目前深松机械主要为拖拉机和深松机器人[1]。农田环境下的路径规划以IMS（Inertial measurement system 惯性导航系统）和GPS（Global position system，全球定位系统）的多传感器融合为主[2]。针对路径轨迹跟踪问题，有学者设计了模糊的闭环控制方式[3]，但是模糊闭环控制对于中高速车辆效果不理想[4]；
DWA（dynamic window approach 滑动窗口法）可以在采集加速度后通过轮式里程计进行速率集合的采集[5]，由于轮子打滑会造成误差，赵明等[6]针对路径规划中局部稳定点的问题提出用域的方法解决，但是用此方法的机器人无法准确到达目标点。Melchior等[7]提出基于粒子RRT（Rapidly-exploring Random Trees 快速扩展随机树算法）的不确定性算法，但此算法不适应于室外环境；
Connolly 等[8]提出一种使用Laplace 方程的路径规划算法，但该算法不适用于特征点少的环境；
谢飞等[9]优化边界条件改进了模拟退火法，但是此方法调参复杂实用性不高。这些机器人导航定位技术有一定准确性[10]，但严重依赖GPS，在部分地区GPS 信号很差[11]，惯导系统在短时间内有准确性，但由于惯导易受到外界影响产生累计误差[12]，且机器人易忽略自身姿态引起的定位误差[13]，因此开发一种多传感器数据融合的深松机器人具有重要意义。采用移动机器人组合导航定位方式，通过多传感器系统进行融合定位，发挥移动机器人导航系统优势，修正机器人在田间作业时因GPS 信号弱引起的定位误差，从而提高系统的定位精度和可靠性。
深松机器人在solidwork 软件中经行机器人模型砖换，根据机器人运动学模型建立坐标轴、连杆和创建continue 类型joint 的旋转轴并且进行urdf 文件转换，并导入碰撞系数模型后在ROS 中显示。
搭建基于ROS 的深松机器人需要在urdf 模型的基础上优化成xacro 文件，并加入激光雷达、里程计、IMS 模块，ROS 系统下的软件可以查看机器人模型，Gazebo 会根据导入模型的运动模型进行动力学仿真，通过Gazebo 模拟器的动态模拟，能够复现深松机需要的耕地环境，搭建一个80m×80m 的仿真环境，搭建的仿真过程如图1 所示。
图1 仿真建立过程
ROS 是基于分布式框架操作机器人的开源系统，各模块通过节点（即通信单元）与节点间的松耦合连接。图2 为ROS 各节点话题通信，其节点关系为：目标发送节点/next_goel 发送下一目标点，/move_base 速度控制节点控制机器人移动，gazebo 节点/gazebo 发布雷达数据，机器人位姿/robot_state_publisher 发布/tf_static 数据到/amcl 进行定位与建图，通过/tf 进行机器人各个位姿下的坐标变换，并将变换后的位姿加载到/map 地图，将地图/map 传输到/move_base 导航功能中，根据A* 算法生成全局路径规划，下位机输出移动控制指令，/cmd_vel 控制速度实现全覆盖路径规划。
图2 ROS 各节点话题通信
根据根据深松机器人运动学模型如图3 所示，可以进一步计算机器人里程计模型下的公式推导。
图3 机器人运动学模型
（x0，y0，θ0）是0 时刻时机器人在世界坐标系下的位姿；
（x1，y1，θ1）是1 时刻时机器人在世界坐标系下的位姿；
（dx，dy，dθ）是机器人坐标系下的位姿变化增量；
（ζx，ζy，ζθ）是机器人位姿的表示ζx，ζy，表示平移，ζθ表示旋转。
深松机器人先对轮式里程计和IMS 进行积分处理，得到深松机器人的原始位姿，接着对雷达点云数据进行优化原始的优化，进行滤波后与IMS 和轮式里程计数据融合，做好初始准备后构建地图。
Cartographer 算法利用局部优化和全局优化的方法对机器人位姿进行优化[14]。深松机器人根据轮式里程计和IMS 数据融合得到机器人的初始位姿，开始进行局部优化。局部优化是由两个部分构建：子地图和扫描匹配，首先，通过迭代的方式对齐子地图参考帧和激光雷达扫描帧（激光雷达扫描图像被称为帧）构建子地图，下一步将构建好的子地图与激光雷达扫描帧的进行匹配，设置初始扫描帧位于机器人初始位姿的原点（0，0），后续扫描的点集则描述为H＝{hk}k＝1，2，3，4，5，…，k。TζM为转换系数，激光数据帧构建子地图和激光关键帧的过程就是用转换系数TζM将扫描点集转换到子地图的姿态变换的过程，通过Tζ可以严格地将扫描点从扫描帧中映射到子地图中，这个过程可以用（1）式表达：
在迭代扫描帧建立子地图后，在子地图被插入雷达扫描帧之前，需要对观测到的位姿进行优化，通过Ceres 库扫描匹配器对位姿进行优化，将姿态求解问题转化为非线性最小二乘求解问题。如公式（2）所示。
其中Tξ将ξ从雷达坐标系转换到子地图坐标系，通过对局部子地图映射中的概率值使用双三次插值进行平滑处理，并且将局部子地图中的概率值限制在[0，1]内。在求出闭环扫描帧后，进行全局地图优化部分，全局地图优化主要根据扫描帧之间的姿态关系进行全局映射优化，每个扫描帧只对应当前子地图进行映射匹配构成子地图，多个子地图构成地图，随着子图的搭建，扫描匹配处理过程中子地图、扫描帧之间的误差会随着时间而累积。在深松机器人构建子地图后继续进行激光关键帧数据采集，生成回环检测用于改善校准子地图信息。为了优化所有扫描帧和子图的位姿，采用了稀疏姿态的方法调整子地图、扫描帧的累计误差，将用于闭环优化的扫描帧的相对姿势存储在存储器中，当子图不再改变时，将子地图和扫描帧构成的组用于闭环检测，如果检测到子地图与扫描帧映射匹配良好，则将该组相对位姿添加到优化环节中。在优化子地图之间的误差后，建立地图。
全局路径规划采用A* 算法可以让深松机在诸多路径中能够快速找出最短路径，在A*算法的路径规划中，利用Close 以及Open 两个列表对节点进行储存。已经被搜索过，生成估价值的节点会被Open存储起来。估计代价最小的节点由Close 存储起来，通过Close 各节点轨迹做处理，形成移动轨迹。估价函数的设置对A* 算法来说至关重要，估价函数以（3）式子来表示：
G（n）表示耗散函数，从起始节点到节点n的真实代价值；
启发函数用H（n）来代表，研究从当前节点到目标节点的估计代价值，F（n）表示当前节点的估价函数，A*算法采用欧几里得距离：在二维平面内，假设起始点坐标为（Xx，Xy），目标点坐标（Dx，Dy），则欧几里得距离的启发函数H（n）表达式为式（4）全局路径规划的算法具体流程见图4。
图4 全局路径规划的算法具体流程
深松机在工作的时候工作环境人烟稀少，但为了避免误伤到人或动物，在全局路径规划的基础上加入局部路径规划动态窗口法（DWA 算法）实现实时避障。动态窗口法在深松机移动速度空间内（v，w）进行多组样本，根据速度模拟轨迹，通过轨迹评价函数选取适合用于选择最优运动轨迹转化为约束当前轨迹速度。为了实现动态障碍物避障与突变点的路径规划，将所有的分割区域视作待访问节点，求取经过所有节点的最短路径，能够实现最短路径，采用回溯方法来实现求解，运用优化后的ROS 框架下的move_base 导航栈功能包作为控制机器人追踪全覆盖路径的模块。系统结构图如图5 所示。
图5 move_base 导航框架
传统的全局历遍路径规划导致机器人行动缓慢深松效率低，参考纯追踪算法的思想，编写目标点位发送程序，实现目标点的动态发送，算法步骤如下：
（1）根据当前里程计反馈回来的位姿数据，计算机器人到当前目标点的欧式距离，当小于设定阈值时候，发送下一目标点；
（2）在路径回调函数中对接受的路径进行判断，当路径长度为0 或者长度没有发生变化时候，判定为上个路径，不对系统中存储的路径进行更新，反之则更新路径，并从头追踪。
4.1 SLAM 建图
采用cartogrpher 算法进行深松机器人的建图和定位，通过2D 雷达构建栅格地图的过程如下图所示，图6 使用仿真环境建立栅格地图过程。考虑到田地里田埂的高度，为了避免触碰翻越田埂，设置田埂区域为地图边界，设置地图边界的膨胀半径为11 图中深色区域表示不可通行区域，浅色区域表示可通行区域，黑色区域表示激光雷达扫描到的障碍物，灰色代表未知区域。深松机器人采用cartographer 算法作为SLAM 算法，可以构建大型地图，图6 中c 图为深松机器人进行路径规划的基础，深松机器人构建的地图边界清晰且有极高的准确性。
图6 栅格地图建立过程
4.2 路径规划及避障
基于农作物对土壤含氧量要求的不同以及土地松散度不同的情况，研究出两种深松机器人工作模式：（1）大型微耕模式：设置深松机器人的路径稀疏规划，快速开始路径规划和寻线导航，适用于土壤较为疏松且土壤含氧量较好的环境；
（2）大型深耕模式：设置深松机器人的路径稠密规划，适用于土壤较为紧实且土壤含氧量低的环境根据上一节设计的两种路径规划方式进行实验，深松机器人可以根据土壤深松模式的不同执行不同的路径规划，现进行两组不同稀疏路径导航的效果展示，进一步来说明深松机器人实验的可靠性，路径规划图如图7 所示。
图7 路径规划效果
图中细线表示深松机器人的全局路径规划路线，粗线表示深松机器人走过的路径。由于稠密的全局历遍路径规划和稀疏的全局历遍路径规划的运作场合过于特殊，通过激光雷达建立栅格地图，实现路径可视化，边缘处的小点是激光雷达扫描的田地边缘，从该界面可以清楚看到田地环境及障碍物。
用深松机器人硬件系统设计的深松机器人仿真系统根据田地情况模拟试验场地进行深松机器人仿真的可行性验证，实验先采用Cartographer 算法实现定位与建图，根据建立栅格的地图，采用A* 算法在地图上进行全局路径规划等技术的研究，确定其可行性。结果表明：
（1）采用ROS 框架搭建深松机器人，ROS 的开源性和代码易于移植的优势为机器人研发提供了更多的算法选择。
（2）采用基于Cartographer 算法实现环境中的定位与建图清晰、有效。
（3）采用基于A*算法在地图上进行路径规划等技术的研究，能够完成避障和自主导航的功能。
猜你喜欢 里程计位姿全局 融合二维图像和三维点云的相机位姿估计光学精密工程(2022年22期)2022-11-28室内退化场景下UWB双基站辅助LiDAR里程计的定位方法导航定位学报(2022年5期)2022-10-13Cahn-Hilliard-Brinkman系统的全局吸引子数学物理学报(2022年4期)2022-08-22量子Navier-Stokes方程弱解的全局存在性数学物理学报(2022年2期)2022-04-26船舶清理机器人定位基准位姿测量技术研究浙江海洋大学学报(自然科学版)(2020年5期)2020-06-19车载自主导航系统里程计误差在线标定方法数字通信世界(2019年12期)2020-01-14一种单目相机/三轴陀螺仪/里程计紧组合导航算法中国惯性技术学报(2019年1期)2019-05-21落子山东，意在全局金桥(2018年4期)2018-09-26大角度斜置激光惯组与里程计组合导航方法导航定位与授时(2016年6期)2016-03-16小型四旋翼飞行器位姿建模及其仿真湖北工业大学学报(2016年5期)2016-02-27