CN112147656B - 一种gnss双天线航向安装角度偏置估计方法 - Google Patents
一种gnss双天线航向安装角度偏置估计方法 Download PDFInfo
- Publication number
- CN112147656B CN112147656B CN202010950068.1A CN202010950068A CN112147656B CN 112147656 B CN112147656 B CN 112147656B CN 202010950068 A CN202010950068 A CN 202010950068A CN 112147656 B CN112147656 B CN 112147656B
- Authority
- CN
- China
- Prior art keywords
- time
- agricultural machinery
- gnss
- antenna
- course
- 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
- 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/40—Correcting position, velocity or attitude
-
- 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)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Guiding Agricultural Machines (AREA)
Abstract
Description
技术领域
本发明涉及一种GNSS双天线航向安装角度偏置估计方法,属于多信息融合技术领域。
背景技术
在农机自动驾驶技术领域内,自动驾驶路线的规划需要确定农机的当前位置和实时航向,而获取农机的实时航向的技术中,有一种方法是通过在农机车顶的左右两侧横向安装GNSS(Global Navigation Satellite System,全球导航卫星系统)的主副双天线用以测量航向,通常主副双天线的测量航向旋转90度后即是车头方向。
但是天线安装存在误差,因此GNSS主副双天线的测量航向旋转90度后的方向和农机实际运动轨迹的方向存在一个固定的小夹角即航向安装偏置角。该偏置角的大小在GNSS主副双天线安装后不会再发生变化,或者因为机器使用磨损等原因存在很小的变化,但是一旦GNSS主副双天线重新安装,就会产生一个新的安装偏置角,也就是说它是随着GNSS主副双天线的安装随机产生的。由于安装偏置角的存在,使得农机自动驾驶时会一直偏向目标线的一侧,导致农机自动驾驶路线偏航,给农业生产生活带来不便。
发明内容
为了解决目前校准GNSS主副双天线的时存在的费时费力且不能实时进行校准的问题,本发明提供了一种基于RTK定位的GNSS双天线的估计方法,用以实时估计出GNSS双天线的安装角度偏置,进而对GNSS双天线测量航向进行实时校准,所述方法包括:
根据通过GNSS双天线实时测量的农机车头方向和预测得到的农机运动轨迹预测航向角进行组合滤波估计,实时估计得到GNSS双天线航向安装角度偏置以便根据对农机车头方向进行补偿修正,得到补偿修正后的农机车头航向
其中,所述GNSS双天线横向安装在农机车顶的左右两侧,
状态量X为:
状态转移矩阵φ为:
其中,τ为根据GNSS双天线定向特性确定的相关时间。
Pn|n-1=φn|n-1Pn-1φn|n-1 T+(φn|n-1Q+Qφn|n-1 T)δt/2
Pn-1为n-1时刻状态协方差矩阵,Q为系统噪声矩阵;δt为滤波估计更新周期。
H=[1 0];
Kn为n时刻滤波增益;
Kn=Pn|n-1Hn T(HnPn|n-1Hn T+Rn)-1
其中,Rn为n时刻观测噪声协方差矩阵,由GNSS信息统计得到;
根据下述公式更新n时刻的状态协方差:
Pn=(I-KnHn)Pn|n-1(I-KnHn)T+KnRnKn T
其中,I为单位矩阵。
本申请的第二个目的在于提供一种基于RTK定位的GNSS双天线测量航向的校准方法,所述GNSS双天线横向安装在农机车顶的左右两侧,所述方法采用上述的GNSS双天线航向安装角度偏置的估计方法得到GNSS双天线航向安装角度偏置进而根据 得到经补偿修正后的农机车头航向 为GNSS双天线实时测量的农机车头方向。
本申请的第三个目的在于提供一种农机自动驾驶路线规划方法,所述方法采用GNSS双天线实时测量农机车头方向所述GNSS双天线横向安装在农机车顶的左右两侧,所述方法采用上述的GNSS双天线航向安装角度偏置的估计方法得到GNSS双天线航向安装角度偏置或者采用上述基于RTK定位的GNSS双天线测量航向的校准方法得到经补偿修正后的农机车头航向进而根据GNSS双天线航向安装角度偏置或者经补偿修正后的农机车头航向进行农机自动驾驶路线规划。
可选的,所述农机包括拖拉机、插秧机、平地机、收割机、打药机。
本发明有益效果是:
通过提供一种基于RTK定位的GNSS双天线的估计方法,GNSS双天线横向安装在农机车顶的左右两侧,根据通过GNSS双天线实时测量的农机车头方向和预测得到的农机运动轨迹预测航向角进行组合滤波估计,实时估计得到GNSS双天线航向安装角度偏置以便根据对农机车头方向进行补偿修正,得到补偿修正后的农机车头航向此方法不但避免了需要专业人员多次校准带来的费时费力的麻烦,而且实时校准还避免了农机因为使用磨损而导致的使用误差,同时,本申请方法的估计精度在±0.15度范围内,误差非常小;校准时间10秒可以收敛到±0.5度,校准时间在50s内可以完全收敛,相对于传统校准方法,时间大大缩短,而且,采用本申请方法不需要专业的校准人员,只需要开动农机即可。
附图说明
为了更清楚地说明本发明实施例中的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是本发明一个实施例中GNSS双天线航向安装角度偏置的估计方法的流程图。
图2是一种车辆运动学轨迹模型示意图。
图3是本发明一个实施例中基于RTK定位的GNSS双天线测量航向的校准方法的流程图。
图4是本发明一个实施例中基于RTK定位的GNSS双天线航向安装角度偏置的估计方法的估计结果仿真图。
图5是本发明一个实施例中基于RTK定位的GNSS双天线测量航向的校准方法修正后的农机航向角仿真图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚,下面将结合附图对本发明实施方式作进一步地详细描述。
实施例一:
本实施例提供一种基于RTK定位的GNSS双天线的估计方法,参见图1,所述方法包括:
根据通过GNSS双天线实时测量的农机车头方向和预测得到的农机运动轨迹预测航向角进行组合滤波估计,实时估计得到GNSS双天线航向安装角度偏置以便根据对农机车头方向进行补偿修正,得到补偿修正后的农机车头航向
在组合滤波估计时,包括:
状态量X为:
其中,τ为根据GNSS双天线定向特性确定的相关时间。
Pn|n-1=φn|n-1Pn-1φn|n-1 T+(φn|n-1Q+Qφn|n-1 T)δt/2
Pn-1为n-1时刻状态协方差矩阵,Q为系统噪声矩阵;δt为滤波估计更新周期。
Kn=Pn|n-1Hn T(HnPn|n-1Hn T+Rn)-1
其中,Rn为n时刻观测噪声协方差矩阵,由GNSS信息统计得到;
根据下述公式更新n时刻的状态协方差:
Pn=(I-KnHn)Pn|n-1(I-KnHn)T+KnRnKn T
其中,I为单位矩阵。
实施例二
本实施例提供一种基于RTK(Real-Time Kinematic,实时动态)定位的GNSS双天线测量航向的校准方法,应用于农机设备中,将GNSS双天线横向安装在农机车顶的左右两侧,所述方法包括:
其中,如图2所示,假设k-1时刻农机的位置为Pk-1(xk-1,yk-1),k时刻农机的位置为Pk(xk,yk);
δx:δx=xk-xk-1,x轴方向的位置差;
δy:δy=yk-yk-1,y轴方向的位置差;
S3 Kalman滤波估计:
估计流程如图3所示,该估计过程为不断迭代的过程,包括:
3.1GNSS双天线航向更新;
3.2农机航向角修正:
其中:
由图3可知第一次补偿修正时,GNSS双天线短时间内变化非常小接近为零,因此可假设此时根据Kalman滤波估计得到的为零,也即第一次进行角度补偿修正时,经补偿修正后的农机车头航向就等于GNSS双天线实时测量的航向旋转90度后得到的方向。
3.3判断农机是否静止或者角速度过大;
当农机静止时,其位置变化量非常小,静止状态时位移量通常低于5cm,此时位置差分得到的预测航向角误差非常大;而当农机快速转弯时即车身的旋转角速率较大,由于位置差分的滞后,此时差分得到的航向角误差也比较大。因此如果此时继续进行滤波估计会带来较大的误差甚至是滤波发散。
考虑GNSS双天线航向安装角偏置在短时间内是不变的,此时统一进行零速修正,无限放大观测噪声,滤波估计中的观测量即预测航向权重非常小,在静止状态下其权重系数直接接近于0,等价于不滤波修正,因此若判断结果为农机处于静止状态或者角速度大于阈值30度/秒,则修改观测噪声矩阵R。
若判断结果为农机并不是处于静止状态且角速度小于判定阈值,则进行下一步状态转移矩阵的计算。
3.4状态转移矩阵计算:
具体的,选取农机航向角误差和GNSS双天线航向安装角偏置作为滤波估计的状态量,即:
其中:
其中:
τ:相关时间,根据GNSS双天线定向特性可选定3600秒,也可以选择其他值,比如7200秒、600秒等。
3.5Kalman一步预测:
Pn|n-1=φn|n-1Pn-1φn|n-1 T+(φn|n-1Q+Qφn|n-1 T)δt/2
其中:
Pn-1:n-1时刻状态协方差矩阵;
φn|n-1:n-1时刻到n时刻状态转移矩阵;
Q:系统噪声矩阵;
δt:滤波估计更新周期。
3.6Kalman滤波估计更新:
Kalman滤波估计更新状态量和状态协方差矩阵。
根据下式计算得到n时刻GNSS双天线航向安装角度偏置:
滤波估计时的滤波增益:
Kn=Pn|n-1Hn T(HnPn|n-1Hn T+Rn)-1
其中:
Kn:n时刻滤波增益;
Rn:n时刻观测噪声协方差矩阵,由GNSS信息统计得到,接近直行时选定2deg/s,随着角速率增大,观测噪声Rn线性变大,斜率系数为2,即
Rn=2+2*ωn,ωn是n时刻的农机旋转角速率,由GNSS双天线航向差分得到。
根据下述公式进行状态更新:
其中:
根据下述公式进行状态协方差更新:
Pn=(I-KnHn)Pn|n-1(I-KnHn)T+KnRnKn T
其中:
Pn:n时刻状态协方差矩阵;
I:单位矩阵。
3.7反馈修正:
为验证本申请所提出的基于RTK定位的GNSS双天线的估计方法实时估计出的准确性,本实施例进行了验证实验,预先采用其他方式测量得到GNSS双天线在农机上的为-1.5度。
如图4所示,为采用本实施例提供的基于RTK定位的GNSS双天线的估计方法的仿真结果,根据图4可知,采用本实施例滤波估计的稳定在-1.5度左右,误差范围为±0.15度,且根据图4可知,本申请提供的基于RTK定位的GNSS双天线的估计方法在50s时已完全趋于稳定状态,相对于传统校准方法,时间大大缩短,而且,采用本申请方法不需要专业的校准人员。
图5为手动开动农机实际运行轨迹,从图中可以看出运动轨迹存在频繁的调头(航向变化180度)、角速率很大的快速调头(航向变化非常快)、农机静止(航向基本不变)等各种运动场景,图4是对应滤波估计的安装角度偏置结果,从图4可以看出采用本实例方法估计的安装角偏置非常稳定且精度高,该方法适用于各种场景。
本发明实施例中的部分步骤,可以利用软件实现,相应的软件程序可以存储在可读取的存储介质中,如CPU或光盘或硬盘等。
以上所述仅为本发明的较佳实施例,并不用以限制本发明,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (9)
1.一种GNSS双天线航向安装角度偏置的估计方法,其特征在于,所述GNSS双天线横向安装在农机车顶的左右两侧,所述方法包括:
根据通过GNSS双天线实时测量的农机车头方向和预测得到的农机运动轨迹预测航向角进行组合滤波估计,实时估计得到GNSS双天线航向安装角度偏置以便根据对农机车头方向进行补偿修正,得到补偿修正后的农机车头航向
H=[1 0]
Kn为n时刻滤波增益;
Kn=Pn|n-1Hn T(HnPn|n-1Hn T+Rn)-1
其中,Rn为n时刻观测噪声协方差矩阵,由GNSS信息统计得到;
根据下述公式更新n时刻的状态协方差:
Pn=(I-KnHn)Pn|n-1(I-KnHn)T+KnRnKn T
其中,I为单位矩阵。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010950068.1A CN112147656B (zh) | 2020-09-09 | 2020-09-09 | 一种gnss双天线航向安装角度偏置估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010950068.1A CN112147656B (zh) | 2020-09-09 | 2020-09-09 | 一种gnss双天线航向安装角度偏置估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112147656A CN112147656A (zh) | 2020-12-29 |
CN112147656B true CN112147656B (zh) | 2021-05-04 |
Family
ID=73890113
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010950068.1A Active CN112147656B (zh) | 2020-09-09 | 2020-09-09 | 一种gnss双天线航向安装角度偏置估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112147656B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114543795B (zh) * | 2021-12-31 | 2024-01-02 | 文远苏行(江苏)科技有限公司 | 双天线航向角的安装误差估计方法和调整方法及相关设备 |
CN116892898B (zh) * | 2023-09-11 | 2024-02-02 | 农业农村部南京农业机械化研究所 | 农机的轨迹误差检测方法、装置及系统 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102169184A (zh) * | 2011-01-04 | 2011-08-31 | 北京航空航天大学 | 组合导航系统中测量双天线gps安装失准角的方法和装置 |
CN104459744A (zh) * | 2014-12-15 | 2015-03-25 | 湖南航天电子科技有限公司 | 一种虚拟稳定基线卫星定向方法及装置 |
CN105115518A (zh) * | 2015-07-28 | 2015-12-02 | 中国运载火箭技术研究院 | 一种用于惯性导航系统与gps双天线航向偏角标定方法 |
CN106443744A (zh) * | 2016-09-28 | 2017-02-22 | 武汉迈普时空导航科技有限公司 | Gnss双天线姿态的标定和校准方法 |
CN107063241A (zh) * | 2017-03-16 | 2017-08-18 | 上海联适导航技术有限公司 | 基于双gnss天线及单轴mems陀螺的前轮测角系统 |
CN108802788A (zh) * | 2018-04-10 | 2018-11-13 | 拓攻(南京)机器人有限公司 | 一种航向偏差的确定方法、装置、设备以及存储介质 |
CN110262479A (zh) * | 2019-05-28 | 2019-09-20 | 南京天辰礼达电子科技有限公司 | 一种履带式拖拉机运动学估计及偏差校准方法 |
-
2020
- 2020-09-09 CN CN202010950068.1A patent/CN112147656B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102169184A (zh) * | 2011-01-04 | 2011-08-31 | 北京航空航天大学 | 组合导航系统中测量双天线gps安装失准角的方法和装置 |
CN104459744A (zh) * | 2014-12-15 | 2015-03-25 | 湖南航天电子科技有限公司 | 一种虚拟稳定基线卫星定向方法及装置 |
CN105115518A (zh) * | 2015-07-28 | 2015-12-02 | 中国运载火箭技术研究院 | 一种用于惯性导航系统与gps双天线航向偏角标定方法 |
CN106443744A (zh) * | 2016-09-28 | 2017-02-22 | 武汉迈普时空导航科技有限公司 | Gnss双天线姿态的标定和校准方法 |
CN107063241A (zh) * | 2017-03-16 | 2017-08-18 | 上海联适导航技术有限公司 | 基于双gnss天线及单轴mems陀螺的前轮测角系统 |
CN108802788A (zh) * | 2018-04-10 | 2018-11-13 | 拓攻(南京)机器人有限公司 | 一种航向偏差的确定方法、装置、设备以及存储介质 |
CN110262479A (zh) * | 2019-05-28 | 2019-09-20 | 南京天辰礼达电子科技有限公司 | 一种履带式拖拉机运动学估计及偏差校准方法 |
Non-Patent Citations (1)
Title |
---|
基于双GNSS天线及单陀螺的车轮转角测量系统;缪存孝 等;《农业机械学报》;20170930;第48卷(第9期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN112147656A (zh) | 2020-12-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
AU2004201913B2 (en) | Direct modification of DGPS information with inertial measurement data | |
AU2004201910B2 (en) | Inertial augmentation for GPS navigation on ground vehicles | |
CN112147656B (zh) | 一种gnss双天线航向安装角度偏置估计方法 | |
AU2019268055B2 (en) | System and method for terrestrial vehicle navigation | |
Chiang et al. | Assessment for INS/GNSS/odometer/barometer integration in loosely-coupled and tightly-coupled scheme in a GNSS-degraded environment | |
WO2020238011A1 (zh) | 一种履带式拖拉机运动学估计及偏差校准方法 | |
CN107741745A (zh) | 一种实现移动机器人自主定位与地图构建的方法 | |
US9969081B2 (en) | Method and system for the directed control of robotic assets | |
CN113203429B (zh) | 一种陀螺仪温度漂移误差的在线估计及补偿方法 | |
CN112146561A (zh) | 一种霍尔角度传感器安装角度偏置的估计方法 | |
CN111703432B (zh) | 一种智能履带车辆滑动参数实时估计方法 | |
US8229661B2 (en) | Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering | |
Brunker et al. | GNSS-shortages-resistant and self-adaptive rear axle kinematic parameter estimator (SA-RAKPE) | |
CN114966629A (zh) | 一种基于ekf算法框架的车体激光雷达外参标定方法 | |
EP4143507B1 (en) | Navigation apparatus and method in which measurement quantization errors are modeled as states | |
CN113074757A (zh) | 车载惯导安装误差角的标定方法 | |
EP3879232B1 (en) | Method and system for detection of roll sensor bias | |
González et al. | Comparative study of localization techniques for mobile robots based on indirect kalman filter | |
US11762104B2 (en) | Method and device for locating a vehicle | |
JP7407947B2 (ja) | 車両制御装置 | |
Parviainen et al. | Doppler radar and MEMS gyro augmented DGPS for large vehicle navigation | |
WO2017168588A1 (ja) | 測定装置、測定方法、及び、プログラム | |
CN108957508B (zh) | 车载pos离线组合估计方法和装置 | |
CN114076610B (zh) | Gnss/mems车载组合导航系统的误差标定、导航方法及其装置 | |
CN116608852B (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 | ||
PE01 | Entry into force of the registration of the contract for pledge of patent right | ||
PE01 | Entry into force of the registration of the contract for pledge of patent right |
Denomination of invention: A GNSS dual antenna heading installation angle offset estimation method Effective date of registration: 20220506 Granted publication date: 20210504 Pledgee: Jiangsu SINOSURE technology microfinance Co.,Ltd. Pledgor: WUXI KALMAN NAVIGATION TECHNOLOGY CO.,LTD. Registration number: Y2022320000200 |