基于四元數自適應卡爾曼濾波的快速對準算法

徐曉蘇，周 峰，張 濤，徐 祥

（1. 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096；2. 東南大學 儀器科學與工程學院，南京 210096）

基于四元數自適應卡爾曼濾波的快速對準算法

徐曉蘇1,2，周 峰1,2，張 濤1,2，徐 祥1,2

（1. 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096；2. 東南大學 儀器科學與工程學院，南京 210096）

針對捷聯慣導初始對準問題，提出了一種具有干擾抑制能力的四元數自適應卡爾曼濾波初始對準算法。通過將初始對準問題轉化為Wahba姿態確定問題，直接建立四元數的濾波模型，并采用自適應卡爾曼濾波對初始時刻姿態四元數進行估計，利用姿態四元數更新求出當前姿態來實時地反映載體的姿態變化。針對直接構建量測模型導致收斂速度慢的問題，提出一種基于最優四元數估計法構造K矩陣原理的改進算法。利用三軸轉臺模擬不同的搖擺環境進行實驗，轉臺實驗表明了改進算法具有較快的收斂速度和良好的穩定性及精度，中等精度的慣導系統在150 s至200 s的對準時間內，航向角均值誤差小于2′。

初始對準；四元數；自適應卡爾曼濾波；姿態確定

捷聯慣性導航初始對準作為 SINS研究領域的關鍵技術之一，一直是國內外學者研究的熱點。SINS初始對準一般分為粗對準階段和精對準兩個階段：粗對準階段大多數采用解析法完成，如凝固解析法；精對準階段一般采用狀態估計法，如卡爾曼濾波。SINS初始對準的本質是通過矢量觀測來確定姿態，其求解的算法可分為確定性算法和狀態估計法[1]。在確定性方法中，常用的有基于雙矢量定姿原理的TRAID法[2-3]和基于求解特征值所對應的特征向量的最優四元數估計法[4-5]。傳統TRAID法僅僅利用兩個不同時刻的矢量觀測信息，其余時刻的觀測矢量信息并未充分利用，所選時刻觀測矢量的精度對對準結果的影響較大。文獻[6]中基于慣性空間中重力積分矢量的解析粗對準算法，研究了基于多矢量定姿的粗對準方法，雖然對觀測矢量的利用率大大提升，但是對準精度同樣與所選時刻觀測矢量的精度有較大相關，并且不能實時地反應載體姿態的變化。文獻[7-10]通過觀測矢量和參考矢量構造出K矩陣，并采用相應的估計方法求出K矩陣特征值所對應的特征向量，其本質都為最優四元數估計法（q-method）。與雙矢量定姿法相比，q-method具有較快的收斂速度和良好的精度。文獻[1]中提出直接以對準初始時刻的四元數作為估計狀態的四元數卡爾曼濾波初始對準算法，獲得了較快的收斂速度和滿意的精度，但在該文中所使用的卡爾曼濾波器算法中，需要提前獲悉加速度計隨機噪聲的統計特性，才能保證濾波的穩定性。

本文同樣直接以初始對準時刻的四元數作為估計狀態，建立卡爾曼濾波模型，并采用實用的自適應卡爾曼濾波算法進行估計，該算法具有計算量小，濾波初始參數設置簡單，易于工程實現的特點。針對本文提出的四元數自適應濾波算法存在收斂速度慢的問題，提出了一種改進的四元數自適應濾波算法。該算法基于最優四元數估計法構造K矩陣的原理，通過對觀測模型中的觀測矩陣進行改造，有效地加快了算法的收斂速度。三軸轉臺實驗表明了本文提出的對準方法具有較快的收斂速度和良好的穩定性及精度。

1 坐標系定義

本文涉及的相關坐標系定義如下：

b—載體坐標系，原點位于載體的質心且與載體固連，x沿載體橫軸向右，y沿載體縱軸向前，z沿載體立軸向上，xyz構成右手直角坐標系。

i—基座慣性坐標系，初始對準起始時刻，b系相對于慣性空間凝固后所得的慣性坐標系，它在慣性空間內的指向保持不變。

e—地心地球坐標系，原點位于地心且與地球固連，x軸在赤道平面內與本初子午線相交，y軸與z、x構成右手直角坐標系，z軸沿地球極軸的方向。

n—導航坐標系，x軸沿當地緯線切線指向東，y軸沿著當地經線方向指向北，z軸沿地理垂線向上，也被稱為“東北天”坐標系。

2 四元數初始對準方法

2.1 最優四元數估計法

對準初始時刻t0，由于i系與載體系b系重合，所以根據四元數運算關系有：

在載體無線速度的情況下，由比力方程可得：

式(3)用四元數表示為：

將式(1)代入并整理可得：

令：

則式(5)可表示為：

令：

式中：(α×)、(β×)分別為以α、β向量構成的反對稱矩陣。

式(7)可化為：

此時，根據最優姿態確定的方法，可將對準問題轉化為Wahba姿態確定問題：

只要求出 K矩陣的最小特征值所對應的特征向量并歸一化，即可求得再通過式(1)可求出

2.2 四元數自適應卡爾曼濾波法

式中，Qk和Qk-1分別表示在 tk和 tk-1時刻對應的

由式(9)可建立如下量測模型：

通過以上濾波模型的建立，利用實用的自適應卡爾曼濾波算法進行估計，具體估計過程如下：

2.3 搖擺基座仿真實驗

搖擺基座仿真實驗時采用的運動模型為：

式中：縱搖搖擺幅值Aθ=8°，搖擺頻率fθ=0.15 Hz；橫搖搖擺幅值Aγ=6°，搖擺頻率fγ=0.2 Hz；航向搖擺幅值Aψ=5°，搖擺頻率fψ=0.125 Hz；搖擺起始相位都為0°。仿真采用的捷聯慣性儀表指標為：陀螺常值漂移0.01 (°)/h，陀螺隨機漂移0.01 (°)/h；加速度計常值偏置 50 μg，加速度計隨機偏置 50 μg；經度為118.786365°；緯度為 32.057313°；儀表采樣頻率為200 Hz；捷聯解算周期為5 ms，仿真時間為200 s。

在水平姿態搖擺中心為 0°，航向角搖擺中心為45°時，兩種對準方法的姿態誤差結果如圖1所示。

圖1 最優四元數法和四元數濾波法對準姿態誤差Fig.1 Alignment errors of q-method and QAKF

從圖1中可以看出，相比于最優四元數法，四元數濾波法誤差收斂速度很慢。為此，將針對四元數濾波法存在收斂速度慢的問題進行分析并對算法作改進。

3 問題分析及算法改進

3.1 四元數濾波法對準誤差分析

根據式(6)得出，β(t)是通過當地重力加速度g從n系到i系轉換得到的矢量，由于當地地理位置是已知的，因此，可以認為觀測矢量β(t)是無誤差的，即M(β(t))是無誤差的。考察α(t)，該觀測矢量是加速度計輸出構成的，而加速度計輸出中包含加速度計零偏和隨機偏置，因此，觀測矢量α(t)中是包含誤差的形式，即M(α(t))中是存在誤差的，再由式(12)可知，四元數濾波法觀測模型中的觀測陣Hk是存在誤差的。

式(16)兩邊同乘Qk并結合式(12)得：

將式(17)右邊部分移至左邊得：

從式(18)可以看出，實際的觀測模型中包含了加速度計零偏和加速度計隨機偏置相關項。由于在式(12)中直接忽略了加速度計零偏和隨機偏置相關項的影響，所以從理論上分析得出，導致四元數濾波法收斂慢主要是由于加速度計零偏和隨機偏置所致。

為了驗證以上分析的正確性，本文設計加速度計隨機偏置如表1所示的三種情況。

表1 三種加速度計隨機偏置Tab.1 Three kinds of random bias of accelerometer

仿真實驗其他條件不變，對準姿態誤差如圖2所示。

從圖2中可以看出，在加速度計隨機偏置為0 μg、50 μg和100 μg時，隨著隨機偏置的增大，航向角對準誤差收斂速度由快到慢，且相差明顯，因此，可以得出以下結論：四元數濾波法航向角的收斂速度對加速度計的隨機偏置比較敏感，隨機偏置越大，其收斂速度越慢。在實際工程當中，加速度計的隨機偏置一般都較大，因此，若不對該算法進行改進，將不利于四元數濾波法的實際應用，甚至是失效。加速度計零偏對該算法的影響與隨機偏置相似，由于篇幅所限，本文不再作分析。

圖2 四元數濾波法三種隨機偏置下的對準誤差Fig.2 Alignment errors of QAKF with three kinds of random biases

3.2 改進四元數濾波法

本節通過最優四元數法構造K矩陣的原理，對四元數濾波法進行改進，以加快其收斂速度。構造如下K矩陣：

式(20)兩邊同乘Qk并結合式(12)得：

將式(21)右邊部分移至左邊得：

與式(18)相比較發現：加速度計零偏和隨機偏置項以積分形式出現，且與矩陣相乘。由式不難得出各項元素都為小于 1的實數，因此，式(22)中加速度計零偏和隨機偏置相關項的影響大為減小。

仿真實驗條件同上，最優四元數法、四元數濾波法和改進四元數濾波法三者在20 s到200 s內的對準誤差結果，如圖3所示。

從圖3中可以看出，改進的四元數濾波法的航向角誤差收斂速度和精度都要明顯優于四元數濾波法，且與最優四元數法相當。因此，本文提出的改進四元數濾波法有效。從圖中亦可看出，改進四元數濾波法的穩定性要優于最優四元數法。為了更好地對比改進四元數濾波法和最優四元數法，將加速度計隨機偏置設置為200 μg，其他仿真條件不變，對準姿態誤差如圖4所示。

圖4 最優四元數法和改進四元數濾波法對準誤差Fig.4 Alignment errors of q-method and improved-QAKF

通過圖4可知，在加速度計具有較大的隨機噪聲下，改進四元數濾波法和最優四元數法的水平姿態精度相當且符合粗對準的極限精度，而兩者的航向角對準誤差在 150 s到 200 s以內的均值和標準差分別為0.0030°、0.0032°和 0.0050°、0.0056°。兩者的航向角對準精度相當，但是，前者的穩定性要優于后者。從圖中也可直觀地看出，改進四元數濾波法的穩定性要優于最優四元數法。

4 轉臺實驗驗證及結果分析

為驗證本文算法的有效性，利用FOSN光纖捷聯慣組在三軸轉臺上完成不同實驗條件下的對準實驗，其中光纖陀螺的零位穩定性為0.006 (°)/h (1σ)，零位重復性為0.006 (°)/h (1σ)，隨機游走系數為(1σ)，石英撓性加速度計的零位穩定性為50 μg (1σ)，零位重復性為50 μg (1σ)，捷聯慣組采樣頻率為200 Hz。捷聯慣組安裝如圖5所示。

圖5 捷聯慣組在轉臺安裝位置Fig.5 Placement of IMU on turntable

本文設計多組實驗驗證算法的有效性，實驗1為靜基座實驗，實驗2為小幅度搖擺實驗，實驗3為大幅度搖擺實驗。為了更好地模擬艦船實際情況，縱搖、橫搖和航向搖擺中心分別為2°、-2°和90°，縱搖角、橫搖角和航向角的搖擺頻率分別為0.15 Hz、0.2 Hz和0.125 Hz。實驗條件設置如表2所示。

表2 實驗條件設置Tab.2 Settings of experiment condition

鑒于篇幅所限，本文僅對實驗3進行結果分析。實驗3中，改進四元數濾波法的對準姿態變化如圖6所示，改進四元數濾波法和最優四元數法在80 s 至200 s對準時間內的對準姿態誤差如圖7所示。

由圖7可知，兩者的水平姿態誤差相當，但在航向角誤差上，改進四元數濾波法的波動較小，穩定性明顯優于最優四元數法。在80 s時，改進四元數濾波法的航向角誤差為0.2366°。在對準150 s至200 s的時間內，改進四元數濾波法的均值和標準差分別為0.0198°和 0.0396°。改進四元數濾波法三組實驗在對準150 s至200 s時間內的均值誤差統計如表3所示。

圖6 搖擺及對準姿態變化曲線Fig.6 Attitude changing curve of turntable and alignment

圖7 改進四元數濾波法和最優四元數法對準誤差Fig.7 Alignment errors of q-method and improved-QAKF

表3 對準實驗結果Tab.3 Results of alignment experiment

5 結 論

本文針對捷聯慣導初始對準問題，提出一種四元數自適應卡爾曼濾波對準算法。該算法直接對初始時刻姿態四元數進行估計，并采用了較為簡單且易于工程實現的自適應卡爾曼濾波器。然而，仿真實驗表明了該算法存在收斂速度慢的缺陷。通過對該問題的理論分析，本文進一步提出了一種改進的四元數自適應濾波算法。該算法基于最優四元數估計法構造K矩陣的原理，通過對觀測模型中的觀測矩陣進行改造，有效地降低了加速度計零偏和隨機誤差的影響。仿真實驗及三軸轉臺實驗表明了該算法的正確性和有效性。

（Reference）：

[1] 周琪, 秦永元, 張金紅, 等. 基于四元數卡爾曼濾波的捷聯慣導初始對準算法[J]. 中國慣性技術學報, 2012, 20(2): 162-167. Zhou Qi, Qin Yong-yuan, Zhang Jin-hong, et al. Initial alignment algorithm for SINS based on quaternion Kalman filter[J]. Journal of Chinese Inertial Technology, 2012, 20(2): 162-167.

[2] Cilden D, Conguroglu E S, Hajiyev C. Covariance analysis of three-axis attitude determination using two vector measurements[C]//International Conference on Recent Advances in Space Technologies. IEEE, 2015.

[3] Lee S H, Ahn H S, Yong K L. Three-axis attitude determination using incomplete vector observations[J]. Acta Astronautica, 2009, 65(7): 1089-1093.

[4] Shuster M D. Filter QUEST or REQUEST[J]. Journal of Guidance Control & Dynamics, 2012, 32(2): 643-645.

[5] Crassidis J L, Markley F L, Cheng Y. Survey of nonlinear attitude estimation methods[J]. Journal of Guidance Control & Dynamics, 2007, 30(1): 12-28.

[6] 鄭振宇, 高延濱, 苑志江, 等. 捷聯慣導系統慣性系粗對準原理及試驗[J]. 中國航海, 2015, 38(04): 6-10. Zheng Zhen-yu, Gao Yan-bin, Yuan Zhi-jiang, et al. Principle and experimental study of coarse alignment of SINS in inertial frame[J]. Navigation of China, 2015, 38(4): 6-10.

[7] 李楊, 高敬東, 胡柏青, 等. 艦載捷聯慣導動基座F-QUEST初始對準方法[J]. 海軍工程大學學報, 2014(6): 32-36. Li Yang, Gao Jing-dong, Hu Bai-qing, et al. Shipborne SINS in-movement F-QUEST initial alignment algorithm [J]. Journal of Naval University of Engineering, 2014(6): 32-36.

[8] 高薪, 卞鴻巍, 傅中澤, 等. 捷聯慣導晃動基座四元數估計對準算法[J]. 中國慣性技術學報, 2014, 22(6): 724-727, 740. Gao Xin, Bian Hong-wei, Fu Zhong-ze, Zhang Li-wei, et al. Alignment algorithm based on quaternion estimator for SINS on rocking base[J]. Journal of Chinese Inertial Technology, 2014, 22(6): 724-727, 740.

[9] 王躍鋼, 楊家勝, 楊波. 緯度未知條件下捷聯慣導系統晃動基座的初始對準[J]. 航空學報, 2012, 33(12): 2322-2329. Wang Yue-gang, Yang Jia-sheng, Yang Bo. SINS initial alignment of swaying base under geographic latitude uncertainty[J]. Acta Aeronautica et Astronautica Sinica, 2012, 33(12): 2322-2329.

[10] Wu M, Wu Y, Hu X, et al. Optimization-based alignment for inertial navigation systems: Theory and algorithm[J]. Aerospace Science & Technology, 2011, 15(1): 1-17.

Initial alignment algorithm for SINS based on quaternion adaptive Kalman filter

XU Xiao-su1,2, ZHOU Feng1,2, ZHANG Tao1,2, XU Xiang1,2
(1. Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology, Ministry of Education, Southeast University, Nanjing 210096, China; 2. School of Instrument Science & Engineering, Southeast University, Nanjing 210096, China)

An anti-disturbance initial alignment algorithm with quaternion adaptive Kalman filter is presented for strap-down inertial navigation system (SINS). By transforming the problem of initial alignment into Wahba attitude determination, the measurement model with quaternion is built directly, and an adaptive Kalman filter is adopted to estimate the initial attitude quaternion. By using the updating of attitude quaternion, the body’s current attitude is obtained to reflect the attitude’s change in real time. In view that the direct-constructed measurement model has the problem of slow convergence, an improved quaternion adaptive Kalman filter is proposed, which is based on the principle of building K matrix with optimal quaternion method, which has. Experiments under different swing environment with three-axis turntable show that this algorithm has fast convergence speed, high stability and good precision. The mean headingerror of the medium-accuracy SINS is less than 2′ for alignment time from 150 s to 200 s.

initial alignment; quaternion; adaptive Kalman filter; attitude determination

U666.1

：A

2016-05-21；

：2016-08-03

國家自然科學基金項目（51175082，61473085，51375088）

徐曉蘇（1961—），男，博士生導師，從事測控技術與導航定位領域的研究。E-mail: xxs@seu.edu.cn

1005-6734(2016)04-0454-06

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