激光雷达与视觉融合的跟随运输机器人设计*

赵铖钥，马伟，苏道毕力格，谭彧

(1.中国农业大学工学院,北京市,100083;2.中国农业科学院都市农业研究所,成都市,610213)

0 引言

根据WCO最新统计数据,2021年全球柑橘总产量达15 850万吨,而我国达到了4 460万吨,居世界首位,且近几年产能还在逐步扩大[1-2]。然而,与之配套的柑橘收获装备短板问题突出。目前我国柑橘收获时普遍采用人力劳动,但是大量农村人员涌入城市,造成了柑橘采收获季节的劳动力短缺,制约着我国柑橘产业的发展[3]。要夯实柑橘产业发展基础,就必须提高我国柑橘收获机械化和自动化水平,而果园机械自动导航技术是实现机械化和自动化的关键,是目前研究的热点。目前,果园机械自动导航技术的研究多集中在单一功能自动化装备关键技术研究,主要导航方式有GPS导航、机器视觉导航、激光雷达导航以及多传感器融合导航等[4-8]。但由于果园环境的复杂程度较高,GPS信号易受到高山、树冠的遮挡产生误差,激光雷达和视觉容易受到环境中杂草等因素的影响丢失道路特征信息,导致导航信息失效[9];而机器人引导与自主跟随导航可以降低环境因素的影响,提高导航稳定性、可靠性,还可以满足运输机器人跟随采摘机器人前进的协同工作模式。目前国内外农业领域的跟随导航研究主要集中在农田环境中[10-13],在果园环境中的研究比较少。丁勇前等[14]设计一种基于红外传感器的车辆自主跟随控制系统,使用阵列红外测距传感器,获取引导车和跟随车之间的相对航向偏角,通过控制前轮转向角跟随前进,经过试验表明该系统能实现车辆的自主跟随,系统运行稳定可靠;毕伟平等[15]设计一种基于双目视觉的果园作业车辆跟随系统,使用黑白棋格特征板作为引导特征,跟随机器人通过双目视觉识别黑白棋格获取引导和跟随车辆之间的相对位姿信息,最终可以实现车辆自主跟随。这些为果园环境中的运输机器人跟随导航提供了研究思路。

视觉可以通过指定颜色将引导目标与环境进行明显区分,二维激光雷达可以提取出目标距离、航向偏角等多种信息,且提取算法易于实现,二者融合可以提高跟随系统的精准性和稳定性。针对柑橘采摘时的运输工作,本文搭建两个履带式机器人,采摘机器人作为引导机器人,由操作员驾驶,在机器人后方固定一个红色矩形特征板;运输机器人作为跟随机器人,通过搭载的二维激光雷达和视觉传感器,获取引导机器人特征板的二维信息,计算出二者的相对位姿,通过控制算法使跟随机器人进行自主跟随导航。在模拟环境中,对不同运行速度和轨迹情况,对建立的机器人跟随系统进行跟随稳定性试验和行间跟随停车定位精准性试验。

1 系统构建

1.1 硬件构建

跟随机器人系统包括激光雷达视觉系统与机器人平台,激光雷达视觉系统主要由2D LiDAR、摄像头、装有Ubuntu18.04操作系统的工控机以及机器人平台组成。2D LiDAR为一款单线二维激光雷达,安装在移动机器人正中心,其扫描角度为360°,在帧率为8 Hz时水平角分辨率为0.96°,点频率为8 kHz,最大测距为12 m,测距精度为0.02 m;视觉传感器为高清免驱摄像头,安装在激光雷达正前下方,其分辨率设置为640×480,帧率为30帧/s,视场角为90°。

移动机器人以STM32F1为主控板,主进程通过UART总线接收激光雷达视觉系统传输的速度控制信息;从进程通过读取编码器获得两侧履带的实际前进速度,并输出两侧电机的控制信号,实现机器人前进与差速转向。图1为跟随机器人系统硬件平台。

图1 跟随机器人系统硬件平台Fig.1 Hardware platform of following robot system1.移动机器人 2.显示器 3.工控机 4.2D LiDAR 5.摄像头

引导机器人的标志物为一红色矩形特征板,垂直放置在引导机器人后方,用于引导跟随机器人,引导机器人由操作者遥控前进,引导机器人与跟随机器人构成的系统如图2所示。

图2 引导跟随机器人系统Fig.2 Guided-following robot system

1.2 软件构建

1.2.1 特征识别方法

本文选取红色矩形板作为引导目标,如图3(a)所示。通过与环境颜色进行区分,来确定目标在视觉图像中的位置,获取引导机器人在跟随机器人坐标系下的角度范围。本文将彩色图像从RGB(Red,Green and Blue)空间转换到HSV(Hue,Saturation and Value)颜色空间中,分别调节H、S、V三个分量的阈值,获得二值图像,如图3(b)所示。其中目标的白色像素点最多,记录相连区域的白色像素点,数量最多的为兴趣区域(ROI),并使用最大矩形在图像中框选出兴趣区域(ROI),如图3(c)所示。矩形框含有左右两侧边框像素横坐标信息x1,x2,然后通过计算获取目标方向,其方向计算示意图如图3(d)所示。

(a) 输入图像

(1)

1.2.2 点云提取方法

目标点云提取的示意图如图4所示。设置跟随机器人局部坐标系的原点与激光雷达中心重合,x轴为前进方向,y轴垂直前进方向向左。获取与图像相同时刻的激光雷达数据并选取正前方180°的数据点,根据图像识别角度范围提取激光点云。该范围的点云中包含目标点云和无效点云,需要进行聚类以及特征提取,获得目标点云信息。

图4 目标点云示意图Fig.4 Schematic diagram of the target point cloud

由于二维激光雷达是单线扫描式,激光数据点Pi以极坐标形式(li,θi)存储,对点云遍历并计数,通过距离公式计算相邻两点距离di,如式(2)所示。

(2)

当相邻点间距离超过设置阈值时,判断点集计数值k,当大于指定数量时就保存到点云类列表G,直至遍历完成。

通过坐标转换公式将极坐标转换到笛卡尔坐标系下,如式(3)所示。

(3)

根据每个点云类的两侧端点坐标(xl,yl)(xr,yr)求解出直线方程Ax+By+C=0的各个系数,并根据式(2)计算出点云边界长度li,然后根据点到直线距离公式,计算点云类中每个点到该直线的距离Di,如式(4)所示。

(4)

然后求出平均距离,由于本文特征为直线,平均距离趋近于0,故选择小于阈值且与实际目标长度最接近的点云类,最终提取出目标点云集Gt。

1.2.3 导航计算方法

如图5所示,引导机器人中心O为原点,跟随机器人中心O′为原点,与激光雷达中心重合。通过获取的目标点云集Gt提取出两侧边缘点的距离值lA、lB,角度值θA、θB,然后通过计算得到两机器人的纵向距离d、横向偏差f以及航向角偏差θ三个导航参数。

图5 机器人相对位置示意图Fig.5 Schematic diagram of the robots relative position

由于激光雷达无法精准测得矩形板的两侧端点,故其长度l通过激光雷达获得的端点计算获得,激光雷达相邻点间隔角度较小,其产生的误差在本文中可以进行忽略,得到l的计算公式,如式(5)所示。

(5)

跟随机器人与引导机器人的纵向距离d的计算公式,如式(6)、式(7)所示。

d=h+lAsinα

(6)

(7)

式中：α——点云Gt左侧边缘点线束与矩形板夹角,(°);

h——引导机器人中心到矩形板的距离,m。

跟随机器人与引导机器人的横向偏差f的计算公式,如式(8)所示。

(8)

跟随机器人与引导机器人的航向角偏差θ的计算公式,如式(9)、式(10)所示。

(9)

(10)

式中：β——点云Gt右侧边缘点线束与矩形板夹角,(°)。

1.2.4 跟随控制方法

跟随机器人的控制框图如图6所示,其前进控制使用测得的纵向间距d和期望间距之间的误差作为输入,输出跟随线速度v,控制机器人前进;转向控制根据测得的航向角偏差θ和横向偏差f与期望偏差之间的误差作为输入,输出跟随机器人的角速度ωc,控制机器人转向;然后进行差速模型运动学逆向运算,获取两侧履带的目标线速度vl和vr,以编码器返回的线速度作为反馈,控制输出两侧履带电机的PWM电压信号,最终实现机器人的运动调整。

图6 跟随机器人控制框图Fig.6 Block diagram of follow-control rob

2 试验结果与分析

2.1 检测标定试验

将引导机器人放置在跟随机器人前方0°,±20°航向,两者直线距离为1.2 m,然后对引导机器人的航向进行0°,±20°旋转,产生9种相对位置情况,每种情况下采集10次数据,通过检测识别得到跟随标志的激光数据。

图7为跟随机器人±20°航向的检测结果,根据式(6)、式(8)、式(9)获得两者的纵横向偏差和航向角偏差,通过与摆放位置实际值相减计算三者的平均偏差,其中平均纵向偏差为0.012 m,标准差0.031 m,平均横向偏差0.015 m,标准差0.022 m,平均航向角偏差0.18°,标准差0.54°,因此本文提出的检测方法具有较高精度,可以满足跟随要求。

(a) -20°航向、-20°姿态 (b) -20°航向、+20°姿态

2.2 算法对比试验

对比引导机器人在0.3 m/s的运行速度下分别以直线和90°圆弧轨迹前进时,跟随机器人应用几种传统控制算法的跟随性能。设定轨迹长度为5 m,目标纵向距离1.2 m,目标横向偏差0 m,目标航向角偏差0°,跟随机器人以8 Hz的频率返回获取的导航参数,并计算偏差结果,试验结果见表1。

表1 不同控制算法对比Tab.1 Comparison of different control algorithms

从表1可以看出相对于单独的模糊控制,PID算法和模糊PID算法对跟踪性能均有提升,而PID算法和模糊PID算法在纵向、横向以及航向角三者的平均偏差虽互有不同,但趋于近似,算法并未有显著提升,由于PID算法参数的调节难度更低、使用更加方便,且控制结果基本满足了跟随要求,故选用PID算法作为跟随系统的控制算法。

2.3 跟随性能试验

本试验主要研究在不同运行速度和轨迹的情况下,运输机器人以指定距离和航向角跟随引导机器人的跟随性能。引导机器人在0.3 m/s、0.4 m/s、0.5 m/s的三种运行速度下,分别以直线和90°圆弧(圆弧半径1.5 m)轨迹行进,设定轨迹长度为5 m,运输机器人自动跟随引导机器人,目标纵向距离1.2 m,目标横向偏差0 m,目标航向角偏差0°,跟随机器人以8 Hz的频率返回获取的导航参数,并计算偏差结果,表2为试验的纵向偏差统计结果,表3为试验的横向偏差统计结果,表4位试验的航向角偏差统计结果。

表2 纵向偏差试验结果Tab.2 Test results of longitudinal deviation

表3 横向偏差试验结果Tab.3 Test results of lateral deviation

表4 航向角偏差试验结果Tab.4 Test results of yaw deviation

由表2～表4可知,运行速度对跟随性能的影响较小,可以通过PID算法的参数进行修正;跟随机器人对于直线轨迹行驶跟随性能较好,平均纵向偏差小于0.015 m,标准差小于0.08 m,平均横向偏差小于0.01 m,标准差小于0.018 m,平均航向偏差小于1.107°,标准差小于1.665°;而跟随90°弧线轨迹行驶时,纵向偏差小于0.078 m,平均横向偏差在0.3 m左右,最大横向偏差达到了0.56 m,平均航向角偏差在21°左右,最大航向角偏差达到了43°,虽然圆弧轨迹跟随时横向偏差和航向角偏差较大,但是并未丢失跟随目标,满足跟随任务要求。

2.4 停车定位试验

由于柑橘运输机器人跟随采摘机器人在行间工作时,采摘机器人需要停下进行采摘并把柑橘存放到运输机器人的储存仓内,所以运输机器人需要停止在采摘机器人机械臂投放范围内。本文设置引导机器人在0.3 m/s、0.4 m/s、0.5 m/s的速度下直线行驶,进行跟随机器人停车定位试验,试验结果如表5所示。可以看出随着速度的上升,纵向偏差和横向偏差的值都有所上升,在最大运行速度0.5 m/s下,其最大纵向偏差为0.205 m,最大横向偏差为0.053 m,可以满足实际使用。

表5 停车定位试验结果Tab.5 Test results of position when stopping

3 结论

本文提出了一种柑橘运输机器人目标检测定位及协同导航方法,并在模拟环境下对该方法的工作质量进行了验证,该方法为柑橘自主运输机械提供了技术支撑,为实际果园工作奠定基础。

1) 通过视觉图像HSV阈值分割融合激光点云聚类特征识别方法,提取目标引导标志物信息,实现相对位姿获取。并对该方法进行了识别定位精度试验,平均横、纵向偏差均小于0.015 m,平均航向角偏差0.018°,证明该检测方法具有较高精度。

2) 通过试验对比不同跟随控制算法,选择了适合本系统的PID控制算法,该方法参数调节方便且满足工作要求。并在不同速度、轨迹条件下对跟随性能进行了试验,试验表明速度的升高对跟随精度影响不大。在最大0.5 m/s的跟随速度下,直线轨迹的平均横、纵向偏差小于0.015 m,平均航向偏差1.107°,跟随性能良好;弧线轨迹跟踪偏差较大,平均横向偏差在0.3 m左右,但能满足跟随任务要求。

3) 通过跟随停车定位试验,纵向偏差小于0.205 m,证明本系统在运输时满足存储工作条件。