黃心昱，侯宏錄

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

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

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

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

因此，為更好解決桿臂誤差對SINS/GNSS組合導(dǎo)航系統(tǒng)對準(zhǔn)精度影響的問題，作者在分析桿臂誤差產(chǎn)生機(jī)理的基礎(chǔ)上，將桿臂誤差及加速度計和陀螺的標(biāo)度因數(shù)誤差引入狀態(tài)變量中，采用UKF非線性濾波算法實現(xiàn)桿臂誤差的在線估計和補(bǔ)償。通過仿真實驗，驗證了UKF 對動基座對準(zhǔn)桿臂誤差抑制的有效性，實現(xiàn)快速、高精度的陸地組合對準(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 幾何中心相對于載體幾何中心的矢量為ROI，GNSS 接收機(jī)天線相位中心相對于載體幾何中心的矢量為ROG，接收機(jī)相位中心相對于慣組的矢量為δR，三者之間的矢量關(guān)系滿足：

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

則桿臂加速度為

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

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

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

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

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

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

2 無跡卡爾曼濾波算法

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

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

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

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

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

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

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

濾波增益：

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

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

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

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

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

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

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

式中：

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

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

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

4 仿真實驗

以東-北-天地理坐標(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。加速度計的常值漂移為100 μg，GNSS 位置精度為1～2 m。假設(shè)桿臂誤差沿載體坐標(biāo)系3 個軸方向的值分別為δR=［1 2 3］m，精對準(zhǔn)仿真時間為320 s。

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

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

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

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

圖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ǔ)償前的角度隨時間變化波動大、精度低，且航向角為大失準(zhǔn)角。

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

圖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)過UKF 補(bǔ)償桿臂誤差后有效抑制了組合導(dǎo)航系統(tǒng)的俯仰角、橫滾角和航向角隨時間變化的漂移程度，提高了組合導(dǎo)航系統(tǒng)的測姿精度。

如表1所示為UKF 桿臂誤差補(bǔ)償前和補(bǔ)償后組合導(dǎo)航系統(tǒng)姿態(tài)角誤差的一組對比數(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)過桿臂補(bǔ)償后組合導(dǎo)航系統(tǒng)姿態(tài)角誤差相較于補(bǔ)償前誤差減小了，且航向角的大失準(zhǔn)角誤差變化最為明顯，達(dá)到了兩個數(shù)量級的變化。

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

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

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

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

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

圖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 個軸向的速度誤差在桿臂誤差補(bǔ)償前波動大，收斂慢，而經(jīng)UKF 算法補(bǔ)償后各軸向速度誤差明顯減小且波動小，收斂速度快。

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

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

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

圖15 桿臂誤差對天向位置誤差的影響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 桿臂誤差對北向位置誤差的影響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ǔ)償前有顯著減小，表明外桿臂誤差對組合導(dǎo)航系統(tǒng)的動基座對準(zhǔn)精度有著不可忽略的影響和UKF 算法對動基座下組合導(dǎo)航系統(tǒng)桿臂誤差補(bǔ)償?shù)挠行浴?/p>

5 結(jié)語

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