[發明專利]一種基于二階量測更新的低成本姿態估計方法

有效

• 申請號: 201410047878.0
• 申請日: 2014-02-11
• 公開（公告）號: CN103822633A
• 公開（公告）日: 2014-05-28
• 發明（設計）人: 周廣濤;孫艷濤;姜鑫;梁宏;時貴敏;趙博;郝勤順;李佳璇;林萌萌;張麗麗
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于二階量測更新的低成本姿態估計方法，其特征是：

(1)在k=0時刻，即初始時刻由初始對準得到初始姿態φ₀，所述初始姿態包括橫搖、縱搖和航向；

(2)在k≥1時刻，利用微機械陀螺輸出的載體三軸的角速率數據，計算出姿態角的變化值Δφ，由式計算出濾波開始前的粗略姿態角；

(3)采集微機械加速度計和磁力計輸出的載體三軸線的加速度和磁力數據；

(4)設濾波的狀態變量為x=[q_e?b_g?b_a]^T∈R^9×1；式中：q_e表示四元數誤差矢量部分，b_g表示陀螺漂移，b_a表示加速度計零偏；假設四元數誤差q~e=1qeT,]]>根據四元數相關性質建立狀態方程：

q·eb·gb·a=-[yg×]-0.5I0000000qebgba+-0.5vgvbgvba]]>

式中：[yg×]=0-ygzygyygz0-ygx-ygyygx0,]]>下標g代表陀螺、a代表加速度計、m代表磁力計，y_g表示陀螺實際輸出。傳感器噪聲v_a、v_g以及v_m為零均值高斯白噪聲；

取觀測量為z=ya-C(q^)g~ym-C(q^)m~,]]>由于q_e為小量，再忽略二階小項，建立量測方程：

ya-C(q^)g~=2[C(q^)g~×]qe+va+ba]]>

ym-C(q^)m~=2[C(q^)m~×]qe+vm]]>

式中：表示由估計四元數組成的姿態矩陣，y_a和y_m分別為加速度計和磁強計輸出，定義g~=00gT]]>和m~=cosα0-sinαT,]]>g為重力加速度，α為磁傾角；

(5)利用步驟(4)中建立的狀態方程進行濾波的時間更新，由下式

x^k-=Φkx^k-1]]>

Pk-=ΦkPk-1ΦkT+Qk]]>

分別計算出一步預測量和狀態誤差協方差陣

式中：Φ_k為系統狀態轉移矩陣，Q_k為系統噪聲協方差陣；

(6)在濾波的量測更新階段采用二階量測更新，先進行加速度計量測更新；取此階段觀測量根據建立的量測方程有其更新過程如下：

先計算濾波增益：Ka,k=Pk-Ha,k′(Ha,kPk-Ha,k′+Ra)-1;]]>

然后計算狀態估計量：x^a,k=x^k-+Ka,k(za,k-Ha,kx^k-);]]>

再計算狀態誤差協方差陣：Pa,k=(I-Ka,kHa,k)Pk-(I-Ka,kHa,k)′+Ka,kRaKa,k′;]]>

式中：下標a表示加速度計量測更新階段，R_a為加速度計噪聲協方差陣

此時的9個狀態都會更新，利用更新用代表已更新狀態，取即前三個變量值，利用式進行校正，然后進行四元數規范化：

q^⇐q^||q^||]]>

加速度計更新結束后，將的q_e部分設置為零；

(7)利用步驟(6)中得到的進行磁力計量測更新；取此階段觀測量為zm,k≡ym,k-C(q^k)m~,]]>則得Hm,k≡2[C(q^)m^×]00,]]>其更新過程如下：

取此時狀態誤差協方差陣為

Pm,k-=Pa,k(1:3,1:3)03×606×306×6]]>

由此計算濾波增益：Km,k=r3r3′03×606×306×6Pm,k-Hm,k′(Hm,kPm,k-Hm,k′+Rm)-1;]]>

然后計算狀態估計量：xk′=x^a,k+Km,k(zm,k-Hm,kx^a,k);]]>

再計算狀態誤差協方差陣：P_k=(I-K_m,kH_m,k)P_a,k(I-K_m,kH_m,k)′+K_m,kR_mK′_m,k；

式中：下標m表示磁力計量測更新階段，P_a,k(1:3,1:3)表示由P_a,k前三行前三列所組成的矩陣，0_mn為m行n列的零矩陣并且r3=C(q^)001T;]]>

(8)根據濾波計算出的四元數誤差，校正步驟(2)中的粗略姿態角，得到精確的姿態角。