[發明專利]一種GNSS雙天線航向安裝角度偏置估計方法

有效

• 申請號: 202010950068.1
• 申請日: 2020-09-09
• 公開（公告）號: CN112147656B
• 公開（公告）日: 2021-05-04
• 發明（設計）人: 朱龍泉;陳彬;程方;陳向東;王浩;王爍;吳飛
• 申請（專利權）人: 無錫卡爾曼導航技術有限公司
• 主分類號: G01S19/40
• 分類號: G01S19/40
• 代理公司: 哈爾濱市陽光惠遠知識產權代理有限公司 23211
• 代理人: 林娟

1.一種GNSS雙天線航向安裝角度偏置的估計方法，其特征在于，所述GNSS雙天線橫向安裝在農機車頂的左右兩側，所述方法包括：

根據RTK定位的精確位置進行差分，預測農機運動軌跡方向，得到農機運動軌跡預測航向角

通過GNSS雙天線實時測量農機車頭方向

根據通過GNSS雙天線實時測量的農機車頭方向和預測得到的農機運動軌跡預測航向角進行組合濾波估計，實時估計得到GNSS雙天線航向安裝角度偏置以便根據對農機車頭方向進行補償修正，得到補償修正后的農機車頭航向

所述根據通過GNSS雙天線實時測量的農機車頭方向和預測得到的農機運動軌跡預測航向角進行組合濾波估計，實時估計得到GNSS雙天線航向安裝角度偏置包括：

2.1選取農機航向角觀測誤差和GNSS雙天線航向安裝角度偏置作為濾波估計的狀態量X；其中，農機航向角觀測誤差為經補償修正后的農機車頭航向真值與農機運動軌跡預測航向角的差值；

2.2根據農機航向角觀測誤差與GNSS雙天線計算所述狀態量X的狀態轉移矩陣φ；

2.3根據所述狀態量X的狀態轉移矩陣φ一步預測n-1時刻到n時刻狀態量狀態協方差矩陣P_n|n-1；

2.4更新n時刻的狀態估計量和n時刻的狀態協方差P_n；

2.5根據更新后的n時刻的狀態估計量計算得到GNSS雙天線航向安裝角度偏置

2.根據權利要求1所述的方法，其特征在于，所述2.2根據農機航向角觀測誤差與GNSS雙天線計算所述狀態量X的狀態轉移矩陣φ，包括：

狀態量X為：

狀態轉移矩陣φ為：

其中，τ為根據GNSS雙天線定向特性確定的相關時間。

3.根據權利要求2所述的方法，其特征在于，所述2.3根據所述狀態量X的狀態轉移矩陣φ一步預測n-1時刻到n時刻狀態量狀態協方差矩陣P_n|n-1，包括：

根據下述公式預測得到n-1時刻到n時刻狀態量狀態協方差矩陣P_n|n-1：

P_n|n-1＝φ_n|n-1P_n-1φ_n|n-1^T+(φ_n|n-1Q+Qφ_n|n-1^T)δt/2

其中，φ_n|n-1為n-1時刻到n時刻狀態轉移矩陣；為n-1時刻狀態量X的狀態估計值；P_n-1為n-1時刻狀態協方差矩陣，Q為系統噪聲矩陣；δt為濾波估計更新周期。

4.根據權利要求3所述的方法，其特征在于，所述2.4更新n時刻的狀態估計量和n時刻的狀態協方差P_n，包括：

根據下述公式更新n時刻的狀態估計量

其中，Z_n為n時刻的觀測量即修正后的農機航向角實際測量值和預測得到的農機運動軌跡預測航向角之間差值；H_n為n時刻的觀測矩陣，由農機航向角觀測誤差與估計的觀測誤差之間的關系得到：

H＝[1 0]

K_n為n時刻濾波增益；

K_n＝P_n|n-1H_n^T(H_nP_n|n-1H_n^T+R_n)^-1

其中，R_n為n時刻觀測噪聲協方差矩陣，由GNSS信息統計得到；

根據下述公式更新n時刻的狀態協方差：

P_n＝(I-K_nH_n)P_n|n-1(I-K_nH_n)^T+K_nR_nK_n^T

其中，I為單位矩陣。