動態(tài)傾斜校準(zhǔn)傳感器的姿態(tài)角補(bǔ)償優(yōu)化算法

高 進(jìn)，周大錚

(西北機(jī)電工程研究所 7室，陜西咸陽 712099)

在本測量系統(tǒng)中，有3個傾斜角敏感器，2個角速率敏感器分別固聯(lián)于載體相應(yīng)的坐標(biāo)軸上，直接測量載體角運動，它要求較高的測量帶寬。傾斜角敏感器輸出載體姿態(tài)角靜態(tài)初值，角速率敏感器提供積分時間內(nèi)載體的角增量的累加值［1］。剛體的有限轉(zhuǎn)動不是矢量，其轉(zhuǎn)動次序不可交換。無限小轉(zhuǎn)動是矢量。Bortz的轉(zhuǎn)動向量［2］(Orientation Vector)微分方程為計算捷聯(lián)系統(tǒng)的姿態(tài)矩陣建立了全面的理論基礎(chǔ)［3］。轉(zhuǎn)動向量的變化速率是慣性測量的角速率向量與計算得到的非互易速率向量二者之和。后者是影響捷聯(lián)慣性系統(tǒng)姿態(tài)角精度的一個重要因素［4］。因此，在高角速率動態(tài)環(huán)境中，為防止姿態(tài)誤差積累，必須對非互易速率向量進(jìn)行補(bǔ)償。對非互易速率向量進(jìn)行補(bǔ)償?shù)挠嬎阋话惴Q為圓錐補(bǔ)償算法。要提高系統(tǒng)精度，可以有兩種選擇，一種是對角速率敏感器輸出信號進(jìn)行高速采樣，使用簡單的圓錐補(bǔ)償算法;另一種是適當(dāng)降低對角速率敏感器輸出信號采樣的速率，使用復(fù)雜但精確的圓錐補(bǔ)償算法。

設(shè)計出補(bǔ)償精度高、計算量少的圓錐補(bǔ)償算法一直是人們不斷追求得目標(biāo)。Miller提出了在圓錐運動條件下優(yōu)化的三子樣算法［5］，Jang G.Lee等提出了四子樣算法［6］，這兩種算法沒有利用前一個圓錐補(bǔ)償周期的角速率敏感器輸出信號，而且計算量較大。Yeon Fuh Jiang等提出了利用前一補(bǔ)償周期的一類算法，包含了圓錐補(bǔ)償周期內(nèi)角速率敏感器輸出信號的所有可能的叉乘項，并利用前一補(bǔ)償周期的角速率敏感器輸出累加角增量［7－9］。Musoff提出了圓錐算法的優(yōu)化指標(biāo)，分析了圓錐補(bǔ)償后的算法誤差與補(bǔ)償周期的冪次r的關(guān)系［8］。本文提出的算法利用在經(jīng)典圓錐運動下，相同時間間隔的角增量向量的叉乘對圓錐誤差的貢獻(xiàn)相等的特點，推導(dǎo)出一類計算量較少的圓錐補(bǔ)償算法。同時利用前一圓錐補(bǔ)償周期的角速率敏感器輸出信號，提高了補(bǔ)償精度，縮短了運算時間。

1 補(bǔ)償算法

載體轉(zhuǎn)動向量的微分方程為［3］

式中，Φ是轉(zhuǎn)動向量;ω是機(jī)體角速率向量。方程右邊第2項與第3項之和是非互易誤差。對于小Φ，為簡化計算，忽略右邊第3項，并用式(2)代替第2項中的Φ。

這樣，式(1)可簡化為

β就是需要進(jìn)行補(bǔ)償?shù)姆腔ヒ紫蛄喀?。隨著圓錐補(bǔ)償周期內(nèi)的采樣次數(shù)不同，β可以有多種方式進(jìn)行計算。

對于經(jīng)典圓錐運動，它的轉(zhuǎn)動向量為［7－9］

式中，Ω是圓錐運動角頻率;a是圓錐運動的幅度。采用上標(biāo)B表示機(jī)體坐標(biāo)系，得到

假設(shè)在t～t+h間隔內(nèi)，對角速率敏感器輸出信號進(jìn)行N次采樣，θi表示第i次采樣的角速率敏感器輸出的角增量信號。

則在t～t+h間隔內(nèi)，轉(zhuǎn)動向量的估計值的計算公式為

根據(jù)式(4)，圓錐補(bǔ)償可計算如下

右邊共有N(N－1)/2項。由式(9)，得

由式(12)可以看出，在θi×θj中，x分量只與相對時間(i－j)h有關(guān)，而與絕對時間無關(guān)，y和z分量是絕對時間t的余弦函數(shù)。在圓錐運動下，能引起漂移的誤差出現(xiàn)在x軸上。因此，具有相同時間間隔的兩個角增量向量的叉乘對圓錐誤差的貢獻(xiàn)相等，而可以不考慮其與絕對時間的關(guān)系。利用這個性質(zhì)，可以將圓錐誤差補(bǔ)償公式簡化為［7］

比較式(13)和式(11)可以看出，簡化后的算法，計算量大大減小。簡化帶來的缺陷是在一般機(jī)動條件下，圓錐誤差補(bǔ)償算法的誤差會增大。但即使在非常大的機(jī)動條件下，算法誤差也可以忽略，而且這種誤差只是在機(jī)動期間產(chǎn)生。因此，對圓錐補(bǔ)償算法的簡化是合理的。

如果考慮到可以利用前一圓錐補(bǔ)償周期的角增量輸出，則可以得到以下算式

式中，θ'是前一圓錐補(bǔ)償周期的累加值。對應(yīng)采樣為2～5等情況，可得到簡化算法式，如(14)和式(15)的各種形式。

2 算法應(yīng)用情況

為驗證算法的有效性，把上述算法轉(zhuǎn)化成動態(tài)傾斜校準(zhǔn)傳感器內(nèi)的計算機(jī)程序，把動態(tài)傾斜校準(zhǔn)傳感器放在雙軸搖擺臺上對比輸出曲線。圖1中虛線是搖擺臺輸出曲線，實線是傾斜校準(zhǔn)傳感器輸出曲線，可見在擺頻2.5 Hz條件下，傾斜校準(zhǔn)傳感器輸出的跟蹤曲線已經(jīng)比較完美，但仍有少量數(shù)據(jù)出現(xiàn)較大誤差，這是因為搖擺臺由曲柄連桿驅(qū)動，在其轉(zhuǎn)動拐點處曲線可看到分明的棱角，在該處角速度和角加速度發(fā)生反向突變，高頻分量豐富，遠(yuǎn)超出傾斜校準(zhǔn)傳感器的帶寬范圍。即便在苛刻的工作環(huán)境下，傾斜校準(zhǔn)傳感器輸出的跟蹤曲線仍能準(zhǔn)確地描述轉(zhuǎn)臺曲線的棱角形狀。

圖1 輸出曲線

3 結(jié)束語

選定的載體圓錐運動條件下的應(yīng)用結(jié)果表明:(1)這類算法比相同精度其他算法的計算量可減少1/3。(2)所有算法的精度都隨著對敏感器輸出信號的采樣頻率提高而提高。(3)文中提出的采樣算法漂移小，表明在對敏感器輸出信號采樣頻率一定時，一個圓錐補(bǔ)償周期內(nèi)的采樣數(shù)不是越多越好。(4)圓錐補(bǔ)償周期的采樣數(shù)≥4時，利用前一補(bǔ)償周期的采樣，對增加精度效果不明顯。圓錐補(bǔ)償周期的采樣數(shù)＜4時，分別利用前一圓錐補(bǔ)償周期敏感器輸出信號的各次采樣，而不是利用前一補(bǔ)償周期各次采樣輸出的累加量，補(bǔ)償精度將更高。(5)圓錐補(bǔ)償周期的采樣數(shù)＜4時，在圓錐補(bǔ)償算法中，利用前一圓錐補(bǔ)償周期對敏感器輸出信號，相當(dāng)于提高了對敏感器輸出信號的采樣頻率。(6)使用內(nèi)置DSP計算機(jī)，處理速度快，實時性好。

［1］陳哲.捷聯(lián)慣性導(dǎo)航系統(tǒng)原理［M］.北京:宇航出版社，1986.

［2］BORTZ J E.A new mathematical formulation for strapdown inertial navigation［J］.IEEE Transacfions on Aerospace and Electronic Systems，1971，7(1):61 －66.

［3］IGNAGNI M B.On the orientation vector differential equation in strapdown inertial systems［J］.IEEE Transactions on Aerospace and Electronic Systerms，1994，30(4):1076 －1081.

［4］SAVAGE P G.Strapdown inertial navigation integration algorithm design，Part l:Attitude algorithm［J］.Journal of Guidance，Control，and Dynamics，1998，21(1):19 －28.

［5］MILLE R B.A new strapdown attitude algorithm［J］.Journal of Guidance，Control，and Dynamics，1983，6(4):287 －291.

［6］LEE J G，YOON Y J，MARK J G.Extension of strapdown attitude algorithm for highfrequency base motion［J］.Journal of Guidance，Control，and Dynamics，1990，13(4):738 －743.

［7］JIANG Y F，LIN Y P.Improved strapdown coning algorithm［J］.IEEE Transactions on Aerospace and Electronic Systems，1992，28(2):484 －490.

［8］MUSOFF H，MURPHY J H.Study of strapdown navigation attitude algorithms ［J］.Journal of Guidance，Control，and Dynamics，1995，18(2):287 －290.

［9］JIANG Y F，LIN Y P.On the rotation vector differential equation［J］.IEEE Transactions on Aerospace and Electronic Systems，1991，27(1):181－183.