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

摘要

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

权利要求

1.一种GNSS双天线航向安装角度偏置的估计方法，其特征在于，所述GNSS双天线横向安装在农机车顶的左右两侧，所述方法包括：根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角

通过GNSS双天线实时测量农机车头方向根据通过GNSS双天线实时测量的农机车头方向 和预测得到的农机运动轨迹预测航向角 进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置 以便根据对农机车头方向 进行补偿修正，得到补偿修正后的农机车头航向所述根据通过GNSS双天线实时测量的农机车头方向 和预测得到的农机运动轨迹预测航向角 进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置 包括：

2.1选取农机航向角观测误差 和GNSS双天线航向安装角度偏置 作为滤波估计的状态量X；其中，农机航向角观测误差 为经补偿修正后的农机车头航向 真值与农机运动轨迹预测航向角 的差值；

2.2根据农机航向角观测误差 与GNSS双天线 计算所述状态量X的状态转移矩阵φ；

2.3根据所述状态量X的状态转移矩阵φ一步预测n‑1时刻到n时刻状态量 状态协方差矩阵Pn|n‑1；

2.4更新n时刻的状态估计量 和n时刻的状态协方差Pn；

2.5根据更新后的n时刻的状态估计量 计算得到GNSS双天线航向安装角度偏置

2.根据权利要求1所述的方法，其特征在于，所述2.2根据农机航向角观测误差 与GNSS双天线 计算所述状态量X的状态转移矩阵φ，包括：状态量X为：

状态转移矩阵φ为：

其中，τ为根据GNSS双天线定向特性确定的相关时间。

3.根据权利要求2所述的方法，其特征在于，所述2.3根据所述状态量X的状态转移矩阵φ一步预测n‑1时刻到n时刻状态量 状态协方差矩阵Pn|n‑1，包括：根据下述公式预测得到n‑1时刻到n时刻状态量 状态协方差矩阵Pn|n‑1：T T

Pn|n‑1＝φn|n‑1Pn‑1φn|n‑1+(φn|n‑1Q+Qφn|n‑1)δt/2其中，φn|n‑1为n‑1时刻到n时刻状态转移矩阵； 为n‑1时刻状态量X的状态估计值；

Pn‑1为n‑1时刻状态协方差矩阵，Q为系统噪声矩阵；δt为滤波估计更新周期。

4.根据权利要求3所述的方法，其特征在于，所述2.4更新n时刻的状态估计量 和n时刻的状态协方差Pn，包括：

根据下述公式更新n时刻的状态估计量其中，Zn为n时刻的观测量即修正后的农机航向角实际测量值和预测得到的农机运动轨迹预测航向角 之间差值；Hn为n时刻的观测矩阵，由农机航向角观测误差 与估计的观测误差 之间的关系得到：

H＝[1 0]

Kn为n时刻滤波增益；

T T ‑1

Kn＝Pn|n‑1Hn(HnPn|n‑1Hn+Rn)其中，Rn为n时刻观测噪声协方差矩阵，由GNSS信息统计得到；

根据下述公式更新n时刻的状态协方差：T T

Pn＝(I‑KnHn)Pn|n‑1(I‑KnHn) +KnRnKn其中，I为单位矩阵。

5.根据权利要求4所述的方法，其特征在于，所述2.5根据更新后的n时刻的状态估计量得到GNSS双天线航向安装角度偏置，即根据下式计算得到n时刻GNSS双天线航向安装角度偏置：

其中 为状态量X在n时刻的第二维估计值。

6.根据权利要求5所述的方法，其特征在于，所述根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角 包括：假设k‑1时刻农机的位置为Pk‑1(xk‑1，yk‑1)，k时刻农机的位置为Pk(xk，yk)，则根据下述公式预测农机运动轨迹预测航向角其中，δx＝xk‑xk‑1，表示x轴方向的位置差；δy＝yk‑yk‑1，表示y轴方向的位置差；

表示位置差的算术平方根。

7.根据权利要求6所述的方法，其特征在于，所述方法在通过GNSS双天线实时测量的农机车头方向 和预测得到的农机运动轨迹预测航向角 进行组合滤波估计的时机为农机处于非静止状态且角速度小于30度/秒。

8.一种基于RTK定位的GNSS双天线测量航向的校准方法，其特征在于，所述GNSS双天线横向安装在农机车顶的左右两侧，所述方法采用权利要求1‑7任一项所述的GNSS双天线航向安装角度偏置的估计方法得到GNSS双天线航向安装角度偏置 进而根据得到经补偿修正后的农机车头航向 为GNSS双天线实时测量的农机车头方向。

9.一种农机自动驾驶路线规划方法，所述方法采用GNSS双天线实时测量农机车头方向所述GNSS双天线横向安装在农机车顶的左右两侧，其特征在于，所述方法采用权利要求

1‑7任一项所述的GNSS双天线航向安装角度偏置的估计方法得到GNSS双天线航向安装角度偏置 或者采用权利要求8所述的基于RTK定位的GNSS双天线测量航向的校准方法得到经补偿修正后的农机车头航向 进而根据GNSS双天线航向安装角度偏置 或者经补偿修正后的农机车头航向 进行农机自动驾驶路线规划。

说明书

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

技术领域

[0001] 本发明涉及一种GNSS双天线航向安装角度偏置估计方法，属于多信息融合技术领域。

背景技术

[0002] 在农机自动驾驶技术领域内，自动驾驶路线的规划需要确定农机的当前位置和实时航向，而获取农机的实时航向的技术中，有一种方法是通过在农机车顶的左右两侧横向
安装GNSS(Global Navigation Satellite System，全球导航卫星系统)的主副双天线用以
测量航向，通常主副双天线的测量航向 旋转90度后即是车头方向。

[0003] 但是天线安装存在误差，因此GNSS主副双天线的测量航向 旋转90度后的方向和农机实际运动轨迹的方向存在一个固定的小夹角即航向安装偏置角。该偏置角的大小在
GNSS主副双天线安装后不会再发生变化，或者因为机器使用磨损等原因存在很小的变化，
但是一旦GNSS主副双天线重新安装，就会产生一个新的安装偏置角，也就是说它是随着
GNSS主副双天线的安装随机产生的。由于安装偏置角的存在，使得农机自动驾驶时会一直
偏向目标线的一侧，导致农机自动驾驶路线偏航，给农业生产生活带来不便。

[0004] 传统的校准方法是在相同路段上由专业的安装服务人员自动驾驶一个约70米长度的来回，采用均值法进行校准，即获取农机在相同路段上自动驾驶时主副双天线的多个
测量航向 值，对多个测量航向 值取均值作为主副双天线的安装偏置角。此方法需要专
业人员进行操作且费时费力，而且如果GNSS双天线的任何一个天线位置因为重新安装等原
因发生变化，该安装角就会发生变化，需要重新校准，也即需要专业的安装服务人员重复上
述校准过程，费时费力且会带来较大的售后服务成本。

发明内容

[0005] 为了解决目前校准GNSS主副双天线的时存在的费时费力且不能实时进行校准的问题，本发明提供了一种基于RTK定位的GNSS双天线的估计方法，用以实时估计出GNSS双天
线的安装角度偏置，进而对GNSS双天线测量航向进行实时校准，所述方法包括：

[0006] 根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角

[0007] 通过GNSS双天线实时测量农机车头方向

[0008] 根据通过GNSS双天线实时测量的农机车头方向 和预测得到的农机运动轨迹预测航向角 进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置 以便根
据 对农机车头方向 进行补偿修正，得到补偿修正后的农机车头航向

[0009] 其中，所述GNSS双天线横向安装在农机车顶的左右两侧，

[0010] 可选的，所述根据通过GNSS双天线实时测量的农机车头方向 和预测得到的农机运动轨迹预测航向角 进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置
包括：

[0011] 2.1选取农机航向角误差 和GNSS双天线航向安装角度偏置 作为滤波估计的状态量X；其中，农机航向角误差 为经补偿修正后的农机车头航向 真值与农机运动轨
迹预测航向角 的差值；

[0012] 2.2根据农机航向角观测误差 与GNSS双天线 计算所述状态量X的状态转移矩阵φ；

[0013] 2.3根据所述状态量X的状态转移矩阵φ一步预测n‑1时刻到n时刻状态量 状态协方差矩阵Pn|n‑1；

[0014] 2.4更新n时刻的状态估计量 和n时刻的状态协方差Pn；

[0015] 2.5根据更新后的n时刻的状态估计量 计算得到GNSS双天线航向安装角度偏置

[0016] 可选的，所述2.2根据农机航向角观测误差 与GNSS双天线 计算所述状态量X的状态转移矩阵φ，包括：

[0017] 状态量X为：

[0018]

[0019] 状态转移矩阵φ为：

[0020]

[0021] 其中，τ为根据GNSS双天线定向特性确定的相关时间。

[0022] 可选的，所述2.3根据所述状态量X的状态转移矩阵φ一步预测n‑1时刻到n时刻状态量 状态协方差矩阵Pn|n‑1，包括：

[0023] 根据下述公式预测得到n‑1时刻到n时刻状态量 状态协方差矩阵Pn|n‑1：

[0024]

[0025] Pn|n‑1＝φn|n‑1Pn‑1φn|n‑1T+(φn|n‑1Q+Qφn|n‑1T)δt/2

[0026] 其中，φn|n‑1为n‑1时刻到n时刻状态转移矩阵； 为n‑1时刻状态量X的状态估计值；

[0027] Pn‑1为n‑1时刻状态协方差矩阵，Q为系统噪声矩阵；δt为滤波估计更新周期。

[0028] 可选的，所述2.4更新n时刻的状态估计量 和n时刻的状态协方差Pn，包括：

[0029] 根据下述公式更新n时刻的状态估计量

[0030]

[0031] 其中，Zn为n时刻的观测量即修正后的农机航向角实际测量值和预测得到的农机运动轨迹预测航向角 之间差值；Hn为n时刻的观测矩阵，由农机航向角观测误差 与估计
的观测误差 之间的关系得到：

[0032] H＝[1 0]；

[0033] Kn为n时刻滤波增益；

[0034] Kn＝Pn|n‑1HnT(HnPn|n‑1HnT+Rn)‑1

[0035] 其中，Rn为n时刻观测噪声协方差矩阵，由GNSS信息统计得到；

[0036] 根据下述公式更新n时刻的状态协方差：

[0037] Pn＝(I‑KnHn)Pn|n‑1(I‑KnHn)T+KnRnKnT

[0038] 其中，I为单位矩阵。

[0039] 可选的，所述2.5根据更新后的n时刻的状态估计量 计算得到GNSS双天线航向安装角度偏置 即根据下式计算得到n时刻GNSS双天线航向安装角度偏置：

[0040]

[0041] 其中 为状态量X在n时刻的第二维估计值。可选的，所述根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角 包括：

[0042] 假设k‑1时刻农机的位置为Pk‑1(xk‑1,yk‑1)，k时刻农机的位置为Pk(xk,yk)，则根据下述公式预测农机运动轨迹预测航向角

[0043]

[0044] 其中，δx＝xk‑xk‑1，表示x轴方向的位置差；δy＝yk‑yk‑1，表示y轴方向的位置差；表示位置差的算术平方根。

[0045] 可选的，所述方法在通过GNSS双天线实时测量的农机车头方向 和预测得到的农机运动轨迹预测航向角 进行组合滤波估计的时机为农机处于非静止状态且角速度小
于30度/秒。

[0046] 本申请的第二个目的在于提供一种基于RTK定位的GNSS双天线测量航向的校准方法，所述GNSS双天线横向安装在农机车顶的左右两侧，所述方法采用上述的GNSS双天线航
向安装角度偏置的估计方法得到GNSS双天线航向安装角度偏置 进而根据
得到经补偿修正后的农机车头航向 为GNSS双天线实时测量的农机车头方向。

[0047] 本申请的第三个目的在于提供一种农机自动驾驶路线规划方法，所述方法采用GNSS双天线实时测量农机车头方向 所述GNSS双天线横向安装在农机车顶的左右两侧，
所述方法采用上述的GNSS双天线航向安装角度偏置的估计方法得到GNSS双天线航向安装
角度偏置 或者采用上述基于RTK定位的GNSS双天线测量航向的校准方法得到经补偿
修正后的农机车头航向 进而根据GNSS双天线航向安装角度偏置 或者经补偿修正后
的农机车头航向 进行农机自动驾驶路线规划。

[0048] 可选的，所述农机包括拖拉机、插秧机、平地机、收割机、打药机。

[0049] 本发明有益效果是：

[0050] 通过提供一种基于RTK定位的GNSS双天线的估计方法，GNSS双天线横向安装在农机车顶的左右两侧，根据通过GNSS双天线实时测量的农机车头方向 和预测得到的农机
运动轨迹预测航向角 进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置
以便根据 对农机车头方向 进行补偿修正，得到补偿修正后的农机车头航向
此方法不但避免了需要专业人员多次校准带来的费时费力的麻烦，而且实时校准还避免了
农机因为使用磨损而导致的使用误差，同时，本申请方法的估计精度在±0.15度范围内，误
差非常小；校准时间10秒可以收敛到±0.5度，校准时间在50s内可以完全收敛，相对于传统
校准方法，时间大大缩短，而且，采用本申请方法不需要专业的校准人员，只需要开动农机
即可。

附图说明

[0051] 为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于
本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他
的附图。

[0052] 图1是本发明一个实施例中GNSS双天线航向安装角度偏置的估计方法的流程图。

[0053] 图2是一种车辆运动学轨迹模型示意图。

[0054] 图3是本发明一个实施例中基于RTK定位的GNSS双天线测量航向的校准方法的流程图。

[0055] 图4是本发明一个实施例中基于RTK定位的GNSS双天线航向安装角度偏置的估计方法的估计结果仿真图。

[0056] 图5是本发明一个实施例中基于RTK定位的GNSS双天线测量航向的校准方法修正后的农机航向角仿真图。

具体实施方式

[0057] 为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。

[0058] 实施例一：

[0059] 本实施例提供一种基于RTK定位的GNSS双天线的估计方法，参见图1，所述方法包括：

[0060] 根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角

[0061] 通过GNSS双天线实时测量农机车头方向

[0062] 根据通过GNSS双天线实时测量的农机车头方向 和预测得到的农机运动轨迹预测航向角 进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置 以便根
据 对农机车头方向 进行补偿修正，得到补偿修正后的农机车头航向

[0063] 在组合滤波估计时，包括：

[0064] 2.1选取农机航向角误差 和GNSS双天线航向安装角度偏置 作为滤波估计的状态量X；其中，农机航向角误差 为经补偿修正后的农机车头航向 真值与农机运动轨
迹预测航向角 的差值；

[0065] 状态量X为：

[0066]

[0067] 2.2根据农机航向角观测误差 与GNSS双天线 计算所述状态量X的状态转移矩阵φ；状态转移矩阵φ为：

[0068]

[0069] 其中，τ为根据GNSS双天线定向特性确定的相关时间。

[0070] 2.3根据所述状态量X的状态转移矩阵φ一步预测n‑1时刻到n时刻状态量 状态协方差矩阵Pn|n‑1；

[0071] 根据下述公式预测得到n‑1时刻到n时刻状态量 状态协方差矩阵Pn|n‑1：

[0072]

[0073] Pn|n‑1＝φn|n‑1Pn‑1φn|n‑1T+(φn|n‑1Q+Qφn|n‑1T)δt/2

[0074] 其中，φn|n‑1为n‑1时刻到n时刻状态转移矩阵； 为n‑1时刻状态量X的状态估计值；

[0075] Pn‑1为n‑1时刻状态协方差矩阵，Q为系统噪声矩阵；δt为滤波估计更新周期。

[0076] 2.4更新n时刻的状态估计量 和n时刻的状态协方差Pn；

[0077] 根据下述公式更新n时刻的状态估计量

[0078]

[0079] 其中，Zn为n时刻的观测量即修正后的农机航向角实际测量值和预测得到的农机运动轨迹预测航向角 之间差值；Hn为n时刻的观测矩阵，由农机航向角观测误差 与估计
的观测误差 之间的关系得到；Kn为n时刻滤波增益；

[0080] Kn＝Pn|n‑1HnT(HnPn|n‑1HnT+Rn)‑1

[0081] 其中，Rn为n时刻观测噪声协方差矩阵，由GNSS信息统计得到；

[0082] 根据下述公式更新n时刻的状态协方差：

[0083] Pn＝(I‑KnHn)Pn|n‑1(I‑KnHn)T+KnRnKnT

[0084] 其中，I为单位矩阵。

[0085] 2.5根据更新后的n时刻的状态估计量 计算得到GNSS双天线航向安装角度偏置

[0086] 实施例二

[0087] 本实施例提供一种基于RTK(Real‑Time Kinematic，实时动态)定位的GNSS双天线测量航向的校准方法，应用于农机设备中，将GNSS双天线横向安装在农机车顶的左右两侧，
所述方法包括：

[0088] S1根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角

[0089]

[0090] 其中，如图2所示，假设k‑1时刻农机的位置为Pk‑1(xk‑1,yk‑1)，k时刻农机的位置为Pk(xk,yk)；

[0091] 农机运动轨迹预测航向角，即农机运动轨迹预测航向与正北方向的夹角，范围0到360度；

[0092] δx：δx＝xk‑xk‑1，x轴方向的位置差；

[0093] δy：δy＝yk‑yk‑1，y轴方向的位置差；

[0094] δxy： 位置差的算术平方根；

[0095] S2通过GNSS双天线实时测量农机车头方向 即GNSS双天线实时测量的航向旋转90度后得到的方向与正北方向的夹角；

[0096] S3 Kalman滤波估计：

[0097] 根据S2中通过GNSS双天线实时测量农机车头方向 和S1预测得到的农机运动轨迹预测航向角 进行组合滤波估计，实时估计和修正GNSS双天线。

[0098] 估计流程如图3所示，该估计过程为不断迭代的过程，包括：

[0099] 3.1GNSS双天线航向更新；

[0100] 即获取GNSS双天线实时测量的农机车头方向

[0101] 3.2农机航向角修正：

[0102] 根据Kalman滤波估计的 实时补偿修正农机车头方向，得到经补偿修正后的农机车头航向

[0103]

[0104] 其中：

[0105] 经补偿修正后的农机车头航向；

[0106] GNSS双天线实时测量的航向旋转90度后得到的方向；

[0107] Kalman滤波估计的GNSS双天线。

[0108] 由图3可知第一次补偿修正时，GNSS双天线短时间内变化非常小接近为零，因此可假设此时根据Kalman滤波估计得到的 为零，也即第一次进行角度补偿修正时，经补偿
修正后的农机车头航向 就等于GNSS双天线实时测量的航向旋转90度后得到的方向。

[0109] 3.3判断农机是否静止或者角速度过大；

[0110] 当农机静止时，其位置变化量非常小，静止状态时位移量通常低于5cm，此时位置差分得到的预测航向角误差非常大；而当农机快速转弯时即车身的旋转角速率较大，由于
位置差分的滞后，此时差分得到的航向角误差也比较大。因此如果此时继续进行滤波估计
会带来较大的误差甚至是滤波发散。

[0111] 考虑GNSS双天线航向安装角偏置在短时间内是不变的，此时统一进行零速修正，无限放大观测噪声，滤波估计中的观测量即预测航向权重非常小，在静止状态下其权重系
数直接接近于0，等价于不滤波修正，因此若判断结果为农机处于静止状态或者角速度大于
阈值30度/秒，则修改观测噪声矩阵R。

[0112] 若判断结果为农机并不是处于静止状态且角速度小于判定阈值，则进行下一步状态转移矩阵的计算。

[0113] 3.4状态转移矩阵计算：

[0114] 具体的，选取农机航向角误差和GNSS双天线航向安装角偏置作为滤波估计的状态量，即：

[0115]

[0116] 其中：

[0117] 农机航向角观测误差，即为经补偿修正后的农机车头航向 真值与S1中农机运动轨迹预测航向角 的差值；

[0118] Kalman滤波估计的GNSS双天线航向安装角偏置。

[0119] 同样由于GNSS双天线短时间内变化非常小甚至是不变的，其安装偏置角误差模型可近似为一阶马尔科夫过程，则 写成矩阵形式可以得到状态转移矩阵φ
如下：

[0120]

[0121] 其中：

[0122] τ：相关时间，根据GNSS双天线定向特性可选定3600秒，也可以选择其他值，比如7200秒、600秒等。

[0123] 3.5Kalman一步预测：

[0124] 在得到上述状态转移矩阵后，一步预测n‑1时刻到n时刻状态量 状态协方差矩阵Pn|n‑1。

[0125]

[0126] Pn|n‑1＝φn|n‑1Pn‑1φn|n‑1T+(φn|n‑1Q+Qφn|n‑1T)δt/2

[0127] 其中：

[0128] n‑1时刻状态量X的估计值；

[0129] Pn‑1：n‑1时刻状态协方差矩阵；

[0130] φn|n‑1：n‑1时刻到n时刻状态转移矩阵；

[0131] Q：系统噪声矩阵；

[0132] δt：滤波估计更新周期。

[0133] 3.6Kalman滤波估计更新：

[0134] Kalman滤波估计更新状态量和状态协方差矩阵。

[0135] 根据下式计算得到n时刻GNSS双天线航向安装角度偏置：

[0136]

[0137] 其中 为状态量X在n时刻的第二维估计值。

[0138] 为防止滤波估计出现异常偏差，在滤波估计时加入质量控制，即当农机航向角观测误差 与估计的观测误差 相差较大时，需要对估计的观测误差进行一定的修正。

[0139] 滤波估计时的滤波增益：

[0140] Kn＝Pn|n‑1HnT(HnPn|n‑1HnT+Rn)‑1

[0141] 其中：

[0142] Kn：n时刻滤波增益；

[0143] Hn：n时刻的观测矩阵，由农机航向角观测误差 与估计的观测误差 之间的关系得到。H＝[1 0]；

[0144] Rn：n时刻观测噪声协方差矩阵，由GNSS信息统计得到，接近直行时选定2deg/s，随着角速率增大，观测噪声Rn线性变大，斜率系数为2，即

[0145] Rn＝2+2*ωn，ωn是n时刻的农机旋转角速率,由GNSS双天线航向差分得到。

[0146] 根据下述公式进行状态更新：

[0147]

[0148] 其中：

[0149] n时刻的状态估计量；

[0150] Zn：n时刻的观测量即修正后的农机航向角实际测量值和S1预测得到的农机运动轨迹预测航向角 之间差值即：

[0151]

[0152] 根据下述公式进行状态协方差更新：

[0153] Pn＝(I‑KnHn)Pn|n‑1(I‑KnHn)T+KnRnKnT

[0154] 其中：

[0155] Pn：n时刻状态协方差矩阵；

[0156] I：单位矩阵。

[0157] 3.7反馈修正：

[0158] 滤波估计后利用估计后的状态量对当前航向安装角偏置和农机航向进行反馈修正，得到修正后的农机车头航向

[0159] 为验证本申请所提出的基于RTK定位的GNSS双天线的估计方法实时估计出的准确性，本实施例进行了验证实验，预先采用其他方式测量得到GNSS双天线在农机上的为‑1.5
度。

[0160] 如图4所示，为采用本实施例提供的基于RTK定位的GNSS双天线的估计方法的仿真结果，根据图4可知，采用本实施例滤波估计的稳定在‑1.5度左右，误差范围为±0.15度，且
根据图4可知，本申请提供的基于RTK定位的GNSS双天线的估计方法在50s时已完全趋于稳
定状态，相对于传统校准方法，时间大大缩短，而且，采用本申请方法不需要专业的校准人
员。

[0161] 图5为手动开动农机实际运行轨迹，从图中可以看出运动轨迹存在频繁的调头(航向变化180度)、角速率很大的快速调头(航向变化非常快)、农机静止(航向基本不变)等各
种运动场景，图4是对应滤波估计的安装角度偏置结果，从图4可以看出采用本实例方法估
计的安装角偏置非常稳定且精度高，该方法适用于各种场景。

[0162] 本发明实施例中的部分步骤，可以利用软件实现，相应的软件程序可以存储在可读取的存储介质中，如CPU或光盘或硬盘等。

[0163] 以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。