季云健

（杭州萬(wàn)向職業(yè)技術(shù)學(xué)院，浙江 杭州 31000）

0 引言

隨著衛(wèi)星導(dǎo)航系統(tǒng)和傳感技術(shù)的發(fā)展，低價(jià)捷慣性導(dǎo)航得到廣泛應(yīng)用[1]。通過(guò)實(shí)地測(cè)量獲取慣導(dǎo)元件輸出數(shù)據(jù)，受試驗(yàn)周期、成本、環(huán)境等多種因素影響，難度較大[2-3]。為避免實(shí)地測(cè)量周期長(zhǎng)、成本高、易受干擾等，越來(lái)越多的學(xué)者通過(guò)建立飛行軌跡模型來(lái)獲取慣導(dǎo)元件的輸出數(shù)據(jù)[4]。軌跡發(fā)生器是慣性導(dǎo)航系統(tǒng)進(jìn)行算法驗(yàn)證和仿真的基礎(chǔ)，用于產(chǎn)生仿真所需要的傳感器數(shù)據(jù)和BDS參數(shù)[5]。

本文設(shè)計(jì)一種通過(guò)建立控制方程計(jì)算飛行器飛行狀態(tài)，生成理想軌跡的軌跡發(fā)生器，對(duì)組合導(dǎo)航系統(tǒng)中慣性測(cè)量單元（IMU）[6]、BDS位置和速度等多類型傳感器數(shù)據(jù)生成機(jī)理進(jìn)行分析,設(shè)計(jì)了捷聯(lián)慣性組合導(dǎo)航軌跡發(fā)生器。利用軌跡發(fā)生器生成的仿真數(shù)據(jù)進(jìn)行組合導(dǎo)航解算,導(dǎo)航結(jié)果驗(yàn)證了所設(shè)計(jì)的軌跡發(fā)生器的正確性和有效性。

1 基本理論

1.1 載體軌跡發(fā)生器原理

軌跡發(fā)生器用于產(chǎn)生模擬場(chǎng)景中各個(gè)時(shí)刻載體的導(dǎo)航參數(shù):位置、速度、姿態(tài)、比力加速度積分增量和角速度積分增量[7]。加速度增量和角速度增量的估計(jì)是設(shè)計(jì)軌跡發(fā)生器的的關(guān)鍵。為了實(shí)現(xiàn)誤差控制，本文計(jì)算飛行器飛行狀態(tài)，生成理想軌跡的軌跡發(fā)生器。具體步驟如下。

1）直線運(yùn)動(dòng)（靜止/勻速）階段，俯仰角、航向角、橫滾角、加速度為0。

2）加速減速運(yùn)動(dòng)階段，飛行器姿態(tài)角不變，三方向加速度任取一個(gè)常值。

3）上升/下降運(yùn)動(dòng)階段，載體俯仰角以等角速度增大。

4）沿軸滾準(zhǔn)運(yùn)動(dòng)階段，載體俯仰角和航向角不變，橫滾角以等速度遞加，三方向加速度不變。

5）轉(zhuǎn)彎運(yùn)動(dòng)階段，載體航向角以等角速度增大。

1.2 標(biāo)準(zhǔn)SRCKF算法

標(biāo)準(zhǔn)SRCKF算法包括初始化、時(shí)間更新和量測(cè)更新三個(gè)步驟[8]：

1）初始化

假設(shè)n維系統(tǒng)狀態(tài)x的初始概率分布滿足

其中，S0是初始協(xié)方差矩陣P0的平方根，通過(guò)楚列斯基分解chol(· )計(jì)算得到，需要注意的是，在SRCKF 中得到不斷更新的協(xié)方差矩陣是Sk|k，而在CKF中不斷更新的協(xié)方差矩陣是Pk|k。

2）時(shí)間更新（預(yù)測(cè)過(guò)程）

計(jì)算k- 1時(shí)刻的容積點(diǎn)Xk-1|k-1：

其中，[ 1]i為n維向量，是n×n維單位矩陣In×n的第i列。

將容積點(diǎn)代入非線性系統(tǒng)方程，計(jì)算預(yù)測(cè)容積點(diǎn)Xi,k|k-1和預(yù)測(cè)值：

更新狀態(tài)協(xié)方差矩陣平方根

其中，SQ,k-1為系統(tǒng)噪聲協(xié)方差矩陣Qk-1的平方根，即滿足為矩陣三角化算法，一般使用QR 分解實(shí)現(xiàn)矩陣三角化的表達(dá)式如下：

3）量測(cè)更新

計(jì)算k時(shí)刻的預(yù)測(cè)容積點(diǎn)Xk|k-1：

將預(yù)測(cè)容積點(diǎn)代入非線性觀測(cè)方程，計(jì)算觀測(cè)量的預(yù)測(cè)值

計(jì)算觀測(cè)量預(yù)測(cè)值協(xié)方差矩陣平方根

其中，SR,k為系統(tǒng)觀測(cè)量協(xié)方差矩陣Rk的平方根，即滿足Rk=SR,k·，可以仿照使用楚列斯基分解計(jì)算SR,k，Zk|k-1的表達(dá)式如下：

計(jì)算濾波器增益矩陣Kk：

使用觀測(cè)值z(mì)k完成狀態(tài)量測(cè)更新

更新狀態(tài)協(xié)方差矩陣平方根

1.3 載體軌跡發(fā)生器

假設(shè)載體為某飛行器，仿真過(guò)程包含了飛行器從靜止到滑跑、上升、轉(zhuǎn)彎和平飛等全過(guò)程，將仿真時(shí)間設(shè)定為1小時(shí)，飛行器的運(yùn)動(dòng)狀態(tài)設(shè)計(jì)如表1所示。

表1 飛行器運(yùn)動(dòng)狀態(tài)及相關(guān)參數(shù)設(shè)計(jì)

根據(jù)上述飛行器狀態(tài)設(shè)計(jì)，生成的飛行器姿態(tài)角速率、姿態(tài)角、加速度和速度如圖1所示。

圖1 飛行器姿態(tài)角速率、姿態(tài)角、加速度和速度的生成

圖2 仿真生成的飛行器軌跡圖

圖3 仿真生成的加速度計(jì)輸出和陀螺輸出圖

1.4 理想慣性傳感器數(shù)據(jù)

根據(jù)設(shè)計(jì)的飛行器姿態(tài)角速率、姿態(tài)角、加速度和速度，生成理想的慣性傳感器數(shù)據(jù)。慣性傳感器數(shù)據(jù)[9]包括三個(gè)陀螺和三個(gè)加速度計(jì)。

定義導(dǎo)航坐標(biāo)系n為東-北-天坐標(biāo)系，定義載體坐標(biāo)系b為右-前-上坐標(biāo)系，定義由導(dǎo)航坐標(biāo)系到載體坐標(biāo)系的旋轉(zhuǎn)由三次坐標(biāo)旋轉(zhuǎn)組成，第一次為繞z軸旋轉(zhuǎn)，第二次繞x軸旋轉(zhuǎn)，第三次繞y軸旋轉(zhuǎn)

的方向余弦矩陣為

其中，θ為俯仰角，γ為橫滾角，?為航向角。在軌跡生成中，得到的速度是載體系下的速度，根據(jù)飛行器姿態(tài)和上述的方向余弦矩陣定義，可以將載體系下的速度轉(zhuǎn)換為導(dǎo)航系下的速度，包括北向速度vN、東向速度vE和天向速度vN，對(duì)其積分后可得到載體的緯度L、經(jīng)度λ和高度h。

載體坐標(biāo)系b相對(duì)于導(dǎo)航坐標(biāo)系n的旋轉(zhuǎn)角速度計(jì)算公式如下

其中，dθ/dt，dγ/dt和d?/dt分別表示俯仰角速率、橫滾角速率和航向角速率，矩陣M的定義如下

陀螺輸出的角速度為載體系相對(duì)于慣性空間的轉(zhuǎn)動(dòng)角速度在載體坐標(biāo)系下的投影，其計(jì)算方法為

其中，Ω為地球自轉(zhuǎn)角速率，RN為子午圈半徑，RE為卯酉圈半徑。

將前后兩次載體系下的速度求差并除以仿真補(bǔ)償后，得到載體系下的加速度，加速度計(jì)所測(cè)量的載體系下的比力計(jì)算公式為

其中為地球重力加速度矢量。仿真得到的飛行器軌跡、加速度計(jì)輸出和陀螺輸出如下圖所示：

1.5 含有誤差的慣性傳感器數(shù)據(jù)生成

為了使仿真貼近實(shí)際，需要在生成的慣性傳感器數(shù)據(jù)中加入誤差。向理想傳感器數(shù)據(jù)中加入的誤差包括固定誤差[10]和隨機(jī)誤差[11]。

固定誤差項(xiàng)包括：零偏項(xiàng)、標(biāo)度因數(shù)誤差和慣性傳感器安裝誤差。固定誤差項(xiàng)設(shè)置如表2所示。

表2 固定誤差項(xiàng)參數(shù)設(shè)置

慣性傳感器的隨機(jī)誤差包括：隨機(jī)零偏、一階馬爾可夫噪聲和白噪聲。隨機(jī)誤差參數(shù)設(shè)置如表3所示。

表3 隨機(jī)誤差參數(shù)設(shè)置

仿真生成的慣性傳感器誤差如圖4所示。

圖4 仿真生成的慣性傳感器誤差

1.6 BDS仿真數(shù)據(jù)生成

根據(jù)載體軌跡設(shè)計(jì)中生成的緯度、經(jīng)度和高度、東向速度、北向速度和天向速度，向其中加入誤差得到BDS仿真數(shù)據(jù)，BDS的誤差參數(shù)設(shè)置如表4所示。

表4 BDS的誤差參數(shù)設(shè)置（0.01m/s）

表5 BDS位置粗差（50m）

仿真生成的BDS位置和速度如圖5所示。

圖5 仿真生成的BDS位置和速度圖

2 仿真驗(yàn)證及分析

為驗(yàn)證軌跡發(fā)生器的正確性，將以CKF為模型得到標(biāo)準(zhǔn)CKF組合導(dǎo)航結(jié)果，組合導(dǎo)航與飛行過(guò)程真實(shí)軌跡對(duì)比如圖6所示。

圖6 標(biāo)準(zhǔn)CKF導(dǎo)航軌跡圖

圖7 未加入誤差卡爾曼濾波組合導(dǎo)航緯度、經(jīng)度、高度、位置誤差圖

現(xiàn)在向BDS 位置觀測(cè)量加入粗差，在[800,900]平飛階段，加入100秒位置誤差，如下所示。

圖8 加入誤差下卡爾曼濾波組合導(dǎo)航緯度、經(jīng)度、高度、位置誤差圖

由上述實(shí)驗(yàn)結(jié)果可以看到，從第800秒（0.22h）開始加入BDS 位置粗差，至900 秒（0.25h）撤銷粗差，組合導(dǎo)航定位精度受到了嚴(yán)重的影響。水平位置精度在粗差撤銷后180秒回歸正常水平，而高度精度在粗差撤銷后越1600秒才回歸正常水平，該現(xiàn)象與慣性導(dǎo)航系統(tǒng)的天向通道本身不穩(wěn)定的特性有關(guān)。

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

本文利用軌跡發(fā)生器產(chǎn)生的軌跡數(shù)據(jù)和導(dǎo)航算法進(jìn)行了組合驗(yàn)證，結(jié)果表明了本文設(shè)計(jì)的有效性。但當(dāng)BDS位置觀測(cè)量中含有粗差時(shí)，給組合導(dǎo)航精度帶來(lái)了不可接受的顯著影響，因此必須設(shè)法使組合導(dǎo)航濾波器具有一定的抗差自適應(yīng)能力。為解決這一問(wèn)題，接下來(lái)將深入研究抗差自適應(yīng)容積卡爾曼濾波器。