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

本发明专利技术公开了一种GNSS双天线航向安装角度偏置估计方法，属于多信息融合技术领域。所述方法中GNSS双天线横向安装在农机车顶的左右两侧，根据GNSS双天线实时测量的农机车头方向

【技术实现步骤摘要】
一种GNSS双天线航向安装角度偏置估计方法
本专利技术涉及一种GNSS双天线航向安装角度偏置估计方法，属于多信息融合

技术介绍
在农机自动驾驶
内，自动驾驶路线的规划需要确定农机的当前位置和实时航向，而获取农机的实时航向的技术中，有一种方法是通过在农机车顶的左右两侧横向安装GNSS(GlobalNavigationSatelliteSystem，全球导航卫星系统)的主副双天线用以测量航向，通常主副双天线的测量航向旋转90度后即是车头方向。但是天线安装存在误差，因此GNSS主副双天线的测量航向旋转90度后的方向和农机实际运动轨迹的方向存在一个固定的小夹角即航向安装偏置角。该偏置角的大小在GNSS主副双天线安装后不会再发生变化，或者因为机器使用磨损等原因存在很小的变化，但是一旦GNSS主副双天线重新安装，就会产生一个新的安装偏置角，也就是说它是随着GNSS主副双天线的安装随机产生的。由于安装偏置角的存在，使得农机自动驾驶时会一直偏向目标线的一侧，导致农机自动驾驶路线偏航，给农业生产生活带来不便。传统的校准方法是在相同路段上由专业的安装服务人员自动驾驶一个约70米长度的来回，采用均值法进行校准，即获取农机在相同路段上自动驾驶时主副双天线的多个测量航向值，对多个测量航向值取均值作为主副双天线的安装偏置角。此方法需要专业人员进行操作且费时费力，而且如果GNSS双天线的任何一个天线位置因为重新安装等原因发生变化，该安装角就会发生变化，需要重新校准，也即需要专业的安装服务人员重复上述校准过程，费时费力且会带来较大的售后服务成本。
技术实现思路
为了解决目前校准GNSS主副双天线的时存在的费时费力且不能实时进行校准的问题，本专利技术提供了一种基于RTK定位的GNSS双天线的估计方法，用以实时估计出GNSS双天线的安装角度偏置，进而对GNSS双天线测量航向进行实时校准，所述方法包括：根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角通过GNSS双天线实时测量农机车头方向根据通过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.2根据农机航向角观测误差与GNSS双天线计算所述状态量X的状态转移矩阵φ，包括：状态量X为：状态转移矩阵φ为：其中，τ为根据GNSS双天线定向特性确定的相关时间。可选的，所述2.3根据所述状态量X的状态转移矩阵φ一步预测n-1时刻到n时刻状态量状态协方差矩阵Pn|n-1，包括：根据下述公式预测得到n-1时刻到n时刻状态量状态协方差矩阵Pn|n-1：Pn|n-1＝φn|n-1Pn-1φn|n-1T+(φn|n-1Q+Qφn|n-1T)δt/2其中，φn|n-1为n-1时刻到n时刻状态转移矩阵；为n-1时刻状态量X的状态估计值；Pn-1为n-1时刻状态协方差矩阵，Q为系统噪声矩阵；δt为滤波估计更新周期。可选的，所述2.4更新n时刻的状态估计量和n时刻的状态协方差Pn，包括：根据下述公式更新n时刻的状态估计量其中，Zn为n时刻的观测量即修正后的农机航向角实际测量值和预测得到的农机运动轨迹预测航向角之间差值；Hn为n时刻的观测矩阵，由农机航向角观测误差与估计的观测误差之间的关系得到：H＝[10]；Kn为n时刻滤波增益；Kn＝Pn|n-1HnT(HnPn|n-1HnT+Rn)-1其中，Rn为n时刻观测噪声协方差矩阵，由GNSS信息统计得到；根据下述公式更新n时刻的状态协方差：Pn＝(I-KnHn)Pn|n-1(I-KnHn)T+KnRnKnT其中，I为单位矩阵。可选的，所述2.5根据更新后的n时刻的状态估计量计算得到GNSS双天线航向安装角度偏置即根据下式计算得到n时刻GNSS双天线航向安装角度偏置：其中为状态量X在n时刻的第二维估计值。可选的，所述根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角包括：假设k-1时刻农机的位置为Pk-1(xk-1,yk-1)，k时刻农机的位置为Pk(xk,yk)，则根据下述公式预测农机运动轨迹预测航向角其中，δx＝xk-xk-1，表示x轴方向的位置差；δy＝yk-yk-1，表示y轴方向的位置差；表示位置差的算术平方根。可选的，所述方法在通过GNSS双天线实时测量的农机车头方向和预测得到的农机运动轨迹预测航向角进行组合滤波估计的时机为农机处于非静止状态且角速度小于30度/秒。本申请的第二个目的在于提供一种基于RTK定位的GNSS双天线测量航向的校准方法，所述GNSS双天线横向安装在农机车顶的左右两侧，所述方法采用上述的GNSS双天线航向安装角度偏置的估计方法得到GNSS双天线航向安装角度偏置进而根据得到经补偿修正后的农机车头航向为GNSS双天线实时测量的农机车头方向。本申请的第三个目的在于提供一种农机自动驾驶路线规划方法，所述方法采用GNSS双天线实时测量农机车头方向所述GNSS双天线横向安装在农机车顶的左右两侧，所述方法采用上述的GNSS双天线航向安装角度偏置的估计方法得到GNSS双天线航向安装角度偏置或者采用上述基于RTK定位的GNSS双天线测量航向的校准方法得到经补偿修正后的农机车头航向进而根据GNSS双天线航向安装角度偏置或者经补偿修正后的农机车头航向进行农机自动驾驶路线规划。可选的，所述农机包括拖拉机、插秧机、平地机、收割机、打药机。本专利技术有益效果是：通过提供一种基于RTK定位的GNSS双天线的估计方法，GNSS双天线横向安装在农机车顶的左右两侧，根据通过GNSS双天线实时测量的农机车头方向和预测得到的农机运动轨迹预测航向角进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置以便根据对农机车头方向进行补偿修正，得到补偿修正后的农本文档来自技高网...

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

【技术特征摘要】
1.一种GNSS双天线航向安装角度偏置的估计方法，其特征在于，所述GNSS双天线横向安装在农机车顶的左右两侧，所述方法包括：
根据RTK定位的精确位置进行差分，预测农机运动轨迹方向，得到农机运动轨迹预测航向角
通过GNSS双天线实时测量农机车头方向
根据通过GNSS双天线实时测量的农机车头方向和预测得到的农机运动轨迹预测航向角进行组合滤波估计，实时估计得到GNSS双天线航向安装角度偏置以便根据对农机车头方向进行补偿修正，得到补偿修正后的农机车头航向


2.根据权利要求1所述的方法，其特征在于，所述根据通过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双天线航向安装角度偏置


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



状态转移矩阵φ为：



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


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



Pn|n-1＝φn|n-1Pn-1φn|n-1T+(φn|n-1Q+Qφn|n-1T)δt/2
其中，φn|n-1为n-1时刻到n时刻状态转移矩阵；为n-1时刻状态量X的状态估计值；Pn-1为n-1时刻状态协方差矩阵，Q为系统噪声矩阵；δt为滤波估计更新周期。


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

【专利技术属性】
技术研发人员：朱龙泉，陈彬，程方，陈向东，王浩，王烁，吴飞，
申请(专利权)人：无锡卡尔曼导航技术有限公司，
类型：发明
国别省市：江苏;32