1、2023年月期第54卷第农报学业机械doi:10.6041/j.issn.1000-1298.2023.07.006基于多约束条件的果园喷雾机器人路径规划方法沈跃天刘子涵函刘慧杜伟（江苏大学电气信息工程学院，镇江2 12 0 13）摘要：果园喷雾机器人路径轨迹规划影响机器人行驶路线的平滑线和行驶过程的可靠性和平稳性。针对目前果园喷雾机器人路径规划中存在的转弯处参考轨迹不够平滑、曲率较大、行驶不平稳等问题，提出了一种基于果园喷雾机器人运动学多约束条件的三次非均匀B样条曲线果园喷雾机器人轨迹优化方法。通过先验地图获取树行位置信息对行间路径点进行拟合处理，保证果园喷雾机器人行驶在树行中心线上符合喷雾
2、作业要求，综合考虑最小转弯半径、首末端点约束、转向机构延迟约束、曲率连续等多约束条件构建路径曲率最小化目标函数，并通过最优化算法求解待优化的曲线参数，生成符合果园喷雾机器人行驶要求的全局路径，最后采用纯跟踪算法进行跟踪试验来验证机器人行驶精度。仿真与试验结果表明，规划生成轨迹的最大曲率为0.31m，平均曲率为0.15m-,符合果园喷雾机器人的行驶要求；针对该轨迹跟踪行驶的平均横向误差为0.2 2 5m，标准差为0.0 31m，满足果园喷雾机器人在果园内喷雾作业时对行驶精度的要求。关键词：果园机器人；路径规划；三次B样条曲线；轨迹优化中图分类号：TP242.6文献标识码：A文章编号：10 0 0
3、-12 9 8（2 0 2 3）0 7-0 0 56-12OSID:Orchard Spray Robot Planning Algorithm Based onMultiple ConstraintsSHENYueLIU ZihanLIU HuiDU Wei(School of Electrical and Information Engineering,Jiangsu University,Zhenjiang 212013,China)Abstract:The path trajectory planning of orchard spray robot affects the smoot
4、h line of robot driving routeand the reliability and smoothness of the driving process which needs more comprehensive considerationand more comprehensive planning.Aiming at the problems that the reference trajectory at the turn is notsmooth enough and the curvature is large in the path planning of o
5、rchard spray robot,a trajectoryoptimization method of cubic non-uniform B-spline curve for orchard spray robot based on kinematicsmultiple constraints of orchard spray robot was proposed.The prior map was used to obtain the positioninformation of the tree rows,and the path points between the rows we
6、re fitted to ensure that the orchardspray robot driving on the center line of the tree row met the requirements of the spray operation.Theobjective function of minimizing the path curvature was constructed by considering the minimum turningradius,the constraint of the first and end points,the delay
7、constraint of the steering mechanism,and thecontinuity of curvature.The curve parameters to be optimized were solved by the optimization algorithm,and the global path that met the driving requirements of the orchard spray robot was generated.Finally,the pure tracking algorithm was used to verify the
8、 driving accuracy of the robot.The simulation and testresults showed that the maximum curvature of the planned trajectory was O.31 m,and the averagecurvature was O.15 m,which met the driving requirements of the orchard spray robot.The averageerror of the trajectory tracking driving was 0.225 m,and t
9、he mean square error was 0.031 m,which metthe requirements of the orchard spray robot for driving accuracy when spraying in the orchard.Key words:orchard robot;path planning;cubic B-spline curve;trajectory optimization收稿日期：2 0 2 2-11-2 3修回日期：2 0 2 3-0 3-2 0基金项目：国家自然科学基金项目（32 17 190 8）作者简介：沈跃（197 8 一
10、），男，教授，博士生导师，主要从事无人驾驶、智能感知、移动机器人和特种无人机等研究，E-mail：s h e n u j s.e d u.c n57沈跃等：基于多约束条件的果园喷雾机器人路径规划方法第7 期0引言随着农业生产的现代化、智能化发展1，新兴的农业生产设备与完善的配套算法成为了新的研究趋势。自主行走的喷雾机器人被广泛地应用2】，有效减轻了果农的劳动强度，提高了作业效率与质量。路径规划3 是果园喷雾机器人的研究热点和重要问题，即在复杂的果园环境下能够寻找到一条符合机器人运动要求的连续路径。路径点搜索4是路径规划的基础，基于栅格的路径规划算法是目前的主流方法，如Dijkstra算法、A*
11、算法等5。但传统的路径规划算法具有路径结构复杂、曲率大，不便于跟踪行驶等问题6 。在实际的果园环境应用中，除了考虑路径代价尽可能小、碰撞安全之外，还需保证行驶轨迹符合机器人的运动学约束。因此，一些学者提出了将路径规划与轨迹优化结合的方法，对规划好的路径进行轨迹优化处理7 。陈成等8 采用四阶贝塞尔曲线来表述轨迹形状，并结合转向结构的约束与速度约束保证轨迹的可行性，但贝塞尔曲线的局限性限制了轨形的可塑性，不适用于复杂路面情况。曹如月等9 用贝塞尔曲线对转角处路径进行平滑处理，以“直线一曲线一直线”的方式表示全局路径，以分步的形式优化全局路径，但分段方法无法保证整体路径的连续性。吕恩利等10 1利
12、用三次均匀B样条曲线描述叉车路径，优化了整体轨迹形状，效果很好，但此方法主要是针对短距离路线的简单作业情况，对于复杂路面情况的应用仍然存在一定的局限性。本文通过分析果园作业环境，根据实际地图利用A*算法进行全局路径搜索，根据特征点曲率筛选出分段点与转弯曲线，以转弯处首末端点约束、机器人转弯半径约束与转向机构延迟约束构建约束条件，以转弯曲线曲率均值最小为目标建立B样条曲线并进行参数求解。将采用三次非均匀B样条（No n-u n i f o r m r a t i o n a l B-s p l i n e s，NU R BS）曲线表示全局路径，并通过仿真与试验验证算法的可行性。1果园喷雾机器人作
13、业分析1.1作业环境果园场景下喷雾机器人的路径规划问题需在满足喷雾机运动学约束条件的前提下，以最小化损耗为目标生成安全平滑的全局路线。除此以外，整体行驶路线还需满足机器人的喷雾作业要求，为果园喷雾机器人提供一个安全可靠、节能高效的规划路径。果园喷雾机器人的路径规划方案需结合喷雾作业的实际要求。果园环境具有“长廊式”的特点，如图1所示。在果园喷雾机器人的行驶过程中，其喷雾作业设备会对两侧检测到的果树进行喷雾施药。因此，在树行间行驶时需要尽可能与两侧果树保持相同的施药距离以保证喷雾作业效果，即以树行中心线为目标路线。同时，果园喷雾机器人中心与树行边缘存在一个极限最小距离以保证行驶路线的安全无碰撞。
14、在基于经纬度信息建立的全局坐标系下，果园喷雾机器人从第一排树行外侧为起点，在树行末端做连续的“U”形运动，以最后一排树行外侧末端为目标点，在二维平面内建立一条从起始点到目标点、符合果园喷雾机器人运动学约束的安全平滑全局路径。图1果园喷雾机器人路径规划模型示意图Fig.1Schematic of path planning model oforchard spray robot1.2路径规划问题描述基于北斗导航系统获取起始点与目标点的精准全局坐标，并通过激光传感器获得树行位置信息建立全局栅格地图为前提，研究果园环境下的路径规划问题。本文的研究内容包括路径点搜索后的处理以及生成全局路径路径规划整体
15、流程如图2 所示。在已经获取树行位置信息与目标点坐标的前提下，果园喷雾机器开始北斗导航系统获取喷雾机器人位姿信息A算法获得全局路径点提取特征点进行初优化结合约束条件采用三次非均匀B样条优化轨迹纯跟踪算法进行轨迹跟踪结束图2果园喷雾机器人路径规划流程图Fig.2Flowchart of path planning for orchard spray robot式农582023年机报学业械人首先通过北斗导航系统获取自身位姿信息并以自身位置为起点，利用A*算法进行全局规划，生成一条最优或较优的全局路径，其结果是一系列离散的路径点。将获得的路径点提取为特征点，并结合果园实际环境信息与作业要求对特征点进
16、行初优化处理。结合果园喷雾机器人运动学约束，采用三次非均匀B样条曲线来进行轨迹优化并获得轨迹曲线。最后通过纯跟踪算法进行轨迹跟踪，完成果园内自主行驶的任务。2基于多约束条件的路径规划策略2.1A*算法路径点搜索A*算法是一种启发式搜索算法与深度优先算法结合的算法，具有导向性，是静态环境中用于求解最优路径最有效的直接搜索算法。A*算法通过一个估价函数f（n）来确定搜索方向【12】，从起始点向周围扩展，同时通过启发函数h（n）来计算周围各个节点到目标点的估计代价13，以此来选择最小代价节点作为下一个扩展节点，不断重复这一过程直至到达目标点。这一过程中搜索出的所有节点均为最小代价节点14，将这些节点
17、以路径点的形式保存，生成最终路径，此路径为最小代价路径。A*算法的代价函数f(n)表示为15f(n)=g(n)+h(n)(1)式中n一待扩展的节点g（n）一一从起点到当前节点的实际路径代价h(n）一一启发函数，前节点到目标点估计代价值对于代价函数f(n），当g（n）=0 时，A*算法成为单纯的贪心算法，计算速度快但不一定得到最优解；当h（n）=0 时，式（1）成为单纯的Dijkstra算法，计算量大，效率低。A*算法在搜索过程中同时计算g（n）和h（n）,在考虑搜索效率的同时保证得到最优路径。A*算法在执行路径规划任务时主要用2 个表实现节点的扩展和最优点的选择，即通过Openlist表和Cl
18、oselist表来记录节点。其中Openlist表是为了保存搜索过程中遇到的扩展节点，同时将这些节点按代价进行排序，选出其中f（n）值最小的节点，作为下一个扩展节点；然后又将该节点的所有邻近节点存人Openlist表中，直至目标点也被存人16 2.2特征点初优化策略2.2.1行间优化结合果园喷雾机器人实际工作情况，要求果园喷雾机器人行驶在树行中心线上，为喷雾作业效果与行驶安全提供保障。因此，在通过A*算法获得全局路径点后需要进行优化处理将路径点提取为特征点后，首先对获得的路径点进行行间判断，判断特征点是否在两排树行当中，并将处于行间的特征点投影至树行中心线。为了方便计算，首先建立一个相对坐标系
19、。假设所有树行均平行且与全局坐标系Y轴夹角为,则将全局坐标系旋转至树行与Y轴平行，如图3所示。此时树行亦与X轴垂直，利用旋转矩阵计算出各路径点坐标以及树行边界点坐标，即cosOsing1r(2)singcosolLY图3坐标转换示意图Fig.3Diagram of coordinate transformation完成坐标转换后，此时便可通过特征点与树行边界的X轴坐标来判断其是否在树行之中，并可以通过改变X轴坐标将路径点投影到树行中心线上，无需进行投影计算，其判断方法为(x;Xj+1)X;=X,+X,1(X,x;Xj+1)2(i=0,l,.,n;j=0,1,.,m)(3)中一路路径点序号，共有
20、n+1个路径点-树行边界序号，共有m+1个树行边界X一树行边界X轴坐标此时行间的特征点已全部投影到了树行中心线上，再将相对坐标系中的特征点转换回全局坐标系下，即将坐标系反方向旋转计算出特征点坐标，即cos-sing1r(4)二singcosO2.2.2路径点分段优化将特征点初步优化处理后，为更好地发挥三次非均匀B样条曲线在表达曲线形状特征方面的优势，在进行插值前仍需对特征点进行处理17 。由于路径点的曲率信息能够较好地反映曲线的形状特征且对于后续生成的全局路径的实用性有较大参考价值，把特征点与曲率关联起来进行优化处理，可以更好地显示曲线的形状特征。min(k向后差分极小值max(k向前差分极大
21、值59沈跃等：基于多约束条件的果园喷雾机器人路径规划方法第7 期(1)曲率计算采用“三点法”来计算初始特征点曲率18 ，即用相邻3个特征点形成近似圆弧的方法来对各个初始特征点的曲率进行求解。当存在n+1个路径点Q（i=0,1，,n）,其对应的曲率半径为p.（i=0,1,：，n），近似求解公式为P;=21lo.0.o.0.-.(5)曲率k,为（2）曲率分段点计算初步优化后的特征点组成的线段按照曲率大小可划分为若干个大曲率段和若干个小曲率段。大曲率段曲线是路径中的转弯部分，弯曲程度较大，需要更加细化的计算来确定最终特征点的位置；小曲率段相对而言较为平直，即表示在经过初步优化后的行间直行部分，在三次
22、非均匀B样条曲线中，可通过特征点的位置限制拉伸为直线。当第i个特征点Q.对应的曲率k；与邻近的路径点之间满足以下条件之一时，则可将Q，初步选作曲率分段点19,即k;ki-j(j=1,2)(7)kk时，计算其向前差分与向后差分的值并进行判断，其公式为2ZAk;=Z Ikij+-ki,(8)min(k;)=mi n(Ik i-j -k i+1 I)(9)1j2max(Aht)=max(Iki+j+1-ki+1l)(10)Ij2式中ZA一由曲率向前差分之和如在路径点Q：处min（h）A h t，且Kmax（h）k,时有1k.：(6)Pi2ZA;=Ik;-ij-ki-j+1 I(11)二j=1max(
23、k;)=ma x (x(I ki-j-1-ki-1 I)(12)1j2min(Akt)=min(Iki+j-ki-1 1)(13)1j2如在路径点Q：处min（A h t）Ah,且Kmax（k i）k,则可将Q确定为分段点。在每个转弯曲线内存在一个曲率极大值点，曲率极大值点对于曲线形状的影响有着重要的作用，对假设段内存在d个路径点，则曲率极大值为=max(hi,k2,ka)(14)max该点即为曲率极大值点。在使用A*算法搜索路径点的过程中，由于存在启发函数h（n）使得搜索过程具有方向性，一部分路径点会向着目标点方向偏移。这种偏移会导致部分路径形状特征复杂、不利于果园喷雾机器人的实际跟踪行驶。
24、提取特征点后通过行间优化，行间特征点的偏移问题得到解决。在确定分段点位置后，对起始点与首个分段点之间的路径点进行矫正，投影至包含起始点的平行于树行的直线上，其效果如图4所示。在已知果树位置的情况下，根据果树直径与整体树行的长度对整体树行做拟合处理并依次将树行延长至果园边界形成“U”形走廊12108642024681012141618X/m(a）优化前12108三2024681012141618X/m(b）优化后图4特征点初优化对比Fig.4Comparison diagrams of initial optimizationof featurepoints2.3多约束条件下三次非均匀B样条轨迹
25、优化本文研究对象为阿克曼底盘果园喷雾机器人，其具有非完整性约束的特点，若要改变航向，需要满足曲率约束条件。因此在换行转弯情况下，对于农602023年机报学业械2.2节中确定的初始特征点，需对其中转弯曲率不满足机器人转弯条件的特征点进行修正，优化流程如图5所示开始喷雾机器人运动学模型建立根据曲率分段点确定转弯曲线根据转弯曲线内部特征点曲率均值建立最优化评价函数根据机器人运动学模型与曲线特征建立约束条件调用PSO计算出最小评价函数值的曲线参数计算节点向量反算控制点生成三次非均匀B样条曲线结束图5轨迹优化算法流程图Fig.5Flowchart of trajectory optimization a
26、lgorithm2.3.1果园喷雾机器人运动学模型建立果园喷雾机器人在果园执行喷雾作业任务时运动速度较为缓慢，在低速行驶时受到的侧向力较小，假设其不发生侧滑现象，因此在进行路径规划时只考虑机器人运动学模型。假设果园喷雾机器人车体为刚性，车轮变形可忽略不计，且车轮与地面的每个接触点仅产生滚动无滑动，其模型如图6 所示。由于两个前轮的转向角不同造成了两个后轮旋转半径也不相同，于是做出了相应的简化，在前轴中心拟合出一个中心前轮，其方向表示机器人整体的运动方向。因此转向角计算式为18=(8,+8,)(15)2式中8简化后车辆前轮转向角一左侧前轮转角8,一右侧前轮转角简化后的模型称为自行车模型，计算式为
27、1R(16)tanotan(17)=V1式中1-前后轴之间距离R旋转半径后轮速度车体旋转角一后轮与目标点夹角Y41RX图6果园喷雾机器人底盘运动学模型Fig.6Kinematics model of spray robot chassis2.3.2三次非均匀B样条曲线一条p次NURBS曲线可以表示为2 0 nNi,(u),P;C(u)i=0(0u1)(18)Zw,Ni.(u)i=0式中PNURBS曲线的控制点控制点的权因子控制点个数为n+1个，所有控制点的连线为NURBS曲线的特征多边形，节点个数为m+1,其中m=n+p+1,Ni（u）即B样条基函数，可由DeBoor-Cox递推公式推导得出2
28、 11(u,uui+)10(其他）uN+1u1.p-1(u）山;+PuU;+p+1;i+10规定=00(19)选用累积弦长参数化法进行特征点的参数化处理2 。累积弦长参数化法的节点矢量参数计算公式为u=u,=.=u=0ui+=Ui+k-I+Q,(i=1,2,n-1)(20)lun+k=un+k+1=unn+h+3=1Q;-Q:-1其中S式中向前差分矢量S一所有特征点的相邻距离总和一为了简化轨迹曲线计算过程，将式（18）中的权因子，均设为1,即将NURBS曲线简化为非均匀B样条曲线2 3,其表达式为n+2Q(ui+3)=N,(ui+3)P,(21)j=061沈跃等：基于多约束条件的果园喷雾机器人
29、路径规划方法第7 期式（2 1）表示的线性方程组由n+1个矢量方程组成，包含n+3个控制点，未知控制点数大于方程数。因此，还需要添2 个边界条件来求解方程组。采用切失条件作为边界条件。整理可得abCId,ea2b2C2d2&2.三：(22)6denna6Cdn+1n+1n+u;+3Wi+2其中ai+3-;3-ui+22（+2一ui+3-u;(u;+2-ui+1)(u;+4-ui+2)Ui+4-Ui+1(u+2-u;+1)Ui+4-ui+1e;=(Ai+2+1)Qi-1=(ui+3-ui+1)Qi-1(1in)取首末重复度为4，三次非均匀B样条曲线的首末控制顶点即为特征点，即Q。=P。，Q,=P
30、,+2。且首末处有切失条件2 4a,=1b,=ci=0e=Q十qo3n+2a6qnn+1n+13(23)式中90vqn首末切失条件由式（2 0）可计算出节点矢量，代人式（2 2）可计算出所有控制点，将控制点代人式（2 1）即可求出三次非均匀B样条曲线。2.3.3基于多约束条件的三次非均匀B样条曲线在果园喷雾机器人的路径规划过程中对于轨迹曲线应有以下要求：（1)曲率连续约束2 5。为避免喷雾机行驶过程中出现急转、急停等现象，需要确保轨迹曲线的曲率连续。采用三次非均匀B样条曲线，满足C连续性质。(2)喷雾机转弯半径约束2 6 。为保证轨迹曲线具有实际可行性，避免机器人出现无法跟踪的情况，轨迹上的任
31、一点曲率应满足机器人的最小1转弯半径约束，即k，其中Rmin为喷雾机最小Rmin转弯半径2 7(3)转向机构延迟约束。对于绝大多数农业机器人，其前轮转向机构具有一定的时间延迟，即从指令下发到转向机构做出角度转变需要一定的转换时间，这段时间差会使得机器人在实际跟踪行驶过程中出现误差。这种延迟带来的误差可以通过两方面来减少。一方面是优化转向机构减少反应时间，这种方式直接有效但会增加成本且无法根本解决问题；另一方面可以将反应时间加人轨迹的优化过程中，通过增加转向角的角速度约束来保证给机器人留有足够的反应时间从而减少延迟带来的误差。对于两个分段中的转弯曲线，可以通过调整其特征点的方式来改变曲线的形状，
32、其中特征点即为位置约束点，默认将前面优化后的路径点作为特征点。特征点与曲线关系示意图如图7 所示。YQ4(x4,y4)Q,(x，y)Q,(xQ.(0Yo0X图7 转弯曲线特征点与曲线关系Fig.7Diagram of feature points and curves ofturning curves图中点Q1、Q，为分段点、曲率极大值点。在已知分段点、曲率极大值点的条件下，通过改变点Q2和Q4的位置可以改变曲线的形状，其中与两点设定为关于点Q，对称，简化了计算且符合转弯逻辑。因此，通过求解多约束条件下点Q2的坐标即可得到最优的转弯轨迹。点Q。与Q。分别为两侧分段点的前后一个特征点，分别形成两
33、条直线，即部分行间轨迹。在优化计算的过程中，代人两个特征点Q。与Q。可以保证转角轨迹与行间轨迹曲率的约束以及连续性。如图7 所示，转角轨迹优化的初始已知条件为：喷雾机起始位置为Q。（o，y）、目标位置为Q。（x，）、转角顶点为Q（x，y），变量点Q（x 2，y 2），可得Q4坐标为41-101rx21201r31Q4=(24)000该轨迹上任一点曲率为2 8 K(u(25)3式中u一三次B样条曲线的节点向量机器人底盘的最小转弯半径为Rmin，轨迹曲线应满足约束条件1k(u）Rmin(26)机器人底盘跟踪轨迹曲线时，其前轮转向角为d前视距离其中62农2023年机业报学械1(u)y(u)-y(u)
34、x(u)S=arctanarctanR(x2(u)+y(u)(27)前轮转角不能超过机器人底盘的最大前轮转角。当选择变量Q2的坐标时，易见得Q，应处于Q1与Q3连线的上方，否则轨迹曲率会变化较大，不利于跟踪。因此有约束条件y3Y2x2-x,)+yi(28)X3对于果园喷雾机器人，其前轮转向角度差为=20(29)式中一一转向机构单向最大转角因此果园喷雾机器人最大转向角速度为max(30)二maxt式中转向机构动作反应时间一在果园喷雾机器人的轨迹规划过程中应该考虑到其曲率所反映的前轮转角变化速度应小于转向机构的最大角速度。对于转向机构的角速度约束，则需要转向角参数和时间参数。由式（2 7）和自行车
35、模型下的几何公式得转向角为2LsinS=arctan(31)lak(u)=arcsin(32)2机器人位置与目标点夹角在进行角速度规划时，如果曲线长度未知，则很难进行时间规划，因此，可以通过积分来计算曲线长度2 9L(u)=C(u)du=+y(u)du(33)通过路径长度计算出每一段的预期时间t.为L(u)ta=(34)U,n式中期望速度nB样条曲线段数因此对于每一段路径曲线都有转向角速度约束2Lsindarctandod(arctan(k(u)L)dtdtdtmax(35)其中dt的分布范围为o,nt其中dt的分布范围为0,nta。2.3.4参数最优化求解假设转角轨迹上任一点的曲率为k（u）
36、。用三次非均匀B样条曲线将轨迹表示后，使用最优化的方法来求解在满足多约束条件下的曲线参数。最优化的目标为转角轨迹曲线的曲率均值，即n-1E;=min(36)通过控制变量Q，的坐标，改变曲线形状，使得曲率均值最小，获得光滑连续的轨迹曲线30 。同时，最优化过程有以下约束条件x(u)y(u)-y(u)x（u）1k(u）3(x(u)+y2(u)Rmin23y2X2-x)+J1dQ(37)dtmax=arctanRmaxtang=13仿真及验证分析3.1全局路径仿真与评估在第2 节建立转弯曲线的数学模型后，以式（36）为目标函数，点Q2的坐标为变量，式（37）为约束条件，调用PSO函数，可求得在转弯场
37、景下的最优曲线参数31。将各转弯曲线的特征点参数代人式（2 1）可计算出最终用三次非均匀B样条曲线表示的全局路径如图8 所示。12108三2024681012 141618X/m图：最终三次非均匀B样条全局路径曲线Fig.8Final three nonuniform B-spline global path curves图8 中绿色圆点为模拟果树，黑色矩形为障碍物信息。实际果园中，同一树行的果树存在间距但喷雾机不应从中穿过，因此以果树的宽度与树行的长度整体生成矩形障碍物。同时，为保证喷雾机以“U形运动轨迹遍历果园，分别将障碍物延伸至果园边界，形成“U”形长廊式结构，不同方法的优化结果如图9、
38、10 所示。63沈跃等：基于多约束条件的果园喷雾机器路径规划方法第7 期12斤型值点一初始A路径10一本文方法修改后折线段一均匀B样条8一本文方法平滑后64202468101214X/m图9轨迹优化方法对比Fig.9Diagram of fragment trajectory optimizationmethods3.2轨迹跟踪仿真验证在Matlab中根据果园喷雾机器人底盘模型编写跟踪代码进行跟踪仿真试验。选用纯跟踪算法，设定轴距为0.6 14m，前视距离为1m。将生成的全局路径导人跟踪算法的参考路径中，观察其横向误差、航向角以及前轮转角。分别对传统无约束B样条函数优化的轨迹曲1.0一最大曲率
39、一无约束B样条算法一初始A算法一本文方法0.8-叫/率甲0.60.40.201020304050路程/m图10U形环境轨迹曲率对比Fig.10Curvature contrast diagram of U-shapedenvironmental trajectories线和本文基于多约束条件优化生成的轨迹曲线进行跟踪仿真，其结果如图11、12 所示。无约束路径点存在较大转角，由于轴距和前轮转角的限制无法精准跟踪，产生较大的前轮转角波动，影响果园喷雾机器人行驶稳定性。跟踪横向误差如图13和表1所示，原始折线路径由于存在尖角其误差最大。通过增加三次B样条函数优化与约束条件，可以有效缩小横向误差。1
40、2280一规划轨迹一实际行驶轨迹9140pe/回603-1-402-8002468100200400600800050010001500X/m采样点序号采样点序号图11无约束B样条优化轨迹跟踪结果Fig.11Results of unconstrained B-spline optimization trajectory tracking1260r一规划轨迹一实际行驶轨迹9130(）/转转603-1-30-2-6002468100200400600800050010001500X/m采样点序号采样点序号图12多约束条件下B样条优化轨迹跟踪结果Fig.12Results of B-spline
41、optimization trajectory tracking under multiple constraints4跟踪试验验证4.1试验平台本文算法验证使用阿克曼底盘的无人喷雾机器人作为试验平台【32】，如图14所示。无人喷雾机器人主要由机器人底盘（hunter2.0，深圳市松灵机器人）、中央计算机（英特尔酷睿八代i78 56 5u）、组合导航系统（高精度组合导航系统X1，北云科技）和喷雾作业机构组成。本研究使用中央计算机装载Ubuntu18.04LTS操作系统，并基于机器人操作系统（Robotoperatingsystem,ROS）进行开发。基于ROS平台编写机器人64农2023年机报
42、业学械一初始A算法0.6r一无约束B样条算法一本文算法0.40.20-0.2-0.4-0.60200400600800采样点序号图13轨迹跟踪仿真误差对比Fig.13Comparison of trajectory tracking errors表1仿真机器人跟踪轨迹误差Tab.1Tracking error of simulation robotm规划方法最大误差平均误差均方差初始A*算法0.5480.3830.152无约束B样条算法0.3810.1690.112本文算法0.1090.0530.055图14果园喷雾机器人Fig.14Orchard spray robot1.喷雾作业机构2.中
43、央计算机3.高精度组合导航系统X14.GPS天线通讯与行驶代码，并将规划后的路径载人预编好的容器中，通过可视化软件RVIZ可观察规划路径与实际行驶轨迹4.2试验设计试验场地位于江苏大学果园（32.2 18 8 7 9 1N，119.5099572E）。在该试验场地中，共有5列树行,行距平均为3.3m，株距平均为0.8 m，每列树行平均长10.5m，如图15所示。主要考虑果园环境特征为连续的U形走廊，将果树树行视为障碍物后搜索出从起始点到末行末尾的路径，并采用不同的优化算法进行处理，图15果园环境试验场景Fig.15Test scenario of orchard navigation采用纯跟踪
44、算法控制果园喷雾机器人对不同的轨迹进行跟踪行驶，并实时计算横向误差作为评价指标。纯跟踪算法的前视距离与速度会影响跟踪效果，为避免跟踪时出现振荡不稳定的情况，前视距离设置为1.0 m。同时为贴合实际工作情况，行驶速度设置为起始速度0.5m/s并匀速加速至最大速度1.2 m/s。4.3试验结果与分析通过对不同算法生成的路径进行跟踪试验，过程通过北斗导航系统中采集喷雾机器人实时位置信息并记录为文本文件，将该文本文件导人Matlab中33 得到果园环境下的跟踪结果如图16 所示。由图16 可看出，当进行直线跟踪时其实际行驶轨迹与目标路径较为贴合，横向误差较小；当进行转弯行驶时，皆与目标路径产生了较大差
45、距，即横向误差变大。转弯处的横向误差主要有2 个原因：纯跟踪算法的固有缺陷。目标路径不符合果园喷雾机器人的行驶要求，机器人无法行驶到该路径的位置点上。在图16 a中A*算法生成的路径点折线段的曲率较大，机器人无法跟踪到其线段的转折点上进而出现横向误差；在图16 b中，无约束均匀B样条曲线两侧转弯处的轨迹形状复杂，且中间两处曲率较大，由于果园喷雾机器人底盘存在轴距与前轮转角的限制，其整体转弯角存在最大角度，当目标路径点的曲率较大时，机器人无法从当前位置点行驶至下10r10规划轨迹10规划轨迹规划轨迹实际行驶轨迹实际行驶轨迹实际行驶轨迹555三三00050612182406121824061218
46、24X/mX/mX/m(a)初始A*算法(b）无约束B样条算法(c）本文算法图16果园环境下规划路径跟踪结果Fig.16Results of trajectory tracking in orchard environment65沈跃等：基于多约束条件的果园喷雾机器人路径规划方法第7 期一个目标点，从而出现了较大横向误差；在图16 c中，增加约束条件的优化轨迹其曲率较小，且无较大波动，因此从3种算法的跟踪行驶效果中可以较为直观地观测出多约束条件下非均匀B样条优化算法生成的目标路径更符合机器人的行驶要求如图17 与表2 所示，在起始时由于距离目标轨迹较远造成误差较大。在机器人行驶到目标轨迹直线部
47、分时，误差开始收敛，此时开始计算平均误差。一初始A*算法一无约束B样条算法2.0一本文算法1.51.00.50-0.5-1.0-1.5-2.0-2.50100200300400500600采样点序号图17果园环境中规划路径跟踪误差对比Fig.17Comparison of planning path trackingerrors in orchard environment表2果园环境下轨迹曲率及机器人跟踪效果Tab.2Tracking error of robot in orchard environment平均曲最大曲平均横向标准差/规划方法率/m=1率/m-1误差/mm初始A*算法0.5
48、50.820.7260.447无约束B样条算法0.320.530.5850.342本文算法0.150.310.2250.031转弯处3种算法生成轨迹的跟踪误差差距较为明显，经多约束条件下三次非均匀B样条优化后的轨迹曲线其曲率最小，跟踪误差最小，行驶过程中最大曲率为0.31m-,横向误差标准差为0.0 31m，符合果园喷雾机器人作业要求。5结论（1)针对传统A*算法在果园环境中出现的路径点偏移不适合直接代入优化的问题，提出了一种初优化策略，根据树行位置信息矫正行间路径点。根据曲率分段划分路径点，并结合始末端点矫正首末树行外侧路径点。（2)针对果园环境中喷雾机器人自主行驶过程，提出了一种基于三次非
49、均匀B样条曲线的果园行驶路径规划方法。结合喷雾机器人最小转弯半径约束、转弯处首末端点连续约束、转向机构延迟约束等，建立多约束条件，以曲率均值最小为优化目标建立数学模型并求解。生成轨迹的最大曲率为0.31m-1,平均曲率为0.15m-1,相对于初始A*算法和三次均匀B样条算法大大缩小了轨迹曲率，符合果园喷雾机器人运动学约束。（3）跟踪试验表明，果园喷雾机器人对果园行驶路径规划算法生成的路径可以较好地进行跟踪，平均横向误差为0.2 2 5m，标准差为0.0 31m，满足果园喷雾机器人行驶精度要求。参考文献1王宁，韩雨晓，王雅萱，等.农业机器人全覆盖作业规划研究进展J.农业机械学报，2 0 2 2,
50、53（增刊1）：119.WANG Ning,HAN Yuxiao,WANG Yaxuan,et al.Research progerss of agricultural robot full coverage operation planning J.Transactions of the Chinese Society for Agricultural Machinery,2022,53(Supp.1):1-19.(in Chinese)2宫金良，王伟，张彦斐，等.基于农田环境的农业机器人群协同作业策略J.农业工程学报,2 0 2 1,37（2：11-19.GONG Jinliang,WAN