嚴恭敏,李思錦,秦永元
(西北工業(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)用價值。
等效旋轉(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),可得:
在實際利用計算機進行求解時,需對式(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°,則在計算時可忽略及更高冪次項的影響。
傳統(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)象。
圓錐運動和多項式角運動常常用來評價姿態(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],這里不再贅述。
表1 各階多項式系數(shù)誤差收斂特性Tab.1 Convergence of polynomial coefficient errors
由表1可見:第1次迭代后,2階項系數(shù)誤差為0(收斂);之后每增加1次迭代,收斂階數(shù)加1;經(jīng)過6次迭代后,7階項系數(shù)完全收斂。這表明,等效旋轉(zhuǎn)矢量迭代收斂的速度是很快的,從而決定了算法的計算量不是太大。
同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)多子樣算法成果進行對比。
傳統(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)用價值。