用于农机无人驾驶的自动掉头路径规划及其控制方法与流程

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

背景技术：

农机在导航作业时一般需要进行自动掉头对准下一作业行的跟踪，控制农机准确的掉头技术是实现精准农业的关键，该技术可提高农机的作业精度和作业效率，使驾驶员摆脱长时间劳累的重复驾驶工作，降低劳动力。

在现有的农机路径跟踪方法上，主要有基于模型的控制方法和与模型无关的控制方法。在模型的控制方法上，主要是基于运动学模型和动力学模型的路径跟踪方法。基于运动学模型的控制方法是对模型进行小角度的线性化逼近，在常速假设条件下进行控制器设计这样不但引入了线性化误差，而且速度变化时控制器的鲁棒性较差；基于动力学模型的控制方法虽然模型的精度高，但是动力学模型的参数很难实时获取。在与模型无关的控制方法上面，纯追踪方法的前视距离的在线自适应确定问题还没有很好的解决，技术不成熟，控制精度低；智能方法虽然有传统的控制方法无法比拟的仿人智能和非线性映射能力，但是设计需要一定的经验知识和复杂的学习训练过程，总之，现有的路径轨迹的控制方法不仅无法同时实现高的控制精度和实时获取农机的运动参数，而且对设计人员的要求很高，适应性差。

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

技术实现要素：

针对现有技术中的缺陷，本发明的目的在于克服上述现有技术中的不足之处，解决现有技术中控制精度低且无法实时获取农机的运动学模型参数的技术问题，提供一种用于农机无人驾驶的自动掉头路径规划及其控制方法，本发明可实时获取农机的运动学参数，控制精度高，适用范围广。

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

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

步骤2：获取农机所处环境的地理信息，根据农机实际的作业模式选择对应的掉头路径；

步骤3：农机在前进过程中，农机通过传感器获得农机位置信息，使用曲线跟踪方法获得实时曲线曲率、航向偏差和横向偏差，用状态反馈控制器和自适应控制器的结合计算当前的前轮转向角，控制农机的转向角实现农机的自动掉头。

本发明工作时，输入农机的作业模式，农机感应周围的环境信息，农机掉头时，农机根据实际的作业模式选择设定好的掉头路径，农机通过传感器检测获得农机的位置信息，使用曲线跟踪方法实时获得设定好路径的曲线曲率、农机航向偏差和横向偏差，用状态反馈控制器和自适应控制器的结合计算出当前的前轮转向角，通过控制农机的前轮转向角使农机沿着设定的曲线行走，从而实现农机的掉头。

与现有技术相比，本发明的有益效果在于：

(1)本发明使用曲线跟踪方法实时获得设定好的路径曲线曲率、航向偏差和横向偏差，使用状态反馈控制方法解算出当前的前轮转向角，控制农机的前轮转向角使农机沿着设定好的曲线行走，控制精度高；

(2)本发明通过农机的实际作业模式选择相对应的掉头路径，适应范围更加广泛；

本发明可应用于控制农机无人驾驶的自动掉头的工作中。

为了进一步提高农机掉头速度，本发明的步骤1中，农机的作业模式包括播种和耕地；步骤2中，播种情况下对应的掉头路径为多线型路径一，耕地情况下对应的掉头路径包括交叉型路径、圆弧路径和多线型路径二，且根据不同的犁具宽度选择不同的掉头路径，犁具宽度包括小宽幅、中宽幅和大宽幅，小宽幅、中宽幅和大宽幅作业模式下对应的掉头路径分别为交叉型路径、圆弧路径和多线型路径二，小宽幅对应的犁具宽度的数值范围为rmin<w<1.5rmin,中宽幅对应的犁具宽度的数值范围为1.5rmin<w≤2rmin,大宽幅对应的犁具宽度的数值范围为w>2rmin，rmin为农机的最小转弯半径，w为犁具宽度；此设计中，将农机的作业模式分为播种和耕地，不同作业模式，对应的掉头路径不同，优化掉头曲线，缩短农机掉头时间。

为了进一步提高农机掉头效率，所述多线型路径一由直线段一、圆弧段一和圆弧段二组成，圆弧段一和圆弧段二相切且对应的半径相同，农机依次经过直线段一、圆弧段一和圆弧段二实现掉头；所述交叉型路径由圆弧段三、直线段二和圆弧段四组成，圆弧段三和圆弧段四相交且对应的半径大小相同，农机依次经过圆弧段三、直线段二和圆弧段四实现掉头；所述圆弧路径由圆弧段五、圆弧段六和圆弧段七组成，所述圆弧段六分别和圆弧段五、圆弧段七相切，所述圆弧段五和圆弧段七关于圆弧段六的中心线对称设置，农机依次经过圆弧段五、圆弧段六和圆弧段七实现掉头；所述多线型路径二由圆弧段八、直线段三和圆弧段九组成，圆弧段八和圆弧段九关于直线段三的中心线对称设置。

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

r1＝r2＝rmin,ac＝w,r1+r1cosα＝m+r1-r1cosα,ae＝2r1sinα(1)

其中，r1为圆弧段一对应的半径，r2为圆弧段二对应的半径，ae为直线段一的长度，ac为农机开始转弯的后轴中心与农机转弯结束的后轴中心间连接线段的长度；

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

r3＝r4＝rmin,a1b1＝w,c1d1＝2rmin-w(2)

其中，r3为圆弧段三对应的半径，r4为圆弧段四对应的半径，c1d1为直线段二的长度，a1b1农机开始转弯的后轴中心与农机转弯结束的后轴中心间连接线段的长度；

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

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

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

r5＝r6＝rmin,a2d2＝w,b2c2＝w-2rmin(4)

其中，r5为圆弧段八对应的半径，r6为圆弧段九对应的半径，b2c2为直线段三的长度，a2d2为圆弧段八的圆心和圆弧段九的圆心的连线；

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

为了进一步提高获得农机运动学参数的准确性，步骤3中，将农机简化成二轮车模型进行运动学分析，并利用曲线跟踪方法建立农机运动学模型，如下式所示：

其中，s表示m点沿着弧长运动的距离，m点是曲线路径上距离农机后轴中心最近的点；y表示农机和m点的横向偏差，θ是农机航向偏差角，为转向角加速度；规定当点沿着曲线顺时针运动，曲率c为负，沿着曲线逆时针运动，曲率c为正；当农机的后轴中心点在曲线的外侧，横向偏差y为正，当农机的后轴中心点在曲线的内侧，横向偏差y为负；

先使用链式控制理论将农机的非线性模型转化成近似线性模型，再使用状态反馈控制方法计算出控制率，当农机沿着曲线逆时针运动，控制率如公式(6)所示：

当农机沿着曲线顺时针运动，控制率如公式(7)所示：

其中，(kd,kp)∈r⁺²，δ(y,θ)为关于横向偏差和农机航向偏差角变量的理论前轮转向角函数；此设计中，先利用曲线跟踪方法建立农机运动模型，再使用链式控制理论将非线性的农机模型转化为近似线性模型，可实时获得运动学参数，通过控制前轮转向角达到控制农机实际行走路径的目的，误差小，控制精度高。

为了进一步提高曲线跟踪的控制精度，步骤3中，使用链式控制理论计算出理论前轮转向角δ(y,θ)后，利用自适应控制器解算出期望转向补偿角δc，将理论前轮转向角δ(y,θ)和期望转向补偿角δc相加后得到农机的实际转向角δe并输出给农机模型,控制农机的实际转向角δe使农机沿着设定的曲线行走；此设计中，考虑到农机的模型精确度将对农机的控制品质有较大的影响，在公式(5)中，我们认为c(x)vsinθdt近似为0，并且ds近似为dx，当跟踪曲线的曲率较小时，假设条件基本可以满足，随着曲率的增大，农机模型的精确度将会下降，控制性能会降低，加入期望转向补偿角δc后辅助控制，进一步提高农机的控制精度。

作为本发明的进一步改进，所述自适应控制器有2个输入变量，分别为横向偏差变量y和横向偏差微分dy，自适应控制器的输出是农机的期望转向补偿角δc，其中变量dy作为农机横向偏差的趋势，dy的计算公式如公式(8)所示：

其中，yt为t时刻农机的横向偏差，yt-δt是t-δt时刻的横向偏差，这里选取参数△t为1s；

将输入输出变量进行模糊化

(1)横向偏差y

基本论域：[-60,60]，量化等级：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，量化因子为ky＝6/60＝0.1；

(2)横向偏差微分dy

基本论域：[-6,6]，量化等级：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，量化因子为ky＝6/6＝1；

(3)补偿角δc

基本论域:[-8,8]，量化等级：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，量化因子ky＝6/8＝0.75；

横向偏差y、横向偏差微分dy和补偿角δc3个变量的模糊等级均为：负大(nb)、负中(nm)、负小(ns)、零(zo)、正小(ps)、正中(pm)、正大(pb)，三个隶属度函数均采用高斯函数。

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

附图说明

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

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

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

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

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

图6为本发明中农机与曲线路径间的运动关系图。

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

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

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

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

具体实施方式

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

如图1～10所示的一种用于农机无人驾驶的自动掉头路径规划及其控制方法，具体包括以下步骤：

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

步骤2：获取农机所处环境的地理信息，根据农机实际的作业模式选择对应的掉头路径；

步骤3：农机在前进过程中，农机通过传感器获得农机位置信息，使用曲线跟踪方法获得实时曲线曲率、航向偏差和横向偏差，用状态反馈控制器和自适应控制器的结合计算当前的前轮转向角，控制农机的转向角实现农机的自动掉头。

本发明的步骤1中，农机的作业模式包括播种和耕地；另外，步骤1中的传感器包括位置传感器和角度传感器，角度传感器检测农机的转向角，位置传感器获得农机的位置信息；农机的前后侧分别设有视觉机器相机，视觉机器相机获取农机所处环境的地理信息。

本发明的步骤2中，播种情况下对应的掉头路径为多线型路径一(如图1所示)，多线型路径一由直线段一l1、圆弧段一ρ1和圆弧段二ρ2组成，圆弧段一ρ1和圆弧段二ρ2相切且对应的半径相同，农机依次经过直线段一l1、圆弧段一ρ1和圆弧段二ρ2实现掉头；耕地情况下，根据不同的犁具宽度设置不同的掉头路径，犁具宽度包括小宽幅、中宽幅和大宽幅，小宽幅对应的犁具宽度的数值范围为rmin<w<1.5rmin,中宽幅对应的犁具宽度的数值范围为1.5rmin<w≤2rmin,大宽幅对应的犁具宽度的数值范围为w>2rmin，rmin为农机的最小转弯半径，w为犁具宽度；小宽幅、中宽幅和大宽幅下对应的掉头路径分别为交叉型路径、圆弧路径和多线型路径二，交叉型路径(如图2所示)由圆弧段三ρ3、直线段二l2和圆弧段四ρ4组成，圆弧段三ρ3和圆弧段四ρ4相交且对应的半径大小相同，农机依次经过圆弧段三ρ3、直线段二l2和圆弧段四ρ4实现掉头；圆弧路径(如图3所示)由圆弧段五ρ5、圆弧段六ρ6和圆弧段七ρ7组成，圆弧段六ρ6分别和圆弧段五ρ5、圆弧段七ρ7相切，圆弧段五ρ5和圆弧段七ρ7关于圆弧段六ρ6的中心线对称设置，农机依次经过圆弧段五ρ5、圆弧段六ρ6和圆弧段七ρ7实现掉头；多线型路径二(如图4所示)由圆弧段八ρ8、直线段三l3和圆弧段九ρ9组成，圆弧段八ρ8和圆弧段九ρ9关于直线段三l3的中心线对称设置，农机依次经过圆弧段八ρ8、直线段三l3和圆弧段九ρ9实现掉头；

其中，多线型路径一的各个线段的数学关系式为：

r1＝r2＝rmin,ac＝w,r1+r1cosα＝m+r1-r1cosα,ae＝2r1sinα(1)

其中，r1为圆弧段一ρ1对应的半径，r2为圆弧段二ρ2对应的半径，ae为直线段一l1的长度，ac为农机开始转弯的后轴中心与农机转弯结束的后轴中心间连接线段的长度；

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

r3＝r4＝rmin,a1b1＝w,c1d1＝2rmin-w(2)

其中，r3为圆弧段三ρ3对应的半径，r4为圆弧段四ρ4对应的半径，c1d1为直线段二l2的长度，a1b1农机开始转弯的后轴中心与农机转弯结束的后轴中心间连接线段的长度；

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

其中，rt为圆弧段五ρ5、圆弧段六ρ6和圆弧段七ρ7对应的半径，圆弧段五ρ5、圆弧段六ρ6和圆弧段七ρ7的圆心的连线构成三角形，β为圆弧段六ρ6的圆心分别与圆弧段五ρ5和圆弧段七ρ7的圆心连线之间的夹角，α为圆弧段五ρ5的圆心分别与圆弧段六ρ6和圆弧段七ρ7的圆心连线之间的夹角，γ为圆弧段七ρ7的圆心分别与圆弧段五ρ5和圆弧段六ρ6的圆心连线之间的夹角；

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

r5＝r6＝rmin,a2d2＝w,b2c2＝w-2rmin(4)

其中，r5为圆弧段八ρ8对应的半径，r6为圆弧段九ρ9对应的半径，b2c2为直线段三l3的长度，a2d2为圆弧段八ρ8的圆心和圆弧段九ρ9的圆心的连线；

步骤3中，在不考虑车轮与地面的相互作用，无侧倾俯仰、侧滑等运动的情况下，将农机简化成二轮车模型进行运动学分析，并利用曲线跟踪方法建立农机运动学模型(如图6所示)，根据图6，结合几何知识及物理规律，推导出农机运动学模型的微分方程组，农机模型如式(5)所示：

其中，s表示m点沿着弧长运动的距离，m点是曲线路径上距离农机后轴中心最近的点；y表示农机和m点的横向偏差，θ是农机航向偏差角，为转向角加速度；规定当点沿着曲线顺时针运动，曲率c为负，沿着曲线逆时针运动，曲率c为正；当农机的后轴中心点在曲线的外侧，横向偏差y为正，当农机的后轴中心点在曲线的内侧，横向偏差y为负；

由(5)可知农机的模型是高度的非线性系统，为了应用线性系统控制方法，需要将(5)进行近似线性化。我们可以使用链式控制理论将农机模型转化成近似线性模型，这个方法可以使我们使用线性控制方法应用在非线性系统；

将农机的非线性模型转化成通用的链式系统，通用的三阶链式系统的方程如下所示：

其中，a＝(a1a2a3)^t是系统的状态变量，m＝(m1m2)^t是系统的控制变量。为了使方程(9)线性化，需要将状态变量a1求导，记

则方程(9)可以写成如下形式：

系统(9)中的后两个方程(10)和(11)很明显是线性系统，则我们也能推导出n维的链式系统中有n-1个线性子系统；

接着将农机模型(5)转化成链式系统的形式，设定a1＝s，同时状态变量a2和a3设定为y和θ相关的变量，我们可以选择最简单的形式，设定a2＝y，

新的控制变量m1将写成如下的形式：

同时，设定

将农机模型(5)带入公式(12)、(13)，可以将农机模型(5)转化成(9)的形式，推导过程如下所示：

其中，必须满足以下条件：

因为模型(13)是线性系统，我们可以使用状态反馈控制方法对系统进行控制，状态反馈控制器的通用表达式如下所示：

m3＝-kda3-kpa2(kd,kp)∈r⁺²(16)

将方程(16)带入方程(13)中，我们可以得到如下的控制律：

a2”+kda2'+kpa2＝0(17)

该控制律是为了控制a2和a3趋近于0，同样地，该结论可以用来控制y和θ趋近于0来达到曲线跟踪控制的目的。

将方程(16)带入方程(14)，可以得到实际的控制表达式：

使用链式控制理论将农机的非线性模型转化成近似线性模型，当农机沿着曲线逆时针运动，控制率如公式(6)所示：

当农机沿着曲线顺时针运动，控制率如公式(7)所示：

其中，δ(y,θ)为关于横向偏差和农机航向偏差角变量的理论前轮转向角。

考虑到农机的模型精确度将对农机的控制品质有较大的影响，在公式(5)中，我们认为c(x)vsinθdt近似为0，并且ds近似为dx，当跟踪曲线的曲率较小时，假设条件基本可以满足，随着曲率的增大，农机模型的精确度将会下降，控制性能会降低，加入期望转向补偿角δc后辅助控制，具体的为，步骤3中，使用链式控制理论计算出理论前轮转向角δ(y,θ)后，利用自适应控制器解算出期望转向补偿角δc，将理论前轮转向角δ(y,θ)和期望转向补偿角δc相加后得到农机的实际转向角δe并输出给农机模型,控制农机的实际转向角δe使农机沿着设定的曲线行走；

自适应控制器有2个输入变量，分别为横向偏差变量y和横向偏差微分dy，自适应控制器的输出是农机的期望转向补偿角δc，其中变量dy作为农机横向偏差的趋势，dy的计算公式如公式(8)所示：

其中，yt为t时刻农机的横向偏差，yt-δt是t-δt时刻的横向偏差，这里选取参数△t为1s；

将输入输出变量进行模糊化，具体的为：

(1)横向偏差y

基本论域：[-60,60]，量化等级：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，量化因子为ky＝6/60＝0.1；

(2)横向偏差微分dy

基本论域：[-6,6]，量化等级：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，量化因子为ky＝6/6＝1；

(3)补偿角δc

基本论域:[-8,8]，量化等级：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}，量化因子ky＝6/8＝0.75；

横向偏差y、横向偏差微分dy和补偿角δc3个变量的模糊等级均为：负大(nb)、负中(nm)、负小(ns)、零(zo)、正小(ps)、正中(pm)、正大(pb)，三个隶属度函数均采用高斯函数，模糊控制规则如表1所示。

例如，当车体在曲线路径的外侧，横向偏差为正的最大且横向偏差有增大的趋势时，自适应控制器输出负的最大期望转向补偿角δc，使农机的横向偏差减小，用模糊规则描述为：ify＝pbanddy＝pb,thenδc＝nb；其它的模糊规则见表1，总共有49条控制规则，车体在其它情况下与以上例子类似分析即可，这里对车体的任意情况不再一一说明。

使用matlab软件对本发明进行仿真，设定由直线和圆弧构成的曲线，给定农机的起始位置为[0,0]，初始角度为0rad，选取kd＝0.6，kp＝0.09，利用本发明的控制方法对设定好的曲线进行跟踪，图7～图10的横坐标均为农机的行驶距离，从图7中可以看出，跟踪曲线与设定曲线基本重合；从图8中可以看出，横向偏差维持在10cm左右；从图9中可以看出，航向偏差大概在0.02rad左右，其中波动比较大的点主要是此处的曲线斜率较大，采样点比较稀疏，导致m点不是很精确，但是在实际运行过程中，采样点比较密集，会解决m点不精确的问题；从图10中可以看出，前轮转向角为1阶惯性环节，没有突变，和实际相符；通过以上分析，使用本发明中的控制方法进行农机的掉头路径控制，控制精度高，农机基本按照设定的曲线路径行走。

本发明工作时，输入农机的作业模式，视觉机器相机采集农机周围的环境信息，根据周围环境信息确认农机是否掉头；农机掉头时，农机根据实际的作业模式选择设定好的掉头路径，当作业模式为播种时农机的掉头路径选择多线型路径一，农机通过位置传感器检测获得农机的位置信息，角度传感器实时检测前轮的转向角并实时反馈给农机的导航系统，以便于更加准确地控制农机的前轮转角，使用曲线跟踪方法实时获得设定好路径的曲线曲率、农机航向偏差和横向偏差，使用链式控制理论将原本的农机模型转化为线性模型，用状态反馈控制方法对链式系统进行控制，通过状态反馈控制器得到理论前轮转向角，通过自适应控制器计算出期望补偿转向角，将理论前轮转向角和期望补偿转向角相加后得到实际前轮转向角并将实际前轮转向角输出给农机模型，位置传感器实时检测农机所在位置并将位置信息发送给状态反馈控制器和自适应控制器，通过控制农机的前轮转向角使农机沿着设定的曲线行走，从而实现农机的掉头；当作业模式为耕地时，根据农机上悬挂的犁具宽度选择掉头路径，掉头路径轨迹选定后，按照上述相同的控制过程控制农机的掉头；本发明使用曲线跟踪方法实时获得设定好的路径曲线曲率、航向偏差和横向偏差，使用状态反馈控制方法解算出理论前轮转向角，为了进一步提高曲线跟踪的控制精度，加入期望转向补偿角后辅助控制，使农机沿着设定好的曲线行走，控制精度高；同时，本发明通过农机的实际作业模式选择相对应的掉头路径，适应范围更加广泛；可应用于控制农机无人驾驶的自动掉头的工作中。

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