CN114674271A - 单天线gnss辅助陀螺仪的农机转向角测量方法及装置 - Google Patents
单天线gnss辅助陀螺仪的农机转向角测量方法及装置 Download PDFInfo
- Publication number
- CN114674271A CN114674271A CN202210129973.XA CN202210129973A CN114674271A CN 114674271 A CN114674271 A CN 114674271A CN 202210129973 A CN202210129973 A CN 202210129973A CN 114674271 A CN114674271 A CN 114674271A
- Authority
- CN
- China
- Prior art keywords
- steering angle
- gyroscope
- measurement
- agricultural machine
- agricultural
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims description 19
- 238000005259 measurement Methods 0.000 claims abstract description 69
- 238000001914 filtration Methods 0.000 claims abstract description 12
- 230000008859 change Effects 0.000 claims abstract description 11
- 238000000691 measurement method Methods 0.000 claims abstract description 5
- 230000001360 synchronised effect Effects 0.000 claims abstract description 5
- 239000011159 matrix material Substances 0.000 claims description 12
- XYDVHKCVOMGRSY-UHFFFAOYSA-N 4-(4-benzylphenyl)-1,3-thiazol-2-amine Chemical compound S1C(N)=NC(C=2C=CC(CC=3C=CC=CC=3)=CC=2)=C1 XYDVHKCVOMGRSY-UHFFFAOYSA-N 0.000 claims description 8
- 101001050286 Homo sapiens Jupiter microtubule associated homolog 1 Proteins 0.000 claims description 8
- 101000928034 Homo sapiens Proteasomal ubiquitin receptor ADRM1 Proteins 0.000 claims description 8
- 102100023133 Jupiter microtubule associated homolog 1 Human genes 0.000 claims description 8
- 102100036915 Proteasomal ubiquitin receptor ADRM1 Human genes 0.000 claims description 8
- 230000035945 sensitivity Effects 0.000 claims description 2
- 238000009434 installation Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 230000007246 mechanism Effects 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
- 230000008054 signal transmission Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
- G01B21/00—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant
- G01B21/22—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant for measuring angles or tapers; for testing the alignment of axes
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Guiding Agricultural Machines (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
Abstract
本发明提供了一种单天线GNSS辅助陀螺仪的农机转向角测量方法及装置，测量方法包括：基于农机航向角变化率和作业速度，计算出农机期望转向角；由k时刻的期望转向角
和陀螺仪原始量测计算的转向角
进行卡尔曼滤波更新，估计出陀螺仪测量偏置bg,k和k时刻转向角测量误差δθk；以滤波周期Δt内陀螺仪输出角速率ωk减去农机航向角变化率
并积分，可得补偿后陀螺仪计算的转向角；利用补偿后陀螺仪计算的转向角和所述δθk，得到k时刻农机转向角；最终实现农机左、右转向角的同步测量输出。本发明能以任意更新率输出农机航向角及航向角速率，补偿由GNSS量测延迟引入的转向角计算时滞。
Description
技术领域
本发明属于智能农业机械技术领域，具体涉及一种单天线GNSS辅助陀螺仪的农机转角测量方法及测量装置。
背景技术
农业机械转向角度测量是农机自动驾驶系统的重要组成部分，其测量精度和可靠性对行驶控制的稳定性影响显著。传统的基于接触式连杆机构测量转角的方法，受传感器安装误差影响较大，且水田深泥脚作业时容易导致支撑轴与连杆间几何位置形变，产生角度测量误差。近年来随着微惯性技术的发展，基于陀螺仪的非接触式转角测量方法受到广泛关注，其具有安装简单、维护方便等特点。繆存孝等人在“基于双GNSS天线及单陀螺的车轮转角测量系统”(农业机械学报，48(9):17-23,2017)中，利用角速率陀螺积分角度和车辆运动学模型计算的转向角度差值，实现陀螺仪偏置和角度计算误差的补偿。上述方法不适用于单天线GNSS配置下农机转角测量，且其直接基于GNSS速度量测进行理论转角计算，更新率低且受制于GNSS信号传输稳定性。
发明内容
有鉴于此，本发明提供一种单天线GNSS辅助陀螺仪的农机转角测量方法及测量装置。
本发明是通过以下技术手段实现上述技术目的的。
单天线GNSS辅助陀螺仪的农机转角测量方法，具体为：
基于农机航向角变化率和作业速度，计算出农机期望转向角；
由k时刻的期望转向角
和陀螺仪原始量测计算的转向角
进行卡尔曼滤波更新，估计出陀螺仪测量偏置bg,k和k时刻转向角测量误差δθk；
以滤波周期Δt内陀螺仪输出角速率ωk减去农机航向角变化率
并积分，可得补偿后陀螺仪计算的转向角；利用补偿后陀螺仪计算的转向角和所述δθk，得到k时刻农机转向角；最终实现农机左、右转向角的同步测量输出。
进一步的技术方案，所述陀螺仪输出角速率ωk由陀螺仪敏感角速率减去陀螺仪测量偏置得到。
进一步的技术方案，所述农机期望转向角的计算公式为：
其中：L表示农机轴距，
表示tn时刻农机航向角变化率，V(tn)表示tn时刻农机的速度。
更进一步的技术方案，所述tn时刻农机的速度的计算公式为：
其中：
为未知矩阵的估计值，t0为初始时刻。
更进一步的技术方案，所述未知矩阵的估计值满足：
其中：N为位置序列的点数，
为未知矩阵的估计值，Δn＝tn-t0为时刻tn至初始时刻t0的时间间隔，P表示轨迹方程。
更进一步的技术方案，所述轨迹方程由GNSS定位天线输出的N+1个位置点序列(P0…PN)构造而成。
进一步的技术方案，由k时刻的期望转向角
和陀螺仪原始量测计算的转向角
进行卡尔曼滤波更新，具体为：以
与
的差值为量测，构造更新量测方程：zk＝Hkxk+vk，其中
为卡尔曼滤波器输入量测数据，vk为转向角测量误差量测模型的零均值高斯噪声，Hk为量测矩阵。
一种单天线GNSS辅助陀螺仪的农机转角测量装置，包括：GNSS定位天线、定位接收机、ARM1微控制器、ARM2微控制器、DSP、FPGA以及两个单轴陀螺仪；
所述GNSS定位天线与定位接收机进行连接，GNSS定位天线和定位接收机提供农机位置数据；
所述两个单轴陀螺仪分别安装在农机左前轮和右前轮转动部件上；
所述ARM1微控制器采集两个单轴陀螺仪的角速率量测并送入FPGA中，所述ARM2微控制器采集定位接收机输出的位置信息并送入FPGA中；
所述FPGA实现陀螺仪和定位接收机数据的同步；
所述DSP基于FPGA，在线估计陀螺测量偏置与转向角误差，进而实现农机前轮左、右转向角的同时测量输出。
本发明的有益效果为：
(1)本发明基于GNSS定位天线输出的N+1个位置点序列构造出一种能以任意更新率输出农机航向角及航向角速率的框架，补偿了基于运动学模型计算期望转向角过程中由GNSS量测延迟引入的转向角计算时滞；
(2)本发明公开了一种基于单天线GNSS量测信息与单轴陀螺仪数据融合实现农业机械转向角测量的方法，实现农机转弯时左、右转向角的同时测量，拓展了单轴陀螺仪实现转角测量的硬件配置场景。
附图说明
图1为本发明所述单天线GNSS辅助陀螺仪的农机转角测量装置结构示意图；
图2为本发明所述单天线GNSS辅助陀螺仪的农机转角测量方法流程图。
具体实施方式
下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。
如图1所示，本发明一种单天线GNSS辅助陀螺仪的农机转角测量装置，包括GNSS定位天线、定位接收机、ARM1微控制器、ARM2微控制器、DSP以及FPGA、电源管理模块、两个单轴陀螺仪(对应图1中的陀螺仪1和陀螺仪2)；其中GNSS定位天线与定位接收机进行连接，GNSS定位天线和定位接收机提供农机位置数据，两个单轴陀螺仪分别安装在农机左前轮和右前轮转动部件上，ARM1微控制器采集两个单轴陀螺仪的角速率量测并送入FPGA中，ARM2微控制器采集定位接收机输出的位置信息并送入FPGA中；FPGA基于数据同步协议和单轴陀螺仪输出频率实现两类传感器(陀螺仪和定位接收机)数据的同步，DSP基于FPGA，在线估计陀螺测量偏置与转向角误差，进而实现农机前轮左、右转向角的同时测量输出；电源管理模块为ARM1微控制器、ARM2微控制器、DSP、FPGA、两个单轴陀螺仪及定位接收机供电。
如图2所示，本发明一种单天线GNSS辅助陀螺仪的农机转向角测量方法，具体包括如下步骤：
步骤(1)，布置GNSS定位天线、定位接收机及单轴陀螺仪
如图1所示，单轴陀螺仪的敏感轴垂直地面安装在左前轮和右前轮车轮转动部件上，GNSS天线安装于农机车体后轮轴心无遮挡位置处，且GNSS定位天线处测量位置和速度代表农机测量位置、速度，ARM1微控制器、ARM2微控制器以及FPGA实现单轴陀螺仪和定位接收机数据的同步采集。
步骤(2)，更新陀螺仪测量偏置和农机转向角测量误差
步骤(2.1)，建立转向角测量误差卡尔曼滤波模型
设状态量为xk＝[δθ bg]T，其中δθ为农机转向角测量误差值，bg为陀螺仪测量偏置；
首先，构造转向角测量误差更新状态方程：
xk＝Φk|k-1xk-1+wk
其中：wk为转向角测量误差系统模型的零均值高斯噪声，Φk|k-1为状态转移矩阵，并定义为：
其中：τ为陀螺仪偏置一阶马尔可夫模型的相关时间常数，Δt为滤波周期；
其次，构造转向角测量误差更新量测方程，单天线GNSS位置拟合转向角(即农机期望转向角)
可提供额外信息以辅助陀螺仪测量误差的估计与补偿，以陀螺仪原始量测计算的转向角
与
的差值的差值为量测，构造更新量测方程：
zk＝Hkxk+vk
其中
为卡尔曼滤波器输入量测数据，vk为转向角测量误差量测模型的零均值高斯噪声，Hk为量测矩阵，其定义为：
Hk＝[1 0]
步骤(2.2)，采集农机位置数据，计算期望转向角
当位置序列长度大于N时，由GNSS定位天线输出的N+1个位置点序列(P0…PN)，可构造轨迹方程：
为求解未知矩阵P0、V0及A0，构造目标函数
进行最小二乘求解，得到：
其中：N为位置序列的点数，
为未知矩阵的估计值，Δn＝tn-t0为时刻tn至初始时刻t0的时间间隔；
进而可得任意时刻tn的速度方程为：
对应时刻tn农机航向角ψ(tn)和航向角变化率有：
ψ(tn)＝atan2(VE(tn),VN(tn))
其中：VE、VN为导航坐标系下的东向和北向速度；
基于农机航向角变化率和作业速度，计算农机期望转向角
的公式为：
其中L表示农机轴距。
步骤(2.3)，补偿陀螺仪测量偏置
设前轮转动角速率真值(即陀螺仪输出角速率)为ω(tk)、陀螺仪敏感角速率为
定义
由k时刻的期望转向角
和陀螺仪原始量测计算的转向角
进行卡尔曼滤波更新，估计出陀螺仪测量偏置bg,k和k时刻转向角测量误差δθk；估计出陀螺仪k时刻测量偏置bg,k后，则
其中
步骤(3)，进行转向角测量与误差补偿
以滤波周期Δt内陀螺仪输出角速率ωk减去农机航向角变化率
并积分，可得补偿后陀螺仪计算的转向角：
则k时刻转向角误差δθk补偿后的农机转向角为：
其中θk为k时刻农机转向角测量结果。
在DSP中重复步骤(2)、(3)，实现农机左、右转向角的同步测量输出。
所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。
Claims (8)
1.单天线GNSS辅助陀螺仪的农机转角测量方法，其特征在于：
基于农机航向角变化率和作业速度，计算出农机期望转向角；
由k时刻的期望转向角
和陀螺仪原始量测计算的转向角
进行卡尔曼滤波更新，估计出陀螺仪测量偏置bg,k和k时刻转向角测量误差δθk；
以滤波周期Δt内陀螺仪输出角速率ωk减去农机航向角变化率
并积分，可得补偿后陀螺仪计算的转向角；利用补偿后陀螺仪计算的转向角和所述δθk，得到k时刻农机转向角；最终实现农机左、右转向角的同步测量输出。
2.根据权利要求1所述的单天线GNSS辅助陀螺仪的农机转角测量方法，其特征在于，所述陀螺仪输出角速率ωk由陀螺仪敏感角速率减去陀螺仪测量偏置得到。
3.根据权利要求1所述的单天线GNSS辅助陀螺仪的农机转角测量方法，其特征在于，所述农机期望转向角的计算公式为：
其中：L表示农机轴距，
表示tn时刻农机航向角变化率，V(tn)表示tn时刻农机的速度。
4.根据权利要求3所述的单天线GNSS辅助陀螺仪的农机转角测量方法，其特征在于，所述tn时刻农机的速度的计算公式为：
其中：
为未知矩阵的估计值，t0为初始时刻。
5.根据权利要求4所述的单天线GNSS辅助陀螺仪的农机转角测量方法，其特征在于，所述未知矩阵的估计值满足：
其中：N为位置序列的点数，
为未知矩阵的估计值，Δn＝tn-t0为时刻tn至初始时刻t0的时间间隔，P表示轨迹方程。
6.根据权利要求5所述的单天线GNSS辅助陀螺仪的农机转角测量方法，其特征在于，所述轨迹方程由GNSS定位天线输出的N+1个位置点序列(P0…PN)构造而成。
7.根据权利要求1所述的单天线GNSS辅助陀螺仪的农机转角测量方法，其特征在于，由k时刻的期望转向角
和陀螺仪原始量测计算的转向角
进行卡尔曼滤波更新，具体为：以
与
的差值为量测，构造更新量测方程：zk＝Hkxk+vk，其中
为卡尔曼滤波器输入量测数据，vk为转向角测量误差量测模型的零均值高斯噪声，Hk为量测矩阵。
8.一种实现权利要求1-7任一项所述的单天线GNSS辅助陀螺仪的农机转角测量方法的测量装置，其特征在于，包括：GNSS定位天线、定位接收机、ARM1微控制器、ARM2微控制器、DSP、FPGA以及两个单轴陀螺仪；
所述GNSS定位天线与定位接收机进行连接，GNSS定位天线和定位接收机提供农机位置数据；
所述两个单轴陀螺仪分别安装在农机左前轮和右前轮转动部件上；
所述ARM1微控制器采集两个单轴陀螺仪的角速率量测并送入FPGA中，所述ARM2微控制器采集定位接收机输出的位置信息并送入FPGA中；
所述FPGA实现陀螺仪和定位接收机数据的同步；
所述DSP基于FPGA，在线估计陀螺测量偏置与转向角误差，进而实现农机前轮左、右转向角的同时测量输出。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210129973.XA CN114674271A (zh) | 2022-02-11 | 2022-02-11 | 单天线gnss辅助陀螺仪的农机转向角测量方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210129973.XA CN114674271A (zh) | 2022-02-11 | 2022-02-11 | 单天线gnss辅助陀螺仪的农机转向角测量方法及装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114674271A true CN114674271A (zh) | 2022-06-28 |
Family
ID=82073082
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210129973.XA Pending CN114674271A (zh) | 2022-02-11 | 2022-02-11 | 单天线gnss辅助陀螺仪的农机转向角测量方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114674271A (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101326094A (zh) * | 2005-10-15 | 2008-12-17 | 天宝导航有限公司 | 基于转向组件角度及角速率传感器的车辆陀螺仪 |
| CN105823481A (zh) * | 2015-12-21 | 2016-08-03 | 上海华测导航技术股份有限公司 | 一种基于单天线的gnss-ins车辆定姿方法 |
| CN107063241A (zh) * | 2017-03-16 | 2017-08-18 | 上海联适导航技术有限公司 | 基于双gnss天线及单轴mems陀螺的前轮测角系统 |
| CN110426011A (zh) * | 2018-04-28 | 2019-11-08 | 西安合众思壮导航技术有限公司 | 车辆转向角测量系统及方法 |
| CN112014122A (zh) * | 2020-07-21 | 2020-12-01 | 无锡卡尔曼导航技术有限公司 | 一种车轮转角测量装置及运行方法 |
-
2022
- 2022-02-11 CN CN202210129973.XA patent/CN114674271A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101326094A (zh) * | 2005-10-15 | 2008-12-17 | 天宝导航有限公司 | 基于转向组件角度及角速率传感器的车辆陀螺仪 |
| CN105823481A (zh) * | 2015-12-21 | 2016-08-03 | 上海华测导航技术股份有限公司 | 一种基于单天线的gnss-ins车辆定姿方法 |
| CN107063241A (zh) * | 2017-03-16 | 2017-08-18 | 上海联适导航技术有限公司 | 基于双gnss天线及单轴mems陀螺的前轮测角系统 |
| CN110426011A (zh) * | 2018-04-28 | 2019-11-08 | 西安合众思壮导航技术有限公司 | 车辆转向角测量系统及方法 |
| CN112014122A (zh) * | 2020-07-21 | 2020-12-01 | 无锡卡尔曼导航技术有限公司 | 一种车轮转角测量装置及运行方法 |
Non-Patent Citations (2)
| Title |
|---|
| 李淼: "单天线旋转非连续卫星信号定位及滚转检测方法研究", 《中国优秀硕士学位论文全文数据库基础科学辑》, no. 3, 15 March 2020 (2020-03-15), pages 9 - 53 * |
| 缪存孝 等: "基于双GNSS天线及单陀螺的车轮转角测量系统", 《农业机械学报》, vol. 48, no. 9, 30 September 2017 (2017-09-30), pages 17 - 23 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110501024B (zh) | 一种车载ins/激光雷达组合导航系统的量测误差补偿方法 | |
| CN110221333B (zh) | 一种车载ins/od组合导航系统的量测误差补偿方法 | |
| CN111156994B (zh) | 一种基于mems惯性组件的ins/dr&gnss松组合导航方法 | |
| CN110133694B (zh) | 基于双天线gnss航向和轮速辅助的车辆定位方法及系统 | |
| US8756001B2 (en) | Method and apparatus for improved navigation of a moving platform | |
| CN111426318A (zh) | 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 | |
| CN110779521A (zh) | 一种多源融合的高精度定位方法与装置 | |
| CN108594283B (zh) | Gnss/mems惯性组合导航系统的自由安装方法 | |
| CN110906923A (zh) | 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆 | |
| CN106950586A (zh) | 用于农机作业的gnss/ins/车辆组合导航方法 | |
| CN112014122B (zh) | 一种车轮转角测量装置及运行方法 | |
| CN113063429B (zh) | 一种自适应车载组合导航定位方法 | |
| CN112505737B (zh) | 一种gnss/ins组合导航方法 | |
| CN110530361B (zh) | 基于农业机械双天线gnss自动导航系统的转向角度估计器 | |
| CN111947681B (zh) | 一种gnss与惯导组合导航位置输出的滤波校正方法 | |
| CN109781098B (zh) | 一种列车定位的方法和系统 | |
| CN110133695B (zh) | 一种双天线gnss位置延迟时间动态估计系统及方法 | |
| CN113029139B (zh) | 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法 | |
| CN113203418A (zh) | 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统 | |
| CN103852760A (zh) | 一种基于刚性和柔性基线组合的多基线测量方法 | |
| CN106526644A (zh) | 一种用于计算载具部件之间相对角度变化量的方法 | |
| CN113203429A (zh) | 一种陀螺仪温度漂移误差的在线估计及补偿方法 | |
| CN112326990A (zh) | 一种作业车辆测速方法及系统 | |
| CN113008229B (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
| CN112762944B (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 | ||
| CB03 | Change of inventor or designer information |
Inventor after: Chen Jiaxuan Inventor after: Cui Bingbo Inventor after: Du Zhuowen Inventor after: Wei Xinhua Inventor after: Zhu Yongyun Inventor before: Cui Bingbo Inventor before: Chen Jiaxuan Inventor before: Du Zhuowen Inventor before: Wei Xinhua Inventor before: Zhu Yongyun |
|
| CB03 | Change of inventor or designer information | ||
| CB03 | Change of inventor or designer information |
Inventor after: Cui Bingbo Inventor after: Chen Jiaxuan Inventor after: Du Zhuowen Inventor after: Wei Xinhua Inventor after: Zhu Yongyun Inventor before: Chen Jiaxuan Inventor before: Cui Bingbo Inventor before: Du Zhuowen Inventor before: Wei Xinhua Inventor before: Zhu Yongyun |
|
| CB03 | Change of inventor or designer information |