基于組網(wǎng)導(dǎo)航系統(tǒng)的運動體姿態(tài)估計算法

劉 帥，趙國榮，韓 旭，李海君

(1.海軍航空大學(xué) 岸防兵學(xué)院， 山東 煙臺 264001； 2.中國人民解放軍91001部隊， 北京 100048； 3.中國人民解放軍91115部隊， 浙江 舟山 316000)

1 引言

計算機和通信技術(shù)的日趨成熟，促進了網(wǎng)絡(luò)化系統(tǒng)的發(fā)展。組網(wǎng)導(dǎo)航系統(tǒng)(networked navigation system，NNS)是網(wǎng)絡(luò)化系統(tǒng)在導(dǎo)航領(lǐng)域的一個典型應(yīng)用，通過無線網(wǎng)絡(luò)將集群運動體關(guān)聯(lián)起來組成導(dǎo)航網(wǎng)絡(luò)，參與組網(wǎng)的節(jié)點按照設(shè)定的通信協(xié)議進行信息交互，協(xié)作完成定位、測姿等導(dǎo)航任務(wù)，能夠最大化地利用導(dǎo)航資源，提高系統(tǒng)的導(dǎo)航性能[1-4]。NNS是對傳統(tǒng)單平臺導(dǎo)航方式的拓展，對提高導(dǎo)航的“性價比”具有重要意義。

姿態(tài)信息的獲取對運動體的導(dǎo)航與控制是至關(guān)重要的?，F(xiàn)有的姿態(tài)測量系統(tǒng)主要包括慣性導(dǎo)航系統(tǒng)、視覺導(dǎo)航系統(tǒng)、地磁導(dǎo)航系統(tǒng)、GNSS測姿系統(tǒng)等[5-12]。慣性導(dǎo)航系統(tǒng)具有自主導(dǎo)航、短時精度高、抗干擾性強等優(yōu)點，但其姿態(tài)估計誤差會隨時間累積[5-6]。視覺測量技術(shù)具有成本低、精度高等優(yōu)點，但其測量范圍受限，更適合用于近距離位姿測量[7-8]。地磁導(dǎo)航系統(tǒng)以地球磁場矢量為參考基準進行姿態(tài)解算，具有隱蔽性強、無累積誤差、響應(yīng)速度快等特點，但在使用過程中容易受到電磁干擾的影響[9]。GNSS測姿系統(tǒng)具有成本低、無累計誤差、精度高等優(yōu)點，但在姿態(tài)估計過程中需要進行復(fù)雜的模糊度解算，且無法在室內(nèi)等GNSS信號遮擋嚴重的環(huán)境中使用[10-11]。本文研究基于NNS的姿態(tài)估計技術(shù)，是在定位功能基礎(chǔ)上，對NNS導(dǎo)航功能的拓展,既可以作為獨立的導(dǎo)航信源為運動體提供姿態(tài)數(shù)據(jù)，也可以與其他測姿系統(tǒng)進行信息融合，以獲取更好的導(dǎo)航性能。

2 NNS工作流程

NNS是以組網(wǎng)運動體為載體，以運動體的導(dǎo)航、定位、測姿和信息共享為核心任務(wù)，由導(dǎo)航傳感器、組網(wǎng)協(xié)議棧、導(dǎo)航處理器等通過實時網(wǎng)絡(luò)構(gòu)成閉環(huán)反饋的分布式導(dǎo)航系統(tǒng)[3-4]。假設(shè)NNS中共有M個節(jié)點，采用有向圖G=(V,A)對系統(tǒng)進行描述，其中，V和A分別為G的節(jié)點集和邊集。對于V中的任意節(jié)點i(i=1,2，…,M)，其協(xié)作鄰節(jié)點可表示為N[i]={j∈V∶(i,j)∈A}。

典型的組網(wǎng)導(dǎo)航流程可作如下描述：

1) 節(jié)點i通過網(wǎng)絡(luò)接收來自其鄰節(jié)點j(j∈N[i])的協(xié)作數(shù)據(jù)包，每個數(shù)據(jù)包包含發(fā)送者的導(dǎo)航信息和信號的基本信息。具備目標跟蹤功能的節(jié)點發(fā)送的數(shù)據(jù)包中，還可包含對目標狀態(tài)的估計。

2) 節(jié)點i利用鄰節(jié)點的導(dǎo)航信息和節(jié)點間的相對測量值進行自身導(dǎo)航狀態(tài)的更新。

3) 節(jié)點i將自身的導(dǎo)航信息打包，并在其發(fā)送時序通過網(wǎng)絡(luò)發(fā)送給其他節(jié)點，作為其他節(jié)點的協(xié)作信息。

3 基于NNS的姿態(tài)解算模型

3.1 坐標系與姿態(tài)

載體的姿態(tài)，是指載體坐標系相對于當?shù)氐乩碜鴺讼档姆轿魂P(guān)系[13]?；贜NS進行運動體姿態(tài)估計，需要在運動體的中心處安裝一個節(jié)點作為主節(jié)點。如圖1所示，測姿系統(tǒng)的載體系以主節(jié)點所在位置為原點，以運動體主軸方向為Yb軸，Zb軸垂直于節(jié)點安裝平面指向上方，Xb軸由Yb軸圍繞Zb軸順時針旋轉(zhuǎn)90°得到。

圖1 天線安裝及載體坐標系示意圖

本文采用“東北天”坐標系作為當?shù)氐乩碜鴺讼担灾鞴?jié)點所在的位置為坐標原點，Xn軸指向東(E)，Yn軸指向北(N)，Zn軸指向天頂方向。載體坐標系可由地理坐標系經(jīng)過3次旋轉(zhuǎn)得到，3次旋轉(zhuǎn)分別對應(yīng)偏航角ψ、俯仰角θ、滾轉(zhuǎn)角γ。3次旋轉(zhuǎn)可用以下變換矩陣表示[14]：

于是可得，由地理坐標系到載體坐標系的變換矩陣為

3.2 基線矢量的求解

按照圖1所示，在運動體的主軸上安裝2個組網(wǎng)導(dǎo)航節(jié)點。導(dǎo)航節(jié)點在安裝時會出現(xiàn)安裝誤差，工程應(yīng)用中，一方面要采用盡可能高的安裝工藝來減小安裝誤差，另一方面可通過一定的標校手段獲取安裝誤差值，并在解算過程中加入修正量抵消安裝誤差造成的影響。

圖2 基于基線矢量的姿態(tài)估計示意圖

(1)

由三角函數(shù)的相關(guān)性質(zhì)可得：

(2)

結(jié)合式(1)、式(2)可得：

(3)

(4)

將式(4)代入式(3)可得：

組網(wǎng)導(dǎo)航系統(tǒng)可直接測量節(jié)點間的相對距離。na、nb與nj間的相對距離測量方程為

(5)

定義如下變量：

(6)

則根據(jù)協(xié)方差傳播定律可得：

(7)

結(jié)合式(3)、式(5)、式(6)可得：

工程上可采用超寬帶(Ultra-wide band，UWB)技術(shù)進行組網(wǎng)測距?？紤]實際的安裝條件，有時會借助輔助天線來實現(xiàn)測距功能，此時，可將天線相位中心視為節(jié)點所在位置。另外，本文在建模過程中將節(jié)點視為質(zhì)點，在組網(wǎng)導(dǎo)航過程中，信號的收發(fā)和導(dǎo)航信息的共享都是以組網(wǎng)模塊(或天線)的相位中心為終端完成的。

若在同一時刻2個天線都能接收到n個鄰居節(jié)點的協(xié)作信息，則可將姿態(tài)測量模型寫成如下形式：

zk=Bkbk+vk

(8)

式中的參數(shù)如下：

結(jié)合式(8)可利用最小二乘法求解基線矢量：

考慮不同測量值間的質(zhì)量差異，可采用加權(quán)最小二乘法[15]估計基線矢量：

(9)

3.3 姿態(tài)角求解

結(jié)合姿態(tài)變換矩陣，導(dǎo)航體的偏航角和俯仰角可利用基線矢量通過下列式子求出：

(10)

(11)

表1 航向角

為了獲取3個姿態(tài)角，需考慮雙基線的情況，假設(shè)天線布局如圖3所示，na安裝在導(dǎo)航體中心，nb安裝在導(dǎo)航體主軸上，na、nb構(gòu)成基線1；nc安裝在XBOYB平面上，na、nc構(gòu)成基線2，兩條基線間的夾角為φ。

圖3 三節(jié)點布局示意圖

(12)

由式(12)可得:

需要說明的是，3個天線中對主天線(天線a)的要求較高，除了基本的測距功能外，還需具備測角功能或者較高的定位精度；而輔助天線只需具有測距功能即可。

4 基于EKF的姿態(tài)估計

第3節(jié)提出的算法只利用了觀測信息，如果能夠充分利用運動體的運動模型信息，將大大提高導(dǎo)航體姿態(tài)解算的效率和精度。本節(jié)從狀態(tài)方程的角度，采用EKF算法進行狀態(tài)估計，首先，構(gòu)造如下狀態(tài)向量：

xk+1=Fxk+Gwk

(13)

式中，wk表示過程噪聲，假設(shè)其方差為Q；F、G為系數(shù)矩陣，滿足：

k時刻，使用導(dǎo)航體的姿態(tài)角，在當?shù)氐乩碜鴺讼迪?，可將主基線表示為

代入式(8)可得觀測方程：

zk=h(ψk,θk)+vk

(14)

式中，

根據(jù)式(13)和式(14)，可按照以下步驟進行運動體的姿態(tài)估計：

步驟2：一步預(yù)測：

xk|k-1=Fxk-1

Pk|k-1=FPk-1FT+GQGT

步驟3：根據(jù)式(6)、式(7) 計算Rk

步驟4：計算增益矩陣：

步驟5：更新：

Pk=(I-KkHk)Pk|k-1

5 仿真驗證

其中，N表示仿真次數(shù);ek, j表示第j次仿真中k時刻的誤差。

設(shè)定主節(jié)點和鄰節(jié)點定位誤差0.1 m、基線長度2 m，分別采用加權(quán)最小二乘法和EKF算法進行姿態(tài)解算，仿真結(jié)果如圖4所示。顯然，算法2(EKF算法)的測姿精度優(yōu)于算法1(加權(quán)最小二乘法)。

圖4 算法性能曲線

對比2種方法可以發(fā)現(xiàn)，加權(quán)最小二乘法具有使用范圍廣、約束條件少的特點；EKF算法能獲得更高的精度，但需要初始值和準確的運動方程建模為前提。在實際應(yīng)用中，可先根據(jù)加權(quán)最小二乘法確定姿態(tài)解算的初始值，然后根據(jù)EKF算法進行姿態(tài)估計。

設(shè)定主節(jié)點定位誤差0.1 m、鄰節(jié)點定位誤差0.1 m，基線長度分別取1 m、2 m、5 m，研究基線長度對姿態(tài)估計的影響，結(jié)果如圖5所示。隨著基線長度的增加，測姿精度明顯提高，但在工程應(yīng)用中，還需要根據(jù)運動體的實際安裝條件來確定基線長度。

圖5 基線長度對測姿結(jié)果的影響曲線

令基線長度為2 m、鄰節(jié)點定位誤差0.1 m，主節(jié)點定位誤差分別取5 cm、10 cm、15 cm，仿真結(jié)果如圖6所示。隨著主節(jié)點定位誤差的增大，測姿誤差明顯增大，說明主節(jié)點定位誤差對測姿精度有較大的影響。

圖6 主節(jié)點定位誤差對測姿結(jié)果的影響曲線

令基線長度為2 m，主節(jié)點定位誤差5 cm，鄰節(jié)點定位誤差分別取5 cm、10 cm、15 cm，仿真結(jié)果如圖7所示，3種情況下的測姿精度變化不大，說明鄰節(jié)點定位誤差在一定范圍內(nèi)的變化基本不影響測姿精度。

圖7 鄰節(jié)點定位誤差測姿結(jié)果的影響曲線

6 結(jié)論

基于測距信息建立組網(wǎng)姿態(tài)解算模型，根據(jù)加權(quán)最小二乘法求解基線矢量坐標，結(jié)合坐標轉(zhuǎn)換關(guān)系得到姿態(tài)角。建立了運動體姿態(tài)變化的運動學(xué)方程和觀測方程，并基于EKF方法設(shè)計了組網(wǎng)姿態(tài)和姿態(tài)角速度估計算法。仿真結(jié)果表明，相比最小二乘法，本文提出的姿態(tài)估計精度更高。另外，基線長度、主節(jié)點定位誤差對濾波精度影響較大，基線長度越長、主節(jié)點定位誤差越小，則濾波精度越高，而鄰節(jié)點定位誤差對精度幾乎無影響。