CN108693773B - 一种农业机械自动驾驶滑坡偏差自适应估计方法 - Google Patents

一种农业机械自动驾驶滑坡偏差自适应估计方法 Download PDF

Info

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
Application number
CN201810294986.6A
Other languages
English (en)
Other versions
CN108693773A (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.)
Nanjing Tianchen Lida Electronic Technology Co ltd
Original Assignee
Nanjing Tianchen Lida Electronic Technology Co ltd
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 Nanjing Tianchen Lida Electronic Technology Co ltd filed Critical Nanjing Tianchen Lida Electronic Technology Co ltd
Priority to CN201810294986.6A priority Critical patent/CN108693773B/zh
Publication of CN108693773A publication Critical patent/CN108693773A/zh
Application granted granted Critical
Publication of CN108693773B publication Critical patent/CN108693773B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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)所示；
Figure BDA0001618503270000021
Figure BDA0001618503270000022
Figure BDA0001618503270000023
其中F1为车辆垂直地面的力，Ft是农用机械动力输出的推力，K是一个变量，包含了牵引力和农用机械的重量；
步骤(1.3)：滑坡偏差自适应Kalman估计模型建立。
上述的一种农业机械自动驾驶滑坡偏差自适应估计方法，其中，所述步骤(1.3)具体包括：
状态变量的建立，公式(5)所示，x＝[ψ δ d K1 K2]T (5)
农用车辆运动学模型的建立，如公式(6)所示；
Figure BDA0001618503270000031
Figure BDA0001618503270000032
Figure BDA0001618503270000033
把倾斜角和侧滑抑制角μ相互依赖关系纳入观测方程，则观测方程变为：
Figure BDA0001618503270000034
其中Vx为车辆速度，u为控制量，L为车辆前后轮轴间距，K1为侧滑角对航向偏差影响系数，K1为侧滑角对前轮转角影响系数，其中K1和K2为状态变量。
上述的一种农业机械自动驾驶滑坡偏差自适应估计方法，其中，所述步骤(2)具体包括：
依据EKF过程对数据进行估计，如公式(8)所示：
Figure BDA0001618503270000035
Figure BDA0001618503270000036
Figure BDA0001618503270000037
Figure BDA0001618503270000038
Pk(+)＝[I-KkHk]Pk(-)
Figure BDA0001618503270000039
Figure BDA00016185032700000311
其中k为采样时刻，
Figure BDA00016185032700000310
为***状态估计量，(-)为前一时刻，(+)为后一时刻，Φk为状态转移矩阵，Pk为最小均方误差矩阵，Q为状态向量对应的协方差矩阵，Kk为误差增益，yk为观测向量，Hk为观测方程转移矩阵，Rk为观测向量对应的协方差矩阵。
上述的一种农业机械自动驾驶滑坡偏差自适应估计方法，其中，所述步骤(1)还包括：定义向量
Figure BDA0001618503270000041
和
Figure BDA0001618503270000042
其中
Figure BDA0001618503270000043
为目标跟踪轨迹的单位向量，
Figure BDA0001618503270000044
为车身姿态欧拉角单位向量，农用车辆在目标跟踪轨迹方向行驶，如果没有坡度角影响，则
Figure BDA0001618503270000045
和
Figure BDA0001618503270000046
应该是重合的，即
Figure BDA0001618503270000047
反之，坡度角则与这两个单位向量的叉乘相关；
根据目标轨迹和车身姿态角组建
Figure BDA0001618503270000048
和
Figure BDA0001618503270000049
单位向量，一般来说，目标跟踪轨迹为2D，所以p3一般不涉及，所以可以根据公式(9)计算获得
Figure BDA00016185032700000410
Figure BDA00016185032700000411
定义向量
Figure BDA00016185032700000412
Figure BDA00016185032700000413
Figure BDA00016185032700000414
本发明具有以下有益效果：
(1)本发明实现的滑坡偏差自适应估计算法能够快速精度估计由于坡度地形变化引起的航向角和前轮转角误差，从而提高数据源精度。
(2)相比较零偏增广模型，在有坡度的地面上，本发明能将自动驾驶性能指标提高大约25％。
(3)本发明计算量小，实时性好，能够适应地形起伏变化，提高自动驾驶***环境适应性。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明及其特征、外形和优点将会变得更明显。在全部附图中相同的标记指示相同的部分。并未刻意按照比例绘制附图，重点在于示出本发明的主旨。
图1为本发明提供的农用机械车身和前轮在地面的受力示意图；
图2为本发明提供的农用车辆轮胎在地面上施加的力的上视图。
具体实施方式
在下文的描述中，给出了大量具体的细节以便提供对本发明更为彻底的理解。然而，对于本领域技术人员而言显而易见的是，本发明可以无需一个或多个这些细节而得以实施。在其他的例子中，为了避免与本发明发生混淆，对于本领域公知的一些技术特征未进行描述。
为了彻底理解本发明，将在下列的描述中提出详细的步骤以及详细的结构，以便阐释本发明的技术方案。本发明的较佳实施例详细描述如下，然而除了这些详细描述外，本发明还可以具有其他实施方式。
参照图1和图2所示，本发明提供了一种农业机械自动驾驶滑坡偏差自适应估计方法，包括以下步骤：
步骤(1)：计算破面坡度角并建立农用车辆离散线性化后的运动学模型；具体包括：
步骤(1.1)：计算农用机械车身在滑坡上的侧滑力，如下公式(1)所示；
F2＝Nsinζ (1)，其中N为车辆垂直向下的重力，F2为侧滑力，ζ为破面坡度角；
步骤(1.2)：计算农用机械的行驶角度，如下公式(2)、(3)和(4)所示；
Figure BDA0001618503270000051
Figure BDA0001618503270000052
Figure BDA0001618503270000053
其中F1为车辆垂直地面的力，Ft是农用机械动力输出的推力，K是一个变量，包含了牵引力和农用机械的重量；
步骤(1.3)：滑坡偏差自适应Kalman估计模型建立，还包括：
状态变量的建立，公式(5)所示，x＝[ψ δ d K1 K2]T (5)
农用车辆运动学模型的建立，如公式(6)所示；
Figure BDA0001618503270000061
Figure BDA0001618503270000062
Figure BDA0001618503270000063
把倾斜角和侧滑抑制角μ相互依赖关系纳入观测方程，则观测方程变为：
Figure BDA0001618503270000064
其中Vx为车辆速度，u为控制量，L为车辆前后轮轴间距，K1为侧滑角对航向偏差影响系数，K1为侧滑角对前轮转角影响系数，其中K1和K2为状态变量；
步骤(2)：将上述模型离散线性化之后，实施EKF滤波，具体包括：
依据EKF过程对数据进行估计，如公式(8)所示：
Figure BDA0001618503270000065
Figure BDA0001618503270000066
Figure BDA0001618503270000067
Figure BDA0001618503270000068
Pk(+)＝[I-KkHk]Pk(-)
Figure BDA0001618503270000069
Figure BDA00016185032700000610
其中k为采样时刻，
Figure BDA00016185032700000611
为***状态估计量，(-)为前一时刻，(+)为后一时刻，Φk为状态转移矩阵，Pk为最小均方误差矩阵，Q为状态向量对应的协方差矩阵，Kk为误差增益，yk为观测向量，Hk为观测方程转移矩阵，Rk为观测向量对应的协方差矩阵；
步骤(3)：基于上述的EKF滤波，输出处理结果ψ、δ和d，其中ψ航向偏差，δ为前轮转向角度，d为车身位置到期望路径的垂直距离。
其中，步骤(1)还包括：定义向量
Figure BDA0001618503270000071
和
Figure BDA0001618503270000072
其中
Figure BDA0001618503270000073
为目标跟踪轨迹的单位向量，
Figure BDA0001618503270000074
为车身姿态欧拉角单位向量，农用车辆在目标跟踪轨迹方向行驶，如果没有坡度角影响，则
Figure BDA0001618503270000075
和
Figure BDA0001618503270000076
应该是重合的，即
Figure BDA0001618503270000077
反之，坡度角则与这两个单位向量的叉乘相关；
根据目标轨迹和车身姿态角组建
Figure BDA0001618503270000078
和
Figure BDA0001618503270000079
单位向量，一般来说，目标跟踪轨迹为2D，所以p3一般不涉及，所以可以根据公式(9)计算获得
Figure BDA00016185032700000710
Figure BDA00016185032700000711
定义向量
Figure BDA00016185032700000712
Figure BDA00016185032700000713
Figure BDA00016185032700000714
在本发明中，如图1所示，农用机械车身和前轮在地面的受力示意图，N为车辆垂直向下的重力，F1为车辆垂直地面的力，F2为侧滑力，ζ为破面坡度角。
F2＝Nsinζ (公式1)
农用车辆必须克服这种侧滑力，才能够精确跟踪目标轨迹。图中还显示了农用车辆轮胎在地面上施加的力的上视图。如果农用车辆能够保持μ这样一个正确的角度，那么轮胎就会施加一个侧向力F2来平衡侧滑。Ft是农用机械动力输出的推力
Figure BDA0001618503270000081
Figure BDA0001618503270000082
Figure BDA0001618503270000083
K是一个变量，它包含了牵引力和拖拉机的重量。这些对斜角的要求是直观的：随着可用的牵引力增加，所需的μ减小。换句话说，一辆牵引力较小的农用车辆比牵引力大的农用车辆所需要的μ更大。因此，对于一阶，偏航和转向偏差应与斜率的正弦值成比例。
滑坡偏差自适应Kalman估计模型建立
其中ψ为航向偏差(车身航向角与期望路径方向的偏差角度)，δ为前轮转向角度，d为车身位置到期望路径的垂直距离，Vx为车辆速度，u为控制量，L为车辆前后轮轴间距，K1为侧滑角对航向偏差影响系数，K1为侧滑角对前轮转角影响系数，其中K1和K2为状态变量。
状态变量：
x＝[ψ δ d K1 K2]T (公式5)
农用车辆运动学模型：
Figure BDA0001618503270000084
Figure BDA0001618503270000085
Figure BDA0001618503270000086
把倾斜角和侧滑抑制角μ相互依赖关系纳入观测方程，则观测方程变为：
Figure BDA0001618503270000091
将上述模型离散线性化之后，依据以下EKF过程对数据进行估计：
Figure BDA0001618503270000092
Figure BDA0001618503270000093
Figure BDA0001618503270000094
Figure BDA0001618503270000095
Pk(+)＝[I-KkHk]Pk(-)
Figure BDA0001618503270000096
Figure BDA0001618503270000097
其中，k为采样时刻，
Figure BDA0001618503270000098
为***状态估计量，(-)为前一时刻，(+)为后一时刻，Φk为状态转移矩阵，Pk为最小均方误差矩阵，Q为状态向量对应的协方差矩阵，Kk为误差增益，yk为观测向量，Hk为观测方程转移矩阵，Rk为观测向量对应的协方差矩阵。
EKF更新后状态变量ψ、δ和d即为对滑坡偏差处理后的真实数据。
以下提供一具体的实施方式
第一步：计算坡度角ζ
定义向量
Figure BDA0001618503270000099
和
Figure BDA00016185032700000910
其中
Figure BDA00016185032700000911
为目标跟踪轨迹的单位向量，
Figure BDA00016185032700000912
为车身姿态欧拉角(横滚、俯仰和航向)单位向量。农用车辆在目标跟踪轨迹方向行驶，如果没有坡度角影响，则
Figure BDA00016185032700000913
和
Figure BDA00016185032700000914
应该是重合的，即
Figure BDA00016185032700000915
反之，坡度角则与这两个单位向量的叉乘相关。
根据目标轨迹和车身姿态角组建
Figure BDA00016185032700000916
和
Figure BDA00016185032700000917
单位向量，一般来说，目标跟踪轨迹为2D，所以p3一般不涉及，所以可以根据公式9计算获得：
Figure BDA0001618503270000101
Figure BDA0001618503270000102
定义向量
Figure BDA0001618503270000103
Figure BDA0001618503270000104
Figure BDA0001618503270000105
第二步：根据公式5、6、7建立农用车辆离散线性化后的运动学模型。
第三步：根据公式8，实施EKF滤波，输出处理结果ψ、δ和d。
第四步：根据现代或经典控制方法，对农用车辆进行实时控制，从而实现自动驾驶功能。
如图1所示，农用机械车身和前轮在地面的受力示意图，N为车辆垂直向下的重力，F1为车辆垂直地面的力，F2为侧滑力，ζ为破面坡度角。
F2＝Nsinζ (公式1)
农用车辆必须克服这种侧滑力，才能够精确跟踪目标轨迹。图中还显示了农用车辆轮胎在地面上施加的力的上视图。如果农用车辆能够保持μ这样一个正确的角度，那么轮胎就会施加一个侧向力F2来平衡侧滑。Ft是农用机械动力输出的推力
Figure BDA0001618503270000106
Figure BDA0001618503270000107
Figure BDA0001618503270000108
K是一个变量，它包含了牵引力和拖拉机的重量。这些对斜角的要求是直观的：随着可用的牵引力增加，所需的μ减小。换句话说，一辆牵引力较小的农用车辆比牵引力大的农用车辆所需要的μ更大。因此，对于一阶，偏航和转向偏差应与斜率的正弦值成比例。
滑坡偏差自适应Kalman估计模型建立
其中ψ为航向偏差(车身航向角与期望路径方向的偏差角度)，δ为前轮转向角度，d为车身位置到期望路径的垂直距离，Vx为车辆速度，u为控制量，L为车辆前后轮轴间距，K1为侧滑角对航向偏差影响系数，K1为侧滑角对前轮转角影响系数，其中K1和K2为状态变量。
状态变量：
x＝[ψ δ d K1 K2]T (公式5)
农用车辆运动学模型：
Figure BDA0001618503270000111
Figure BDA0001618503270000112
Figure BDA0001618503270000113
把倾斜角和侧滑抑制角μ相互依赖关系纳入观测方程，则观测方程变为：
Figure BDA0001618503270000114
将上述模型离散线性化之后，依据以下EKF过程对数据进行估计：
Figure BDA0001618503270000115
Figure BDA0001618503270000116
Figure BDA0001618503270000117
Figure BDA0001618503270000118
Pk(+)＝[I-KkHk]Pk(-)
Figure BDA0001618503270000119
Figure BDA00016185032700001110
其中，k为采样时刻，
Figure BDA0001618503270000121
为***状态估计量，(-)为前一时刻，(+)为后一时刻，Φk为状态转移矩阵，Pk为最小均方误差矩阵，Q为状态向量对应的协方差矩阵，Kk为误差增益，yk为观测向量，Hk为观测方程转移矩阵，Rk为观测向量对应的协方差矩阵。
第一步：计算坡度角ζ
定义向量
Figure BDA0001618503270000122
和
Figure BDA0001618503270000123
其中
Figure BDA0001618503270000124
为目标跟踪轨迹的单位向量，
Figure BDA0001618503270000125
为车身姿态欧拉角(横滚、俯仰和航向)单位向量。农用车辆在目标跟踪轨迹方向行驶，如果没有坡度角影响，则
Figure BDA0001618503270000126
和
Figure BDA0001618503270000127
应该是重合的，即
Figure BDA0001618503270000128
反之，坡度角则与这两个单位向量的叉乘相关。
根据目标轨迹和车身姿态角组建
Figure BDA0001618503270000129
和
Figure BDA00016185032700001210
单位向量，一般来说，目标跟踪轨迹为2D，所以p3一般不涉及，所以可以根据公式9计算获得：
Figure BDA00016185032700001211
Figure BDA00016185032700001212
定义向量
Figure BDA00016185032700001213
Figure BDA00016185032700001214
Figure BDA00016185032700001215
第二步：根据公式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)所示；
Figure FDA0002902015830000011
Figure FDA0002902015830000012
Figure FDA0002902015830000013
其中F1为车辆垂直地面的力，Ft是农用机械动力输出的推力，K是一个变量，包含了牵引力和农用机械的重量；
步骤(1.3)：滑坡偏差自适应Kalman估计模型建立，所述步骤(1.3)具体包括：
状态变量的建立，公式(5)所示，x＝[ψ δ d K1 K2]T (5)，其中ψ航向偏差，δ为前轮转向角度，d为车身位置到期望路径的垂直距离
农用车辆运动学模型的建立，如公式(6)所示；
Figure FDA0002902015830000021
Figure FDA0002902015830000022
Figure FDA0002902015830000023
把倾斜角和侧滑抑制角μ相互依赖关系纳入观测方程，则观测方程变为：
Figure FDA0002902015830000024
其中Vx为车辆速度，u为控制量，L为车辆前后轮轴间距，K1为侧滑角对航向偏差影响系数，K1为侧滑角对前轮转角影响系数，其中K1和K2为状态变量。
2.如权利要求1所述的一种农业机械自动驾驶滑坡偏差自适应估计方法，其特征在于，所述步骤(2)具体包括：
依据EKF过程对数据进行估计，如公式(8)所示：
Figure FDA0002902015830000025
Figure FDA0002902015830000026
Figure FDA0002902015830000027
Figure FDA0002902015830000028
Pk(+)＝[I-KkHk]Pk(-)
Figure FDA0002902015830000029
Figure FDA00029020158300000210
其中k为采样时刻，
Figure FDA00029020158300000211
为***状态估计量，(-)为前一时刻，(+)为后一时刻，Φk为状态转移矩阵，Pk为最小均方误差矩阵，Q为状态向量对应的协方差矩阵，Kk为误差增益，yk为观测向量，Hk为观测方程转移矩阵，Rk为观测向量对应的协方差矩阵。
CN201810294986.6A 2018-04-04 2018-04-04 一种农业机械自动驾驶滑坡偏差自适应估计方法 Active CN108693773B (zh)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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

Patent Citations (4)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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