果园移动机器人激光雷达双源信息融合实时导航方法

刘 慧 段云鹏 沈 跃

(江苏大学电气信息工程学院, 镇江 212013)

0 引言

植保机械作业是现代化农业经济发展的前提,是林果园增效的重要保障[1-2]。林果园机械自主导航是农业移动机器人研究的一个重要组成部分,对我国农业机械智能化、农业生产精细化有着重大的促进作用[3-6]。

机器人自主导航是根据自身传感器所获取的周围环境信息,规划出前进路线并对自身进行控制,实现自主行驶[7-8]。目前广泛使用的导航传感器有全球卫星导航系统(Global navigation satellite system, GNSS)、惯性测量单元(Inertial measurement unit, IMU)、视觉相机、激光雷达等[9-12]。ZHANG等[13]使用北斗卫星导航系统和IMU实现果园中定位,利用误差状态卡尔曼滤波方法提高定位精度,但是存在果树遮挡导致定位信号丢失的问题。OPIYO等[14]使用RealSense相机获取果树信息,通过提取地面图像的中轴线作为导航路径。张振乾等[15]在香蕉园中采用改进K-means算法将道路两侧香蕉树分离,基于最小包围矩阵提取导航路径。但机器视觉导航技术受光照强度、视角范围等条件限制较大。

激光雷达传感器能精准获取环境的距离信息,相比于视觉相机传感器,具有测距精度高、抗干扰能力强等优点[16-18]。二维激光雷达因成本较低而被一些学者广泛使用。ZHANG等[19]对二维激光点云建立了不同的过滤阈值,有效消除二维激光点云数据的离散点,并基于最小二乘法对导航路径进行拟合。李秋洁等[20]采用二维激光雷达获取树干信息,提出两步树行分割法获取相邻树行数据,将树行中心线作为导航路径,但该方法只进行了仿真实验。刘星星等[21]使用二维激光雷达融合姿态传感器进行数据校正,利用最小二乘法拟合树行,结合支持向量机(Support vector machine,SVM)算法,预测果园行间中心线。二维激光扫描仪在果园导航中存在感知信息量少、无法应对枝叶茂密或果树相连成片的复杂果园环境等问题[22-23],更多学者采用三维激光雷达作为导航传感器。MENGOLI等[24]使用三维激光雷达检测树干信息并通过霍夫变换方法拟合树行。LIU等[25]使用随机采样一致性方法拟合树行。杨洋等[26]提出基于感兴趣区域(Region of interest,ROI)的甘蔗垄间导航线实时提取算法,其中甘蔗垄行拟合采用最小二乘算法。刘伟洪等[27]使用挖空打断后的树墙体心等效树干位置,根据左右树行的最佳平行度对随机采样一致性算法与最小二乘法拟合的树行进行互补融合并求其中心线得到导航线。以上研究多以检测单株或作物行为主,规划路径为直线,不利于躲避行间果树冠层与行人等障碍物。胡广锐等[28]在直线路径的基础上加入了改进人工势场法,但未对改进后的导航路径进行优化,不利于农业机械行走跟踪控制。

相较于二维激光雷达,三维激光雷达具有检测距离远(100 m)、感知信息丰富等优点,三维点云数据可同时用于导航线提取、障碍物检测与识别、对靶喷雾和即时定位与地图构建(Simultaneous localization and mapping,SLAM)等,因此,本文以三维激光雷达作为导航传感器,提出一种果园行间路径提取方法。对果园三维点云数据进行直通滤波、降采样和离群点去除等预处理,使用牛顿插值方法和非线性支持向量机方法提取行间导航线;为了提升系统的鲁棒性与容错率,将高频更新的插值导航线与低频更新的NSVM导航线进行互补融合,并对融合后导航线进行优化;基于履带式差速模型,采用纯跟踪算法对最终导航线进行跟踪。

1 材料与方法

1.1 果园移动机器人导航系统

本研究以履带式移动机器人为实验平台,其外形尺寸(长×宽×高)为1.25 m×0.85 m×1.90 m,如图1a所示。果园移动机器人主要由履带式移动底盘(bunker型,深圳市松灵机器人)、中央计算机(英特尔酷睿八代i7-8565u)、三维激光雷达(VLP-16,Velodyne公司)、组合导航系统(高精度组合导航系统X1,北云科技)和变量喷雾系统组成。其中履带式移动底盘使用无刷直流电机驱动,符合差速运动模型,轴距为0.68 m,最大行驶速度为1.5 m/s。三维激光雷达具体参数如表1所示,安装于机器人前方,距离地面1.25 m,激光雷达坐标系xlylzl与车体坐标系xbybzb如图1b所示。

表1 Velodyne VLP-16型三维激光雷达参数Tab.1 Parameters of Velodyne VLP-16 LiDAR

图1 移动机器人平台Fig.1 Platform of mobile robot1.变量喷雾系统框架 2.VLP-16激光雷达 3.中央计算机 4.高精度组合导航X1 5.GPS接收天线

本研究使用中央计算机装载Ubuntu 18.04 LTS操作系统,并基于机器人操作系统(Robot operating system,ROS)与点云库(Point cloud library,PCL)进行开发。果园移动机器人运动控制系统如图2所示,使用三维激光雷达实时获取树木点云数据,通过以太网口传输至中央计算机,进行点云处理、导航线规划和轨迹跟踪等运算,将控制指令通过CAN通信发送至下位机,实现移动机器人行走控制。

图2 移动机器人运动控制系统原理图Fig.2 Mobile robot motion control system schematic

1.2 点云预处理

(1)

式中 (Δx,Δy,Δz)——平移向量

三维激光雷达每帧原始点云数据理论上有28 800个点,为减少计算量和无关点对路径规划的影响,需要对点云数据进行预处理。调用点云库中的相应函数,设定合理的阈值参数,对三维点云进行感兴趣区域(Region of interest,ROI)划分、下采样和去除离群点等处理。

1.3 插值导航线拟合

使用DBSCAN聚类方法对预处理后的三维点云进行聚类,使用果树点云的中心位置表示果树位置,对左右两侧果树位置进行牛顿插值,得到两侧树行曲线,取树行曲线中心线为插值导航线。

DBSCAN聚类是一种基于密度的聚类算法,能够把具有足够高密度的区域划分为簇[29];KD-Tree(K-dimension tree)是一种平衡二叉树,基于KD-Tree的近邻查询算法可以加快聚类时的搜索速度[30-31]。使用一帧三维点云数据构建KD-Tree,将点云数据中同一棵树的点云数据存放在一个容器中,因为激光雷达的扫描特性,同一棵果树单次扫描只能获取果树单侧轮廓的点云信息,本文采用单棵果树点云中心表示果树位置以减小偏差,果树的等效位置Oi(xOi,yOi,zOi)为

(2)

式中xmax、xmin——单棵果树x轴最大、最小值

ymax、ymin——单棵果树y轴最大、最小值

为了减少计算量,将果树位置投影至车体坐标系的xbobyb平面。根据果树位置y轴坐标的正负可以划分为左侧果树和右侧果树。以单侧果树位置为节点,建立差商表,进行牛顿插值,如图3所示,其中x轴方向均匀插入树行点,y轴方向使用牛顿插值法进行计算。取左右两侧树行曲线的中心线为插值导航线Wp={wpi|i=0,1,2,…},导航线第i个导航点wpi(xwpi,ywpi)坐标为

图3 插值导航线拟合Fig.3 Fitting of interpolated navigation line

(3)

式中δ——间隔系数

ylefti——左侧第i个树行点y轴坐标

yrighti——右侧第i个树行点y轴坐标

1.4 NSVM导航线拟合

SVM属于机器学习监督算法中的统计分类方法,是一种二分类模型[21,32]。本文采用非线性支持向量机的方法,对左右果树冠层点云进行二分类,在树行中间寻找一条与两侧树冠点云间隔最大化的最优分类线,以此分割线为NSVM导航线。

为减少训练时间,对预处理后点云再次进行下采样和离群点去除。对二次处理后的每个点Pi(xPi,yPi)添加标签ti

(4)

在经过训练之后,对于车体坐标系xbobyb平面中的任一点pk(xpk,ypk),都可以预测其标签tk,若|tk|=0,则pk位于最大间隔分隔线上。在x轴方向上均匀地选择最大间隔分隔线上的点,将其连接成NSVM导航线Ws={wsi|i=0,1,2,…},NSVM导航线中第i个导航点wsi(xwsi,ywsi)坐标为

(5)

式中ypk——点pk的y轴坐标

tk——点pk的预测标签,为0

xpk——点pk的x轴坐标

1.5 导航线融合

插值导航线是以果树中心位置信息生成的导航线,未考虑果树边缘点信息,NSVM导航线是以果树边缘点信息生成的导航线,未考虑果树中心位置信息,因此两种方法在果园树行中形成互补。牛顿插值算法计算量较小,计算频率较高,NSVM算法计算量较大,计算频率较低,本文将高频更新的插值导航线与低频更新的NSVM导航线进行融合,融合方法为:接收到一帧点云新数据时,更新插值导航线,若此时上一帧NSVM导航线还未计算完毕,则不更新NSVM导航线,若已计算完毕,则更新NSVM导航线。记融合后导航线为Wt={wti|i=0,1,2,…},融合后导航线上第i个导航点为wti(xwti,ywti),在计算插值导航线与NSVM导航线时,设置两者在x轴方向的间隔系数δ相等,即

xwti=xwpi=xwsi=δi(i=0,1,2,…)

(6)

(7)

然后对两导航线上对应导航点的横向距离进行归一化,有

(8)

式中ρ——归一化后权重

以归一化后结果计算权重

ywti=(1-0.5ρ)ywpi+0.5ρywsi

(9)

插值导航线权重一直保持在0.5以上,若插值导航线与NSVM导航线的横向距离较小,则插值导航线权重较重,融合后导航线更接近插值导航线;反之,两者权重接近0.5,融合后导航线接近两者中间;在插值导航线与NSVM导航线横向距离最大处,两者权重都为0.5,融合后导航线在两者中间。

1.6 导航线优化

在不考虑机器人初始位置时可以计算出行间导航线,但是在后续进行轨迹跟踪时,需要对导航线进行优化,使导航线起点与机器人位置重合,即导航线经过车体坐标系原点,本文使用线性化的方法,重新计算融合后导航线前2 m内导航点的坐标:记x=2 m处的导航点为wtd2(xwtd2,ywtd2),优化后的每个导航点x轴坐标值不变,y轴坐标值ywti为

(10)

(11)

式中n——5组导航线剔除发生跳变导航线后,保留下的导航线数量

ywt1i——队列中第1组融合后导航线上第i个导航点的y轴坐标

(12)

对融合后导航线进行平滑处理,本文采用三次准均匀B样条算法。若平面中有4个点p0、p1、p2、p3,则三次准均匀B样条插值的方程为

(13)

式中t——插值的间隔比例,为插值数量的倒数

本文仅对导航点进行优化,不进行插值,所以设置t=0。要使三次B样条优化后导航点的数量不会减少,且优化后的导航线经过优化前的导航线的起点和终点,需要构造新的点p′0=2p0-p1与p′n=2pn-pn-1,其中n为融合后导航点的数量,分别将p′0添加到p0之前,将p′n添加到pn之后。平滑后导航点为

(14)

至此,导航线融合与优化全部完成。

1.7 轨迹跟踪

机器人移动底盘为履带式结构,满足差速运动学模型,如图4所示,采用纯跟踪算法进行轨迹跟踪,在前视距离l处的行驶目标轨迹上选取跟踪点P,机器人从当前位置O前进至点P的路径可以近似看作一段半径为R的圆弧,R即机器人转弯半径,根据三角函数关系有

图4 差速运动学模型示意图Fig.4 Schematic of differential kinematics model

(15)

式中l——前视距离α——航向偏差

化简后有

(16)

机器人前进线速度v为恒定速度,转弯角速度ω与前进线速度v之间的关系为

(17)

结合式(16)与式(17)可求得

(18)

2 实验

本研究在江苏省镇江市丹徒区林果园内(32.12°N,119.27°E)和江苏大学校园内(32.22°N,119.51°E)进行行间自主导航实验,如图5所示,丹徒区林果园内两侧为桂花树,行距约6.2 m,株距3.0～3.2 m,随机选择5棵桂花树进行测量,可知树高为3.0～3.5 m,树宽为1.2～1.6 m,树木大小基本一致,树行呈直线,路面较崎岖;江苏大学内实验场景两侧植株为樱花树,行距为5.5 m,株距为2.5～3.5 m,经测量可知树高为2.1～3.0 m,树宽为1.5～2.2 m,树木大小略有差距,树行呈“S”形曲线,路面较平坦;符合本研究实验要求。实验时选择相同的起始位置,分别使用插值导航线、NSVM导航线和融合后导航线进行导航,并使用纯跟踪算法进行跟踪,每种方法重复3次,记录导航线与行驶轨迹。

图5 实验场景Fig.5 Experimental scene

2.1 点云预处理结果

对原始激光点云进行坐标变换,手动测量车体坐标系到激光坐标系之间的平移向量(Δx,Δy,Δz)=(0.35 m,0,1.25 m),代入式(1),将点云转换到车体坐标系下,后续所有计算都是在车体坐标系下进行。通过PCL点云库中直通滤波算法划分感兴趣区域,林果园场景设置滤波范围在x轴方向上为(0,12.0 m),在y轴方向上为(-4.0 m,4.0 m),在z轴方向上为(-0.7 m,2.25 m);江苏大学内场景设置滤波范围在x轴方向上为(0,12.0 m),在y轴方向上为(-3.5 m,3.5 m),在z轴方向上为(-0.7 m,2.25 m),保留范围内点云数据。使用体素化网格方法实现下采样,在减少点云数量的同时保持点云的形状特性,本文两种场景中都设置体素为边长0.1 m的立方体。使用统计滤波方法移除噪声点与离群点,本文上述场景中都设置k临近搜索点数为50个,判定阈值为1.0倍标准差。林果园场景点云预处理结果如图6所示。

图6 林果园场景点云预处理结果Fig.6 Results of point cloud preprocessing in field of orchard

江苏大学校园内场景点云预处理结果如图7所示。

图7 江苏大学内场景点云预处理结果Fig.7 Results of point cloud preprocessing in field of UJS

三维激光原始点云为每帧约包含28 800个点,林果园场景中去除无效点后约包含17 000个点,感兴趣区域内包含3 000～5 500个点,下采样后包含1 100～1 300个点,去除离群点后包含800～1 000个点;江苏大学内场景中去除无效点后约包含15 000个点,感兴趣区域内包含1 000～3 000个点,下采样后包含400～600个点,去除离群点后包含300～500个点。

感兴趣区域划分后,x轴保留机器人前方12 m范围内点云信息,去除较远距离点云与机器人后方点云,y轴范围保留左右两侧树行点云信息,去除隔壁树行的点云,z轴去除地面点云,激光安装高度为1.25 m,因此可以保留0.55 m以上树木点云,针对本文中两种实验场景,都可以有效保留树木冠层点云。下采样和统计滤波后点云数量可以减少至1 000以下,下降约96.5%,极大地减少了计算量,并成功保留了树木冠层轮廓信息,为后续导航线提取做准备。

2.2 插值导航线

使用DBSCAN方法对预处理后点云进行聚类,设置聚类近邻搜索的搜索半径为0.5 m,在三维点云数据预处理后,林果园场景中一帧点云的数量有800～1 000点,一棵桂花树的平均点云数量为100～140点,因此设置DBSCAN算法中每一类的点云数目最少为80,最多为200;按照同样的方法,江苏大学内场景中设置每一类的点云数目最少为40,最多为150,如图8所示,在感兴趣区域内每棵樱花树的点云都可以成功聚为一类,使用外接立方体将每一类进行标记。

图8 点云聚类效果Fig.8 Clustering effects of point cloud

采用式(2)中的方法计算每棵树木在车体坐标系下的位置,设置式(3)中的间隔系数δ=0.2,即插值导航线上相临两导航点间的x轴距离为0.2 m,感兴趣区域在x轴方向为0～12 m,所以插值导航线共包含61个导航点。经过测试,插值算法的代码运行时间为0.022 s,所以设置插值算法的代码运行频率为10 Hz,与激光雷达频率相同。林果园场景中树行拟合与插值导航线计算结果如图9a所示,导航线与左右两侧桂花树中心的距离相同,但在感兴趣区域内单侧只能检测到3～5棵桂花树,以此为节点进行插值,所得到的树行曲线曲率变化较大,在树行曲线的首尾端曲率变化最大,因此,单独使用牛顿插值方法生成的导航线在首尾端存在曲率过大的问题。如图9b所示,在樱花树大小不均匀的校园内场景中,插值导航线与较大樱花树(树1)间的距离小于机器人宽度的一半,会发生机器人与樱花树冠层碰撞的问题。

图9 插值导航线Fig.9 Interpolated navigation line

2.3 NSVM导航线

对预处理后的点云再次进行降采样和统计滤波,体素网格降采样方法中设置网格为0.2 m×0.2 m×1 m的立方体,统计滤波器中k临近搜索点数为30个,判定阈值为1.0倍标准差,二次处理后点云数量减少至80左右。本文设置NSVM的高斯核带宽σ=7,式(5)中的间隔系数δ=0.2,与式(3)中相同。每帧点云训练和NSVM导航线提取时间约为0.3 s,所以设置NSVM部分的代码运行频率为3 Hz。林果园场景中NSVM导航线提取结果如图10a所示,导航线与左右两侧桂花树冠层边缘的距离基本相同。在樱花树大小不均匀的校园内场景中,如图10b所示,根据NSVM原理可知,较大樱花树的边缘点会被选定为支持向量,由支持向量来计算最大分割线,会导致图中NSVM导航线与树1和树2之间的距离相同,但与树3和树4之间的距离不同的问题。本实验所采用的平台为果园喷雾机器人,若单独使用非线性支持向量机的方法生成导航线,会导致机器人对不同樱花树(例如树3和树4)的喷雾量与喷雾效果不同的问题。

图10 NSVM导航线Fig.10 NSVM navigation line

2.4 融合优化导航线

设置式(6)中的间隔系数δ=0.2,将插值导航线与NSVM导航线进行融合,并对其进行优化,结果如图11所示,图中黄色曲线为插值导航线,蓝色曲线为NSVM导航线,红色曲线为融合优化后导航线,林果园场景中,融合后导航线位于插值导航线与NSVM导航线中间,且更靠近插值导航线。在樱花树大小不均匀的场景下,如图11b所示,融合优化后导航线解决了仅使用插值导航线或NSVM导航线时所存在问题,保证喷雾机器人在树行间行驶时,既不会与樱花树发生碰撞,又可以对不同体积的樱花树进行均匀喷洒。

图11 融合优化后导航线Fig.11 Integrated optimized navigation line

机器人在树行间采集100帧点云数据,分别计算插值导航线、NSVM导航线和融合后导航线,统计每个导航点的平均曲率,如图12所示,导航线前端1/3部分,插值导航线和NSVM导航线曲率都较大,原因有两点:算法自身的原因导致导航线首尾部分的曲率相较于中间部分偏大;对导航线前端部分进行了调整,使其经过车体坐标系原点;导航线中间1/3部分,3条导航线的曲率及曲率变化幅度都较小;导航线后端1/3部分,插值导航线与NSVM导航线曲率较大,原因是由算法自身导致。总体来看,融合后的导航线曲率及曲率变化都小于插值导航线和NSVM导航线,且没有发生锯齿状波动。计算每条导航线的曲率最大值、绝对平均值以及标准偏差,结果如表2所示,融合后导航线曲率最大值为0.048 m-1,平均值为0.018 m-1,标准偏差为0.011 m-1,相较于插值导航线分别下降81%、54%、74%,相较于NSVM导航线分别下降62%、55%、66%,表明融合后导航线更适合作为行间导航线。

表2 导航线曲率Tab.2 Curvature of navigation line m-1

图12 导航点平均曲率Fig.12 Average curvature of navigation point

随机选取1帧点云数据,分别计算插值导航线、NSVM导航线和融合后导航线,喷雾机器人以0.5 m/s和1.0 m/s不同的行驶速度对每条导航线进行轨迹跟踪实验,实验中设置前视距离为2.0 m;机器人的位姿通过RTK信号获取,实验结果如图13所示,图中黄色曲线为插值导航线,蓝色曲线为NSVM导航线,红色曲线为融合优化后导航线,绿色曲线为跟踪曲线,即轨迹跟踪时记录的RTK值。记录机器人行驶轨迹与导航线之间的横向误差,结果如图14所示,整体来看,机器人行驶速度较慢时,跟踪效果较好。统计各跟踪曲线横向误差的最大值、绝对平均值和标准偏差,结果如表3所示,机器人行驶速度为0.5 m/s时,融合后导航线轨迹跟踪横向误差最大值为0.104 m,平均值为0.053 m,标准偏差为0.026 m-1,相较于插值导航线分别下降47%、39%、40%,相较于NSVM导航线分别下降58%、41%、51%;行驶速度为1.0 m/s时,融合后导航线轨迹跟踪横向误差最大值为0.130 m,平均值为0.049 m,标准偏差为0.039 m,相较于插值导航线分别下降41%、36%、34%,相较于NSVM导航线分别下降46%、54%、46%。机器人在低速行驶时,RTK定位精度更准确,机器人底盘控制效果更好,因此机器人轨迹跟踪更准确。机器人以不同行驶速度对融合后导航线跟踪的最大横向误差为0.13 m,表明融合后导航线更有利于轨迹跟踪,该算法能够满足果园行间自主导航的需求。

表3 导航线跟踪横向误差Tab.3 Lateral error of navigation line tracking

图13 轨迹跟踪Fig.13 Trajectory tracking

图14 跟踪横向误差Fig.14 Tracking lateral error

3 结论

(1)对激光雷达获取的点云进行预处理以减小计算量,分别通过滤波、聚类、曲线拟合的方法计算插值导航线,通过非线性支持向量机的方法计算NSVM导航线,动态计算两者权重,将高频更新的插值导航线与低频更新的NSVM导航线进行融合与优化。

(2)融合优化后导航线平均曲率为0.018 m-1,较插值导航线平均曲率下降54%,较NSVM导航线平均曲率下降55%,表明融合后导航线更适合作为行间导航线。

(3)轨迹跟踪实验表明,行驶速度为0.5 m/s时,融合后导航线轨迹跟踪横向误差最大值为0.104 m,行驶速度为1.0 m/s时,其横向误差最大值为0.130 m,证明融合优化后导航线满足果园行间自主导航的需求,可为果园地面装备的行间自主导航提供技术参考。