CN106909150A - 用于农机无人驾驶的避障、掉头路径规划及其控制方法 - Google Patents

Abstract

本发明提供了农业驾驶控制技术领域内的一种用于农机无人驾驶的避障、掉头路径规划及其控制方法，具体的为，通过传感器获取农机环境信息做出掉头或避障决策；若为避障决策，规划避障路径轨迹，使用改进的最短切线法离线计算出一条理论避障路径，利用基于Bezier曲线的路径优化方法优化步骤2中的理论避障路径得到实际避障路径；若为掉头决策，规划掉头路径轨迹，输入农机的作业模式，根据农机实际的作业模式选择掉头路径；农机在前进过程中，实时更新农机的位置信息，使用育瞄控制器和PI控制器的结合计算当前的前轮转向角，控制农机的转角实现农机的自动掉头或避障；本发明可实时获取农机的运动学参数，控制精度高，适应性强。

Description

用于农机无人驾驶的避障、掉头路径规划及其控制方法

技术领域

本发明涉及一种路径规划及其控制方法，特别涉及一种用于农机无人驾驶的避障、掉头路径规划及其控制方法。

背景技术

农机在自动导航作业时多在环境部分未知的情况下运行，实现对人进行安全保护和对农作物的伤害程度减到最低，同时又能最大的发挥自主导航农业车辆的生产效率，将是一个重要的研究问题，同时农机可能会遇到电线杆、小石块等相对比较小的障碍物或接近地头时，农机需要自动做出避障或掉头决策。

当进行避障时，现有技术中，针对比较小的障碍物，采用最短切线法设置避障路径，最短切线法形成的避障路径由两段直线段和一段圆弧段组成，直线段分别与圆弧段相切，这种避障路径虽然简单快捷，对于具有最小转弯半径的拖拉机很难按照折角进行转弯且很难控制，若控制农机按照这种避障路径行走，农机的控制精度很低；当进行自动掉头时需对准下一作业行的跟踪，控制农机准确的掉头技术是实现精准农业的关键；该技术可提高农机的作业精度和作业效率，使驾驶员摆脱长时间劳累的重复驾驶工作，降低劳动力。

在现有的农机路径跟踪方法上，另外，控制农机按照设定好的避障路径行走的控制方法有多种，如BUG算法、人工势场法、VFH算法、模糊逻辑算法、模糊神经网络算法等，这些算法的应用场景均是复杂的作业环境，算法的逻辑复杂， 应用于农机作业不复杂的作业环境下，反应速度较慢，反而降低其控制精度；总而言之，无论是路径规划还是路径的控制方法，控制农机按照设定好的路径行走的精度很低，偏离设定的路径曲线，导致农机绕过障碍物或掉头时行走的路程远，从开始进入设定曲线的起点至回到农机原始直线行走路径的时间长。

另外，现有技术中，无法根据农机的实际作业模式进行路径控制，适用范围小。

发明内容

针对现有技术中的缺陷，本发明的目的在于克服上述现有技术中的不足之处，解决现有技术中路径不容易控制且控制精度低的技术问题，提供一种用于农机无人驾驶的避障、掉头路径规划及其控制方法，本发明的控制方法实现简单且控制精度高，规划好的路径易控制，适用范围广。

本发明的目的是这样实现的：一种用于农机无人驾驶的避障、掉头路径规划及其控制方法，具体包括以下步骤，

步骤1：通过传感器获取农机环境信息做出掉头或避障决策；若为避障决策，规划避障路径轨迹，具体包括以下步骤，

步骤101：使用改进的最短切线法离线计算出一条理论避障路径；

步骤102：利用基于Bezier曲线的路径优化方法优化步骤2中的理论避障路径得到实际避障路径；

若为掉头决策，规划掉头路径轨迹，具体包括以下步骤，

步骤101’：输入农机的作业模式；

步骤102’：根据农机实际的作业模式选择对应的掉头路径；

步骤2：农机在前进过程中，农机通过传感器获得农机位置信息，使用育瞄控制器和PI控制器的结合计算当前的前轮转向角，控制农机的转向角使农机沿着设定的曲线行走实现农机的自动掉头或避障。

本发明工作时，通过传感器获取农机周围的环境信息，当农机的前方有障碍物时，做出避障决策，使用改进的最短切线法计算出一条理论避障路径，对理论避障路径进行优化得到一条更加容易控制的实际避障路径；当农机前方为地头时，做出掉头决策，输入农机的作业模式，农机感应周围的环境信息，农机根据实际的作业模式选择设定好的掉头路径；通过预瞄控制器和PI控制器的结合控制农机的前轮转向角使农机沿着设定好的避障曲线行走，控制容易且控制精度高，从而实现农机在导航作业时的自动避障或自动掉头；本发明通过预瞄控制和PI控制器的结合计算出农机前轮的转向角，控制容易，控制农机的前轮转向角使农机沿着设定好的曲线行走，控制精度高；同时，通过农机的实际作业模式选择相对应的掉头路径，适应范围更加广泛；另外，本发明通过改进后的最短切线法计算出一条理论避障路径，使用基于Bezier曲线的路径优化方法对理论避障路径进行优化，使避障路径更加容易控制；可应用于农机无人驾驶在田间导航作业时的自动掉头和自动避障的控制工作中。

为了进一步提高获得理论避障路径的可靠性，步骤101中，计算理论避障路径具体的为，计算农机前方障碍物的特征圆的大小、农机与障碍物的距离，根据特征圆的大小设定安全距离，根据农机的犁具宽度和农机最小转弯半径，设定一条理论避障路径；改进的最短切线法具体的为，以障碍物的中心为圆心做特征圆，特征圆的半径为r_min+w/2，理论避障路径由圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三组成，圆弧段一的一端与农机原始的直线路径相切，圆弧段一的另一端与直线段一的一端相切，直线段一的另一端和直线段二的一端分别与圆弧段二相切，直线段二的另一端与圆弧段三相切，圆弧段二为特征圆上的一段，圆弧段一和圆弧段三关于圆弧段二的中心线对称设置，农机依次经过圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三绕过障碍物；其中，r_min为农机的最小转弯半径，w为农机的作业宽度，障碍物的外接圆半径小于最小转弯半径。

为了进一步提高农机掉头路径的精度，所述圆弧段一的半径为r_min，所述圆弧段三的半径为r_min，圆弧段一的起点记为H点，圆弧段一的圆心记为O₁点，直线段一与农机原始的直线路径的相交点记为J，直线段一与圆弧段二的相切点记为D，农机原始路径与特征圆的相交点分别记为K和K’，JK=w/2，圆弧段二的圆心记为O点，O的坐标设为（a，b），圆弧段二的中心点记为B点，J点的坐标记为（x1，y1），JD的方程可以写成：

（1-1）；

特征圆的方程可以写成：

（1-2）

通过（1-1）和（1-2）可以求出k，D点为JD和特征圆的相交点，以此解出D点坐标；

设点O₁的坐标为（x₂,y₂），则点O₁到直线JD的距离为：

根据公式（1-3）和（1-4）求出O₁的坐标；则H点的坐标为（x₂，y₁），B点的坐标为（a，b+r）。

步骤102中，利用基于Bezier曲线的路径优化方法优化步骤2中的理论避障路径，具体的为，建立Bezier方程，

(1)给定空间n+1个点的位置矢量，则参数曲线上各点坐标的插值公式为：

（2-1）

其中构成该曲线的特征点，是n次Bernstein基函数：

（2-2）

由上述公式，可以得出三次和二次Bezier曲线的数学表达式，当n=3时，Q(t)为三次多项式，有四个控制点，其矩阵形式表达为：

（2-3）

当n=2,Q(t)为二次多项式，有三个控制点，矩阵表达式为：

（2-4）

（2）Bezier曲线的曲率表达式为：

（2-5）

其中，y=f（x）表示曲线的方程，y’为曲线的一阶导数，y”为二阶导数；

曲率半径为：

（2-6）

选取三次Bezier曲线，针对三次Bezier曲线：

（2-7）

（2-8）

其中，X0、X1、X2、X3分别为P0点、P1点、P2点和P3点处的横向坐标，Y0、Y1、Y2和Y3分别为P0点、P1点、P2点和P3处的纵向坐标；

P0点对应圆弧段一的起点H（x₂，y₁），P3点对应圆弧段二的中心点B（a，b+r），P1点（（x₂+a）/2，y₁），P2点（（x₂+a）/2，b+r），则实际故障路径对应的曲线的曲率半径计算公式为：

（2-9）

其中，由点（x₂，y₁）、（a，b+r）、（（x₂+a）/2，y₁）和（（x₂+a）/2，b+r）生成的Bezier曲线为实际避障曲线一，实际避障曲线一和实际避障曲线二关于圆弧段二的中心对称；

此设计中，提出Bezier曲线优化方法对理论避障路径进行优化，具体的将曲率不连续的理论避障路径优化成曲率连续的实际避障路径，优选三次Bezier曲线对理论避障路径进行优化，提高优化曲线的可控性，优化后的实际避障路径更加容易控制。

作为本发明的进一步改进，步骤101’中，农机的作业模式包括播种和耕地；步骤2中，播种情况下对应的掉头路径为多线型路径一，耕地情况下对应的掉头路径包括交叉型路径、圆弧路径和多线型路径二，且根据不同的犁具宽度选择不同的掉头路径，犁具宽度包括小宽幅、中宽幅和大宽幅，小宽幅、中宽幅和大宽幅作业模式下对应的掉头路径分别为交叉型路径、圆弧路径和多线型路径二；所述多线型路径一由直线段三、圆弧段四和圆弧段五组成，圆弧段四和圆弧段五相切且对应的半径相同，农机依次经过直线段三、圆弧段四和圆弧段五实现掉头；所述交叉型路径由圆弧段六、直线段四和圆弧段七组成，圆弧段六和圆弧段七相交且关于直线段四的中心对称设置，圆弧段六和圆弧段七的半径大小相同，农机依次经过圆弧段六、直线段五和圆弧段七实现掉头；所述圆弧路径由圆弧段八、圆弧段九和圆弧段十组成，所述圆弧段九分别和圆弧段八、圆弧段十相切，所述圆弧段八和圆弧段十关于圆弧段九的中心线对称设置，农机依次经过圆弧段八、圆弧段九和圆弧段十实现掉头；所述多线型路径二由圆弧段十一、直线段六和圆弧段十二四组成，圆弧段十一和圆弧段十二关于直线段六的中心线对称设置；小宽幅对应的犁具宽度的数值范围为r_min<W<1.5r_min,中宽幅对应的犁具宽度的数值范围为1.5r_min<W≤2 r_min,大宽幅对应的犁具宽度的数值范围为W>2r_min，W为犁具宽度；此设计中，根据不同的作业模式设置不同的掉头路径，适用范围更广。

为了进一步提高农机掉头路径的精度，所述多线型路径一的各个线段的数学关系式为：

（3-1）

其中，R1为圆弧段四对应的半径，R2为圆弧段五对应的半径，AE为直线段三的长度，AC为农机开始转弯的后轴中心与农机转弯结束的后轴中心间连接线段的长度；

所述交叉型路径的各个线段的数学关系式为：

（3-2）

其中，R3为圆弧段六对应的半径，R4为圆弧段七对应的半径，C₁D₁为直线段四的长度，A₁B₁农机开始转弯的后轴中心与农机转弯结束的后轴中心间连接线段的长度；

所述圆弧路径的各个线段的数学关系式为：

（3-3）

其中，Rt为圆弧段八、圆弧段九和圆弧段十对应的半径，圆弧段八、圆弧段九和圆弧段十的圆心的连线构成三角形，β为圆弧段九的圆心分别与圆弧段八和圆弧段十的圆心连线之间的夹角，α为圆弧段八的圆心分别与圆弧段九和圆弧段十的圆心连线之间的夹角，γ为圆弧段十的圆心分别与圆弧段八和圆弧段九的圆心连线之间的夹角；

所述多线型路径二的各个线段的数学关系式为：

（3-4）；

其中，R5为圆弧段十一对应的半径，R6为圆弧段十二对应的半径，B₂C₂表示直线段六，A₂D₂为圆弧段十一的圆心和圆弧段十二的圆心的连线；

此设计中，根据不同作业模式建立不同的路径模型，为准确跟踪曲线建立基础。

为了进一步提高获得农机运动学参数的准确性，步骤2中，利用预瞄控制器计算出理论前轮转角，具体的为，确定农机的前视距离l，取路径上的一点为预瞄点（x₀，y₀），R为前视距离对应的圆弧段的半径， l、R和x之间的关系式为：

（4-1）

将农机简化为二轮车，建立农机的运动学模型：

（4-2）

根据阿克曼转向几何关系，农机的转弯半径和前轮转角、轴距的关系式为：

（4-3）

将（4-2）和（4-3）结合起来得到理论转角的计算公式为：

（4-4）

其中，θ为农机的航向偏差角，农机后轴中心记为点A，农机后轴中心A和预瞄点P连线记为AP，航向偏差角为农机航向与AP之间的夹角，δ为农机的理论前轮转角，L为农机的轴距，v为农机的行驶速度，设定的曲线路径上距离农机中心最近的点即为M。

为了进一步提高控制精度，步骤2中，使用PI控制方法计算出补偿前轮转角，具体的包括以下步骤：

（201）根据农机的位置和预瞄点计算出农机的航向偏差角θ作为PI的误差输入e（k）；

（202）计算出当前的积分累计误差；

（203）PI控制输出补偿前轮转角，补偿前轮转角的计算公式为：

（5）

其中，K_p为比例增益，K_i为积分增益，e（i）为i时间点下对应的误差输入，k为总采样时间点数，u（k）为PI控制的输出，具体的为当前的补偿前轮转角；

将预瞄控制器输出的理论前轮转角和PI控制器输出的补偿前轮转角相加后为实际前轮转角，实际前轮转角实时输出给农机模型，控制农机的前轮转角实现农机自动避障；

此设计中，农机在实际运行中，转向关系并不是完全满足预瞄控制器中的阿克曼转向原理，会存在一定的控制误差，将PI控制方法和预瞄控制方法相结合来消除预瞄控制器带来的误差，进一步提高控制精度。

作为本发明的进一步改进，所述传感器包括位置传感器、角度传感器和机器视觉相机，所述角度传感器检测农机的转向角，所述位置传感器获得农机的位置信息；所述视觉机器相机设有2个并分别设置在农机的前后侧，视觉机器相机获取农机所处环境的地理信息。

附图说明

图1为本发明中播种时的掉头路径轨迹图。

图2为本发明中耕地时小幅宽的掉头路径轨迹图。

图3为本发明中耕地时中幅宽的掉头路径轨迹图。

图4为本发明中耕地时大幅宽的掉头路径轨迹图。

图5为本发明中的理论避障路径轨迹图。

图6为本发明中的实际避障路径轨迹图。

图7为本发明中三次Bizier曲线一特性图一。

图8为本发明中三次Bezier曲线的特性图二。

图9为本发明的控制框图。

图10为本发明中预瞄算法纯追踪模型几何解析图。

图11为本发明中预瞄算法的曲线跟踪示意图。

图12为本发明中设定曲线与跟踪曲线的仿真模拟曲线跟踪对比图。

图13为本发明中的航向偏差模拟图。

图14为本发明中的横向偏差模拟图。

图15为本发明中的前轮转向角曲线图。

具体实施方式

下面结合附图对本发明作进一步的说明。

如图1～15所示的一种用于农机无人驾驶的避障、掉头路径规划及其控制方法，步骤1：通过传感器获取农机环境信息做出掉头或避障决策；若为避障决策，规划避障路径轨迹，具体包括以下步骤，

步骤101：使用改进的最短切线法离线计算出一条理论避障路径；

步骤102：利用基于Bezier曲线的路径优化方法优化步骤2中的理论避障路径得到实际避障路径；

若为掉头决策，规划掉头路径轨迹，具体包括以下步骤，

步骤101’：输入农机的作业模式；

步骤102’：根据农机实际的作业模式选择对应的掉头路径；

步骤2：农机在前进过程中，农机通过传感器获得农机位置信息，使用育瞄控制器和PI控制器的结合计算当前的前轮转向角，控制农机的转向角使农机沿着设定的曲线行走实现农机的自动掉头或避障；

其中，传感器包括位置传感器和角度传感器，角度传感器检测农机的转向角，位置传感器获得农机的位置信息；农机的前后侧分别设有视觉机器相机，视觉机器相机获取农机所处环境的地理信息；

步骤101中，计算理论避障路径具体的为，计算农机前方障碍物的特征圆的大小、农机与障碍物的距离，根据特征圆的大小设定安全距离，根据农机的犁具宽度和农机最小转弯半径，设定一条理论避障路径；改进的最短切线法具体的为，以障碍物的中心为圆心做特征圆，特征圆的半径为r_min+w/2，理论避障路径由圆弧段一ρ1、直线段一l1、圆弧段二ρ2、直线段二l2和圆弧段三ρ3组成，圆弧段一ρ1的一端与农机原始的直线路径相切，圆弧段一ρ1的另一端与直线段一l1的一端相切，直线段一l1的另一端和直线段二l2的一端分别与圆弧段二ρ2相切，直线段二l2的另一端与圆弧段三ρ3相切，圆弧段二ρ2为特征圆上的一段，圆弧段一ρ1和圆弧段三ρ3关于圆弧段二ρ2的中心线对称设置，农机依次经过圆弧段一ρ1、直线段一l1、圆弧段二ρ2、直线段二l2和圆弧段三ρ3绕过障碍物；其中，r_min为农机的最小转弯半径，w为农机的作业宽度，障碍物的外接圆半径小于最小转弯半径。

为了进一步提高农机掉头路径的精度，圆弧段一ρ1的半径为r_min，圆弧段三ρ3的半径为r_min，圆弧段一ρ1的起点记为H点，圆弧段一ρ1的圆心记为O₁点，直线段一l1与农机原始的直线路径的相交点记为J，直线段一l1与圆弧段二ρ2的相切点记为D，农机原始路径与特征圆的相交点分别记为K和K’，JK=w/2，圆弧段二ρ2的圆心记为O点，O的坐标设为（a，b），圆弧段二ρ2的中心点记为B点，J点的坐标记为（x1，y1），JD的方程可以写成：

（1-1）；

特征圆的方程可以写成：

（1-2）

通过（1-1）和（1-2）可以求出k，D点为JD和特征圆的相交点，以此解出D点坐标；

设点O₁的坐标为（x₂,y₂），则点O₁到直线JD的距离为：

根据公式（1-3）和（1-4）求出O₁的坐标；则H点的坐标为（x₂，y₁），B点的坐标为（a，b+r）；

步骤102中，利用基于Bezier曲线的路径优化方法优化步骤2中的理论避障路径，具体的为，建立Bezier方程，

(1)给定空间n+1个点的位置矢量，则参数曲线上各点坐标的插值公式为：

（2-1）

其中构成该曲线的特征点，是n次Bernstein基函数：

（2-2）

由上述公式，可以得出三次和二次Bezier曲线的数学表达式，当n=3时，Q(t)为三次多项式，有四个控制点，其矩阵形式表达为：

（2-3）

当n=2，Q(t)为二次多项式，有三个控制点，矩阵表达式为：

（2-4）

（2）Bezier曲线的曲率表达式为：

（2-5）

其中，y=f（x）表示曲线的方程，y’为曲线的一阶导数，y”为二阶导数；

曲率半径为：

（2-6）

选取三次Bezier曲线，针对三次Bezier曲线：

（2-7）

（2-8）

其中，X0、X1、X2、X3分别为P0点、P1点、P2点和P3点处的横向坐标，Y0、Y1、Y2和Y3分别为P0点、P1点、P2点和P3处的纵向坐标；

P0点对应圆弧段一ρ1的起点H（x₂，y₁），P3点对应圆弧段二ρ2的中心点B（a，b+r），P1点对应的坐标为（（x₂+a）/2，y₁），P2点对应的坐标为（（x₂+a）/2，b+r），则实际故障路径对应的曲线的曲率半径计算公式为：

（2-9）；

用三次Bezier曲线优化理论避障路径形成的实际避障路径由两条关于圆弧段二的中心对称设置的实际避障曲线ρ0组成；

步骤101’中，农机的作业模式包括播种和耕地；步骤2中，播种情况下对应的掉头路径为多线型路径一，耕地情况下对应的掉头路径包括交叉型路径、圆弧路径和多线型路径二，且根据不同的犁具宽度选择不同的掉头路径，犁具宽度包括小宽幅、中宽幅和大宽幅，小宽幅、中宽幅和大宽幅作业模式下对应的掉头路径分别为交叉型路径、圆弧路径和多线型路径二；多线型路径一由直线段三l3、圆弧段四ρ4和圆弧段五ρ5组成，圆弧段四ρ4和圆弧段五ρ5相切且对应的半径相同，农机依次经过直线段三l3、圆弧段四ρ4和圆弧段五ρ5实现掉头；交叉型路径由圆弧段六ρ6、直线段四l4和圆弧段七ρ7组成，圆弧段六ρ6和圆弧段七ρ7相交且关于直线段l4的中心对称设置，圆弧段六ρ6和圆弧段七ρ7的半径大小相同，农机依次经过圆弧段六ρ6、直线段四12和圆弧段七ρ7实现掉头；圆弧路径由圆弧段八ρ8、圆弧段九ρ9和圆弧段十ρ10组成，圆弧段九ρ9分别和圆弧段八ρ8、圆弧段十ρ10相切，圆弧段八ρ8和圆弧段十ρ10关于圆弧段九ρ9的中心线对称设置，农机依次经过圆弧段八ρ8、圆弧段九ρ9和圆弧段十ρ10实现掉头；多线型路径二由圆弧段十一ρ11、直线段六l6和圆弧段十二ρ12组成，圆弧段十一ρ11和圆弧段十二ρ12关于直线段六l6的中心线对称设置，农机依次经过圆弧段十一ρ11、直线段六l6和圆弧段十二ρ12实现掉头；小宽幅对应的犁具宽度的数值范围为r_min<W<1.5r_min,中宽幅对应的犁具宽度的数值范围为1.5r_min<W≤2 r_min,大宽幅对应的犁具宽度的数值范围为W>2r_min，W为犁具宽度；

为了进一步提高农机掉头路径的精度，多线型路径一的各个线段的数学关系式为：

（3-1）

其中，R1为圆弧段四ρ4对应的半径，R2为圆弧段五ρ5对应的半径，AE为直线段三l3的长度，AC为农机开始转弯的后轴中心与农机转弯结束的后轴中心间连接线段的长度；

交叉型路径的各个线段的数学关系式为：

（3-2）

其中，R3为圆弧段六ρ6对应的半径，R4为圆弧段七ρ7对应的半径，C₁D₁为直线段四l4的长度，A₁B₁农机开始转弯的后轴中心与农机转弯结束的后轴中心间连接线段的长度；

圆弧路径的各个线段的数学关系式为：

（3-3）

其中，Rt为圆弧段八ρ8、圆弧段九ρ9和圆弧段十ρ10对应的半径，圆弧段八ρ8、圆弧段九ρ9和圆弧段十ρ10的圆心的连线构成三角形，β为圆弧段九ρ9的圆心分别与圆弧段八ρ8和圆弧段十ρ10的圆心连线之间的夹角，α为圆弧段八ρ8的圆心分别与圆弧段九ρ9和圆弧段十ρ10的圆心连线之间的夹角，γ为圆弧段十ρ10的圆心分别与圆弧段八ρ8和圆弧段九ρ9的圆心连线之间的夹角；

多线型路径二的各个线段的数学关系式为：

（3-4）；

其中，R5为圆弧段十一ρ11对应的半径，R6为圆弧段十二ρ12对应的半径，B₂C₂表示直线段六l6，A₂D₂为圆弧段十一ρ11的圆心和圆弧段十二ρ12的圆心的连线；

利用预瞄算法计算出理论前轮转角，具体的为，确定农机的前视距离l，取路径上的一点为预瞄点（x₀，y₀），R为前视距离对应的圆弧段的半径， l、R和x之间的关系式为：

（4-5）

由（4-5）可以得到：

（4-1）

将农机简化为二轮车，建立农机的运动学模型：

（4-2）

根据阿克曼转向几何关系，农机的转弯半径和前轮转角、轴距的关系式为：

（4-3）

将（4-2）和（4-3）结合起来得到理论转角的计算公式为：

（4-4）

其中，θ为农机的航向偏差角，农机后轴中心记为点A，农机后轴中心A和预瞄点P连线记为AP，航向偏差角为农机航向与AP之间的夹角，δ为农机的理论前轮转角，L为农机的轴距，v为农机的行驶速度，设定的曲线路径上距离农机中心最近的点即为M；

步骤2中，使用PI控制方法计算出补偿前轮转角，具体的包括以下步骤：

（201）根据农机的位置和预瞄点计算出农机的航向偏差角θ作为PI的误差输入e（k）；

（202）计算出当前的积分累计误差；

（203）PI控制输出补偿前轮转角，补偿前轮转角的计算公式为：

（5）

其中，K_p为比例增益，K_i为积分增益，e（i）为i时间点下对应的误差输入，k为总采样时间点数，u（k）为PI控制的输出，具体的为当前的补偿前轮转角；

将预瞄控制器输出的理论前轮转角和PI控制器输出的补偿前轮转角相加后为实际前轮转角，实际前轮转角实时输出给农机模型，控制农机的前轮转角使农机沿着规划好的实际避障路径行走，实现农机的自动避障。

使用matlab软件针对本发明中提出的PI控制和预瞄控制算法对设定曲线进行仿真，给定农机的起始位置为（-13,1），初始航向角度为0rad，Kp取为2，Ki取为0.01；图12～图15中的横坐标均为农机的行驶距离，从图12中可以看出，跟踪曲线与设定曲线基本重合；从图13中可以看出，航向偏差大概在0.08rad左右；从图14中可以看出，横向偏差在10cm左右，横向偏差为A点与M点之间的距离；从图15中可以看出，前轮转向角为1阶惯性环节，没有突变，和实际相符；通过以上分析，使用本发明中的控制方法进行农机的掉头或避障路径控制，控制精度高，农机基本按照设定的曲线路径行走。

本发明工作时，视觉机器相机采集农机周围的环境信息，根据周围环境信息确认农机是否进入避障，若检测到农机前方有小障碍物时，农机进入避障导航状态，农机通过传感器检测获得农机的位置信息，计算农机前方障碍物的特征圆的大小、农机与障碍物的距离，根据农机的犁具宽度和农机最小转弯半径确定特征圆的大小以设定安全距离，使用改进的最短切线法设定理论避障路径，但是由于理论避障路径的曲率不连续，使农机避障的控制精度降低，使用Bezier曲线优化方法优化理论避障路径生成新的实际避障路径，通过预瞄控制器得到理论前轮转角，PI控制器补偿预瞄控制器产生的控制误差输出补偿前轮转角，将理论前轮转向角和补偿前轮转角相加后得到实际前轮转角并将前轮转角输出给农机模型，位置传感器实时检测农机所在位置并将位置信息发送给预瞄控制器和PI控制器，通过控制农机的前轮转角使农机沿着设定的曲线行走，从而使农机自动绕开障碍物；当检测到农机前方为地头时，做出掉头决策时，农机根据实际的作业模式选择设定好的掉头路径，当作业模式为播种时农机的掉头路径选择多线型路径一，农机通过传感器检测获得农机的位置信息，根据以上所述控制方法控制农机的掉头，使农机按照设定的掉头路径曲线行走；本发明通过改进后的最短切线法计算出一条理论避障路径，使用基于Bezier曲线的路径优化方法对理论避障路径进行优化，使避障路径更加容易控制，通过预瞄控制器和PI控制器的结合控制农机的前轮转向角使农机沿着设定好的避障曲线行走，控制精度高；通过农机的实际作业模式选择相对应的掉头路径，适应范围更加广泛；可应用于无人驾驶的农机在田间导航作业时自动避开小障碍物和自动掉头的控制工作中。

本发明并不局限于上述实施例，在本发明公开的技术方案的基础上，本领域的技术人员根据所公开的技术内容，不需要创造性的劳动就可以对其中的一些技术特征作出一些替换和变形，这些替换和变形均在本发明保护范围内。

Claims (9)

1.一种用于农机无人驾驶的避障、掉头路径规划及其控制方法，其特征在于，

步骤1：通过传感器获取农机环境信息做出掉头或避障决策；若为避障决策，规划避障路径轨迹，具体包括以下步骤，

步骤101：使用改进的最短切线法离线计算出一条理论避障路径；

步骤102：利用基于Bezier曲线的路径优化方法优化步骤2中的理论避障路径得到实际避障路径；

若为掉头决策，规划掉头路径轨迹，具体包括以下步骤，

步骤101’：输入农机的作业模式；

步骤102’：根据农机实际的作业模式选择对应的掉头路径；

步骤2：农机在前进过程中，农机通过传感器获得农机位置信息，使用育瞄控制器和PI控制器的结合计算当前的前轮转向角，控制农机的转向角使农机沿着设定的曲线行走实现农机的自动掉头或避障。

2.根据权利要求1所述的用于农机无人驾驶的避障、掉头路径规划及其控制方法，其特征在于，步骤101中，计算理论避障路径具体的为，计算农机前方障碍物的特征圆的大小、农机与障碍物的距离，根据特征圆的大小设定安全距离，根据农机的犁具宽度和农机最小转弯半径，设定一条理论避障路径；改进的最短切线法具体的为，以障碍物的中心为圆心做特征圆，特征圆的半径为r_min+w/2，理论避障路径由圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三组成，圆弧段一的一端与农机原始的直线路径相切，圆弧段一的另一端与直线段一的一端相切，直线段一的另一端和直线段二的一端分别与圆弧段二相切，直线段二的另一端与圆弧段三相切，圆弧段二为特征圆上的一段，圆弧段一和圆弧段三关于圆弧段二的中心线对称设置，农机依次经过圆弧段一、直线段一、圆弧段二、直线段二和圆弧段三绕过障碍物；其中，r_min为农机的最小转弯半径，w为农机的作业宽度，障碍物的外接圆半径小于最小转弯半径。

3.根据权利要求2所述的用于农机无人驾驶的避障、掉头路径规划及其控制方法，其特征在于，所述圆弧段一的半径为r_min，所述圆弧段三的半径为r_min，圆弧段一的起点记为H点，圆弧段一的圆心记为O₁点，直线段一与农机原始的直线路径的相交点记为J，直线段一与圆弧段二的相切点记为D，农机原始路径与特征圆的相交点分别记为K和K’，JK=w/2，圆弧段二的圆心记为O点，O的坐标设为（a，b），圆弧段二的中心点记为B点，J点的坐标记为（x1，y1），JD的方程可以写成：

（1-1）；

特征圆的方程可以写成：

（1-2）

通过（1-1）和（1-2）可以求出k，D点为JD和特征圆的相交点，以此解出D点坐标；

设点O₁的坐标为（x₂,y₂），则点O₁到直线JD的距离为：

根据公式（1-3）和（1-4）求出O₁的坐标；则H点的坐标为（x₂，y₁），B点的坐标为（a，b+r）。

4.根据权利要求3所述的用于农机无人驾驶的避障、掉头路径规划及其控制方法，其特征在于，步骤102中，利用基于Bezier曲线的路径优化方法优化步骤101中的理论避障路径，优化后的实际避障路径由实际避障曲线一和实际避障曲线二组成，具体的为，建立Bezier方程，

(1)给定空间n+1个点的位置矢量，则参数曲线上各点坐标的插值公式为：

（2-1）

其中构成该曲线的特征点，是n次Bernstein基函数：

（2-2）

由上述公式，可以得出三次和二次Bezier曲线的数学表达式，当n=3时，Q(t)为三次多项式，有四个控制点，其矩阵形式表达为：

（2-3）

当n=2,Q(t)为二次多项式，有三个控制点，矩阵表达式为：

（2-4）

（2）Bezier曲线的曲率表达式为：

（2-5）

其中，y=f（x）表示曲线的方程，y’为曲线的一阶导数，y”为二阶导数；

曲率半径为：

（2-6）

选取三次Bezier曲线，针对三次Bezier曲线：

（2-7）

（2-8）

其中，X0、X1、X2、X3分别为P0点、P1点、P2点和P3点处的横向坐标，Y0、Y1、Y2和Y3分别为P0点、P1点、P2点和P3处的纵向坐标；

P0点对应圆弧段一的起点H（x₂，y₁），P3点对应圆弧段二的中心点B（a，b+r），P1点（（x₂+a）/2，y₁），P2点（（x₂+a）/2，b+r），则实际故障路径对应的曲线的曲率半径计算公式为：

（2-9）

其中，由点（x₂，y₁）、（a，b+r）、（（x₂+a）/2，y₁）和（（x₂+a）/2，b+r）生成的Bezier曲线为实际避障曲线一，实际避障曲线一和实际避障曲线二关于圆弧段二的中心对称。

5.根据权利要求1～4任一项所述的用于农机无人驾驶的避障、掉头路径规划及其控制方法，其特征在于，步骤101’中，农机的作业模式包括播种和耕地；步骤2中，播种情况下对应的掉头路径为多线型路径一，耕地情况下对应的掉头路径包括交叉型路径、圆弧路径和多线型路径二，且根据不同的犁具宽度选择不同的掉头路径，犁具宽度包括小宽幅、中宽幅和大宽幅，小宽幅、中宽幅和大宽幅作业模式下对应的掉头路径分别为交叉型路径、圆弧路径和多线型路径二；所述多线型路径一由直线段三、圆弧段四和圆弧段五组成，圆弧段四和圆弧段五相切且对应的半径相同，农机依次经过直线段三、圆弧段四和圆弧段五实现掉头；所述交叉型路径由圆弧段六、直线段四和圆弧段七组成，圆弧段六和圆弧段七相交且关于直线段四的中心对称设置，圆弧段六和圆弧段七的半径大小相同，农机依次经过圆弧段六、直线段五和圆弧段七实现掉头；所述圆弧路径由圆弧段八、圆弧段九和圆弧段十组成，所述圆弧段九分别和圆弧段八、圆弧段十相切，所述圆弧段八和圆弧段十关于圆弧段九的中心线对称设置，农机依次经过圆弧段八、圆弧段九和圆弧段十实现掉头；所述多线型路径二由圆弧段十一、直线段六和圆弧段十二四组成，圆弧段十一和圆弧段十二关于直线段六的中心线对称设置，农机依次经过圆弧段十一、直线段六和圆弧段十二实现掉头；小宽幅对应的犁具宽度的数值范围为r_min<W<1.5r_min,中宽幅对应的犁具宽度的数值范围为1.5r_min<W≤2r_min,大宽幅对应的犁具宽度的数值范围为W>2r_min，W为犁具宽度。

6.根据权利要求5所述的用于农机无人驾驶的避障、掉头路径规划及其控制方法，其特征在于，所述多线型路径一的各个线段的数学关系式为：

（3-1）

其中，R1为圆弧段四对应的半径，R2为圆弧段五对应的半径，AE为直线段三的长度，AC为农机开始转弯的后轴中心与农机转弯结束的后轴中心间连接线段的长度；

所述交叉型路径的各个线段的数学关系式为：

（3-2）

其中，R3为圆弧段六对应的半径，R4为圆弧段七对应的半径，C₁D₁为直线段四的长度，A₁B₁农机开始转弯的后轴中心与农机转弯结束的后轴中心间连接线段的长度；

所述圆弧路径的各个线段的数学关系式为：

（3-3）

其中，Rt为圆弧段八、圆弧段九和圆弧段十对应的半径，圆弧段八、圆弧段九和圆弧段十的圆心的连线构成三角形，β为圆弧段九的圆心分别与圆弧段八和圆弧段十的圆心连线之间的夹角，α为圆弧段八的圆心分别与圆弧段九和圆弧段十的圆心连线之间的夹角，γ为圆弧段十的圆心分别与圆弧段八和圆弧段九的圆心连线之间的夹角；

所述多线型路径二的各个线段的数学关系式为：

（3-4）；

其中，R5为圆弧段十一对应的半径，R6为圆弧段十二对应的半径，B₂C₂表示直线段六，A₂D₂为圆弧段十一的圆心和圆弧段十二的圆心的连线。

7.根据权利要求6所述的用于农机无人驾驶的避障、掉头路径规划及其控制方法，其特征在于，步骤2中，利用预瞄控制器计算出理论前轮转角，具体的为，确定农机的前视距离l，取路径上的一点为预瞄点（x₀，y₀），R为前视距离对应的圆弧段的半径，l、R和x之间的关系式为：

（4-1）

将农机简化为二轮车，建立农机的运动学模型：

（4-2）

根据阿克曼转向几何关系，农机的转弯半径和前轮转角、轴距的关系式为：

（4-3）

将（4-2）和（4-3）结合起来得到理论转角的计算公式为：

（4-4）

其中，θ为农机的航向偏差角，农机后轴中心记为点A，农机后轴中心A和预瞄点P连线记为AP，航向偏差角为农机航向与AP之间的夹角，δ为农机的理论前轮转角，L为农机的轴距，v为农机的行驶速度，设定的曲线路径上距离农机中心最近的点即为M。

8.根据权利要求7所述的用于农机无人驾驶的避障、掉头路径规划及其控制方法，其特征在于，步骤2中，使用PI控制方法计算出补偿前轮转角，具体的包括以下步骤：

（201）根据农机的位置和预瞄点计算出农机的航向偏差角θ作为PI的误差输入e（k）；

（202）计算出当前的积分累计误差；

（203）PI控制输出补偿前轮转角，补偿前轮转角的计算公式为：

（5）

其中，K_p为比例增益，K_i为积分增益，e（i）为i时间点下对应的误差输入，k为总采样时间点数，u（k）为PI控制的输出，具体的为当前的补偿前轮转角；

将预瞄控制器输出的理论前轮转角和PI控制器输出的补偿前轮转角相加后为实际前轮转角，实际前轮转角实时输出给农机模型，控制农机的前轮转角实现农机自动避障。

9.根据权利要求1～4任一项或6～8任一项所述的用于农机无人驾驶的避障、掉头路径规划及其控制方法，其特征在于,所述传感器包括位置传感器和角度传感器，所述角度传感器检测农机的转向角，所述位置传感器获得农机的位置信息；农机的前后侧分别设有视觉机器相机，所述视觉机器相机获取农机所处环境的地理信息。