CN105867377B - 一种农业机械自动导航控制方法 - Google Patents
一种农业机械自动导航控制方法 Download PDFInfo
- Publication number
- CN105867377B CN105867377B CN201610228933.5A CN201610228933A CN105867377B CN 105867377 B CN105867377 B CN 105867377B CN 201610228933 A CN201610228933 A CN 201610228933A CN 105867377 B CN105867377 B CN 105867377B
- Authority
- CN
- China
- Prior art keywords
- point
- front wheel
- wheel angle
- deviation
- control
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 71
- 230000007246 mechanism Effects 0.000 title claims abstract description 14
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 41
- 230000033001 locomotion Effects 0.000 claims abstract description 13
- 238000013139 quantization Methods 0.000 claims description 7
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 239000002131 composite material Substances 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 3
- 239000000203 mixture Substances 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 2
- 230000008901 benefit Effects 0.000 abstract description 2
- 230000000295 complement effect Effects 0.000 abstract description 2
- 238000012360 testing method Methods 0.000 description 12
- 241000209094 Oryza Species 0.000 description 9
- 235000007164 Oryza sativa Nutrition 0.000 description 9
- 235000009566 rice Nutrition 0.000 description 9
- 230000008859 change Effects 0.000 description 5
- 238000013461 design Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000008676 import Effects 0.000 description 2
- 150000001875 compounds Chemical class 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 230000003472 neutralizing effect Effects 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 238000005192 partition Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 239000002689 soil Substances 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Guiding Agricultural Machines (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种农业机械自动导航控制方法,该方法包括如下步骤:(1)GPS接收机实时测得当前时刻农业机械的运动状态数据;(2)采用预瞄控制方法,将运动状态数据与预定义路径数据进行比对分析,推算出横向偏差和航向偏差;(3)通过模糊控制算法进行模糊化,然后将模糊量导入模糊控制器中,得到前轮转角;通过改进型纯追踪算法建立几何模型,从而导求出前轮转角;将两种算法得到的前轮转角合成一个前轮转角;(4)最后根据前轮转角实时对农业机械进行控制,实现路径跟踪。本发明由两种导航方法结合在一起实现自动控制,是稳定性良好,抗干扰能力强,能够提高***的鲁棒性。两种方法的结合,优势互补,使其满足旱地和水田的作业需要。
Description
技术领域
本发明涉及农业机械导航控制领域,特别涉及一种农业机械自动导航控制算法。
背景技术
农业机械导航控制的主要任务是根据各个传感器得到农业机械的导航位置,确定该位置与预定义路径的位置关系,综合农业机械的运动状态以及合适的控制算法,决策出相应的前轮转角,以修正路径跟踪误差。
导航控制方法常用的有线性模型控制方法、模糊控制方法和最优控制方法。日本东京大学利用机器视觉技术,根据线性转向控制模型,将目标方向角和农业机械纵向角融合,计算出前轮转角,实现了转弯控制。随后东京大学又采用模糊控制技术来进行导航研究,模糊控制器输入的是方向偏差和位置偏差,输出的是前轮的转向角度和左右执行的时间,从而完成导航控制。O’Connor等人采用方向偏差、方向偏差变化率、转向角度、转向角度变化率和跟踪位置误差这五个状态变量建立农业机械运动学方程,以角度变化率最小为优化目标,利用农业机械线性运动学方程组建立了基于LQR的最优控制器,从而实现了导航控制。Benson等人应用PID控制方法设计了PID控制器,该算法不依赖精确的数学模型,避免了繁琐的建模过程,只需要一些对象的响应特征来组合控制,对算法的比例参数、积分参数和微分参数进行合理的调节,即可实现良好的路径跟踪效果。专利ZL200710029018.4中应用模糊控制算法和PID算法设计控制器,当横向偏差较小时,应用PID控制;当横向偏差较大时,采用模糊控制。该方法在一定范围内保证了稳态跟踪精度和控制的稳定性,但由于在实际导航中的每一时刻,只采用了其中一种控制算法,因此很难发挥两种控制算法的互补优势。
通过以上分析可知,导航控制的难点和重点是提高路径跟踪的精度和转向控制的稳定性。常规的线性模型控制方法,包括PID控制方法,能够获得较高精度的路径跟踪效果,但是抗干扰能力较弱;模糊控制算法具有良好的稳定性,但是路径跟踪的稳态精度难以保证;最优控制器的建立要依赖精确的农业机械运动学和动力学模型,但是在田间作业条件下,土壤特性的变化、农业机械与地面相互作用的变化,导致农业机械运动学和动力学模型的建立比较困难,
发明内容
本发明的目的在于改进和完善导航控制技术中的不足,提供一种路径跟踪精度相对较高、稳定性较好的导航控制方法。该方法基于预瞄跟随理论,采用改进型纯追踪算法和模糊控制相结合的方法来设计控制器,建立一种稳定性好、精度高的路径跟踪算法。
为了达到上述目的,本发明所采用的技术方案如下:一种农业机械自动导航控制方法,该方法包括如下步骤:
(1)通过GPS接收机实时测得当前时刻农业机械的运动状态数据;
(2)采用预瞄控制方法,动态搜索目标点,将步骤1得到运动状态数据与预定义路径数据进行比对分析,推算出横向偏差和航向偏差;
(3)通过模糊控制算法将步骤2得到的横向偏差和航向偏差模糊化,然后将模糊量导入模糊控制器中,得到前轮转角;通过改进型纯追踪算法建立几何模型,从而由步骤2中得到的航向偏差推导求出前轮转角;将模糊控制算法和改进型纯追踪算法得到的前轮转角合成一个最终的前轮转角;
(4)最后根据前轮转角实时对农业机械进行控制,实现路径跟踪。
进一步的,所述当前时刻农业机械的运动状态数据包括当前位置Pr(xr,yr)、当前速度V和当前航向角度q;所述预定义路径数据为n+1个位置点的集合,即 {P0(x0,y0),P1(x1,y1),...,Pk(xk,yk),Pk+1(xk+1,yk+1),...,Pn(xn,yn)}。
进一步的,所述预瞄控制方法,即动态搜索目标点,是通过动态计算前视距离,进而确定预定义路径上的预瞄点:
(1)前视距离公式由下述公式计算得出:
Lf=L0+kvV+(Lb-kwW)
其中,Lf为前视距离;L0为前视距离基值;Lb为弯度的最大量化值;kv和kw分别是速度和弯度的比例系数,这两个系数可在仿真或试验中通过整定获得;W为弯度,弯度的计算方法是:首先通过快速查找算法确定预定义路径上与当前农业机械位置最近的点Pk(xk,yk),由该点向后每隔i个点再取3个点,i为正整数,由此可得到4个点,分别是Pk、Pk+i、Pk+2i和Pk+3i,用直线连接这四个点,取直线PkPk+i和直线Pk+2iPk+3i所夹的较小的角为弯度值,取值为0~180:
(2)预瞄点Pm的确定:在农业机械当前纵向方向上,由当前位置点Pr向前截取前视距离Lf,得到P′m点,再通过快速查找算法得到预定义路径上与P′m点最近的点,该点即是预瞄点Pm。
进一步的,所述的横向偏差和航向偏差的求解过程如下:
(1)横向偏差d的确定:
首先通过快速查找算法得到预定义路径上与当前农业机械位置最近的点Pk(xk,yk),然后将农业机械当前位置点Pr向直线Pk(xk,yk)Pk+1(xk+1,yk+1)投影,得到点P′r,则横向偏差d=|PrP′r|;
(2)航向偏差qe的确定:
航向偏差规定为农业机械的目标方向与当前农业机械的航向之间的角度之差:农业机械的当前位置点Pr与预瞄点Pm构成的向量为设该向量沿逆时针方向旋转与y轴正向的夹角为那么航向偏差
进一步的,所述的模糊控制方法具体为:
(1)将步骤2得到的横向偏差和航向偏差模糊化,即采用基本连续论域,离散化采用均匀量化的方法,将连续论域中的值经量化因子变换后取整,具体为:
q′e=int(kqqe)
d'=int(kdd)
其中q′e、d'均为连续数据取整后的离散数据;输入量化因子kq的确定方法是:根据仿真和试验测试结果,确定航向偏差的误差范围,由离散论域最大值除以相应误差范围最大值即可获得量化因子,获得之后再通过试验进行修正即可;
横向偏差的连续论域离散化具体为:将横向偏差分为四个等级,当测得|d|∈[0,0.1]时,则kd=a1,当得|d|∈(0.1,0.3]时,则kd=a2,当|d|∈(0.3,05]时,则kd=a3,当d为其它值时,则kd=a4,其中a1、a2、a3和a4分别为横向偏差在四个等级下对应的取值;
(2)使用模糊控制表查询,打开规则观测器,输入不同的离散量d',q′e,得到对应的输出离散量
(3)输出去模糊控制,即模糊控制算法得出的前轮转角从模糊控制量到实际输出控制量的转换公式如下:
其中,输出的比例因子的确定方法同输入量化因子。
进一步的,所述的改进型纯追踪算法为:
利用航向偏差建立几何模型,求解农业机械到达目标点所需要的转弯半径,然后由转弯半径再求解出前轮转角,同时把横向偏差作为观测量,如果横向偏差超过设定值,那么给前轮转角一个补偿量以增大前轮转角,使农业机械快速转弯到达目标点;
Pr为当前农业机械的位置,Pk+j为目标点,R为转弯半径,C为圆心点,qe为 航向偏差,L为农业机械前后轮轴间距,为改进型纯追踪算法求得的前轮转角,那么在直角三角形BPrC中可以得出:
由于qe为圆C的弦切角,因此可得:
-PrCPk+j=2qe
在等腰三角形PrCPk+j中,由正弦定理可得:
由上述三式消去R可得到前轮转角:
式中PrPk+j为农业机械当前位置与目标点的之间的距离;另外,将横向偏差作为控制器的一个观测量,再设定一个预设值dr,当观测到横向偏差d≠dr时,则将改进纯追型踪算法求得的前轮转角乘以一个比例系数kdr,由此可以增大前轮转角,使其快速到达目标点;其中dr和kdr的大小通过仿真或试验整定得出。
进一步的,所述步骤3具体为:通过改进型纯追踪算法和模糊控制算法分别得出前轮转角,然后将两个转角合成一个最终的转角,实现两种方法复合控制,其中合成公式如下:
其中k1、k2分别为模糊控制器输出的前轮转角和改进型纯追踪控制器输出的前轮转角的折算因子,两者之和为1,两者取值由仿真和试验整定得出。
进一步的,所述快速查找算法是计算预定义路径的每个点与当前农业机械坐标点的距离,距离最小的那个点即为最近点。
本发明与现有技术相比有如下特点:
(1)本发明对传统纯追踪算法做了改进,使其不仅适应直线行驶还能满足曲线转弯,同时结合模糊控制算法,在实际导航中充分发挥了上述两种导航算 法各自的优势并达到互补的效果,因此极大地提高了控制的精度和稳定性。
(2)本发明在导航农业机械沿预定义路径行驶时,直线段行驶的跟踪误差小于5cm,曲线转弯段行驶的跟踪误差小于20cm。
(3)本发明适合于农业机械自动化装备,能够满足旱地的作业需要,同时也可满足水田作业要求。
附图说明
图1为弯度求解图;
图2为横向偏差求解图;
图3为横向偏差正负号求解图;
图4为航向偏差求解图;
图5为模糊变量的隶属函数图;
图6为模糊控制输出的三维外观图;
图7为改进型纯追踪算法原理图;
图8为导航控制原理框图。
具体实施方式
为使本发明更明显易懂,兹以一实施例作进一步详细描述,但不用来限制本发明的范围。
一种农业机械自动导航控制方法,该方法包括如下步骤:
(1)通过GPS接收机实时测得当前时刻农业机械的运动状态数据;
(2)采用预瞄控制方法,动态搜索目标点,将步骤1得到运动状态数据与预定义路径数据进行比对分析,推算出横向偏差和航向偏差;
(3)通过模糊控制算法将步骤2得到的横向偏差和航向偏差模糊化,然后将模糊量导入模糊控制器中,得到前轮转角;通过改进型纯追踪算法建立几何模 型,从而由步骤2中得到的航向偏差推导求出前轮转角;将模糊控制算法和改进型纯追踪算法得到的前轮转角合成一个最终的前轮转角;
(4)最后根据前轮转角实时对农业机械进行控制,实现路径跟踪。
进一步的,所述当前时刻农业机械的运动状态数据包括当前位置Pr(xr,yr)、当前速度V和当前航向角度q,如图1所示;具体定义如下:
(1)当前定位点Pr(xr,yr),由GPS接收机获得当前的差分定位数据;
(2)当前速度V,由速度传感器测得当前农业机械纵向方向的前进速度;
(3)当前航向角度q,由电子罗盘获得当前农业机械纵向方向的角度,在高斯投影平面坐标系中,表示为与y轴顺时针旋转的夹角,范围为0~360。
所述的预定义路径是由人工驾驶车辆,GPS接收机记录的路径数据,是一个点数据,其数学表达式为:
{P0(x0,y0),P1(x1,y1),...,Pk-1(xk-1,yk-1),Pk(xk,yk)}
需要说明的是,预定义路径由点数据集表示,Pk为第k个点,(xk,yk)为第k个点的坐标,所有点的坐标均在WGS-84大地坐标系中,表示为高斯投影平面坐标。
所述预瞄控制方法,即动态搜索目标点,是通过动态计算前视距离,进而确定预定义路径上的预瞄点:
(1)前视距离公式由下述公式计算得出:
Lf=L0+kvV+(Lb-kwW)
其中,Lf为前视距离;L0为前视距离基值;Lb为弯度的最大量化值;kv和kw分别是速度和弯度的比例系数,这两个系数可在仿真或试验中通过整定获得;W为弯度,如图1所示,弯度的计算方法是:首先通过快速查找算法确定预定义路径上与当前农业机械位置最近的点Pk(xk,yk),由该点向后每隔i个点再取3个点,i为正整数,由此可得到4个点,分别是Pk、Pk+i、Pk+2i和Pk+3i,用直线连接 这四个点,取直线PkPk+i和直线Pk+2iPk+3i所夹的较小的角为弯度值,取值为0~180:
(2)预瞄点Pm的确定:在农业机械当前纵向方向上,由当前位置点Pr向前截取前视距离Lf,得到P′m点,再通过快速查找算法得到预定义路径上与P′m点最近的点,该点即是预瞄点Pm。
所述的横向偏差和航向偏差的求解过程如下:
(1)横向偏差d的确定:
如图2所示,首先通过快速查找算法得到预定义路径上与当前农业机械位置最近的点Pk(xk,yk),然后将农业机械当前位置点Pr向直线Pk(xk,yk)Pk+1(xk+1,yk+1)投影,得到点P′r,则横向偏差d=|PrP′r|;
为了导航控制算法设计的方便,这里对横向偏差正负号,在车辆沿预定义路径前进的方向上,如果车辆位于预定义路径右侧,则d为正,如果在左侧,则d为负;判断正负的方法是:如图3所示,首先在预定义路径中找到距离车辆当前位置点最近的点Pk,然后沿着路径前进方向寻找下一个点Pk+1,于是构成一个向量设该向量逆时针旋转与y轴正向的夹角为a1。由车辆当前位置点Pr与点Pk+1,又可组成一个向量设该向量逆时针旋转与y轴正向的夹角为a2。如果a1-a2为正,则车辆在右侧,那么横向偏差为正,否则为负。
(2)航向偏差qe的确定:
航向偏差规定为农业机械的目标方向与当前农业机械的航向之间的角度之差:如图4所示,农业机械的当前位置点Pr与预瞄点Pm构成的向量为设该向量沿逆时针方向旋转与y轴正向的夹角为那么航向偏差角度的取值范围为-90~90,如果为正,则表示车辆当前的航向在目标方向的左边,否则在右边。
所述的模糊控制方法具体为:
模糊控制是将驾驶员的驾驶经验转换为模糊规则,进而决策出前轮转角。 基本思路是当横向偏差和航向偏差越大,那么控制的程度越强,即前轮转角越大;当横向偏差和航向偏差越小,那么控制的程度越弱,即前轮转角越小;当两者一个较大一个较小,那么控制的程度为中。规定前轮右转时为正,左转时为负。驾驶经验转换为模糊规则的方法是:将农业机械相对于预定义路径的状态分为七种,即严重偏左、偏左、稍微偏左、正对着路径、稍微偏右、偏右、严重偏右,根据这七种状态,驾驶员会做出相应的转向控制策略,对应的也分为七种,即大幅度右转、右转、稍微右转、不转向、稍微左转、左转、大幅度左转。据此建立模糊控制规则表,如表1所示。具体设计步骤是在Matlab环境中设计的,如下所示:
(1)首先将两个输入变量d、qe和一个输出变量进行模糊化,其模糊子集分别选为:
Fd={NL,NM,NS,ZO,PS,PM,PL}
三个变量的模糊分割均为7级,即负大、负中、负小、零、正小、正中和正大。选取横向偏差、航向偏差和输出转角的离散论域分别为:
d={-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}
qe={-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}
模糊推理***的基本属性确定为:“与”运算采用极小运算,“或”运算采用极大运算,模糊蕴涵采用极小运算,模糊规则综合采用极大运算,去模糊化采用重心法。航向偏差的基本连续论域离散化采用均匀量化的方法,即将连续论域中的连续值经量化因子比例变换后四舍五入变为离散论域的整数值,具体求解公式为:
q′e=int(kqqe)
d'=int(kdd)
其中q′e、d'均为连续数据取整后的离散数据;输入量化因子kq的确定方法是:根据仿真和试验测试结果,确定航向偏差的误差范围,由离散论域最大值除以相应误差范围最大值即可获得量化因子,获得之后再通过试验进行修正即可;
横向偏差的连续论域离散化具体为:将横向偏差分为四个等级,当测得|d|∈[0,0.1]时,则kd=a1,当得|d|∈(0.1,0.3]时,则kd=a2,当|d|∈(0.3,05]时,则kd=a3, 当d为其它值时,则kd=a4,其中a1、a2、a3和a4分别为横向偏差在四个等级下对应的取值,其数值通过仿真或试验整定得出。
在Matlab环境中,建立模糊变量横向偏差、航向偏差和输出转角的隶属函数曲线,如图5(a)、(b)和(c)所示。对应的模糊控制规则表,利用Matlab软件绘出模糊控制表的三维外观图,如图6所示。
(2)使用模糊控制表查询,模糊控制表如表2所示,打开规则观测器,输入不同的离散量d',q′e,得到对应的输出离散量
(3)输出去模糊控制,即模糊控制算法得出的前轮转角从模糊控制量到实际输出控制量的转换公式如下:
其中,输出的模糊控制算法得出的前轮转角的比例因子的确定方法同输入量化因子。
进一步的,所述的改进型纯追踪算法为:
利用航向偏差建立几何模型,求解农业机械到达目标点所需要的转弯半径,然后由转弯半径再求解出前轮转角,同时把横向偏差作为观测量,如果横向偏差超过设定值,那么给前轮转角一个补偿量以增大前轮转角,使农业机械快速转弯到达目标点;
在图6中,一组点为预定义路径点,Pr为当前农业机械的位置,Pk+j为目标点,R为转弯半径,C为圆心点,qe为航向偏差,L为农业机械前后轮轴间距, 为改进型纯追踪算法求得的前轮转角,那么在直角三角形BPrC中可以得出:
由于qe为圆C的弦切角,因此可得:
—PrCPk+j=2qe
在等腰三角形PrCPk+j中,由正弦定理可得:
由上述三式消去R可得到前轮转角:
式中PrPk+j为农业机械当前位置与目标点的之间的距离;另外,将横向偏差作为控制器的一个观测量,再设定一个预设值dr,当观测到横向偏差d≠dr时,则将改进纯追型踪算法求得的前轮转角乘以一个比例系数kdr,由此可以增大前轮转角,使其快速到达目标点;其中dr和kdr的大小通过仿真或试验整定得出。
进一步的,所述步骤3具体为:通过改进型纯追踪算法和模糊控制算法分别得出前轮转角,然后将两个转角合成一个最终的转角,实现两种方法复合控制,其中合成公式如下:
其中k1、k2分别为模糊控制器输出的前轮转角和改进型纯追踪控制器输出的前轮转角的折算因子,两者之和为1,两者取值由仿真和试验整定得出。
进一步的,所述快速查找算法是计算预定义路径的每个点与当前农业机械坐标点的距离,距离最小的那个点即为最近点。
上述两种导航控制方法的结合,既能提高路径跟踪的稳态精度,又能提高路径跟踪的稳定性。其中改进型纯追踪算法是从几何的角度推算的,由航向偏差经过几何运算得出前轮转角,运算过程未进行近似处理,因此得出的是精确解。该解对于消除导航控制***中的偏差和稳态误差起到重要作用,有利于提高导航精度。
模糊控制方法是一种近似求解方法,该方法的特点是不需要确切的理论模型,仅根据人的驾驶经验来制定模糊规则,建立模糊控制器。因此抗干扰能力较强,不会因模型变化而改变控制效果,能够消除导航***的振荡误差。
本实施例使用的农业机械为井关插秧机,在其上装载了导航传感器组合、转向控制***和车载计算机,各部分之间通信使用总线,由此构成导航控制***,其原理结构如图8所示。导航传感器组合包括差分GPS、电子罗盘、速度传感器和微机械陀螺仪。转向控制***包括操纵控制器、转向驱动机构和测试传感器。车载计算机主要实现数据采集、数据预处理、导航控制算法和控制指令输出等功能,动态数据采集频率为5Hz。
具体实施的步骤如下:
1、预定义路径数据获取
首先由驾驶员驾驶插秧机在水田中正常作业行走,由GPS接收机实时获取定位信息并以总线数据的形式输出,该数据构成预定义路径,路径包括直线段和曲线转弯段,其数学表达式为P|n]={(x0,y0),(x1,y1),...,(xk,yk),...,(xn,yn)}
2、导航数据获取
(1)插秧机位姿和速度参数的获取
涉及数据包括有:插秧机当前定位点坐标、当前航向角度和当前插秧机速度。
(2)前视距离的求解
前视距离的定义如图2所示,求解方法采用前述公式(1)进行求解,利用速度和路径的弯度来确定合适的前视距离,进而可以确定目标点。各参数取值如下:
L0=0.15,Lb=0.62,kv=0.31,kw=0.1
(3)导航偏差的求解
所述的导航偏差分为横向偏差和航向偏差,定义及计算过程如下所示:
1)目标方向的计算
在图2中,即为矢量为的方向,以高斯投影平面坐标系xoy中y轴正向为0度,顺时针旋转到矢量的角度即为该角度取值范围为0~360。
2)航向偏差qe的计算
在图2中,即为目标方向角与插秧机当前航向角度之间的差值。
3)横向偏差d的计算
在图2中,设点P′r为农业机械当前位置点向预定义路径上投影的点,则d=|PrP′r|,判断正负的方法同前所述。
(4)前轮转角模糊控制算法
将横向误差和航向误差以及前轮转角的离散论域分别选取为:
d={-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}
qe={-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}
1)输入变量模糊化
将输入连续论域中的连续值经量化因子比例转换为离散量,然后四舍五入取值。其中航向偏差与横向偏差分别按上述方法进行量化,其中各参数取值如下所示:
kq=0.25,a1=12、a2=2、a3=0.2、a4=0
2)模糊控制表查询
输入不同的离散输入量d'和q′e,依据路径跟踪模糊控制表,如表2所示,即可得到对应的输出离散量
3)输出控制量清晰化
模糊量到实际输出控制量的转换公式如下所示:
其中参数
(5)前轮转角改进型纯追踪算法
首先根据插秧机当前的航向偏差计算出插秧机到达目标点所需的转弯半径,进而求出前轮转角,计算公式如公式(9)所示,然后利用横向偏差作为观测量,如果偏差过大,那么将前轮转角乘以一个系数,将其增大,这样插秧机能够快速到达目标点,各参数分别为:
dr=0.02、kdr=1.08
(6)模糊控制和改进型纯追踪控制复合决策
上述两种算法得出两个前轮转角,利用如下公式进行综合:
其中整定出的两个参数数值分别为:
k1=0.2、k2=0.8
(7)输出控制参数
将决策出的前轮转角按一定的总线通信格式打包,发送给转向控制***,即可实现前轮转角的控制。
表1、模糊控制规则表
表2、路径跟踪模糊控制表
Claims (7)
1.一种农业机械自动导航控制方法,其特征在于,该方法包括如下步骤:
(1)通过GPS接收机实时测得当前时刻农业机械的运动状态数据;
(2)采用预瞄控制方法,动态搜索目标点,将步骤(1)得到运动状态数据与预定义路径数据进行比对分析,推算出横向偏差和航向偏差;
(3)通过模糊控制算法将步骤(2)得到的横向偏差和航向偏差模糊化,然后将模糊量导入模糊控制器中,得到前轮转角;通过改进型纯追踪算法建立几何模型,从而由步骤(2)中得到的航向偏差推导求出前轮转角;将模糊控制算法和改进型纯追踪算法得到的前轮转角合成一个最终的前轮转角;
所述的改进型纯追踪算法为:
利用航向偏差建立几何模型,Pr为当前农业机械的位置,Pk+j为目标点,R为转弯半径,C为圆心点,qe为航向偏差,L为农业机械前后轮轴间距,d2为改进型纯追踪算法求得的前轮转角,那么在直角三角形BPrC中可以得出:
由于qe为圆C的弦切角,因此可得:
—PrCPk+j=2qe
在等腰三角形PrCPk+j中,由正弦定理可得:
由上述三式消去R可得到前轮转角:
式中PrPk+j为农业机械当前位置与目标点的之间的距离;另外,将横向偏差作为控制器的一个观测量,再设定一个预设值dr,当观测到横向偏差d≠dr时,则将改进纯追型踪算法求得的前轮转角d2乘以一个比例系数kdr,由此可以增大前轮转角,使其快速到达目标点;
(4)最后根据前轮转角实时对农业机械进行控制,实现路径跟踪。
2.如权利要求1所述的农业机械自动导航控制方法,其特征在于,所述当前时刻农业机械的运动状态数据包括当前位置Pr(xr,yr)、当前速度V和当前航向角度q;所述预定义路径数据为n+1个位置点的集合,即{P0(x0,y0),P1(x1,y1),...,Pk(xk,yk),Pk+1(xk+1,yk+1),...,Pn(xn,yn)}。
3.如权利要求1所述的农业机械自动导航控制方法,其特征在于,所述预瞄控制方法,即动态搜索目标点,是通过动态计算前视距离,进而确定预定义路径上的预瞄点:
(1)前视距离公式由下述公式计算得出:
Lf=L0+kvV+(Lb-kwW)
其中,Lf为前视距离;L0为前视距离基值;Lb为弯度的最大量化值;kv和kw分别是速度和弯度的比例系数;W为弯度,弯度的计算方法是:首先通过快速查找算法确定预定义路径上与当前农业机械位置最近的点Pk(xk,yk),由该点向后每隔i个点再取3个点,i为正整数,由此可得到4个点,分别是Pk、Pk+i、Pk+2i和Pk+3i,用直线连接这四个点,取直线PkPk+i和直线Pk+ 2iPk+3i所夹的较小的角为弯度值,取值为0~180度;
(2)预瞄点Pm的确定:在农业机械当前纵向方向上,由当前位置点Pr向前截取前视距离Lf,得到P'm点,再通过快速查找算法得到预定义路径上与P'm点最近的点,该点即是预瞄点Pm。
4.如权利要求1所述的农业机械自动导航控制方法,其特征在于,所述的横向偏差和航向偏差的求解过程如下:
(1)横向偏差d的确定:
首先通过快速查找算法得到预定义路径上与当前农业机械位置最近的点Pk(xk,yk),然后将农业机械当前位置点Pr向直线Pk(xk,yk)Pk+1(xk+1,yk+1)投影,得到点Pr',则横向偏差d=|PrPr'|;
(2)航向偏差qe的确定:
航向偏差规定为农业机械的目标方向与当前农业机械的航向之间的角度之差:农业机械的当前位置点Pr与预瞄点Pm构成的向量为设该向量沿逆时针方向旋转与y轴正向的夹角为b,那么航向偏差qe=b-q,q为当前航向角度。
5.如权利要求1所述的农业机械自动导航控制方法,其特征在于,所述的模糊控制方法具体为:
(1)将步骤(2)得到的横向偏差和航向偏差模糊化,即采用基本连续论域,离散化采用均匀量化的方法,将连续论域中的值经量化因子变换后取整,具体为:
q'e=int(kqqe)
d'=int(kdd)
其中q'e、d'均为连续数据取整后的离散量;kq为输入量化因子,qe为航向偏差;
横向偏差的连续论域离散化具体为:将横向偏差分为四个等级,当测得|d|∈(0,0.1]时,则kd=a1,当得|d|∈(0.1,0.3]时,则kd=a2,当|d|∈(0.3,05]时,则kd=a3,当d为其它值时,则kd=a4,其中a1、a2、a3和a4分别为横向偏差在四个等级下对应的取值;
(2)使用模糊控制表查询,打开规则观测器,输入不同的离散量d',q'e,得到对应的输出离散量d'1;
(3)输出去模糊控制,即模糊控制算法得出的前轮转角d1:从模糊控制量到实际输出控制量的转换公式如下:
d1=kd1d'1
其中,输出的比例因子kd1的确定方法同输入量化因子。
6.如权利要求1所述的农业机械自动导航控制方法,其特征在于,所述步骤(3)具体为:通过改进型纯追踪算法和模糊控制算法分别得出前轮转角,然后将两个转角合成一个最终的转角,实现两种方法复合控制,其中合成公式如下:
d=k1d1+k2d2
其中k1、k2分别为模糊控制器输出的前轮转角和改进型纯追踪控制器输出的前轮转角的折算因子,两者之和为1。
7.如权利要求3或4所述的农业机械自动导航控制方法,其特征在于,所述快速查找算法是计算预定义路径的每个点与当前农业机械坐标点的距离,距离最小的那个点即为最近点。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610228933.5A CN105867377B (zh) | 2016-04-13 | 2016-04-13 | 一种农业机械自动导航控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610228933.5A CN105867377B (zh) | 2016-04-13 | 2016-04-13 | 一种农业机械自动导航控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105867377A CN105867377A (zh) | 2016-08-17 |
CN105867377B true CN105867377B (zh) | 2019-01-08 |
Family
ID=56637607
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610228933.5A Active CN105867377B (zh) | 2016-04-13 | 2016-04-13 | 一种农业机械自动导航控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105867377B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP3761065A1 (en) * | 2019-07-05 | 2021-01-06 | Deere & Company | Apparatus and methods for vehicle steering to follow a curved path |
Families Citing this family (33)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108121335A (zh) * | 2016-11-28 | 2018-06-05 | 比亚迪股份有限公司 | 车辆路径跟踪控制方法、装置及车辆 |
CN106546253B (zh) * | 2017-01-13 | 2023-05-05 | 山东农业大学 | 一种农田作业机械路径追踪装置及其追踪方法 |
CN106909153B (zh) * | 2017-03-21 | 2020-07-31 | 北京京东尚科信息技术有限公司 | 无人车横向控制方法和装置 |
CN109002035A (zh) * | 2017-06-07 | 2018-12-14 | 北京京东尚科信息技术有限公司 | 机器人运动的控制方法和装置、计算机可读存储介质 |
CN107264621B (zh) * | 2017-06-15 | 2019-05-17 | 驭势科技(北京)有限公司 | 车辆预瞄距离计算方法、装置、介质及转向控制方法 |
CN108388241B (zh) * | 2018-01-05 | 2021-02-12 | 广州科语机器人有限公司 | 移动机器人的路径跟踪方法 |
CN108227715B (zh) * | 2018-01-12 | 2020-12-04 | 上海海事大学 | 一种抗浪节能的无人艇路径跟踪方法 |
CN108303982B (zh) * | 2018-01-31 | 2021-11-30 | 深圳力子机器人有限公司 | 自动引导运输车、其控制方法及控制*** |
CN108490943B (zh) * | 2018-04-04 | 2021-08-31 | 上海华测导航技术股份有限公司 | 一种农业机械自适应曲线自动驾驶控制方法 |
CN108415435B (zh) * | 2018-04-04 | 2021-08-31 | 上海华测导航技术股份有限公司 | 一种农业机械圆曲线自动驾驶控制方法 |
CN108919792B (zh) * | 2018-05-30 | 2021-08-24 | 华南农业大学 | 一种自动导航***路径规划控制方法 |
CN109291989A (zh) * | 2018-10-24 | 2019-02-01 | 山东农业大学 | 一种用于农业机械的电动方向盘及自动驾驶方法 |
CN109542099B (zh) * | 2018-11-26 | 2021-05-07 | 江苏农牧科技职业学院 | 一种农机控制方法 |
CN109823393A (zh) * | 2018-12-14 | 2019-05-31 | 清华大学苏州汽车研究院(相城) | 一种智能驾驶车辆轨迹跟踪控制方法 |
CN109407674A (zh) * | 2018-12-19 | 2019-03-01 | 中山大学 | 基于遗传算法整定参数的Pure Pursuit结合PI的路径跟踪方法 |
CN110109451B (zh) * | 2019-04-10 | 2022-07-12 | 东南大学 | 一种考虑路径曲率的新型几何学路径跟踪算法 |
CN110286672A (zh) * | 2019-05-13 | 2019-09-27 | 深圳创动科技有限公司 | 机器人及其导航控制方法、导航控制装置以及存储介质 |
CN110398963A (zh) * | 2019-07-13 | 2019-11-01 | 珠海格力智能装备有限公司 | 无轨导航纠偏控制方法、装置、存储介质及控制器 |
CN110840344B (zh) * | 2019-09-30 | 2021-08-06 | 湖南格兰博智能科技有限责任公司 | 一种基于pid控制算法的扫地机器人沿线行走控制***及其控制方法 |
CN110641462B (zh) * | 2019-10-30 | 2020-11-17 | 山东理工大学 | 一种农业机械自动转向方法 |
CN110955245B (zh) * | 2019-12-09 | 2021-06-08 | 深圳市银星智能科技股份有限公司 | 路径追踪方法、装置、移动机器人及非暂态计算机可读存储介质 |
CN113805577A (zh) * | 2020-08-18 | 2021-12-17 | 北京京东乾石科技有限公司 | 目标移动体控制方法、装置及相关设备 |
CN112269388A (zh) * | 2020-10-30 | 2021-01-26 | 广东杜尼智能机器人工程技术研究中心有限公司 | 无人驾驶扫地车自动导航控制方法、装置及存储介质 |
CN112698564A (zh) * | 2020-12-21 | 2021-04-23 | 桂林航天工业学院 | 基于动态双三角形原理的自走式平地机智能控制方法 |
US11914379B2 (en) | 2020-12-23 | 2024-02-27 | Deere & Company | Methods and apparatus to generate a path plan |
CN112684483B (zh) * | 2021-01-22 | 2024-02-06 | 浙江理工大学 | 基于卫星和视觉融合的导航偏差感知及其信息获取方法 |
CN113190018B (zh) * | 2021-05-24 | 2022-07-01 | 东南大学 | 一种基于改进航向误差率的智能体路径控制方法 |
CN113848953B (zh) * | 2021-10-25 | 2023-12-08 | 济南大学 | 一种大型液压植保机无人驾驶路径跟踪控制方法 |
CN114237214A (zh) * | 2021-10-28 | 2022-03-25 | 北京星航机电装备有限公司 | 一种基于模糊控制与级联控制的差速agv控制方法及*** |
CN113985729B (zh) * | 2021-11-16 | 2024-05-14 | 江苏大学 | 基于变论域模糊pid理论的拖拉机水田作业路径跟踪方法及*** |
CN114721375B (zh) * | 2022-03-14 | 2024-03-26 | 常州江苏大学工程技术研究院 | 一种农机单天线导航路径跟踪方法 |
CN115326054A (zh) * | 2022-08-24 | 2022-11-11 | 中国热带农业科学院农业机械研究所 | 一种履带式农用车的自动导航方法 |
CN116466589B (zh) * | 2023-04-26 | 2024-02-09 | 长春理工大学 | 一种基于自抗扰模型的路径跟踪控制方法、装置、存储介质及电子设备 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101093396A (zh) * | 2007-07-04 | 2007-12-26 | 华南农业大学 | 一种农业机械的导航控制方法 |
CN103439884A (zh) * | 2013-07-19 | 2013-12-11 | 大连理工大学 | 一种基于模糊滑模的智能汽车横向控制方法 |
CN103914066A (zh) * | 2012-12-28 | 2014-07-09 | 中国科学院沈阳自动化研究所 | 农机自动导航控制器及其控制方法 |
CN105182976A (zh) * | 2015-09-17 | 2015-12-23 | 西北农林科技大学 | 一种农业机器人的视觉导航策略 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8694382B2 (en) * | 2011-02-18 | 2014-04-08 | Cnh America Llc | System and method for automatic guidance control of a vehicle |
-
2016
- 2016-04-13 CN CN201610228933.5A patent/CN105867377B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101093396A (zh) * | 2007-07-04 | 2007-12-26 | 华南农业大学 | 一种农业机械的导航控制方法 |
CN103914066A (zh) * | 2012-12-28 | 2014-07-09 | 中国科学院沈阳自动化研究所 | 农机自动导航控制器及其控制方法 |
CN103439884A (zh) * | 2013-07-19 | 2013-12-11 | 大连理工大学 | 一种基于模糊滑模的智能汽车横向控制方法 |
CN105182976A (zh) * | 2015-09-17 | 2015-12-23 | 西北农林科技大学 | 一种农业机器人的视觉导航策略 |
Non-Patent Citations (1)
Title |
---|
基于模糊自适应纯追踪模型的农业机械路径跟踪方法;李逃昌 等;《农业机械学报》;20130131;第44卷(第1期);第205-210页 |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP3761065A1 (en) * | 2019-07-05 | 2021-01-06 | Deere & Company | Apparatus and methods for vehicle steering to follow a curved path |
Also Published As
Publication number | Publication date |
---|---|
CN105867377A (zh) | 2016-08-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105867377B (zh) | 一种农业机械自动导航控制方法 | |
CN111610780B (zh) | 一种自动驾驶车路径跟踪控制方法及其装置 | |
CN109375643B (zh) | 基于领航-跟随式三角形编队的多四旋翼对峙跟踪目标制导律 | |
WO2019184179A1 (zh) | 控制自主式移动机器移动的方法、装置、机器及存储介质 | |
CN107831761B (zh) | 一种智能车的路径跟踪控制方法 | |
KR101145112B1 (ko) | 자율이동차량의 조향제어장치, 이를 구비하는 자율이동차량 및 자율이동차량의 조향제어방법 | |
CN107943020B (zh) | 一种轮胎吊大车自动纠偏方法 | |
CN106909151A (zh) | 用于农机无人驾驶的路径规划及其控制方法 | |
CN113359710B (zh) | 一种基于los理论的农机路径跟踪方法 | |
CN105987696A (zh) | 一种低成本车辆自动驾驶设计实现方法 | |
CN102736626B (zh) | 基于视觉的移动小车位姿镇定控制方法 | |
CN107045347A (zh) | 用于农机无人驾驶的自动掉头路径规划及其控制方法 | |
CN110471289B (zh) | 一种基于视觉导航移动设备的自适应路径跟踪方法及*** | |
CN106371442B (zh) | 一种基于张量积模型变换的移动机器人控制方法 | |
CN106647770A (zh) | 用于农机无人驾驶的田间掉头路径规划及其控制方法 | |
CN108459605A (zh) | 基于agv***的轨迹跟踪控制方法 | |
CN111240362B (zh) | 一种智能引导飞行器转弯的控制方法及装置 | |
CN108490943B (zh) | 一种农业机械自适应曲线自动驾驶控制方法 | |
CN108415435B (zh) | 一种农业机械圆曲线自动驾驶控制方法 | |
CN108681324A (zh) | 基于全局视觉的移动机器人轨迹跟踪控制方法 | |
CN112650217B (zh) | 一种基于评估函数的机器人轨迹跟踪策略动态优化方法 | |
JP2018084899A (ja) | 自律走行車両、コントローラ、コンピュータプログラム、自律走行車両の制御方法 | |
EP4269044A1 (en) | Slope location correction method and apparatus, robot and readable storage medium | |
CN115480570A (zh) | 农机车辆自动导航控制方法和装置、农机车辆 | |
CN104527780A (zh) | 一种四轮独立转向车辆的转向控制方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |