[發明專利]一種位置姿態系統的濾波方法

有效

• 申請號: 201210027735.4
• 申請日: 2012-02-08
• 公開（公告）號: CN102538792A
• 公開（公告）日: 2012-07-04
• 發明（設計）人: 房建成;陳霖周廷;宮曉琳
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01S19/49
• 代理公司: 北京科迪生專利代理有限責任公司 11251
• 代理人: 成金玉

1.一種位置姿態系統的濾波方法，其特征在于：包括下列步驟：

(1)建立位置姿態系統的高階誤差模型；

位置姿態系統的高階誤差模型包括系統狀態方程和量測方程：

a)系統狀態方程

X·=F·X+G·W]]>

其中，X=δLδλδhδVEδVNδVUφEφNφU▿bx▿by▿bz▿rx▿ry▿rz]]>▿mx▿my▿mzδKAxδKAyδKAzδAyzδAzyδAxzδAzxδAxyδAyxϵbxϵbyϵbzϵrxϵryϵrzϵmxϵmyϵmz]]>δKGxδKGyδKGzδGyzδGzyδGxzδGzxδGxyδGyx1×45T]]>為系統狀態向量；δL、δλ、δh為緯度誤差、經度誤差、高度誤差；δV_E、δV_N、δV_U為東向速度誤差、北向速度誤差、天向速度誤差；φ_E、φ_N、φ_U為東向水平姿態失準角、北向水平姿態失準角、航向失準角；為加速度計三個測量軸上的隨機常值偏置；為加速度計三個測量軸上的隨機游走偏置；為加速度計三個測量軸上的一階馬爾科夫過程偏置；δK_Ax、δK_Ay、δK_Az為加速度計三個測量軸上的刻度因子誤差；δA_zx、δA_yx，δA_zy、δA_xy，δA_yz、δA_xz為加速度計三個測量軸上的安裝誤差，每個加速度計測量軸的安裝誤差用兩個參數表示，因此加速度計三個測量軸的安裝誤差共六個；ε_bx、ε_by、ε_bz為陀螺三個測量軸上的隨機常值漂移；ε_rx、ε_ry、ε_rz為陀螺三個測量軸上的隨機游走漂移；ε_mx、ε_my、ε_mz為陀螺三個測量軸上的一階馬爾科夫過程漂移；δK_Gx、δK_Gy、δK_Gz為陀螺三個測量軸上的刻度因子誤差；δG_zx、δG_yx，δG_zy、δG_xy，δG_yz、δG_xz為陀螺三個測量軸上的安裝誤差，每個陀螺測量軸的安裝誤差用兩個參數表示，因此陀螺三個測量軸的安裝誤差共六個；為系統狀態向量的導數，F為系統狀態轉移矩陣，G為系統噪聲分配矩陣，W為系統噪聲向量，具體表達式如下：

W=ωaxωayωazωgxωgyωgzωarxωaryωarzωamxωamyωamzωgrxωgryωgrzωgmxωgmyωgmz1×18T]]>

G=03×303×303×303×303×303×3Cbn03×303×303×303×303×303×3Cbn03×303×303×303×303×303×303×303×303×303×303×303×3I3×303×303×303×303×303×303×3I3×303×303×3012×3012×3012×3012×3012×3012×303×303×303×303×3I3×303×303×303×303×303×303×3I3×309×309×309×309×309×309×3,45×18]]>F=F13×903×303×303×903×303×303×9F23×9CbnCbnF33×903×303×303×9F43×903×303×303×9CbnCbnF53×903×903×303×303×903×303×303×903×903×3F63×303×903×303×303×9012×9012×3012×3012×9012×3012×3012×903×903×303×303×903×3F73×303×909×909×309×309×909×309×309×945×45,]]>

F1=00-VN(Rm+h)201Rm+h0000VEsecLtanLRn+h0-VEsecL(Rn+h)2secLRn+h000000000010003×9,]]>

F3=Cbn(1,1)fxCbn(1,2)fyCbn(1,3)fzCbn(1,1)fyCbn(1,2)fxCbn(1,1)fzCbn(1,3)fxCbn(1,2)fzCbn(1,3)fyCbn(2,1)fxCbn(2,2)fyCbn(2,3)fzCbn(2,1)fyCbn(2,2)fxCbn(2,1)fzCbn(2,3)fxCbn(2,2)fzCbn(2,3)fyCbn(3,1)fxCbn(3,2)fyCbn(3,3)fzCbn(3,1)fyCbn(3,2)fxCbn(3,1)fzCbn(3,3)fxCbn(3,2)fzCbn(3,3)fy3×9,]]>

F5=Cbn(1,1)ωxCbn(1,2)ωyCbn(1,3)ωzCbn(1,1)ωyCbn(1,2)ωxCbn(1,1)ωzCbn(1,3)ωxCbn(1,2)ωzCbn(1,3)ωyCbn(2,1)ωxCbn(2,2)ωyCbn(2,3)ωzCbn(2,1)ωyCbn(2,2)ωxCbn(2,1)ωzCbn(2,3)ωxCbn(2,2)ωzCbn(2,3)ωyCbn(3,1)ωxCbn(3,2)ωyCbn(3,3)ωzCbn(3,1)ωyCbn(3,2)ωxCbn(3,1)ωzCbn(3,3)ωxCbn(3,2)ωzCbn(3,3)ωy3×9,]]>

F6=diag(-1α,-1α,-1α),]]>F7=diag(-1β,-1β,-1β);]]>ω_ax、ω_ay、ω_az為加速度計三個測量軸上的白噪聲偏置；ω_gx、ω_gy、ω_gz為陀螺三個測量軸上的白噪聲漂移；ω_arx、ω_ary、ω_arz為加速度計三個測量軸上的隨機游走偏置驅動白噪聲；ω_amx、ω_amy、ω_amz為加速度計三個測量軸上的一階馬爾科夫過程偏置驅動白噪聲；ω_grx、ω_gry、ω_grz為陀螺三個測量軸上的隨機游走漂移驅動白噪聲；ω_gmx、ω_gmy、ω_gmz為陀螺三個測量軸上的一階馬爾科夫過程漂移驅動白噪聲；為載體坐標系到地理系的轉換矩陣，I為單位陣，V_E、V_N、V_U為運動載體速度在地理系下的分量，R_n與R_m分別為卯酉圈與子午圈的主曲率半徑，h為運動載體的高度，L為運動載體的緯度，ω_ie為地球自轉角速度，f_E、f_N、f_U為運動載體加速度在地理坐標系下的分量，f_x、f_y、f_z為運動載體加速度在載體坐標系下的分量，ω_x、ω_y、ω_z為運動載體角速度在載體坐標系下的分量，α與β為陀螺和加速度計一階馬爾科夫過程的相關時間；

b)量測方程

Z＝H·X+v

其中，Z為量測向量，H為量測矩陣，X為系統狀態向量，v為測量噪聲向量，具體表達式如下：

Z=LSINS-LGNSSλSINS-λGNSShSINS-hGNSSVESINS-VEGNSSVNSINS-VNGNSSVUSINS-VUGNSS6×1,]]>v=vLvλvhvvevvnvvu6×1,]]>H=Rm0000001×390RncosL000001×3900100001×3900010001×3900001001×3900000101×396×45;]]>

L_SINS-L_GNSS，λ_SINS-λ_GNSS，h_SINS-h_GNSS，V_ESINS-V_EGNSS，V_NSINS-V_NGNSS，V_USINS-V_UGNSS，為SINS與GNSS輸出的緯度、經度、高度、東向速度、北向速度、天向速度之差；v_L，v_λ，v_h，v_ve，v_vn，v_vu為GNSS的緯度、經度、高度、東向速度、北向速度、天向速度的測量噪聲，均為零均值隨機白噪聲，其方差陣為R；R_n與R_m分別為卯酉圈與子午圈的主曲率半徑，L為當地緯度；

(2)設計基于三參數遞推的卡爾曼濾波器，其設計步驟為：

a)將系統狀態方程離散化，為：

Xk=Fk/k-1Xk-1+Wk-1Fk/k-1=I+TF(tk-1)+T22!F2(tk-1)+T33!F3(tk-1)+···]]>

其中，下標k-1、k分別表示t_k-1和t_k時刻，X_k、X_k-1為t_k、t_k-1時刻的系統狀態向量，W_k-1為t_k-1時刻系統噪聲向量陣，F_k/k-1為t_k-1時刻到t_k時刻的系統狀態一步轉移陣，I為單位陣，T＝t_k-t_k-1為濾波周期，F(t_k-1)為常數矩陣；

b)在離散化后的系統狀態方程和量測方程引入基于三參數遞推的濾波模型：

Xk=C1Fk/k-1C2Fk/k-2C3Fk/k-3Xk-1Xk-2Xk-3+Wk-1,]]>ZkZk-1Zk-2=HXkXk-1Xk-2+vkvk-1vk-2]]>

其中，下標k、k-1、k-2分別表示t_k、t_k-1、t_k-2時刻，X_k、X_k-1、X_k-2、X_k-3為t_k、t_k-1、t_k-2、t_k-3時刻的系統狀態向量，F_k/k-1為k-1時刻到k時刻的系統狀態一步轉移陣，F_k/k-2為k-2時刻到k時刻的系統狀態轉移陣，F_k/k-3為k-3時刻到k時刻的系統狀態轉移陣，C₁、C₂、C₃為狀態轉移陣F_k/k-1、F_k/k-2、F_k/k-3的加權系數，W_k-1為t_k-1時刻系統噪聲向量陣，Z_k、Z_k-1、Z_k-2為t_k、t_k-1、t_k-2時刻的量測向量，H為量測矩陣，v_k、v_k-1、v_k-2為t_k、t_k-1、t_k-2時刻的測量噪聲向量；

(3)基于步驟(1)中建立的位置姿態系統的高階誤差模型，取SINS與GNSS的定位結果之差作為觀測量，采用SINS/GNSS分布式組合形式，通過步驟(2)設計的基于三參數遞推的卡爾曼濾波器對各誤差狀態進行最優濾波估計；

(4)將步驟(3)中計算得到的位置誤差、速度誤差、姿態誤差和慣性器件誤差的最優濾波估計值反饋回SINS，并對陀螺漂移、加速度計偏置以及載體坐標系相對于計算坐標系的轉換矩陣進行校正，計算出更加精確的位置、速度和姿態信息；

修正SINS的位置、速度、姿態的公式為：

a)位置修正

Lat′=Lat-δ^L]]>

Lon′=Lon-δ^λ]]>

Height′=Height-δ^h]]>

其中，Lat′、Lon′、Height′為修正后的緯度、經度、高度信息；Lat、Lon、Height為SINS輸出的緯度、經度、高度信息；為緯度誤差、經度誤差、高度誤差的最優濾波估計值；

b)速度修正

Ve′=Ve-δ^Ve]]>

Vn′=Vn-δ^Vn]]>

Vu′=Vu-δ^Vu]]>

其中，Ve′、Vn′、Vu′為修正后的東向速度、北向速度、天向速度信息；Ve、Vn、Vu為SINS輸出的東向速度、北向速度、天向速度信息；為東向速度誤差、北向速度誤差、天向速度誤差的最優濾波估計值；

c)姿態修正

Cnc=cosφU×cosφN-sinφU×sinφE×sinφNsinφU×cosφN+cosφU×sinφE×sinφN-cosφE×sinφN-sinφU×cosφEcosφU×cosφEsinφEcosφU×sinφN+sinφU×sinφE×cosφNsinφU×sinφN-cosφU×sinφE×cosφNcosφE×cosφN]]>

其中，為地理系n到計算系c的方向余弦矩陣，即姿態誤差矩陣；φ_E、φ_N、φ_U為東向水平姿態失準角、北向水平姿態失準角、航向失準角的最優濾波估計值：

Cnb=Ccb×Cnc]]>

其中，為地理系n到載體系b的方向余弦矩陣，即修正后的姿態矩陣；為計算系c到載體系b的方向余弦矩陣，即SINS輸出的姿態矩陣；根據即可計算出載體的姿態角，包括航向角、俯仰角、橫滾角。