WO2020238011A1 - 一种履带式拖拉机运动学估计及偏差校准方法 - Google Patents
一种履带式拖拉机运动学估计及偏差校准方法 Download PDFInfo
- Publication number
- WO2020238011A1 WO2020238011A1 PCT/CN2019/115000 CN2019115000W WO2020238011A1 WO 2020238011 A1 WO2020238011 A1 WO 2020238011A1 CN 2019115000 W CN2019115000 W CN 2019115000W WO 2020238011 A1 WO2020238011 A1 WO 2020238011A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- tractor
- crawler
- heading angle
- deviation
- crawler tractor
- Prior art date
Links
- 238000000034 method Methods 0.000 title claims abstract description 32
- 238000006073 displacement reaction Methods 0.000 claims abstract description 25
- 230000000694 effects Effects 0.000 claims abstract description 11
- 238000009434 installation Methods 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 41
- 238000001914 filtration Methods 0.000 claims description 9
- 230000009977 dual effect Effects 0.000 claims description 4
- 238000004458 analytical method Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 230000006866 deterioration Effects 0.000 description 1
- 238000009313 farming Methods 0.000 description 1
- 230000005654 stationary process Effects 0.000 description 1
Images
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
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
-
- 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
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- 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
- 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
-
- 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/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0891—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for land vehicles
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
- G06F17/12—Simultaneous equations, e.g. systems of linear equations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
- G06F17/13—Differential equations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
Definitions
- the invention relates to the technical field of automatic driving of agricultural machinery, in particular to a method for kinematics estimation and deviation calibration of a crawler tractor.
- Tractors can be divided into wheel tractors and crawler tractors according to the walking mode. Compared with wheeled tractors, crawler tractors have the advantages of large contact surface, low ground pressure, good traction adhesion performance, and not easy to slip. They are more suitable for working in relatively harsh environments, such as snow, mountain slopes, mud, etc. Grassland, plateau, etc., effectively filled the shortage of wheeled tractors.
- the present invention proposes a method for kinematics estimation and deviation calibration of the crawler tractor to realize the accurate estimation of the position and posture information of the crawler tractor and enhance the adaptability of the system to the external environment And anti-interference, improve the operation accuracy of crawler tractor automatic driving system.
- the purpose of the present invention is to accurately estimate the heading angle deviation in real time by designing a suitable estimation method, and to compensate and correct the heading angle, so as to improve the adaptability of the system to the environment.
- the purpose of the present invention is to solve the problems of poor control effect and low operation accuracy caused by the interference of various factors in the actual operating environment of the crawler tractor.
- the present invention proposes an anti-interference factor kinematic estimation and deviation of the crawler tractor Calibration method, the algorithm can quickly and accurately estimate the heading angle error caused by factors such as ground ups and downs, GNSS dual antenna installation deviation, etc., and compensate the heading angle, thereby improving the automatic driving control algorithm of the crawler tractor to various environments such as the ground Adaptability of interference factors.
- a method for kinematics estimation and deviation calibration of a crawler tractor includes the following steps,
- x is the east displacement coordinate component of the crawler tractor
- y is the north displacement coordinate component
- v is the travel speed of the crawler tractor
- ⁇ is the angular velocity of the crawler tractor body
- v l is the running speed of the left crawler
- v r is the running speed of the right crawler
- R is the turning radius
- b is the width of the car body
- u is the speed difference between the left and right crawlers, which is the control quantity
- X is the system estimation vector
- Z is the system observation vector
- the Kalman filter model of the crawler tractor constructed in S7 is a nonlinear model.
- the EKF filter algorithm is used to obtain the Jacobian matrix and linearize the model to obtain the corresponding system linear state space equation:
- X(0), P(0), Z(0) are the initial values of the state vector X, the covariance matrix P, and the observation state vector Z, respectively;
- K(k+1) P(k+1
- k+1 represents the next moment
- k represents the current moment
- K is the Kalman gain
- step S18 is further included, and steps S13-S17 are continuously repeated.
- step S19 in the process of offline analysis and debugging using actual data, the observation noise matrix, the process noise matrix, and the covariance matrix are adjusted to achieve the desired filtering effect, and the actual heading angle deviation versus heading angle is estimated Make compensation.
- the tracked tractor kinematics estimation and deviation calibration method realized by the present invention can quickly and accurately estimate the heading angle deviation caused by ground undulation, GNSS antenna installation deviation, etc., thereby compensating the heading angle and improving the system's ground performance Adaptability
- the present invention can filter the data source of the crawler tractor automatic driving control algorithm, reduce data noise, and reduce the influence of external environmental interference factors and system noise on the performance of the crawler tractor automatic driving system. Improved the control accuracy and system stability of the crawler tractor automatic driving system;
- the invention has a small amount of calculation and high real-time performance.
- the automatic driving performance of a crawler tractor can be improved by about 25%.
- Figure 1 shows the kinematics model of a crawler tractor.
- Figure 2 is a flowchart of Kalman filtering estimation.
- a method for kinematics estimation and deviation calibration of a crawler tractor includes the following steps:
- Step 1 Build a kinematic model of a crawler tractor:
- x is the east displacement coordinate component of the crawler tractor
- y is the north displacement coordinate component
- v is the travel speed of the crawler tractor
- ⁇ is the angular velocity of the crawler tractor body
- Step 2 Since the angular velocity of the left wheel, right wheel and the center of mass are equal when the crawler tractor is turning, it can be derived:
- v l is the running speed of the left crawler
- v r is the running speed of the right crawler
- R is the turning radius
- b is the width of the vehicle body.
- u is the speed difference between the left and right crawlers, which is the control quantity
- Step 5 During the automatic driving operation of agricultural machinery, in order to ensure the quality of crop cultivation, the tractor is set to move in a straight line at a uniform speed, so that:
- Step 6 In the actual situation, due to the fluctuation of the ground and the deviation of the GNSS dual antenna installation, the heading angle deviation is caused, which leads to the deterioration of the path tracking effect. To simplify the model, the heading angle deviation is approximated as a certain value, then:
- the purpose of the present invention is to accurately estimate the heading angle deviation ⁇ in real time by designing an appropriate estimation method, and to compensate and correct the heading angle;
- Step 7 Through the above process, the Kalman filter nonlinear differential equation model of the crawler tractor constructed by the present invention is as follows:
- Step 8 Select the east displacement coordinate component x, the north displacement coordinate component y, the tractor speed v, and the tractor heading angle
- the heading angle deviation ⁇ is used as the system state quantity, the east displacement coordinate component x, the north displacement coordinate component y, the tractor speed v, and the tractor heading angle
- X is the system estimation vector
- Z is the system observation vector
- Step 9 The kalman filter model of the crawler tractor constructed in the seventh step is a nonlinear model.
- the present invention uses the EKF filter algorithm to obtain the corresponding system linear state space equation by linearizing the model by obtaining the Jacobian matrix:
- Step 10 Discretize the continuous system to obtain the state transition matrix ⁇ and the observation matrix H:
- Step 11 Select the process noise covariance matrix Q and the observation noise covariance matrix R:
- the observation noise covariance matrix R by collecting the pose data of the tracked tractor automatic driving system during a period of time (10-20min) in the stationary process, the east displacement coordinate component x, the north displacement coordinate component y, and the tractor travel speed v, Tractor heading angle The standard deviation of each group of data is calculated to obtain the system observation noise covariance matrix R.
- the twelfth step by collecting the east displacement coordinate component x, the north displacement coordinate component y, the tractor speed v, and the tractor heading angle during the movement of the crawler tractor (in automatic driving mode)
- the control quantity u and the vehicle body angular velocity ⁇ are used as the observation vector Z;
- Step 13 Initialize the state vector X, the covariance matrix P, and the observation state vector Z:
- X(0), P(0), Z(0) are the initial values of the state vector X, the covariance matrix P, and the observation state vector Z, respectively.
- the size of P(0) will directly affect the convergence speed of the EKF algorithm;
- Step 14 One-step prediction of Kalman filtering state:
- Step 15 Calculate the one-step prediction covariance matrix:
- Step 16 Calculate Kalman gain:
- K(k+1) P(k+1
- Step 17 Calculate the estimated value:
- Step 18 Update the covariance matrix:
- k+1 represents the next moment
- k represents the current moment
- K is the kkalman gain
- the nineteenth step Repeat steps fourteenth to eighteenth;
- the present invention adjusts the observation noise matrix R, the process noise matrix Q, and the covariance matrix P(0) to achieve the desired filtering effect, and estimates the actual heading angle deviation versus heading angle Make compensation
- the invention applies the kinematic estimation and calibration method of the crawler tractor to the automatic driving process of the crawler tractor, and performs online filtering, data processing, and estimation of the heading angle deviation to achieve better control effects;
- the crawler tractor kinematics estimation and deviation calibration method proposed by the present invention can quickly and accurately estimate the heading angle error caused by the ground undulation, thereby improving the adaptability of the control algorithm to the ground undulation, specifically including the following points:
- the tracked tractor kinematics estimation and deviation calibration method realized by the present invention can quickly and accurately estimate the heading angle deviation caused by ground undulation, GNSS antenna installation deviation, etc., thereby compensating the heading angle and improving the system's ground performance Adaptability
- the present invention can filter the data source of the crawler tractor automatic driving control algorithm, reduce data noise, and reduce the influence of external environmental interference factors and system noise on the performance of the crawler tractor automatic driving system. Improved the control accuracy and system stability of the crawler tractor automatic driving system;
- the invention has a small amount of calculation and high real-time performance.
- the automatic driving performance of a crawler tractor can be improved by about 25%.
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- Automation & Control Theory (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Operations Research (AREA)
- Computing Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种履带式拖拉机运动学估计及偏差校准方法，属于农机自动驾驶技术领域，包括以下步骤，构建履带式拖拉机运动学模型，在实际情况中，由于地面起伏变化、GNSS双天线安装偏差因素，引起航向角偏差，导致路径跟踪效果变差，为简化模型，可近似认为航向角偏差为一定值，选取东向位移坐标分量、北向位移坐标分量、拖拉机速度、北向位移坐标分量、拖拉机速度、拖拉机航向角作为系统观测量，构建的履带式拖拉机kalman滤波模型为非线性模型。能够快速精确估计出由于地面起伏变化、GNSS天线安装偏差引起的航向角偏差，从而对航向角进行补偿，提高系统对地面的适应性。
Description
本发明涉及农机自动驾驶技术领域，尤其涉及一种履带式拖拉机运动学估计及偏差校准方法。
随着GNSS高精度卫星导航定位技术、自动驾驶以及信息技术的飞速发展，现代农业正逐步向数字农业、精准农业的方向发展。拖拉机作为一种常用的农业装备，其信息化、智能化程度对精准农业的发展具有重要意义。
按照行走方式可将拖拉机分为轮式拖拉机及履带式拖拉机。与轮式拖拉机相比，履带式拖拉机具有接触面大、接地比压小、牵引附着性能好、不易打滑等优点，更适合在条件相对比较恶劣的环境中作业，如雪地、山坡、泥泞、草地、高原等，有效地填补了轮式拖拉机的不足。然而在履带式拖拉机自动驾驶作业过程中，由于实际作业环境中存在着大量的不可预测的干扰，如耕种农田地面的复杂状况、运动学模型不精确、GNSS天线安装角度偏差、GPS信号遮挡及反射、系统噪声以及外界环境噪声干扰等因素，致使拖拉机位置姿态相关信息出现异常等，严重影响履带式拖拉机自动驾驶控制精度，不仅造成农户作业强度及经济成本增加，也大大降低了农业耕作效率和土地利用率。
本发明针对履带式拖拉机自动驾驶控制技术中存在的问题，提出了一种履带式拖拉机运动学估计及偏差校准方法，以实现履带式拖拉机位置姿态信息的精确估计，增强系统对外界环境的适应性以及抗干 扰性，提高履带式拖拉机自动驾驶系统作业精度。本发明的目的则在于通过设计合适的估计方法，能够实时准确地估计出航向角偏差，并对航向角进行补偿和校正，以提高系统对环境的适应性。
发明内容
本发明的目的是针对履带式拖拉机在实际作业环境中由于受多种因素干扰而引起的控制效果差、作业精度低的问题，本发明提出一种抗干扰因素的履带式拖拉机运动学估计及偏差校准方法，该算法能够快速精度估计出由于地面起伏变化、GNSS双天线安装偏差等因素引起的航向角误差，并对航向角进行补偿，从而提高履带式拖拉机自动驾驶控制算法对地面等各种环境干扰因素的适应性。
为了实现上述目的，本发明采用了如下技术方案：
一种履带式拖拉机运动学估计及偏差校准方法，包括以下步骤，
S1，构建履带式拖拉机运动学模型：
其中，x为履带式拖拉机东向位移坐标分量，y为北向位移坐标分量，v为履带式拖拉机行驶速度，
为履带式拖拉机航向角，ω为履带式拖拉机车体角速度；
S2：由于履带式拖拉机在进行转向时，左轮、右轮及质心处角速度相等，故可推导出：
其中，v
l为为左侧履带的行驶速度，v
r为为右侧履带的行驶速度，R为转弯半径，b为车体宽度；
S3：联立方程求解可得：
S4：由公式(3)及(6)可推出：
式中，u为左右履带速度差，即为控制量；
S5：在农机自动驾驶作业过程中，为保证农作物耕种质量，拖拉机设置做匀速直线运动，因此可得：
S6：在实际情况中，由于地面起伏变化、GNSS双天线安装偏差因素引起航向角偏差，导致路径跟踪效果变差，为简化模型，可近似认为航向角偏差为恒定值，则有：
S7：通过S1-S6过程，构建的履带式拖拉机kalman滤波非线性微分方程模型如下：
S8：选取东向位移坐标分量x、北向位移坐标分量y、拖拉机速度v、拖拉机航向角
以及航向角偏差δ作为系统状态量，以东向位移坐标分量x、北向位移坐标分量y、拖拉机速度v、拖拉机航向角
作为系统观测量，则有：
其中，X为系统估计向量，Z为系统观测向量；
S9：S7中构建的履带式拖拉机kalman滤波模型为非线性模型，采用EKF滤波算法，通过求取雅可比矩阵，对模型进行线性化得到对应的系统线性状态空间方程：
S10：将连续系统离散化得到状态转移矩阵Φ以及观测矩阵H：
S11：选取过程噪声协方差矩阵Q以及观测噪声协方差矩阵R：
S12：初始化状态向量X、协方差矩阵P以及观测状态向量Z：
X(0)＝E[X(0)] (19)；
P(0)＝var[X(0)] (20)；
Z(0)＝Z
0 (21)；
其中，X(0)、P(0)、Z(0)分别为状态向量X、协方差矩阵P以及观测状态向量Z的初始值；
S13：Kalman滤波状态一步预测：
S14：计算一步预测协方差矩阵：
P(k+1|k)＝Φ(k+1|k)P(k|k)Φ
T(k+1|k)+Q(k+1) (23)；
S15：计算Kalman增益：
K(k+1)＝P(k+1|k)H
T(k+1)[H(k+1)P(k+1|k)H
T(k+1)+R(k+1)]
-1 (24)；
S16：计算估计值：
S17：更新协方差矩阵：
P(k+1)＝[I-K(k+1)H(k+1)]P(k+1|k) (26)；
其中，在公式(22-26)中，k+1表示下一时刻，k表示当前时刻，
为系统状态估计量，K为kalman增益。
优选的，还包括步骤S18，不断重复步骤S13-S17。
优选的，还包括步骤S19，利用实际数据进行离线分析调试的过程中，调节观测噪声矩阵、过程噪声矩阵、以及协方差矩阵以达到期望滤波效果，并估计出符合实际的航向角偏差对航向角进行补偿。
与现有技术相比，本发明的有益效果是：
(1)本发明实现的履带式拖拉机运动学估计及偏差校准方法能够快速精确估计出由于地面起伏变化、GNSS天线安装偏差等引起的航向角偏差，从而对航向角进行补偿，提高系统对地面的适应性；
(2)本发明通过采用EKF滤波算法能够对履带式拖拉机自动驾驶控制算法数据源进行滤波处理，减少了数据噪声，降低了外界环境干扰因素及系统噪声对履带式拖拉机自动驾驶系统性能的影响，提升了履带式拖拉机自动驾驶系统控制精度以及系统稳定性；
(3)本发明计算量小，实时性高，相比之下，能将履带式拖拉机自动驾驶性能提升25％左右。
图1为履带式拖拉机运动学模型。
图2为kalman滤波估计流程图。
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
参照图1-2，一种履带式拖拉机运动学估计及偏差校准方法，包括以下步骤，
第一步：构建履带式拖拉机运动学模型：
其中，x为履带式拖拉机东向位移坐标分量，y为北向位移坐标分量，v为履带式拖拉机行驶速度，
为履带式拖拉机航向角，ω为履带式拖拉机车体角速度；
第二步：由于履带式拖拉机在进行转向时，左轮、右轮及质心处角速度相等，故可推导出：
其中，v
l为为左侧履带的行驶速度，v
r为为右侧履带的行驶速度，R为转弯半径，b为车体宽度。
第三步：联立方程求解可得：
第四步：由公式(3)及(6)可推出：
式中，u为左右履带速度差，即为控制量；
第五步：在农机自动驾驶作业过程中，为保证农作物耕种质量，拖拉机设置做匀速直线运动，因此可得：
第六步：在实际情况中，由于地面起伏变化、GNSS双天线安装偏差因素，引起航向角偏差，导致路径跟踪效果变差，为简化模型，近似认为航向角偏差为一定值，则有：
本发明的目的则在于通过设计合适的估计方法，能够实时准确地估计出航向角偏差δ，并对航向角进行补偿和校正；
第七步：通过上述过程，本发明构建的履带式拖拉机kalman滤波非线性微分方程模型如下：
第八步：选取东向位移坐标分量x、北向位移坐标分量y、拖拉机速度v、拖拉机航向角
以及航向角偏差δ作为系统状态量，以东 向位移坐标分量x、北向位移坐标分量y、拖拉机速度v、拖拉机航向角
作为系统观测量，则有：
其中，X为系统估计向量，Z为系统观测向量。
第九步：第七步中构建的履带式拖拉机kalman滤波模型为非线性模型，本发明采用EKF滤波算法，通过求取雅可比矩阵，对模型进行线性化得到对应的系统线性状态空间方程：
第十步：将连续系统离散化得到状态转移矩阵Φ以及观测矩阵H：
第十一步：选取过程噪声协方差矩阵Q以及观测噪声协方差矩阵R：
过程噪声协方差矩阵Q：
观测噪声协方差矩阵R的选取：通过采集一段时间内(10～20min)履带式拖拉机自动驾驶系统在静止过程中位姿数据东向位移坐标分量x、北向位移坐标分量y、拖拉机行驶速度v、拖拉机航向角
并分别对各组数据求标准差得到系统观测噪声协方差矩阵R，拖拉机在运行过程中的观测噪声协方差矩阵R为静止时的n＝3倍：
值得注意的是，此处选取的Q、R并不为定值，可根据滤波效果进行修改和调整，直至达到满意效果为止。
第十二步：通过采集履带式拖拉机在运动过程中(自动驾驶模式下)的东向位移坐标分量x、北向位移坐标分量y、拖拉机速度v、拖拉机航向角
控制量u及车体角速度ω，并将其作为观测向量Z；
第十三步：初始化状态向量X、协方差矩阵P以及观测状态向量Z：
X(0)＝E[X(0)] (19)；
Z(0)＝Z
0 (21)；
其中，X(0)、P(0)、Z(0)分别为状态向量X、协方差矩阵P以及观测状态向量Z的初始值，P(0)的大小将直接影响EKF算法的收敛速度；
第十四步：Kalman滤波状态一步预测：
第十五步：计算一步预测协方差矩阵：
P(k+1|k)＝Φ(k+1|k)P(k|k)Φ
T(k+1|k)+Q(k+1) (23)；
第十六步：计算Kalman增益：
K(k+1)＝P(k+1|k)H
T(k+1)[H(k+1)P(k+1|k)H
T(k+1)+R(k+1)]
-1 (24)；
第十七步：计算估计值：
第十八步：更新协方差矩阵：
P(k+1)＝[I-K(k+1)H(k+1)]P(k+1|k) (26)；
其中，在公式(22-26)中，k+1表示下一时刻，k表示当前时 刻，
为系统状态估计量，K为kkalman增益；
第十九步：不断重复步骤第十四-第十八；
本发明利用实际数据进行离线分析调试的过程中，调节观测噪声矩阵R、过程噪声矩阵Q、以及协方差矩阵P(0)以达到期望滤波效果，并估计出符合实际的航向角偏差对航向角进行补偿；
本发明将履带式拖拉机运动学估计及校准方法应用于履带式拖拉机自动驾驶过程，进行在线滤波、数据处理、估计航向角偏差，以达到较好的控制效果；
本发明提出的履带式拖拉机运动学估计及偏差校准方法能够快速精度估计由于地面起伏变化引起的航向角误差，从而提高控制算法对地面起伏变化的适应性，具体包括以下几点：
(1)本发明实现的履带式拖拉机运动学估计及偏差校准方法能够快速精确估计出由于地面起伏变化、GNSS天线安装偏差等引起的航向角偏差，从而对航向角进行补偿，提高系统对地面的适应性；
(2)本发明通过采用EKF滤波算法能够对履带式拖拉机自动驾驶控制算法数据源进行滤波处理，减少了数据噪声，降低了外界环境干扰因素及系统噪声对履带式拖拉机自动驾驶系统性能的影响，提升了履带式拖拉机自动驾驶系统控制精度以及系统稳定性；
(3)本发明计算量小，实时性高，相比之下，能将履带式拖拉机自动驾驶性能提升25％左右。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技 术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。
Claims (3)
- 一种履带式拖拉机运动学估计及偏差校准方法，其特征在于，包括以下步骤，S1，构建履带式拖拉机运动学模型：
其中，x为履带式拖拉机东向位移坐标分量，y为北向位移坐标分量，v为履带式拖拉机行驶速度，为履带式拖拉机航向角，ω为履带式拖拉机车体角速度；
S2：由于履带式拖拉机在进行转向时，左轮、右轮及质心处角速度相等，故可推导出：
其中，v l为为左侧履带的行驶速度，v r为为右侧履带的行驶速度，R为转弯半径，b为车体宽度；S3：联立方程求解可得：
S4：由公式(3)及(6)可推出：
式中，u为左右履带速度差，即为控制量；S5：在农机自动驾驶作业过程中，为保证农作物耕种质量，拖拉机设置做匀速直线运动，因此可得：
S6：在实际情况中，由于地面起伏变化、GNSS双天线安装偏差因素引起航向角偏差，导致路径跟踪效果变差，为简化模型，可近似认为航向角偏差为恒定值，则有：
S7：通过S1-S6过程，构建的履带式拖拉机kalman滤波非线性微分方程模型如下：
S8：选取东向位移坐标分量x、北向位移坐标分量y、拖拉机速度v、拖拉机航向角以及航向角偏差δ作为系统状态量，以东向位移坐标分量x、北向位移坐标分量y、拖拉机速度v、拖拉机航向角
作为系统观测量，则有：
其中，X为系统估计向量，Z为系统观测向量；S9：S7中构建的履带式拖拉机kalman滤波模型为非线性模型，采用EKF滤波算法，通过求取雅可比矩阵，对模型进行线性化得到对 应的系统线性状态空间方程：
S10：将连续系统离散化得到状态转移矩阵Φ以及观测矩阵H：
S11：选取过程噪声协方差矩阵Q以及观测噪声协方差矩阵R：
S12：初始化状态向量X、协方差矩阵P以及观测状态向量Z：X(0)＝E[X(0)] (19)；P(0)＝var[X(0)] (20)；Z(0)＝Z 0 (21)；其中，X(0)、P(0)、Z(0)分别为状态向量X、协方差矩阵P以及观测状态向量Z的初始值；S13：Kalman滤波状态一步预测：
S14：计算一步预测协方差矩阵：P(k+1|k)＝Φ(k+1|k)P(k|k)Φ T(k+1|k)+Q(k+1) (23)；S15：计算Kalman增益：K(k+1)＝P(k+1|k)H T(k+1)[H(k+1)P(k+1|k)H T(k+1)+R(k+1)] -1 (24)；S16：计算估计值：
S17：更新协方差矩阵：P(k+1)＝[I-K(k+1)H(k+1)]P(k+1|k) (26)；其中，在公式(22-26)中，k+1表示下一时刻，k表示当前时刻，为系统状态估计量，K为kalman增益。
- 根据权利要求1所述的一种履带式拖拉机运动学估计及偏差 校准方法，其特征在于，还包括步骤S18，不断重复步骤S13-S17。
- 根据权利要求2所述的一种履带式拖拉机运动学估计及偏差校准方法，其特征在于，还包括步骤S19，利用实际数据进行离线分析调试的过程中，调节观测噪声矩阵、过程噪声矩阵、以及协方差矩阵以达到期望滤波效果，并估计出符合实际的航向角偏差对航向角进行补偿。
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910448762.0 | 2019-05-28 | ||
| CN201910448762.0A CN110262479A (zh) | 2019-05-28 | 2019-05-28 | 一种履带式拖拉机运动学估计及偏差校准方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| WO2020238011A1 true WO2020238011A1 (zh) | 2020-12-03 |
Family
ID=67915572
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| PCT/CN2019/115000 WO2020238011A1 (zh) | 2019-05-28 | 2019-11-01 | 一种履带式拖拉机运动学估计及偏差校准方法 |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN110262479A (zh) |
| WO (1) | WO2020238011A1 (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114200925A (zh) * | 2021-11-10 | 2022-03-18 | 江苏大学 | 基于自适应时域模型预测的拖拉机路径跟踪控制方法及系统 |
| CN114545944A (zh) * | 2022-02-24 | 2022-05-27 | 合肥工业大学 | 一种基于磁钉磁场强度纠正的agv航向定位导航方法 |
| CN117270535A (zh) * | 2023-09-25 | 2023-12-22 | 青岛农业大学 | 一种适用于履带式薯类收获机的辅助导航系统及控制方法 |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110262479A (zh) * | 2019-05-28 | 2019-09-20 | 南京天辰礼达电子科技有限公司 | 一种履带式拖拉机运动学估计及偏差校准方法 |
| CN111532337B (zh) * | 2020-05-18 | 2022-01-07 | 新乡北方车辆仪表有限公司 | 一种用于综合双流电传动的控制方法 |
| CN111766879A (zh) * | 2020-06-24 | 2020-10-13 | 天津大学 | 一种基于自主协同导航的智能车编队系统 |
| CN112146561B (zh) * | 2020-09-09 | 2021-04-02 | 无锡卡尔曼导航技术有限公司 | 一种霍尔角度传感器安装角度偏置的估计方法 |
| CN112147656B (zh) * | 2020-09-09 | 2021-05-04 | 无锡卡尔曼导航技术有限公司 | 一种gnss双天线航向安装角度偏置估计方法 |
| CN113900126B (zh) * | 2021-12-07 | 2022-03-25 | 广东皓行科技有限公司 | 双天线位置确定方法及装置 |
| CN115480579A (zh) * | 2022-10-13 | 2022-12-16 | 华侨大学 | 履带式移动机械及其既定轨迹跟踪控制方法、装置、介质 |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH03266173A (ja) * | 1990-03-16 | 1991-11-27 | Nec Corp | 方程式解析装置 |
| WO2008018188A1 (fr) * | 2006-08-08 | 2008-02-14 | Kyoto University | dispositif de décomposition de valeur propre et procédé de décomposition de valeur propre |
| CN102673569A (zh) * | 2012-05-25 | 2012-09-19 | 同济大学 | 车辆状态测算装置、方法及使用该装置的车辆 |
| CN206161820U (zh) * | 2016-11-04 | 2017-05-10 | 首都师范大学 | 一种基于扩展卡尔曼粒子滤波的系统 |
| CN107633141A (zh) * | 2017-09-22 | 2018-01-26 | 中国水利水电科学研究院 | 一种一维水动力模型泵站特性曲线系数的辨识方法 |
| CN107804315A (zh) * | 2017-11-07 | 2018-03-16 | 吉林大学 | 一种考虑驾驶权实时分配的人车协同转向控制方法 |
| CN107991060A (zh) * | 2017-11-20 | 2018-05-04 | 南京航空航天大学 | 基于自适应和迭代算法的载荷分布式光纤辨识方法 |
| CN108279025A (zh) * | 2017-12-22 | 2018-07-13 | 中国船舶重工集团公司第七0七研究所 | 一种基于重力信息的光纤陀螺罗经快速精对准方法 |
| CN108454628A (zh) * | 2018-04-17 | 2018-08-28 | 吉林大学 | 一种驾驶员在环的人车协同转向滚动优化控制方法 |
| CN109240082A (zh) * | 2018-08-25 | 2019-01-18 | 南京理工大学 | 一种履带式移动机器人滑模云模型交叉耦合控制方法 |
| CN110262479A (zh) * | 2019-05-28 | 2019-09-20 | 南京天辰礼达电子科技有限公司 | 一种履带式拖拉机运动学估计及偏差校准方法 |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CZ20031848A3 (cs) * | 2000-12-27 | 2003-12-17 | Hoei Shokai Co., Ltd. | Kontejner k přepravě roztavených kovů |
| CN107991110B (zh) * | 2017-11-29 | 2019-11-12 | 安徽省一一通信息科技有限公司 | 一种履带式机器人滑动参数检测方法 |
| CN108592911B (zh) * | 2018-03-23 | 2021-09-17 | 南京航空航天大学 | 一种四旋翼飞行器动力学模型/机载传感器组合导航方法 |
| CN108438048B (zh) * | 2018-04-04 | 2021-05-14 | 上海华测导航技术股份有限公司 | 一种新型的履带式拖拉机自动转向控制系统及控制方法 |
| CN108693773B (zh) * | 2018-04-04 | 2021-03-23 | 南京天辰礼达电子科技有限公司 | 一种农业机械自动驾驶滑坡偏差自适应估计方法 |
-
2019
- 2019-05-28 CN CN201910448762.0A patent/CN110262479A/zh active Pending
- 2019-11-01 WO PCT/CN2019/115000 patent/WO2020238011A1/zh active Application Filing
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH03266173A (ja) * | 1990-03-16 | 1991-11-27 | Nec Corp | 方程式解析装置 |
| WO2008018188A1 (fr) * | 2006-08-08 | 2008-02-14 | Kyoto University | dispositif de décomposition de valeur propre et procédé de décomposition de valeur propre |
| CN102673569A (zh) * | 2012-05-25 | 2012-09-19 | 同济大学 | 车辆状态测算装置、方法及使用该装置的车辆 |
| CN206161820U (zh) * | 2016-11-04 | 2017-05-10 | 首都师范大学 | 一种基于扩展卡尔曼粒子滤波的系统 |
| CN107633141A (zh) * | 2017-09-22 | 2018-01-26 | 中国水利水电科学研究院 | 一种一维水动力模型泵站特性曲线系数的辨识方法 |
| CN107804315A (zh) * | 2017-11-07 | 2018-03-16 | 吉林大学 | 一种考虑驾驶权实时分配的人车协同转向控制方法 |
| CN107991060A (zh) * | 2017-11-20 | 2018-05-04 | 南京航空航天大学 | 基于自适应和迭代算法的载荷分布式光纤辨识方法 |
| CN108279025A (zh) * | 2017-12-22 | 2018-07-13 | 中国船舶重工集团公司第七0七研究所 | 一种基于重力信息的光纤陀螺罗经快速精对准方法 |
| CN108454628A (zh) * | 2018-04-17 | 2018-08-28 | 吉林大学 | 一种驾驶员在环的人车协同转向滚动优化控制方法 |
| CN109240082A (zh) * | 2018-08-25 | 2019-01-18 | 南京理工大学 | 一种履带式移动机器人滑模云模型交叉耦合控制方法 |
| CN110262479A (zh) * | 2019-05-28 | 2019-09-20 | 南京天辰礼达电子科技有限公司 | 一种履带式拖拉机运动学估计及偏差校准方法 |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114200925A (zh) * | 2021-11-10 | 2022-03-18 | 江苏大学 | 基于自适应时域模型预测的拖拉机路径跟踪控制方法及系统 |
| CN114200925B (zh) * | 2021-11-10 | 2024-05-14 | 江苏大学 | 基于自适应时域模型预测的拖拉机路径跟踪控制方法及系统 |
| CN114545944A (zh) * | 2022-02-24 | 2022-05-27 | 合肥工业大学 | 一种基于磁钉磁场强度纠正的agv航向定位导航方法 |
| CN114545944B (zh) * | 2022-02-24 | 2024-04-16 | 合肥工业大学 | 一种基于磁钉磁场强度纠正的agv航向定位导航方法 |
| CN117270535A (zh) * | 2023-09-25 | 2023-12-22 | 青岛农业大学 | 一种适用于履带式薯类收获机的辅助导航系统及控制方法 |
| CN117270535B (zh) * | 2023-09-25 | 2024-03-12 | 青岛农业大学 | 一种适用于履带式薯类收获机的辅助导航系统及控制方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110262479A (zh) | 2019-09-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2020238011A1 (zh) | 一种履带式拖拉机运动学估计及偏差校准方法 | |
| CN106950586A (zh) | 用于农机作业的gnss/ins/车辆组合导航方法 | |
| CN111238471B (zh) | 一种适用于农业机械直线导航的侧滑角度估计方法及估计器 | |
| CN110530361B (zh) | 基于农业机械双天线gnss自动导航系统的转向角度估计器 | |
| CN111703432B (zh) | 一种智能履带车辆滑动参数实时估计方法 | |
| CN106772517A (zh) | 基于双天线gnss接收机/陀螺仪信息融合的农机横滚角测试方法 | |
| CN109115225A (zh) | 一种无人作业谷物联合收割机导航方法与导航装置 | |
| CN105987696A (zh) | 一种低成本车辆自动驾驶设计实现方法 | |
| CN108438048B (zh) | 一种新型的履带式拖拉机自动转向控制系统及控制方法 | |
| CN110716565B (zh) | 一种履带式车辆导航轨迹跟踪控制系统 | |
| CN112146561B (zh) | 一种霍尔角度传感器安装角度偏置的估计方法 | |
| AU2020104234A4 (en) | An Estimation Method and Estimator for Sideslip Angle of Straight-line Navigation of Agricultural Machinery | |
| WO2019173769A1 (en) | Kalman filter for an autonomous work vehicle system | |
| CN107943060A (zh) | 一种自动驾驶仪、沿着跟踪直线引导车辆的方法以及计算机可读介质 | |
| Miao et al. | Steering angle adaptive estimation system based on GNSS and MEMS gyro | |
| CN111189454A (zh) | 基于秩卡尔曼滤波的无人车slam导航方法 | |
| CN112147656B (zh) | 一种gnss双天线航向安装角度偏置估计方法 | |
| CN113008229B (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
| CN104132664A (zh) | 一种农用履带机器人滑动量的估计方法 | |
| Bevly | High speed, dead reckoning, and towed implement control for automatically steered farm tractors using GPS | |
| CN116338719A (zh) | 基于b样条函数的激光雷达-惯性-车辆融合定位方法 | |
| CN113721609B (zh) | 一种侧滑情况下4wid高地隙喷雾机轨迹跟踪控制方法 | |
| Li et al. | Development of the automatic navigation system for combine harvester based on GNSS | |
| CN115790629A (zh) | 一种自动驾驶观光车路径跟踪精度检测方法 | |
| CN112684483B (zh) | 基于卫星和视觉融合的导航偏差感知及其信息获取方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| 121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 19931429 Country of ref document: EP Kind code of ref document: A1 |
|
| NENP | Non-entry into the national phase |
Ref country code: DE |
|
| 122 | Ep: pct application non-entry in european phase |
Ref document number: 19931429 Country of ref document: EP Kind code of ref document: A1 |