動(dòng)基座下SINS/GNSS組合導(dǎo)航系統(tǒng)桿臂誤差補(bǔ)償

黃心昱，侯宏錄

（1.西安工業(yè)大學(xué) 光電工程學(xué)院，西安710021；2.西安中科華芯測(cè)控有限公司，西安710300）

慣導(dǎo)系統(tǒng)是制導(dǎo)武器系統(tǒng)的核心[1]。以陸戰(zhàn)武器裝備，例如：坦克、裝甲車等為應(yīng)用背景，針對(duì)其在作戰(zhàn)過(guò)程中的動(dòng)態(tài)變化需要外部導(dǎo)航設(shè)備的輔助進(jìn)行對(duì)準(zhǔn)的問(wèn)題展開動(dòng)基座下SINS/GNSS 組合導(dǎo)航系統(tǒng)桿臂誤差補(bǔ)償?shù)难芯俊?/p>

SINS 是自主導(dǎo)航，但SINS 的導(dǎo)航誤差會(huì)隨時(shí)間累積，難以單獨(dú)進(jìn)行長(zhǎng)時(shí)間的高精度導(dǎo)航[2]。而GNSS 可以為海、陸、空、天的各種用戶提供高精度的導(dǎo)航信息，誤差不隨時(shí)間累積。但GNSS 在地面復(fù)雜環(huán)境中的信號(hào)易受遮擋。因此，兩者優(yōu)勢(shì)互補(bǔ)組合形成SINS/GNSS組合導(dǎo)航系統(tǒng)，提供高性能的導(dǎo)航服務(wù)[2]。但SINS/GNSS組合導(dǎo)航系統(tǒng)中GNSS天線和SINS 的IMU 幾何中心不可能安裝在同一位置，因此由GNSS 測(cè)得的導(dǎo)航信息和由IMU 測(cè)得的信息所對(duì)應(yīng)的點(diǎn)是不同的，從而產(chǎn)生外桿臂效應(yīng)，進(jìn)而影響到組合導(dǎo)航系統(tǒng)中量測(cè)信息的準(zhǔn)確性。

針對(duì)上述問(wèn)題，文獻(xiàn)[3]將SINS/GNSS組合導(dǎo)航系統(tǒng)的桿臂誤差擴(kuò)展到狀態(tài)向量里，采用一般卡爾曼濾波建模實(shí)現(xiàn)桿臂誤差的在線估計(jì)和補(bǔ)償。文獻(xiàn)[4]將慣導(dǎo)內(nèi)部的加速度計(jì)誤差和陀螺漂移擴(kuò)入狀態(tài)變量中再采用一般卡爾曼濾波方法建模實(shí)現(xiàn)桿臂誤差的在線估計(jì)和補(bǔ)償。但動(dòng)態(tài)情況下，慣導(dǎo)輸出的航向角為大失準(zhǔn)角，非線性強(qiáng)，一般卡爾曼濾波方法不足以解決這一問(wèn)題。目前涌現(xiàn)出解決強(qiáng)非線性的濾波方法主要集中在UKF 和容積卡爾曼濾波（CKF）。因此文獻(xiàn)[5]應(yīng)用5 階CKF 進(jìn)行載體行進(jìn)間對(duì)準(zhǔn)桿臂誤差的實(shí)時(shí)估計(jì)和補(bǔ)償。但CKF 濾波算法對(duì)角運(yùn)動(dòng)不敏感[6]，使得導(dǎo)航系統(tǒng)行進(jìn)間對(duì)準(zhǔn)過(guò)程中產(chǎn)生角運(yùn)動(dòng)時(shí)的桿臂誤差不能得到有效補(bǔ)償。

因此，為更好解決桿臂誤差對(duì)SINS/GNSS組合導(dǎo)航系統(tǒng)對(duì)準(zhǔn)精度影響的問(wèn)題，作者在分析桿臂誤差產(chǎn)生機(jī)理的基礎(chǔ)上，將桿臂誤差及加速度計(jì)和陀螺的標(biāo)度因數(shù)誤差引入狀態(tài)變量中，采用UKF非線性濾波算法實(shí)現(xiàn)桿臂誤差的在線估計(jì)和補(bǔ)償。通過(guò)仿真實(shí)驗(yàn)，驗(yàn)證了UKF 對(duì)動(dòng)基座對(duì)準(zhǔn)桿臂誤差抑制的有效性，實(shí)現(xiàn)快速、高精度的陸地組合對(duì)準(zhǔn)技術(shù)，提升陸戰(zhàn)武器裝備的作戰(zhàn)性能和靈活自主性。

1 SINS/GNSS組合導(dǎo)航系統(tǒng)桿臂效應(yīng)機(jī)理

假設(shè)SINS/GNSS組合導(dǎo)航系統(tǒng)的安裝位置如圖1所示。

圖1 組合導(dǎo)航安裝方式示意圖Fig.1 Integrated navigation installation schematic diagram

假設(shè)慣導(dǎo)中的IMU 幾何中心相對(duì)于載體幾何中心的矢量為ROI，GNSS 接收機(jī)天線相位中心相對(duì)于載體幾何中心的矢量為ROG，接收機(jī)相位中心相對(duì)于慣組的矢量為δR，三者之間的矢量關(guān)系滿足：

根據(jù)絕對(duì)加速度推導(dǎo)原理得：

則桿臂加速度為

考慮一般的導(dǎo)航解算均是在導(dǎo)航坐標(biāo)系下進(jìn)行，將桿臂加速度推算到導(dǎo)航坐標(biāo)系中得：

再通過(guò)積分推導(dǎo)，得到關(guān)于桿臂效應(yīng)的速度和位置的表達(dá)式為

由式（6）可知，組合導(dǎo)航系統(tǒng)的桿臂誤差會(huì)影響速度和位置精度的測(cè)量。

在捷聯(lián)慣導(dǎo)系統(tǒng)中，角速度信息是由捷聯(lián)陀螺提供的[7]，因此按如下公式計(jì)算：

又本文以東-北-天地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，因此：

式中：VN，VE，L為載體實(shí)時(shí)運(yùn)動(dòng)過(guò)程中導(dǎo)航更新的值；RN為卯酉圈曲率半徑；RM為子午圈曲率半徑。

2 無(wú)跡卡爾曼濾波算法

當(dāng)載體處于動(dòng)基座下，系統(tǒng)輸出航向角為大失準(zhǔn)角，具有強(qiáng)非線性，因此采用UKF 濾波算法進(jìn)行建模分析和處理，降低導(dǎo)航誤差的發(fā)散度。

UKF 濾波是無(wú)跡變換UT 與標(biāo)準(zhǔn)卡爾曼濾波的結(jié)合，可以提高非線性系統(tǒng)濾波的精度和效率，系統(tǒng)非線性越強(qiáng)，UKF 濾波的優(yōu)勢(shì)越顯著[8]。UT 變換是構(gòu)造一組Sigma 采樣點(diǎn)來(lái)近似系統(tǒng)狀態(tài)的先驗(yàn)統(tǒng)計(jì)特性，其詳細(xì)推導(dǎo)過(guò)程可參考文獻(xiàn)[8]。

對(duì)于隨機(jī)向量X 和一個(gè)非線性變換Y=f（X），以及另一個(gè)非線性變換Z＝h（X），互協(xié)方差Pzy的計(jì)算用函數(shù)表示UTc：

對(duì)n維離散的非線性系統(tǒng)，其非線性動(dòng)態(tài)系統(tǒng)模型如式（10）所示：

式中：Xk和Xk-1分別為k時(shí)刻和k-1 時(shí)刻的系統(tǒng)狀態(tài)向量；Zk為系統(tǒng)觀測(cè)向量；f為狀態(tài)轉(zhuǎn)移函數(shù)；h為系統(tǒng)觀測(cè)函數(shù)；Uk為輸入控制；wk為服從高斯分布的過(guò)程噪聲，其均值為0，協(xié)方差矩陣為Qk；vk為服從高斯分布的觀測(cè)噪聲，其均值為0，協(xié)方差矩陣為Rk。

UKF 濾波在誤差協(xié)方差矩陣預(yù)測(cè)和更新時(shí)都用無(wú)跡變換來(lái)計(jì)算[8]，公式如式（11）～式（14）所示。

狀態(tài)估計(jì)預(yù)報(bào)、誤差協(xié)方差矩陣預(yù)報(bào)：

濾波增益：

狀態(tài)估計(jì)更新：

誤差協(xié)方差矩陣更新：

3 桿臂補(bǔ)償濾波模型建立

SINS 的加速度計(jì)和陀螺的標(biāo)度因數(shù)誤差同樣會(huì)對(duì)導(dǎo)航精度產(chǎn)生影響，因此在桿臂誤差的實(shí)時(shí)在線補(bǔ)償非線性濾波模型中必須加入這些誤差參數(shù)。

計(jì)算導(dǎo)航坐標(biāo)系與實(shí)際導(dǎo)航坐標(biāo)系之間的誤差角為Φ＝（ΦE，ΦN，ΦU），轉(zhuǎn)換矩陣為導(dǎo)航坐標(biāo)系的速度誤差為，航位坐標(biāo)系速度誤差表示為，位置誤差為航位推算位置誤差為。假設(shè)航位坐標(biāo)系和載體坐標(biāo)系重合，則GNSS 的誤差主要是刻度系數(shù)誤差δK，加速度計(jì)標(biāo)度因數(shù)誤差為δKAx、δKAy，陀螺的標(biāo)度因數(shù)誤差δKGz，加速度計(jì)的常值零偏和白噪聲分別為。陀螺的常值漂移和白噪聲分別為

建立系統(tǒng)的誤差方程為

綜上所述，帕金森病患者病恥感的研究尚處于探索階段，角度局限且不夠成熟，今后應(yīng)考慮更多的質(zhì)性和量性研究。因此，本研究后續(xù)將用信效度良好的量表調(diào)查帕金森病患者病恥感的現(xiàn)狀，并分析其影響因素，為行之有效的干預(yù)方法提供理論依據(jù)。

式中：

取系統(tǒng)濾波器狀態(tài)向量：

式中：φn為姿態(tài)角，且。

根據(jù)SINS 實(shí)測(cè)位置和GNSS 實(shí)測(cè)位置的差值構(gòu)造量測(cè)方程：

4 仿真實(shí)驗(yàn)

以東-北-天地理坐標(biāo)系為導(dǎo)航坐標(biāo)系。載體初始位置坐標(biāo)為（L：34.1446°，λ：108.5434°，H：380 m）。采用的激光陀螺儀常值漂移為0.01°/h，隨機(jī)漂移為0.001°/h，標(biāo)度因數(shù)誤差為4 PPM。加速度計(jì)的常值漂移為100 μg，GNSS 位置精度為1～2 m。假設(shè)桿臂誤差沿載體坐標(biāo)系3 個(gè)軸方向的值分別為δR=［1 2 3］m，精對(duì)準(zhǔn)仿真時(shí)間為320 s。

如圖2所示為補(bǔ)償前桿臂長(zhǎng)度估計(jì)值，圖3 為補(bǔ)償后桿臂長(zhǎng)度。

圖2 補(bǔ)償桿臂誤差前的桿臂長(zhǎng)度示意圖Fig.2 Diagram of arm length before compensating lever arm error

圖3 補(bǔ)償桿臂誤差后的桿臂長(zhǎng)度示意圖Fig.3 Diagram of the length of the arm after compensating the error of the lever arm

圖4 和圖5 是補(bǔ)償前組合導(dǎo)航系統(tǒng)的俯仰角、橫滾角和航向角隨時(shí)間的變化。

圖4 補(bǔ)償桿臂誤差前的系統(tǒng)俯仰角和橫滾角Fig.4 System pitch angle and roll angle before compensating lever arm error

圖5 補(bǔ)償桿臂誤差前的系統(tǒng)航向角Fig.5 System heading angle before compensating lever arm error

由圖4 和圖5 可以看出補(bǔ)償前的角度隨時(shí)間變化波動(dòng)大、精度低，且航向角為大失準(zhǔn)角。

圖6 和圖7 是補(bǔ)償后組合導(dǎo)航系統(tǒng)的俯仰角、橫滾角和航向角隨時(shí)間的變化。

圖6 補(bǔ)償桿臂誤差后的系統(tǒng)俯仰角和橫滾角Fig.6 System pitch angle and roll angle after compensating the error of the lever arm

圖7 補(bǔ)償桿臂誤差后的系統(tǒng)航向角Fig.7 Heading angle of the system after compensating the error of the lever arm

由圖6 和圖7 可以看出經(jīng)過(guò)UKF 補(bǔ)償桿臂誤差后有效抑制了組合導(dǎo)航系統(tǒng)的俯仰角、橫滾角和航向角隨時(shí)間變化的漂移程度，提高了組合導(dǎo)航系統(tǒng)的測(cè)姿精度。

如表1所示為UKF 桿臂誤差補(bǔ)償前和補(bǔ)償后組合導(dǎo)航系統(tǒng)姿態(tài)角誤差的一組對(duì)比數(shù)據(jù)。

表1 桿臂誤差補(bǔ)償前和補(bǔ)償后的組合導(dǎo)航系統(tǒng)姿態(tài)角誤差Tab.1 Attitude angle error of integrated navigation system before and after bar arm error compensation

由表1 中的補(bǔ)償后姿態(tài)角數(shù)據(jù)可以看出，經(jīng)過(guò)桿臂補(bǔ)償后組合導(dǎo)航系統(tǒng)姿態(tài)角誤差相較于補(bǔ)償前誤差減小了，且航向角的大失準(zhǔn)角誤差變化最為明顯，達(dá)到了兩個(gè)數(shù)量級(jí)的變化。

如圖8所示為載體完整的行駛軌跡，圖9 為軌跡的局部放大圖。

圖8 載體運(yùn)動(dòng)軌跡Fig.8 Carrier trajectory

圖9 載體運(yùn)動(dòng)軌跡局部放大圖Fig.9 Locally enlarged view of the trajectory of the carrier

由圖8 和圖9 可以看出，經(jīng)過(guò)UKF 補(bǔ)償桿臂誤差后組合導(dǎo)航系統(tǒng)無(wú)論在加減速直線運(yùn)動(dòng)過(guò)程中還是轉(zhuǎn)彎運(yùn)動(dòng)過(guò)程中的位置精度均有顯著提升。

如圖10、圖11 和圖12所示分別為UKF 桿臂誤差補(bǔ)償前和UKF 桿臂補(bǔ)償補(bǔ)償后組合導(dǎo)航系統(tǒng)東-北-天3 個(gè)方向的速度誤差仿真圖。

圖10 桿臂誤差補(bǔ)償前和補(bǔ)償后系統(tǒng)東向速度誤差Fig.10 Lever arm error compensation before and after compensation system eastward velocity error

圖11 桿臂誤差補(bǔ)償前和補(bǔ)償后系統(tǒng)北向速度誤差Fig.11 Lever arm error compensation before and after compensation system northward velocity error

圖12 桿臂誤差補(bǔ)償前和補(bǔ)償后系統(tǒng)天向速度誤差Fig.12 The radial velocity error of the system is compensated before and after the lever error compensation

由圖10～圖12 可以看出，組合導(dǎo)航系統(tǒng)中3 個(gè)軸向的速度誤差在桿臂誤差補(bǔ)償前波動(dòng)大，收斂慢，而經(jīng)UKF 算法補(bǔ)償后各軸向速度誤差明顯減小且波動(dòng)小，收斂速度快。

圖13～圖15 分別為桿臂誤差補(bǔ)償前和補(bǔ)償后組合導(dǎo)航系統(tǒng)東-北-天方向的位置誤差仿真圖。

由圖13～圖15 可以看出，未補(bǔ)償桿臂誤差前，3個(gè)方向的位置誤差波動(dòng)大，收斂慢。而經(jīng)桿臂補(bǔ)償后的3 個(gè)方向位置誤差得到了抑制，波動(dòng)小，收斂快。

圖13 桿臂誤差對(duì)東向位置誤差的影響Fig.13 Influence of the lever arm error on the eastward position error

圖15 桿臂誤差對(duì)天向位置誤差的影響Fig.15 Influence of the lever arm error on the sky position error

如表2所示為UKF 桿臂誤差補(bǔ)償前和UKF 桿臂誤差補(bǔ)償后組合導(dǎo)航系統(tǒng)速度誤差和位置誤差的方差數(shù)據(jù)。

圖14 桿臂誤差對(duì)北向位置誤差的影響Fig.14 Influence of lever arm error on the northward position error

表2 桿臂誤差補(bǔ)償前與補(bǔ)償后系統(tǒng)速度和位置誤差的均方根誤差Tab.2 Root mean square error of the system velocity and position error before and after the lever arm error compensation

由表2 可以看出，桿臂誤差補(bǔ)償后相較補(bǔ)償前的速度和位置誤差的均方根誤差相較補(bǔ)償前有顯著減小，表明外桿臂誤差對(duì)組合導(dǎo)航系統(tǒng)的動(dòng)基座對(duì)準(zhǔn)精度有著不可忽略的影響和UKF 算法對(duì)動(dòng)基座下組合導(dǎo)航系統(tǒng)桿臂誤差補(bǔ)償?shù)挠行浴?/p>

5 結(jié)語(yǔ)

在動(dòng)基座對(duì)準(zhǔn)過(guò)程中，桿臂誤差對(duì)組合導(dǎo)航系統(tǒng)的姿態(tài)、速度和位置精度均有顯著影響，本文采用的UKF 濾波算法是將桿臂誤差以及陀螺儀和加速度計(jì)的標(biāo)度因數(shù)誤差列入狀態(tài)變量中，從而使得桿臂長(zhǎng)度可以實(shí)時(shí)估計(jì)和補(bǔ)償，并進(jìn)一步減小標(biāo)度因數(shù)誤差影響。仿真實(shí)驗(yàn)表明，通過(guò)UKF 濾波算法進(jìn)行桿臂誤差補(bǔ)償后相較補(bǔ)償前速度誤差的均方根誤差最大減小了0.0067 m/s，位置誤差的均方根誤差最大減小了0.7786 m，有效提升了SINS/GNSS組合導(dǎo)航系統(tǒng)的動(dòng)基座的對(duì)準(zhǔn)精度。