姿態(tài)卡爾曼濾波對FAST饋源艙BDS/SINS組合導(dǎo)航位姿精度的提高*

李明輝 姜 鵬 于東俊 宋本寧 駱亞波 孫京海

(1中國科學(xué)院國家天文臺北京100101)

(2中國科學(xué)院大學(xué)北京100049)

(3長沙理工大學(xué)交通運輸工程學(xué)院長沙410076)

1 引言

目前,500 m球面射電望遠(yuǎn)鏡(The Five-hundred-meter Aperture Spherical radio Telescope,FAST)是世界口徑最大且靈敏度最高的單口徑射電望遠(yuǎn)鏡(圖1).FAST的接收機(jī)安裝在30噸重的饋源艙底部,饋源艙位置和姿態(tài)的測量精度會影響接收機(jī)的對準(zhǔn)精度.為了提高對饋源艙位置和姿態(tài)的導(dǎo)航精度,采用北斗衛(wèi)星導(dǎo)航系統(tǒng)(BeiDou Navigation Satellite System,BDS)和捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strap-down Inertial Navigation System,SINS)組合的導(dǎo)航方式.一般情況,將BDS與SINS兩者的位置和速度信息分別做差,差值作為卡爾曼濾波算法的量測值,對組合導(dǎo)航系統(tǒng)的狀態(tài)量進(jìn)行最優(yōu)估計[1–2].但隨著衛(wèi)星導(dǎo)航精度的提高,采用多天線技術(shù)可以解算出載體的姿態(tài)[3–4],將姿態(tài)差值也作為量測值,卡爾曼濾波算法的量測值維度就可以由原來的6維擴(kuò)充到9維,從而提高導(dǎo)航信息狀態(tài)量最優(yōu)估計的精度.如果采用傳統(tǒng)的BDS多天線定姿算法,用3個姿態(tài)角表示空間姿態(tài)旋轉(zhuǎn)矩陣,那求解過程需要采用泰勒級數(shù)將非線性方程轉(zhuǎn)化成線性方程,解算過程復(fù)雜.為了簡化姿態(tài)求解,本文采用羅格里德矩陣空間坐標(biāo)轉(zhuǎn)化方法[5–6],規(guī)避非線性解算問題,降低空間旋轉(zhuǎn)矩陣求解復(fù)雜度.

圖1 FAST示意圖Fig.1 Illustration of the FAST

2 基于羅格里德矩陣的姿態(tài)解算模型

姿態(tài)求解是確定一個坐標(biāo)系相對另一個坐標(biāo)系的空間轉(zhuǎn)換關(guān)系.首先,饋源艙的姿態(tài)導(dǎo)航就是確定饋源艙坐標(biāo)系相對于導(dǎo)航坐標(biāo)系的轉(zhuǎn)動情況;其次,SINS系統(tǒng)內(nèi)部進(jìn)行姿態(tài)導(dǎo)航的陀螺是以地心慣性系和地球坐標(biāo)系為參考系的;再次,FAST望遠(yuǎn)鏡內(nèi)部設(shè)備眾多,為了坐標(biāo)系統(tǒng)一,人為定義了一個FAST坐標(biāo)系.

2.1 坐標(biāo)系定義

f-FAST坐標(biāo)系:對FAST饋源艙的導(dǎo)航是將FAST坐標(biāo)系作為參考系的,FAST坐標(biāo)系的原點是望遠(yuǎn)鏡基準(zhǔn)球面的球心,xf軸指向地理東向,yf軸指向地理北向,zf軸垂直于當(dāng)?shù)厮矫嬷赶蛱煜?

b-饋源艙坐標(biāo)系:其原點為饋源艙的重心,yb軸指向饋源艙運動方向,xb軸在水平面內(nèi)垂直yb軸,zb軸垂直于水平面,3軸遵守右手定則;

e-地球坐標(biāo)系:原點為地球中心,xe軸和ye軸在地球赤道平面內(nèi),其中xe軸指向本初子午線,ze軸為地球自轉(zhuǎn)軸,并指向北極,地球坐標(biāo)系為右手直角坐標(biāo)系;

i-地心慣性坐標(biāo):原點為地球中心,xi軸和yi軸在地球赤道平面內(nèi),其中xi軸指向春分點(赤道面與黃道面的交線再與天球相交的交點之一),春分點是天文測量中確定恒星時的起始點,zi軸為地球自轉(zhuǎn)軸,并指向北極.慣性傳感器的輸出就是以該坐標(biāo)系為參考基準(zhǔn)的;

n-導(dǎo)航坐標(biāo)系:原點定義為運載體的重心,xn軸指向地理東向,yn軸指向地理北向,zn軸垂直于當(dāng)?shù)匦D(zhuǎn)橢球面指向天向.

BDS多天線方法解算饋源艙姿態(tài)時,先要解算b系到f系的旋轉(zhuǎn)矩陣,然后由旋轉(zhuǎn)矩陣求解出航向角ψ,俯仰角θ,橫滾角γ.e系、i系、b系和n系用于求解SINS的位置,速度和姿態(tài)更新過程.對比f系和n系,可見他們的3軸指向是一樣的,只是原點不同,所以可認(rèn)為f系和n系是平移關(guān)系,所以BDS多天線饋源艙姿態(tài)解算的等于SINS姿態(tài)更新求解的.

2.2 空間姿態(tài)旋轉(zhuǎn)矩陣解算

設(shè)f系坐標(biāo)與b系坐標(biāo)變換關(guān)系如(1)式,式中,Xf、Yf、Zf是BDS接收機(jī)在FAST坐標(biāo)系(f系)的坐標(biāo),xb、yb、zb是BDS接收機(jī)在饋源艙坐標(biāo)系(b系)的坐標(biāo),是空間姿態(tài)旋轉(zhuǎn)矩陣,?Xf、?Yf、?Zf是平移量.

(4)式中含有a、b、c和?Xf、?Yf、?Zf6個未知數(shù),但饋源艙上分布3個北斗接收機(jī),可以列9個方程式,按最小二乘擬合原則,當(dāng)未知數(shù)個數(shù)小于方程式個數(shù)時會引入殘差量,(4)式的右邊分別引入殘差量εx、εy、εz,設(shè)殘差向量為V.設(shè)擬合參數(shù)=[a;b;c;d;e;f],其中d=??Xf?c?Yf?b?Zf,e=c?Xf??Yf?a?Zf,f=b?Xf+a?Yf??Zf.設(shè)常數(shù)項L=[xb?Xf;yb?Yf;zb?Zf],當(dāng)選用3個北斗天線坐標(biāo)時,L是9×1的向量.設(shè)系數(shù)矩陣A1,1號天線的系數(shù)矩陣如(5)式,A2和A3分別是2號和3號天線的系數(shù),應(yīng)用3個天線時,A是9×6的矩陣.

所以(4)式可以改寫成矩陣形式,如(7)式:

3 BDS/SINS組合導(dǎo)航

BDS/SINS組合導(dǎo)航可以確定運動物體的空間位置、速度和姿態(tài),單臺BDS導(dǎo)航可以確定物體的位置和速度,3臺以上BDS共同導(dǎo)航時,還可以確定物體空間姿態(tài),但衛(wèi)星導(dǎo)航穩(wěn)定性差,會有如衛(wèi)星信號丟失、多路徑效應(yīng)等問題.SINS導(dǎo)航是自主導(dǎo)航系統(tǒng),不受外界干擾,但SINS導(dǎo)航算法是積分過程,初始值誤差和系統(tǒng)誤差都分別被積累,使得導(dǎo)航結(jié)果快速發(fā)散.將兩者結(jié)合使用,可以提高導(dǎo)航的穩(wěn)定性和精確度.

3.1 天線分布

饋源艙是一個正六邊體,在其頂部選擇等間隔的3個頂角分別放置北斗天線,天線構(gòu)型是等邊三角形,邊長約9 m,天線分布俯視圖如圖2.

圖2 北斗天線在饋源艙的分布示意圖Fig.2 Distribution diagram of Beidou’s antenna on the feed cabin

3.2 姿態(tài)卡爾曼濾波方程

卡爾曼濾波是一種最優(yōu)遞推濾波方法,是對系統(tǒng)狀態(tài)的線性最小方差無偏估計.卡爾曼濾波方程包括空間狀態(tài)方程和量測方程,如(9)式所示,

式中,下標(biāo)k表示k時刻,k/k?1表示由k?1時刻向k時刻轉(zhuǎn)移.Xk是k時刻的狀態(tài)向量,Zk是量測向量,?k/k?1是狀態(tài)轉(zhuǎn)移矩陣,Hk是量測矩陣,Γk/k?1是系統(tǒng)噪聲分配矩陣,Wk?1、Vk分別是系統(tǒng)噪聲向量和量測噪聲向量,都是均值為零的高斯白噪聲,且系統(tǒng)噪聲協(xié)方差陣Qk是非負(fù)定的,量測噪聲協(xié)方差陣Rk是正定的.

其中E表示求期望.根據(jù)卡爾曼濾波公式[3],狀態(tài)一步預(yù)測為:

狀態(tài)一步預(yù)測均方誤差陣為:

濾波增益為:

狀態(tài)估計為:

狀態(tài)估計均方誤差陣為:

將(12)式代入(13)式中可知,Qk越小,濾波增益Kk越小,(14)式中的對的貢獻(xiàn)越大,物理意義是系統(tǒng)噪聲越小,系統(tǒng)狀態(tài)在狀態(tài)估計(14)式中占的權(quán)重越大;而Rk越小,Kk越大,(14)式中的Zk對的貢獻(xiàn)越大,即量測噪聲越小,測量向量在估計中占的權(quán)重越大.所以在濾波時,噪聲參數(shù)Qk和Rk的選取很重要,對濾波結(jié)果影響很大.

狀態(tài)向量X是15維的,

式中,?是SINS失準(zhǔn)角,?v是SINS速度誤差,?p是SINS位置誤差,ε是陀螺器件誤差,σ是加速度計器件誤差,下標(biāo)1×3表示矩陣是1行3列.傳統(tǒng)卡爾曼濾波中,量測向量Zk一般選取的是衛(wèi)星導(dǎo)航位置參數(shù)與SINS導(dǎo)航位置參數(shù)的差值和兩者的速度差值.在第1部分,采用北斗多天線方案可以解算出姿態(tài)參數(shù),所以在量測向量中增加衛(wèi)星導(dǎo)航姿態(tài)參數(shù)與SINS導(dǎo)航姿態(tài)參數(shù)的差值,這樣提高導(dǎo)航精度.姿態(tài)卡爾曼濾波中,量測向量Zk的形式如下:

式中下標(biāo)B表示北斗導(dǎo)航數(shù)據(jù),S表示SINS導(dǎo)航數(shù)據(jù),次下標(biāo)E、N、U分別表示東向、北向、天向,v、L、λ和h分別是速度、緯度、經(jīng)度和高度.

4 實驗數(shù)據(jù)分析

實驗數(shù)據(jù)為2020年9月16日北京時間14:11–14:15期間望遠(yuǎn)鏡為跟蹤觀測模式時,BDS接收機(jī)和SINS內(nèi)加速度計、光纖陀螺的實時數(shù)據(jù).將兩者數(shù)據(jù)用于組合導(dǎo)航解算,求解出饋源艙實時姿態(tài)和位置,然后與理論規(guī)劃數(shù)據(jù)進(jìn)行對比,獲得Rmse(Root Mean Squared Error)值.組合導(dǎo)航中,采用兩種濾波方法,方法1是量測向量Zk只包括BDS和SINS的速度差值和位置差值的傳統(tǒng)濾波;方法2是姿態(tài)Kalman濾波,量測向量Zk如(17)式所示,增加了兩者的姿態(tài)差值.兩種方法的對比結(jié)果如圖3–8所示,其中圖3–5顯示的是兩種濾波方法下饋源艙的實際導(dǎo)航姿態(tài)與理論規(guī)劃姿態(tài)的誤差值,圖3是姿態(tài)中橫滾角的誤差值,圖4是姿態(tài)中俯仰角的誤差值,圖5是姿態(tài)中航向角的誤差值;圖6–8顯示的是兩種濾波方法下饋源艙的實際導(dǎo)航位置與理論規(guī)劃位置的誤差值,圖6–8分別是xf、yf、zf軸位置的誤差值.圖中的紅線表示姿態(tài)濾波方法的導(dǎo)航結(jié)果與理論規(guī)劃數(shù)據(jù)的誤差值,藍(lán)線表示傳統(tǒng)濾波方法的導(dǎo)航結(jié)果與理論規(guī)劃數(shù)據(jù)的誤差值.圖例的avp中a表示BDS與SINS的姿態(tài)(attitude)差值,v表示BDS與SINS的速度(velocity)差值,p表示BDS與SINS的位置(position)差值.

圖3 兩種濾波方式的橫滾角誤差值對比圖Fig.3 Error value of roll angle with different filters

圖3中紅線表示姿態(tài)濾波方法解算下,饋源艙實際橫滾角與理論橫滾角的誤差值,藍(lán)線是傳統(tǒng)濾波方法下,實際與理論俯仰角的誤差值.兩種濾波方法的誤差值穩(wěn)定性情況相似,誤差數(shù)據(jù)波動幅值也相似,但姿態(tài)濾波方法的誤差均值的絕對值小于傳統(tǒng)濾波方法,表明姿態(tài)濾波方法的橫滾角精度更高.圖4是俯仰角誤差對比圖,結(jié)論與圖3相似.

圖5是航向角誤差對比圖,在組合導(dǎo)航中航向角的最優(yōu)估計是最難的,因為航向角不準(zhǔn)確所引起的誤差量級遠(yuǎn)小于俯仰角和橫滾角引起的誤差量級,所以在濾波中容易被視為系統(tǒng)誤差而濾除,導(dǎo)致航向角很難通過傳統(tǒng)濾波方法進(jìn)行優(yōu)化.從圖中可以看出姿態(tài)濾波方法的誤差值區(qū)間明顯更小且均值更小,姿態(tài)濾波的航向角誤差均值是0.020°,傳統(tǒng)濾波的航向角誤差均值是0.028°.在均方根誤差值上,姿態(tài)濾波的優(yōu)勢更明顯,Rmse值由傳統(tǒng)濾波的0.0035°,下降到0.0017°,表明姿態(tài)濾波可以使航向角精度穩(wěn)定性提高一倍.

圖5 兩種濾波方式的航向角誤差值對比圖Fig.5 Error value of yaw angle with different filters

圖6 兩種濾波方式在xf軸位置誤差對比圖Fig.6 Error value of position in xf axis with different filters

圖7 兩種濾波方式在yf軸位置誤差對比圖Fig.7 Error value of position in yf axis with different filters

圖6–8是兩種濾波方法下,導(dǎo)航位置與理論規(guī)劃位置的誤差對比.從圖6可見,姿態(tài)濾波可以使xf軸位置精度有所提高.從圖8可見,在zf軸位置上,姿態(tài)濾波與傳統(tǒng)濾波效果基本相同.從圖7可以看出姿態(tài)濾波對yf軸位置的精度提高明顯,姿態(tài)濾波的yf軸位置的誤差均方根是0.00638 m,傳統(tǒng)濾波的yf軸位置的誤差均方根是0.00736 m,表明姿態(tài)濾波可以使yf軸位置精度提高.

圖8 兩種濾波方式在zf軸位置誤差對比圖Fig.8 Error value of position in zf axis with different filters

5 結(jié)論

采用兩種濾波方法,對獲得的組合導(dǎo)航結(jié)果進(jìn)行分析,將位置和姿態(tài)的Rmse值列入表1中.從表1可見,在傳統(tǒng)卡爾曼濾波的測量向量Zk中增加姿態(tài)差值后,饋源艙組合導(dǎo)航結(jié)果的精度有提高,尤其對于導(dǎo)航中最不容易進(jìn)行最優(yōu)估計的航向角,姿態(tài)濾波可以使航向角的精度提高一倍.此外,yf軸位置精度提高了13%,但增加了姿態(tài)差值的同時,也增加了量測噪聲協(xié)方差陣Rk的維度,增加了濾波器參數(shù)調(diào)節(jié)的工作量.

表1 兩種濾波方法的饋源艙組合導(dǎo)航結(jié)果Rmse值Table 1 The Rmse of the integrated navigation results with the difference filtering methods