晃動基座行進間對準問題的QUEST算法

郭玉勝，付夢印，鄧志紅，莊廣琛，鄧 亮

（1. 北京理工大學自動化學院，北京 100081；2. 北京自動化控制設備研究所，北京 100074 ）

晃動基座行進間對準問題的QUEST算法

郭玉勝1,2，付夢印1，鄧志紅1，莊廣琛2，鄧 亮2

（1. 北京理工大學自動化學院，北京 100081；2. 北京自動化控制設備研究所，北京 100074 ）

針對慣導系統晃動基座行進間初始對準問題，提出了一種基于重力矢量的QUEST（Quaternion Estimator）姿態最優估計算法。在傳統基于重力矢量的初始對準方法基礎上，將方向余弦矩陣的求解過程轉換為Wahba問題，實現對重力矢量信息的充分利用，并通過QUEST算法實現了晃動基座下行進間粗對準的最優算法，改善了原有算法魯棒性以及實時性等方面的問題。仿真和湖試實驗驗證表明，更改后的算法能夠明顯提高慣導系統在動基座下的姿態估計速度和精度，特別在周期性線振動環境下的對準精度能夠提高一個數量級。多航次的湖試實驗結果表明，改進后的算法能夠有效提高航向對準精度，綜合導航速度精度能夠提高50%左右。

捷聯慣導系統；晃動基座；初始對準；重力矢量

慣導系統的粗對準問題一直是慣性導航領域的關鍵性問題，傳統解析式粗對準方法早已無法滿足動態環境下對準的需求[1-7]。文獻[8]通過計算重力矢量在慣性空間內旋轉的角度，擺脫了對信噪比極低的地球旋轉角速度的要求，解決了晃動基座下的對準問題，適用于水面系泊狀態下艦船的對準需求，但當載體存在線運動情況時，無法消除由于運動產生的加速度和角速度，從而會產生對準誤差。文獻[9]在此基礎上進一步考慮了載體線運動的影響，通過重新分解，提出了一種解決行進間粗對準的新方法。但在這些方法中，為了減小載體搖擺產生的加速度的干擾，使用了加速度積分來對重力積分進行平滑。由于求解方向余弦矩陣需要至少兩組重力矢量在不同坐標系下的觀測量，同時為了防止出現矩陣病態的問題，又需要這兩組矢量不能過于平行，因此這就要求兩組矢量積分的積分時間不能過于相近。權衡平滑效果和矢量夾角的要求，一般情況下會選擇取和時刻的兩組積分值（為對準時間），這種情況下，后時間內的計算過程對前時間內積分值沒有幫助，這樣便導致對重力信息的利用并不充分，結果也不是最優的。與此同時，積分計算使得整個tk時間內無法輸出姿態信息，這對于系統實際應用來說是非常不方便的。針對以上缺點，本文將矩陣的求解過程轉換為Wahba問題，實現對重力矢量信息的充分利用，并通過QUEST算法實現了晃動基座下行進間粗對準的最優算法。

1 晃動基座行進間對準算法

定義慣導系統載體坐標系，以航行器艏向為X正向，以垂直向上方向為Y正向，Z向定義滿足右手定則，記為b系。慣導系統在上電時刻載體坐標系為，上電后在慣性空間內保持不動，其他坐標系定義如圖1所示。

圖1 坐標系轉換關系示意圖Fig.1 Schematic of coordinate transformation

粗對準的目的是求取對準結束時刻慣導系統載體坐標系b和導航坐標系n之間的轉換關系。

初始對準姿態矩陣可分為兩個矩陣相乘的形式：

經轉換可得：

分別取當前時刻t以及時刻的積分值(t)、(t/2)、Vi0(t)和Vi0(t/2)構造正交矩陣，則有：

2 Wahba問題及QUEST算法

Wahba問題是Grace Wahba針對空間飛行器定姿問題提出的一種算法[10-11]，定義如下的最優估計：

其中，A為所要求取的方向余弦陣，和為同一個矢量在兩個坐標系下的測量值，αi為權重系數。

因此求L(A)最小值轉變為求g(A)的最大值。

但是由于A包含9個元素，并且元素之間互相約束，直接求取A比較困難。因此轉換思路，求取等效的四元數。四元數表示如下：

其中，為旋轉軸，θ為旋轉角度。

四元數的元素具有唯一約束：

姿態矩陣A與四元數之間的轉換關系為

將式(17)帶入式(14)中得：

為了求取式(18)在約束條件(16)下的最大值，構建如下方程：

由此可知，λ是K的一個特征根，則是對應的特征向量。因此，當λmax為K的最大特征向量時，對應的特征向量為最優的四元數估計值。

根據算法的實時性要求，需要避免直接求取特征根和特征向量，QUEST算法便是其中一種遞推算法。

將式(20)改寫為如下形式：

根據Wahba問題原理可知，應該有λmax>>1，因此將λ0=1帶入式(22)中，使用牛頓-拉夫遜法進行迭代：

將α、β和γ帶入式(22)和式(24)中，可以得到轉換后的特征多項式：

可得姿態四元數的最優解為

3 仿真和試驗驗證

為了驗證本文算法的效果，通過仿真和湖試試驗對對準結果進行驗證。

仿真條件為：參照第1節中載體系定義，以慣導系統z軸為中心軸搖擺，幅值10°，周期8 s；沿慣導系統x軸做線運動，0.1 m/s，周期5 s，系統對準過程曲線如圖2所示（為了便于比較，在計算過程中保留了改進前算法的全部數值以顯示對準過程中的姿態），姿態對準結果如表1所示。從對準結果可以看出，改進后的算法對姿態的估計要明顯優于改進前的算法。

圖2 仿真對準過程姿態曲線Fig 2 Attitude curve during alignment in simulation test

表1 仿真試驗姿態對準誤差Tab.1 Statistics of attitude error during alignment in simulation test

湖試試驗共進行了3個航次，所使用的慣導系統為某光纖捷聯慣導系統，精度指標為：陀螺漂移0.05 (°)/h，隨機游走0.001(°)/h1/2；加速度計零位100μg，隨機游走300μg/Hz1/2。其中一個航次的對準姿態曲線如圖3所示；對準后開始航行，速度誤差如圖4所示。3個航次速度誤差統計結果如表2所示。

航次1和航次3是在勻速直行情況下的對準結果，改進算法后系統的綜合速度精度提高 42.2%。航次 2為存在隨機加減速的情況，改進算法后系統的綜合速度精度提高了56.7%。

圖3 湖試對準過程姿態曲線Fig.3 Attitude curve during alignment in lake tes

圖4 湖試導航速度誤差曲線Fig.4 Velocity error curve during navigation in lake test

表2 湖試速度誤差統計表（1σ）Tab.2 Statistics of velocity errors during navigation in lake test ( 1σ)

4 結 論

從仿真和湖試試驗的結果可以看出，使用改進后的QUEST算法估計得到的姿態精度要明顯高于改進前的算法，這是由于改進后的算法使用了整個對準過程中的全部信息，得到了最優估計，也因此使得慣導系統對線運動過程中的線振動干擾具有更強的魯棒性，航向的收斂速度更快。雖然改進后的算法也是批處理算法，但是由于計算過程只需要計算各個變量的累加和，因此能夠得到與遞推算法相同的效果，能夠滿足實際系統對姿態的實時性需求。

（References）：

[1] Chang G. Robust Kalman filtering based on Mahalanobis distance as outlier judging criterion[J]. IEEE Transactions on Instrumentation and measurements, 2015, 64(3): 784-794.

[2] Liu Fei, Li Jie, Wang Haifu, et al. An improved quarternion Gauss-Newton algorithm for attitude determination using magnetometer and accelerometer[J]. Chinese Journal of Aeronautics, 2014, 27(4): 986-993.

[3] 王勇軍, 徐景碩, 盛飛, 等. 基于最優三軸姿態測定算法的艦載慣導粗對準方法[J]. 中國慣性技術學報, 2013, 21(3): 294-297. Wang Yong-jun, Xu Jing-shuo, Sheng Fei, et al. Coarse alignment method based on optimized three-axis attitude determination algorithm for shipboard SINS[J]. Journal of Chinese Inertial Technology, 2013, 21(3): 294-297.

[4] Wang B, Deng Z, Liu C, et al. Estimation of information sharing error by dynamic deformation between inertial navigation systems[J]. IEEE Transaction on Industrial Electronics, 2014, 61(4): 2015-2023.

[5] Chang L, Hu B, Li A, etal. Strapdown inertial navigation system alignment based on marginalised unscented Kalman filter[J]. IET Science, Measurement & Technology, 2013, 7(2): 128-138.

[6] Wu Xu, Sun Feng. Simulation study for FOG strapdowninertial navigation nonlinear alignment based on SVD-cubature Kalman filter[C]//Symposium on Photonics and Optoelectronics. Shanghai, 2012: 1-4.

[7] Hu Jie, Cheng Xiang-hong. A new in-motion initial alignment for land-vehicle SINS/OD integrated system [C]// 2014 Position, Location and Navigation Symposium. Monterey, CA, 2014: 407-412.

[8] 秦永元, 嚴恭敏, 顧冬晴, 等. 搖擺基座上基于信息的捷聯慣導粗對準研究[J]. 西北工業大學學報, 2005, 23(5): 681-684. Qin Yong-yuan, Yan Gong-min, Gu Dong-qing, et al. A clever way of SINS coarse alignment despite rocking ship[J]. Journal of Northwestern Poly technical University, 2005, 23(5): 681-684.

[9] 嚴恭敏, 秦永元, 衛育新, 等. 一種適用于 SINS動基座初始對準的新算法[J]. 系統工程與電子技術, 2009, 31(3): 635-637. Yan Gong-min, Qin Yong-yuan, Wei Yu-xin etal. New initial alignment algorithm for SINS on moving base[J]. Systems Engineering and Electronic, 2009, 31(3): 635-637.

[10] Wahba G. A least squares estimate of satellite attitude[J]. SIAM Review, 1965, 7(3): 409-409.

[11] Markley F L, Crassidis J L. Fundamentals of spacecraft attitude determination and control[M]. New York: Springer New York, 2014.

[12] 劉義亭, 徐曉蘇, 張濤, 等. 基于外參考速度輔助的行進間羅經法對準[J]. 中國慣性技術學報, 2015, 23(2): 165-171. Liu Yi-ting, Xu Xiao-su, Zhang Tao, et al. Compassing alignment in motion based on external reference velocity[J]. Journal of Chinese Inertial Technology, 2015, 23(2): 165-171.

Application of quaternion estimator algorithm dedicated on alignment of swaying and moving carrier

GUO Yu-sheng1,2, FU Meng-yin1, DENG Zhi-hong1, ZHUANG Guang-chen2, DENG Liang2
(1. School of Automation, Beijing Institute of Technology, Beijing 100081, China;
2. Beijing Automation Control Equipment institute, Beijing 100074, China)

An optimal attitude estimation method with Quaternion Estimator based on gravity vectors is developed to solve the initial alignment problem of the inertial navigation system in swaying and moving carrier. Based on the traditional initial alignment method with gravity vectors, the process of solving the direction cosine matrix is converted to Wahba problem, which can make full use of the gravity vector information and realize the optimal algorithm for the coarse alignment of the swaying basement by Quaternion Estimator algorithm. Simulation and lake experiments demonstrate that the modified method can significantly increase the precision and rapidity of the attitude estimation and the precision of the heading alignment on a swaying and moving carrier. The precision can be increased by one order of magnitude on the carrier with periodically linear movement, and the overall velocity precision can be increased by 50%.

strapdown inertial navigation system; swaying base; initial alignment; gravity vectors

U666.1

A

1005-6734(2017)02-0182-04

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

2017-01-12；

2017-03-28

海裝“十三五”預先研究課題（3020603030403）

郭玉勝 (1982—)，男，博士研究生，主要從事慣性系統及組合導航技術研究。E-mail: guoyusheng7209@139.com

聯 系 人：付夢印（1964—），男，教授，博士生導師。E-mail: fumy@bit.edu.cn