組合導航載體軌跡發生器的設計與實現

季云健

（杭州萬向職業技術學院，浙江 杭州 31000）

0 引言

隨著衛星導航系統和傳感技術的發展，低價捷慣性導航得到廣泛應用[1]。通過實地測量獲取慣導元件輸出數據，受試驗周期、成本、環境等多種因素影響，難度較大[2-3]。為避免實地測量周期長、成本高、易受干擾等，越來越多的學者通過建立飛行軌跡模型來獲取慣導元件的輸出數據[4]。軌跡發生器是慣性導航系統進行算法驗證和仿真的基礎，用于產生仿真所需要的傳感器數據和BDS參數[5]。

本文設計一種通過建立控制方程計算飛行器飛行狀態，生成理想軌跡的軌跡發生器，對組合導航系統中慣性測量單元（IMU）[6]、BDS位置和速度等多類型傳感器數據生成機理進行分析,設計了捷聯慣性組合導航軌跡發生器。利用軌跡發生器生成的仿真數據進行組合導航解算,導航結果驗證了所設計的軌跡發生器的正確性和有效性。

1 基本理論

1.1 載體軌跡發生器原理

軌跡發生器用于產生模擬場景中各個時刻載體的導航參數:位置、速度、姿態、比力加速度積分增量和角速度積分增量[7]。加速度增量和角速度增量的估計是設計軌跡發生器的的關鍵。為了實現誤差控制，本文計算飛行器飛行狀態，生成理想軌跡的軌跡發生器。具體步驟如下。

1）直線運動（靜止/勻速）階段，俯仰角、航向角、橫滾角、加速度為0。

2）加速減速運動階段，飛行器姿態角不變，三方向加速度任取一個常值。

3）上升/下降運動階段，載體俯仰角以等角速度增大。

4）沿軸滾準運動階段，載體俯仰角和航向角不變，橫滾角以等速度遞加，三方向加速度不變。

5）轉彎運動階段，載體航向角以等角速度增大。

1.2 標準SRCKF算法

標準SRCKF算法包括初始化、時間更新和量測更新三個步驟[8]：

1）初始化

假設n維系統狀態x的初始概率分布滿足

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

2）時間更新（預測過程）

計算k- 1時刻的容積點Xk-1|k-1：

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

將容積點代入非線性系統方程，計算預測容積點Xi,k|k-1和預測值：

更新狀態協方差矩陣平方根

其中，SQ,k-1為系統噪聲協方差矩陣Qk-1的平方根，即滿足為矩陣三角化算法，一般使用QR 分解實現矩陣三角化的表達式如下：

3）量測更新

計算k時刻的預測容積點Xk|k-1：

將預測容積點代入非線性觀測方程，計算觀測量的預測值

計算觀測量預測值協方差矩陣平方根

其中，SR,k為系統觀測量協方差矩陣Rk的平方根，即滿足Rk=SR,k·，可以仿照使用楚列斯基分解計算SR,k，Zk|k-1的表達式如下：

計算濾波器增益矩陣Kk：

使用觀測值zk完成狀態量測更新

更新狀態協方差矩陣平方根

1.3 載體軌跡發生器

假設載體為某飛行器，仿真過程包含了飛行器從靜止到滑跑、上升、轉彎和平飛等全過程，將仿真時間設定為1小時，飛行器的運動狀態設計如表1所示。

表1 飛行器運動狀態及相關參數設計

根據上述飛行器狀態設計，生成的飛行器姿態角速率、姿態角、加速度和速度如圖1所示。

圖1 飛行器姿態角速率、姿態角、加速度和速度的生成

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

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

1.4 理想慣性傳感器數據

根據設計的飛行器姿態角速率、姿態角、加速度和速度，生成理想的慣性傳感器數據。慣性傳感器數據[9]包括三個陀螺和三個加速度計。

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

的方向余弦矩陣為

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

載體坐標系b相對于導航坐標系n的旋轉角速度計算公式如下

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

陀螺輸出的角速度為載體系相對于慣性空間的轉動角速度在載體坐標系下的投影，其計算方法為

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

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

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

1.5 含有誤差的慣性傳感器數據生成

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

固定誤差項包括：零偏項、標度因數誤差和慣性傳感器安裝誤差。固定誤差項設置如表2所示。

表2 固定誤差項參數設置

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

表3 隨機誤差參數設置

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

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

1.6 BDS仿真數據生成

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

表4 BDS的誤差參數設置（0.01m/s）

表5 BDS位置粗差（50m）

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

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

2 仿真驗證及分析

為驗證軌跡發生器的正確性，將以CKF為模型得到標準CKF組合導航結果，組合導航與飛行過程真實軌跡對比如圖6所示。

圖6 標準CKF導航軌跡圖

圖7 未加入誤差卡爾曼濾波組合導航緯度、經度、高度、位置誤差圖

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

圖8 加入誤差下卡爾曼濾波組合導航緯度、經度、高度、位置誤差圖

由上述實驗結果可以看到，從第800秒（0.22h）開始加入BDS 位置粗差，至900 秒（0.25h）撤銷粗差，組合導航定位精度受到了嚴重的影響。水平位置精度在粗差撤銷后180秒回歸正常水平，而高度精度在粗差撤銷后越1600秒才回歸正常水平，該現象與慣性導航系統的天向通道本身不穩定的特性有關。

3 結束語

本文利用軌跡發生器產生的軌跡數據和導航算法進行了組合驗證，結果表明了本文設計的有效性。但當BDS位置觀測量中含有粗差時，給組合導航精度帶來了不可接受的顯著影響，因此必須設法使組合導航濾波器具有一定的抗差自適應能力。為解決這一問題，接下來將深入研究抗差自適應容積卡爾曼濾波器。