簡化超球體SRUKF的GPS/MIMU組合姿態(tài)估計

沈曉衛(wèi)，常瑞花，袁 丁

（1. 火箭軍工程大學 核工程系，西安 710025；2. 武警工程大學 研究所，西安 710086）

簡化超球體SRUKF的GPS/MIMU組合姿態(tài)估計

沈曉衛(wèi)1，常瑞花2，袁 丁1

（1. 火箭軍工程大學 核工程系，西安 710025；2. 武警工程大學 研究所，西安 710086）

針對加速度計、磁強計易受機動加速度和周圍環(huán)境影響的問題，提出了一種單基線GPS/MIMU組合姿態(tài)估計方法。該方法利用單基線GPS天線代替磁強計提供航向信息，并利用GPS的速度信息對加速度計輸出的機動加速度分量進行補償，構成單基線GPS/MIMU組合姿態(tài)確定單元。同時考慮實時性要求，在SRUKF的基礎上，引入加性非擴展形式和超球體單形采樣，提出了簡化超球體平方根UKF算法，通過減少狀態(tài)維數(shù)和采樣點的數(shù)量，降低算法計算量。建立加性噪聲下的單基線GPS/MIMU姿態(tài)模型，并采用簡化超球體平方根UKF算法進行姿態(tài)估計。實驗結果表明，單基線GPS的速度信息可以有效提高加速度計對水平傾角的確定精度，姿態(tài)估計算法收斂后的最大誤差小于0.8°，估計精度與UKF相當，且執(zhí)行時間僅是UKF的42%。

姿態(tài)估計；平方根無跡卡爾曼濾波；機動加速度；超球體分布采樣

姿態(tài)確定在無人機、穩(wěn)定平臺和天線穩(wěn)定系統(tǒng)等領域得到了廣泛應用。為保證測控的精度，往往采用高精度航姿系統(tǒng)（attitude and heading reference system，AHRS）或者高精度慣性導航系統(tǒng)（inertial navigation system，INS）對擾動進行穩(wěn)定隔離[1]。這種方法雖然測控精度高 ，運行穩(wěn)定可靠，但成本較高，僅AHRS或INS的價格就達數(shù)十萬乃至上百萬，嚴重制約了推廣應用。

為了降低成本，一個有效的途徑是采用微機械慣性傳感器的姿態(tài)確定方法。由于微機械陀螺存在漂移及姿態(tài)初值問題，需要以陀螺為基準，通過姿態(tài)估計算法融合加速度計和磁強計等傳感器的輔助信息，提高姿態(tài)確定精度[2]。但加速度計易受機動加速度的影響，磁強計易受周圍環(huán)境如車體平臺、車上元件，以及附近的建筑物、高壓電線、大的金屬物等電磁場的干擾，大多應用在低精度、低動態(tài)的無人機和機器人等領域。為了提高機動狀態(tài)下的姿態(tài)估計精度，文獻[3]提出了一種自適應擴展卡爾曼濾波算法，通過對載體機動行為的判斷，自適應調節(jié)測量噪聲方差，但這種方法采用向量模的平方作為判據(jù)，存在判斷靈敏度低的問題。文獻[4]利用模糊邏輯對機動狀態(tài)進行判別，結合Kalman濾波器的觀測噪聲方差進行自適應調整，構成自適應姿態(tài)算法。上述方法均采用自適應濾波器對機動加速度進行抑制，在有機動加速度時，通過調整觀測噪聲方差，更多地依賴陀螺信息估計姿態(tài)。由于陀螺誤差，載體在短時間機動時可取得較好的效果，但長期機動時卻不能保持穩(wěn)定的姿態(tài)精度。另外，姿態(tài)確定系統(tǒng)大多是非線性系統(tǒng)，多采用非線性濾波算法進行融合。傳統(tǒng)的擴展卡爾曼濾波（extended Kalman filter，EKF）是姿態(tài)確定領域應用最廣的非線性濾波方法，但EKF簡單地將非線性模型線性化，存在估計精度較低和發(fā)散問題。相對于EKF的一階泰勒近似，無跡卡爾曼濾波（unscented Kalman filter，UKF）對任何非線性非高斯數(shù)據(jù)可達到二階精確，但存在計算量大的問題[5]。文獻[6]將超球體采樣和平方根濾波相結合，提出了超球體平方根無跡卡爾曼算法（square root unscented Kalman filter，SRUKF），采用超球體采樣減少采樣點數(shù)量，同時在UKF濾波的時間更新和量測更新階段采用平方根濾波將狀態(tài)估計誤差協(xié)方差陣的Cholesky因子形式直接傳遞，解決了UKF數(shù)值不穩(wěn)定性。但是該算法采用非擴展形式，將過程噪聲和測量噪聲引入狀態(tài)向量中，導致了維數(shù)和sigma點的增加。

針對上述問題，本文采用單基線GPS天線代替磁強計提供航向信息，并利用GPS的速度信息對加速度計輸出的機動加速度分量進行補償，構成單基線GPS/MIMU組合姿態(tài)確定單元。同時考慮實時性要求，在SRUKF的基礎上，引入加性非擴展形式和超球體單形采樣，提出了簡化超球體平方根 UKF算法（simple spherical simplex square root unscented Kalman filter，SSSRUKF）。實驗結果表明該算法能改善UKF的計算效率。

1 基于單基線GPS/MIMU的組合模型

MIMU（包括三軸微機械陀螺和三軸加速度計）具有短時間高精度特性，能及時敏感載體的姿態(tài)變化，但其單獨工作下姿態(tài)誤差較大；單基線GPS天線在載體上前后放置，在無陰影情況下可以準確輸出速度和航向角信息，但也存在信號遮蔽、更新頻率低等缺陷。因此，MIMU和單基線GPS具有很好的互補特性，兩者構成的組合姿態(tài)確定模塊可以有效提高精度。

如圖1所示，本文利用單基線GPS提供的航向角和加速度計提供的俯仰角、橫滾角作為輔助觀測信息，并建立基于單基線GPS/MIMU的狀態(tài)方程和測量方程，通過姿態(tài)估計算法對姿態(tài)進行估計。由于加速度計是利用重力分量計算水平傾角，為了避免機動加速度對其影響，利用GPS接收機的速度信息結合載體角速率對加速度計輸出進行補償，然后再以此計算水平傾角。

圖1 單基線GPS天線/MIMU組合框架Fig.1 Combination architecture of single baseline GPS/MIMU

1.1 狀態(tài)方程

定義導航坐標系和載體坐標系分別為參考坐標系和移動坐標系。導航坐標系（n系），是一種當?shù)氐乩碜鴺讼?，坐標軸指向北、東和地。載體坐標系（b系），軸向分別對應載體的橫滾軸、俯仰軸和偏航軸。

定義狀態(tài)變量為x=q，根據(jù)式(1)可以得到狀態(tài)方程為

1.2 測量方程

加速度計置于載體上敏感的是比力，輸出為[7]

當載體處于靜止和勻速時，可忽略機動加速度和向心加速度的影響，此時加速度計敏感的是重力場分量。根據(jù)重力場分量得到載體的水平傾角：

其中，θacc和φacc分別為加速度計輸出估計的俯仰角和橫滾角。

對機動加速度補償后，式(4)可表示為：

結合單基線GPS的航向角和加速度計的水平傾角，根據(jù)歐拉角與四元數(shù)的關系，其測量方程可以表示為：

測量噪聲v為隨機獨立的零均值高斯白噪聲序列。

2 簡化超球體平方根UKF姿態(tài)估計算法

根據(jù)公式(2)陀螺角速率提供的狀態(tài)方程和公式(7)輔助測量信息提供的測量方程，建立姿態(tài)確定系統(tǒng)的非線性系統(tǒng)模型，離散化后表示為：

針對公式(8)中過程噪聲和測量噪聲都是加性的，研究簡化超球體平方根 UKF算法融合各個傳感器的信息，提高姿態(tài)精度。簡化超球體平方根UKF算法在平方根UKF的基礎上，引入加性噪聲非擴展濾波減少狀態(tài)維數(shù)，同時采用超球體單形采樣減少采樣點的數(shù)量，降低了算法的計算量。

2.1 加性超球體采樣策略

UKF通過設計采樣Sigma點的unscented變換（UT）實現(xiàn)預測過程，其計算量與采樣點數(shù)量密切相關。在對實時性要求比較高的系統(tǒng)中，可采用超球體單形采樣減少Sigma點的數(shù)目。超球體單形采樣如下：

2）確定Sigma權值

3）比例修正

4）初始化向量序列（對應狀態(tài)為1維的情況）

式中，j為向量的維數(shù)，i為采樣點的順序。

6）定義Sigma權值

2.2 簡化超球體平方根UKF算法

采用加性非擴展形式和超球體單形采樣策略進行平方根 Unscented卡爾曼濾波運算，并對預測方程和更新方程作相應的修改，即可構成簡化超球體平方根UKF算法。具體步驟為：

1）初始化狀態(tài)變量和協(xié)方差平方根

2）根據(jù)公式(12)計算Sigma點

3）時間更新

4）測量更新

5）k=k+1，轉至第2）步。

3 實驗分析

為了驗證本文算法的有效性，通過跑車實驗對上面的單基線GPS/MIMU組合姿態(tài)確定模型和姿態(tài)估計算法進行驗證。如圖2所示，MIMU采用星網(wǎng)宇達公司的XW-IMU5220，陀螺零偏小于0.08 (°)/s，零偏穩(wěn)定性小于0.05 (°)/s，加速度計零偏小于0.005g，零偏穩(wěn)定性小于0.001g；單基線GPS接收模塊采用XW-ADU3601，其航向角誤差小于0.1°（2 m基線），速度誤差小于0.02 m/s；光纖航姿系統(tǒng)XW-ADU7612的姿態(tài)信息作為參考值，姿態(tài)角誤差小于0.1°。實驗中測試車沿典型水泥路面機動，車速限制在15 m/s，車輛行駛狀態(tài)包括直線運動、轉彎運動等。車輛周圍較為空曠，單基線GPS接收模塊可以準確輸出載體的航向角和速度信息，其中，MIMU采樣頻率為100 Hz，GPS接收模塊的輸出頻率為10 Hz，基線長度為1.57 m。

圖2 設備安裝圖Fig.2 Diagram of equipment installation

圖3 加速度計校正前后的傾角對比Fig.3 Comparison on inclinations of accelerometer before and after correction

圖3給出了加速度計校正前后的傾角對比。加速度計校正之前，加速度計輸出值受機動加速度的影響，其計算的傾角存在較大偏差（最大誤差接近 7°）。其中，載體變速運動產生的前向加速度對俯仰角影響較大，而載體轉彎運動產生的向心加速度對橫滾角影響較大。利用 GPS的速度信息對上述機動加速度補償后，俯仰角和橫滾角估計精度均有了較大提高，其中俯仰角最大誤差小于0.5°，橫滾角最大誤差小于2°。

圖4給出了SSSRUKF估計的姿態(tài)曲線，結果表明單基線GPS/MIMU經過SSSRUKF融合后，姿態(tài)精度得到了較大改善，機動加速度對姿態(tài)估計影響不大，其估計值與參考值基本吻合。

圖5給出了SSSRUKF和UKF在相同條件下的姿態(tài)估計誤差，SSSRUKF需要一段時間才能收斂。表1從估計精度和計算耗時兩方面對 ASSRUKF與 UKF進行比較，其中濾波耗時為獨立運行10次的平均值。

從表 1可以看出：UKF的姿態(tài)估計精度略好于SSSRUKF；UKF和SSSRUKF同在四維狀態(tài)變量情況下，SSSRUKF平均仿真耗時6.84 s（仿真完所有的采樣點），而UKF平均仿真耗時16.30 s，SSSRUKF執(zhí)行時間僅是UKF的算法的42%，且SSSRUKF收斂后的最大誤差小于0.8°。表明從加性噪聲非擴展濾波和超球體單形采樣兩方面，可以有效降低算法的計算量。

圖4 姿態(tài)估計曲線圖Fig.4 Curve of attitude estimation

圖5 SSSRUKF和UKF的估計誤差Fig.5 Estimation errors of SSSRUKF and UKF

表1 SSSRUKF和UKF算法性能對比Tab.1 Comparison on SSSRUKF and UKF

4 結 語

本文構建了一種基于單基線GPS和MIMU的組合姿態(tài)確定模型，俯仰角和橫滾角上利用GPS速度信息對加速度計的非重力加速度分量進行補償，抑制機動加速度對姿態(tài)估計的影響；航向上選擇單基線GPS的信息作為觀測量，校正陀螺漂移誤差。采用簡化超球體平方根 UKF姿態(tài)估計算法來融合各個傳感器信息，在平方根UKF的基礎上引入加性非擴展形式和超球體單形采樣，減少了狀態(tài)維數(shù)和采樣點的數(shù)量。該方法在精度上與UKF相當，執(zhí)行時間僅是UKF的算法的42%。此外，本文研究主要針對是單基線GPS接收模塊收星正常的情況，實際上載體的運動環(huán)境復雜，后續(xù)工作可針對不同的收星情況展開研究。

（References）：

[1] 張華強, 許敬, 趙剡. 一種用于天線跟蹤穩(wěn)定平臺的SINS轉動基座初始對準方法[J]. 中國慣性技術學報, 2014, 22(1): 32-37. Zhang Hua-qiang, Xu Jing, Zhao Yan. Initial alignment of SINS on revolving base for antenna tracking stabilized platform[J]. Journal of Chinese Inertial Technology, 2014, 22(1): 32-37.

[2] Sheng Han-lin, Zhang Tian-hong. MEMS-based low-cost strap-down AHRS research[J]. Measurement, 2015, 59: 63-72.

[3] Li Wei, Wang Jin-ling. Effective adaptive Kalman filter for MEMS-IMU/magnetometers integrated attitude and heading reference systems[J]. Journal of Navigation, 2015, 66(1): 99-113.

[4] Wang Ling-ling, Fu Li, Hu Xiao-guang, et, al. Attitude estimation for UAV with low-cost IMU/ADS based on adaptive-gain complementary filter[C]//13th International Symposium on Neural Networks. St. Petersburg, Russia, 2016: 346-355.

[5] Zhao Jian-wei, Jia Wei-min, Wang Rong, et, al. Attitude estimation based on the spherical simplex transformation modified unscented Kalman filter[J]. Mathematical Problems in Engineering, 2014: 1-10.

[6] Tang Xiao-jun, Yan Jie, Zhong Du-du. Square-root sigmapoint Kalman filtering for spacecraft relative navigation [J]. Acta Astronautica, 2010, 66: 704-713.

[7] Shen Xiao-wei, Yao Min-li, Jia Wei-min, et al. Adaptive complementary filter using fuzzy logic and simultaneous perturbation stochastic approximation algorithm[J]. Measurement, 2012, 45(5): 1257-1265.

[8] Julier S J. The scaled unscented transformation[C]//Proceedings of the American Control Conference. Jefferson City, USA, 2002: 4555-4559.

[9] Giannitrapani A, Ceccarelli N, Scortecci F. Comparison of EKF and UKF for spacecraft localization via angle measurements[J]. IEEE Transactions on Aerospace and Electronic Systems, 2011, 47(1): 75-84.

[10] Henrique M T M, Jo?o Y I, Geovany A B, et, al. A systematization of the unscented Kalman filter theory[J]. IEEE Transactions on Automatic Control, 2015, 60(10): 2583-2598.

GPS/MIMU integrated attitude estimation based on simple spherical simplex SRUKF

SHEN Xiao-wei1, CHANG Rui-hua2, YUAN Ding1
(1. Department of Nuclear Engineering, Rocket Force University of Engineering, Xi’an 710025, China;
2. Research Department, Engineering University of CAPF, Xi’an 710086, China)

In view that accelerometers and magnetometer are susceptible to maneuvering acceleration and surrounding environment, an attitude estimation method combining single baseline GPS with MIMU is proposed. The single baseline GPS antenna instead of the magnetometer is used to provide the heading information. The velocity information coming from GPS measurements can be used to compensate the outputs of accelerometers and eliminate the influence of maneuver acceleration on attitude estimation. Based on the square-root unscented Kalman filter, an additive non-augmented unscented transforms and a spherical simplex sampling are introduced to reduce the state dimension and the number of sigma points. Then the proposed algorithm is used to estimate the attitude under the additive noise condition. The results show that the single baseline GPS can effectively improve the horizontal-inclination determination accuracy of the accelerometer. The maximum error by the proposed algorithm is less than 0.8 ° after the convergence, which is equivalent to that of UKF, while the execution time is only 42% of UKF’s.

attitude estimation; square-root unscented Kalman filter; maneuver acceleration; spherical simplex sampling

V249.3

A

1005-6734(2017)02-0197-06

10.13695/j.cnki.12-1222/o3.2017.02.011

2017-01-10；

2017-03-25

國家自然科學基金（61179005，61179004）

沈曉衛(wèi)（1982—），男，博士，講師，研究方向為慣性導航、非線性濾波、陣列信號處理。E-mail: shenxw602@163.com