魏宗康，高榮榮

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

0 引 言

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

在利用火箭橇試驗(yàn)對(duì)慣性測(cè)量系統(tǒng)進(jìn)行性能和精度評(píng)估時(shí)，主要是根據(jù)火箭橇在直線軌道上的單方向一維運(yùn)動(dòng)特點(diǎn)。文獻(xiàn)[1]給出了一種過(guò)載曲線設(shè)計(jì)，給出了在軌道長(zhǎng)度限制條件下在主動(dòng)段、自由滑行段和制動(dòng)段的時(shí)變過(guò)載值。文獻(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)方向上的問(wèn)題。文獻(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)誤差等因素造成的。為解決上述問(wèn)題，本文提出了一種慣性測(cè)量系統(tǒng)火箭橇試驗(yàn)一維運(yùn)動(dòng)約束方法。利用開(kāi)環(huán)卡爾曼濾波器來(lái)開(kāi)環(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)過(guò)程中，火箭橇的運(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)過(guò)程中，Y 軸和Z 軸的位移應(yīng)為0，但導(dǎo)航解算后的位移和速度都不為0，位移最大可達(dá)10 m。經(jīng)過(guò)顯著性分析，在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)通過(guò)基于軌道坐標(biāo)系的導(dǎo)航算法求解得到3 個(gè)方向上的位置、速度和姿態(tài)角，利用Y 軸和Z軸方向上的位置真值和速度真值為0 來(lái)實(shí)時(shí)估計(jì)姿態(tài)角誤差，并從X 軸方向上的位置和速度中扣除掉姿態(tài)角誤差引起的分量[8～10]。

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

式中 φ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 軸方向上的速度和位置來(lái)估計(jì)姿態(tài)角誤差，再把姿態(tài)角誤差折合到X 軸進(jìn)行修正和補(bǔ)償?shù)倪^(guò)程。

另外，橇體導(dǎo)航過(guò)程有兩個(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)一維化處理的必要性問(wèn)題，本節(jié)提出一種開(kāi)環(huán)估計(jì)和補(bǔ)償?shù)姆椒ǎ上齓 軸和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)行方向過(guò)載ax較大，ay和az相對(duì)較小，如果只根據(jù)前兩行則不能判斷δφx能否完全辨識(shí)出來(lái)。第3 行沒(méi)有參考價(jià)值，而根據(jù)第4 行可給出δφx能否完全辨識(shí)出來(lái)的兩種情況。

第1 種情況是 ωz≈0，此時(shí) axgωz≈ aygωz≈0，都為小量，因此，辨識(shí)出來(lái)的δφx置信度較低。第2 種情況是ωz值較大，此時(shí) axgωz＞ aygωz，因此，辨識(shí)出來(lái)的δφ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)行方向過(guò)載ax較大，只根據(jù)前兩行即可將δφy和δφz完全辨識(shí)出來(lái)，因此，6 階系統(tǒng)完全可觀。

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

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

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

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

設(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è)性矩陣為滿(mǎn)秩，系統(tǒng)完全可觀。

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

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

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

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

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

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

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

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

下面給出卡爾曼濾波器對(duì)姿態(tài)角、速度誤差和位移誤差的估計(jì)結(jié)果及經(jīng)過(guò)開(kāi)環(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，用開(kāi)環(huán)卡爾曼濾波器對(duì)7 個(gè)誤差項(xiàng)vy，vz，ry，rz，δφx，δφy和δφz進(jìn)行估計(jì)，姿態(tài)角誤差見(jiàn)圖5，速度和位移誤差見(jiàn)圖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)過(guò)誤差開(kāi)環(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)過(guò)誤差補(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)過(guò)本文介紹的一維運(yùn)動(dòng)約束后的結(jié)果。在后續(xù)誤差分離時(shí)，應(yīng)以圖8 的結(jié)果為準(zhǔn)。

總之，通過(guò)以上基于開(kāi)環(huán)估計(jì)和實(shí)時(shí)補(bǔ)償?shù)倪^(guò)程，把慣性測(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)行開(kāi)環(huán)估計(jì)，并利用Y 軸和Z 軸上速度和位移誤差為零原理進(jìn)行誤差修正補(bǔ)償，使得橇體在Y 軸和Z 軸上的位置和速度近似為零，最終將火箭橇的三維運(yùn)動(dòng)簡(jiǎn)化為精確的一維直線運(yùn)動(dòng)。通過(guò)試驗(yàn)驗(yàn)證，經(jīng)過(guò)卡爾曼濾波估計(jì)和零速修正補(bǔ)償后的Y 軸和Z 軸的位移誤差已減小到1 cm左右，近似為0，實(shí)現(xiàn)了火箭橇試驗(yàn)運(yùn)動(dòng)一維化的處理。