CN112147656A - 一种gnss双天线航向安装角度偏置估计方法 - Google Patents

一种gnss双天线航向安装角度偏置估计方法 Download PDF

Info

Publication number
CN112147656A
CN112147656A CN202010950068.1A CN202010950068A CN112147656A CN 112147656 A CN112147656 A CN 112147656A CN 202010950068 A CN202010950068 A CN 202010950068A CN 112147656 A CN112147656 A CN 112147656A
Authority
CN
China
Prior art keywords
agricultural machinery
time
antenna
gnss
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.)
Granted
Application number
CN202010950068.1A
Other languages
English (en)
Other versions
CN112147656B (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.)
Wuxi Kalman Navigation Technology Co ltd
Original Assignee
Wuxi Kalman Navigation 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 Wuxi Kalman Navigation Technology Co ltd filed Critical Wuxi Kalman Navigation Technology Co ltd
Priority to CN202010950068.1A priority Critical patent/CN112147656B/zh
Publication of CN112147656A publication Critical patent/CN112147656A/zh
Application granted granted Critical
Publication of CN112147656B publication Critical patent/CN112147656B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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

本发明公开了一种GNSS双天线航向安装角度偏置估计方法，属于多信息融合技术领域。所述方法中GNSS双天线横向安装在农机车顶的左右两侧，根据GNSS双天线实时测量的农机车头方向
Figure DDA0002673455390000011
和预测得到的农机运动轨迹预测航向角
Figure DDA0002673455390000012
进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置
Figure DDA0002673455390000013
根据
Figure DDA0002673455390000014
对
Figure DDA0002673455390000015
进行补偿修正，得到补偿修正后的农机车头航向
Figure DDA0002673455390000016
此方法避免了需要专业人员多次校准带来的费时费力的麻烦，实时校准还避免了农机因为使用磨损而导致的使用误差，同时，该方法估计精度在±0.15度内；校准时间10秒可以收敛到±0.5度，校准时间在50s内可以完全收敛，相对于传统校准方法，时间大大缩短。

Description

一种GNSS双天线航向安装角度偏置估计方法
技术领域
本发明涉及一种GNSS双天线航向安装角度偏置估计方法，属于多信息融合技术领域。
背景技术
在农机自动驾驶技术领域内，自动驾驶路线的规划需要确定农机的当前位置和实时航向，而获取农机的实时航向的技术中，有一种方法是通过在农机车顶的左右两侧横向安装GNSS(Global Navigation Satellite System，全球导航卫星系统)的主副双天线用以测量航向，通常主副双天线的测量航向
Figure BDA0002673455370000011
旋转90度后即是车头方向。
但是天线安装存在误差，因此GNSS主副双天线的测量航向
Figure BDA0002673455370000012
旋转90度后的方向和农机实际运动轨迹的方向存在一个固定的小夹角即航向安装偏置角。该偏置角的大小在GNSS主副双天线安装后不会再发生变化，或者因为机器使用磨损等原因存在很小的变化，但是一旦GNSS主副双天线重新安装，就会产生一个新的安装偏置角，也就是说它是随着GNSS主副双天线的安装随机产生的。由于安装偏置角的存在，使得农机自动驾驶时会一直偏向目标线的一侧，导致农机自动驾驶路线偏航，给农业生产生活带来不便。
传统的校准方法是在相同路段上由专业的安装服务人员自动驾驶一个约70米长度的来回，采用均值法进行校准，即获取农机在相同路段上自动驾驶时主副双天线的多个测量航向
Figure BDA0002673455370000013
值，对多个测量航向
Figure BDA0002673455370000014
值取均值作为主副双天线的安装偏置角。此方法需要专业人员进行操作且费时费力，而且如果GNSS双天线的任何一个天线位置因为重新安装等原因发生变化，该安装角就会发生变化，需要重新校准，也即需要专业的安装服务人员重复上述校准过程，费时费力且会带来较大的售后服务成本。
发明内容
为了解决目前校准GNSS主副双天线的时存在的费时费力且不能实时进行校准的问题，本发明提供了一种基于RTK定位的GNSS双天线的估计方法，用以实时估计出GNSS双天线的安装角度偏置，进而对GNSS双天线测量航向进行实时校准，所述方法包括：
根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角
Figure BDA0002673455370000015
通过GNSS双天线实时测量农机车头方向
Figure BDA0002673455370000016
根据通过GNSS双天线实时测量的农机车头方向
Figure BDA0002673455370000017
和预测得到的农机运动轨迹预测航向角
Figure BDA0002673455370000024
进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置
Figure BDA0002673455370000025
以便根据
Figure BDA0002673455370000026
对农机车头方向
Figure BDA0002673455370000027
进行补偿修正，得到补偿修正后的农机车头航向
Figure BDA0002673455370000028
其中，所述GNSS双天线横向安装在农机车顶的左右两侧，
可选的，所述根据通过GNSS双天线实时测量的农机车头方向
Figure BDA0002673455370000029
和预测得到的农机运动轨迹预测航向角
Figure BDA00026734553700000210
进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置
Figure BDA00026734553700000211
包括：
2.1选取农机航向角误差
Figure BDA00026734553700000212
和GNSS双天线航向安装角度偏置
Figure BDA00026734553700000213
作为滤波估计的状态量X；其中，农机航向角误差
Figure BDA00026734553700000214
为经补偿修正后的农机车头航向
Figure BDA00026734553700000215
真值与农机运动轨迹预测航向角
Figure BDA00026734553700000216
的差值；
2.2根据农机航向角观测误差
Figure BDA00026734553700000217
与GNSS双天线
Figure BDA00026734553700000218
计算所述状态量X的状态转移矩阵φ；
2.3根据所述状态量X的状态转移矩阵φ一步预测n-1时刻到n时刻状态量
Figure BDA00026734553700000219
状态协方差矩阵Pn|n-1；
2.4更新n时刻的状态估计量
Figure BDA00026734553700000220
和n时刻的状态协方差Pn；
2.5根据更新后的n时刻的状态估计量
Figure BDA00026734553700000221
计算得到GNSS双天线航向安装角度偏置
Figure BDA00026734553700000222
可选的，所述2.2根据农机航向角观测误差
Figure BDA00026734553700000223
与GNSS双天线
Figure BDA00026734553700000224
计算所述状态量X的状态转移矩阵φ，包括：
状态量X为：
Figure BDA0002673455370000021
状态转移矩阵φ为：
Figure BDA0002673455370000022
其中，τ为根据GNSS双天线定向特性确定的相关时间。
可选的，所述2.3根据所述状态量X的状态转移矩阵φ一步预测n-1时刻到n时刻状态量
Figure BDA00026734553700000225
状态协方差矩阵Pn|n-1，包括：
根据下述公式预测得到n-1时刻到n时刻状态量
Figure BDA00026734553700000226
状态协方差矩阵Pn|n-1：
Figure BDA0002673455370000023
Pn|n-1＝φn|n-1Pn-1φn|n-1 T+(φn|n-1Q+Qφn|n-1 T)δt/2
其中，φn|n-1为n-1时刻到n时刻状态转移矩阵；
Figure BDA00026734553700000227
为n-1时刻状态量X的状态估计值；
Pn-1为n-1时刻状态协方差矩阵，Q为系统噪声矩阵；δt为滤波估计更新周期。
可选的，所述2.4更新n时刻的状态估计量
Figure BDA0002673455370000035
和n时刻的状态协方差Pn，包括：
根据下述公式更新n时刻的状态估计量
Figure BDA0002673455370000036
Figure BDA0002673455370000031
其中，Zn为n时刻的观测量即修正后的农机航向角实际测量值和预测得到的农机运动轨迹预测航向角
Figure BDA0002673455370000037
之间差值；Hn为n时刻的观测矩阵，由农机航向角观测误差
Figure BDA0002673455370000038
与估计的观测误差
Figure BDA0002673455370000039
之间的关系得到：
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为单位矩阵。
可选的，所述2.5根据更新后的n时刻的状态估计量
Figure BDA00026734553700000310
计算得到GNSS双天线航向安装角度偏置
Figure BDA00026734553700000311
即根据下式计算得到n时刻GNSS双天线航向安装角度偏置：
Figure BDA0002673455370000032
其中
Figure BDA00026734553700000312
为状态量X在n时刻的第二维估计值。可选的，所述根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角
Figure BDA00026734553700000313
包括：
假设k-1时刻农机的位置为Pk-1(xk-1,yk-1)，k时刻农机的位置为Pk(xk,yk)，则根据下述公式预测农机运动轨迹预测航向角
Figure BDA00026734553700000314
Figure BDA0002673455370000033
其中，δx＝xk-xk-1，表示x轴方向的位置差；δy＝yk-yk-1，表示y轴方向的位置差；
Figure BDA0002673455370000034
表示位置差的算术平方根。
可选的，所述方法在通过GNSS双天线实时测量的农机车头方向
Figure BDA0002673455370000041
和预测得到的农机运动轨迹预测航向角
Figure BDA0002673455370000042
进行组合滤波估计的时机为农机处于非静止状态且角速度小于30度/秒。
本申请的第二个目的在于提供一种基于RTK定位的GNSS双天线测量航向的校准方法，所述GNSS双天线横向安装在农机车顶的左右两侧，所述方法采用上述的GNSS双天线航向安装角度偏置的估计方法得到GNSS双天线航向安装角度偏置
Figure BDA0002673455370000044
进而根据
Figure BDA0002673455370000043
Figure BDA0002673455370000045
得到经补偿修正后的农机车头航向
Figure BDA0002673455370000046
Figure BDA0002673455370000047
为GNSS双天线实时测量的农机车头方向。
本申请的第三个目的在于提供一种农机自动驾驶路线规划方法，所述方法采用GNSS双天线实时测量农机车头方向
Figure BDA0002673455370000048
所述GNSS双天线横向安装在农机车顶的左右两侧，所述方法采用上述的GNSS双天线航向安装角度偏置的估计方法得到GNSS双天线航向安装角度偏置
Figure BDA0002673455370000049
或者采用上述基于RTK定位的GNSS双天线测量航向的校准方法得到经补偿修正后的农机车头航向
Figure BDA00026734553700000410
进而根据GNSS双天线航向安装角度偏置
Figure BDA00026734553700000411
或者经补偿修正后的农机车头航向
Figure BDA00026734553700000412
进行农机自动驾驶路线规划。
可选的，所述农机包括拖拉机、插秧机、平地机、收割机、打药机。
本发明有益效果是：
通过提供一种基于RTK定位的GNSS双天线的估计方法，GNSS双天线横向安装在农机车顶的左右两侧，根据通过GNSS双天线实时测量的农机车头方向
Figure BDA00026734553700000413
和预测得到的农机运动轨迹预测航向角
Figure BDA00026734553700000414
进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置
Figure BDA00026734553700000415
以便根据
Figure BDA00026734553700000416
对农机车头方向
Figure BDA00026734553700000417
进行补偿修正，得到补偿修正后的农机车头航向
Figure BDA00026734553700000418
此方法不但避免了需要专业人员多次校准带来的费时费力的麻烦，而且实时校准还避免了农机因为使用磨损而导致的使用误差，同时，本申请方法的估计精度在±0.15度范围内，误差非常小；校准时间10秒可以收敛到±0.5度，校准时间在50s内可以完全收敛，相对于传统校准方法，时间大大缩短，而且，采用本申请方法不需要专业的校准人员，只需要开动农机即可。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明一个实施例中GNSS双天线航向安装角度偏置的估计方法的流程图。
图2是一种车辆运动学轨迹模型示意图。
图3是本发明一个实施例中基于RTK定位的GNSS双天线测量航向的校准方法的流程图。
图4是本发明一个实施例中基于RTK定位的GNSS双天线航向安装角度偏置的估计方法的估计结果仿真图。
图5是本发明一个实施例中基于RTK定位的GNSS双天线测量航向的校准方法修正后的农机航向角仿真图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。
实施例一：
本实施例提供一种基于RTK定位的GNSS双天线的估计方法，参见图1，所述方法包括：
根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角
Figure BDA0002673455370000052
通过GNSS双天线实时测量农机车头方向
Figure BDA0002673455370000053
根据通过GNSS双天线实时测量的农机车头方向
Figure BDA0002673455370000054
和预测得到的农机运动轨迹预测航向角
Figure BDA0002673455370000055
进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置
Figure BDA0002673455370000056
以便根据
Figure BDA0002673455370000057
对农机车头方向
Figure BDA0002673455370000058
进行补偿修正，得到补偿修正后的农机车头航向
Figure BDA0002673455370000059
在组合滤波估计时，包括：
2.1选取农机航向角误差
Figure BDA00026734553700000510
和GNSS双天线航向安装角度偏置
Figure BDA00026734553700000511
作为滤波估计的状态量X；其中，农机航向角误差
Figure BDA00026734553700000512
为经补偿修正后的农机车头航向
Figure BDA00026734553700000513
真值与农机运动轨迹预测航向角
Figure BDA00026734553700000514
的差值；
状态量X为：
Figure BDA0002673455370000051
2.2根据农机航向角观测误差
Figure BDA00026734553700000515
与GNSS双天线
Figure BDA00026734553700000516
计算所述状态量X的状态转移矩阵φ；状态转移矩阵φ为：
Figure BDA0002673455370000061
其中，τ为根据GNSS双天线定向特性确定的相关时间。
2.3根据所述状态量X的状态转移矩阵φ一步预测n-1时刻到n时刻状态量
Figure BDA0002673455370000064
状态协方差矩阵Pn|n-1；
根据下述公式预测得到n-1时刻到n时刻状态量
Figure BDA0002673455370000065
状态协方差矩阵Pn|n-1：
Figure BDA0002673455370000062
Pn|n-1＝φn|n-1Pn-1φn|n-1 T+(φn|n-1Q+Qφn|n-1 T)δt/2
其中，φn|n-1为n-1时刻到n时刻状态转移矩阵；
Figure BDA0002673455370000066
为n-1时刻状态量X的状态估计值；
Pn-1为n-1时刻状态协方差矩阵，Q为系统噪声矩阵；δt为滤波估计更新周期。
2.4更新n时刻的状态估计量
Figure BDA0002673455370000067
和n时刻的状态协方差Pn；
根据下述公式更新n时刻的状态估计量
Figure BDA0002673455370000068
Figure BDA0002673455370000063
其中，Zn为n时刻的观测量即修正后的农机航向角实际测量值和预测得到的农机运动轨迹预测航向角
Figure BDA0002673455370000069
之间差值；Hn为n时刻的观测矩阵，由农机航向角观测误差
Figure BDA00026734553700000610
与估计的观测误差
Figure BDA00026734553700000611
之间的关系得到；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为单位矩阵。
2.5根据更新后的n时刻的状态估计量
Figure BDA00026734553700000612
计算得到GNSS双天线航向安装角度偏置
Figure BDA00026734553700000613
实施例二
本实施例提供一种基于RTK(Real-Time Kinematic，实时动态)定位的GNSS双天线测量航向的校准方法，应用于农机设备中，将GNSS双天线横向安装在农机车顶的左右两侧，所述方法包括：
S1根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角
Figure BDA0002673455370000077
Figure BDA0002673455370000071
其中，如图2所示，假设k-1时刻农机的位置为Pk-1(xk-1,yk-1)，k时刻农机的位置为Pk(xk,yk)；
Figure BDA0002673455370000078
农机运动轨迹预测航向角，即农机运动轨迹预测航向与正北方向的夹角，范围0到360度；
δx：δx＝xk-xk-1，x轴方向的位置差；
δy：δy＝yk-yk-1，y轴方向的位置差；
δxy：
Figure BDA0002673455370000072
位置差的算术平方根；
S2通过GNSS双天线实时测量农机车头方向
Figure BDA0002673455370000079
即GNSS双天线实时测量的航向旋转90度后得到的方向与正北方向的夹角；
S3 Kalman滤波估计：
根据S2中通过GNSS双天线实时测量农机车头方向
Figure BDA00026734553700000710
和S1预测得到的农机运动轨迹预测航向角
Figure BDA00026734553700000711
进行组合滤波估计，实时估计和修正GNSS双天线。
估计流程如图3所示，该估计过程为不断迭代的过程，包括：
3.1GNSS双天线航向更新；
即获取GNSS双天线实时测量的农机车头方向
Figure BDA00026734553700000712
3.2农机航向角修正：
根据Kalman滤波估计的
Figure BDA00026734553700000713
实时补偿修正农机车头方向，得到经补偿修正后的农机车头航向
Figure BDA00026734553700000714
Figure BDA0002673455370000073
其中：
Figure BDA0002673455370000074
经补偿修正后的农机车头航向；
Figure BDA0002673455370000075
GNSS双天线实时测量的航向旋转90度后得到的方向；
Figure BDA0002673455370000076
Kalman滤波估计的GNSS双天线。
由图3可知第一次补偿修正时，GNSS双天线短时间内变化非常小接近为零，因此可假设此时根据Kalman滤波估计得到的
Figure BDA0002673455370000084
为零，也即第一次进行角度补偿修正时，经补偿修正后的农机车头航向
Figure BDA0002673455370000085
就等于GNSS双天线实时测量的航向旋转90度后得到的方向。
3.3判断农机是否静止或者角速度过大；
当农机静止时，其位置变化量非常小，静止状态时位移量通常低于5cm，此时位置差分得到的预测航向角误差非常大；而当农机快速转弯时即车身的旋转角速率较大，由于位置差分的滞后，此时差分得到的航向角误差也比较大。因此如果此时继续进行滤波估计会带来较大的误差甚至是滤波发散。
考虑GNSS双天线航向安装角偏置在短时间内是不变的，此时统一进行零速修正，无限放大观测噪声，滤波估计中的观测量即预测航向权重非常小，在静止状态下其权重系数直接接近于0，等价于不滤波修正，因此若判断结果为农机处于静止状态或者角速度大于阈值30度/秒，则修改观测噪声矩阵R。
若判断结果为农机并不是处于静止状态且角速度小于判定阈值，则进行下一步状态转移矩阵的计算。
3.4状态转移矩阵计算：
具体的，选取农机航向角误差和GNSS双天线航向安装角偏置作为滤波估计的状态量，即：
Figure BDA0002673455370000081
其中：
Figure BDA0002673455370000086
农机航向角观测误差，即为经补偿修正后的农机车头航向
Figure BDA0002673455370000087
真值与S1中农机运动轨迹预测航向角
Figure BDA0002673455370000088
的差值；
Figure BDA0002673455370000089
Kalman滤波估计的GNSS双天线航向安装角偏置。
同样由于GNSS双天线短时间内变化非常小甚至是不变的，其安装偏置角误差模型可近似为一阶马尔科夫过程，则
Figure BDA0002673455370000082
写成矩阵形式可以得到状态转移矩阵φ如下：
Figure BDA0002673455370000083
其中：
τ：相关时间，根据GNSS双天线定向特性可选定3600秒，也可以选择其他值，比如7200秒、600秒等。
3.5Kalman一步预测：
在得到上述状态转移矩阵后，一步预测n-1时刻到n时刻状态量
Figure BDA0002673455370000093
状态协方差矩阵Pn|n-1。
Figure BDA0002673455370000091
Pn|n-1＝φn|n-1Pn-1φn|n-1 T+(φn|n-1Q+Qφn|n-1 T)δt/2
其中：
Figure BDA0002673455370000094
n-1时刻状态量X的估计值；
Pn-1：n-1时刻状态协方差矩阵；
φn|n-1：n-1时刻到n时刻状态转移矩阵；
Q：系统噪声矩阵；
δt：滤波估计更新周期。
3.6Kalman滤波估计更新：
Kalman滤波估计更新状态量和状态协方差矩阵。
根据下式计算得到n时刻GNSS双天线航向安装角度偏置：
Figure BDA0002673455370000092
其中
Figure BDA0002673455370000095
为状态量X在n时刻的第二维估计值。
为防止滤波估计出现异常偏差，在滤波估计时加入质量控制，即当农机航向角观测误差
Figure BDA0002673455370000096
与估计的观测误差
Figure BDA0002673455370000097
相差较大时，需要对估计的观测误差进行一定的修正。
滤波估计时的滤波增益：
Kn＝Pn|n-1Hn T(HnPn|n-1Hn T+Rn)-1
其中：
Kn：n时刻滤波增益；
Hn：n时刻的观测矩阵，由农机航向角观测误差
Figure BDA0002673455370000098
与估计的观测误差
Figure BDA0002673455370000099
之间的关系得到。H＝[1 0]；
Rn：n时刻观测噪声协方差矩阵，由GNSS信息统计得到，接近直行时选定2deg/s，随着角速率增大，观测噪声Rn线性变大，斜率系数为2，即
Rn＝2+2*ωn，ωn是n时刻的农机旋转角速率,由GNSS双天线航向差分得到。
根据下述公式进行状态更新：
Figure BDA0002673455370000101
其中：
Figure BDA0002673455370000103
n时刻的状态估计量；
Zn：n时刻的观测量即修正后的农机航向角实际测量值和S1预测得到的农机运动轨迹预测航向角
Figure BDA0002673455370000104
之间差值即：
Figure BDA0002673455370000102
根据下述公式进行状态协方差更新：
Pn＝(I-KnHn)Pn|n-1(I-KnHn)T+KnRnKn T
其中：
Pn：n时刻状态协方差矩阵；
I：单位矩阵。
3.7反馈修正：
滤波估计后利用估计后的状态量对当前航向安装角偏置和农机航向进行反馈修正，得到修正后的农机车头航向
Figure BDA0002673455370000105
为验证本申请所提出的基于RTK定位的GNSS双天线的估计方法实时估计出的准确性，本实施例进行了验证实验，预先采用其他方式测量得到GNSS双天线在农机上的为-1.5度。
如图4所示，为采用本实施例提供的基于RTK定位的GNSS双天线的估计方法的仿真结果，根据图4可知，采用本实施例滤波估计的稳定在-1.5度左右，误差范围为±0.15度，且根据图4可知，本申请提供的基于RTK定位的GNSS双天线的估计方法在50s时已完全趋于稳定状态，相对于传统校准方法，时间大大缩短，而且，采用本申请方法不需要专业的校准人员。
图5为手动开动农机实际运行轨迹，从图中可以看出运动轨迹存在频繁的调头(航向变化180度)、角速率很大的快速调头(航向变化非常快)、农机静止(航向基本不变)等各种运动场景，图4是对应滤波估计的安装角度偏置结果，从图4可以看出采用本实例方法估计的安装角偏置非常稳定且精度高，该方法适用于各种场景。
本发明实施例中的部分步骤，可以利用软件实现，相应的软件程序可以存储在可读取的存储介质中，如CPU或光盘或硬盘等。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种GNSS双天线航向安装角度偏置的估计方法，其特征在于，所述GNSS双天线横向安装在农机车顶的左右两侧，所述方法包括：
根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角
Figure FDA0002673455360000011
通过GNSS双天线实时测量农机车头方向
Figure FDA0002673455360000012
根据通过GNSS双天线实时测量的农机车头方向
Figure FDA0002673455360000013
和预测得到的农机运动轨迹预测航向角
Figure FDA0002673455360000014
进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置
Figure FDA0002673455360000015
以便根据
Figure FDA0002673455360000016
对农机车头方向
Figure FDA0002673455360000017
进行补偿修正，得到补偿修正后的农机车头航向
Figure FDA0002673455360000018
2.根据权利要求1所述的方法，其特征在于，所述根据通过GNSS双天线实时测量的农机车头方向
Figure FDA0002673455360000019
和预测得到的农机运动轨迹预测航向角
Figure FDA00026734553600000110
进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置
Figure FDA00026734553600000111
包括：
2.1选取农机航向角误差
Figure FDA00026734553600000112
和GNSS双天线航向安装角度偏置
Figure FDA00026734553600000113
作为滤波估计的状态量X；其中，农机航向角误差
Figure FDA00026734553600000114
为经补偿修正后的农机车头航向
Figure FDA00026734553600000115
真值与农机运动轨迹预测航向角
Figure FDA00026734553600000116
的差值；
2.2根据农机航向角观测误差
Figure FDA00026734553600000117
与GNSS双天线
Figure FDA00026734553600000118
计算所述状态量X的状态转移矩阵φ；
2.3根据所述状态量X的状态转移矩阵φ一步预测n-1时刻到n时刻状态量
Figure FDA00026734553600000119
状态协方差矩阵Pn|n-1；
2.4更新n时刻的状态估计量
Figure FDA00026734553600000120
和n时刻的状态协方差Pn；
2.5根据更新后的n时刻的状态估计量
Figure FDA00026734553600000121
计算得到GNSS双天线航向安装角度偏置
Figure FDA00026734553600000122
3.根据权利要求2所述的方法，其特征在于，所述2.2根据农机航向角观测误差
Figure FDA00026734553600000123
与GNSS双天线
Figure FDA00026734553600000124
计算所述状态量X的状态转移矩阵φ，包括：
状态量X为：
Figure FDA00026734553600000125
状态转移矩阵φ为：
Figure FDA00026734553600000126
其中，τ为根据GNSS双天线定向特性确定的相关时间。
4.根据权利要求3所述的方法，其特征在于，所述2.3根据所述状态量X的状态转移矩阵φ一步预测n-1时刻到n时刻状态量
Figure FDA0002673455360000021
状态协方差矩阵Pn|n-1，包括：
根据下述公式预测得到n-1时刻到n时刻状态量
Figure FDA0002673455360000022
状态协方差矩阵Pn|n-1：
Figure FDA0002673455360000023
Pn|n-1＝φn|n-1Pn-1φn|n-1 T+(φn|n-1Q+Qφn|n-1 T)δt/2
其中，φn|n-1为n-1时刻到n时刻状态转移矩阵；
Figure FDA0002673455360000024
为n-1时刻状态量X的状态估计值；Pn-1为n-1时刻状态协方差矩阵，Q为系统噪声矩阵；δt为滤波估计更新周期。
5.根据权利要求4所述的方法，其特征在于，所述2.4更新n时刻的状态估计量
Figure FDA0002673455360000025
和n时刻的状态协方差Pn，包括：
根据下述公式更新n时刻的状态估计量
Figure FDA0002673455360000026
Figure FDA0002673455360000027
其中，Zn为n时刻的观测量即修正后的农机航向角实际测量值和预测得到的农机运动轨迹预测航向角
Figure FDA0002673455360000028
之间差值；Hn为n时刻的观测矩阵，由农机航向角观测误差
Figure FDA0002673455360000029
与估计的观测误差
Figure FDA00026734553600000210
之间的关系得到：
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为单位矩阵。
6.根据权利要求5所述的方法，其特征在于，所述2.5根据更新后的n时刻的状态估计量
Figure FDA00026734553600000211
得到GNSS双天线航向安装角度偏置，即根据下式计算得到n时刻GNSS双天线航向安装角度偏置：
Figure FDA00026734553600000212
其中
Figure FDA0002673455360000031
为状态量X在n时刻的第二维估计值。
7.根据权利要求6所述的方法，其特征在于，所述根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角
Figure FDA0002673455360000032
包括：
假设k-1时刻农机的位置为Pk-1(xk-1,yk-1)，k时刻农机的位置为Pk(xk,yk)，则根据下述公式预测农机运动轨迹预测航向角
Figure FDA0002673455360000033
Figure FDA0002673455360000034
其中，δx＝xk-xk-1，表示x轴方向的位置差；δy＝yk-yk-1，表示y轴方向的位置差；
Figure FDA0002673455360000035
表示位置差的算术平方根。
8.根据权利要求7所述的方法，其特征在于，所述方法在通过GNSS双天线实时测量的农机车头方向
Figure FDA0002673455360000036
和预测得到的农机运动轨迹预测航向角
Figure FDA0002673455360000037
进行组合滤波估计的时机为农机处于非静止状态且角速度小于30度/秒。
9.一种基于RTK定位的GNSS双天线测量航向的校准方法，其特征在于，所述GNSS双天线横向安装在农机车顶的左右两侧，所述方法采用权利要求1-8任一项所述的GNSS双天线航向安装角度偏置的估计方法得到GNSS双天线航向安装角度偏置
Figure FDA0002673455360000038
进而根据
Figure FDA0002673455360000039
得到经补偿修正后的农机车头航向
Figure FDA00026734553600000310
Figure FDA00026734553600000311
为GNSS双天线实时测量的农机车头方向。
10.一种农机自动驾驶路线规划方法，所述方法采用GNSS双天线实时测量农机车头方向
Figure FDA00026734553600000312
所述GNSS双天线横向安装在农机车顶的左右两侧，其特征在于，所述方法采用权利要求1-8任一项所述的GNSS双天线航向安装角度偏置的估计方法得到GNSS双天线航向安装角度偏置
Figure FDA00026734553600000313
或者采用权利要求9所述的基于RTK定位的GNSS双天线测量航向的校准方法得到经补偿修正后的农机车头航向
Figure FDA00026734553600000314
进而根据GNSS双天线航向安装角度偏置
Figure FDA00026734553600000315
或者经补偿修正后的农机车头航向
Figure FDA00026734553600000316
进行农机自动驾驶路线规划。
CN202010950068.1A 2020-09-09 2020-09-09 一种gnss双天线航向安装角度偏置估计方法 Active CN112147656B (zh)

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 true CN112147656A (zh) 2020-12-29
CN112147656B 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)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114543795A (zh) * 2021-12-31 2022-05-27 文远苏行(江苏)科技有限公司 双天线航向角的安装误差估计方法和调整方法及相关设备
CN116892898A (zh) * 2023-09-11 2023-10-17 农业农村部南京农业机械化研究所 农机的轨迹误差检测方法、装置及系统

Citations (7)

* Cited by examiner, † Cited by third party
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 南京天辰礼达电子科技有限公司 一种履带式拖拉机运动学估计及偏差校准方法

Patent Citations (7)

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

* Cited by examiner, † Cited by third party
Title
缪存孝 等: "基于双GNSS天线及单陀螺的车轮转角测量系统", 《农业机械学报》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114543795A (zh) * 2021-12-31 2022-05-27 文远苏行(江苏)科技有限公司 双天线航向角的安装误差估计方法和调整方法及相关设备
CN114543795B (zh) * 2021-12-31 2024-01-02 文远苏行(江苏)科技有限公司 双天线航向角的安装误差估计方法和调整方法及相关设备
CN116892898A (zh) * 2023-09-11 2023-10-17 农业农村部南京农业机械化研究所 农机的轨迹误差检测方法、装置及系统
CN116892898B (zh) * 2023-09-11 2024-02-02 农业农村部南京农业机械化研究所 农机的轨迹误差检测方法、装置及系统

Also Published As

Publication number Publication date
CN112147656B (zh) 2021-05-04

Similar Documents

Publication Publication Date Title
CN112147656B (zh) 一种gnss双天线航向安装角度偏置估计方法
CN107741745A (zh) 一种实现移动机器人自主定位与地图构建的方法
AU2004201913B2 (en) Direct modification of DGPS information with inertial measurement data
Chiang et al. Assessment for INS/GNSS/odometer/barometer integration in loosely-coupled and tightly-coupled scheme in a GNSS-degraded environment
AU2004201910B2 (en) Inertial augmentation for GPS navigation on ground vehicles
WO2020238011A1 (zh) 一种履带式拖拉机运动学估计及偏差校准方法
CN107132563B (zh) 一种里程计结合双天线差分gnss的组合导航方法
AU2016247575A1 (en) Dynamic motion compensation
CN111703432B (zh) 一种智能履带车辆滑动参数实时估计方法
CN110631574B (zh) 一种惯性/里程计/rtk多信息融合方法
US9969081B2 (en) Method and system for the directed control of robotic assets
CN112146561A (zh) 一种霍尔角度传感器安装角度偏置的估计方法
CN113203429B (zh) 一种陀螺仪温度漂移误差的在线估计及补偿方法
CN110346821B (zh) 一种解决gps长时间失锁问题的sins/gps组合定姿定位方法及系统
CN112050809A (zh) 轮式里程计与陀螺仪信息融合的无人车定向定位方法
CN112731354B (zh) Agv上激光雷达位姿的自标定方法
US8229661B2 (en) Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering
CN114966629A (zh) 一种基于ekf算法框架的车体激光雷达外参标定方法
Brunker et al. GNSS-shortages-resistant and self-adaptive rear axle kinematic parameter estimator (SA-RAKPE)
CN110395297B (zh) 列车定位方法
EP4143507B1 (en) Navigation apparatus and method in which measurement quantization errors are modeled as states
CN113074757A (zh) 车载惯导安装误差角的标定方法
González et al. Comparative study of localization techniques for mobile robots based on indirect kalman filter
EP3879232B1 (en) Method and system for detection of roll sensor bias
CN116338719A (zh) 基于b样条函数的激光雷达-惯性-车辆融合定位方法

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