多传感器融合的温室机器人自主导航系统设计

吴 凡, 张 华, 童 以, 刘苏杭

(安徽科技学院 机械工程学院,安徽 凤阳 233100)

近年来,随着世界经济的发展,智能化产业不断升级,以温室大棚为代表的设施农业逐渐代替传统农业[1]。随着科技的进步,温室智能装备可有效减少劳动者在高温、高湿度及存在有害气体的温室环境下发生危险的概率,降低劳动强度,提升综合作业效率[2]。中国在温室智能装备方面起步较晚,较国外存在较大差距,尤其是在温室自主导航移动机器人方面[3];同时温室空间有限、种植密度高及非结构化的环境,进一步加大了自主导航难度[4]。因此,对于温室机器人的自主导航研究具有重要意义。

温室中完成自主导航首先需要采用SLAM技术[5]构建环境地图。激光SLAM主要是基于粒子滤波的Gmapping算法[6],算法易于实现,但在大范围场景下会发生粒子退化问题。基于高斯牛顿的Hector算法[7],对硬件性能要求较高。基于图优化Cartographer算法[8],提出子图的概念,通过回环检测在硬件性能较低情况下运行效果较好,但算法整体计算量较大。地图构建后需要规划路径,路径规划包括全局及局部路径规划[9-11],全局路径规划根据地图进行静态规划,为局部路径规划提供导向和约束,局部路径规划完成躲避路径上出现的动态障碍物。全局路径规划有Dijkstra算法[12]、A*算法[13]、蚁群算法[14]等,局部路径规划有DWA算法[15]、人工势场法[16]等。由于单一传感器的导航系统存在鲁棒性差等问题。季宇寒等[17]设计了基于激光雷达的巡检机器人导航系统,能够稳定运行,但单一雷达存在扫描盲区。金珍珍等[18]融合相机与激光雷达,设计的导航系统便于移植,但单一里程计存在累计误差。顾皞伟等[19]将超声波与激光雷达结合实现了对透明物体的检测。Emmi等[20]融合激光雷达与图像实现道路边界检测,但存在枝叶覆盖遮挡时会导致检测不准确,不适用于存在遮挡的复杂场景。

本研究通过ROS系统,使用前后激光雷达减少机器人扫描盲区,融合里程计、IMU信息,使用优化参数配置的Cartographer算法进行定位建图,并将定位信息应用到Navigation优化导航框架,结合Dijkstra算法与DWA算法完成路径规划,实现自主导航控制,设计的导航系统能够满足温室环境下运行需求。

1 材料与方法

1.1 系统硬件

系统的硬件架构如图1所示,IMU型号为轮趣科技N100,输出频率为400 Hz。激光雷达型号为思岚科技A1M8,测量范围为12 m。搭载于中科深谷移动作业机器人平台上,通过轮式编码可以实时发送里程计信息。传感器在采集信息后实时发送到工控机中进行环境建模和最优路径规划,将规划信息发送至下位机,解算后发送速度指令到电机驱动器,控制机器人按照预设轨迹移动,实现自主导航功能。

1.2 系统软件

系统软件架构如图2所示,通过ROS(Ubuntu18.04-Melodic版本),采用Cartographer算法、Navigation导航框架为核心的控制系统架构。采用Cartographer算法进行定位与建图,通过Navigation功能框架在建好的地图基础上进行定位与路径规划,并与下位机通过串口通信。由于AMCL只使用里程计与激光雷达信息,定位精度较差,将Cartographer定位信息应用到Navigation框架中取代原有定位算法进行定位,配合Dijkstra算法与DWA算法完成路径规划。

图2 系统软件架构Fig.2 System software architecture

Cartographer算法将激光雷达点云数据融合后通过体素滤波器减少误差及噪点[21],使用自适应体素滤波器进一步进行计算采样,压缩点云数量的同时保留关键位置信息,同时里程计和IMU会预测位姿并对激光雷达的畸变进行校正。通过局部SLAM不断建立子图,全局SLAM配合传感器数据估计自身位姿,通过扫描匹配将子图连接生成全局地图,完成地图构建。

局部SLAM中,每次得到一帧点云数据后,会与其最近的子图进行匹配,找到最佳位姿用以构建下一个子图[22]。由于返回点云数据是在激光雷达坐标系下,需进行坐标转换:

(1)

其中,ξx、ξy为点云横纵坐标,ξθ为旋转角,Tξ为点云在子图坐标系下位姿,p为栅格存在障碍物的概率。一定数量的点云数据插入后构成一个子图,每当有新点云插入时,会对栅格概率值进行更新,以减少动态障碍物的影响。一帧点云数据插入子图之前,还会使用Ceres扫描匹配器再对扫描位姿进行优化,找到点云对应概率最大的位姿。该过程是一个最小二乘问题,最优位姿通过计算点云与当前子图匹配程度的误差,使用Ceres扫描匹配器进行迭代使其误差最小,匹配程度通过使用残差函数来计算,残差函数公式为:

(2)

其中,Msmooth为平滑函数,hk为点云数量。全局SLAM中,通过回环检测和回环优化将子图间累计误差消除[23]。回环检测过程中如果当前的点云和已创建的子图中的点云匹配位姿相似,则完成回环检测。同时为了降低计算量和提高效率,Cartographer算法使用分支定界的方法进行加速处理。回环优化过程中,通过稀疏位姿的方法优化点云和子图间的位姿,与雷达点云插入子图过程类似,仍使用残差函数计算其误差。

通过Navigation[24]功能框架实现导航,通过传感器获取环境信息,结合SLAM构建地图将多层代价地图叠加,使用Cartographer定位取代框架中AMCL定位,在接收位置信息基础上,进行规划到目标点的路径。规划的步骤如下:(1)机器人在栅格代价地图上获得目标位置信息后调动全局规划器进行静态规划,使用Dijkstra算法以起点为中心遍历到目标点的所有节点,依据权重求解出最优的路径;(2)当全局规划路径中有动态障碍物时,通过局部规划器使用DWA算法依据机器人运动模型、障碍物信息、全局路径信息等在全局路径上规划移动路线,避开障碍物;(3)路径规划完成后,核心节点move_base会发布指令,控制机器人移动,完成导航任务。通过可视化工具Rviz能够实时查看导航过程。

1.3 多传感器融合

机器人在导航的过程中依赖外部传感器获取信息,传感器的信息精确与否对导航的可靠性与稳定性有着极大的影响。通过分析单一传感器的优势与缺点进行互补,使获取的信息更准确。轮式编码器依据两轮间的速度推算出移动距离,但机器人在温室中车轮极易发生打滑,导致推算的信息产生误差。IMU由加速度计、陀螺仪组成,不通过车轮进行推算,因此不受打滑影响,能够弥补里程计因打滑失准问题。因此,使用多个传感器互补,能极大地提高导航的可靠性与稳定性。

IMU与里程计融合时需结合机器人的线速度、角速度与两轮速度之间的转换关系。图3为机器人的运动学模型,θ1为机器人绕圆弧转动的角度,θ3为航向角变化量,l为两轮的间距,d为左右两轮因速度差产生的距离,r为机器人运动半径。机器人线速度、角速度、左轮速度、右轮速度分别设为v、ω、vl、vr。

图3 差速运动学模型Fig.3 Differential kinematics model

机器人的前进速度等于左右两轮速度的平均值,从图3中可知几何关系θ1=θ2=θ3,可得出机器人线速度、偏转角度、角速度分别为式(3)、(4)、(5):

v=(vr+vl)/2

(3)

θ3=θ1=d/l=(vr-vl)ht/l

(4)

ω=θ3/ht=(vr-vl)/l

(5)

通过式(3)～(5),在单位时间t0内采集1次两轮速度,可计算出机器人的线速度为vx,角速度为vθ,则机器人单位时间内x、y方向的位移为式(6)、(7),旋转角度如式(8)所示:

Dx=Vxcosθt0

(6)

Dy=vxsinθt0

(7)

Dθ=vθt0

(8)

通过以上计算可得里程信息,如式(9)所示:

Xt=Xt-1+Dx,Yt=Yt-1+Dy,θt=θt-1+Dθ

(9)

通过将IMU测量出的旋转角速度vθ代入式(8)可实现与里程计的信息融合。

2 结果与分析

2.1 定位精度测试

在ROS系统中使用含激光雷达、里程计、IMU信息的rosbag数据集,分别使用单一里程计与激光雷达及加入IMU融合后预测的位姿点位信息进行录制,通过MATLAB读取rosbag数据集记录的位姿坐标信息,绘制轨迹对比图,不同轨迹点位如图4所示。

图4 轨迹对比图Fig.4 Trajectory comparison chart

从图4可以看出,仅使用里程计信息预测的轨迹相较于参考轨迹有较大误差。而融合IMU后的轨迹更加接近参考轨迹,相较于仅使用里程计信息,融合后的定位精度有显著提升。

2.2 路径规划算法仿真对比

为选取合适的全局路径规划算法,对Navigation功能框架中Dijkstra算法和A*算法进行对比。构建仿真环境,如图5所示,黑色部分代表障碍物,白色部分代表无障碍可行走区域,绿色圆点表示起点,蓝色圆点表示终点,×符号表示搜索的点数,分别使用A*算法和Dijkstra算法进行规划路径。从图5可以看出,A*算法相较于Dijkstra算法搜索点数更少,搜索更有目的性,但规划的不是最优路径,且路径中拐点过多,在复杂的实际情况下频繁转弯更容易发生碰撞,不利于机器人导航的安全,综合考虑选取Dijkstra算法作为全局规划算法。

图5 仿真规划路径对比Fig.5 Comparison of simulation planning paths

2.3 仿真导航试验

为确定导航系统能否稳定运行,进行仿真验证。在Gazebo中配置Word文件、机器人模型URDF文件构建仿真环境,如图6所示。然后运行仿真机器人启动的launch文件,在ROS中订阅机器人前后激光雷达的“/scan_1”、“/scan_2”话题及IMU与里程计的“/imu”、“/odom”话题,通过键盘节点订阅“/cmd_vel”话题控制机器人在仿真环境中运行,机器人在运动过程中环境地图被完整构建出来,地图构建过程如图7所示。构建的地图仿真环境基本一致,没有严重的漂移、重影等现象。地图中的黑色图线表示扫描到的障碍物,机器人无法通行;白色区域表示无障碍物区域,机器人可正常通行;地图边界上的绿色点集为激光雷达扫描到障碍物的点;地图中灰色剩余区域表示未搜索扫描的区域。

图6 Gazebo仿真环境与机器人模型Fig.6 Gazebo simulation environment and robot model

图7 可视化地图构建Fig.7 Visual Map Construction

建图完成后,将Cartographer定位模式单独调用并发布定位信息,通过Rviz可显示机器人在地图上位置,如图8所示。通过观察Gazebo仿真环境与Rviz中机器人位置,机器人本体通过TF树描述出来,机器人朝向通过红色箭头表示,分别在起始位置(图8(a))与移动后的位置(图8(b))对比机器人定位精准性,可看出二者位置基本保持一致。

定位完成后,将配置yaml文件写入启动导航框架的launch文件中实现自主导航,如图9所示。绿色点集为雷达扫描到障碍物的点;灰色凸显区域为静态障碍物膨胀层,防止机器人与障碍物发生碰撞;机器人周围白色方框为局部规划范围,可识别动态障碍物并进行规划。给定目标位置后,分别进行无动态障碍物自主导航(图9(a))和有动态障碍物自主导航(图9(b)),可看出机器人在给定目标位置后,规划出路径后不断向目标点移动,同时对于出现的动态障碍物会规划路径躲避,完成自主导航。

2.4 实际导航试验

为验证机器人在实际场景中运行的准确性和稳定性,进行实际场景导航试验。如图10(a)所示,地图构建完成后,机器人在给定目标点情况下,结合传感器及地图信息避开静态障碍物规划出路径,沿着规划路径朝目标点前进。在规划好的路径上引入动态障碍物,雷达会扫描出障碍物信息,当进入局部规划范围时,能够结合障碍物信息规划出路径避开,如图10(b)所示。如图10(c)所示,避开障碍物后沿规划路径到达目标点。由此可得出导航系统能够稳定运行且满足温室环境下运行需求。

3 结论与讨论

通过ROS系统完成导航系统的设计,分析各单一传感器的不足之处,结合各传感器优点进行互补,使用前后激光雷达、IMU、里程计多个传感器获取环境信息并进行位姿估计,解决了机器人因为打滑而存在的定位失准,以及扫描盲区等问题,提高了环境感知的准确性,实现了机器人从起始位置到目标位置的自主导航。本试验验证了该系统可以满足运行需求,可为同类研究提供参考。