CN100504694C - 一种农业机械的导航控制方法 - Google Patents

一种农业机械的导航控制方法 Download PDF

Info

Publication number
CN100504694C
CN100504694C CNB2007100290184A CN200710029018A CN100504694C CN 100504694 C CN100504694 C CN 100504694C CN B2007100290184 A CNB2007100290184 A CN B2007100290184A CN 200710029018 A CN200710029018 A CN 200710029018A CN 100504694 C CN100504694 C CN 100504694C
Authority
CN
China
Prior art keywords
agricultural machinery
xte
error
deviation
fuzzy
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
Application number
CNB2007100290184A
Other languages
English (en)
Other versions
CN101093396A (zh
Inventor
罗锡文
张智刚
赵祚喜
林卫平
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China Agricultural University
Original Assignee
South China Agricultural University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by South China Agricultural University filed Critical South China Agricultural University
Priority to CNB2007100290184A priority Critical patent/CN100504694C/zh
Publication of CN101093396A publication Critical patent/CN101093396A/zh
Application granted granted Critical
Publication of CN100504694C publication Critical patent/CN100504694C/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Guiding Agricultural Machines (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种农业机械的导航控制方法,包括通过导航传感器测量得到农业机械当前时刻的位姿和运动状态数据;采用基于预瞄控制的动态目标点搜索算法,推算出两个导航状态参数即横向跟踪误差和纵向航向偏差;再通过Fuzzy-PID控制算法推算出操纵控制量即前轮转向偏角;最后根据所述操纵控制量对农业机械进行实时控制,实现路径跟踪。本发明充分利用传统导航控制方法的优点,结合模糊控制方法,兼顾稳态跟踪精度和控制稳定性,提高了农业机械作业条件的导航跟踪精度、稳定性和安全性;不仅适用于旱地农业机械的作业需要,还适用于水田农业机械的作业需要,具有广阔的推广应用前景。

Description

一种农业机械的导航控制方法
技术领域
本发明涉及农业机械领域,特别涉及一种农业机械的导航控制方法。
背景技术
农业机械导航控制的主要任务是根据导航定位结果,确定农业机械与预定路线的位置关系,进而结合农业机械的运动状态决策出合适的转向轮操纵角,以修正路径跟踪误差。国外常用的导航控制方法有三种,即线性模型控制方法、最优控制方法和模糊控制方法。东京大学利用机器视觉技术进行KUBOTA SPJ45A型久保田插秧机的导航控制技术研究,他将目标方向角度与车辆纵向方向角度进行对比,用Abe的线性转向控制模型计算得出转向轮转向角度。Qiu提出了一种基于预瞄的导航控制算法,该导航控制器包括一个前馈比例控制器和反馈PD控制器;前进速度决定前视距离,位置误差决定前馈增益;前馈控制器的输入是路径曲率,反馈控制器的输入是航向偏差,两控制器的输出合成为期望转向轮操纵角。东京大学采用模糊控制技术来进行导航控制研究,模糊控制器根据方向偏差和位置偏差确定转向角度和左转执行时间及右转执行时间。韩国的Lee等人利用机器视觉、DGPS、超声波传感器和模糊控制技术进行果树喷雾机的导航控制研究。O’Connor等人采用方向偏差、方向偏差变化率、转向角度、转向角度变化率、跟踪误差等五个状态变量建立车辆运动学方程,以转向角度变化率最小为优化指标,利用车辆线性运动学方程组建立了基于LQR的最优导航控制器。Kise等人开发了最优操纵控制器用于农用车辆导航,在曲线路径跟踪方面取得了一定的效果。周俊(2003)提出了视觉导航的图象处理技术,同时还利用线性状态反馈控制方法、二值控制方法建立了导航控制的内外环结构;在车辆纵向速度较高的状态下,提出了把横向偏差、航向偏差以及横向加速度作为输入量的三维横向模糊控制算法,以保证车辆横向运动的平稳性。毛恩荣等人(2005)研究了在铁牛654拖拉机上搭建DGPS自动驾驶系统的硬件组成及关键技术,根据最优控制理论,研究了横向偏差和航向角输出反馈控制系数的离线优化调节方法,为试验凑试最优控制参数提供依据。本发明人在2005年将GPS技术、计算机技术、传感器技术进行集成,研制了一种以蓄电池为电源、电动机为动力的农用智能移动作业平台,该样机采用前轮驱动、差速转向方式;并以DGPS、电子罗盘为主要导航传感器,在样机上建立了DGPS导航控制系统;还设计了基于预瞄跟随的导航控制算法,其核心是由航向偏差线性决定驱动轮速度差。由于该智能移动作业平台采用差速转向方式,路径跟踪的控制难度较大,导航精度难以提高。
常规线性模型控制方法,包括PID控制方法,可以获得高精度的路径跟踪效果,但是抗干扰能力弱。模糊控制方法具有良好的稳定性效果,但路径跟踪的稳态精度难以保证。最优控制器的建立主要依据农业机械运动学和动力学模型,其模型精确性对控制器性能有重要影响。但是,田间作业条件下农业机械运动学和动力学模型的建立是比较困难的。田间作业时,随着农业机械、作业机具与地面相互作用的变化,田间土壤特性的变化,农业机械运动特性必然是时变的,还有作业工况差引起的传感器测试干扰等,这些因素对常规线性模型控制方法和最优控制方法的影响都较大。
发明内容
本发明的目的在于克服现有技术中存在的缺点,提供一种导航跟踪精度高、稳定性好的农业机械的导航控制方法。该方法基于预瞄跟随理论,采用PID和模糊逻辑相结合的方法来设计路径跟踪的控制算法,充分利用PID路径跟踪的高精度和模糊控制的稳定性特点,建立稳定性好、精度高的路径跟踪控制算法。
本发明一种农业机械的导航控制方法,包括下述步骤:
(1)通过导航传感器测量得到农业机械当前时刻的位姿和运动状态数据;
(2)采用基于预瞄控制的动态目标点搜索算法,将所述位姿和运动状态数据与预定义路线数据进行对比分析,推算出两个导航状态参数即横向跟踪误差和纵向航向偏差;
(3)再通过Fuzzy-PID控制算法推算出操纵控制量即前轮转向偏角;
(4)最后根据所述操纵控制量对农业机械进行实时控制,实现路径跟踪。
所述的预定义路线P[N]由点数组表示,包含元素为:
{P0,P1,...,Pk-1,Pk,Pk+1,...,PN}
其数学表达式为:
{(x0,y0),(x1,y1),...,(xk-1,yk-1),(xk,yk),(xk+1,yk+1),...,(xn,yn)}
在GIS中,预定义路线由点数据集表示。Pk点的坐标(xk,yk)为WGS-84大地坐标系的高斯投影平面坐标。点与点之间的平均间距表示为Ls。本发明中所述坐标点均为WGS-84大地坐标系的高斯投影平面坐标。
所述导航传感器包括差分全球定位系统(DGPS)接收模块,机器视觉定位,超声波、激光等测距定位,以及陀螺仪、电子罗盘、速度计、里程计等航位推算定位(DR)设备。
所述的农业机械当前时刻的位姿和运动状态数据包括当前定位点、当前航向角度和当前速度,如图1所示,具体定义如下:
(1)当前定位点Pc:由DGPS接收机获得的当前时刻差分定位数据,以(xc,yc)表示;
(2)当前航向角度θ:由电子罗盘数据获得的当前时刻农业机械纵向方向的角度;以高斯投影平面坐标系xoy中x轴正向为0度,逆时针方向为正;
(3)当前速度V:由速度传感器获得的当前时刻农业机械纵向方向的前进速度。
所述的基于预瞄控制的动态目标点搜索算法,是通过动态计算前视距离,确定预定义路线上的预瞄点,具体步骤如下:
(1)前视距离Lf的确定:
如图1所述,前视距离Lf可解释为在农业机械当前纵向方向上的预瞄距离;考虑农业机械速度的影响,前视距离Lf可通过下述公式计算得到:
Lf=L0+KlfrV+KlfrRdf                           (1)
其中,L0为前视距离基值;Klfv,Klfr分别为前进速度、前视偏差变化率的比例系数,可在仿真或实验过程中通过整定获得,Klfv与Klfr均取正值,Klfv与Klfr的取值范围为0≤Klfv≤1,0≤Klfr≤1;Rdf为前视偏差变化率,定义为:
R df = | P k + 2 m P ′ k + 2 m → | - | P k P ′ k → | | P k + m P ′ k + m → | - | P k P ′ k → | - - - ( 2 )
其中 m = int ( VT s L s ) , Pk表示预定义路线上的第k坐标点,P′k映射点为在直线PkPk-1上以Pk为垂足引垂线和农业机械当前纵向方向线的相交点,Ts为导航的采样和控制周期,Ls为预定义路线上点与点之间的平均间距;
(2)预瞄点P′p的确定:在农业机械当前纵向方向上,由当前定位点Pc点起截取前视距离Lf,得到Pp点;再由Pp点向预定义路线投影,得到预瞄点P′p
所述的两个导航状态参数即横向跟踪误差和纵向航向偏差的推算过程如下:
(1)纵向航向偏差θe的确定:纵向航向偏差θe为目标方向θp与当前农业机械航向角度θ之间的差值;其中目标方向θp为矢量PcP′p的方向,以高斯投影平面坐标系xoy中x轴正向为0度,逆时针方向为正;
(2)横向跟踪误差XTE的确定:
如图1所示,设Pc到预定义路线的投影是P′c,则XTE=|PcP′c|,即
XTE = ( x c - x ′ c ) 2 + ( y c - y ′ c ) 2 - - - ( 3 )
为了导航控制算法设计的方便,这里对XTE规定正负号,在农业机械沿预定义路线的前进方向上,若农业机械当前坐标点位于预定义路线的右侧,XTE为正;若当前坐标点位于预定义路线的左侧,XTE为负;判断XTE正负号的方法是:如图2所示,首先在预定义路线P[N]中,寻找到距离当前定位点Pc最近的节点Pk,沿预定义路线前进方向寻找下一节点Pk+1;规定向量
Figure C200710029018D00082
Figure C200710029018D00083
的夹角顺时针为负,逆时针为正;若向量
Figure C200710029018D00084
Figure C200710029018D00085
的夹角为正,表明当前坐标点位于预定义路线的左侧,XTE为负;若向量
Figure C200710029018D00086
Figure C200710029018D00087
的夹角为负,表明当前坐标点位于预定义路线的右侧,XTE为正。
所述通过Fuzzy-PID控制算法推算出操纵控制量即前轮转向偏角的具体方法步骤是:当位置偏差在区间[-a,a]且航向偏差在区间[-b,b]时,为PID控制方法的作用范围;其他区间为模糊逻辑方法Fuzzy的作用范围,实现两种决策算法的复合控制;其中,a和b的取值范围为0≤a≤0.5,0≤b≤45,其原理如图7所示。
其中,PID控制方法的算法为:该算法的实质是将位置偏差和航向偏差转换为合成误差ERROR,并将合成误差ERROR作为控制输入量输入到PID控制器,然后决策输出前轮转向偏角,达到在路径跟踪过程中既消除航向偏差又消除位置误差的目的;ERROR定义为:
ERROR=α×XTE+β×θe                                     (4)
其中,α和β分别为横向跟踪误差、纵向航向偏差相对于合成误差的折算因子,α和β的确定主要根据试验效果进行现场整定,其中α和β的取值范围为0≤α≤10,0≤β≤1。
采用增量式PID控制算法来决策前轮转向偏角,具体如下:
U i = K [ E i - E i - 1 + T s T i E i + T d T s ( E i - 2 E i - 1 + E i - 2 ) ] + U i - 1 - - - ( 5 )
其中,Ui、Ui-1为第i、i-1时刻的转向轮期望偏角,Ei、Ei-1、Ei-2为第i、i-1、i-2时刻的转向轮期望偏角与实际偏角之差。Ts为采样周期,K为比例系数,Td为微分时间常数,T1为积分时间常数;K、Td、Ti三参数根据试验效果进行现场整定,其取值范围为:0≤K≤10,0≤Td≤1,0≤Ti≤0.5。
本发明将基于合成误差的PID决策算法用于农业机械导航,决策前轮转向偏角,对提高导航系统性能具有一定作用。PID控制器的比例调节系数K起偏角决策作用,而其中的微分环节可有效抑制导航控制系统中的振荡和不稳定,积分环节对于消除导航控制系统中的系统偏差将起到重要作用。PID决策算法不仅继承了传统线性决策模型中零稳态跟踪误差的重要特性,而且对于提高系统稳定性也具有重要作用。
其中,使用模糊逻辑Fuzzy方法来决策转向轮偏角的思路是将驾驶员的驾驶经验转化为模糊规则,建立模糊逻辑控制器,实现前轮转向偏角控制量决策。驾驶经验转化为模糊规则的步骤是:将农业机械与预定义路线之间的位置关系分为九种状态,如图3所示;根据驾驶员控制作业机械沿预定路线行驶的经验,建立了相应的转向控制策略,如表1所示;控制强度指转向轮偏转幅度,与期望转向轮偏角成正比;控制方向为前轮偏转方向,向左时期望偏角为正,向右时期望偏角为负;据此建立模糊控制规则表,如表2所示。
在Matlab环境下设计该模糊逻辑控制器的具体步骤如下:
(1)首先将输入变量模糊化:在Matlab环境下,建立模糊变量横向跟踪误差XTE,方位偏差θe的隶属函数曲线,如图4(a)、(b)所示;建立模糊变量转向控制量δp的隶属函数曲线,如图5所示;对应于转向模糊控制规则表,利用Matlab软件绘制出模糊控制表三维外观图,如图6所示。
其中,XTE、θe和δp作为模糊语言变量时,其模糊子集分别选为:
Fxte={NB,NM,NS,NO,PO,PS,PM,PB}
F θ z = { NB , NM , NS , NO , PO , PS , PM , PB }
F δ p = { NB , NM , NS , O , PS , PM , PB }
语言变量XTE和θe组成模糊输入空间,δp为模糊输出空间,输入和输出空间的模糊分割分别为:Fxte分为8级,即负大、负中、负小、负零、正零、正小、正中和正大;
Figure C200710029018D0009082001QIETU
分为8级,即负大、负中、负小、负零、正零、正小、正中和正大;
Figure C200710029018D0009082015QIETU
分为7级,即负大、负中、负小、零、正小、正中和正大。
选取横向跟踪误差XTE、方位偏差θe和转向控制量δp的离散论域分别为:
XTE={-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6};
θe={-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6};
δp={-7,-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6,7};
模糊推理系统的基本属性确定为:“与”运算采用极小运算,“或”运算采用极大运算,模糊蕴涵采用极小运算,模糊规则综合采用极大运算,去模糊化采用重心法。基本连续论域离散化采用均匀量化的方法,即将连续论域中的连续值经量化因子比例变换后四舍五入变为离散论域的整数值,具体算式如下:
XTE′=int(KxteXTE)                                   (6)
θ e ′ = int ( K θe θ e ) - - - ( 7 )
上两式中,输入量化因子Kxte、Kθe的确定方法是:根据试验测试结果,确定横向跟踪误差XTE、方位偏差θe的误差范围,由离散论域最大值除于相应误差范围最大值即可获得量化因子值。
(2)然后模糊控制表查询:打开规则观测器,输入不同的离散量XTE′,θe′,得到对应的清晰控制量δp,进而构成模糊控制表,如表3所示。
(3)最后输出去模糊控制量:从模糊控制量到实际输出控制量的转换算式如下:
δ p ′ = K δ p δ p - - - ( 8 )
上式中,输出比例因子
Figure C200710029018D0010082414QIETU
的确定方法同输入量化因子。
由于PID控制方法有利于提高路径跟踪的稳态精度,但不能提高路径跟踪的稳定性,抗干扰能力较差;模糊逻辑控制方法有利于提高路径跟踪的稳态性,但不能显著提高路径跟踪的精度。PID控制方法和模糊逻辑控制方法在路径跟踪控制过程中存在互补性。因此,本发明采用PID和FUZZY的转向轮偏角复合决策算法来进行实际控制。实际应用中,可根据试验效果对两个偏差区间进行合理调节,以确定PID和FUZZY控制的相应作用范围。
本发明与现有技术相比具有如下优点和效果:
(1)本发明充分利用传统导航控制方法的优点,结合模糊控制方法,兼顾稳态跟踪精度和控制稳定性,提高了农业机械作业条件下的导航跟踪精度、稳定性和安全性。
(2)本发明可以控制农业机械沿着预定义路线或者地物标志线自动行走,路径跟踪误差小于30cm。
(3)本发明适用于智能农业机械装备的辅助或者自动导航、变量处方作业、农情信息采集的场合;不仅适用于旱地农业机械的作业需要,还适用于水田农业机械的作业需要,具有广阔的推广应用前景。
附图说明
图1为农业机械与预定义路线的位置关系解析原理图
图2为XTE正负号的确定方法
图3为农业机械与预定义路线之间的九种位置关系状态图
图4为模糊输入变量的隶属函数曲线
图5为模糊输出变量的隶属函数曲线
图6为模糊控制输出的三维外观图
图7为转向轮偏角的FUZZY-PID决策算法原理图
图8为导航控制系统的原理结构框图
图9为转向控制系统框图
图10为转向传动机构的安装示意图
图11为基于DGPS的路径跟踪测试结果
图12为在有初始偏差条件下基于DGPS的路径跟踪测试结果
具体实施方式
下面结合实施例对本发明做进一步详细的描述,但本发明的实施方式不限于此。
本实施例采用的农业机械为久保田插秧机,在其上装载了导航传感器组合、转向控制系统和便携式计算机,便携计算机通过USB-RS232数据传输线与导航传感器组合、转向控制系统相连接,构成导航控制系统,原理结构如图8。
导航传感器组合包括伪距差分GPS、电子罗盘和微机械陀螺。伪距差分GPS可以采集插秧机在WGS-84坐标系下的定位坐标,作为初步定位信息;电子罗盘测量其航向角度信息(以正北方向为0,逆时针方向为正);微机械陀螺测量其横向角速率(逆时针方向为正)。
转向控制系统包括转向操纵控制器、转向驱动机构和测试传感器三部分,图9是转向控制系统组成图。转向操纵控制器是车载计算机的下位机,主要完成转向控制过程中的转向轮偏角检测,根据便携计算机的转向信息指令,控制驱动机构正确的执行动作;转向驱动机构则完成驱动力传递,带动转向轮偏转,及时准确地实现转向动作。图10是转向驱动机构的安装示意图。
便携计算机实现传感器数据采集、数据预处理、导航控制算法以及控制指令输出等功能。动态数据采集频率是1Hz。数据采集、数据预处理和导航控制算法是采用Visual C++编程工具在Windows XP操作系统下开发完成的。
具体实施步骤如下:
1、预定路线设定
由熟练驾驶员驾驶插秧机沿预定作业路线匀速行走,由导航控制系统采集DGPS导航传感器数据并保存,构成预定义路线P[N]。其数学表达式为:
{(x0,y0),(x1,y1),...,(xk-1,yk-1),(xk,yk),(xk+1,yk+1),...,(xn,yn)}
对于直线路径跟踪,可以在预定直线上选定两个坐标点P[1]、P[2](通过DGPS静态定位获得),建立预定义路线的解析几何描述。
2、启动导航控制算法,具体运行流程是:
(1)插秧机位姿和运动状态参数采集
涉及数据有:当前定位点Pc坐标、当前航向角度θ(以高斯投影平面坐标系xoy中x轴正向为0度,逆时针方向为正)、当前速度V。
(2)动态目标点搜索
基于农业机械与预定跟踪路线的位置解析图1,本发明采用前述前视偏差变化率Rdf确定当前前视距离Lf。Rdf的解算采用公式(2),前视距离Lf的解算采用公式(1);并确立预瞄点P′p
(3)导航状态参数计算
所述的两个导航状态横向跟踪误差和纵向航向偏差的定义及计算过程如下:
1)目标方向θp计算:
在图1中,即为矢量PcP′p的方向,以高斯投影平面坐标系xoy中x轴正向为0度,逆时针方向为正。
2)纵向航向偏差θe
即为目标方向与当前插秧机航向角度之间的差值。
3)横向跟踪误差XTE:
在图1中,设Pc到预定义路线的投影是P′c,则XTE=|PcP′c|,也即
XTE = ( x c - x ′ c ) 2 + ( y c - y ′ c ) 2
判断XTE正负号的方法同前所述。
(4)转向轮偏角的PID决策算法
1)横向跟踪误差和纵向航向偏差合成
合成误差可用式(4)计算,α、β的经验值分别取10,1。
2)PID决策转向轮偏角
本发明使用增量式PID控制算法的算式决策转向轮偏角,如式(5)所示。根据试验测试结果,现场整定K、Td、Ti三参数分别为:0.8,0.5,0.01。
(5)转向轮偏角的模糊逻辑决策算法
选取横向跟踪误差XTE与方位偏差θe和转向轮偏角控制量δp的离散论域分别为:
XTE={-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6};
θe={-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6};
δp={-7,-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6,7};
1)输入变量模糊化
基本输入连续论域离散化采用均匀量化的方法,即将连续论域中的连续值经量化因子比例变换后四舍五入变为离散论域的整数值。具体算式如(6)、(7)。设定Kxte横向跟踪误差XTE与方位偏差θe的经验最大值分别为1m,90°,则Kxte=0.6,Kθe=0.06。
2)模糊控制表查询
输入不同的离散输入量XTE′,依据模糊控制表,得到对应的清晰控制量δp
3)输出控制量去模糊
模糊量到实际输出控制量的转换算式如下:
δ p ′ = K δ p δ p
(6)基于PID和FUZZY的转向轮偏角复合决策
设定位置偏差区间[-0.3,0.3]、航向偏差区间[-30,30]为PID控制方法的作用范围(即a=0.3m,b=30°),其他区间为模糊逻辑方法的作用范围,实现两种决策算法的复合控制,可用下式描述:
Figure C200710029018D00141
(7)输出控制参数
将决策的转向轮偏角按一定的串行通信格式打包后,发送给转向控制系统,实现转向轮偏转控制。串行通信格式如下:
$CON,a,b,c*DD<CR><LF>
上述格式中,$CON表示转向轮偏角控制量,是标识字符串,也是字符串起始标志;a,b,c表示一个ACSII字符,实际代表一个十进制数据,分别表示转向轮偏角的百位,十位和个位。$与之间所有字符代码的校验和由后面的两位字符DD表示,校验和D为半Byte校验,后第1个D表示高4位校验和,第2个D表示低4位校验和,得到校验值后,再转换成ASCII字符得到;<CR><LF>是字符串结束标志。
导航控制算法的流程按照1Hz的频率运行,实现插秧机的路径跟踪控制。
直线路径跟踪的试验结果如图11、12:采用DGPS导航插秧机进行路径跟踪,以0.75m/s速度直线行走,最大横向跟踪误差0.136m,平均偏差0.044m。有初始航向偏差情况下,路径跟踪误差和航向偏差可以快速修正,路径跟踪过程平稳,稳态跟踪精度高,最大横向跟踪误差0.2m,平均偏差006m。
表1、驾驶员控制车辆的策略表
 
策略序号 方向偏差 位置偏差 控制强度 控制方向 位置关系
1 + - 向左 (a)
2 + 0 向左 (b)
3 + + 向左 (c)
4 - + 向右 (d)
5 - 0 向右 (e)
6 - - 向右 (f)
7 0 - 向右 (g)
8 0 0 (h)
9 0 + 向左 (i)
表2、模糊控制规则表
Figure C200710029018D00151
表3、路径跟踪的模糊控制表
Figure C200710029018D00161

Claims (6)

1、一种农业机械的导航控制方法,其特征在于包括下述步骤:
(1)通过导航传感器测量得到农业机械当前时刻的位姿和运动状态数据;
(2)采用基于预瞄控制的动态目标点搜索算法,将所述位姿和运动状态数据与预定义路线数据进行对比分析,推算出两个导航状态参数即横向跟踪误差和纵向航向偏差;
(3)再通过Fuzzy-PID控制算法推算出操纵控制量即前轮转向偏角;即当位置偏差在区间[-a,a]且航向偏差在区间[-b,b]时,为PID控制方法的作用范围;其他区间为模糊逻辑方法Fuzzy的作用范围,实现两种决策算法的复合控制;其中,a和b的取值范围为0≤a≤0.5,0≤b≤45。
(4)最后根据所述操纵控制量对农业机械进行实时控制,实现路径跟踪。
2、根据权利要求1所述的农业机械的导航控制方法,其特征在于:所述农业机械当前时刻的位姿和运动状态数据包括当前定位点、当前航向角度θ和当前速度V。
3、根据权利要求1所述的农业机械的导航控制方法,其特征在于:所述基于预瞄控制的动态目标点搜索算法,是通过动态计算前视距离,确定预定义路线上的预瞄点:
(1)前视距离Lf通过下述公式计算得到:
Lf=L0+KlfvV+KlfrRdf
其中,L0为前视距离基值;Klfv,Klfr分别为前进速度、前视偏差变化率的比例系数,可在仿真或实验过程中通过整定获得,Klfv与Klfr均取正值;Rdf为前视偏差变化率,定义为:
R df = | P k + 2 m P &prime; k + 2 m &RightArrow; | - | P k P &prime; k &RightArrow; | | P k + m P &prime; k + m | &RightArrow; - | P k P &prime; k &RightArrow; | - - - ( 2 )
其中 m = int ( VT s L s ) , Pk表示预定义路线上的第k坐标点,P′k映射点为在直线PkPk-1上以Pk为垂足引垂线和农业机械当前纵向方向线的相交点,Ts为导航的采样和控制周期,Ls为预定义路线上点与点之间的平均间距;
(2)预瞄点P′p的确定:在农业机械当前纵向方向上,由当前定位点Pc点起截取前视距离Lf,得到Pp点;再由Pp点向预定义路线投影,得到预瞄点P′p
4、根据权利要求1所述的农业机械的导航控制方法,其特征在于:所述推算两个导航状态参数即横向跟踪误差和纵向航向偏差的过程如下:
(1)纵向航向偏差θe的确定:纵向航向偏差θe为目标方向θp与当前航向角度θ之间的差值;
(2)横向跟踪误差XTE的确定:
XTE = ( x c - x &prime; c ) 2 + ( y c - y &prime; c ) 2
其中,Pc(xc,yc)为当前定位点;P′c(x′c,y′c)为Pc到预定义路线的投影;并且若农业机械当前坐标点位于预定义路线的右侧,XTE为正;若当前坐标点位于预定义路线的左侧,XTE为负。
5、根据权利要求1所述的农业机械的导航控制方法,其特征在于:所述PID控制方法是指,将位置偏差和航向偏差转换为合成误差ERROR,并将合成误差ERROR作为控制输入量输入到PID控制器,然后决策输出前轮转向偏角;
(1)ERROR定义为:
ERROR=α×XTE+β×θe
其中,α和β分别为横向跟踪误差、纵向航向偏差相对于合成误差的折算因子,α和β的确定主要根据试验效果进行现场整定;
(2)采用增量式PID控制算法来决策转向轮偏角:
U i = K [ E i - E i - 1 + T s T i E i + T d T s ( E i - 2 E i - 1 + E i - 2 ) ] + U i - 1
其中,Ui、Ui-1为第i、i-1时刻的转向轮期望偏角,Ei、Ei-1、Ei-2为第i、i-1、i-2时刻的转向轮期望偏角与实际偏角之差。Ti为采样周期,K为比例系数,Td为微分时间常数,Ti为积分时间常数,K、Td、Ti三参数根据试验效果进行现场整定。
6、根据权利要求1所述的农业机械的导航控制方法,其特征在于:所述模糊逻辑方法Fuzzy是指:
(1)首先将输入变量横向跟踪误差XTE与方位偏差θe模糊化,即采用基本连续论域离散化均匀量化的方法,将连续论域中的连续值经量化因子比例变换后四舍五入变为离散论域的整数值,算法为:
XTE′=int(KxieXTE)
θe′=int(Kθeθe)
其中,输入量化因子Kxie、Kθe的确定方法是:根据试验测试结果,确定横向跟踪误差XTE、方位偏差θe的误差范围,由离散论域最大值除于相应误差范围最大值即可获得输入量化因子值;
(2)然后进行模糊控制表查询,即输入不同的离散输入量XTE′,θe′,依据模糊控制表,得到对应的清晰控制量即转向轮偏角δp
(3)最后将输出控制量去模糊,转换公式为: &delta; p &prime; = K &delta; p &delta; p , 其中输出比例因子的确定方法与上述输入量化因子的确定方法相同。
CNB2007100290184A 2007-07-04 2007-07-04 一种农业机械的导航控制方法 Active CN100504694C (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CNB2007100290184A CN100504694C (zh) 2007-07-04 2007-07-04 一种农业机械的导航控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CNB2007100290184A CN100504694C (zh) 2007-07-04 2007-07-04 一种农业机械的导航控制方法

Publications (2)

Publication Number Publication Date
CN101093396A CN101093396A (zh) 2007-12-26
CN100504694C true CN100504694C (zh) 2009-06-24

Family

ID=38991686

Family Applications (1)

Application Number Title Priority Date Filing Date
CNB2007100290184A Active CN100504694C (zh) 2007-07-04 2007-07-04 一种农业机械的导航控制方法

Country Status (1)

Country Link
CN (1) CN100504694C (zh)

Families Citing this family (57)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101833334B (zh) * 2010-02-09 2011-09-21 北京农业信息技术研究中心 拖拉机自动导航控制系统及其方法
CN101976079B (zh) * 2010-08-27 2013-06-19 中国农业大学 智能导航控制系统及方法
CN102393741B (zh) * 2011-08-25 2013-07-10 东南大学 视觉引导移动机器人的控制系统及其方法
US9020757B2 (en) * 2012-05-11 2015-04-28 Trimble Navigation Limited Path planning autopilot
FR2993376B1 (fr) * 2012-07-12 2014-07-25 Renault Sa Procede de controle de trajectoire d'un vehicule autonome a roues directrices
CN102795225B (zh) * 2012-09-10 2015-01-21 北京理工大学 利用驾驶员侧纵向控制模型检测驾驶员干扰状态的方法
CN103895698B (zh) * 2012-12-27 2015-12-23 中国科学院沈阳自动化研究所 液压式农机自动转向控制装置及控制方法
CN103914067A (zh) * 2013-01-05 2014-07-09 联想(北京)有限公司 一种控制方法及电子设备
CN103247213B (zh) * 2013-03-13 2015-04-15 北京农业智能装备技术研究中心 拖拉机自动导航实验台
CN104216405B (zh) * 2013-06-04 2017-12-29 内蒙古大学 田间机器人的导航方法及设备
TWM477598U (zh) * 2013-11-18 2014-05-01 Weistech Technology Co Ltd 具有路線校正功能之移動裝置
CN103760902B (zh) * 2014-01-14 2016-06-08 上海海事大学 一种基于模糊控制的无人水面艇直线路径跟踪方法
CN103777522B (zh) * 2014-01-21 2016-09-28 上海海事大学 基于模糊pid的无人水面艇直线跟踪方法
CN104898664A (zh) * 2015-04-10 2015-09-09 哈尔滨力盛达机电科技有限公司 一种农用全地形车转向跟踪复合控制方法
CN105147505B (zh) * 2015-09-09 2018-03-27 北京航空航天大学 一种基于预瞄跟踪的盲人行走闭环诱导控制方法
CN105137997B (zh) * 2015-09-22 2017-12-19 清华大学 水利施工振动碾压机自动驾驶系统与方法
CN105629973B (zh) * 2015-12-18 2019-05-17 广州中海达卫星导航技术股份有限公司 一种基于rtk技术的农机直线行驶的方法及装置
CN105867377B (zh) * 2016-04-13 2019-01-08 浙江理工大学 一种农业机械自动导航控制方法
CN105987696A (zh) * 2016-04-20 2016-10-05 上海雷易工业自动化有限公司 一种低成本车辆自动驾驶设计实现方法
CN106179625B (zh) * 2016-07-09 2018-05-08 青岛大学 一种对主轴电机模糊控制的研磨化工机械系统
CN106179630B (zh) * 2016-07-09 2018-04-13 青岛大学 一种筒体温度模糊控制的研磨化工机械系统
CN108121335A (zh) * 2016-11-28 2018-06-05 比亚迪股份有限公司 车辆路径跟踪控制方法、装置及车辆
CN106647770A (zh) * 2017-01-22 2017-05-10 无锡卡尔曼导航技术有限公司 用于农机无人驾驶的田间掉头路径规划及其控制方法
CN107015477B (zh) * 2017-04-05 2019-10-18 同济大学 基于状态反馈的车辆路径跟踪h∞控制方法
CN107153420B (zh) * 2017-05-25 2021-01-22 广州汽车集团股份有限公司 路径跟踪控制方法、装置及智能汽车
CN107264621B (zh) * 2017-06-15 2019-05-17 驭势科技(北京)有限公司 车辆预瞄距离计算方法、装置、介质及转向控制方法
CN107315345B (zh) * 2017-06-23 2020-10-09 华南农业大学 基于双天线gnss和预瞄追踪模型的农机自动导航控制方法
CN107272707B (zh) * 2017-08-03 2020-11-27 郑州轻工业学院 基于ipv6的无人车自动轨迹模糊pid跟随控制方法
CN109656240A (zh) * 2017-10-10 2019-04-19 雷沃重工股份有限公司 一种车辆路径跟踪控制装置、方法和车辆
CN107733321B (zh) * 2017-10-26 2020-09-25 江苏大学 一种播种机监控系统和监控方法
CN108116497B (zh) * 2017-12-08 2019-09-17 驭势科技(北京)有限公司 用于智能驾驶的转向预瞄点、转向角的确定方法及装置
CN109991974B (zh) * 2018-01-02 2022-06-03 中国移动通信有限公司研究院 自动驾驶路径跟随方法、装置及控制设备
CN110007667B (zh) * 2018-01-04 2021-06-11 中国农业机械化科学研究院 一种履带拖拉机及其路径跟踪控制方法和系统
CN108388241B (zh) * 2018-01-05 2021-02-12 广州科语机器人有限公司 移动机器人的路径跟踪方法
CN108445877A (zh) * 2018-02-06 2018-08-24 武汉理工大学 基于移动3g/4g网络环境下的仿生运动控制系统与控制方法
CN110308717B (zh) 2018-03-27 2020-12-22 广州汽车集团股份有限公司 控制自主式移动机器移动的方法、装置、机器及存储介质
CN108549371A (zh) * 2018-03-28 2018-09-18 安徽农业大学 一种高地隙植保机械路径跟踪方法及系统
CN108415435B (zh) * 2018-04-04 2021-08-31 上海华测导航技术股份有限公司 一种农业机械圆曲线自动驾驶控制方法
CN108490943B (zh) * 2018-04-04 2021-08-31 上海华测导航技术股份有限公司 一种农业机械自适应曲线自动驾驶控制方法
CN108609014B (zh) * 2018-05-04 2021-01-05 奇瑞汽车股份有限公司 智能车辆的横向控制方法及装置
CN109000660A (zh) * 2018-06-26 2018-12-14 苏州路特工智能科技有限公司 基于超宽带定位的全自动压路机施工路径规划方法
CN109283923A (zh) * 2018-07-02 2019-01-29 清博(昆山)智能科技有限公司 一种拖拉机自动转向系统的建模系统
CN109444171A (zh) * 2018-09-18 2019-03-08 山东理工大学 基于无人机的一体化桥梁检测方法
CN109407517B (zh) * 2018-12-19 2021-12-14 驭势科技(北京)有限公司 一种双轴拖车系统路径跟踪方法、装置、系统及存储介质
CN110515385A (zh) * 2019-09-09 2019-11-29 金鹏电子信息机器有限公司 一种移动机器人的路径跟踪方法及装置
CN110502020B (zh) * 2019-09-18 2022-05-17 北京工业大学 一种农机的自动导航控制方法
CN110840344B (zh) * 2019-09-30 2021-08-06 湖南格兰博智能科技有限责任公司 一种基于pid控制算法的扫地机器人沿线行走控制系统及其控制方法
CN111158377B (zh) * 2020-01-15 2021-04-27 浙江吉利汽车研究院有限公司 一种用于车辆的横向控制方法、系统及车辆
CN111766873B (zh) * 2020-06-12 2023-12-05 广州极飞科技股份有限公司 作业设备的引导方法及相关装置
CN112009453B (zh) * 2020-07-30 2021-06-29 北京农业智能装备技术研究中心 果园作业车辆的行走控制方法及装置
CN112947433B (zh) * 2021-02-03 2023-05-02 中国农业大学 一种果园移动机器人及其自主导航方法
CN113406956B (zh) * 2021-05-17 2022-11-18 中国农业大学 一种玉米联合收获机偏差检测传感装置及自动对行方法
CN113419523A (zh) * 2021-06-08 2021-09-21 无锡卡尔曼导航技术有限公司 一种适用于无角度传感器的农机获得前轮期望角度的方法
CN114237033A (zh) * 2021-12-14 2022-03-25 山东大学 一种车辆路径跟踪控制方法、控制器及控制系统
CN114234983B (zh) * 2021-12-23 2023-09-12 仲继锋 一种行走式农田移动平台机器人导航方法
CN114675659B (zh) * 2022-05-28 2022-09-23 广州市盛望信息科技有限公司 一种农用机器人运动控制方法及系统
CN115326054A (zh) * 2022-08-24 2022-11-11 中国热带农业科学院农业机械研究所 一种履带式农用车的自动导航方法

Non-Patent Citations (9)

* Cited by examiner, † Cited by third party
Title
农用智能移动作业平台模型的研制. 罗锡文,区颖刚,赵祚喜,赵新,张智刚,张志斌,卢广宇,李俊岭.农业工程学报,第21卷第2期. 2005
农用智能移动作业平台模型的研制. 罗锡文,区颖刚,赵祚喜,赵新,张智刚,张志斌,卢广宇,李俊岭.农业工程学报,第21卷第2期. 2005 *
基于地图匹配的定位数据模糊校正算法研究. 罗锡文,赵祚喜,周志艳,张智刚.中国农业工程学会2005年学术年会论文集. 2005
基于地图匹配的定位数据模糊校正算法研究. 罗锡文,赵祚喜,周志艳,张智刚.中国农业工程学会2005年学术年会论文集. 2005 *
应用模糊自适应PID和预瞄策略的自主车辆转向控制. 王京起,陈慧岩,郑培.汽车工程,第Vol. 25卷第2003年第4期. 2003
应用模糊自适应PID和预瞄策略的自主车辆转向控制. 王京起,陈慧岩,郑培.汽车工程,第Vol.25卷第2003年第4期. 2003
应用模糊自适应PID和预瞄策略的自主车辆转向控制. 王京起,陈慧岩,郑培.汽车工程,第Vol. 25卷第2003年第4期. 2003 *
汽车驾驶员自适应模糊PID控制模型. 马军,贺岩松,李兴泉,徐中明,张志飞.自动控制与检测,第2007卷第02期. 2007
汽车驾驶员自适应模糊PID控制模型. 马军,贺岩松,李兴泉,徐中明,张志飞.自动控制与检测,第2007卷第02期. 2007 *

Also Published As

Publication number Publication date
CN101093396A (zh) 2007-12-26

Similar Documents

Publication Publication Date Title
CN100504694C (zh) 一种农业机械的导航控制方法
EP0772807B1 (en) Automatic steering system for an unmanned vehicle
CN101082493B (zh) 一种农业机械导航的组合定位方法
CN112129297B (zh) 一种多传感器信息融合的自适应校正室内定位方法
CN101221447A (zh) 一种机械自动转向控制方法
CN103914068A (zh) 一种基于栅格地图的服务机器人自主导航方法
Amer et al. Path tracking controller of an autonomous armoured vehicle using modified Stanley controller optimized with particle swarm optimization
CN103324196A (zh) 基于模糊逻辑的多机器人路径规划与协调避碰方法
CN108036792A (zh) 一种用于移动机器人的里程计与测量位姿的数据融合方法
Roselli et al. H∞ control with look-ahead for lane keeping in autonomous vehicles
CN106969784A (zh) 一种并发建图定位与惯性导航的组合误差融合系统
CN108490943B (zh) 一种农业机械自适应曲线自动驾驶控制方法
Lhomme-Desages et al. Trajectory control of a four-wheel skid-steering vehicle over soft terrain using a physical interaction model
Ridley et al. Autonomous control of an underground mining vehicle
Yang et al. Path tracking control of an articulated road roller with sideslip compensation
CN104132664A (zh) 一种农用履带机器人滑动量的估计方法
CN114489036B (zh) 一种基于slam的室内机器人导航控制方法
Bakirci et al. Improving position-time trajectory accuracy in vehicle stop-and-go scenarios by using a mobile robot as a testbed
Jiao et al. A sliding parameter estimation method based on UKF for agricultural tracked robot
Wang et al. Research on mobile robot localization and mapping method for underground long-narrow tunnels
Latif et al. Outdoor tests of autonomous navigation system based on two different reference points of PurePursuit algorithm for 10-ton articulated vehicle.
CN209833810U (zh) 基于rtk的六履带车辆自适应行走系统
Areerob et al. Iterative Learning Control for Lateral Tracking With Repeated Path in Autonomous Vehicles for Dynamic Environments
Babu et al. Trajectory Following using Nonlinear Model Predictive Control and 3D Point-Cloud-based Localization for Autonomous Driving
Solaque G Design and test of a path tracking controller to a high capability agricultural vehicle

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
C14 Grant of patent or utility model
GR01 Patent grant