慣性測(cè)量系統(tǒng)火箭橇試驗(yàn)一維運(yùn)動(dòng)約束方法

魏宗康，高榮榮

（北京航天控制儀器研究所，北京，100854）

0 引 言

火箭橇是利用火箭發(fā)動(dòng)機(jī)作動(dòng)力在特定軌道上模擬載體實(shí)際飛行的大過載、高強(qiáng)度、強(qiáng)振動(dòng)和沖擊等力學(xué)環(huán)境的設(shè)備。慣性測(cè)量系統(tǒng)火箭橇試驗(yàn)通過采用精確的時(shí)間、速度和位置等測(cè)試手段，以驗(yàn)證慣性測(cè)量系統(tǒng)的精度，或分離大過載下的慣性器件高階誤差[1,2]。

在利用火箭橇試驗(yàn)對(duì)慣性測(cè)量系統(tǒng)進(jìn)行性能和精度評(píng)估時(shí)，主要是根據(jù)火箭橇在直線軌道上的單方向一維運(yùn)動(dòng)特點(diǎn)。文獻(xiàn)[1]給出了一種過載曲線設(shè)計(jì)，給出了在軌道長(zhǎng)度限制條件下在主動(dòng)段、自由滑行段和制動(dòng)段的時(shí)變過載值。文獻(xiàn)[2]給出了火箭橇試驗(yàn)時(shí)的振動(dòng)傳遞特性。

在分離慣性測(cè)量系統(tǒng)的誤差時(shí)，主要采用導(dǎo)航位置與外測(cè)位置進(jìn)行比較[3,4]。但由于導(dǎo)航位置是三維的，而外測(cè)位置只是一維的，這就面臨著如何把三維導(dǎo)航位置折算到一維軌道運(yùn)動(dòng)方向上的問題。文獻(xiàn)[5]中給出了一種分離慣性測(cè)量系統(tǒng)誤差系數(shù)的火箭橇試驗(yàn)方法，給出了對(duì)導(dǎo)航位置一維化的近似處理，主要是只考慮運(yùn)動(dòng)方向X 軸加速度計(jì)，而忽略了其它兩軸（側(cè)向Y 軸和高度方向Z 軸）的誤差項(xiàng)，但是這種處理存在著很大的方法誤差。如果Y 軸和Z 軸的導(dǎo)航位置誤差為有用信息時(shí)，則只利用X 軸位置誤差序列值分離的加速度計(jì)誤差系數(shù)與真實(shí)值存在著較大的偏差，甚至導(dǎo)致不正確的結(jié)論[6]。

事實(shí)上，火箭橇只在其運(yùn)行方向上存在位置和速度誤差，即在與運(yùn)行方向垂直的另外兩個(gè)方向上的速度和位置均為零。但是，在實(shí)際火箭橇試驗(yàn)的導(dǎo)航解算中其它兩個(gè)方向存在著非零誤差，分析原因主要是由于慣性測(cè)量系統(tǒng)在橇體上的安裝誤差、導(dǎo)航初始對(duì)準(zhǔn)誤差和姿態(tài)誤差等因素造成的。為解決上述問題，本文提出了一種慣性測(cè)量系統(tǒng)火箭橇試驗(yàn)一維運(yùn)動(dòng)約束方法。利用開環(huán)卡爾曼濾波器來開環(huán)估計(jì)橇體的姿態(tài)誤差角，并補(bǔ)償其對(duì)橇體運(yùn)行方向上的影響，最終將火箭橇的三維運(yùn)動(dòng)簡(jiǎn)化為精確的一維運(yùn)動(dòng)。只有在一維運(yùn)動(dòng)的情況下才能分離出慣性測(cè)量系統(tǒng)的誤差系數(shù)。

1 火箭橇試驗(yàn)橇體運(yùn)動(dòng)一維化必要性分析

慣性測(cè)量系統(tǒng)火箭橇試驗(yàn)過程中，火箭橇的運(yùn)動(dòng)為標(biāo)準(zhǔn)的一維直線運(yùn)動(dòng)，即橇體在滑軌的側(cè)向和高度方向上的速度和位移均為零。而在實(shí)際導(dǎo)航解算的結(jié)果中，由于初始對(duì)準(zhǔn)誤差、儀表誤差和其他外界因素的干擾，火箭橇的運(yùn)動(dòng)軌跡并非理想的一維直線，而屬于三維軌跡[7]。

慣性測(cè)量系統(tǒng)基于發(fā)射點(diǎn)軌道坐標(biāo)系的一次導(dǎo)航結(jié)果如圖1 所示。

圖1 慣性測(cè)量系統(tǒng)火箭橇試驗(yàn)導(dǎo)航結(jié)果Fig.1 Navigation Result of Rocket Sled Testing of Inertial Testing System

慣性測(cè)量系統(tǒng)火箭橇試驗(yàn)過程中，Y 軸和Z 軸的位移應(yīng)為0，但導(dǎo)航解算后的位移和速度都不為0，位移最大可達(dá)10 m。經(jīng)過顯著性分析，在Y 軸和Z 軸的位置誤差主要是由于初始對(duì)準(zhǔn)誤差引起的，該項(xiàng)誤差反映了初始對(duì)準(zhǔn)的角度大小，為有用信息。如果按照文獻(xiàn)[5]直接忽略Y 軸和Z 軸的位置誤差進(jìn)行誤差分離將導(dǎo)致分離的誤差系數(shù)有偏差，只考慮運(yùn)行方向X 軸的位置誤差如圖2 所示，在圖2 基礎(chǔ)上進(jìn)行誤差分離將導(dǎo)致分離的誤差系數(shù)有偏差。為此，需要研究一種將Y軸和Z 軸的位置誤差折合到X 軸方向上的一維約束方法。

圖2 慣性測(cè)量系統(tǒng)火箭橇試驗(yàn)X 軸導(dǎo)航位置及其誤差Fig.2 Position and Position Error of X Axis of Rocket Sled Testing

2 火箭橇試驗(yàn)橇體運(yùn)動(dòng)一維化原理

為了減小X 方向上的非加速度計(jì)儀表誤差因素，同時(shí)減小Y 軸和Z 軸的導(dǎo)航解算位移誤差，應(yīng)改善導(dǎo)航算法和提高導(dǎo)航。

橇體運(yùn)動(dòng)一維化的原理為：在橇體運(yùn)行時(shí)，慣性測(cè)量系統(tǒng)通過基于軌道坐標(biāo)系的導(dǎo)航算法求解得到3 個(gè)方向上的位置、速度和姿態(tài)角，利用Y 軸和Z軸方向上的位置真值和速度真值為0 來實(shí)時(shí)估計(jì)姿態(tài)角誤差，并從X 軸方向上的位置和速度中扣除掉姿態(tài)角誤差引起的分量[8～10]。

在橇體運(yùn)行時(shí)，姿態(tài)誤差方程寫成分量形式為

式中 φx，φy，φz分別為橇體在X，Y，Z 方向上的姿態(tài)角；δφx，δφy，δφz分別為橇體在X，Y，Z 方向上的姿態(tài)角誤差；ωx，ωy，ωz分別為橇體在X，Y，Z 方向上的角速度；分別為地球自轉(zhuǎn)角速度相對(duì)于X，Y，Z 坐標(biāo)軸的角速度在軌道坐標(biāo)系下的分量；分別為地球自轉(zhuǎn)角速度相對(duì)于X，Y，Z 坐標(biāo)軸的角速度在地理坐標(biāo)系下的分量。

由于Y，Z 方向的速度真值為零，因此Y，Z 方向的速度計(jì)算值等于速度誤差值，得到的速度誤差方程如下：

式中 vy，vz分別為橇體在Y，Z 方向上的速度計(jì)算值；δvy，δvz分別為橇體在Y，Z 方向上的速度誤差值；ax，ay，az分別表示橇體在X，Y，Z 方向上的加速度。

位置誤差方程為

式中 ry，rz分別表示橇體在Y，Z 方向上的位移。

因此，橇體運(yùn)動(dòng)一維化的基本思路就是，利用Y軸和Z 軸方向上的速度和位置來估計(jì)姿態(tài)角誤差，再把姿態(tài)角誤差折合到X 軸進(jìn)行修正和補(bǔ)償?shù)倪^程。

另外，橇體導(dǎo)航過程有兩個(gè)階段：第1 階段為橇體從發(fā)射時(shí)刻到停止時(shí)刻的運(yùn)行段；第2 階段為橇體停止后的靜止段。在這兩個(gè)階段的一維化處理方法不同，所以，橇體運(yùn)動(dòng)一維化也應(yīng)該分運(yùn)行段和靜止段。

3 火箭橇運(yùn)行時(shí)的姿態(tài)角誤差估計(jì)的可觀測(cè)性分析

針對(duì)第1 節(jié)提出的火箭橇試驗(yàn)橇體運(yùn)動(dòng)一維化處理的必要性問題，本節(jié)提出一種開環(huán)估計(jì)和補(bǔ)償?shù)姆椒?，可消除Y 軸和Z 軸的位置誤差，使得兩個(gè)軸的位移始終近似為0，則橇體在三維空間的運(yùn)動(dòng)可簡(jiǎn)化為一維的直線運(yùn)動(dòng)。

3.1 估計(jì)姿態(tài)角誤差的狀態(tài)方程和觀測(cè)方程

設(shè)狀態(tài)變量為 vy，ry，vz，rz，δφx，δφy和δφz，有7 維狀態(tài)方程：

式中

觀測(cè)方程為

3.2 橇體運(yùn)行時(shí)估計(jì)姿態(tài)角誤差的可觀測(cè)性分析

在分析可觀測(cè)性時(shí)，為分析方便，定義分塊矩陣：

在一個(gè)導(dǎo)航周期內(nèi)各元素可看作常值，對(duì)于線性定常連續(xù)系統(tǒng)有可觀性矩陣：

對(duì)Q 進(jìn)行初等行變換，有：

忽略小量，有：

在式（10）中前兩行各元素只取決于3 個(gè)方向的加速度，后兩行除加速度外，還取決于橇體運(yùn)行的角速度和地球轉(zhuǎn)速。由于地球轉(zhuǎn)速對(duì)可觀測(cè)性分析的影響可以忽略不計(jì)，因此，上式可進(jìn)一步簡(jiǎn)化為

式（11）決定了系統(tǒng)是否完全可觀測(cè)。在進(jìn)行火箭橇試驗(yàn)時(shí)，由于運(yùn)行方向過載ax較大，ay和az相對(duì)較小，如果只根據(jù)前兩行則不能判斷δφx能否完全辨識(shí)出來。第3 行沒有參考價(jià)值，而根據(jù)第4 行可給出δφx能否完全辨識(shí)出來的兩種情況。

第1 種情況是 ωz≈0，此時(shí) axgωz≈ aygωz≈0，都為小量，因此，辨識(shí)出來的δφx置信度較低。第2 種情況是ωz值較大，此時(shí) axgωz＞ aygωz，因此，辨識(shí)出來的δφx置信度較高，且其可觀測(cè)性隨著ωz值由大到小逐漸變?nèi)?。即?/p>

在橇體運(yùn)行時(shí)方位角的變化近似為0，因此可設(shè)ωz=0，如果在誤差辨識(shí)時(shí)繼續(xù)采用以上7 階誤差模型，則辨識(shí)出的δφx置信度較低或不可觀測(cè)。因此可采用模型降階。令δφx=0，將7 階姿態(tài)角誤差模型降為6 階，可觀測(cè)性分析方法與7 階模型相同。因此，系統(tǒng)完全可觀測(cè)的充要條件是，忽略小量，即式（11）的第1 列，由可觀測(cè)性矩陣得到：

在式（13）中前兩行各元素只取決于X 軸方向的加速度，后兩行除加速度外，還取決于橇體運(yùn)行的角速度和地球轉(zhuǎn)速。在火箭橇試驗(yàn)時(shí)，由于運(yùn)行方向過載ax較大，只根據(jù)前兩行即可將δφy和δφz完全辨識(shí)出來，因此，6 階系統(tǒng)完全可觀。

4 火箭橇停止時(shí)的姿態(tài)角誤差估計(jì)

由于方位角不完全可觀，當(dāng)火箭橇停止時(shí)，令δφz=0，則姿態(tài)誤差方程寫成分量形式為

另外，忽略天向通道后的速度誤差方程為

忽略天向通道的位置誤差方程為

設(shè)狀態(tài)變量為vy，ry，δvx，δrx，δφx，δφy，有狀態(tài)方程：

式中

觀測(cè)方程為

當(dāng)火箭橇停止運(yùn)行時(shí)，用3.2 小節(jié)相同的方法對(duì)其狀態(tài)方程和觀測(cè)方程進(jìn)行可觀測(cè)性分析，可知其觀測(cè)性矩陣為滿秩，系統(tǒng)完全可觀。

5 橇體姿態(tài)角誤差的開環(huán)估計(jì)和補(bǔ)償方法

下面利用開環(huán)卡爾曼濾波器來開環(huán)估計(jì)δφy和δφz，并實(shí)時(shí)補(bǔ)償其對(duì)X 軸橇體運(yùn)行方向上的影響。所謂實(shí)時(shí)開環(huán)修正，就是利用卡爾曼濾波器對(duì)導(dǎo)航誤差的姿態(tài)角估計(jì)值，在導(dǎo)航過程中只對(duì)X 軸加速度、速度和位置進(jìn)行開環(huán)修正。

在火箭橇系統(tǒng)運(yùn)行時(shí)，利用卡爾曼濾波器對(duì)導(dǎo)航誤差進(jìn)行實(shí)時(shí)估計(jì)，但不修正各項(xiàng)誤差，工作原理如圖3 所示。

圖3 火箭橇運(yùn)行中開環(huán)估計(jì)流程Fig.3 Diagram of Open-loop Estimation During the Rocket Sled Test Running

在火箭橇系統(tǒng)停止時(shí)，利用零速修正原理來求解兩個(gè)水平姿態(tài)角，工作原理如圖4 所示。

圖4 火箭橇停止后開環(huán)估計(jì)流程Fig.4 Diagram of Open-loop Estimation after the Rocket Sled Test Stop

利用式（20）對(duì)導(dǎo)航算法中速度實(shí)時(shí)補(bǔ)償過程為

6 仿真試驗(yàn)驗(yàn)證

下面給出卡爾曼濾波器對(duì)姿態(tài)角、速度誤差和位移誤差的估計(jì)結(jié)果及經(jīng)過開環(huán)補(bǔ)償之后的導(dǎo)航結(jié)果。

以圖1 實(shí)際試驗(yàn)導(dǎo)航數(shù)據(jù)為例，取卡爾曼濾波器I 的初始參數(shù)P0=I7，Q0=1×10-9I7，R0=1×106I4，用開環(huán)卡爾曼濾波器對(duì)7 個(gè)誤差項(xiàng)vy，vz，ry，rz，δφx，δφy和δφz進(jìn)行估計(jì)，姿態(tài)角誤差見圖5，速度和位移誤差見圖6。

把估計(jì)的3 個(gè)姿態(tài)角誤差δφx，δφy和δφz重新代入式（21）的補(bǔ)償公式后進(jìn)行導(dǎo)航解算，結(jié)果如圖7 所示。從導(dǎo)航結(jié)果可以看出，Y 軸和Z 軸的位移誤差已減小到1 cm 左右。

圖5 運(yùn)行段姿態(tài)角誤差的估計(jì)結(jié)果Fig.5 Estimation of Attitude Angle Error During the Rocket Sled Test Running

圖6 速度和位移誤差的估計(jì)結(jié)果Fig.6 Estimation of Velocity Error and Position Error

圖7 經(jīng)過誤差開環(huán)補(bǔ)償后的導(dǎo)航結(jié)果Fig.7 Navigation Result after Open-loop Compensation

圖7 中與X 軸運(yùn)動(dòng)有關(guān)的遙外測(cè)誤差如圖8 所示。

圖8 經(jīng)過誤差補(bǔ)償后的遙外測(cè)誤差導(dǎo)航結(jié)果Fig.8 Position and Position Error of X Axis after Error Compensation

比較圖2 和圖8 可以看出，X軸位置誤差明顯不同，圖2 是直接忽略了Y 軸和Z 軸的誤差，而圖8 是經(jīng)過本文介紹的一維運(yùn)動(dòng)約束后的結(jié)果。在后續(xù)誤差分離時(shí)，應(yīng)以圖8 的結(jié)果為準(zhǔn)。

總之，通過以上基于開環(huán)估計(jì)和實(shí)時(shí)補(bǔ)償?shù)倪^程，把慣性測(cè)量系統(tǒng)火箭橇試驗(yàn)時(shí)的三維運(yùn)動(dòng)簡(jiǎn)化為一維直線運(yùn)動(dòng)。此時(shí)，Y 軸和Z 軸的線位移近似于0，只需考慮X 軸方向上的運(yùn)動(dòng)即可。

7 結(jié) 論

由于慣性測(cè)量系統(tǒng)在橇體上的安裝誤差、導(dǎo)航初始對(duì)準(zhǔn)誤差和姿態(tài)角誤差等因素的影響，造成火箭橇運(yùn)動(dòng)在側(cè)向Y 軸和高度方向Z 軸上的速度和位移均不為零，從而導(dǎo)致導(dǎo)航解算后的X 方向上的運(yùn)動(dòng)并非精確的一維直線運(yùn)動(dòng)。

為此本文提出了一種慣性測(cè)量系統(tǒng)火箭橇試驗(yàn)一維運(yùn)動(dòng)約束方法。利用卡爾曼濾波器對(duì)火箭橇姿態(tài)角誤差、速度誤差和位移誤差狀態(tài)量進(jìn)行開環(huán)估計(jì)，并利用Y 軸和Z 軸上速度和位移誤差為零原理進(jìn)行誤差修正補(bǔ)償，使得橇體在Y 軸和Z 軸上的位置和速度近似為零，最終將火箭橇的三維運(yùn)動(dòng)簡(jiǎn)化為精確的一維直線運(yùn)動(dòng)。通過試驗(yàn)驗(yàn)證，經(jīng)過卡爾曼濾波估計(jì)和零速修正補(bǔ)償后的Y 軸和Z 軸的位移誤差已減小到1 cm左右，近似為0，實(shí)現(xiàn)了火箭橇試驗(yàn)運(yùn)動(dòng)一維化的處理。