嚴(yán)恭敏，李思錦，秦永元

（西北工業(yè)大學(xué) 自動(dòng)化學(xué)院，西安 710072）

在捷聯(lián)慣導(dǎo)算法中，目前主流的姿態(tài)更新求解方法是先使用陀螺角增量（或角速率）的多子樣采樣計(jì)算等效旋轉(zhuǎn)矢量，補(bǔ)償轉(zhuǎn)動(dòng)不可交換誤差，再使用等效旋轉(zhuǎn)矢量計(jì)算姿態(tài)更新四元數(shù)[1-6]。但是傳統(tǒng)多子樣算法都是在等效旋轉(zhuǎn)矢量微分方程（Bortz方程）二階近似的基礎(chǔ)上進(jìn)行推導(dǎo)的，從源頭上看不可避免地存在原理性誤差，使得在大機(jī)動(dòng)環(huán)境下選用子樣數(shù)越多，精度往往越差[7]。

最近，數(shù)位研究者注意到了傳統(tǒng)多子樣算法存在的缺陷，提出了一些改進(jìn)的新算法。文獻(xiàn)[8-9]考慮了Bortz方程中三階甚至四階項(xiàng)的影響，提出一種改善算法精度的方法，增加了角增量的三階和四階叉乘項(xiàng)進(jìn)行不可交換誤差補(bǔ)償，但是該方法的推導(dǎo)過(guò)程比較繁瑣，難以擴(kuò)展到五階及以上叉乘運(yùn)算，原理上精度受限。文獻(xiàn)[10]提出基于四元數(shù)微分方程迭代的姿態(tài)更新算法，文獻(xiàn)[11]提出基于羅德里格參數(shù)微分方程迭代的姿態(tài)更新算法，兩者除了使用的數(shù)學(xué)描述工具不同外，其總體設(shè)計(jì)思路基本一致，都是在假設(shè)角速度為多項(xiàng)式的條件下，通過(guò)迭代計(jì)算能夠精確給出姿態(tài)更新的數(shù)值解。相比而言，前者在表示形式上更為簡(jiǎn)潔且計(jì)算量稍少一些。類似于上述兩種迭代算法，本文將利用Bortz微分方程給出基于多項(xiàng)式迭代的等效旋轉(zhuǎn)矢量精確數(shù)值算法，有效避免了原理性近似誤差。同時(shí)，本文對(duì)算法的精度和和收斂速度進(jìn)行了分析。研究結(jié)果表明，與四元數(shù)迭代方法相比，新算法具有同樣的數(shù)值計(jì)算精度，且計(jì)算量相當(dāng)，具有較好的應(yīng)用價(jià)值。

1 算法理論分析

1.1 迭代算法推導(dǎo)

等效旋轉(zhuǎn)矢量微分方程（Bortz方程）為[1]

其中，ω為動(dòng)坐標(biāo)系相對(duì)于參考坐標(biāo)系的轉(zhuǎn)動(dòng)角速度矢量，φ=φ為等效旋轉(zhuǎn)矢量φ的模值。將式(1)中的余切函數(shù)作泰勒級(jí)數(shù)展開(kāi)，經(jīng)整理后可得：

式(2)兩邊同時(shí)在時(shí)間區(qū)間[0,t]上積分，得：

將式(3)左邊φ(0)移到右邊，并改寫(xiě)成迭代的形式，有：

其中，φ(i)(i=0,1,2,…)表示第i次迭代的等效旋轉(zhuǎn)矢量，且有迭代初值φ(0)=0。不失一般性，可令初值φ(0)=0。

假設(shè)角速度ω為時(shí)間t的有限次（N-1次）多項(xiàng)式：

如果實(shí)際陀螺采集的為N子樣的角增量數(shù)據(jù)，則可先按文獻(xiàn)[10]的方法變換為上述角速度多項(xiàng)式，再使用本文方法。

再假設(shè)經(jīng)過(guò)第i次迭代后的等效旋轉(zhuǎn)矢量φ(i)多項(xiàng)式為

注意到式(4)中φ(i+1)為多項(xiàng)式的積分，因而φ(i+1)（或φ(i)）的多項(xiàng)式零次項(xiàng)系數(shù)必為 0，即有

將式(5)和式(6)代入式(4)右端的被積函數(shù)各項(xiàng)中作進(jìn)一步計(jì)算，有：

其中，運(yùn)算符“*”表示兩個(gè)多項(xiàng)式系數(shù)向量之間的卷積運(yùn)算。

再將式(5)(7)(8)(10)代入式(4)，可得：

1.2 精度分析

在實(shí)際利用計(jì)算機(jī)進(jìn)行求解時(shí)，需對(duì)式(4)中的余切函數(shù)展開(kāi)取有限階次。一般角速度的零次項(xiàng)多項(xiàng)式系數(shù)不為0，則當(dāng)?shù)螖?shù)時(shí)，多項(xiàng)式的不為 0的最低冪次為 1。如果存在不可交換誤差，的多項(xiàng)式表示系數(shù)中不為 0的最低冪次一般為2不為0的最低冪次為3不為0的最低冪次為()。因此，若忽略了式(4)右端被積函數(shù)中與模值有關(guān)的不小于的所有項(xiàng)，則將可能影響被積函數(shù)次冪的計(jì)算準(zhǔn)確性，再經(jīng)過(guò)積分之后等效旋轉(zhuǎn)矢量多項(xiàng)式的階精度會(huì)受到影響。換句話說(shuō)，若忽略及其以上項(xiàng)后，等效旋轉(zhuǎn)矢量的多項(xiàng)式解在理論上只具有階精度。

作為對(duì)比，傳統(tǒng)的基于等效旋轉(zhuǎn)矢量泰勒級(jí)數(shù)展開(kāi)法的多子樣算法是在如下近似公式(14)的基礎(chǔ)上作進(jìn)一步推導(dǎo)的。

在保留余切函數(shù)展開(kāi)無(wú)窮冪次的情況下，式(4)理論上是嚴(yán)格成立的，即對(duì)于均成立。但如果僅保留有限冪次，為了保證等式近似程度足夠高，必須使余切函數(shù)展開(kāi)的高冪次項(xiàng)迅速衰減，以至于在數(shù)值計(jì)算上誤差可以忽略不計(jì)。

式(15)可近似為（實(shí)際上是條件變嚴(yán)格了）：

由式(16)求解得：

這意味著，在更新周期[0,t]內(nèi)，如果等效旋轉(zhuǎn)矢量模值（可近似為角增量模值變化）小于 3.3°，則在計(jì)算時(shí)可忽略及更高冪次項(xiàng)的影響。

1.3 計(jì)算量分析

傳統(tǒng)機(jī)械設(shè)計(jì)作業(yè)中主要存在的問(wèn)題之一即為：設(shè)計(jì)成本較高。具體分析設(shè)計(jì)成本高主要體現(xiàn)為：設(shè)計(jì)產(chǎn)品通過(guò)生產(chǎn)線進(jìn)行生產(chǎn)制造，并基于實(shí)際生產(chǎn)的機(jī)械設(shè)備進(jìn)行性能，質(zhì)量等基礎(chǔ)參數(shù)的測(cè)試。實(shí)際測(cè)試中如出現(xiàn)問(wèn)題則需重新生產(chǎn)制造，從而造成了設(shè)計(jì)成本高的現(xiàn)象。分析自動(dòng)化技術(shù)在應(yīng)用中通過(guò)在線模擬測(cè)試的方式，有效的對(duì)機(jī)械裝置的力學(xué)性能，結(jié)構(gòu)安全性，進(jìn)行了完善的測(cè)試和評(píng)估。后期結(jié)合3D打印技術(shù)進(jìn)行相關(guān)設(shè)計(jì)成果的制造，有效的降低了生產(chǎn)線生產(chǎn)產(chǎn)生的高設(shè)計(jì)成本現(xiàn)象。

2 仿真驗(yàn)證

2.1 圓錐運(yùn)動(dòng)誤差仿真

圓錐運(yùn)動(dòng)和多項(xiàng)式角運(yùn)動(dòng)常常用來(lái)評(píng)價(jià)姿態(tài)更新算法的精度優(yōu)劣，下面也采用圓錐運(yùn)動(dòng)環(huán)境來(lái)對(duì)比本文算法與傳統(tǒng)優(yōu)化圓錐算法的姿態(tài)解算精度。

圖1 圓錐運(yùn)動(dòng)算法誤差對(duì)比Fig.1 Comparison on algorithm errors of coning motion

從圖1中可以看出，傳統(tǒng)算法在錐軸上的漂移誤差逐漸變大，在1 s時(shí)為0.1″，在非錐軸上的誤差呈周期性變化，最大達(dá)到了5″；而新算法的三軸誤差始終都很小，經(jīng)放大觀察僅為10-7″量級(jí)。這說(shuō)明，如果出現(xiàn)不規(guī)則非整周期的圓錐運(yùn)動(dòng)，傳統(tǒng)算法可能會(huì)引起較大的姿態(tài)解算誤差，而新算法能近乎完美的解算復(fù)現(xiàn)真實(shí)的姿態(tài)運(yùn)動(dòng)，新算法對(duì)于復(fù)雜的角運(yùn)動(dòng)具有非常高的求解精度。

進(jìn)一步仿真表明，本文算法與文獻(xiàn)[10]所提的四元數(shù)迭代算法具有同等數(shù)值精度，更多的仿真情況可參考文獻(xiàn)[10]，這里不再贅述。

2.2 迭代收斂性仿真

表1 各階多項(xiàng)式系數(shù)誤差收斂特性Tab.1 Convergence of polynomial coefficient errors

由表1可見(jiàn)：第1次迭代后，2階項(xiàng)系數(shù)誤差為0（收斂）；之后每增加1次迭代，收斂階數(shù)加1；經(jīng)過(guò)6次迭代后，7階項(xiàng)系數(shù)完全收斂。這表明，等效旋轉(zhuǎn)矢量迭代收斂的速度是很快的，從而決定了算法的計(jì)算量不是太大。

3 與羅德里格參數(shù)迭代法的對(duì)比討論

同Bortz方程(1)的結(jié)構(gòu)相似，羅德里格參數(shù)微分方程為[11]

前文1.1節(jié)分析的迭代方法也可應(yīng)用于式(21)，迭代公式為

采用式(22)迭代的優(yōu)點(diǎn)是不含三角函數(shù)運(yùn)算，計(jì)算量會(huì)比式(4)更少（約減少三分之一）。此外，在實(shí)際計(jì)算時(shí)式(4)需截取余切函數(shù)的有限項(xiàng)，使得存在如式(17)所示的角速度范圍限制，而式(22)在理論上對(duì)任意角速度都是迭代收斂的。

仿真還表明，迭代式(22)也具有如表1所列的收斂特性。當(dāng)然，與羅德里格參數(shù)表示相比，采用等效旋轉(zhuǎn)矢量的優(yōu)點(diǎn)是物理意義更直觀，方程(21)右端的第三項(xiàng)意義不夠直觀，而方程(1)右端的第二、三項(xiàng)均表示不可交換誤差，所以本文關(guān)于等效旋轉(zhuǎn)矢量迭代的研究結(jié)果容易直接與很多已有的傳統(tǒng)多子樣算法成果進(jìn)行對(duì)比。

4 結(jié) 論

傳統(tǒng)圓錐誤差補(bǔ)償算法在純圓錐運(yùn)動(dòng)環(huán)境下且半錐角比較小時(shí)是非常有效的，然而對(duì)于大錐角情況或者應(yīng)用于大角度機(jī)動(dòng)環(huán)境，都會(huì)產(chǎn)生較大的姿態(tài)漂移誤差。

為了避免這一問(wèn)題，本文根據(jù)Bortz方程構(gòu)造了多項(xiàng)式積分迭代公式，給出了一種新的直接求解等效旋轉(zhuǎn)矢量的數(shù)值算法，在算法推導(dǎo)過(guò)程中考慮了不可交換誤差高階項(xiàng)的影響，算法計(jì)算精度高，具有與四元數(shù)微分方程迭代解相同的數(shù)值精度[10]。通過(guò)與傳統(tǒng)的基于等效旋轉(zhuǎn)矢量的多子樣算法對(duì)比仿真，驗(yàn)證了新算法在大幅值圓錐運(yùn)動(dòng)條件下具有明顯的精度優(yōu)勢(shì)。新算法的計(jì)算量較小，具有較好的應(yīng)用價(jià)值。