CN108693773B - 一种农业机械自动驾驶滑坡偏差自适应估计方法 - Google Patents
一种农业机械自动驾驶滑坡偏差自适应估计方法 Download PDFInfo
- Publication number
- CN108693773B CN108693773B CN201810294986.6A CN201810294986A CN108693773B CN 108693773 B CN108693773 B CN 108693773B CN 201810294986 A CN201810294986 A CN 201810294986A CN 108693773 B CN108693773 B CN 108693773B
- Authority
- CN
- China
- Prior art keywords
- angle
- vehicle
- deviation
- agricultural
- landslide
- 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
Images
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
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了一种农业机械自动驾驶滑坡偏差自适应估计方法,包括以下步骤:步骤(1):计算破面坡度角并建立农用车辆离散线性化后的运动学模型;步骤(2):将上述模型离散线性化之后,实施EKF滤波;步骤(3):基于上述的EKF滤波,输出处理结果ψ、δ和d,其中ψ航向偏差,δ为前轮转向角度,d为车身位置到期望路径的垂直距离,本发明的滑坡偏差自适应估计算法能够快速精度估计由于坡度地形变化引起的航向角和前轮转角误差,从而提高控制算法对地形变化的适应性。
Description
技术领域
本发明涉及数据处理领域,具体涉及到一种农业机械自动驾驶滑坡偏差自适应估计方法,尤其是涉及针对农业机械自动驾驶控制***中对带有一定坡度地形数据处理方法。
背景技术
随着MEMS(Micro-Electro-Mechanical-System)传感器、导航和控制技术的发展以及国家对农业扶持力度的进一步加大,精准农业正在快速变成一种趋势,而在农业机械辅助驾驶控制过程中,车体的姿态(包括俯仰角、翻滚角和航向角)、速度和位置信息能够实时反映出车体的运动和位置信息,这些信息能够为高精度的组合导航和控制算法提供重要的数据输入。
在斜坡地形上,农用车辆趋向于下坡下滑动,而大部分侧滑和轮胎、土质条件和农具负载有关。这些因素变化非常大,而且土质条件和农具负载是时变的。其中农具负载变化是最明显的,而且它随着地形倾斜变化非常明显。因此,如果没有地形的先验知识,倾斜地形对拖拉机的动力学影响就不能***。如果对于这些变化量不能及时准确处理,则会导致控制精度下降、或者是表现为***对环境变化的适应性差。
本专利发明了一种新的滑坡误差估计算法,它对比与标准滑坡估计(零偏增广模型)相比,估计精度和实时性都有较大的提高。从而保证了自动驾驶在有坡度的地形上也能保持其应有的控制精度。
发明内容
为了解决上述的缺陷,本发明提供了一种农业机械自动驾驶滑坡偏差自适应估计方法,本发明的滑坡偏差自适应估计算法能够快速精度估计由于坡度地形变化引起的航向角和前轮转角误差,从而提高控制算法对地形变化的适应性。
本发明提供了一种农业机械自动驾驶滑坡偏差自适应估计方法,包括以下步骤:
步骤(1):计算破面坡度角并建立农用车辆离散线性化后的运动学模型;
步骤(2):将上述模型离散线性化之后,实施EKF滤波;
步骤(3):基于上述的EKF滤波,输出处理结果ψ、δ和d,其中ψ航向偏差,δ为前轮转向角度,d为车身位置到期望路径的垂直距离。
上述的一种农业机械自动驾驶滑坡偏差自适应估计方法,其中,步骤(1)具体包括:
步骤(1.1):计算农用机械车身在滑坡上的侧滑力,如下公式(1)所示;
F2=Nsinζ (1),其中N为车辆垂直向下的重力,F2为侧滑力,ζ为破面坡度角;
步骤(1.2):计算农用机械的行驶角度,如下公式(2)、(3)和(4)所示;
步骤(1.3):滑坡偏差自适应Kalman估计模型建立。
上述的一种农业机械自动驾驶滑坡偏差自适应估计方法,其中,所述步骤(1.3)具体包括:
状态变量的建立,公式(5)所示,x=[ψ δ d K1 K2]T (5)
农用车辆运动学模型的建立,如公式(6)所示;
把倾斜角和侧滑抑制角μ相互依赖关系纳入观测方程,则观测方程变为:
上述的一种农业机械自动驾驶滑坡偏差自适应估计方法,其中,所述步骤(2)具体包括:
依据EKF过程对数据进行估计,如公式(8)所示:
Pk(+)=[I-KkHk]Pk(-)
其中k为采样时刻,为***状态估计量,(-)为前一时刻,(+)为后一时刻,Φk为状态转移矩阵,Pk为最小均方误差矩阵,Q为状态向量对应的协方差矩阵,Kk为误差增益,yk为观测向量,Hk为观测方程转移矩阵,Rk为观测向量对应的协方差矩阵。
上述的一种农业机械自动驾驶滑坡偏差自适应估计方法,其中,所述步骤(1)还包括:定义向量和其中为目标跟踪轨迹的单位向量,为车身姿态欧拉角单位向量,农用车辆在目标跟踪轨迹方向行驶,如果没有坡度角影响,则和应该是重合的,即反之,坡度角则与这两个单位向量的叉乘相关;
本发明具有以下有益效果:
(1)本发明实现的滑坡偏差自适应估计算法能够快速精度估计由于坡度地形变化引起的航向角和前轮转角误差,从而提高数据源精度。
(2)相比较零偏增广模型,在有坡度的地面上,本发明能将自动驾驶性能指标提高大约25%。
(3)本发明计算量小,实时性好,能够适应地形起伏变化,提高自动驾驶***环境适应性。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述,本发明及其特征、外形和优点将会变得更明显。在全部附图中相同的标记指示相同的部分。并未刻意按照比例绘制附图,重点在于示出本发明的主旨。
图1为本发明提供的农用机械车身和前轮在地面的受力示意图;
图2为本发明提供的农用车辆轮胎在地面上施加的力的上视图。
具体实施方式
在下文的描述中,给出了大量具体的细节以便提供对本发明更为彻底的理解。然而,对于本领域技术人员而言显而易见的是,本发明可以无需一个或多个这些细节而得以实施。在其他的例子中,为了避免与本发明发生混淆,对于本领域公知的一些技术特征未进行描述。
为了彻底理解本发明,将在下列的描述中提出详细的步骤以及详细的结构,以便阐释本发明的技术方案。本发明的较佳实施例详细描述如下,然而除了这些详细描述外,本发明还可以具有其他实施方式。
参照图1和图2所示,本发明提供了一种农业机械自动驾驶滑坡偏差自适应估计方法,包括以下步骤:
步骤(1):计算破面坡度角并建立农用车辆离散线性化后的运动学模型;具体包括:
步骤(1.1):计算农用机械车身在滑坡上的侧滑力,如下公式(1)所示;
F2=Nsinζ (1),其中N为车辆垂直向下的重力,F2为侧滑力,ζ为破面坡度角;
步骤(1.2):计算农用机械的行驶角度,如下公式(2)、(3)和(4)所示;
步骤(1.3):滑坡偏差自适应Kalman估计模型建立,还包括:
状态变量的建立,公式(5)所示,x=[ψ δ d K1 K2]T (5)
农用车辆运动学模型的建立,如公式(6)所示;
把倾斜角和侧滑抑制角μ相互依赖关系纳入观测方程,则观测方程变为:
步骤(2):将上述模型离散线性化之后,实施EKF滤波,具体包括:
依据EKF过程对数据进行估计,如公式(8)所示:
Pk(+)=[I-KkHk]Pk(-)
其中k为采样时刻,为***状态估计量,(-)为前一时刻,(+)为后一时刻,Φk为状态转移矩阵,Pk为最小均方误差矩阵,Q为状态向量对应的协方差矩阵,Kk为误差增益,yk为观测向量,Hk为观测方程转移矩阵,Rk为观测向量对应的协方差矩阵;
步骤(3):基于上述的EKF滤波,输出处理结果ψ、δ和d,其中ψ航向偏差,δ为前轮转向角度,d为车身位置到期望路径的垂直距离。
其中,步骤(1)还包括:定义向量和其中为目标跟踪轨迹的单位向量,为车身姿态欧拉角单位向量,农用车辆在目标跟踪轨迹方向行驶,如果没有坡度角影响,则和应该是重合的,即反之,坡度角则与这两个单位向量的叉乘相关;
在本发明中,如图1所示,农用机械车身和前轮在地面的受力示意图,N为车辆垂直向下的重力,F1为车辆垂直地面的力,F2为侧滑力,ζ为破面坡度角。
F2=Nsinζ (公式1)
农用车辆必须克服这种侧滑力,才能够精确跟踪目标轨迹。图中还显示了农用车辆轮胎在地面上施加的力的上视图。如果农用车辆能够保持μ这样一个正确的角度,那么轮胎就会施加一个侧向力F2来平衡侧滑。Ft是农用机械动力输出的推力
K是一个变量,它包含了牵引力和拖拉机的重量。这些对斜角的要求是直观的:随着可用的牵引力增加,所需的μ减小。换句话说,一辆牵引力较小的农用车辆比牵引力大的农用车辆所需要的μ更大。因此,对于一阶,偏航和转向偏差应与斜率的正弦值成比例。
滑坡偏差自适应Kalman估计模型建立
其中ψ为航向偏差(车身航向角与期望路径方向的偏差角度),δ为前轮转向角度,d为车身位置到期望路径的垂直距离,Vx为车辆速度,u为控制量,L为车辆前后轮轴间距,K1为侧滑角对航向偏差影响系数,K1为侧滑角对前轮转角影响系数,其中K1和K2为状态变量。
状态变量:
x=[ψ δ d K1 K2]T (公式5)
农用车辆运动学模型:
把倾斜角和侧滑抑制角μ相互依赖关系纳入观测方程,则观测方程变为:
将上述模型离散线性化之后,依据以下EKF过程对数据进行估计:
Pk(+)=[I-KkHk]Pk(-)
其中,k为采样时刻,为***状态估计量,(-)为前一时刻,(+)为后一时刻,Φk为状态转移矩阵,Pk为最小均方误差矩阵,Q为状态向量对应的协方差矩阵,Kk为误差增益,yk为观测向量,Hk为观测方程转移矩阵,Rk为观测向量对应的协方差矩阵。
EKF更新后状态变量ψ、δ和d即为对滑坡偏差处理后的真实数据。
以下提供一具体的实施方式
第一步:计算坡度角ζ
定义向量和其中为目标跟踪轨迹的单位向量,为车身姿态欧拉角(横滚、俯仰和航向)单位向量。农用车辆在目标跟踪轨迹方向行驶,如果没有坡度角影响,则和应该是重合的,即反之,坡度角则与这两个单位向量的叉乘相关。
第二步:根据公式5、6、7建立农用车辆离散线性化后的运动学模型。
第三步:根据公式8,实施EKF滤波,输出处理结果ψ、δ和d。
第四步:根据现代或经典控制方法,对农用车辆进行实时控制,从而实现自动驾驶功能。
如图1所示,农用机械车身和前轮在地面的受力示意图,N为车辆垂直向下的重力,F1为车辆垂直地面的力,F2为侧滑力,ζ为破面坡度角。
F2=Nsinζ (公式1)
农用车辆必须克服这种侧滑力,才能够精确跟踪目标轨迹。图中还显示了农用车辆轮胎在地面上施加的力的上视图。如果农用车辆能够保持μ这样一个正确的角度,那么轮胎就会施加一个侧向力F2来平衡侧滑。Ft是农用机械动力输出的推力
K是一个变量,它包含了牵引力和拖拉机的重量。这些对斜角的要求是直观的:随着可用的牵引力增加,所需的μ减小。换句话说,一辆牵引力较小的农用车辆比牵引力大的农用车辆所需要的μ更大。因此,对于一阶,偏航和转向偏差应与斜率的正弦值成比例。
滑坡偏差自适应Kalman估计模型建立
其中ψ为航向偏差(车身航向角与期望路径方向的偏差角度),δ为前轮转向角度,d为车身位置到期望路径的垂直距离,Vx为车辆速度,u为控制量,L为车辆前后轮轴间距,K1为侧滑角对航向偏差影响系数,K1为侧滑角对前轮转角影响系数,其中K1和K2为状态变量。
状态变量:
x=[ψ δ d K1 K2]T (公式5)
农用车辆运动学模型:
把倾斜角和侧滑抑制角μ相互依赖关系纳入观测方程,则观测方程变为:
将上述模型离散线性化之后,依据以下EKF过程对数据进行估计:
Pk(+)=[I-KkHk]Pk(-)
其中,k为采样时刻,为***状态估计量,(-)为前一时刻,(+)为后一时刻,Φk为状态转移矩阵,Pk为最小均方误差矩阵,Q为状态向量对应的协方差矩阵,Kk为误差增益,yk为观测向量,Hk为观测方程转移矩阵,Rk为观测向量对应的协方差矩阵。
第一步:计算坡度角ζ
定义向量和其中为目标跟踪轨迹的单位向量,为车身姿态欧拉角(横滚、俯仰和航向)单位向量。农用车辆在目标跟踪轨迹方向行驶,如果没有坡度角影响,则和应该是重合的,即反之,坡度角则与这两个单位向量的叉乘相关。
第二步:根据公式5、6、7建立农用车辆离散线性化后的运动学模型。
第三步:根据公式8,实施EKF滤波,输出处理结果ψ、δ和d。
本发明的滑坡偏差自适应估计算法能够快速精度估计由于坡度地形变化引起的航向角和前轮转角误差,从而提高控制算法对地形变化的适应性,具体包括以下几点:(1)本发明实现的滑坡偏差自适应估计算法能够快速精度估计由于坡度地形变化引起的航向角和前轮转角误差,从而提高数据源精度。(2)相比较零偏增广模型,在有坡度的地面上,本发明能将自动驾驶性能指标提高大约25%。(3)本发明计算量小,实时性好,能够适应地形起伏变化,提高自动驾驶***环境适应性。
以上对本发明的较佳实施例进行了描述。需要理解的是,本发明并不局限于上述特定实施方式,其中未尽详细描述的设备和结构应该理解为用本领域中的普通方式予以实施;任何熟悉本领域的技术人员,在不脱离本发明技术方案范围情况下,都可利用上述揭示的方法和技术内容对本发明技术方案做出许多可能的变动和修饰,或修改为等同变化的等效实施例,这并不影响本发明的实质内容。因此,凡是未脱离本发明技术方案的内容,依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰,均仍属于本发明技术方案保护的范围内。
Claims (2)
1.一种农业机械自动驾驶滑坡偏差自适应估计方法,其特征在于,包括以下步骤:
步骤(1):计算坡面坡度角并建立农用车辆离散线性化后的运动学模型;
步骤(2):将上述模型离散线性化之后,实施EKF滤波;
步骤(3):基于上述的EKF滤波,输出处理结果ψ、δ和d,其中ψ航向偏差,δ为前轮转向角度,d为车身位置到期望路径的垂直距离,所述步骤(1)具体包括:
步骤(1.1):计算农用机械车身在滑坡上的侧滑力,如下公式(1)所示;
F2=Nsinζ (1),其中N为车辆垂直向下的重力,F2为侧滑力,ζ为坡面坡度角;
步骤(1.2):计算农用机械的行驶角度,如下公式(2)、(3)和(4)所示;
步骤(1.3):滑坡偏差自适应Kalman估计模型建立,所述步骤(1.3)具体包括:
状态变量的建立,公式(5)所示,x=[ψ δ d K1 K2]T (5),其中ψ航向偏差,δ为前轮转向角度,d为车身位置到期望路径的垂直距离
农用车辆运动学模型的建立,如公式(6)所示;
把倾斜角和侧滑抑制角μ相互依赖关系纳入观测方程,则观测方程变为:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810294986.6A CN108693773B (zh) | 2018-04-04 | 2018-04-04 | 一种农业机械自动驾驶滑坡偏差自适应估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810294986.6A CN108693773B (zh) | 2018-04-04 | 2018-04-04 | 一种农业机械自动驾驶滑坡偏差自适应估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108693773A CN108693773A (zh) | 2018-10-23 |
CN108693773B true CN108693773B (zh) | 2021-03-23 |
Family
ID=63845397
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810294986.6A Active CN108693773B (zh) | 2018-04-04 | 2018-04-04 | 一种农业机械自动驾驶滑坡偏差自适应估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108693773B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110262479A (zh) * | 2019-05-28 | 2019-09-20 | 南京天辰礼达电子科技有限公司 | 一种履带式拖拉机运动学估计及偏差校准方法 |
CN111238471B (zh) * | 2020-01-17 | 2021-08-24 | 青岛农业大学 | 一种适用于农业机械直线导航的侧滑角度估计方法及估计器 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103661393A (zh) * | 2012-08-31 | 2014-03-26 | 福特全球技术公司 | 运动学道路坡度估计 |
CN105203098A (zh) * | 2015-10-13 | 2015-12-30 | 上海华测导航技术股份有限公司 | 基于九轴mems传感器的农业机械全姿态角更新方法 |
CN105891863A (zh) * | 2016-06-16 | 2016-08-24 | 东南大学 | 一种基于高度约束的扩展卡尔曼滤波定位方法 |
CN106840097A (zh) * | 2017-01-24 | 2017-06-13 | 重庆大学 | 一种基于自适应扩展卡尔曼滤波的道路坡度估计方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7228230B2 (en) * | 2004-11-12 | 2007-06-05 | Mitsubishi Denki Kabushiki Kaisha | System for autonomous vehicle navigation with carrier phase DGPS and laser-scanner augmentation |
-
2018
- 2018-04-04 CN CN201810294986.6A patent/CN108693773B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103661393A (zh) * | 2012-08-31 | 2014-03-26 | 福特全球技术公司 | 运动学道路坡度估计 |
CN105203098A (zh) * | 2015-10-13 | 2015-12-30 | 上海华测导航技术股份有限公司 | 基于九轴mems传感器的农业机械全姿态角更新方法 |
CN105891863A (zh) * | 2016-06-16 | 2016-08-24 | 东南大学 | 一种基于高度约束的扩展卡尔曼滤波定位方法 |
CN106840097A (zh) * | 2017-01-24 | 2017-06-13 | 重庆大学 | 一种基于自适应扩展卡尔曼滤波的道路坡度估计方法 |
Non-Patent Citations (3)
Title |
---|
Navigation system for agricultural machines Nonlinear Model Predictive path tracking;J. Backman 等;《Computers and Electronics in Agriculture》;20111220;全文 * |
基于扩展卡尔曼滤波的车辆质量与道路坡度估计;雷雨龙 等;《农业机械学报》;20141130;全文 * |
基于支持向量机的道路坡度实时预测方法试验;张小龙 等;《农业机械学报》;20141130;全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN108693773A (zh) | 2018-10-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106250591B (zh) | 一种考虑侧倾影响的汽车行驶状态估计方法 | |
CN111610780B (zh) | 一种自动驾驶车路径跟踪控制方法及其装置 | |
JP6420173B2 (ja) | 自動走行圃場作業車両 | |
CN108614426B (zh) | 一种基于干扰观测器的多移动机器人编队鲁棒控制方法 | |
WO2017208306A1 (ja) | 自動走行作業車両 | |
CN111982100B (zh) | 一种无人机的航向角解算算法 | |
CN107943060B (zh) | 一种自动驾驶仪、沿着跟踪直线引导车辆的方法以及计算机可读介质 | |
CN105467996B (zh) | 基于微分平坦和自抗扰的四轮转向汽车轨迹跟踪控制方法 | |
CN108839652B (zh) | 一种车辆失稳可控域的自动驾驶紧急避让*** | |
CN108693773B (zh) | 一种农业机械自动驾驶滑坡偏差自适应估计方法 | |
CN110312655B (zh) | 控制装置及操纵装置 | |
CN109017805B (zh) | 一种针对存在不确定性的行驶***车辆稳定性控制方法 | |
CN111688715B (zh) | 四轮驱动电动汽车基于融合技术的质心侧偏角观测方法 | |
Leman et al. | Model predictive controller for path tracking and obstacle avoidance manoeuvre on autonomous vehicle | |
JP2022552087A (ja) | モータ車両の横方向位置を制御するための方法 | |
CN111189454A (zh) | 基于秩卡尔曼滤波的无人车slam导航方法 | |
Leng et al. | A simple tractor-trailer backing control law for path following with side-slope compensation | |
CN113665669B (zh) | 车辆稳定性控制***及方法 | |
CN114728672A (zh) | 转向保持判定装置、转向控制装置和转向装置 | |
CN113401113A (zh) | 基于车辆稳定包络线的无人驾驶汽车直接横摆力矩控制方法及控制器 | |
Cariou et al. | Adaptive control of four-wheel-steering off-road mobile robots: Application to path tracking and heading control in presence of sliding | |
CN114435371B (zh) | 一种道路坡度估计方法和装置 | |
US11648949B2 (en) | Method for controlling a wheeled vehicle in low-grip conditions | |
CN113721609B (zh) | 一种侧滑情况下4wid高地隙喷雾机轨迹跟踪控制方法 | |
CN113650621B (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 |