基于多項式迭代的等效旋轉(zhuǎn)矢量微分方程精確數(shù)值算法

嚴恭敏，李思錦，秦永元

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

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

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

1 算法理論分析

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

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

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

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

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

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

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

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

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

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

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

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

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

1.2 精度分析

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

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

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

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

由式(16)求解得：

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

1.3 計算量分析

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

2 仿真驗證

2.1 圓錐運動誤差仿真

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

圖1 圓錐運動算法誤差對比Fig.1 Comparison on algorithm errors of coning motion

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

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

2.2 迭代收斂性仿真

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

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

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

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

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

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

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

4 結(jié) 論

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

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