CN112904393B - 一种导航路径几何约束辅助的农业机械自主导航方法 - Google Patents
一种导航路径几何约束辅助的农业机械自主导航方法 Download PDFInfo
- Publication number
- CN112904393B CN112904393B CN202110070855.1A CN202110070855A CN112904393B CN 112904393 B CN112904393 B CN 112904393B CN 202110070855 A CN202110070855 A CN 202110070855A CN 112904393 B CN112904393 B CN 112904393B
- Authority
- CN
- China
- Prior art keywords
- agricultural machinery
- measurement
- matrix
- autonomous navigation
- navigation
- 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
- 238000000034 method Methods 0.000 title claims abstract description 30
- 238000005259 measurement Methods 0.000 claims abstract description 53
- 239000013598 vector Substances 0.000 claims abstract description 26
- 239000011159 matrix material Substances 0.000 claims abstract description 25
- 230000007704 transition Effects 0.000 claims abstract description 10
- 230000003321 amplification Effects 0.000 claims abstract description 9
- 238000003199 nucleic acid amplification method Methods 0.000 claims abstract description 9
- 238000001914 filtration Methods 0.000 claims description 5
- 230000003416 augmentation Effects 0.000 claims description 3
- 238000004378 air conditioning Methods 0.000 abstract 1
- 238000007500 overflow downdraw method Methods 0.000 description 6
- 230000004927 fusion Effects 0.000 description 3
- 230000007246 mechanism Effects 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/43—Determining position using carrier phase measurements, e.g. kinematic positioning; using long or short baseline interferometry
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了一种导航路径几何约束辅助的农业机械自主导航方法，属于智能农业机械领域。本发明利用自动驾驶系统和自动导航系统的历史数据，结合农机导航路径的几何形状约束和速度方向约束，构造扩增状态向量和扩增量测向量；利用自动驾驶系统路径跟踪结果，设置量测噪声方差的初始值，在线更新量测方差矩阵基于状态转移矩阵系统噪声方差矩阵量测矩阵及得到扩增状态估计值的前6个元素作为k时刻自主导航系统的状态输出。本发明能提高农机自主导航系统的精度、可靠性和自适应性。
Description
技术领域
本发明涉及智能农业机械领域，具体涉及一种导航路径几何约束辅助的农业机械自主导航方法。
背景技术
农业机械自动驾驶基于导航定位感知系统、路径规划软件、转角传感器和转向执行机构实现农机的导航控制决策与路径跟踪。导航定位感知系统的精度和可靠性对自动驾驶系统性能影响较大，现有的自动驾驶系统多基于高精度卫星定位、视觉导航、惯性导航及其多源数据融合方法实现农机位姿的实时更新。单一的导航系统常常难以满足实际生产的需要，卫星定位易受传输空间电磁波干扰和环境遮挡的影响，视觉传感器受光照环境影响显著，惯性导航解算误差随时间累积，因此多源数据融合的导航方法能取长补短，实现高精度高可靠的导航参数更新，受到广泛的关注。常规的组合导航系统其性能受传感器精度和数据融合方法影响较大，近几年先进的数据融合方法获得快速发展，但高精度的几何空间定位系统价格仍居高不下，且先进数据融合方法对系统硬件要求也较高，造成研制高精度、高更新率的组合导航系统成本较高，且该状况短时间内难以改变，阻碍了农机自动驾驶系统的大面积推广应用。
发明内容
针对现有技术中存在不足，本发明提供了一种导航路径几何约束辅助的农业机械自主导航方法，改善农机自主导航系统的精度与可靠性，提高自主导航系统数据融合方法的自适应性。
本发明是通过以下技术手段实现上述技术目的的。
一种导航路径几何约束辅助的农业机械自主导航方法，利用自动驾驶系统和自动导航系统的历史数据，结合农机导航路径的几何形状约束和速度方向约束，构造扩增状态向量和扩增量测向量；利用自动驾驶系统路径跟踪结果，实现量测噪声方差初始值的设置，在线更新量测方差矩阵/>基于状态转移矩阵/>系统噪声方差矩阵/>量测矩阵及/>得到扩增状态估计值/>的前6个元素即为k时刻自主导航系统的状态输出。
进一步的技术方案，所述扩增量测向量为其中zk为量测向量，/>为约束量测向量，且/>(xk，yk)、(xk-1，yk-1)分别为k、k-1时刻农机在高斯平面坐标系下的位置，/>分别为k、k-1时刻的农机速度。
更进一步的技术方案，所述k、k-1时刻农机在高斯平面坐标系下的位置和农机速度满足如下关系式：
更进一步的技术方案，所述量测矩阵和量测方差矩阵/>分别为：
其中，d表示k时刻之前的滤波时间单位；分别表示x轴向位置、y轴向位置、速度、航向角、转向角、横向偏差和横向偏差一阶差分对应的量测噪声方差；/>分别表示速度vk在x、y轴向上量测分量。
进一步的技术方案，所述扩增量测向量为d表示k时刻之前的滤波时间单位。
进一步的技术方案，所述状态转移矩阵和系统噪声方差矩阵/>分别为：
其中，Φk为状态量xk的状态转移矩阵，I为单位矩阵，Qk为量测噪声。
进一步的技术方案，所述利用自动驾驶系统路径跟踪结果，实现量测噪声方差初始值的设置，具体为：记录至k时刻路径跟踪横向偏差集合/>其中t为初始化时间内路径跟踪结果记录的数据个数，且t＞2；由/>计算得到横向偏差一阶差分量/>其中/>计算/>的方差，作为量测噪声方差/>的初始值。
更进一步的技术方案，所述量测方差阵采用Sage-Husa最大后验估计在线更新。
本发明提出了一种导航路径几何约束辅助的农业机械自主导航方法，相比现有技术，具有以下有益效果：
(1)本发明利用农业机械导航路径往返式直线作业和速度方向恒定的特征，构造自主导航系统的量测约束方程，基于量测约束条件构造约束量测向量，进一步建立扩增量测向量；本发明提高数据融合中先验信息的权重，降低数据融合对传感器量测精度的依赖性；在不增加额外传感器的情况下，改善了农机自主导航系统的精度与可靠性；
(2)本发明基于自动驾驶路径跟踪结果中的横向偏差及其差分形式，构造约束量测向量方差矩阵，实现量测噪声方差初始值的设置，避免了容积卡尔曼滤波器量测更新中的/>秩亏引起的数值计算奇异性问题，提高了自主导航系统数据融合方法的自适应性。
附图说明
图1为本发明所述农机载体坐标系、高斯平面坐标系和导航路径示意图；
图2为本发明导航路径几何约束辅助的农业机械自主导航流程图。
具体实施方式
下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。
如图1所示，一种导航路径几何约束辅助的农业机械自主导航方法基于现有的自主导航系统(本实施例的自主导航系统选用双天线北斗RTK定位系统)，利用高斯平面坐标系(g系，原点为og、横坐标为x、纵坐标为y)下的农机导航路径几何形状约束和农机载体坐标系(b系，原点为ob、横坐标为xb、纵坐标为yb)下的速度方向约束，降低农机组合导航系统对传感器成本的敏感性，其中vk、Δk分别表示k时刻的航向角、速度及横向偏差，Δk-1表示k-1时刻的横向偏差，Δk-2表示k-2时刻的横向偏差。
如图2所示，所述导航路径几何约束辅助的农业机械自主导航方法基于高斯滤波框架下的CKF算法实现，利用自动驾驶系统和自动导航系统的历史数据，结合农机导航路径的几何形状约束和速度方向约束，构造扩增状态向量和扩增量测向量，利用自动驾驶系统路径跟踪结果，实现量测噪声方差初始值的设置，在线更新量测方差矩阵/>基于及/>得到扩增状态估计值/>的前6个元素即为k时刻自主导航系统的状态输出。
所述导航路径几何约束辅助的农业机械自主导航方法在高斯滤波框架下实现，自主导航系统的状态方程和量测方程分别如下所示：
xk+1＝Φkxk+wk (1)
zk＝Hkxk+vk (2)
其中分别表示k时刻的状态转移矩阵和量测矩阵，/> 分别为k时刻的系统噪声和量测噪声，x0(初始状态)、wk、vk相互独立且满足分别表示初始状态、系统噪声和量测噪声，/>表示均值为/>方差为P的高斯分布；/>为实数域，m为量测维数，n为状态维数。
所述自主导航系统的状态量依次表示k时刻平面坐标系中的x轴向位置和速度、y轴向位置和速度、航向角、转向角，即n＝6；以双天线北斗RTK定位系统输出位置(xk，yk)和航向角/>转速传感器测量速度vk及转角传感器输出转角δk为量测输入，设容积卡尔曼滤波器的更新周期为T，即m＝5，根据农业机械运动学方程可定义状态转移矩阵为：
其中Φ66＝1-T/T，/>分别表示k时刻滤波器输出的状态结果，l为农机轴距，τ为转向机构的惯性时间常数。定义量测向量/>对应的量测矩阵为：
其中分别表示速度vk在x、y轴向上量测分量。
所述自主导航系统的量测方程特征为：利用农业机械田间作业过程往返式直线路径中位置(几何形状约束)和速度方向约束，建立直线路径作业时自主导航系统量测约束方程，其形式如下：
其中(xk，yk)、(xk-1，yk-1)、(xk-2，yk-2)为农机在k、k-1、k-2时刻在高斯平面坐标系下的位置，为对应时刻的速度。考虑到上述方程所含信息的冗余性，量测约束条件可进一步简化成：
加入量测约束条件后可构造约束量测向量其形式如下：
自主导航系统的扩增量测向量同时考虑量测向量和约束量测向量，即扩增量测向量其对应的量测矩阵/>和量测噪声方差矩阵/>定义为：
其中d表示k时刻之前的滤波时间单位，分别表示x轴向位置、y轴向位置、速度、航向角、转向角、横向偏差和横向偏差一阶差分对应的量测噪声方差。
所述自主导航系统的状态方程特征为：扩增状态向量对应的状态转移矩阵/>和系统噪声方差矩阵/>定义为：
其中，I为单位矩阵。
为了与约束量测方程(6)对应，优选d＝2，以减少计算量。
利用自动驾驶系统路径跟踪结果，实现约束量测向量方差矩阵的设置与更新，步骤为：记录至k时刻路径跟踪横向偏差集合/>其中t为初始化时间内路径跟踪结果记录的数据个数，且t＞2，由/>计算得到横向偏差一阶差分量其中/>k-t-1≤j≤k，计算/>的方差作为量测噪声方差/>的初始设置值。
本发明的导航路径几何约束辅助的农业机械自主导航方法采用Sage-Husa最大后验估计在线更新量测方差阵实现组合导航状态变量的自适应估计与配置更新。
本发明的导航路径几何约束辅助的农业机械自主导航方法利用容积卡尔曼滤波器，基于及/>得到扩增状态估计值/>(为现有技术)，其中/>的前6个元素即为k时刻自主导航系统的状态输出。
所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。
Claims (7)
1.一种导航路径几何约束辅助的农业机械自主导航方法，其特征在于，利用自动驾驶系统和自动导航系统的历史数据，结合农机导航路径的几何形状约束和速度方向约束，构造扩增状态向量和扩增量测向量；利用自动驾驶系统路径跟踪结果，实现量测噪声方差初始值的设置，在线更新量测方差矩阵/>基于状态转移矩阵/>系统噪声方差矩阵/>量测矩阵/>及/>得到扩增状态估计值/>的前6个元素即为k时刻自主导航系统的状态输出；
所述利用自动驾驶系统路径跟踪结果，实现量测噪声方差初始值的设置，具体为：记录至k时刻路径跟踪横向偏差集合/>其中t为初始化时间内路径跟踪结果记录的数据个数，且t＞2；由/>计算得到横向偏差一阶差分量/>其中/>计算/>的方差，作为量测噪声方差/>的初始值。
2.根据权利要求1所述的农业机械自主导航方法，其特征在于，所述扩增量测向量为其中zk为量测向量，/>为约束量测向量，且/>(xk，yk)、(xk-1，yk-1)分别为k、k-1时刻农机在高斯平面坐标系下的位置，/>分别为k、k-1时刻的农机速度。
3.根据权利要求2所述的农业机械自主导航方法，其特征在于，所述k、k-1时刻农机在高斯平面坐标系下的位置和农机速度满足如下关系式：
4.根据权利要求2所述的农业机械自主导航方法，其特征在于，所述量测矩阵和量测方差矩阵/>分别为：
其中，d表示k时刻之前的滤波时间单位；分别表示x轴向位置、y轴向位置、速度、航向角、转向角、横向偏差和横向偏差一阶差分对应的量测噪声方差；分别表示速度vk在x、y轴向上量测分量。
5.根据权利要求1所述的农业机械自主导航方法，其特征在于，所述扩增状态向量为d表示k时刻之前的滤波时间单位。
6.根据权利要求5所述的农业机械自主导航方法，其特征在于，所述状态转移矩阵和系统噪声方差矩阵/>分别为：
其中，Φk为状态量xk的状态转移矩阵，I为单位矩阵，Qk为量测噪声。
7.根据权利要求1所述的农业机械自主导航方法，其特征在于，所述量测方差矩阵采用Sage-Husa最大后验估计在线更新。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110070855.1A CN112904393B (zh) | 2021-01-19 | 2021-01-19 | 一种导航路径几何约束辅助的农业机械自主导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110070855.1A CN112904393B (zh) | 2021-01-19 | 2021-01-19 | 一种导航路径几何约束辅助的农业机械自主导航方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112904393A CN112904393A (zh) | 2021-06-04 |
| CN112904393B true CN112904393B (zh) | 2023-11-10 |
Family
ID=76116048
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110070855.1A Active CN112904393B (zh) | 2021-01-19 | 2021-01-19 | 一种导航路径几何约束辅助的农业机械自主导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112904393B (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105973238A (zh) * | 2016-05-09 | 2016-09-28 | 郑州轻工业学院 | 一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法 |
| WO2018026544A1 (en) * | 2016-07-22 | 2018-02-08 | Regents Of The University Of Minnesota | Square-root multi-state constraint kalman filter for vision-aided inertial navigation system |
| CN108871365A (zh) * | 2018-07-06 | 2018-11-23 | 哈尔滨工业大学 | 一种航向约束下的状态估计方法及系统 |
| CN109668562A (zh) * | 2017-10-13 | 2019-04-23 | 北京航空航天大学 | 一种考虑偏差时引入伪测量的重力梯度运动学导航方法 |
| CN109945859A (zh) * | 2019-04-01 | 2019-06-28 | 东南大学 | 一种自适应h∞滤波的运动学约束捷联惯性导航方法 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6826478B2 (en) * | 2002-04-12 | 2004-11-30 | Ensco, Inc. | Inertial navigation system for mobile objects with constraints |
| US8659474B2 (en) * | 2011-01-12 | 2014-02-25 | Navcom Technology, Inc. | Navigation system and method for resolving integer ambiguities using double difference ambiguity constraints |
-
2021
- 2021-01-19 CN CN202110070855.1A patent/CN112904393B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105973238A (zh) * | 2016-05-09 | 2016-09-28 | 郑州轻工业学院 | 一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法 |
| WO2018026544A1 (en) * | 2016-07-22 | 2018-02-08 | Regents Of The University Of Minnesota | Square-root multi-state constraint kalman filter for vision-aided inertial navigation system |
| CN109668562A (zh) * | 2017-10-13 | 2019-04-23 | 北京航空航天大学 | 一种考虑偏差时引入伪测量的重力梯度运动学导航方法 |
| CN108871365A (zh) * | 2018-07-06 | 2018-11-23 | 哈尔滨工业大学 | 一种航向约束下的状态估计方法及系统 |
| CN109945859A (zh) * | 2019-04-01 | 2019-06-28 | 东南大学 | 一种自适应h∞滤波的运动学约束捷联惯性导航方法 |
Non-Patent Citations (3)
| Title |
|---|
| A robust strong tracking cubature Kalman filter for spacecraft attitude estimation with quaternion constraint;Wei Huang 等;《Acta Astronautica》;第153-163页 * |
| 四元数约束的容积卡尔曼滤波及其应用;钱华明 等;《哈尔滨工业大学学报》;第46卷(第7期);第40-46页 * |
| 带边界约束的容积卡尔曼滤波;刘志彬 等;《中国海洋大学学报(自然科学版)》;第48卷(第6期);第109-113页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112904393A (zh) | 2021-06-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113311411B (zh) | 一种用于移动机器人的激光雷达点云运动畸变校正方法 | |
| CN110706279A (zh) | 基于全局地图与多传感器信息融合的全程位姿估计方法 | |
| CN105136145A (zh) | 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法 | |
| Xiong et al. | G-VIDO: A vehicle dynamics and intermittent GNSS-aided visual-inertial state estimator for autonomous driving | |
| CN112747750B (zh) | 一种基于单目视觉里程计和imu融合的定位方法 | |
| CN111795686A (zh) | 一种移动机器人定位与建图的方法 | |
| CN110988894B (zh) | 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法 | |
| CN113203429B (zh) | 一种陀螺仪温度漂移误差的在线估计及补偿方法 | |
| CN112083457B (zh) | 一种神经网络优化的imm卫星定位导航方法 | |
| CN111890373A (zh) | 车载机械臂的感知定位方法 | |
| CN110209180A (zh) | 一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法 | |
| CN110174907A (zh) | 一种基于自适应卡尔曼滤波的人体目标跟随方法 | |
| CN113280808A (zh) | 一种移动机器人定位精度提升方法及系统 | |
| CN115752507A (zh) | 基于二维码导航的在线单舵轮agv参数标定方法及系统 | |
| CN115014347A (zh) | 一种快速可观测度分析及其指导的多传感器信息融合方法 | |
| CN112904393B (zh) | 一种导航路径几何约束辅助的农业机械自主导航方法 | |
| CN116608864B (zh) | 一种通信时延影响下基于因子图的auv协同定位方法 | |
| CN117387604A (zh) | 基于4d毫米波雷达和imu融合的定位与建图方法及系统 | |
| CN116380070A (zh) | 一种基于时间戳优化的视觉惯性定位方法 | |
| CN115014321B (zh) | 一种基于自适应鲁棒滤波的仿生偏振多源融合定向方法 | |
| CN116338719A (zh) | 基于b样条函数的激光雷达-惯性-车辆融合定位方法 | |
| CN115930959A (zh) | 视觉初始化方法、装置及飞行汽车 | |
| WO2022237375A1 (zh) | 用于定位装置和里程计的标定方法、程序产品和标定装置 | |
| Wang et al. | SW-LIO: A Sliding Window Based Tightly Coupled LiDAR-Inertial Odometry | |
| CN114353786B (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 |