晃動基座行進(jìn)間對準(zhǔn)問題的QUEST算法

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

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

晃動基座行進(jìn)間對準(zhǔn)問題的QUEST算法

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

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

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

捷聯(lián)慣導(dǎo)系統(tǒng)；晃動基座；初始對準(zhǔn)；重力矢量

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

1 晃動基座行進(jìn)間對準(zhǔn)算法

定義慣導(dǎo)系統(tǒng)載體坐標(biāo)系，以航行器艏向?yàn)閄正向，以垂直向上方向?yàn)閅正向，Z向定義滿足右手定則，記為b系。慣導(dǎo)系統(tǒng)在上電時刻載體坐標(biāo)系為，上電后在慣性空間內(nèi)保持不動，其他坐標(biāo)系定義如圖1所示。

圖1 坐標(biāo)系轉(zhuǎn)換關(guān)系示意圖Fig.1 Schematic of coordinate transformation

粗對準(zhǔn)的目的是求取對準(zhǔn)結(jié)束時刻慣導(dǎo)系統(tǒng)載體坐標(biāo)系b和導(dǎo)航坐標(biāo)系n之間的轉(zhuǎn)換關(guān)系。

初始對準(zhǔn)姿態(tài)矩陣可分為兩個矩陣相乘的形式：

經(jīng)轉(zhuǎn)換可得：

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

2 Wahba問題及QUEST算法

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

其中，A為所要求取的方向余弦陣，和為同一個矢量在兩個坐標(biāo)系下的測量值，αi為權(quán)重系數(shù)。

因此求L(A)最小值轉(zhuǎn)變?yōu)榍骻(A)的最大值。

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

其中，為旋轉(zhuǎn)軸，θ為旋轉(zhuǎn)角度。

四元數(shù)的元素具有唯一約束：

姿態(tài)矩陣A與四元數(shù)之間的轉(zhuǎn)換關(guān)系為

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

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

由此可知，λ是K的一個特征根，則是對應(yīng)的特征向量。因此，當(dāng)λmax為K的最大特征向量時，對應(yīng)的特征向量為最優(yōu)的四元數(shù)估計(jì)值。

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

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

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

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

可得姿態(tài)四元數(shù)的最優(yōu)解為

3 仿真和試驗(yàn)驗(yàn)證

為了驗(yàn)證本文算法的效果，通過仿真和湖試試驗(yàn)對對準(zhǔn)結(jié)果進(jìn)行驗(yàn)證。

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

圖2 仿真對準(zhǔn)過程姿態(tài)曲線Fig 2 Attitude curve during alignment in simulation test

表1 仿真試驗(yàn)姿態(tài)對準(zhǔn)誤差Tab.1 Statistics of attitude error during alignment in simulation test

湖試試驗(yàn)共進(jìn)行了3個航次，所使用的慣導(dǎo)系統(tǒng)為某光纖捷聯(lián)慣導(dǎo)系統(tǒng)，精度指標(biāo)為：陀螺漂移0.05 (°)/h，隨機(jī)游走0.001(°)/h1/2；加速度計(jì)零位100μg，隨機(jī)游走300μg/Hz1/2。其中一個航次的對準(zhǔn)姿態(tài)曲線如圖3所示；對準(zhǔn)后開始航行，速度誤差如圖4所示。3個航次速度誤差統(tǒng)計(jì)結(jié)果如表2所示。

航次1和航次3是在勻速直行情況下的對準(zhǔn)結(jié)果，改進(jìn)算法后系統(tǒng)的綜合速度精度提高 42.2%。航次 2為存在隨機(jī)加減速的情況，改進(jìn)算法后系統(tǒng)的綜合速度精度提高了56.7%。

圖3 湖試對準(zhǔn)過程姿態(tài)曲線Fig.3 Attitude curve during alignment in lake tes

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

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

4 結(jié) 論

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

（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] 王勇軍, 徐景碩, 盛飛, 等. 基于最優(yōu)三軸姿態(tài)測定算法的艦載慣導(dǎo)粗對準(zhǔn)方法[J]. 中國慣性技術(shù)學(xué)報(bào), 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] 秦永元, 嚴(yán)恭敏, 顧冬晴, 等. 搖擺基座上基于信息的捷聯(lián)慣導(dǎo)粗對準(zhǔn)研究[J]. 西北工業(yè)大學(xué)學(xué)報(bào), 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] 嚴(yán)恭敏, 秦永元, 衛(wèi)育新, 等. 一種適用于 SINS動基座初始對準(zhǔn)的新算法[J]. 系統(tǒng)工程與電子技術(shù), 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ìn)間羅經(jīng)法對準(zhǔn)[J]. 中國慣性技術(shù)學(xué)報(bào), 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

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

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

聯(lián) 系 人：付夢印（1964—），男，教授，博士生導(dǎo)師。E-mail: fumy@bit.edu.cn