刁雁華 夏紅偉

（1.南京國(guó)睿防務(wù)系統(tǒng)有限公司 江蘇省南京市 210012）

（2.哈爾濱工業(yè)大學(xué)空間控制與慣性技術(shù)研究中心 黑龍江省哈爾濱市 150001）

1 引言

三軸氣浮臺(tái)作為空間飛行器地面仿真系統(tǒng)的核心設(shè)備，其姿態(tài)確定系統(tǒng)分為兩類，一類依托姿態(tài)敏感器的信息，另一類主要借助計(jì)算機(jī)視覺(jué)完成姿態(tài)的測(cè)量。

確定性算法和狀態(tài)估計(jì)算法是姿態(tài)確定算法的主要研究方向。確定性算法起源于Wahba 問(wèn)題，該方法具有實(shí)用性好，操作性強(qiáng)，精度不高的特點(diǎn)。狀態(tài)估計(jì)算法基于狀態(tài)空間表達(dá)式，根據(jù)姿態(tài)描述特征的不同，發(fā)展出歐拉角、四元數(shù)等改進(jìn)算法。狀態(tài)估計(jì)法模型建立靈活，是目前的主流方法。

本文就三軸氣浮臺(tái)的姿態(tài)確定問(wèn)題，設(shè)計(jì)實(shí)現(xiàn)姿態(tài)確定系統(tǒng)，選定測(cè)量元件和定姿方案。研究并提出了一種高精度姿態(tài)確定算法，并開(kāi)展地面全物理仿真試驗(yàn)工作。

2 系統(tǒng)總體設(shè)計(jì)

三軸氣浮臺(tái)高精度姿態(tài)確定系統(tǒng)，包括高精度激光陀螺1 套、激光跟蹤儀1 臺(tái)、T-Probe 智能測(cè)頭1 個(gè)、光電自準(zhǔn)直儀2 臺(tái)、立方棱鏡1 個(gè)、工業(yè)控制計(jì)算機(jī)2 臺(tái)。

系統(tǒng)分為臺(tái)上、臺(tái)下兩個(gè)子系統(tǒng)。臺(tái)上子系統(tǒng)包括安裝在臺(tái)體上的陀螺、T-Probe、立方體棱鏡和工業(yè)控制計(jì)算機(jī)1（臺(tái)上工控機(jī)）；臺(tái)下子系統(tǒng)包括分布在氣浮臺(tái)周圍的激光跟蹤儀、光電自準(zhǔn)直儀和工業(yè)控制計(jì)算機(jī)2（臺(tái)下工控機(jī)），其中兩臺(tái)光電自準(zhǔn)直儀90°安裝。

在傳感器的位置分布方面，需同時(shí)滿足以下試驗(yàn)條件：

條件1：立方棱鏡能夠同時(shí)進(jìn)入兩臺(tái)光電自準(zhǔn)直儀的視場(chǎng)范圍；

條件2：T-Probe 能夠進(jìn)入激光跟蹤儀的視場(chǎng)范圍；

條件3：激光跟蹤儀的視場(chǎng)可覆蓋光電自準(zhǔn)直儀的視場(chǎng)范圍。

光電自準(zhǔn)直儀、激光跟蹤儀分別搭配立方棱鏡、T-Probe，給出臺(tái)體姿態(tài)；臺(tái)下工控機(jī)接收傳感器數(shù)據(jù)并轉(zhuǎn)發(fā)給臺(tái)上；臺(tái)上工控機(jī)同時(shí)接收臺(tái)下數(shù)據(jù)和陀螺角速度，經(jīng)算法處理后輸出姿態(tài)信息，給到姿態(tài)控制系統(tǒng)的閉環(huán)反饋鏈路，同時(shí)下發(fā)給臺(tái)下子系統(tǒng)；臺(tái)下工控機(jī)完成數(shù)據(jù)記錄，用于事后分析和效能評(píng)估。系統(tǒng)工作流程如圖 1 所示。

圖1： 工作流程與數(shù)據(jù)流情況

3 姿態(tài)描述

3.1 坐標(biāo)系

根據(jù)姿態(tài)描述關(guān)系與傳感器特征，建立地理坐標(biāo)系、氣浮臺(tái)臺(tái)體坐標(biāo)系、陀螺量測(cè)坐標(biāo)系、陀螺立方棱鏡坐標(biāo)系、激光跟蹤儀坐標(biāo)系、T-Probe 坐標(biāo)系、立方棱鏡的標(biāo)定坐標(biāo)系及其量測(cè)坐標(biāo)系。

地理坐標(biāo)系（G 系）視為姿態(tài)參考，氣浮臺(tái)臺(tái)體坐標(biāo)系（B系）與氣浮臺(tái)本體固聯(lián)。陀螺量測(cè)坐標(biāo)系（Gyro 系）描述角速度的分布情況，其陀螺棱鏡坐標(biāo)系（P 系）用于輔助標(biāo)定陀螺的安裝矩陣 。激光跟蹤儀坐標(biāo)系（J 系）與T-Probe坐標(biāo)系（T 系）的相對(duì)關(guān)系表示姿態(tài)變化。立方棱鏡的標(biāo)定坐標(biāo)系（L 系）和量測(cè)坐標(biāo)系（D 系）分別應(yīng)用于位置標(biāo)定和姿態(tài)量測(cè)階段。

3.2 姿態(tài)參數(shù)和運(yùn)動(dòng)學(xué)方程

3.2.1 歐拉四元數(shù)

3.2.2 歐拉四元數(shù)的運(yùn)動(dòng)學(xué)方程

B 系下的角速度為 ，則四元數(shù)的運(yùn)動(dòng)學(xué)方程為：

3.3 姿態(tài)敏感器

3.3.1 陀螺

光電自準(zhǔn)直儀根據(jù)臺(tái)上立方棱鏡反射光線的位置反映姿態(tài)變化，用于小角度場(chǎng)景。定義初始時(shí)刻的臺(tái)體坐標(biāo)系（B系）；當(dāng)前時(shí)刻的臺(tái)體坐標(biāo)系（B系）；初始時(shí)刻的立方棱鏡測(cè)量坐標(biāo)系，（D系）；當(dāng)前時(shí)刻的立方棱鏡測(cè)量坐標(biāo)系（D系）。

4 基于EKF的高精度姿態(tài)確定算法

4.1 偏差四元數(shù)

偏差四元數(shù)描述四元數(shù)實(shí)際值相對(duì)預(yù)測(cè)量的微小轉(zhuǎn)動(dòng)，其標(biāo)量部分近似為1，矢量部分體現(xiàn)轉(zhuǎn)動(dòng)姿態(tài)。設(shè)q 是量測(cè)姿態(tài)四元數(shù)， 是估計(jì)，則偏差四元數(shù)q為：

4.2 偏差四元數(shù)的運(yùn)動(dòng)學(xué)方程

4.3 狀態(tài)方程

表示陀螺的角速度在B 系的投影， 表示臺(tái)體角速度在B 系的投影，其中b 為陀螺三軸漂移在B 系的投影，n為量測(cè)噪聲在B 系的投影：

4.4 量測(cè)方程

選擇B 系相對(duì)I 系到偏差四元數(shù)的矢量部分Q作為量測(cè)值，建立量測(cè)方程，其中v是激光跟蹤儀或光電自準(zhǔn)直儀的量測(cè)噪聲：

5 姿態(tài)確定方案

高精度姿態(tài)確定系統(tǒng)采用量測(cè)元件聯(lián)合確定臺(tái)體姿態(tài)信息，細(xì)分為三種子方案，具體配置如下：

5.1 方案一：“陀螺+光電自準(zhǔn)直儀/激光跟蹤儀”聯(lián)合定姿

場(chǎng)景：臺(tái)體在光電自準(zhǔn)直儀的檢測(cè)范圍內(nèi)。

臺(tái)下子系統(tǒng)：光電自準(zhǔn)直儀和激光跟蹤儀分別確定姿態(tài)角，發(fā)送給臺(tái)上子系統(tǒng)。

臺(tái)上子系統(tǒng)：臺(tái)下姿態(tài)配合陀螺角速度，經(jīng)EKF 算法輸出臺(tái)體姿態(tài)。

5.2 方案二：“陀螺+激光跟蹤儀” 聯(lián)合定姿

場(chǎng)景：臺(tái)體脫離光電自準(zhǔn)直儀的視場(chǎng)范圍，并保持在激光跟蹤儀的視場(chǎng)范圍中。

臺(tái)下子系統(tǒng)：采集并轉(zhuǎn)發(fā)激光跟蹤儀輸出的臺(tái)體姿態(tài)角度給臺(tái)上。

臺(tái)上子系統(tǒng)：接收臺(tái)下信息，配合陀螺經(jīng)EKF 算法，輸出臺(tái)體實(shí)時(shí)姿態(tài)角和姿態(tài)角速度。

5.3 方案三：陀螺獨(dú)立定姿

場(chǎng)景：臺(tái)體脫離激光跟蹤儀的視場(chǎng)范圍。

臺(tái)下子系統(tǒng)：激光跟蹤儀和光電自準(zhǔn)直儀均無(wú)法給出姿態(tài)數(shù)據(jù)。

臺(tái)上子系統(tǒng)：陀螺角速度積分輸出姿態(tài)角和姿態(tài)角速度。

三種子方案聯(lián)合使用，達(dá)到全局定姿的效果，如圖 2 所示，方案一優(yōu)先級(jí)最高，后續(xù)依次為方案二和方案三。

圖2： 聯(lián)合姿態(tài)確定的子方案選擇與切換流程圖

6 仿真試驗(yàn)

試驗(yàn)設(shè)備：光電自準(zhǔn)直儀、激光跟蹤儀、陀螺。

試驗(yàn)描述：控制氣浮臺(tái)繞B 系z(mì) 軸機(jī)動(dòng)。

指令角：[0° 0° 120°]→[0° 0° 170°]→[0° 0° 120°]。

氣浮臺(tái)初始姿態(tài)120°，后機(jī)動(dòng)到170°，最后回歸120°，試驗(yàn)時(shí)長(zhǎng)550s。其中0-6s、533-550s 是穩(wěn)態(tài)階段，7-532s是機(jī)動(dòng)階段。

方案一姿態(tài)確定：0-4.5s 和513.2-550s 時(shí)，臺(tái)上棱鏡處于光電自準(zhǔn)直儀的視野范圍，光電自準(zhǔn)直儀輸出姿態(tài)角與陀螺角速度輸出給EKF 算法處理。

方案二姿態(tài)確定：氣浮臺(tái)離開(kāi)光電自準(zhǔn)直儀視場(chǎng)，切換到子方案二，對(duì)應(yīng)4.6-197.6s 和317.6-513.1s。EKF 處理激光跟蹤儀和陀螺數(shù)據(jù)后給出臺(tái)體姿態(tài)角。

方案三姿態(tài)確定：氣浮臺(tái)離開(kāi)激光跟蹤儀的視場(chǎng)，使用方案三，從198.7 到317.5s，依靠陀螺自主完成姿態(tài)解算工作。

姿態(tài)全程曲線如圖 3 所示，x 軸和y 軸姿態(tài)角維持0°附近，z 軸較穩(wěn)定地實(shí)現(xiàn)機(jī)動(dòng)與回歸。

圖3： 三種定姿方案對(duì)應(yīng)的姿態(tài)角曲線

試驗(yàn)結(jié)論：機(jī)動(dòng)過(guò)程覆蓋全部場(chǎng)景，定姿方案自主切換工作，EKF 算法對(duì)多數(shù)據(jù)源融合濾波，實(shí)現(xiàn)姿態(tài)確定。

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

文章圍繞三軸氣浮臺(tái)高精度姿態(tài)確定問(wèn)題，從系統(tǒng)設(shè)計(jì)、姿態(tài)確定算法、方案配置、地面全物理仿真試驗(yàn)等四個(gè)方面研究分析。搭建姿態(tài)確定系統(tǒng)，基于EKF 技術(shù)，提出適配系統(tǒng)的姿態(tài)確定算法，完成多傳感器數(shù)據(jù)融合與復(fù)雜場(chǎng)景下的聯(lián)合定姿，取得了理想的姿態(tài)確定效果，驗(yàn)證了系統(tǒng)設(shè)計(jì)的可行性和算法的有效性，為空間飛行器及其地面仿真系統(tǒng)的高精度姿態(tài)確定領(lǐng)域提供了研究思路。