王 君，宋蔚陽，吳驍航

一種基于角速率的改進(jìn)旋轉(zhuǎn)矢量姿態(tài)更新算法

王 君，宋蔚陽，吳驍航

（北京航天長征飛行器研究所，北京，100076）

為了提高姿態(tài)更新精度，提出了一種利用前周期角速率進(jìn)行優(yōu)化的改進(jìn)旋轉(zhuǎn)矢量姿態(tài)更新算法。通過對基于角速率的旋轉(zhuǎn)矢量更新算法及其算法誤差的推導(dǎo)，針對單子樣、雙子樣、改進(jìn)雙子樣算法在圓錐運(yùn)動(dòng)條件下情況進(jìn)行了仿真，并開展跑車試驗(yàn)。仿真結(jié)果表明，在高動(dòng)態(tài)的圓錐運(yùn)動(dòng)條件下，利用前周期角速率的優(yōu)化雙子樣算法，比傳統(tǒng)的姿態(tài)更新算法具有更高的精度；跑車試驗(yàn)結(jié)果驗(yàn)證了算法的正確性和有效性。

姿態(tài)更新算法；誤差補(bǔ)償；旋轉(zhuǎn)矢量；優(yōu)化算法

0 引 言

在廣泛使用的捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown Inertial Navigation System，SINS）中，慣性測量器件與載體固連，當(dāng)使用條件是較高動(dòng)態(tài)的環(huán)境時(shí)， SINS對導(dǎo)航算法精度的要求較高。在捷聯(lián)慣性導(dǎo)航系統(tǒng)的姿態(tài)、速度和位置更新算法中，姿態(tài)算法對整個(gè)系統(tǒng)精度的影響最大，是算法研究和設(shè)計(jì)的核心[1]。自捷聯(lián)慣性導(dǎo)航系統(tǒng)的概念提出以來，經(jīng)過三十多年的研究，對捷聯(lián)導(dǎo)航姿態(tài)更新算法己經(jīng)形成了以四元數(shù)導(dǎo)航算法、多子樣旋轉(zhuǎn)矢量圓錐補(bǔ)償算法、劃船補(bǔ)償算法為代表的捷聯(lián)慣性導(dǎo)航姿態(tài)更新算法。

國內(nèi)外許多學(xué)者提出了不同的旋轉(zhuǎn)矢量算法[2~5], 都是基于陀螺的角增量信息。然而對于目前的常用慣性器件輸出的角速率信息，無法直接使用。因此本文針對以光纖陀螺為代表的慣性器件，使用角速率信息推導(dǎo)出一種改進(jìn)旋轉(zhuǎn)矢量的表達(dá)式，經(jīng)過仿真研究和跑車試驗(yàn)，結(jié)果表明此算法具有更高的精度和應(yīng)用價(jià)值。

1 姿態(tài)更新算法

常用的經(jīng)典姿態(tài)更新算法有四元數(shù)法、旋轉(zhuǎn)矢量法等。

1.1 姿態(tài)更新的四元數(shù)方法

傳統(tǒng)四元數(shù)方法中，首先構(gòu)建載體四元數(shù)微分方程，再選用不同的解法來求解四元數(shù)微分方程。

經(jīng)典的四元數(shù)微分方程表示形式如下：

將上述四元數(shù)微分方程寫成矩陣形式為

其中，

通常采用畢卡算法由角增量計(jì)算變換四元數(shù)；通過三角函數(shù)的級數(shù)展開為有限項(xiàng)計(jì)算，包括一階、二階、三階等近似算法。需注意到，姿態(tài)更新的四元數(shù)算法的假設(shè)條件時(shí)載體系在姿態(tài)更新的時(shí)間段內(nèi)為定軸轉(zhuǎn)動(dòng)時(shí)才嚴(yán)格成立。

1.2 姿態(tài)更新的旋轉(zhuǎn)矢量法

當(dāng)考慮載體在更新周期內(nèi)不是定軸轉(zhuǎn)動(dòng)，由角增量直接求解四元數(shù)，容易引起轉(zhuǎn)動(dòng)不可交換誤差。因此研究者們提出了旋轉(zhuǎn)矢量法，即通過角增量求解等效旋轉(zhuǎn)矢量、再使用等效旋轉(zhuǎn)矢量更新姿態(tài)的方法。

使用角增量構(gòu)建旋轉(zhuǎn)矢量的方法，以姿態(tài)更新周期內(nèi)使用的角增量數(shù)量區(qū)分，可以分為單子樣法、雙子樣法、三子樣法等。

觀察式（4）、式（6）中姿態(tài)變換四元數(shù)的形式可以發(fā)現(xiàn)，構(gòu)造姿態(tài)變換四元數(shù)時(shí)兩者的區(qū)別是前者使用角增量，后者使用旋轉(zhuǎn)矢量，構(gòu)造方法一致。

在姿態(tài)更新周期內(nèi)，分別使用常量、一次項(xiàng)、二次項(xiàng)擬合角速度，可得使用不同數(shù)量角增量構(gòu)建旋轉(zhuǎn)矢量的方法[6,7]，單子樣算法、雙子樣算法、三子樣算法分別為

單子樣旋轉(zhuǎn)矢量算法與四元數(shù)法等價(jià)。

2 基于角速率的改進(jìn)旋轉(zhuǎn)矢量姿態(tài)更新算法

2.1 改進(jìn)旋轉(zhuǎn)矢量姿態(tài)更新算法

基于角速率的改進(jìn)旋轉(zhuǎn)矢量姿態(tài)更新算法，在考慮到圓錐運(yùn)動(dòng)會(huì)誘發(fā)捷聯(lián)慣導(dǎo)姿態(tài)更新產(chǎn)生嚴(yán)重漂移，使用該運(yùn)動(dòng)環(huán)境對姿態(tài)更新算法進(jìn)行優(yōu)化?？紤]到常用的慣性器件輸出為角速度，使用角速度構(gòu)建旋轉(zhuǎn)矢量。

推導(dǎo)在圓錐運(yùn)動(dòng)環(huán)境下的旋轉(zhuǎn)矢量優(yōu)化方式。假設(shè)運(yùn)載體存在圓錐運(yùn)動(dòng)，描述該運(yùn)動(dòng)的四元數(shù)如下：

姿態(tài)更新的四元數(shù)更新方程即式（4）可轉(zhuǎn)換成：

上述表達(dá)式為無誤差的旋轉(zhuǎn)矢量增量。

使用角增量代替旋轉(zhuǎn)矢量進(jìn)行姿態(tài)更新時(shí)的誤差可表示為

2.2 基于角速率輸出的姿態(tài)更新算法

考慮到角增量為使用角速度假設(shè)條件下計(jì)算的，當(dāng)陀螺輸出的信息為角速率時(shí)，利用角速率信息經(jīng)過插值積分可以提取角增量信息。

3 試驗(yàn)仿真

3.1 圓錐運(yùn)動(dòng)仿真

雙子樣算法與改進(jìn)型算法的姿態(tài)更新誤差如圖2所示。多次仿真可觀察到優(yōu)化雙子樣迭代算法的誤差比雙子樣算法減小一倍。

圖2 雙子樣與改進(jìn)型算法的姿態(tài)更新誤差曲線

3.2 跑車試驗(yàn)

為驗(yàn)證實(shí)際使用的動(dòng)態(tài)環(huán)境下算法的可靠性和有效性，設(shè)計(jì)跑車試驗(yàn)對算法進(jìn)行驗(yàn)證。試驗(yàn)車上搭載有高精度慣導(dǎo)系統(tǒng)（航向精度0.1°，1σ；水平姿態(tài)精度0.05°，1σ）；高精度慣導(dǎo)系統(tǒng)內(nèi)部包含衛(wèi)星導(dǎo)航接收機(jī)，可滿足對中低精度光纖慣組算法精度的驗(yàn)證。高精度慣導(dǎo)的數(shù)據(jù)更新頻率為200 Hz，中低精度光纖慣組的數(shù)據(jù)更新頻率為200 Hz（姿態(tài)更新頻率為100 Hz），對高精度慣導(dǎo)的輸出結(jié)果進(jìn)行擬合后，與試驗(yàn)設(shè)備進(jìn)行比較。圖3為車載試驗(yàn)高精度慣導(dǎo)（左側(cè)）與中低等精度光纖慣組（零偏1.5（°）/h）的安裝圖示。

圖3 車載試驗(yàn)

圖4為航向角試驗(yàn)結(jié)果。從圖4可見，在約3000 s行車過程后，基于角速率改進(jìn)旋轉(zhuǎn)矢量姿態(tài)更新算法航向角的偏差約0.06°；單子樣算法航向角在0.07°左右，精度提高10%左右。進(jìn)行了3次不同路線跑車試驗(yàn)，使用同樣的離線分析方法對姿態(tài)結(jié)果進(jìn)行分析，將基于角速率的改進(jìn)旋轉(zhuǎn)矢量姿態(tài)更新算法、單子樣算法計(jì)算的姿態(tài)結(jié)果分別與高精度慣導(dǎo)的輸出進(jìn)行對比，計(jì)算姿態(tài)誤差，誤差結(jié)果與第1次試驗(yàn)接近，航向角精度有10%左右的提升，俯仰角和滾動(dòng)角精度無明顯區(qū)別，統(tǒng)計(jì)分析結(jié)果見表1。由于跑車試驗(yàn)只能提供一定程度的動(dòng)態(tài)，在本次試驗(yàn)中基于角速率的改進(jìn)旋轉(zhuǎn)矢量算法與雙子樣算法的精度基本相當(dāng)。

圖4 航向姿態(tài)角解算對比

表1 跑車試驗(yàn)結(jié)果

Tab.1 Result of Vehicle Tests

試驗(yàn)統(tǒng)計(jì)值類型俯仰角偏差/（°）航向角偏差/（°）滾動(dòng)角偏差/（°） 試驗(yàn)一（3023~3123s）單子樣-0.6060.0710.019 改進(jìn)雙子樣-0.6050.0610.018 試驗(yàn)二（2219~2319s）單子樣-0.3850.0450.011 改進(jìn)雙子樣-0.3830.0400.010 試驗(yàn)三（2536~2636s）單子樣-0.5980.0530.012 改進(jìn)雙子樣-0.5960.0460.011

4 結(jié) 論

本文提出了一種基于角速率的改進(jìn)旋轉(zhuǎn)矢量姿態(tài)更新算法。在圓錐運(yùn)動(dòng)環(huán)境下對雙子樣旋轉(zhuǎn)矢量系數(shù)進(jìn)行補(bǔ)償，同時(shí)直接使用當(dāng)前姿態(tài)更新周期和前一時(shí)刻慣組數(shù)據(jù)更新周期的三組角速度信息建立旋轉(zhuǎn)矢量計(jì)算方法。通過數(shù)學(xué)仿真試驗(yàn)和跑車試驗(yàn)進(jìn)行驗(yàn)證，試驗(yàn)結(jié)果表明在高動(dòng)態(tài)的圓錐運(yùn)動(dòng)條件下，利用前周期角速率的優(yōu)化雙子樣算法，比傳統(tǒng)的雙子樣姿態(tài)更新算法具有更高的精度。結(jié)合跑車試驗(yàn)，驗(yàn)證了算法的正確性和有效性，同時(shí)算法具有較高的應(yīng)用價(jià)值。

[1] 嚴(yán)恭敏. 捷聯(lián)慣導(dǎo)算法與組合導(dǎo)航原理[M]. 西安: 西北工業(yè)大學(xué)出版社, 2019.

Yan Gongmin. Stapdown inertial navigation algorithm andintegrated navigation principle[M]. Xi’an: Northwestern polytechnical University Press, 2019.

[2] Jiang Y F, Lin Y P. Improved st rap-down coning algorithms[J]. IEEE Transaction on Arospace and Electronic System, 1992, 28(2): 484 -490.

[3] Yeon F J, Yu P L. Improved strapdown coning algorithms[J]. IEEE Transations on Aerospace and Electronic Systems, 1992, AES-28(2): 738-743.

[4] Savage P G. Coning algorithm design by explicit frequency shaping[J]. Journal of Guidance, Control, and Dynamics, 2010, 33(4): 1123-1132.

[5] 王立東. 基于角增量和角速率的旋轉(zhuǎn)矢量算法的等效性[J]. 宇航學(xué)報(bào), 2014, 35(3): 340-344.

Wang Lidong. Equivalence analysis of rotation vector algorithms based on angle increment and angular velocith[J]. Journal of Astronautics, 2014, 35(3): 340-344.

[6] 顧冬晴. 捷聯(lián)慣導(dǎo)姿態(tài)更新的雙子樣二次迭代優(yōu)化算法研究[J]. 中國慣性技術(shù)學(xué)報(bào), 2002(3): 5-8.

Gu Dongqing. Optimal two-sample iteration algorithm for SINS attitude updating[J]. Journal of Chinese Inertial Technology, 2002(3): 5-8.

[7] 張昀申. 基于改進(jìn)等效旋轉(zhuǎn)矢量的姿態(tài)更新算法研究[J]. 艦船電子工程, 2017(9): 30-32.

Zhang Yunshen. Strapdown algorithm research based on improved equivalent rotation vector[J]. Ship Electronic Engineering, 2017(9): 30-32.

[8] 嚴(yán)恭敏. 捷聯(lián)慣導(dǎo)中求解圓錐誤差系數(shù)的通用算法[J]. 導(dǎo)航定位學(xué)報(bào), 2017, 5(3): 1-4.

Yan Gongmin. A general numerical method to obtaining optimized coning compensation coefficients for strapdown attitude algorithm[J]. Journal of Navigation and Positioning, 2017, 5(3): 1-4.

An Improved Attitude Algorithm of Rotation Vector Based onAngular Velocity

Wang Jun, Song Wei-yang, Wu Xiao-hang

(Beijing Institute of Space Long March Vehicle , Beijing, 100076)

To improve the accuracy of attitude updating, the optimal rotation vector algorithms are proposed based on pre-angular. The rotation vector algorithms using pre-angular and calculation errors are deduced. The simulation analyses of single-sample, two-sample, optimal two-sample are made. The simulation results indicate that the performance of the optimal algorithm using pre-angular increment is better than traditional algorithm. The vehicle test verified the correctness and effectiveness of the algorithm.

strapdown attitude algorithm; error correction; rotation vector; optimal algorithm

1004-7182(2020)03-0038-05

10.7654/j.issn.1004-7182.20200308

O242 .1

A

王 君（1985-），女，工程師，主要研究方向?yàn)榻萋?lián)慣性導(dǎo)航技術(shù)、組合導(dǎo)航系統(tǒng)。

宋蔚陽（1983-），男，高級工程師，主要研究方向?yàn)轱w行器導(dǎo)航、制導(dǎo)與控制。

吳驍航（1990-），男，博士，工程師，主要研究方向?yàn)閷?dǎo)航系統(tǒng)設(shè)計(jì)與仿真、組合導(dǎo)航算法設(shè)計(jì)。

2020-04-16；

2020-05-08