高 進(jìn)，周大錚

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

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

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

1 補(bǔ)償算法

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

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

這樣，式(1)可簡(jiǎn)化為

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

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

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

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

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

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

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

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

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

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

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

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

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

圖1 輸出曲線(xiàn)

3 結(jié)束語(yǔ)

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

［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.