CN110502020A - 一种农机的自动导航控制方法 - Google Patents
一种农机的自动导航控制方法 Download PDFInfo
- Publication number
- CN110502020A CN110502020A CN201910879319.9A CN201910879319A CN110502020A CN 110502020 A CN110502020 A CN 110502020A CN 201910879319 A CN201910879319 A CN 201910879319A CN 110502020 A CN110502020 A CN 110502020A
- Authority
- CN
- China
- Prior art keywords
- agricultural machinery
- data
- error
- prediction
- control amount
- 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.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
- G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
Abstract
本发明公开了一种农机的自动导航控制方法,该方法包括如下步骤,预处理卫星数据,获得追踪控制量,预测运动状态,判断轨迹。本发明提出的控制方法综合了几种现有导航控制方法的优点,弥补了其不足。相较于线性模型控制方法和最优控制方法,模糊+预测纯追踪控制方法能够通过内置模糊和预测模块,对农作业过程中由于侧滑和颠簸等随机干扰产生的出轨现象进行预测和调整,拥有更优的适应性;相较于模糊控制方法,模糊+预测纯追踪控制方法不单单依赖于模糊控制规则的控制效果,还通过线性运动模型保障其追踪效果,在追踪精度上拥有更大的优势。
Description
技术领域
本发明涉及农用机械导航控制技术领域,尤其涉及一种改进的用于农用机械路径跟踪的纯追踪控制方法。
背景技术
近年来,随着卫星定位精度不断提高,导航控制方法的多样化和智能化,高精度农机自动导航装置的实现成为了现实。通过在传统农业机械上配备自动导航装置,农机能够根据配置信息,自动进行农作业,节省人力、时间,提高作业质量。在卫星定位领域,现有技术已经能够达到厘米级别精度,满足农业机械自动导航的需求;而现有导航控制方法,因其设计之初面向的对象多为公路车辆,因此并不十分适用于农机作业场景。现有的导航控制方法,常用的有线性模型控制方法、模糊控制方法和最优控制方法。然而在农机自动导航领域下,上述导航控制方法均存在一些弊端。线性模型控制方法和最优控制方法,需要对受控对象建立运动学模型和动力学模型,然而在农作业环境下,广泛存在农机的侧滑、颠簸现象,难以建立准确描述运动状态的模型,控制效果不佳;模糊控制方法不依赖车辆模型,但是需要输入专家经验信息生成控制规则,跟踪误差大,响应速度慢。
公开号为CN101093396A的发明公开了一种农业机械自动导航控制方法,该方法结合了模糊控制方法和PID控制方法,在综合误差较大的情况下,使用模糊控制方法控制受控对象进行路径追踪,而在综合误差较小的情况下,使用PID控制方法控制受控对象进行路径追踪。虽然使用PID控制方法能够弥补单独使用模糊控制方法时出现的跟踪误差大的问题,但是PID控制方法本身需要进行参数整定,过程繁琐复杂。为解决上述问题,公开号为CN105867377A的发明公开了一种综合模糊控制方法和纯追踪控制方法的综合型控制方法。该方法同时使用模糊控制方法和纯追踪控制方法计算控制量,将两者控制量综合后获得综合控制量,对受控对象进行控制。由于纯追踪控制方法是一种仿生型控制方法,既不需要提供专家经验信息,也不要对受控对象运动状态进行建模,因此在农作业环境下具有优良的效果,弥补了模糊控制方法本身跟踪误差大的弊端。但是,该方法并没有解决模糊控制方法自身响应速度慢的缺陷,在追踪曲率突变的路径或者意外出轨时,需要较多时间才能重新追踪回规划路径。
发明内容
为了克服上述技术方法的缺点,本发明的目的在于提供一种应用于农机自动驾驶领域的改进的模糊+预测纯追踪控制方法。该方法综合农机作业中的位置误差和航向角误差,计算出前轮转角控制量,控制农机改变前轮转角,进行路径追踪。追踪精度高,响应速度快。
为了达到上述目的,本发明采用的技术方案如下:
步骤1,预处理卫星数据。
设定当前控制周期为t周期,通过北斗卫星接收机,农机自动导航装置能够获得农机运动状态数据。随后,对农机运动状态数据进行预处理,预处理包括统一数据单位并消除数据抖动。最后,将处理后的数据输出到步骤2。
预处理步骤如下:
步骤1.1,分离航向角数据和经纬度数据。
通过北斗卫星接收机接收到的原始定位数据为GPRMC数据,该GPRMC数据为字符串类型,不能直接作为数值被算法和接口调用,因此需要对GPRMC数据中表示航向角和经纬度信息的数据进行提取,并转换为数值形式,最后将转换后的数据分别输出到步骤1.2和步骤1.3。
步骤1.2,稳定航向角数据。
由于农田作业环境复杂,所以卫星接收机接收到的航向角数据往往存在抖动和漂移的现象,因此通过设定阈值的方式,忽略S100输入的改变量较小的数据,只保留有效数据,保证数据稳定性,最后将稳定化处理后的航向角数据输入步骤1.4。
步骤1.3,换算经纬度数据为平面坐标数据。
北斗卫星接收机接收到的原始数据为经纬度数据,不容易进行可视化展示和算法计算,因此需要将其转换为平面坐标数据。通过米勒投影法,将S100输入的经纬度数据通过投影公式,转换为平面坐标数据,最后将换算后的坐标数据输入步骤1.4。
步骤1.4,存储航向角数据和平面坐标数据。
为了方便后续模块使用处理后的航向角数据和平面坐标数据,将由步骤1.2获得的航向角数据和由步骤1.3获得的平面坐标数据存储在寄存器中,方便步骤2进行调用。
步骤2,获得追踪控制量。
由步骤1已知t周期农机运动状态,结合已知规划路径点集{T1,T2,T3...Tk...Tn},建立几何学模型,即可获得当前周期农机航向角误差和位置误差,再将误差数据输入模糊纯追踪控制器,即可获得农机前轮转角控制量,进而输入到步骤3中,具体步骤如下:
步骤2.1,计算航向角误差和位置误差。
由步骤1已知当前周期农机运动状态数据,包括航向角数据和位置数据,通过与规划路径点集中每个点建立几何关系,可确定追踪点,进而计算出农机和追踪点之间的航向角误差和位置误差。最后,将误差数据输入到步骤2.2中。
步骤2.2,通过误差数据,结合模糊规则,求前视距离。
通过将步骤2.1计算出的误差数据,输入到由专家经验总结而成的模糊规则中,即可获得前视距离。最后,将前视距离输出到步骤2.3中。
步骤2.3,重新确定追踪点,计算航向角误差和位置误差。
参考S210中获得的前视距离,重复步骤2.1,重新确定追踪点,重新计算农机和追踪点之间航向角误差和位置误差,将误差数据输入到步骤2.4中。
步骤2.4,将误差数据输入纯追踪控制器,计算农机前轮转角控制量。
将由步骤2.3中获得的误差数据,输入到纯追踪控制器的控制公式中,即可获得当前周期的农机前轮转角控制量。最后,将该控制量输入到步骤3中。
步骤3,预测运动状态。
通过步骤2可知t周期农机前轮转角控制量,通过将控制量输入农机运动学模型,可获得t+1周期农机运动状态,结合已知规划路径点集{T1,T2,T3...Tk...Tn},即可获得预测农机航向角误差和预测位置误差,将预测误差数据输入模糊纯追踪控制器,即可获得t+1周期农机前轮转角控制量,具体步骤如下:
步骤3.1,预测t+1周期农机运动状态。
根据农机实际运动情况,能够事先对农机运动状况进行建模,得到农机运动学模型。该模型描述了农机当前位置、速度和航向角之间的关系。因此,通过将农机当前位置、农机当前运动速度和航向角信息输入模型,即可计算出农机下一周期运动状态数据。
通过将步骤2求得的t周期农机前轮转角控制量输入到农机运动学模型中,可以预测t+1周期的农机运动状态数据。将t+1周期的农机运动状态数据输入到S310中。
步骤3.2,计算预测航向角误差和预测位置误差。
由步骤3.1已知预测农机运动状态数据,包括预测航向角数据和预测位置数据,通过与路径点集中每个点建立几何关系,确定预测追踪点,进而计算出农机和预测追踪点之间的预测航向角误差和预测位置误差。最后,将预测误差数据输入到步骤3.3中。
步骤3.3,通过预测误差数据,结合模糊规则,求预测前视距离。
将由步骤3.2中获得的预测误差数据,输入到由专家经验总结而成的模糊规则中,即可获得预测前视距离,将预测前视距离数据输入到步骤3.4中。
步骤3.4,重新确定预测追踪点,计算预测航向角误差和预测位置误差。
参考步骤3.3求得的预测前视距离,重复步骤3.2,重新确定预测追踪点,重新计算农机和预测追踪点之间预测航向角误差和预测位置误差。最后,将预测误差数据输入到步骤3.5中
步骤3.5,将预测误差数据输入纯追踪控制器,计算预测农机前轮转角控制量。
通过将步骤3.5中获得的预测误差数据输入到纯追踪控制器的控制公式中,即可获得预测农机前轮转角控制量,将预测农机前轮转角控制量输入步骤4。
步骤4,判断轨迹。
结合步骤2和步骤3获得的t周期位置误差数据和t+1周期预测位置误差数据,通过计算位置偏离系数,能够判断农机由t周期到t+1周期运动过程中,是否出现出轨现象,进而综合t周期农机前轮转角控制量和t+1周期预测农机前轮转角控制量,获得综合农机前轮转角控制量。具体步骤如下:
步骤4.1,通过计算位置偏离系数,可以判断出农机在t周期到t+1周期运行过程中,是否发生出轨现象。若没有发生出轨现象,则最终农机前轮转角控制量为步骤2计算出的农机前轮转角控制量,步骤结束;若发生出轨情况,则进入步骤4.2.
步骤4.2,综合步骤2的农机前轮转角控制量和步骤3的预测农机前轮转角控制量,获得综合控制量。
通过将步骤2求得的农机前轮转角控制量和步骤3求得的预测农机前轮转角控制量,输入到系数求和公式中进行求和,可以获得综合控制量。与此同时,也可以根据本发明中的方法实际对农机出轨现象的抑制效果,调节系数值。
本发明提出的控制方法综合了几种现有导航控制方法的优点,弥补了其不足。相较于线性模型控制方法和最优控制方法,模糊+预测纯追踪控制方法能够通过内置模糊和预测模块,对农作业过程中由于侧滑和颠簸等随机干扰产生的出轨现象进行预测和调整,拥有更优的适应性;相较于模糊控制方法,模糊+预测纯追踪控制方法不单单依赖于模糊控制规则的控制效果,还通过线性运动模型保障其追踪效果,在追踪精度上拥有更大的优势。
附图说明
图1本发明的农机的自动导航控制方法实施例的流程图;
图2本发明的农机的自动导航控制方法实施例步骤S10的子流程图;
图3本发明的农机的自动导航控制方法实施例步骤S20的子流程图;
图4本发明的农机的自动导航控制方法实施例步骤S200所求误差数据所需几何学模型示意图;
图5本发明的农机的自动导航控制方法实施例步骤S230所述纯追踪模型示意图;
图6本发明的农机的自动导航控制方法实施例步骤S30的子流程图;
图7本发明的农机的自动导航控制方法实施例步骤S210、S320计算前视距离所使用模糊规则表;
图8本发明的农机的自动导航控制方法实施例步骤S210、S320计算前视距离所使用模糊规则表可视化图形示意图;
图9本发明的农机的自动导航控制方法实施例步骤S300所求农机状态数据所需运动学模型示意图;
图10本发明的农机的自动导航控制方法实施例步骤S40的子流程图;
具体实施方式
为了能够更加清楚的说明本发明的方法流程,下面结合具体实施例进行进一步描述,所说明的实施例仅用于说明本发明的技术方案,实施例设置的参数数值并非限定本发明。
本发明实施方式的流程图如图1所示,包括以下步骤:
步骤S10,预处理卫星数据;
步骤S20,获得追踪控制量;
步骤S30,预测运动状态;
步骤S40,判断轨迹。
实施方式的预处理卫星数据步骤S10还包括以下步骤,实施步骤如图2所示:
步骤S100,分离t周期航向角数据Y(t)和经纬度数据Cs(t)。
北斗卫星接收机默认发送数据为GPRMC格式字符串,针对其中需要的航向角数据和经纬度数据,通过按位取字符的方式,取得航向角数据和经纬度数据的字符串,再经过字符串至数值的换算,即可获得t周期航向角数据Y(t)和经纬度数据Cs(t)。将Y(t)和Cs(t)分别输入到S110和S120中。
步骤S110,对航向角数据Y(t)进行稳定化处理。
考虑到农机航向角在农机作业过程中,存在漂移抖动情况,因此,若采集到的航向角数据存在关系|Y(t)-Y(t-1)|<1,则认为该航向角数据的偏移为误差偏移,Y(t)=Y(t-1)。以此方式,即可对由步骤S100中获得的航向角数据Y(t)进行稳定化处理,并将处理后的数据输入S130中。
步骤S120,将经纬度数据Cs(t)转换为平面坐标数据Cp(t)。
通过米勒投影法投影公式:
Cp=f(Cs,Le) (1)
即可将由步骤S100获得的经纬度数据Cs(t)转换为平面坐标数据Cp(t),并将转换后的数据输入S130中,其中Le为地球半径,取值约为6371000米。
步骤S130,存储航向角数据Y(t)和平面坐标数据Cp(t)。
将在S110和S120中预处理后的航向角数据Y(t)和平面坐标数据Cp(t)存储在农机自动导航装置的寄存器中,当农机自动导航装置进行定位及导航工作时,即可从寄存器中调用t周期数据Y(t)和Cp(t)。最后,将Y(t)和Cp(t)输入到S20中。
实施方式的获得追踪控制量步骤S20还包括以下步骤,实施步骤如图3所示:
步骤S200,计算t周期农机的航向角误差Ey(t)和位置误差Ep(t)。
如图4所示,以XOY建立坐标系。其中,P为农机当前位置,由步骤S10已知农机当前航向角数据Y(t)和平面坐标数据Cp(t)。由P点至规划路径建立几何关系,求P点至Tk所有长度,取最小值对应规划路径点Tm(t)。由Tm(t)向规划路径正方向取距离为l的规划路径点Tg(t),该点即为追踪点,此时l的值默认为2米。建立P点至Tg(t)点的几何关系,位置误差:
向量与Y坐标轴正半轴夹角为追踪路径误差角度α,因此通过公式:
Ey(t)=α-Y(t) (3)
即可获得t周期农机航向角误差Ey(t)。最后,将航向角误差Ey(t)和位置误差Ep(t)输入到S210中。
步骤S210,通过模糊规则,计算合适前视距离l(t)。
由步骤S200获得的t周期农机航向角误差Ey(t)和位置误差Ep(t)可以作为模糊规则参数,用来计算前视距离。如图8所示,通过事先将专家经验总结为模糊规则,即可通过将Ey(t)和Ep(t)输入到规则中,获得合适的前视距离l(t)。最后,将前视距离l(t)输入到S220中。
步骤S220,通过S210计算出的前视距离l(t),重新计算航向角误差Ey(t)和位置误差Ep(t)。
由步骤S200获得的Tm位置,向规划路径正方向取距离为l(t)的规划路径点Tg(t),该点为新确定的追踪点。建立P点至Tg(t)点的几何关系,位置误差:
向量与Y坐标轴正半轴夹角为追踪路径误差角度α,因此通过公式:
Ey(t)=α-Y(t) (5)
即可获得t周期农机航向角误差Ey(t)。最后,将航向角误差Ey(t)和位置误差Ep(t)输入到S230中。
步骤S230,通过纯追踪控制器,获得农机前轮转角控制量Cy(t)。
如图5所示,农机运动学模型可以被等效为两轮自行车模型,其中M点为农机进行追踪点追踪时,所行驶追踪弧线的圆心、R为圆的半径、m为农机轴距。
因此存在几何关系:
由于m相比于Ep可以忽略不计,因此有:
联立公式(6)和(7),可得:
所以:
因此,通过将S220获得的航向角误差Ey(t)和位置误差Ep(t)代入公式(9)中,求得t周期农机前轮转角控制量为:
最后,将Cy(t)输入到S30中。
实施方式的预测运动状态步骤S30还包括以下步骤,实施步骤如图6所示:
步骤S300,通过输入步骤S230求得的t周期农机前轮转角控制量Cy(t)到农机运动学模型中,如图7所示,由于满足公式:
Y(t+1)=Y(t)+Cy(t) (11)
Cpx(t+1)=Cpx(t)+vΔt cos Y (12)
Cpy(t+1)=Cpy(t)+vΔt sin Y (13)
其中v为农机当前运动速度,Δt为控制周期时间。通过上述公式,可以获得t+1周期的农机运动状态数据,包括航向角数据Y(t+1)和平面坐标数据Cp(t+1)。最后,将航向角数据Y(t+1)和平面坐标数据Cp(t+1)输入S310。
步骤S310,计算t+1周期农机航向角误差Ey(t+1)和位置误差Ep(t+1)。
以XOY建立坐标系。其中,P为农机当前位置,由步骤S300已知农机当前航向角数据Y(t+1)和平面坐标数据Cp(t+1)。由P点至规划路径建立几何关系,求P点至Tk所有长度,取最小值对应规划路径点Tm(t+1)。由Tm(t+1)向规划路径正方向取距离为l的规划路径点Tg(t+1),该点即为追踪点,此时l的值默认为2米。建立P点至Tg(t+1)点的几何关系,位置误差:
向量与Y坐标轴正半轴夹角为追踪路径误差角度α(t+1),因此通过公式:
Ey(t+1)=α(t+1)-Y(t+1) (15)
即可获得t+1周期农机航向角误差Ey(t+1)。最后,将航向角误差Ey(t+1)和位置误差Ep(t+1)输入S320。
步骤S320,通过模糊规则,计算合适前视距离l(t+1)。
由步骤S310获得的t+1周期农机航向角误差Ey(t+1)和位置误差Ep(t+1)可以作为模糊规则参数,用来计算前视距离。如图8所示,通过事先将专家经验总结为模糊规则,即可通过将Ey(t+1)和Ep(t+1)输入规则中,获得合适的前视距离l(t+1).最后,将前视距离l(t+1)输入S330。
步骤S330,通过S320获得的前视距离l(t+1),重新计算航向角误差Ey(t+1)和位置误差Ep(t+1)。
由步骤S310获得的Tm位置,向规划路径正方向取距离为l(t+1)的规划路径点Tg(t+1),该点为新确定的追踪点。建立P点至Tg(t+1)点的几何关系,位置误差:
向量与Y坐标轴正半轴夹角为追踪路径误差角度α(t+1),因此通过公式:
Ey(t+1)=α(t+1)-Y(t+1) (17)
即可获得t+1周期农机航向角误差Ey(t+1)。最后,将航向角误差Ey(t+1)和位置误差Ep(t+1)输入S340。
步骤S340,通过纯追踪控制器,获得农机前轮转角控制量Cy(t+1)。
几何关系确立方法同步骤S230相同,通过将由S330获得的航向角误差数据Ey(t)和位置误差数据Ep(t)代入公式(9)中,求得t+1周期农机前轮转角控制量为:
最后,将Cy(t+1)输入S40。
实施方式的预测运动状态步骤S40还包括以下步骤,实施步骤如图10所示:
步骤S400,根据公式:
K(t)=Ep(t)*Ep(t+1) (19)
求得农机由t周期到t+1周期运动过程中的位置偏离系数,若K(t)>0,则说明根据预测结果,农机在t+1周期没有出轨,最终控制量为步骤S230计算出的t周期农机前轮转角控制量Cy(t),步骤S40结束;若K(t)<0,则说明农机追踪过程中出轨了,需要对控制量进行修正,进入步骤S410。
步骤S410,若预测到农机将在t周期到t+1周期运动过程中出现出轨现象,则最终农机前轮转角控制量根据公式:
Cy'(t)=(1-ξ)Cy(t)+ξCy(t+1) (20)
其中ζ默认取值为0.5,若在实际测试中,出现出轨抑制效果不良现象,则应该增大ζ,若出现出轨抑制效果过度现象,则应该减少ζ。
Claims (5)
1.一种农机的自动导航控制方法,其特征在于:该方法的实施过程如下,
步骤1,预处理卫星数据;
设定当前控制周期为t周期,通过北斗卫星接收机,农机自动导航装置能够获得农机运动状态数据;随后,对农机运动状态数据进行预处理,预处理包括统一数据单位并消除数据抖动;最后,将处理后的数据输出到步骤2;
步骤2,获得追踪控制量;
由步骤1已知t周期农机运动状态,结合已知规划路径点集{T1,T2,T3...Tk...Tn},建立几何学模型,即可获得当前周期农机航向角误差和位置误差,再将误差数据输入模糊纯追踪控制器,即可获得农机前轮转角控制量,进而输入到步骤3中,
步骤3,预测运动状态;
通过步骤2可知t周期农机前轮转角控制量,通过将控制量输入农机运动学模型,可获得t+1周期农机运动状态,结合已知规划路径点集{T1,T2,T3...Tk...Tn},即可获得预测农机航向角误差和预测位置误差,将预测误差数据输入模糊纯追踪控制器,即可获得t+1周期农机前轮转角控制量,
步骤4,判断轨迹;
结合步骤2和步骤3获得的t周期位置误差数据和t+1周期预测位置误差数据,通过计算位置偏离系数,能够判断农机由t周期到t+1周期运动过程中,是否出现出轨现象,进而综合t周期农机前轮转角控制量和t+1周期预测农机前轮转角控制量,获得综合农机前轮转角控制量。
2.根据权利要求1所述的一种农机的自动导航控制方法,其特征在于:预处理步骤如下,
步骤1.1,分离航向角数据和经纬度数据;
通过北斗卫星接收机接收到的原始定位数据为GPRMC数据,该GPRMC数据为字符串类型,不能直接作为数值被算法和接口调用,因此需要对GPRMC数据中表示航向角和经纬度信息的数据进行提取,并转换为数值形式,最后将转换后的数据分别输出到步骤1.2和步骤1.3;
步骤1.2,稳定航向角数据;
由于农田作业环境复杂,所以卫星接收机接收到的航向角数据往往存在抖动和漂移的现象,因此通过设定阈值的方式,忽略S100输入的改变量较小的数据,只保留有效数据,保证数据稳定性,最后将稳定化处理后的航向角数据输入步骤1.4;
步骤1.3,换算经纬度数据为平面坐标数据;
北斗卫星接收机接收到的原始数据为经纬度数据,不容易进行可视化展示和算法计算,因此需要将其转换为平面坐标数据;通过米勒投影法,将S100输入的经纬度数据通过投影公式,转换为平面坐标数据,最后将换算后的坐标数据输入步骤1.4;
步骤1.4,存储航向角数据和平面坐标数据;
为了方便后续模块使用处理后的航向角数据和平面坐标数据,将由步骤1.2获得的航向角数据和由步骤1.3获得的平面坐标数据存储在寄存器中,方便步骤2进行调用。
3.根据权利要求1所述的一种农机的自动导航控制方法,其特征在于:步骤2的具体步骤如下,
步骤2.1,计算航向角误差和位置误差;
由步骤1已知当前周期农机运动状态数据,包括航向角数据和位置数据,通过与规划路径点集中每个点建立几何关系,可确定追踪点,进而计算出农机和追踪点之间的航向角误差和位置误差;最后,将误差数据输入到步骤2.2中;
步骤2.2,通过误差数据,结合模糊规则,求前视距离;
通过将步骤2.1计算出的误差数据,输入到由专家经验总结而成的模糊规则中,即可获得前视距离;最后,将前视距离输出到步骤2.3中;
步骤2.3,重新确定追踪点,计算航向角误差和位置误差;
参考S210中获得的前视距离,重复步骤2.1,重新确定追踪点,重新计算农机和追踪点之间航向角误差和位置误差,将误差数据输入到步骤2.4中;
步骤2.4,将误差数据输入纯追踪控制器,计算农机前轮转角控制量;
将由步骤2.3中获得的误差数据,输入到纯追踪控制器的控制公式中,即可获得当前周期的农机前轮转角控制量;最后,将该控制量输入到步骤3中。
4.根据权利要求1所述的一种农机的自动导航控制方法,其特征在于:步骤3的具体步骤如下,
步骤3.1,预测t+1周期农机运动状态;
根据农机实际运动情况,能够事先对农机运动状况进行建模,得到农机运动学模型;该模型描述了农机当前位置、速度和航向角之间的关系;因此,通过将农机当前位置、农机当前运动速度和航向角信息输入模型,即可计算出农机下一周期运动状态数据;
通过将步骤2求得的t周期农机前轮转角控制量输入到农机运动学模型中,可以预测t+1周期的农机运动状态数据;将t+1周期的农机运动状态数据输入到S310中;
步骤3.2,计算预测航向角误差和预测位置误差;
由步骤3.1已知预测农机运动状态数据,包括预测航向角数据和预测位置数据,通过与路径点集中每个点建立几何关系,确定预测追踪点,进而计算出农机和预测追踪点之间的预测航向角误差和预测位置误差;最后,将预测误差数据输入到步骤3.3中;
步骤3.3,通过预测误差数据,结合模糊规则,求预测前视距离;
将由步骤3.2中获得的预测误差数据,输入到由专家经验总结而成的模糊规则中,即可获得预测前视距离,将预测前视距离数据输入到步骤3.4中;
步骤3.4,重新确定预测追踪点,计算预测航向角误差和预测位置误差;
参考步骤3.3求得的预测前视距离,重复步骤3.2,重新确定预测追踪点,重新计算农机和预测追踪点之间预测航向角误差和预测位置误差;最后,将预测误差数据输入到步骤3.5中
步骤3.5,将预测误差数据输入纯追踪控制器,计算预测农机前轮转角控制量;
通过将步骤3.5中获得的预测误差数据输入到纯追踪控制器的控制公式中,即可获得预测农机前轮转角控制量,将预测农机前轮转角控制量输入步骤4。
5.根据权利要求1所述的一种农机的自动导航控制方法,其特征在于:步骤4的具体步骤如下,
步骤4.1,通过计算位置偏离系数,可以判断出农机在t周期到t+1周期运行过程中,是否发生出轨现象;若没有发生出轨现象,则最终农机前轮转角控制量为步骤2计算出的农机前轮转角控制量,步骤结束;若发生出轨情况,则进入步骤4.2.
步骤4.2,综合步骤2的农机前轮转角控制量和步骤3的预测农机前轮转角控制量,获得综合控制量;
通过将步骤2求得的农机前轮转角控制量和步骤3求得的预测农机前轮转角控制量,输入到系数求和公式中进行求和,可以获得综合控制量;与此同时,也可以根据本发明中的方法实际对农机出轨现象的抑制效果,调节系数值。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910879319.9A CN110502020B (zh) | 2019-09-18 | 2019-09-18 | 一种农机的自动导航控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910879319.9A CN110502020B (zh) | 2019-09-18 | 2019-09-18 | 一种农机的自动导航控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110502020A true CN110502020A (zh) | 2019-11-26 |
CN110502020B CN110502020B (zh) | 2022-05-17 |
Family
ID=68592102
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910879319.9A Active CN110502020B (zh) | 2019-09-18 | 2019-09-18 | 一种农机的自动导航控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110502020B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113063443A (zh) * | 2021-03-19 | 2021-07-02 | 四川大学 | 基于实际导航性能的飞行误差实时评估方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101093396A (zh) * | 2007-07-04 | 2007-12-26 | 华南农业大学 | 一种农业机械的导航控制方法 |
CN101221447A (zh) * | 2008-01-18 | 2008-07-16 | 中国农业大学 | 一种机械自动转向控制方法 |
CN104699085A (zh) * | 2013-12-09 | 2015-06-10 | 安阳市鑫地农机有限公司 | 一种循迹自动行走田园管理机 |
CN107315345A (zh) * | 2017-06-23 | 2017-11-03 | 华南农业大学 | 基于双天线gnss和预瞄追踪模型的农机自动导航控制方法 |
US20190154546A1 (en) * | 2016-10-27 | 2019-05-23 | Shanghai Huace Navigation Technology Ltd | Automatic calibration method of an angle sensor for an automatic drive control system of a farm machine |
CN109857098A (zh) * | 2018-12-11 | 2019-06-07 | 东南大学 | 一种基于mpc的自动驾驶农机轨迹跟踪系统及方法 |
-
2019
- 2019-09-18 CN CN201910879319.9A patent/CN110502020B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101093396A (zh) * | 2007-07-04 | 2007-12-26 | 华南农业大学 | 一种农业机械的导航控制方法 |
CN101221447A (zh) * | 2008-01-18 | 2008-07-16 | 中国农业大学 | 一种机械自动转向控制方法 |
CN104699085A (zh) * | 2013-12-09 | 2015-06-10 | 安阳市鑫地农机有限公司 | 一种循迹自动行走田园管理机 |
US20190154546A1 (en) * | 2016-10-27 | 2019-05-23 | Shanghai Huace Navigation Technology Ltd | Automatic calibration method of an angle sensor for an automatic drive control system of a farm machine |
CN107315345A (zh) * | 2017-06-23 | 2017-11-03 | 华南农业大学 | 基于双天线gnss和预瞄追踪模型的农机自动导航控制方法 |
CN109857098A (zh) * | 2018-12-11 | 2019-06-07 | 东南大学 | 一种基于mpc的自动驾驶农机轨迹跟踪系统及方法 |
Non-Patent Citations (2)
Title |
---|
何卿等: "接触式拖拉机导航控制系统", 《农业机械学报》 * |
魏爽等: "基于GNSS的农机自动导航路径搜索及转向控制", 《农业工程学报》 * |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113063443A (zh) * | 2021-03-19 | 2021-07-02 | 四川大学 | 基于实际导航性能的飞行误差实时评估方法 |
CN113063443B (zh) * | 2021-03-19 | 2023-12-01 | 四川大学 | 基于实际导航性能的飞行误差实时评估方法 |
Also Published As
Publication number | Publication date |
---|---|
CN110502020B (zh) | 2022-05-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110001637B (zh) | 一种基于多点跟踪的无人驾驶汽车路径跟踪控制装置及控制方法 | |
CN108622093A (zh) | 智能车辆的车道保持控制方法及装置 | |
Milanés et al. | Autonomous vehicle based in cooperative GPS and inertial systems | |
CN109358621B (zh) | 自动驾驶汽车轨迹跟踪控制方法 | |
US10860035B2 (en) | Travel history storage method, method for producing travel path model, method for estimating local position, and travel history storage device | |
CN111006667B (zh) | 高速场景下的自动驾驶轨迹生成系统 | |
CN111578957B (zh) | 基于三维点云地图定位的智能车纯追踪循迹方法 | |
CN108609014A (zh) | 智能车辆的横向控制方法及装置 | |
CN109405846A (zh) | 一种自适应调整前视距离与延迟参数的路径追踪方法 | |
CN108646743A (zh) | 用于移动载具的路径跟踪方法 | |
CN110502009A (zh) | 基于航向预估的无人驾驶车辆路径跟踪控制方法 | |
CN106476883A (zh) | 车辆的行驶控制装置 | |
CN110006419A (zh) | 一种基于预瞄的车辆轨迹跟踪点确定方法 | |
CN105987696A (zh) | 一种低成本车辆自动驾驶设计实现方法 | |
CN113359710B (zh) | 一种基于los理论的农机路径跟踪方法 | |
CN104502942A (zh) | 基于卫星导航和航位推测实现农业机械定位的系统及方法 | |
CN111752274B (zh) | 一种基于强化学习的激光agv的路径跟踪控制方法 | |
CN111158379B (zh) | 一种方向盘零偏自学习的无人车轨迹跟踪方法 | |
CN109813306A (zh) | 一种无人车规划轨迹卫星定位数据可信度计算方法 | |
CN110096051A (zh) | 用于生成车辆控制指令的方法和装置 | |
CN106443737A (zh) | 基于多点gps的无人驾驶汽车行驶中的航迹推算算法 | |
CN110502020A (zh) | 一种农机的自动导航控制方法 | |
CN108944899A (zh) | 一种基于模糊控制的无人驾驶车辆转向盘控制系统及方法 | |
CN112214014A (zh) | 农机自动驾驶控制方法及系统 | |
CN108121347A (zh) | 用于控制设备运动的方法、装置及电子设备 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |