基于多约束条件的果园喷雾机器人路径规划方法

沈 跃 刘子涵 刘 慧 杜 伟

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

0 引言

随着农业生产的现代化、智能化发展[1],新兴的农业生产设备与完善的配套算法成为了新的研究趋势。自主行走的喷雾机器人被广泛地应用[2],有效减轻了果农的劳动强度,提高了作业效率与质量。

路径规划[3]是果园喷雾机器人的研究热点和重要问题,即在复杂的果园环境下能够寻找到一条符合机器人运动要求的连续路径。路径点搜索[4]是路径规划的基础,基于栅格的路径规划算法是目前的主流方法,如Dijkstra 算法、A*算法等[5]。但传统的路径规划算法具有路径结构复杂、曲率大,不便于跟踪行驶等问题[6]。在实际的果园环境应用中,除了考虑路径代价尽可能小、碰撞安全之外,还需保证行驶轨迹符合机器人的运动学约束。因此,一些学者提出了将路径规划与轨迹优化结合的方法,对规划好的路径进行轨迹优化处理[7]。陈成等[8]采用四阶贝塞尔曲线来表述轨迹形状,并结合转向结构的约束与速度约束保证轨迹的可行性,但贝塞尔曲线的局限性限制了轨形的可塑性,不适用于复杂路面情况。曹如月等[9]用贝塞尔曲线对转角处路径进行平滑处理,以“直线—曲线—直线”的方式表示全局路径,以分步的形式优化全局路径,但分段方法无法保证整体路径的连续性。吕恩利等[10]利用三次均匀B样条曲线描述叉车路径,优化了整体轨迹形状,效果很好,但此方法主要是针对短距离路线的简单作业情况,对于复杂路面情况的应用仍然存在一定的局限性。

本文通过分析果园作业环境,根据实际地图利用A*算法进行全局路径搜索,根据特征点曲率筛选出分段点与转弯曲线,以转弯处首末端点约束、机器人转弯半径约束与转向机构延迟约束构建约束条件,以转弯曲线曲率均值最小为目标建立B样条曲线并进行参数求解。将采用三次非均匀B样条(Non-uniform rational B-splines, NURBS)曲线表示全局路径,并通过仿真与试验验证算法的可行性。

1 果园喷雾机器人作业分析

1.1 作业环境

果园场景下喷雾机器人的路径规划问题需在满足喷雾机运动学约束条件的前提下,以最小化损耗为目标生成安全平滑的全局路线。除此以外,整体行驶路线还需满足机器人的喷雾作业要求,为果园喷雾机器人提供一个安全可靠、节能高效的规划路径。

果园喷雾机器人的路径规划方案需结合喷雾作业的实际要求。果园环境具有“长廊式”的特点,如图1所示。在果园喷雾机器人的行驶过程中,其喷雾作业设备会对两侧检测到的果树进行喷雾施药。因此,在树行间行驶时需要尽可能与两侧果树保持相同的施药距离以保证喷雾作业效果,即以树行中心线为目标路线。同时,果园喷雾机器人中心与树行边缘存在一个极限最小距离以保证行驶路线的安全无碰撞。在基于经纬度信息建立的全局坐标系下,果园喷雾机器人从第一排树行外侧为起点,在树行末端做连续的“U”形运动,以最后一排树行外侧末端为目标点,在二维平面内建立一条从起始点到目标点、符合果园喷雾机器人运动学约束的安全平滑全局路径。

图1 果园喷雾机器人路径规划模型示意图

1.2 路径规划问题描述

基于北斗导航系统获取起始点与目标点的精准全局坐标,并通过激光传感器获得树行位置信息建立全局栅格地图为前提,研究果园环境下的路径规划问题。本文的研究内容包括路径点搜索后的处理以及生成全局路径。

路径规划整体流程如图2所示。在已经获取树行位置信息与目标点坐标的前提下,果园喷雾机器人首先通过北斗导航系统获取自身位姿信息并以自身位置为起点,利用A*算法进行全局规划,生成一条最优或较优的全局路径,其结果是一系列离散的路径点。将获得的路径点提取为特征点,并结合果园实际环境信息与作业要求对特征点进行初优化处理。结合果园喷雾机器人运动学约束,采用三次非均匀B样条曲线来进行轨迹优化并获得轨迹曲线。最后通过纯跟踪算法进行轨迹跟踪,完成果园内自主行驶的任务。

图2 果园喷雾机器人路径规划流程图

2 基于多约束条件的路径规划策略

2.1 A*算法路径点搜索

A*算法[11]是一种启发式搜索算法与深度优先算法结合的算法,具有导向性,是静态环境中用于求解最优路径最有效的直接搜索算法。A*算法通过一个估价函数f(n)来确定搜索方向[12],从起始点向周围扩展,同时通过启发函数h(n)来计算周围各个节点到目标点的估计代价[13],以此来选择最小代价节点作为下一个扩展节点,不断重复这一过程直至到达目标点。这一过程中搜索出的所有节点均为最小代价节点[14],将这些节点以路径点的形式保存,生成最终路径,此路径为最小代价路径。A*算法的代价函数f(n)表示为[15]

f(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表和Closelist表来记录节点。其中Openlist表是为了保存搜索过程中遇到的扩展节点,同时将这些节点按代价进行排序,选出其中f(n)值最小的节点,作为下一个扩展节点;然后又将该节点的所有邻近节点存入Openlist表中,直至目标点也被存入[16]。

2.2 特征点初优化策略

2.2.1行间优化

结合果园喷雾机器人实际工作情况,要求果园喷雾机器人行驶在树行中心线上,为喷雾作业效果与行驶安全提供保障。因此,在通过A*算法获得全局路径点后需要进行优化处理。

将路径点提取为特征点后,首先对获得的路径点进行行间判断,判断特征点是否在两排树行当中,并将处于行间的特征点投影至树行中心线。为了方便计算,首先建立一个相对坐标系。假设所有树行均平行且与全局坐标系Y轴夹角为θ,则将全局坐标系旋转θ至树行与Y轴平行,如图3所示。此时树行亦与X轴垂直,利用旋转矩阵计算出各路径点坐标以及树行边界点坐标,即

图3 坐标转换示意图

(2)

完成坐标转换后,此时便可通过特征点与树行边界的X轴坐标来判断其是否在树行之中,并可以通过改变X轴坐标将路径点投影到树行中心线上,无需进行投影计算,其判断方法为

(i=0,1,…,n;j=0,1,…,m)

(3)

式中i——路径点序号,共有n+1个路径点

j——树行边界序号,共有m+1个树行边界

Xj——树行边界X轴坐标

此时行间的特征点已全部投影到了树行中心线上,再将相对坐标系中的特征点转换回全局坐标系下,即将坐标系反方向旋转θ计算出特征点坐标,即

(4)

2.2.2路径点分段优化

将特征点初步优化处理后,为更好地发挥三次非均匀B样条曲线在表达曲线形状特征方面的优势,在进行插值前仍需对特征点进行处理[17]。由于路径点的曲率信息能够较好地反映曲线的形状特征且对于后续生成的全局路径的实用性有较大参考价值,把特征点与曲率关联起来进行优化处理,可以更好地显示曲线的形状特征。

(1)曲率计算

采用“三点法”来计算初始特征点曲率[18],即用相邻3个特征点形成近似圆弧的方法来对各个初始特征点的曲率进行求解。当存在n+1个路径点Qi(i=0,1,…,n),其对应的曲率半径为ρi(i=0,1,…,n),近似求解公式为

(5)

曲率ki为

(6)

(2)曲率分段点计算

初步优化后的特征点组成的线段按照曲率大小可划分为若干个大曲率段和若干个小曲率段。大曲率段曲线是路径中的转弯部分,弯曲程度较大,需要更加细化的计算来确定最终特征点的位置;小曲率段相对而言较为平直,即表示在经过初步优化后的行间直行部分,在三次非均匀B样条曲线中,可通过特征点的位置限制拉伸为直线。

当第i个特征点Qi对应的曲率ki与邻近的路径点之间满足以下条件之一时,则可将Qi初步选作曲率分段点[19],即

(7)

上述条件可保证特征点Qi一侧的曲率大于该点处曲率。但由于特征点曲率可能会出现跳跃,因此仅满足单点大小判断条件不能保证特征点Qi两侧的邻近特征点中一侧的曲率远大于另一侧特征点的曲率。为保证分段点的准确性,还需要根据不同情况对特征点进行再筛选并最终确定分段点。

当第i个特征点Qi对应的曲率ki满足ki-j>ki时,计算其向前差分与向后差分的值并进行判断,其公式为

(8)

(9)

(10)

同理,当第i个路径点Qi对应的曲率ki满足ki+j>ki时有

(11)

(12)

(13)

在每个转弯曲线内存在一个曲率极大值点,曲率极大值点对于曲线形状的影响有着重要的作用,对假设段内存在d个路径点,则曲率极大值为

kmax=max(k1,k2,…,kd)

(14)

该点即为曲率极大值点。

在使用A*算法搜索路径点的过程中,由于存在启发函数h(n)使得搜索过程具有方向性,一部分路径点会向着目标点方向偏移。这种偏移会导致部分路径形状特征复杂、不利于果园喷雾机器人的实际跟踪行驶。提取特征点后通过行间优化,行间特征点的偏移问题得到解决。在确定分段点位置后,对起始点与首个分段点之间的路径点进行矫正,投影至包含起始点的平行于树行的直线上,其效果如图4所示。在已知果树位置的情况下,根据果树直径与整体树行的长度对整体树行做拟合处理并依次将树行延长至果园边界形成“U”形走廊。

图4 特征点初优化对比

2.3 多约束条件下三次非均匀B样条轨迹优化

本文研究对象为阿克曼底盘果园喷雾机器人,其具有非完整性约束的特点,若要改变航向,需要满足曲率约束条件。因此在换行转弯情况下,对于2.2节中确定的初始特征点,需对其中转弯曲率不满足机器人转弯条件的特征点进行修正,优化流程如图5所示。

图5 轨迹优化算法流程图

2.3.1果园喷雾机器人运动学模型建立

果园喷雾机器人在果园执行喷雾作业任务时运动速度较为缓慢,在低速行驶时受到的侧向力较小,假设其不发生侧滑现象,因此在进行路径规划时只考虑机器人运动学模型。

假设果园喷雾机器人车体为刚性,车轮变形可忽略不计,且车轮与地面的每个接触点仅产生滚动无滑动,其模型如图6所示。由于两个前轮的转向角不同造成了两个后轮旋转半径也不相同,于是做出了相应的简化,在前轴中心拟合出一个中心前轮,其方向表示机器人整体的运动方向。因此转向角计算式为

图6 果园喷雾机器人底盘运动学模型

(15)

式中δ——简化后车辆前轮转向角

δl——左侧前轮转角

δr——右侧前轮转角

简化后的模型称为自行车模型,计算式为

(16)

(17)

式中l——前后轴之间距离

R——旋转半径vr——后轮速度

ω——后轮与目标点夹角

2.3.2三次非均匀B样条曲线

一条p次NURBS曲线可以表示为[20]

(18)

式中Pi——NURBS曲线的控制点

ωi——控制点的权因子

控制点个数为n+1个,所有控制点的连线为NURBS曲线的特征多边形,节点个数为m+1,其中m=n+p+1,Ni,k(u)即B样条基函数,可由De Boor-Cox递推公式推导得出[21]

(19)

选用累积弦长参数化法进行特征点的参数化处理[22]。累积弦长参数化法的节点矢量参数计算公式为

(20)

其中

式中 ΔQi——向前差分矢量

s——所有特征点的相邻距离总和

为了简化轨迹曲线计算过程,将式(18)中的权因子ωi均设为1,即将NURBS曲线简化为非均匀B样条曲线[23],其表达式为

(21)

式(21)表示的线性方程组由n+1个矢量方程组成,包含n+3个控制点,未知控制点数大于方程数。因此,还需要添2个边界条件来求解方程组。采用切失条件作为边界条件。整理可得

(22)

其中

ei=(Δi+2+Δi+1)Qi-1=(ui+3-ui+1)Qi-1

(1≤i≤n)

取首末重复度为4,三次非均匀B样条曲线的首末控制顶点即为特征点,即Q0=P0,Qn=Pn+2。且首末处有切失条件[24]

(23)

式中q0、qn——首末切失条件

由式(20)可计算出节点矢量,代入式(22)可计算出所有控制点,将控制点代入式(21)即可求出三次非均匀B样条曲线。

2.3.3基于多约束条件的三次非均匀B样条曲线

在果园喷雾机器人的路径规划过程中对于轨迹曲线应有以下要求:

(1)曲率连续约束[25]。为避免喷雾机行驶过程中出现急转、急停等现象,需要确保轨迹曲线的曲率连续。采用三次非均匀B样条曲线,满足C2连续性质。

(3)转向机构延迟约束。对于绝大多数农业机器人,其前轮转向机构具有一定的时间延迟,即从指令下发到转向机构做出角度转变需要一定的转换时间,这段时间差会使得机器人在实际跟踪行驶过程中出现误差。这种延迟带来的误差可以通过两方面来减少。一方面是优化转向机构减少反应时间,这种方式直接有效但会增加成本且无法根本解决问题;另一方面可以将反应时间加入轨迹的优化过程中,通过增加转向角的角速度约束来保证给机器人留有足够的反应时间从而减少延迟带来的误差。

对于两个分段中的转弯曲线,可以通过调整其特征点的方式来改变曲线的形状,其中特征点即为位置约束点,默认将前面优化后的路径点作为特征点。特征点与曲线关系示意图如图7所示。

图7 转弯曲线特征点与曲线关系

图中点Q1、Q5为分段点、曲率极大值点。在已知分段点、曲率极大值点的条件下,通过改变点Q2和Q4的位置可以改变曲线的形状,其中与两点设定为关于点Q3对称,简化了计算且符合转弯逻辑。因此,通过求解多约束条件下点Q2的坐标即可得到最优的转弯轨迹。点Q0与Q6分别为两侧分段点的前后一个特征点,分别形成两条直线,即部分行间轨迹。在优化计算的过程中,代入两个特征点Q0与Q6可以保证转角轨迹与行间轨迹曲率的约束以及连续性。

如图7所示,转角轨迹优化的初始已知条件为:喷雾机起始位置为Q0(x0,y0)、目标位置为Q6(x6,y6)、转角顶点为Q3(x3,y3),变量点Q2(x2,y2),可得Q4坐标为

(24)

该轨迹上任一点曲率为[28]

(25)

式中u——三次B样条曲线的节点向量

机器人底盘的最小转弯半径为Rmin,轨迹曲线应满足约束条件

(26)

机器人底盘跟踪轨迹曲线时,其前轮转向角为

(27)

前轮转角不能超过机器人底盘的最大前轮转角。

当选择变量Q2的坐标时,易见得Q2应处于Q1与Q3连线的上方,否则轨迹曲率会变化较大,不利于跟踪。因此有约束条件

(28)

对于果园喷雾机器人,其前轮转向角度差为

δmax=2θ

(29)

式中θ——转向机构单向最大转角

因此果园喷雾机器人最大转向角速度为

(30)

式中ts——转向机构动作反应时间

在果园喷雾机器人的轨迹规划过程中应该考虑到其曲率所反映的前轮转角变化速度应小于转向机构的最大角速度。

对于转向机构的角速度约束,则需要转向角参数和时间参数。由式(27)和自行车模型下的几何公式得转向角为

(31)

(32)

式中ld——前视距离

α——机器人位置与目标点夹角

在进行角速度规划时,如果曲线长度未知,则很难进行时间规划,因此,可以通过积分来计算曲线长度[29]

(33)

通过路径长度计算出每一段的预期时间td为

(34)

式中vs——期望速度

n——B样条曲线段数

因此对于每一段路径曲线都有转向角速度约束

(35)

其中dt的分布范围为[0,ntd]。

2.3.4参数最优化求解

假设转角轨迹上任一点的曲率为κ(u)。用三次非均匀B样条曲线将轨迹表示后,使用最优化的方法来求解在满足多约束条件下的曲线参数。最优化的目标为转角轨迹曲线的曲率均值,即

(36)

通过控制变量Q2的坐标,改变曲线形状,使得曲率均值最小,获得光滑连续的轨迹曲线[30]。同时,最优化过程有以下约束条件

(37)

3 仿真及验证分析

3.1 全局路径仿真与评估

在第2节建立转弯曲线的数学模型后,以式(36)为目标函数,点Q2的坐标为变量,式(37)为约束条件,调用PSO函数,可求得在转弯场景下的最优曲线参数[31]。将各转弯曲线的特征点参数代入式(21)可计算出最终用三次非均匀B样条曲线表示的全局路径如图8所示。

图8 最终三次非均匀B样条全局路径曲线

图8中绿色圆点为模拟果树,黑色矩形为障碍物信息。实际果园中,同一树行的果树存在间距但喷雾机不应从中穿过,因此以果树的宽度与树行的长度整体生成矩形障碍物。同时,为保证喷雾机以“U”形运动轨迹遍历果园,分别将障碍物延伸至果园边界,形成“U”形长廊式结构,不同方法的优化结果如图9、10所示。

图9 轨迹优化方法对比

图10 U形环境轨迹曲率对比

3.2 轨迹跟踪仿真验证

在Matlab中根据果园喷雾机器人底盘模型编写跟踪代码进行跟踪仿真试验。选用纯跟踪算法,设定轴距为0.614 m,前视距离为1 m。将生成的全局路径导入跟踪算法的参考路径中,观察其横向误差、航向角以及前轮转角。

分别对传统无约束B样条函数优化的轨迹曲线和本文基于多约束条件优化生成的轨迹曲线进行跟踪仿真,其结果如图11、12所示。无约束路径点存在较大转角,由于轴距和前轮转角的限制无法精准跟踪,产生较大的前轮转角波动,影响果园喷雾机器人行驶稳定性。跟踪横向误差如图13和表1所示,原始折线路径由于存在尖角其误差最大。通过增加三次B样条函数优化与约束条件,可以有效缩小横向误差。

表1 仿真机器人跟踪轨迹误差

图11 无约束B样条优化轨迹跟踪结果

图12 多约束条件下B样条优化轨迹跟踪结果

图13 轨迹跟踪仿真误差对比

4 跟踪试验验证

4.1 试验平台

本文算法验证使用阿克曼底盘的无人喷雾机器人作为试验平台[32],如图14所示。无人喷雾机器人主要由机器人底盘(hunter2.0,深圳市松灵机器人)、中央计算机(英特尔酷睿八代i7-8565u)、组合导航系统(高精度组合导航系统X1,北云科技)和喷雾作业机构组成。

图14 果园喷雾机器人

本研究使用中央计算机装载Ubuntu18.04LTS操作系统,并基于机器人操作系统(Robot operating system,ROS)进行开发。基于ROS平台编写机器人通讯与行驶代码,并将规划后的路径载入预编好的容器中,通过可视化软件RVIZ可观察规划路径与实际行驶轨迹。

4.2 试验设计

试验场地位于江苏大学果园(32.218 879 1°N, 119.509 957 2°E)。在该试验场地中,共有5列树行,行距平均为3.3 m,株距平均为0.8 m,每列树行平均长10.5 m,如图15所示。主要考虑果园环境特征为连续的U形走廊,将果树树行视为障碍物后搜索出从起始点到末行末尾的路径,并采用不同的优化算法进行处理。

图15 果园环境试验场景

采用纯跟踪算法控制果园喷雾机器人对不同的轨迹进行跟踪行驶,并实时计算横向误差作为评价指标。纯跟踪算法的前视距离与速度会影响跟踪效果,为避免跟踪时出现振荡不稳定的情况,前视距离设置为1.0 m。同时为贴合实际工作情况,行驶速度设置为起始速度0.5 m/s并匀速加速至最大速度1.2 m/s。

4.3 试验结果与分析

通过对不同算法生成的路径进行跟踪试验,过程通过北斗导航系统中采集喷雾机器人实时位置信息并记录为文本文件,将该文本文件导入Matlab中[33]得到果园环境下的跟踪结果如图16所示。由图16可看出,当进行直线跟踪时其实际行驶轨迹与目标路径较为贴合,横向误差较小;当进行转弯行驶时,皆与目标路径产生了较大差距,即横向误差变大。转弯处的横向误差主要有2个原因:①纯跟踪算法的固有缺陷。②目标路径不符合果园喷雾机器人的行驶要求,机器人无法行驶到该路径的位置点上。在图16a中A*算法生成的路径点折线段的曲率较大,机器人无法跟踪到其线段的转折点上进而出现横向误差;在图16b中,无约束均匀B样条曲线两侧转弯处的轨迹形状复杂,且中间两处曲率较大,由于果园喷雾机器人底盘存在轴距与前轮转角的限制,其整体转弯角存在最大角度,当目标路径点的曲率较大时,机器人无法从当前位置点行驶至下一个目标点,从而出现了较大横向误差;在图16c中,增加约束条件的优化轨迹其曲率较小,且无较大波动,因此从3种算法的跟踪行驶效果中可以较为直观地观测出多约束条件下非均匀B样条优化算法生成的目标路径更符合机器人的行驶要求。

图16 果园环境下规划路径跟踪结果

如图17与表2所示,在起始时由于距离目标轨迹较远造成误差较大。在机器人行驶到目标轨迹直线部分时,误差开始收敛,此时开始计算平均误差。

表2 果园环境下轨迹曲率及机器人跟踪效果

图17 果园环境中规划路径跟踪误差对比

转弯处3种算法生成轨迹的跟踪误差差距较为明显,经多约束条件下三次非均匀B样条优化后的轨迹曲线其曲率最小,跟踪误差最小,行驶过程中最大曲率为0.31 m-1,横向误差标准差为0.031 m,符合果园喷雾机器人作业要求。

5 结论

(1)针对传统A*算法在果园环境中出现的路径点偏移不适合直接代入优化的问题,提出了一种初优化策略,根据树行位置信息矫正行间路径点。根据曲率分段划分路径点,并结合始末端点矫正首末树行外侧路径点。

(2)针对果园环境中喷雾机器人自主行驶过程,提出了一种基于三次非均匀B样条曲线的果园行驶路径规划方法。结合喷雾机器人最小转弯半径约束、转弯处首末端点连续约束、转向机构延迟约束等,建立多约束条件,以曲率均值最小为优化目标建立数学模型并求解。生成轨迹的最大曲率为0.31 m-1,平均曲率为0.15 m-1,相对于初始A*算法和三次均匀B样条算法大大缩小了轨迹曲率,符合果园喷雾机器人运动学约束。

(3)跟踪试验表明,果园喷雾机器人对果园行驶路径规划算法生成的路径可以较好地进行跟踪,平均横向误差为0.225 m,标准差为0.031 m,满足果园喷雾机器人行驶精度要求。