[發明專利]一種慣導和雙天線GNSS外參標定方法

在審

• 申請號: 202110563502.5
• 申請日: 2021-05-24
• 公開（公告）號: CN113340298A
• 公開（公告）日: 2021-09-03
• 發明（設計）人: 岑益挺;賴際舟;呂品;李志敏;白師宇;王炳清;孫鑫;許曉偉;余文斌;朱靜漪;劉瑞
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/34;G01C25/00;G01S19/47
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 曹蕓

1.一種慣導和雙天線GNSS外參在線標定方法，其特征在于，包括以下步驟：

步驟1，周期采集k時刻車載傳感器信息，包括慣性傳感器信息和雙天線GNSS信息；

步驟2，根據k時刻的慣性傳感器數據，預測k時刻載體的姿態、速度、位置信息；

步驟3，若k時刻采集到雙天線GNSS數據，則通過卡爾曼濾波器估計k時刻慣導和雙天線GNSS之間的外參；否則，跳轉至步驟1；

步驟4，跳轉至步驟1。

2.根據權利要求1所述的一種慣導和雙天線GNSS外參在線標定方法，其特征在于，所述步驟2的具體過程如下：

1)采用如下公式預測載體的姿態：

其中：

q(k)＝[q₀(k) q₁(k) q₂(k) q₃(k)]^T為k時刻的姿態四元數，上標T表示矩陣的轉置，其中q₀(k)、q₁(k)、q₂(k)、q₃(k)為表示k時刻載體姿態的4個數字；

q(k-1)＝[q₀(k-1) q₁(k-1) q₂(k-1) q₃(k-1)]^T為k-1時刻的姿態四元數，其中q₀(k-1)、q₁(k-1)、q₂(k-1)、q₃(k-1)為表示k-1時刻載體姿態的4個數字；

ΔT為離散采樣周期；

其中為k時刻機體系相對于導航系的角速率在機體系X、Y、Z軸上的分量；

為k時刻采集到的三軸陀螺數據；

為k-1時刻的導航系到機體系的姿態轉移矩陣；

為k-1時刻地球自轉角速率在導航系上的分量，ω_ie為地球自轉角速率，L(k-1)為k-1時刻的緯度；

為k-1時刻導航系相對于地球系的角速度在導航系上的分量，為k-1時刻的速度在導航系東向、北向上的分量，L(k-1)、h(k-1)為k-1時刻的緯度和高度，R_M、R_N為地球的子午圈、卯酉圈半徑；

2)采用如下公式預測載體的速度：

其中：

為k時刻的速度，為k時刻的速度在導航系東向、北向、天向上的分量；

為k-1時刻的速度，為k-1時刻的速度在導航系東向、北向、天向上的分量；

為k時刻的機體系到導航系的姿態轉移矩陣；

f^b(k)為k時刻采集到的三軸加速度計數據；

gⁿ為地球重力加速度在導航系上的分量；

3)采用如下公式預測載體的位置：

其中：

λ(k)、L(k)、h(k)為k時刻的經度、緯度和高度；

λ(k-1)、L(k-1)、h(k-1)為k-1時刻的經度、緯度和高度。

3.根據權利要求1所述的一種慣導和雙天線GNSS外參在線標定方法，其特征在于，所述步驟3的具體過程如下：

1)計算狀態量的一步預測值

式中，

Φ_M＝diag{0,0,0,0,0,0,0,0,0,0}

為載體的速度在導航系東向、北向、天向上的分量，L為載體的緯度，h為載體的高度，R為地球半徑，為三軸加速度計的輸出在導航系東北天方向上的分量，即為機體系到導航系的姿態轉移矩陣，0_m×n為m×n的零矩陣，Φ_k,k-1為濾波器k-1時刻到k時刻的一步轉移矩陣，為k-1時刻到k時刻的狀態量一步預測值，為k-1時刻濾波器狀態量估計值，φ_E、φ_N、φ_U為東向、北向、天向平臺誤差角，δv_E、δv_N、δv_U為東向、北向、天向速度誤差，δL、δλ、δh為緯度、經度、高度誤差，為陀螺三軸零偏，為加速度計三軸零偏，ψ為雙天線GNSS基線之間的航向與慣導軸向之間的偏差角，為GNSS主天線與慣導之間的三軸桿臂；

2)計算一步預測均方誤差P_k|k-1

式中，

P_k|k-1為k-1時刻到k時刻的一步預測均方誤差，P_k-1為k-1時刻的狀態估計均方誤差，上標T表示矩陣轉置；

Γ_k-1為濾波器k-1時刻的系統噪聲矩陣；

Q_k-1＝diag{ε_gx²,ε_gy²,ε_gz²,ε_ax²,ε_ay²,ε_az²}

Q_k-1為k-1時刻的系統噪聲，diag表示矩陣對角化，其中ε_gx、ε_gy、ε_gz分別為的模型噪聲，ε_ax、ε_ay、ε_az分別為的模型噪聲；

3)計算k時刻卡爾曼濾波器的濾波增益K_k

式中，

H_p＝[0_3×6 diag{R_M+h,(R_N+h)cosL,1} 0_3×7 H_l]

H_v＝[0_3×3 diag{1,1,1} 0_3×13]

H_k為k時刻的量測矩陣，由位置量測矩陣H_p、速度量測矩陣H_v、航向量測矩陣組成，表示取矩陣的第m行，θ、分別為載體的俯仰角、航向角，可通過下式計算：

其中：q₀、q₁、q₂、q₃為表示載體姿態的四元數；

R_k為k時刻的量測噪聲，diag表示矩陣對角化，其中ε_p為位置量測的噪聲，ε_v為速度量測的噪聲，為角度量測的噪聲，上標-1表示矩陣求逆；

4)計算k時刻卡爾曼濾波器的狀態估計值

其中，為k時刻濾波器狀態量的估計值，為k-1時刻到k時刻的狀態量一步預測值，Z_k為k時刻的量測值，Z_p、Z_v、為位置、速度、航向量測值，L_I、λ_I、h_I為k時刻慣導預測的緯度、經度、高度，L_G、λ_G、h_G為k時刻雙天線GNSS輸出的緯度、經度、高度，V_IE、V_IN、V_IU為k時刻慣導預測的速度在導航系東北天方向上的分量，V_GE、V_GN、V_GU為k時刻雙天線GNSS輸出的速度在導航系東北天方向上的分量，為k時刻慣導預測的航向，為雙天線GNSS輸出的航向；

5)計算k時刻卡爾曼濾波器的估計均方誤差P_k|k

P_k|k＝(I-K_kH_k)P_k|k-1

其中，P_k|k為k時刻的估計均方誤差，I為單位矩陣；

6)基于卡爾曼濾波器，通過量測值Z_k對慣導和雙天線GNSS之間的外參進行估計。