拖拉机自动驾驶系统上线轨迹规划方法

贾　全　张小超　苑严伟　赵　博　伟利国　付　拓

(中国农业机械化科学研究院， 北京 100083)

(i=1,2,…,m-3)

0　引言

拖拉机自动驾驶系统是当前农业机械智能化领域不可或缺的一部分[1-3]，具备自主行驶能力的农业车辆不仅有助于提高劳动生产率，而且有利于解决农村劳动力不足的问题。

路径规划算法[4-5]是自动驾驶系统的关键技术，当前的研究热点大多集中在田间全局路径规划[6]以及地头转弯模式上，实现方法有几何法、人工势场法[7-8]、A*算法[9-10]、神经网络法[11]以及蚁群算法[12]等。关于上线轨迹规划的研究相对较少。因此，现有拖拉机自动驾驶系统只能依靠内置的路径跟踪算法[13-15]从车辆当前位置出发逼近全局路径上的目标上线点，整个上线过程行驶距离较长，且在目标点处会出现震荡，跟踪误差较大。拖拉机驾驶员为了提高上线精度，在上线过程中只能依靠手动驾驶，自动化程序较低。

为解决该问题，本文在现有研究基础上，综合考虑车辆运动学约束条件，对拖拉机从起始位置到全局路径目标点的自动上线轨迹开展相关研究。

1　上线轨迹描述

如图1所示，本文将全局路径上的起始作业位置定义为上线点；将拖拉机从其初始位置出发行驶至全局路径上线点的过程定义为上线过程，上线过程行驶过的距离称为上线距离；将上线过程中拖拉机经过的一系列离散位置点序列s=(Sstart，S1，S2，…，Sn，Sgoal)定义为上线轨迹。图1中，Sgoal是全局路径的起始作业点，也是上线轨迹的目标点。

图1　拖拉机上线轨迹示意图Fig.1　Schematic diagram of tractor guided trajectory

本文把上线轨迹规划问题定义为：在满足多种非线性约束条件的轨迹簇si中寻找一条距离最短的上线轨迹作为最优目标路径。函数描述为

(1)

式中soptimal——最优目标路径

d——第i条轨迹各点间距

(2)

(3)

φ=arctan(Lκ)

(4)

式中L——拖拉机轴距

2　轨迹规划算法

2.1　上线轨迹约束

2.1.1最大曲率约束

为保证上线轨迹具有可跟踪性，需要对轨迹曲率进行限制。曲率与拖拉机转弯半径的关系式为

(5)

式中R——转弯半径

图2所示为简化二轮车模型[17]，拖拉机最小转弯半径主要受轴距和转向前轮的最大转向角限制，其数值是固定的，计算公式为

(6)

式中Rmin——最小转弯半径

φmax——前轮最大转向角

图2　简化二轮车模型Fig.2　Simplified two-wheeled model

结合式(3)、(5)、(6)可知，上线轨迹各点处的曲率均须满足约束

(7)

2.1.2起止点航向约束

为避免拖拉机在上线轨迹的起始时刻出现原地转向或震荡现象，需要对上线路径的初始点航向进行约束。当路径点Si=Sstart时，约束条件为

|θs-θv|≤ε

(8)

式中θs——路径航向θv——车身航向

ε——航向阈值(取较小的正数)

为了保证拖拉机上线轨迹与全局路径结合处的平滑度，提高上线点Sgoal处的路径跟踪精度，当路径点Si=Sgoal时，须满足约束

|θs-θg|≤ε

(9)

式中θg——目标点航向

2.1.3转向角约束

拖拉机前轮转向范围决定了其路径跟踪的灵活性，如果上线轨迹某点处等效前轮转角过大，该点处的路径将不具备可跟踪性。因此，需要对等效前轮转角进行约束。由式(3)、(4)可得到等效前轮转角，建立约束条件为

(10)

实际上线作业过程中，为保证路径跟踪精度，一般将拖拉机行驶速度控制在3 km/h以内。经测试，在该速度条件下只要规划的上线轨迹满足上述约束条件，拖拉机转向系统均可有效跟踪期望转角信号。因此，本文上线轨迹规划算法未考虑转向角速度及角加速度约束。

2.2　B样条曲线

轨迹的表示方法有很多种，文献[18]提到了7种曲线表示方法，并对它们的优缺点进行了对比分析。曲线最终形状方便控制是B样条的优点之一，为了改变B样条曲线的形状，可以修改一个或多个控制参数，如控制点的位置、节点位置或曲线的次数。另外，B样条曲线分段组成，修改某一控制点只引起与该控制点相邻的曲线形状发生变化，而远处的曲线形状不受影响。

B样条函数本身的特性[19-21]使其非常适合应用于路径规划场合，因此本文选用B样条曲线进行上线轨迹规划。

2.2.1B样条曲线定义

B样条曲线表达式为[14]

(11)

其中

Fk,n(u)=

(12)

式中n——B样条曲线阶次

Si,n(u)——对应节点u的第i段曲线上的点

Fk,n(u)——n次B样条基函数

Pi+k——第i+k个控制点

其中，节点u∈[0,1]；设控制点个数为m，则i=1,2，…，m-n，表示整条曲线由m-n段B样条曲线平滑连接而成，每段曲线由n+1个控制点生成。

由式(10)可知上线轨迹至少要保证2阶可导，为满足约束条件且降低计算量，本文选用3次B样条曲线，即n=3。此时，B样条基函数为

(13)

其中

式中u1、u2、u3——节点u的1、2、3次幂

将式(13)代入式(11)可知，上线路径及其1、2阶导数分别为

(14)

(15)

(16)

其中

P=[PiPi+1Pi+2Pi+3]T

(i=1,2,…,m-3)

式中，Pi～Pi+3是用于计算第i段B样条方程的4个控制点。

2.2.2样条曲线控制点确定

当样条曲线的控制点较少时不易求得满足约束的期望路径，控制点过多则会造成方程求解困难，增加运算时间。根据经验采用6个控制点P0～P5生成满足约束条件的三阶B样条上线轨迹。

现有文献大多采用优化算法直接求取N个控制点[16,22]，对于6个控制点P(x，y)需要优化的参数为12个，当使用迭代方法求解时运算量非常大。为了提高轨迹规划速度，有必要研究曲线约束与控制点间的关系以简化计算。

对于三阶B样条曲线，当3个相邻的控制点在同一条直线上且间距相等时，生成的B样条曲线与直线相切于中间的控制点，如图3所示。

图3　B样条曲线控制点关系Fig.3　B-spline control points relationship

依据上线路径初始点的航向角约束，选择控制点P0、P1、P2。当3个控制点满足如下条件，即可满足拖拉机初始状态约束：选定拖拉机初始位置为P1；P0、P1、P2三点共线；矢量P0P2的方向角与拖拉机初始航向一致；P0和P2关于P1对称。

同理，依据目标点航向约束，确定控制点P3、P4、P5，并选定P4为目标点位置坐标。

设线段P0P1和P1P2的长度为l1，线段P3P4和P4P5的长度为l2。依据起止点航向约束选定控制点P1和P4后，只要确定l1和l2的数值即可求得其它控制点并生成上线轨迹，从而把最优轨迹规划问题转变为求l1和l2的最优解问题，相比求解12个参数，该方法只需要优化2个参数，有效减少了算法的运算量。

2.3　基于量子遗传算法的参数优化

本文采用量子遗传算法求解l1和l2，量子遗传算法[22](Quantum genetic algorithm，QGA)是遗传算法和量子计算结合产生的一种新的概率进化算法，比传统遗传算法具有更快的收敛速度和并行处理能力。

将l1、l2作为个体进行寻优，把上线轨迹的距离取负值作为适应度函数(轨迹越短，适应度值越大)，按照以下算法流程循环执行直至达到结束条件：

(1)初始化种群Q(t0)。

(2)对初始化种群Q(t0)中的每个个体(即l1和l2)进行一次测量，得到对应的确定解(上线轨迹距离)。

(3)对各个确定解进行适应度(轨迹距离取负值)评估。

(4)记录最优个体和对应的适应度，以最优个体作为下一代进化目标。

(5)判断是否满足结束条件，是则退出，否则继续计算。

(6)对种群Q(t)中的每个个体实施一次测量，得到对应的确定解。

(7)对各个确定解进行适应度评估。

(8)量子旋转门更新种群，得到下一代种群Q(t+1)。

(9)记录最优个体和对应的适应度。

(10)将迭代次数加1，返回步骤(5)。

3　仿真分析

3.1　仿真实例设计

设拖拉机模型采用前轮转向，轴距为2.2 m，前轮转角范围为[-π/6，π/6]，单位为rad。由2.1节所述方法建立上线轨迹约束模型。

将拖拉机初始位置Pstart定义为(0，0)，初始航向角θstart定义为0 rad。图4中Pgoal是目标点位置，θgoal是目标点航向角，r是拖拉机初始位置和目标点的相对距离，θr是矢量PstartPgoal的方向角。

图4　工况初始状态示意图Fig.4　Schematic diagram of initial state of conditions

为了使仿真实例更具代表性，设计4种作业工况。取θr初值为π/4，按照π/2等间隔分布；令θgoal与θstart的方向角分别相差0、π/4、3π/4、π，各工况目标点位置分布和航向如图4所示，具体参数见表1。初始工况1～4分别代表以下4类作业工况：

初始工况1：|θr-θstart|∈[0，π/2]，|θgoal-θstart|∈[0，π/2)；初始工况2：|θr-θstart|∈(π/2，π]，|θgoal-θstart|∈[0，π/2)；初始工况3：|θr-θstart|∈(π/2，π]，|θgoal-θstart|∈[π/2，π]；初始工况4：|θr-θstart|∈[0，π/2]，|θgoal-θstart|∈[π/2，π]。

表1　4种初始作业工况参数Tab.1　Four initial operating conditions parameters

结合初始状态描述和2.2.2节内容得各控制点计算方法如表2所示，其中相对距离r取20 m，l1和l2采用2.3节所述量子遗传算法求解，为了尽可能找到全局解，取种群规模为60，优化代数为200。

表2　控制点计算方法Tab.2　Control points calculation method

3.2　仿真结果分析

按照仿真实例描述在Matlab软件环境中编程实现各工况条件下上线轨迹规划，采用量子遗传算法优化得到各工况控制点分布如表3所示，上线轨迹仿真结果如图5～9所示。

(1)量子遗传算法优化过程见图5。从图5可知，4种工况条件下目标函数均可在40代左右迭代收敛到最佳值。l1、l2最优解及相应上线轨迹长度如表4所示，对比各初始状态及上线轨迹长度可知，在相对距离r相同情况下4类作业工况所需最小上线距离由小到大排序为工况1、工况4、工况2、工况3。

表3　4种作业工况轨迹控制点Tab.3　Trajectory control points for four operating conditions　m

图5　量子遗传算法优化过程Fig.5　Quantum genetic algorithm optimization process

图6　工况1上线轨迹规划结果Fig.6　Guided trajectory planning results of case 1

图7　工况2上线轨迹规划结果Fig.7　Guided trajectory planning results of case 2

图8　工况3上线轨迹规划结果Fig.8　Guided trajectory planning results of case 3

图9　工况4上线轨迹规划结果Fig.9　Guided trajectory planning results of case 4

(2)各工况上线轨迹规划结果如图6～9所示。由图6～9中的图a和表4可知，4种工况条件下，均可通过优化配置B样条控制点实现上线轨迹规划；由图6～9中的图b可知，上线轨迹各点处曲率连续且均不大于0.26 m-1，上线初始点和目标点处曲率接近零，有利于提高起止点路径跟踪效果。

(3)由图6～9中的图c可知，拖拉机沿上线轨迹行驶时，最大等效前轮转角均小于0.52 rad；等效前轮转角在初始时刻接近于零，有利于避免车辆原地转向情况；转角变化较为平缓，有利于实现控制器对期望角度的跟踪。

表4　l1、l2最优解及上线轨迹长度Tab.4　Optimal solution and guidance trajectory length　m

(4)由图6～9中的图d可知，车身航向在上线过程中平滑过渡，轨迹在起始点处的航向均在零附近，与起始点的真实航向接近；目标点处的航向与仿真实例中设计的目标点航向0、π/4、3π/4、π接近，满足起止点航向约束条件。

4　田间试验

4.1　试验设计

田间试验在中国农业机械化科学研究院北京农机试验站进行。选用雷沃M1004型拖拉机作为试验车辆，选用自主研发的BDLeader-301型农业机械导航系统作为控制平台，试验现场及平台硬件组成如图10所示。

图10　装配BDLeader-301型导航系统的M1004型拖拉机Fig.10　M1004 tractor equipped with BDLeader-301 autopilot system

图11　田间试验设计Fig.11　Field trial design

由仿真结果可知，在第3类工况条件下上线距离较长，曲率和航向变化较大，路径跟踪相比其它几类工况较为复杂。因此，设计了圆形全局路径，并针对第3类工况条件下的上线轨迹规划方法开展田间对比试验研究，如图11所示。分别基于纯追踪算法[23]和上线轨迹规划方法完成上线过程，拖拉机上线行驶速度为3 km/h。

试验流程如图12所示。由于试验田块限制，把圆形路径的半径设为10 m，拖拉机初始位置到目标点的距离r设为6.5 m，以拖拉机到达全局路径终点作为终止条件。

图12　田间试验流程Fig.12　Flow chart of field test

图13　田间试验对比Fig.13　Comparison results of field test

4.2　试验结果分析

田间试验上线轨迹对比如图13所示。从图13可知，上线过程中基于上线轨迹规划方法的跟踪距离相对较短，实际计算结果为23.7 m；单独采用纯追踪算法跟踪效果较差，实测跟踪轨迹长度为78.6 m。

图14和图15对比给出了前轮转角的变化曲线。图中横轴表示拖拉机导航系统在自动跟踪上线轨迹和全局路径过程中，上位机软件对拖拉机前轮角度的离散采样点，纵轴表示拖拉机前轮转角。由图14可知，单独采用纯追踪算法时，拖拉机前轮转角在目标上线点附近出现了较大幅度的摆动，这会导致车身震荡，影响该位置的路径跟踪精度和乘坐舒适性；当增加上线轨迹规划方法后，前轮转角在上线过程中变化连续，可以平稳地过渡到跟踪全局路径所期望的前轮转角，如图15所示。

图14　纯追踪算法前轮转角变化曲线Fig.14　Changing curve of front wheel angle by pure pursuit algorithm

图15　上线轨迹规划算法前轮转角变化曲线Fig.15　Changing curve of front wheel angle by guided trajectory planning algorithm

图16对比了2种上线跟踪方法在全局路径上的横向跟踪误差，误差定义见文献[23]。图中横坐标表示拖拉机导航系统从全局路径起始位置自动跟踪至全局路径终止位置过程中，上位机软件对横向跟踪偏差的采样时间，纵坐标表示横向跟踪误差。从图16可知，单独采用纯追踪算法在全局路径初始阶段的跟踪误差较大，基于上线轨迹方法在全局路径上的跟踪结果相对平稳。

图16　跟踪误差对比Fig.16　Comparison result of tracking errors

5　结论

(1)针对现有拖拉机自动导航系统在上线过程中的不足，提出了一种基于量子遗传算法优化的B样条理论上线轨迹规划方法。综合考虑最大曲率约束、起止点航向约束和最大转向角约束，把最优轨迹规划问题转换为B样条控制点参数优化问题，将上线距离作为优化目标，利用量子遗传算法建立了期望轨迹函数。

(2)根据拖拉机上线作业初始状态的不同，设计了4种典型作业工况验证上线轨迹规划算法。仿真结果表明：基于量子遗传算法优化的B样条理论可以求得满足多个非线性约束条件的期望轨迹，轨迹曲率变化连续，有利于路径跟踪控制器进行曲线跟踪控制。

(3)针对第3类作业工况，开展了田间对比试验，试验结果表明：相比当前拖拉机自动驾驶系统上线模式，基于量子遗传算法优化的B样条理论上线轨迹规划方法有助于减小上线距离，提高了全局路径的导航控制精度。

1ENGLISH A, ROSS P，BALL D, et al. Vision based guidance for robot navigation in agriculture[C]∥2014 IEEE International Conference on Robotics & Automation(ICRA), 2014:1693-1698.

2罗锡文，张智刚，赵作喜，等. 东方红 X-804 拖拉机的 DGPS自动导航控制系统[J]. 农业工程学报，2009， 25(11)：139-145.

LUO X W, ZHANG Z G, ZHAO Z X, et al. Design of DGPS navigation control system for Dongfanghong X-804 tractor[J]. Transactions of the CSAE, 2009, 25(11): 139-145.(in Chinese)

3伟利国，张权，颜华，等. XDNZ630型水稻插秧机GPS自动导航系统[J]. 农业机械学报，2011，42(7)：186-190.

WEI Liguo, ZHANG Quan, YAN Hua, et al. GPS automatic navigation system design for XDNZ630 rice transplanter [J]. Transactions of the Chinese Society for Agricultural Machinery, 2011,42(7): 186-190.(in Chinese)

4GALCERAN E, CARRERAS M. A survey on coverage path planning for robotics[J]. Robotics and Autonomous Systems, 2013, 61(12): 1258-1276.

5HAMEED I A. Intelligent coverage path planning for agricultural robots and autonomous machines on three-dimensional terrain[J]. Journal of Intelligent & Robotic Systems, 2014, 74(3-4): 965-983.

6刘刚,李笑,康熙,等.基于GNSS的农田平整自动导航路径规划方法[J/OL].农业机械学报,2016,47(增刊):21-29.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=2016s004&journal_id=jcsam. DOI：10.6041/j.issn.1000-1298.2016.S0.004.

LIU Gang,LI Xiao,KANG Xi,et al. Automatic navigation path planning method for land leveling based on GNSS[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2016,47(Supp.):21-29.(in Chinese)

7师五喜, 李福荣, 常绍平, 等. 基于递阶模糊系统的人工势场法机器人路径规划[J]. 天津工业大学学报, 2014, 33(6): 45-50.

SHI W X, LI F R, CHANG S P, et al. Robot path planning of artificial potential field method based on hierarchical fuzzy system[J]. Journal of Tianjin Polytechnic University, 2014, 33(6): 45-50.(in Chinese)

8LI G, TAMURA Y, YAMASHITA A, et al. Effective improved artificial potential field-based regression search method for autonomous mobile robot path planning[J]. International Journal of Mechatronics and Automation, 2013, 3(3): 141-170.

9彭松,贾阳.月球车全局路径规划中的A*算法改进[J]. 航天器工程, 2010(4): 80-85.

10黄辰, 费继友, 刘洋, 等.基于动态反馈A*蚁群算法的平滑路径规划方法[J/OL].农业机械学报,2017,48(4):34-40，102. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20170404&journal_id=jcsam. DOI：10.6041/j.issn.1000-1298.2017.04.004.

HUANG Chen, FEI Jiyou, LIU Yang, et al. Smooth path planning method based on dynamic feedback A*ant colony algorithm[J/OL].Transactions of the Chinese Society for Agricultural Machinery,2017,48(4):34-40,102.(in Chinese)

11NOGUCHI N, TERAO H. Path planning of an agricultural mobile robot by neural network and genetic algorithm[J]. Computers and Electronics in Agriculture, 1997, 18(2-3): 187-204.

12ZHOU K, JENSEN A L, SØRENSEN C G, et al. Agricultural operations planning in fields with multiple obstacle areas[J]. Computers and Electronics in Agriculture, 2014, 109: 12-22.

13胡静涛, 高雷, 白晓平, 等. 农业机械自动导航技术研究进展[J]. 农业工程学报, 2015, 31(10): 1-10.

HU Jingtao, GAO Lei, BAI Xiaoping, et al. Review of research on automatic guidance of agricultural vehicles[J]. Transactions of the CSAE, 2015, 31(10): 1-10.(in Chinese)

14孟庆宽，仇瑞承，张漫，等. 基于改进粒子群优化模糊控制的农业车辆导航系统[J/OL]. 农业机械学报，2015，46(3)：29-36，58. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no= 20150305&journal_ id=jcsam. DOI:10.6041/j.issn.1000-1298.2015.03.005.

MENG Qingkuan, QIU Ruicheng, ZHANG Man, et al. Navigation system of agricultural vehicle based on fuzzy logic controller with improved particle swarm optimization algorithm[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2015, 46(3): 29-36,58.(in Chinese)

15白晓平，胡静涛，高雷，等. 农机导航自校正模型控制方法研究[J/OL]. 农业机械学报，2015，46(2)：1-7. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20150201&journal_id=jcsam.DOI:10.6041/j.issn.1000-1298.2015.02.001.

BAI Xiaoping, HU Jingtao, GAO Lei,et al. Self-tuning model control method for farm machine navigation[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2015, 46(2): 1-7.(in Chinese)

16李红, 王文军, 李克强. 基于 B 样条理论的平行泊车路径规划[J]. 中国公路学报, 2016, 29(9): 143-151.

17SNIDER J M. Automatic steering methods for autonomous automobile path tracking[D]. Pittsburgh: Carnegie Mellon University, 2009.

18GALLINA P, GASPARETTO A. A technique to analytically formulate and to solve the 2-dimensional constrained trajectory planning problem for a mobile robot[J]. Journal of Intelligent & Robotic Systems, 2000, 27(3): 237-262.

19de BOOR C. A practical guide to splines[M]. New York: Springer-Verlag, 1978.

20COX M G. The numerical evaluation of B-splines[J]. IMA Journal of Applied Mathematics, 1972, 10(2): 134-149.

21DIERCKX P. Curve and surface fitting with splines[M]. Oxford:Oxford University Press, 1995.

22TAO Jin, SUN Qinglin, ZHU Erlin, et al. Quantum genetic algorithm based homing trajectory planning of parafoil system[C]∥Control Conference (CCC), 2015 34th Chinese. IEEE, 2015: 2523-2528.

23ZHANG Q, QIU H. A dynamic path search algorithm for tractor automatic navigation[J]. Transactions of the ASAE, 2004, 47(2): 639-646.