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

2023年8 月第54卷第8 期农业报机械学doi:10.6041/j.issn.1000-1298.2023.08.024果园移动机器人激光雷达双源信息融合实时导航方法刘慧段云鹏鸟沈跃（江苏大学电气信息工程学院，镇江2 12 0 13）摘要：为提高林果园移动机器人导航系统的精确性与鲁棒性，提出一种基于激光雷达三维点云的果园行间高低频双源信息融合实时导航方法。首先，喷雾机器人搭载三维激光雷达采集两侧果树点云信息，对原始点云数据进行直通滤波、降采样和统计滤波等预处理，保留感兴趣区域内果树冠层点云；然后，将分别基于高频更新的牛顿插值算法和低频更新的非线性支持向量机（Non-linear supportvectormachine,NSVM）算法拟合的行间导航线进行互补融合；最后，在导航线切换时，对融合后导航线的稳定性进行优化，并使用三次B样条算法使导航线平滑。实验结果表明：融合优化后的导航线最大曲率为0.0 48 m-1，平均曲率为0.0 18 m；分别以0.5m/s和1.0 m/s的行驶速度对融合优化后的导航线进行跟踪，绝对横向偏差最大值分别为0.10 4m和0.130 m，平均值分别为0.0 53m和0.049m，说明该导航方法能够满足作业装备在果园行间自主导航作业的需求，为喷雾机器人在果园环境中的自主导航提供技术参考。关键词：果园机器人；激光雷达；自主导航；树行拟合；非线性支持向量机；导航线融合中图分类号：TP242.6文献标识码：A文章编号：10 0 0-12 98（2 0 2 3)0 8-0 2 49-10OSID：Real-time Navigation Method of Orchard Mobile Robot Based onLaser Radar Dual Source Information FusionLIU HuiDUAN YunpengSHENYue(School of Electrical and Information Engineering,Jiangsu University,Zhenjiang 212013,China)Abstract:In order to improve the accuracy and robustness of the navigation system of forest orchardmobile robot,a real-time navigation method was proposed based on high and low frequency dual sourceinformation fusion between orchard rows based on laser radar three-dimensional point cloud.Firstly,thespray robot carried a 3D laser radar to collect the point cloud information of the fruit trees on both sides,and the original point cloud data were preprocessed such as through filtering,down sampling,andstatistical filtering,so as to retain the point cloud of the fruit tree canopy in the region of interest.Thenthe inter row navigation lines fitted by the Newton interpolation algorithm based on high-frequency updateand the non-linear support vector machine（NSVM)a l g o r i t h m b a s e d o n l o w-f r e q u e n c y u p d a t e w e r ecomplementary fused.Finally,when the navigation line was switched,the stability of the mergednavigation line was optimized,and a cubic B-spline algorithm was used to smooth the navigation line.The experimental results showed that the maximum curvature of the fusion optimized navigation line was0.048 m-1,and the average curvature was 0.018 m-1.The fusion optimized navigation line was trackedat the speed of 0.5 m/s and 1.0 m/s,respectively,and the maximum absolute lateral deviation was0.104 m and 0.130 m,and the average value was 0.053 m and 0.049 m,respectively,which showedthat this navigation method can meet the requirements of autonomous navigation of operating equipmentbetween rows in the orchard,which provided a technical reference for autonomous navigation of sprayrobots in the orchard environment.Key words:orchard robot;laser radar;autonomous navigation;tree row fitting;nonlinear support vectormachine;navigation line fusion收稿日期：2 0 2 3-0 1-0 3修回日期：2 0 2 3-0 4-0 1基金项目：国家自然科学基金项目（32 17 190 8）和江苏高校优势学科项目（PAPD）作者简介：刘慧（198 0 一），女，教授，博士生导师，主要从事农业机器人智能感知与控制和多传感器信息融合研究，E-mail：a mi t y u j s.e d u.c n2023年农250业机报学械0引言植保机械作业是现代化农业经济发展的前提，是林果园增效的重要保障1-2 。林果园机械自主导航是农业移动机器人研究的一个重要组成部分，对我国农业机械智能化、农业生产精细化有着重大的促进作用3-6 机器人自主导航是根据自身传感器所获取的周围环境信息，规划出前进路线并对自身进行控制，实现自主行驶7-8 。目前广泛使用的导航传感器有全球卫星导航系统（Global navigation satellite system，GNSS）、惯性测量单元（Inertialmeasurement unit，IMU）、视觉相机、激光雷达等9-12 。ZZHANG 等13使用北斗卫星导航系统和IMU实现果园中定位，利用误差状态卡尔曼滤波方法提高定位精度，但是存在果树遮挡导致定位信号丢失的问题。OPIYO等14 使用RealSense相机获取果树信息，通过提取地面图像的中轴线作为导航路径。张振乾等15 在香蕉园中采用改进K-means算法将道路两侧香蕉树分离，基于最小包围矩阵提取导航路径。但机器视觉导航技术受光照强度、视角范围等条件限制较大。激光雷达传感器能精准获取环境的距离信息，相比于视觉相机传感器，具有测距精度高、抗干扰能力强等优点16-18 。1。二维激光雷达因成本较低而被一些学者广泛使用。ZHANG等19 对二维激光点云建立了不同的过滤阈值，有效消除二维激光点云数据的离散点，并基于最小二乘法对导航路径进行拟合。李秋洁等2 0 1 采用二维激光雷达获取树干信息，提出两步树行分割法获取相邻树行数据，将树行中心线作为导航路径，但该方法只进行了仿真实验。刘星星等2 1 使用二维激光雷达融合姿态传感器进行数据校正，利用最小二乘法拟合树行，结合支持向量机（Supportvectormachine，SVM）算法，预测果园行间中心线。二维激光扫描仪在果园导航中存在感知信息量少、无法应对枝叶茂密或果树相连成片的复杂果园环境等问题2 2-2 3，更多学者采用三维激光雷达作为导航传感器。MENGOLI等2 4 使用三维激光雷达检测树干信息并通过霍夫变换方法拟合树行。LIU等2 5 使用随机采样一致性方法拟合树行。杨洋等2 6 】提出基于感兴趣区域（Region of interest，ROI)的甘蔗垄间导航线实时提取算法,其中甘蔗垄行拟合采用最小二乘算法。刘伟洪等2 7 使用挖空打断后的树墙体心等效树干位置，根据左右树行的最佳平行度对随机采样一致性算法与最小二乘法拟合的树行进行互补融合并求其中心线得到导航线。以上研究多以检测单株或作物行为主，规划路径为直线，不利于躲避行间果树冠层与行人等障碍物。胡广锐等2 8 在直线路径的基础上加入了改进人工势场法，但未对改进后的导航路径进行优化，不利于农业机械行走跟踪控制。相较于二维激光雷达，三维激光雷达具有检测距离远（10 0 m）、感知信息丰富等优点，三维点云数据可同时用于导航线提取、障碍物检测与识别、对靶喷雾和即时定位与地图构建（Simultaneouslocalization andmapping，SLA M）等，因此，本文以三维激光雷达作为导航传感器，提出一种果园行间路径提取方法。对果园三维点云数据进行直通滤波、降采样和离群点去除等预处理，使用牛顿插值方法和非线性支持向量机方法提取行间导航线；为了提升系统的鲁棒性与容错率，将高频更新的插值导航线与低频更新的NSVM导航线进行互补融合，并对融合后导航线进行优化；基于履带式差速模型，采用纯跟踪算法对最终导航线进行跟踪1材料与方法1.1果园移动机器人导航系统本研究以履带式移动机器人为实验平台，其外形尺寸（长宽高）为1.2 5m0.85m1.90m，如图1a所示。果园移动机器人主要由履带式移动底盘（bunker型，深圳市松灵机器人）、中央计算机（英特尔酷睿八代i78 56 5u）、三维激光雷达（VLP-16,Velodyne公司）、组合导航系统（高精度组合导航系统X1,北云科技）和变量喷雾系统组成。其中履带式移动底盘使用无刷直流电机驱动，符合差速运动模型，轴距为0.6 8 m，最大行驶速度为1.5m/s。三维激光雷达具体参数如表1所示，安装于机器人前方，距离地面1.2 5m，激光雷达坐标系xyz与车体坐标系xy2女如图1b所示1-2w06143.851.25m(a）硬件示意图（b）局部坐标系示意图图1移动机器人平台Fig.1Platformofmobilerobot1.变量喷雾系统框架2.VLP-16激光雷达3.中央计算机4.高精度组合导航X15.GPS接收天线251刘慧等：果园移动机器人激光雷达双源信息融合实时导航方法第8 期表1VelodyneVLP-16型三维激光雷达参数Tab.1Parameters of VelodyneVLP-16LiDAR参数数值测距范围/m0100精度/cm3水平视角(）360垂直视角（）30(1515)水平分辨率/（）0.1 0.4垂直分辨率/（）2顿率/Hz520本研究使用中央计算机装载Ubuntu18.04LTS操作系统，并基于机器人操作系统（Robotoperatingsystem,ROS）与点云库（Point cloud library,PCL）进行开发。果园移动机器人运动控制系统如图2 所示，使用三维激光雷达实时获取树木点云数据，通过以太网口传输至中央计算机，进行点云处理、导航线规划和轨迹跟踪等运算，将控制指令通过CAN通信发送至下位机，实现移动机器人行走控制。中央计算机ROS以太网口点云导航线轨迹三维激光雷达处理规划跟踪CANSBUS遥控器下位机行走控制图2移动机器人运动控制系统原理图Fig.2Mobile robot motion control system schematic1.2点云预处理首先进行坐标变化，将激光雷达坐标系下的原始点云转换至车体坐标系下，记原始点云中第i个点为p（x,y i,z），转换至车体坐标系下的第i个点为p（x,y,z）,计算方法为一010 x Ay6yi=-100yi+(1)L260012式中(x,A y,z)平移向量三维激光雷达每顿原始点云数据理论上有28800个点，为减少计算量和无关点对路径规划的影响，需要对点云数据进行预处理。调用点云库中的相应函数，设定合理的阈值参数，对三维点云进行感兴趣区域（Regionofinterest，RO I）划分、下采样和去除离群点等处理1.3插值导航线拟合使用DBSCAN聚类方法对预处理后的三维点云进行聚类，使用果树点云的中心位置表示果树位置，对左右两侧果树位置进行牛顿插值，得到两侧树行曲线，取树行曲线中心线为插值导航线。DBSCAN聚类是一种基于密度的聚类算法，能够把具有足够高密度的区域划分为簇2 9;KD-Tree（K-d i m e n s i o n t r e e）是一种平衡二叉树，基于KD-Tree的近邻查询算法可以加快聚类时的搜索速度30-31。使用一帧三维点云数据构建KD-Tree,将点云数据中同一棵树的点云数据存放在一个容器中，因为激光雷达的扫描特性，同一棵果树单次扫描只能获取果树单侧轮廓的点云信息，本文采用单棵果树点云中心表示果树位置以减小偏差，果树的等效位置0(x0i,y0:,z0）为max+xmin2Ymax+ymin(2)Yo2(zo:=0式中Xmax/Xmin单棵果树x轴最大、最小值Ymaxymin单棵果树轴最大、最小值为了减少计算量，将果树位置投影至车体坐标系的xy平面。根据果树位置y轴坐标的正负可以划分为左侧果树和右侧果树。以单侧果树位置为节点，建立差商表，进行牛顿插值，如图3所示，其中x轴方向均匀插入树行点，y轴方向使用牛顿插值法进行计算。取左右两侧树行曲线的中心线为插值导航线W,=w,li=0,1,2,导航线第i个导航点w,（x w p i,y w p）坐标为=i(i=0,1,2,.)wPiYlfn,+Yrighi(3）wPi2式中间隔系数Ylefi左侧第i个树行点轴坐标Yright右侧第i个树行点轴坐标6树行点果树位置导航点0图3插值导航线拟合Fig.3Fitting of interpolated navigation line1.4NSVM导航线拟合SVM属于机器学习监督算法中的统计分类方2023年农252机业报学械法，是一种二分类模型2 1,32 。本文采用非线性支持向量机的方法，对左右果树冠层点云进行二分类，在树行中间寻找一条与两侧树冠点云间隔最大化的最优分类线，以此分割线为NSVM导航线。为减少训练时间，对预处理后点云再次进行下采样和离群点去除。对二次处理后的每个点P（x p.y p）添加标签t(yp,0)(4)(yp;0 为高斯核带宽。在经过训练之后，对于车体坐标系xo平面中的任一点p（x p，p），都可以预测其标签t，若lt,l=0,则pk位于最大间隔分隔线上。在x轴方向上均匀地选择最大间隔分隔线上的点，将其连接成NSVM导航线W,=w,li=0,1,2,.,NSVM导航线中第i个导航点w（x w s，y w s）坐标为=8i(i=0,1,2,.)wSi(5)=ypk(It;/=0,xp,=8i)式中ypk点P的y轴坐标一一点Pk的预测标签，为0点Pk的x轴坐标1.5导航线融合插值导航线是以果树中心位置信息生成的导航线，未考虑果树边缘点信息，NSVM导航线是以果树边缘点信息生成的导航线，未考虑果树中心位置信息，因此两种方法在果园树行中形成互补。牛顿插值算法计算量较小，计算频率较高，NSVM算法计算量较大，计算频率较低，本文将高频更新的插值导航线与低频更新的NSVM导航线进行融合，融合方法为：接收到一顿点云新数据时，更新插值导航线，若此时上一顿NSVM导航线还未计算完毕，则不更新NSVM导航线，若已计算完毕，则更新NSVM导航线。记融合后导航线为W,=w,li=0,1,2，,融合后导航线上第i个导航点为w,（x m，Ym），在计算插值导航线与NSVM导航线时，设置两者在轴方向的间隔系数相等，即=x=8i(i=0,1,2,.)(6)wt;wPi以插值导航线为基准，将NSVM导航线进行融合，首先找到插值导航线与NSVM导航线之间的最大横向距离ws=maxl y up:-Yus:(7)wps然后对两导航线上对应导航点的横向距离进行归一化，有P(8)ywps式中p一归一化后权重以归一化后结果计算权重ym,=(1-0.5p)yup,+0.5pyu(9)WS插值导航线权重一直保持在0.5以上，若插值导航线与NSVM导航线的横向距离较小，则插值导航线权重较重，融合后导航线更接近插值导航线；反之,两者权重接近0.5,融合后导航线接近两者中间；在插值导航线与NSVM导航线横向距离最大处，两者权重都为0.5,融合后导航线在两者中间。1.6导航线优化在不考虑机器人初始位置时可以计算出行间导航线，但是在后续进行轨迹跟踪时，需要对导航线进行优化，使导航线起点与机器人位置重合，即导航线经过车体坐标系原点，本文使用线性化的方法，重新计算融合后导航线前2 m内导航点的坐标：记x=2m处的导航点为Wta（a Yma），优化后的每个导航点轴坐标值不变，y轴坐标值yut.为(m/2 m)yuti(10)wti(x:2 m)对融合后导航线进行稳定化，使用队列存储最近5组融合后导航线（W(),W(2)，W(s），根据队列先进先出的特性，若获得一组最新的融合后导航线,则丢弃最早的一组。使用随机采样一致性方法去除5组导航线中发生跳变的1组或2 组，对剩余的比较稳定的导航线进行平均化处理，得到稳定后导航线W,=w,li=0,1,2,稳定后导航线上第i个导航点wn（x u t，y mn）坐标为*:=8i(i=0,1,2,.)Yul,+yw2,+ywni(0n5)wft;n(11)式中n5组导航线剔除发生跳变导航线后，保留下的导航线数量Yul,队列中第1组融合后导航线上第i个导航点的轴坐标在接收到一顿新点云数据后，会生成一条新的稳定后导航线Wne，此时机器人会舍弃掉上一顿点253第8 期刘慧等：果园移动机器人激光雷达双源信息融合实时导航方法云计算的稳定后导航线Wla，在进行导航线切换时，机器人航向角会发生突变，因此将连续两帧点云计算的稳定后导航线进行融合，保证机器人前进过程中的连续性和平稳性。融合方法为Yuf.=r0.8 m+.2 m.last(0 xm.1)neunewnewwtdlt(1 m,2)newnewnewwtdl+wtd2wtd1wtlnewnewwtd2newYuft(2xu;(12)式中(xu,yu)新一顿导航线第i个导航点的坐标lastYuf一上一顿导航线第i个导航点y轴坐标对于0 1m内的导航点，进行加权处理，新一顿导航线权重为0.8，上一顿导航线权重为0.2；对于1 2 m内的导航点，使用线性插值方法生成；对于2 m以后的导航点，使用新一导航线的点来替代。记新的稳定导航线上x=1m处的导航点为x.y），x=2 m处的导航点为wm（(xwtd2newnewnew,newWftdlwtd2对融合后导航线进行平滑处理，本文采用三次准均匀B样条算法。若平面中有4个点poPIP2、P3，则三次准均匀B样条插值的方程为厂-13-317Po363P1t?t 1-33P21410(tE0,1(13)式中t一插值的间隔比例，为插值数量的倒数本文仅对导航点进行优化，不进行插值，所以设置t=0。要使三次B样条优化后导航点的数量不会减少，且优化后的导航线经过优化前的导航线的起点和终点，需要构造新的点p。=2 p。-P与p=2p，-p n-1,其中n为融合后导航点的数量,分别将p。添加到p。之前，将p，添加到p,之后。平滑后导航点为=i(i=0,1,2,)121(14)wft3+2至此，导航线融合与优化全部完成。1.7轨迹跟踪机器人移动底盘为履带式结构，满足差速运动学模型，如图4所示，采用纯跟踪算法进行轨迹跟踪，在前视距离1处的行驶目标轨迹上选取跟踪点R1a图4差速运动学模型示意图Fig.4Schematic of differential kinematics modelP，机器人从当前位置O前进至点P的路径可以近似看作一段半径为R的圆弧,R即机器人转弯半径，根据三角函数关系有1R(15)sin(2)Tsin2式中1一前视距离化简后有R=式中1一前视距离航向偏差1R=2sin(16)机器人前进线速度为恒定速度，转弯角速度与前进线速度之间的关系为(17)R结合式（16）与式（17）可求得2usin(18)二12实验本研究在江苏省镇江市丹徒区林果园内（32.12 N,119.2 7 E）和江苏大学校园内（32.2 2 N，119.51E）进行行间自主导航实验，如图5所示，丹徒区林果园内两侧为桂花树，行距约6.2 m，株距3.03.2m，随机选择5棵桂花树进行测量，可知树高为3.0 3.5m，树宽为1.2 1.6 m，树木大小基本一致，树行呈直线，路面较崎岖；江苏大学内实验场景两侧植株为樱花树，行距为5.5m，株距为2.53.5m，经测量可知树高为2.1 3.0 m，树宽为1.52.2m，树木大小略有差距，树行呈“s”形曲线，路面较平坦；符合本研究实验要求。实验时选择相同的起始位置，分别使用插值导航线、NSVM导航线和融合后导航线进行导航，并使用纯跟踪算法进行跟踪，每种方法重复3次，记录导航线与行驶轨迹。2.1点云预处理结果对原始激光点云进行坐标变换，手动测量车体坐标系到激光坐标系之间的平移向量（x,Ay,z）=（0.35m，0,1.2 5m），代人式（1），将点云转换到车2542023年农机业报学械（a）林果园场景(b）江苏大学内场景图5实验场景Fig.5Experimental scene体坐标系下，后续所有计算都是在车体坐标系下进行。通过PCL点云库中直通滤波算法划分感兴趣区域，林果园场景设置滤波范围在x轴方向上为（0，12.0m），在y轴方向上为（-4.0 m，4.0 m），在z轴方向上为（-0.7 m,2.25m）；江苏大学内场景设置滤波范围在x轴方向上为（0,12.0 m），在y轴方向上为（-3.5m,3.5m），在z轴方向上为（-0.7 m，2.25m），保留范围内点云数据。使用体素化网格方法实现下采样，在减少点云数量的同时保持点云的形状特性，本文两种场景中都设置体素为边长0.1m的立方体。使用统计滤波方法移除噪声点与离群点，本文上述场景中都设置k临近搜索点数为50个，判定阈值为1.0 倍标准差。林果园场景点云预处理结果如图6 所示。(a）原始点云（b）感兴趣区域划分(c）下采样(d）统计滤波图6林果园场景点云预处理结果Fig.6Results of point cloud preprocessing in fieldof orchard江苏大学校园内场景点云预处理结果如图7所示。三维激光原始点云为每顿约包含2 8 8 0 0 个点，林果园场景中去除无效点后约包含17 0 0 0 个点，感兴趣区域内包含30 0 0 550 0 个点，下采样后包含11001300个点，去除离群点后包含8 0 0 10 0 0 个点；江苏大学内场景中去除无效点后约包含150 0 0个点，感兴趣区域内包含10 0 0 30 0 0 个点，下采样后包含40 0 6 0 0 个点，去除离群点后包含30 0 500个点。感兴趣区域划分后，x轴保留机器人前方12 m(a）原始点云（b）感兴趣区域划分(c）下采样(d)统计滤波图7江苏大学内场景点云预处理结果Fig.7Results of point cloud preprocessing in field of UJS范围内点云信息，去除较远距离点云与机器人后方点云，轴范围保留左右两侧树行点云信息，去除隔壁树行的点云，轴去除地面点云，激光安装高度为1.25m，因此可以保留0.55m以上树木点云，针对本文中两种实验场景，都可以有效保留树木冠层点云。下采样和统计滤波后点云数量可以减少至1000以下，下降约96.5%，极大地减少了计算量，并成功保留了树木冠层轮廓信息，为后续导航线提取做准备。2.2插值导航线使用DBSCAN方法对预处理后点云进行聚类，设置聚类近邻搜索的搜索半径为0.5m，在三维点云数据预处理后，林果园场景中一帧点云的数量有8001000点，一棵桂花树的平均点云数量为100140点，因此设置DBSCAN算法中每一类的点云数目最少为8 0，最多为2 0 0；按照同样的方法，江苏大学内场景中设置每一类的点云数目最少为40，最多为150，如图8 所示，在感兴趣区域内每棵樱花树的点云都可以成功聚为一类，使用外接立方体将每一类进行标记感兴趣区城感兴趣区域桂花树樱花树(a）林果园场景（b）江苏大学内场景图：点云聚类效果Fig.8Clustering effects of point cloud采用式（2)中的方法计算每棵树木在车体坐标系下的位置，设置式（3)中的间隔系数8=0.2,即插值导航线上相临两导航点间的x轴距离为0.2 m，感兴趣区域在轴方向为0 12 m，所以插值导航线共255刘慧等：果园移动机器人激光雷达双源信息融合实时导航方法第8 期包含6 1个导航点。经过测试，插值算法的代码运行时间为0.0 2 2 s，所以设置插值算法的代码运行频率为10 Hz，与激光雷达频率相同。林果园场景中树行拟合与插值导航线计算结果如图9a所示，导航线与左右两侧桂花树中心的距离相同，但在感兴趣区域内单侧只能检测到3 5棵桂花树，以此为节点进行插值，所得到的树行曲线曲率变化较大，在树行曲线的首尾端曲率变化最大，因此，单独使用牛顿插值方法生成的导航线在首尾端存在曲率过大的问题。如图9b所示，在樱花树大小不均匀的校园内场景中，插值导航线与较大樱花树（树1）间的距离小于机器人宽度的一半，会发生机器人与樱花树冠层碰撞的问题。树行曲线树行曲线插值导航线插值导航线树2树1树4树3树树5树7(a）林果园场景(b）江苏大学内场景图9插值导航线Fig.9Interpolated navigation line2.3NSVM导航线对预处理后的点云再次进行降采样和统计滤波，体素网格降采样方法中设置网格为0.2 m0.2m1m的立方体,统计滤波器中k临近搜索点数为30 个，判定阈值为1.0 倍标准差，二次处理后点云数量减少至8 0 左右。本文设置NSVM的高斯核带宽=7,式（5)中的间隔系数8=0.2,与式(3)中相同。每帧点云训练和NSVM导航线提取时间约为0.3s，所以设置NSVM部分的代码运行频率为3Hz。林果园场景中NSVM导航线提取结果如图10 a所示，导航线与左右两侧桂花树冠层边缘的距离基本相同。在樱花树大小不均匀的校园内场景中，如图10 b所示，根据NSVM原理可知，较大樱花树的边缘点会被选定为支持向量，由支持向量来计算最大分割线，会导致图中NSVM导航线与树1和树2 之间的距离相同，但与树3和树4之间的距离不同的问题。本实验所采用的平台为果园喷雾机器人，若单独使用非线性支持向量机的方法生成导航线，会导致机器人对不同樱花树（例如树3和树4）的喷雾量与喷雾效果不同的问题。2.4融合优化导航线设置式（6)中的间隔系数8=0.2,将插值导航一NSVM导航线一NSVM导航线树1树2树3树4树5树6（a）林果园场景（b）江苏大学内场景图10NSVM导航线Fig.10NSVMnavigationline线与NSVM导航线进行融合，并对其进行优化，结果如图11所示，图中黄色曲线为插值导航线，蓝色曲线为NSVM导航线，红色曲线为融合优化后导航线，林果园场景中，融合后导航线位于插值导航线与NSVM导航线中间，且更靠近插值导航线。在樱花树大小不均匀的场景下，如图11b所示，融合优化后导航线解决了仅使用插值导航线或NSVM导航线时所存在问题，保证喷雾机器人在树行间行驶时，既不会与樱花树发生碰撞，又可以对不同体积的樱花树进行均匀喷洒插值导航线插值导航线NSVM导航线NSVM导航线融合后导航线融合后导航线（a）林果园场景（b）江苏大学内场景图11融合优化后导航线Fig.11Integrated optimized navigation line机器人在树行间采集10 0 顿点云数据，分别计算插值导航线、NSVM导航线和融合后导航线，统计每个导航点的平均曲率，如图12 所示，导航线前端1/3部分，插值导航线和NSVM导航线曲率都较大，原因有两点：算法自身的原因导致导航线首尾部分的曲率相较于中间部分偏大；对导航线前端部分进行了调整，使其经过车体坐标系原点；导航线中间1/3部分，3条导航线的曲率及曲率变化幅度都较小；导航线后端1/3部分，插值导航线与NSVM导航线曲率较大，原因是由算法自身导致。总体来看，融合后的导航线曲率及曲率变化都小于插值导航线和NSVM导航线，且没有发生锯齿状波动。计算每条导航线的曲率最大值、绝对平均值以及标准偏差，结果如表2 所示，融合后256农2023年机业报学械导航线曲率最大值为0.0 48 m1，平均值为0.018m-1,标准偏差为0.0 11m,相较于插值导航线分别下降8 1%、54%、7 4%，相较于NSVM导航线分别下降6 2%、55%、6 6%，表明融合后导航线更适合作为行间导航线。0.25插值导航线-NSVM导航线0.20融合后导航线-/率甲0.150.100.050102030405060导航点序列图12导航点平均曲率Fig.12Average curvature of navigation point表2 导航线曲率Tab.2Curvature of navigation linem导航线类型最大值绝对平均值标准偏差插值导航线0.2550.0390.043NSVM导航线0.1260.0400.032融合后导航线0.0480.0180.011随机选取1顿点云数据，分别计算插值导航线、NSVM导航线和融合后导航线，喷雾机器人以0.5m/s和1.0 m/s不同的行驶速度对每条导航线进行轨迹跟踪实验，实验中设置前视距离为2.0 m；机器人的位姿通过RTK信号获取，实验结果如图13所示，图中黄色曲线为插值导航线，蓝色曲线为NSVM导航线，红色曲线为融合优化后导航线，绿色曲线为跟踪曲线，即轨迹跟踪时记录的RTK值。记录机器人行驶轨迹与导航线之间的横向误差，结果如图14所示，整体来看，机器人行驶速度较慢时，跟踪效果较好。统计各跟踪曲线横向误差的最大值、绝对平均值和标准偏差，结果如表3所示，机器人行驶速度为0.5m/s时，融合后导航线轨迹跟踪横向误差最大值为0.10 4m，平均值为0.0 53m，标准偏一插值导航线一NSVM导航线一融合后导航线12卜一跟踪轨迹12一跟踪轨迹12卜一跟踪轨迹101010888/XW/X666444222000-0.200.200.2500.2Y/mY/mY/m(a)行驶速度为0.5m/s插值导航线一NSVM导航线一融合后导航线一12卜一跟踪轨迹12/一跟踪轨迹12卜一跟踪轨迹101010888三w/X666444222000-0.200.200.2500.2Y/mY/mY/m(b）行驶速度为1.0 m/s图13轨迹跟踪Fig.13Trajectorytracking差为0.0 2 6 m-1,相较于插值导航线分别下降47%、39%、40%，相较于NSVM导航线分别下降58%、41%、51%；行驶速度为1.0 m/s时，融合后导航线轨迹跟踪横向误差最大值为0.130 m，平均值为0.049m，标准偏差为0.0 39m，相较于插值导航线分0.250.25插值导航线一插值导航线一NSVM导航线-NSVM导航线0.20一融合后导航线0.20一融合后导航线0.150.150.100.100.050.05024681012024681012X/mX/m(a)行驶速度为0.5m/s(b）行驶速度为1.0 m/s图14跟踪横向误差Fig.14Tracking lateral error257刘慧等：果园移动机器人激光雷达双源信息融合实时导航方法第8 期表3导航线跟踪横向误差Tab.3Lateral error of navigation line tracking速度/最大值/绝对平均标准偏差/导航线类型(ms-l)m值/mm0.50.1970.0870.043插值导航线1.00.2190.0760.0590.50.2460.0900.053NSVM导航线1.00.2420.1070.0720.50.1040.0530.026融合后导航线1.00.1300.0490.039别下降41%、36%、34%，相较于NSVM导航线分别下降46%、54%、46%。机器人在低速行驶时，RTK定位精度更准确，机器人底盘控制效果更好，因此机器人轨迹跟踪更准确。机器人以不同行驶速度对融合后导航线跟踪的最大横向误差为0.13m，表明融合后导航线更有利于轨迹跟踪，该算法能够满足果园行间自主导航的需求。3结论（1）对激光雷达获取的点云进行预处理以减小计算量，分别通过滤波、聚类、曲线拟合的方法计算插值导航线，通过非线性支持向量机的方法计算NSVM导航线，动态计算两者权重，将高频更新的插值导航线与低频更新的NSVM导航线进行融合与优化。(2)融合优化后导航线平均曲率为0.0 18 m-l，较插值导航线平均曲率下降54%，较NSVM导航线平均曲率下降55%，表明融合后导航线更适合作为行间导航线。(3)轨迹跟踪实验表明，行驶速度为0.5m/s时，融合后导航线轨迹跟踪横向误差最大值为0.104m，行驶速度为1.0 m/s时，其横向误差最大值为0.130 m，证明融合优化后导航线满足果园行间自主导航的需求，可为果园地面装备的行间自主导航提供技术参考。参考文献1张漫，季宇寒，李世超，等.农业机械导航技术研究进展J.农业机械学报，2 0 2 0,51（4）：1-18.ZHANG Man,JI Yuhan,LI Shichao,et al.Research progress of agricultural machinery navigation technology J.Transactionsof the Chinese Society for Agricultural Machinery,2020,51(4):1-18.(in Chinese)2HAN X,XU L,PENG Y,et al.Trend of intelligent robot application based on intelligent agriculture system CJ/2021 3rdInternational Conference on Artificial Intelligence and Advanced Manufacture(AIAM).IEEE,2021:205-209.3许冠杰.自动导航系统在农业果园中的应用及适用性分析J.时代农机，2 0 19,46（11）：10 6 10 7,10 9.XU Guanjie.Application and applicability analysis of automatic navigation system in agricultural orchard J.Times AgriculturalMachinery,2019,46(