科学技术的进步正推动现代农业生产向自动化、规模化、精确化方向快速发展，具备自主导航功能的农业机械越来越多地应用到农业生产中^[1]。自主导航拖拉机作为牵引机械能够高效完成田间播种、施肥和耕地^[2-7]，自主导航联合收割机能够在无人干预的情况下收获小麦、水稻和玉米^[8-13]。当农机工作环境中出现少量障碍物时，障碍物可能会阻碍农机正常工作，因此规划一条建立在农机工作平台上的避障路径、帮助农机在存在障碍物的环境中正常完成作业任务必不可少。

现阶段国内外关于农机平台避障路径规划的研究较少，国内研究更多建立在无障碍物环境中，刘刚等^[14]提出一种基于GNSS的农田平整自动导航路径规划方法，实现拖拉机的高效平地作业；孟志军等^[15]提出一种面向农田作业机械的地块全区域覆盖路径优化方法，以生成转弯数最少、作业消耗最小、总作业路径最短或有效作业路径比最大的最优作业路径。国外学者针对有障碍物环境展开了相应的研究，Taïx等^[16]针对农田中小型静态障碍物提出一种折线避障方法，虽能实现避障功能，但避障代价过大，且未充分考虑农机的运动特性；圆弧避障方法是现阶段应用较为广泛的一种避障路径规划方法，但避障策略较为简单，当障碍物间距离较近时会出现算法失效的情况。现阶段成熟的路径规划算法如A-star算法^[17-18]、SA算法^[19]和人工势场法^[20-21]等大多运用在农业机器人和AGV小车^[22-26]等小型精准控制平台上，这些算法倾向于将控制物体等效于一个运动点，但运用在农机平台(如东方红SG250拖拉机)上时，将农机等效为单个运动点会丢失大量的农机自身运动模型信息。算法提供的控制策略也未充分考虑应用在农机平台的可行性，使得现阶段传统路径规划算法或其改进版无法适用于农机工作平台。

为了自主导航农业机械能够顺利避障，建立在农机自身运动模型以及农机工作环境基础上的避障路径规划必不可少。规划的路径指导自主导航农业机械在工作过程中以尽量小的避障代价实现障碍物的避障，并在避障完成后合理地回归原始工作路径，因此，自主导航农业机械避障路径规划的研究具有很强的实际意义。

1 避障路径规划算法 1.1 单障碍物避障路径规划算法 1.1.1 判断障碍物的威胁性

确定障碍物位置后，需判断障碍物是否会对拖拉机正常工作产生影响，具体工作流程如下：

1) 以一矩形示意表达拖拉机模型，以矩形中心O为原点建立平面坐标系，x轴正方向为水平向右，y轴正方向与拖拉机航向重合。

2) 以障碍物几何中心Z(X_Z，Y_Z)为圆点，Z到障碍物边界的最长距离R_Z为半径作圆，构建防碰撞区域。

3) 计算Z到y轴距离d，设d和R_Z的差值为c，比较c与半车宽L/2的大小。若c>L/2，为无威胁障碍物；反之为有威胁障碍物。

4) 当障碍物为无威胁障碍物时，拖拉机可沿原有导航路线继续自主导航；当障碍物为有威胁障碍物时，拖拉机需根据障碍物位置以及自身运动模型更新避障路径。如图1所示，障碍物Z为有威胁障碍物。

1.1.2 避障安全距离的计算

若障碍物为有威胁障碍物，需计算避障安全距离(D)，如果在自主导航过程中拖拉机与障碍物的距离小于D，即使以最大偏转角避障，拖拉机仍然会与障碍物碰撞。图1中，当拖拉机采用最大偏转角避障时，拖拉机转弯半径为最小转弯半径。具体的计算方法如下：

1) 设拖拉机最小转弯半径为r_L，则拖拉机转向中心X₁位置为(−r_L，0)，连接X₁与矩形右上角点F(X_F，Y_F)，以X₁为圆心，X₁到F距离为半径作圆，则圆X₁为拖拉机右侧边界角点行驶轨迹图。

2) 拖拉机向前行驶时，圆X₁在坐标轴上会随之沿y轴方向平移，若平移后的圆X_1′与防碰撞区域圆Z相切，设此时的圆心坐标X_1′为(−r_L，y)，则变量y满足方程：

$\sqrt {{{\left( { - {r_L} - {X_Z}} \right)}^2} + {{\left( {y - {Y_Z}} \right)}^2}} - {R_Z} = \sqrt {{{\left( {{X_F} + {r_L}} \right)}^2} + {{\left( {{Y_F} - {0}} \right)}^2}} $

解得y值，计算Y_Z与y的差值D，D为避障安全距离。

3) 设拖拉机当前所在位置y_O与Y_Z的差值为d_OZ，若d_OZ小于D，则拖拉机无法成功避障，应立即停止工作，防止与障碍物产生碰撞。

4) 由步骤2)确定拖拉机可向前行驶的安全距离区间为[0，Y_Z−D]，为使拖拉机尽可能地跟踪初始路径，尽量贴近障碍物后再执行避障动作，可将该区间分成若干段[0，d₁]、(d₁，d₂]、(d₂，d₃]…(d_n−1，d_n]，拖拉机在行进一段安全距离d_n(n=1, 2, 3, 4…)后再进行避障路径规划并计算避障总代价，筛选出一条代价最小的避障路径作为最终的避障路径，n的大小根据实际避障时具体D值决定。

1.1.3 避障最大转弯半径的确定

　　1) 如图2所示，当拖拉机向前行驶由1.1.2确定的一段避障安全距离后，便可以计算拖拉机的避障代价，但在此之前需根据障碍物的位置先确定拖拉机避障最大转弯半径，设拖拉机能够避障的最大转弯半径为r_max，则可确定拖拉机的初始转向中心X₁的位置为(−r_max，0)。

2) 根据拖拉机避障过程中临界碰撞条件“拖拉机右边界运动轨迹与圆Z相切”，列出方程：

$\sqrt {{{\left( {{X_Z} + {r_{\max}}} \right)}^{^2}} + {{\left( {{Y_Z} - 0} \right)}^{2}}} - {R_Z} = \sqrt {{{\left( {{X_F} + {r_{\max}}} \right)}^{2}} + {{\left( {{Y_F} - 0} \right)}^{2}}} $

3) 解方程，得避障最大转弯半径r_max。

1.1.4 4段路径规划

　　1) 设车辆实际转向半径为r(r≤r_max)，则实际转向中心X₂为(−r，0)。以X₂为圆心，r为半径作圆X₂，如图3所示：

2) 连接圆心X₂与Z点，与圆X₂相交于O₁点，则 ${ {{O}{O}_1}}$ 为第1段避障路径。

3) 以Z为圆心，以ZO₁距离为半径作圆，与直线y=Y_Z交于O₂点，则 ${ {{O}_1{O}_2}}$ 为第2段避障路径。

4) 作O₁点关于直线y=Y_Z的对称点O₃，则 ${ {{O}_2{O}_3}}$ 为第3段避障路径。

5) 作X₂关于直线y=Y_Z的对称点X₃，以X₃为圆心，X₃到O₃点的距离为半径作圆X₃，与y轴相切于O₄点，则 ${ {{O}_3{O}_4}}$ 为第4段避障路径。

考虑到自主导航拖拉机在偏转角控制上存在一定的滞后与误差，因此在避障路径的规划过程中应尽量减少偏转角变化的次数。4段圆弧避障路径仅在O₁与O₃点要求拖拉机偏转角发生改变，以帮助拖拉机实现最基本的避障与回归原始路径功能。若是3段圆弧路径则只能实现避障功能，拖拉机在完成避障后无法回归原始航线。若是5段圆弧路径，则每段圆弧长度被压缩得相对较短，且会发生3次偏转角变化，实际操作时会放大拖拉机偏转角控制存在误差与滞后的问题，使得拖拉机难以跟踪规划路径。因此，对于拖拉机控制平台4段圆弧路径规划更稳定，实际操作时更易实现。

1.1.5 避障代价的计算

　　1) 以4段避障路径总长度S为避障代价，S由 ${ {{O}{O}_1}}$ 、 ${ {{O}_1{O}_2}}$ 、 ${ {{O}_2{O}_3}}$ 、 ${ {{O}_3{O}_4}}$ 构成。

2) 设O₁X₂与x轴的夹角为θ，则θ的大小为：

$\theta = \arctan \frac{{{Y_Z}}}{{{X_Z} + r}}{\text{。}}$

$ \begin{array}{c} {S_1} = \dfrac{{{\text{π}}r}}{{180}} \arctan \dfrac{{{Y_Z}}}{{{X_Z} + r}}{\text{，}}\\ {S_2} = \dfrac{{{\text{π}}\left( {\sqrt {{{\left( {{X_Z} + r} \right)}^2} + {Y_Z}^2} - r} \right)}}{{180}} \arctan \dfrac{{{Y_Z}}}{{{X_Z} + r}}{\text{，}}\\ {S_3} = \dfrac{{{\text{π}}\left( {\sqrt {{{\left( {{X_Z} + r} \right)}^2} + {Y_Z}^2} - r} \right)}}{{180}} \arctan \dfrac{{{Y_Z}}}{{{X_Z} + r}}{\text{，}}\\ {S_4} = \dfrac{{{\text{π}}r}}{{180}} \arctan \dfrac{{{Y_Z}}}{{{X_Z} + r}}{\text{，}} \end{array}$

3) 设 ${ {{O}{O}_1}}$ 、 ${ {{O}_1{O}_2}}$ 、 ${ {{O}_2{O}_3}}$ 、 ${ {{O}_3{O}_4}}$ 长度为S₁、S₂、S₃、S₄，具体计算方法如下：

则S关于转向半径r的函数为:

$\begin{align} S = & {S_1} + {S_2} + {S_3} + {S_4}= \\ & \frac{{{\text{π}} \left( {\sqrt {{{\left( {{X_Z} + r} \right)}^{2}} + {Y_Z}^{2}} } \right)}}{{90}} \arctan \frac{{{Y_Z}}}{{{X_Z} + r}}{\text{。}} \end{align} $

4) 在r∈[r_L，r_max]区间内，计算可得S最小时的r取值。其中r_L为车辆最小转弯半径，可从车辆说明书中获得，r_max为1.1.3确定的车辆避障最大转弯半径。

1.1.6 避障总行驶路径的计算

自主导航拖拉机执行避障动作时，需确定避障起始点，由1.1.2可确定允许拖拉机向前行驶的距离为d₁、d₂、d₃、d₄…d_n，以1.1.4、1.1.5方法计算相对应的n个避障代价S₁、S₂、S₃…S_n，最终拖拉机完成避障动作所需的总路径长度为C_n = S_n+d_n，当C_n为最小时，设此时规划的避障路径为最优避障路径，此时确定的拖拉机转弯半径r为最优转弯半径。

1.2 双/多障碍物避障路径规划算法

单障碍物避障算法适用于路径上出现单一障碍物的情况，当拖拉机自主导航行驶路径上出现2个障碍物时，可使用双障碍物避障算法予以解决。

1.2.1 障碍物Z避障路径规划

首先根据障碍物Z的位置，由单障碍物避障算法进行初始避障路径规划。

1.2.2 构建安全行驶区域

如图3所示，完成路径规划后，构建障碍物Z的安全行驶区域，该部分覆盖拖拉机避障经过的所有区域，在图中由A、A₁…A₆，B、B₁…B₆以及拖拉机两侧边界延长线构成。

1.2.3 判断障碍物是否存在威胁

首先为障碍物Z₁构建防碰撞安全区域，并判断是否会与1.2.2建立的安全行驶区域产生交集。若产生交集，说明Z₁存在威胁；若无交集，则Z₁无威胁，原有路径无需做出修改，拖拉机可正常行驶通过。

1.2.4 判断障碍物远近属性

若障碍物Z₁存在威胁，则需对障碍物远近属性进行判断。如图3所示，当拖拉机沿着O₁…O₄完成障碍物Z的避障后回归原始航线，中心O点随之运动到O₄，以O₄点为原点重新建立避障坐标系，拖拉机可利用单障碍物避障算法沿着O₅、O₆、O₇ 3个点再次实现Z₁的避障，此时称Z₁为远障碍物。当拖拉机完成Z的避障后回归原始航线，若Z₁和拖拉机距离已小于避障安全距离D，拖拉机无法完成Z₁的避障，此时称障碍物Z₁为近障碍物，如图3所示，Z_1′为近障碍物，Z₁为远障碍物。

1.2.5 近障碍物避障

若障碍物Z₁为近障碍物，则要求拖拉机在障碍物Z的避障过程中，利用单障碍物避障算法对Z₁进行实时避障路径规划，帮助拖拉机完成Z₁的避障。

在Z避障过程中，根据Z₁位置，单障碍物避障算法会采取左、右避障2种不同的避障策略：

1) 左避障策略。如图4所示，若拖拉机在障碍物Z的避障过程中行进到O₂点时，障碍物Z₁处在拖拉机航向右侧，此时应采取左避障策略，沿O₂…O₅完成避障后，拖拉机距离初始路径较远，此时拖拉机应尽快回归初始路径，为保持拖拉机运动连贯性，以Z₁为圆心，Z₁O₅为半径作圆X₄。作圆X₅与Y轴、圆X₄相切于O₆、O₇ 2个点，圆X₅半径与圆X₃半径相同，圆X₃提供Z₁左避障时的第1段避障圆弧 ${ {{O}_3{O}_4}}$ 。当拖拉机沿O₂…O₅完成Z₁避障后，可继续跟踪由O₅、O₆、O₇组成的2段圆弧回归初始路径。

2) 右避障策略。如图4所示，若拖拉机在障碍物Z的避障过程中行驶经过O₂点而继续行进到O_2′点时，Z₁处在拖拉机航向左侧，此时应采取右避障策略，完成避障后，与1)存在相同的问题，同样以圆X₂半径作圆X₆，圆X₆与圆X₄、Y轴相切于O₁₀、O₁₁ 2个点，圆X₂提供Z₁右避障时的第1段避障圆弧 ${ {{{O}_{{{2}'}}}{O}_8}}$ ，当拖拉机沿着O_2′、O₈、O₉完成Z₁的避障后，可继续跟踪由O₉、O₁₀、O₁₁组成的2段圆弧回归初始路径。

1.2.6 多障碍物避障算法

多障碍物避障算法建立在双障碍物避障算法之上，当拖拉机采用单障碍物避障算法实现第1个障碍物的避障动作后，利用双障碍物避障算法可实现第2个障碍物的避障。若存在第3个障碍物，则可在拖拉机完成第2个障碍物的避障后再次调用双障碍物避障算法。以此类推，实现第n−1个障碍物Z_n-1的避障后，可调用双障碍物避障算法实现障碍物Z_n的避障。

2 避障路径规划试验及结果分析 2.1 试验设计

以具备调速和自主导航功能的东方红SG250拖拉机作为试验平台，为使GPS系统正常工作，选择在光线条件良好的晴天的上午开展试验。GPS采用载波相位实时差分技术，定位精度达cm级，将拖拉机跟踪避障路径时的GPS定位数据作为参考标准，以此验证算法的可行性，试验在南京农业大学工学院车库场地进行，如图5所示。

现阶段针对静态障碍物避障路径规划的研究较少，文献[27]提出的避障路径规划算法是现阶段使用较为广泛的方法，与本文所提算法相比，在面对障碍物时均采用4段圆弧避障的形式，具有一定的参考对比价值。下文以L算法指代，利用L算法规划避障路径可通过图6复现。算法由3段圆弧组成，圆弧半径均为拖拉机最小转弯半径，避障路径由 ${ {AB}}$ 、 ${ {BC}}$ 和 ${ {CD}}$ 3段圆弧组成，L算法未涉及避障安全距离。

试验分为单障碍物避障试验与双障碍物避障试验，障碍物为随机设置，试验前由Bumblbee2双目相机完成障碍物的识别与定位。第1次单障碍物避障试验障碍物位置为(−0.125，4.820)，第2次单障碍物避障试验障碍物位置为(−0.125，6.820)，R_Z均为0.45。双障碍物避障试验障碍物位置分别为(0.800，2.500)、(−0.200，5.800)，R_Z= 0.50。

2.2 结果与分析

拖拉机分别以0.3、0.5 m·s⁻¹的恒定速度进行试验。单障碍物避障试验结果如图7、图8所示，双障碍物避障试验结果如图9所示。图7表示单个障碍物在不同位置，拖拉机以不同速度跟踪L算法规划的路径时，行驶路径与规划路径的偏差。图8表示在与图7相同条件下，拖拉机以不同速度跟踪单障碍物避障算法规划的路径时，行驶路径与规划路径的偏差。试验中采用0.5 m作为间隔对安全距离区间进行分段，不同速度下2组单障碍物避障试验规划的第1段路径均为行驶的安全距离，仅在距离长短上存在差异。当障碍物位置为(−0.125，4.820)时，拖拉机行驶的安全距离为1.5 m，当障碍物位置为(−0.125，6.820)时，拖拉机行驶的安全距离为3.5 m。因此最终试验结果以行驶d_n后的起始避障点作为原点，更新障碍物坐标后，2组试验障碍物的位置坐标相对拖拉机当前位置均为(−0.125，3.320)。根据障碍物位置2组试验采用相同的4段圆弧避障策略，最终不同速度下2组避障试验中拖拉机行驶路径与规划路径差异很小(图8)。图9表示在双障碍物条件下拖拉机以不同速度跟踪双/多障碍物避障算法规划路径时，行驶路径与规划路径的偏差。

表1定量分析了在单障碍物和不同行驶速度条件下，单障碍物避障算法与L算法规划路径和行驶路径的差别。通过行驶路径累计误差与方差的对比，反映2个算法在实际应用时稳定性与实用性的差别。

当障碍物位于(−0.125，4.820)时，单障碍物避障算法规划路径比L算法减少32%。试验中当拖拉机行驶速度以0.3、0.5 m·s⁻¹时跟踪单障碍物避障算法规划路径时行驶路径比L算法分别减少35%、38%，行驶路径累计误差分别减少53%、66%。

当障碍物位置位于(−0.125，6.820)时，单障碍物避障算法规划路径比L算法减少26%。试验中当拖拉机行驶速度以0.3、0.5 m·s⁻¹时跟踪单障碍物避障算法规划路径时，行驶路径比L算法分别减少26%、22%，行驶路径累计误差分别减少82%、62%。

由表2可知，双/多障碍物避障算法规划路径为10.29 m，试验中当拖拉机行驶速度为0.3、0.5 m·s⁻¹时，跟踪双/多障碍物避障算法行驶路径为10.30、10.36 m，与规划路径相比仅分别增加0.09%、0.6%，跟踪双/多障碍物避障算法规划路径的行驶路径累计误差为9.99、4.13 m。

图10表示4组试验中由GPS定位数据计算得到的多点误差值，图 10a表示当障碍物位置为(−0.125，4.820)、(−0.125，6.820)时在单障碍物避障算法下行驶路径与规划路径的误差；图 10b表示当障碍物位置为(−0.125，4.820)时在L算法下行驶路径与规划路径的误差；图 10c表示当障碍物位置为(−0.125，6.820)时在L算法下行驶路径与规划路径的误差；图 10d表示当道路上存在2个障碍物，位置为(−0.200，5.800)、(0.800，2.500)时在双/多障碍物避障算法下行驶路径与规划路径的误差。对应的方差值可参照表1、表2，在相同行驶条件下，跟踪单障碍物避障算法规划路径获得的方差值均小于L算法。

3 结论

1)单障碍物避障算法的规划路径与L算法相比大幅减少，障碍物在不同位置下减幅达32%、26%。

2)算法投入实际应用中，由单障碍物避障试验4组数据可得，行驶路径平均减少30%，行驶路径累计误差平均减少65%。

3) L算法在试验过程中出现了算法起始避障点超过拖拉机当前位置点的情况，导致拖拉机无法完整跟踪避障路径。本文算法很好地规避了这个问题，首先利用避障安全距离尽可能地贴近障碍物，再采用避障策略完成避障动作。若拖拉机当前位置过于贴近障碍物，算法会提示拖拉机停止工作，否则会有与障碍物碰撞的风险。

4)由统计误差的方差可知，无论拖拉机行驶速度快或慢，跟踪本文算法避障路径的稳定性更好，基于拖拉机运动模型建立的避障策略更圆滑，很少出现L算法中拖拉机大幅偏离行驶路径的情况。除此之外，本文算法解决了现阶段2个障碍物相隔距离较短时拖拉机无法完成避障的问题。