基于EKF 算法的三軸氣浮臺高精度姿態(tài)確定系統(tǒng)

刁雁華 夏紅偉

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

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

1 引言

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

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

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

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

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

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

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

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

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

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

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

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

3 姿態(tài)描述

3.1 坐標(biāo)系

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

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

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

3.2.1 歐拉四元數(shù)

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

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

3.3 姿態(tài)敏感器

3.3.1 陀螺

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

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

4.1 偏差四元數(shù)

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

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

4.3 狀態(tài)方程

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

4.4 量測方程

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

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

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

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

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

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

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

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

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

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

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

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

場景：臺體脫離激光跟蹤儀的視場范圍。

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

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

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

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

6 仿真試驗(yàn)

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

試驗(yàn)描述：控制氣浮臺繞B 系z 軸機(jī)動。

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

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

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

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

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

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

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

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

7 結(jié)束語

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