趙 彤，李 群，蒲小平, 吳亮華，胡奇林

(1.北京自動化控制設(shè)備研究所，北京 100074; 2.空軍裝備部， 北京 100843)

0 引言

陸基機(jī)動平臺及裝備用慣導(dǎo)系統(tǒng)進(jìn)入正常導(dǎo)航前需要具備較高的初始姿態(tài)精度，利用經(jīng)緯度位置信息、重力加速度和地球自轉(zhuǎn)角速度等信息進(jìn)行自主對準(zhǔn)。車載慣導(dǎo)系統(tǒng)[1]初始自對準(zhǔn)一般包括粗對準(zhǔn)和精對準(zhǔn)2個階段，粗對準(zhǔn)根據(jù)陀螺和加表測量信息解算出粗略的初始姿態(tài)陣[2-3];精對準(zhǔn)采用濾波估計方法，根據(jù)慣導(dǎo)系統(tǒng)誤差模型，利用零速信息，通過Kalman濾波器對慣性器件誤差和姿態(tài)誤差角進(jìn)行最優(yōu)估計和校準(zhǔn)，從而得到慣導(dǎo)系統(tǒng)更為精確的初始姿態(tài)。實際工程應(yīng)用中，當(dāng)車輛受到大幅度干擾，如發(fā)動機(jī)啟動、風(fēng)擾和人員走動等情況時，車輛將不再處于絕對零速狀態(tài)，自對準(zhǔn)精度將受到影響。因此，車載慣導(dǎo)自對準(zhǔn)的抗干擾能力是決定其環(huán)境適應(yīng)性和導(dǎo)航精度的關(guān)鍵，需開展深入研究。

除目前廣泛應(yīng)用的濾波估計方法外，確定性姿態(tài)算法[4-6]也是近年來廣泛研究的初始對準(zhǔn)方法。20世紀(jì)60年代以來，G.Wahba提出了利用觀測矢量進(jìn)行星體姿態(tài)確定的最小二乘優(yōu)化問題[7]；Daveport提出了各種Q-method方法[8]，利用四元數(shù)優(yōu)化獲得姿態(tài)陣。

本文對目前常用的慣性系解析自對準(zhǔn)、Kalman濾波估計自對準(zhǔn)和慣性系奇異值分解[9](Singular Value Decomposition，SVD)自對準(zhǔn)等三種方法進(jìn)行了研究和對比分析，根據(jù)三種對準(zhǔn)方法各自的實現(xiàn)原理，提出了一種有針對性的抗擾動優(yōu)化方法，并通過了數(shù)學(xué)仿真分析和車載試驗驗證，最終得出了研究結(jié)果。

1 自對準(zhǔn)方法

常用的初始對準(zhǔn)方法包括解析對準(zhǔn)[10-11]與Kal-man濾波對準(zhǔn)。

1.1 慣性系解析對準(zhǔn)方法

(1)

(2)

(3)

(4)

將式(1)代入式(4)得

(5)

(6)

可表示為

(7)

其中

取2個適當(dāng)間隔的不同時刻，工程上自對準(zhǔn)時間設(shè)定為300s(一般取tl=150s，tm=300s)。

再利用雙矢量定姿算法進(jìn)行矩陣運算，即可求得常值矩陣:

(8)

1.2 Kalman濾波自對準(zhǔn)方法

目前，Kalman濾波技術(shù)應(yīng)用廣泛，是一種線性最小方差估計算法。在慣導(dǎo)系統(tǒng)初始對準(zhǔn)中，通常利用Kalman濾波器對系統(tǒng)的初始姿態(tài)角、速度、位置和慣性器件誤差進(jìn)行建模并估計，通過反饋修正實時對誤差進(jìn)行補(bǔ)償。通常粗對準(zhǔn)獲得的方位角誤差在3°以內(nèi)，可以實現(xiàn)姿態(tài)誤差方程小角度近似,一般采用速度匹配的方式進(jìn)行對準(zhǔn)。

Kalman濾波模型如下

(9)

根據(jù)姿態(tài)、速度和位置誤差方程，列出Kalman濾波12維狀態(tài)變量

X=[δVnδVuδVeφnφuφe

(10)

濾波觀測量為

(11)

1.3 基于SVD的慣性系自對準(zhǔn)方法

給定三維空間中有m個不共面的矢量，在2個直角坐標(biāo)系下的投影分別表示為ri和bi(i=1,2,…,m)，并滿足

bi=Ari+Δbi

(12)

其中，Δbi為量測噪聲。由于存在誤差，所以多矢量定姿的實質(zhì)是找到姿態(tài)矩陣A使測量誤差最小，并滿足最小二乘最優(yōu)估計，構(gòu)造損失函數(shù)

(13)

其中，wi為權(quán)重系數(shù)，一般取wi=1。

損失函數(shù)進(jìn)一步展開可得

(14)

第一項為已知量，與A陣無關(guān)，為使損失函數(shù)J(A)達(dá)到最小，等價于第二項最大，重新構(gòu)造函數(shù)

(15)

對式(15)作進(jìn)一步變化，可得

(16)

通過矩陣B的奇異值分解，可得最優(yōu)姿態(tài)矩陣A=U·diag(1,1,det(UV)T)VT。

2)根據(jù)式(17)和式(18)計算比力觀測矢量rk和bk

(17)

(18)

根據(jù)式(19)和式(20)計算每一時刻矩陣Bk

ΔB=bk(rk)T

(19)

Bk=Bk-1+ΔBk

(20)

矩陣Bk運用遞推累加的形式來更新，遞推的初值設(shè)置為0，B0=03×3。

2 抗擾動處理

1)對于適用于晃動基座的慣性系解析對準(zhǔn)，根據(jù)雙矢量定姿的原理[14-15]，取適當(dāng)間隔的2個不同時刻，利用2個時刻的測量信息求取姿態(tài)矩陣。當(dāng)外界受到大的干擾影響時, 僅取2個時刻的測量信息對準(zhǔn)結(jié)果必然受到影響。為了降低晃動對精度的影響，本文在原有基礎(chǔ)上進(jìn)行了改進(jìn)，將原來對準(zhǔn)時選取的2個點改成兩段時間平均后的點(tl=120s～180s，tm=240s～300s)。

對一套精度為0.003(°)/h的激光陀螺捷聯(lián)慣導(dǎo)系統(tǒng)進(jìn)行試驗驗證，將慣導(dǎo)系統(tǒng)安裝在試驗車上，車輛處于受干擾狀態(tài)(發(fā)動機(jī)怠速啟動、人員上下)。每次對準(zhǔn)時間5min，一次上電測試20次。改進(jìn)前后自對準(zhǔn)航向角重復(fù)性精度對比圖如圖1所示。

圖1 慣性系解析對準(zhǔn)及改進(jìn)后重復(fù)性對比圖Fig.1 Comparison of repeatability accuracy of inertial frame analytical alignment with that of improved heading angle

經(jīng)過試驗驗證對比可得，均值改進(jìn)后的對準(zhǔn)方法比傳統(tǒng)方法精度提升了15″(3σ)，抗晃動性能得以提升。

2)基于速度匹配的Kalman濾波估計，在車輛晃動的影響下，對準(zhǔn)航向角收斂時存在噪聲引起的震蕩。本文在原有算法的基礎(chǔ)上進(jìn)行改進(jìn)，采用速度積分匹配的方式，能夠起到平滑作用，對系統(tǒng)的誤差模型進(jìn)行調(diào)整，Kalman濾波15維狀態(tài)變量和量測量如下:

X=[δlnδluδleδVnδVuδVeφnφuφe

(21)

(22)

式中

δln、δlu、δle分別為北、天、東速度誤差積分。

這座工廠從2019年夏天起，將成為該品牌首款混合動力轎跑車Polestar 1的全球生產(chǎn)基地。Polestar 1能夠輸出600馬力的最大功率，和1000牛·米的峰值扭矩，它在純電動模式下可以行駛150公里。作為旗艦車型，它的售價預(yù)計在15．5萬歐元左右（約合125萬元人民幣，此處為未計算補(bǔ)貼價格，下同）。而將緊隨其后推出的Polestar 2，是一款完全由電力驅(qū)動的車型，預(yù)計定價從4萬歐元起（約合32萬元人民幣）。銷售思路有所創(chuàng)新，以定制租賃為主，包括按月付款的全包式購車模式。

在與1)相同條件下進(jìn)行試驗驗證， 速度匹配和速度積分匹配對比圖如圖2所示。

圖2 Kalman速度積分匹配與速度匹配航向角收斂對比圖Fig.2 Comparison of course angle convergence between velocity integral matching with velocity matching

速度積分匹配與速度匹配相比,經(jīng)試驗驗證，航向角收斂更加平滑,收斂速度更快，精度更高，抗擾動能力更強(qiáng)。

3)選擇比力矢量作觀測量，航向角收斂時也存在震蕩。為了提升SVD算法的性能，本文通過姿態(tài)陣更新隔離外界角運動干擾，對比力矢量積分并增加低通濾波器降低線振動的影響，從而提升系統(tǒng)的抗晃動能力。

改進(jìn)后的算法原理圖如圖3所示。

圖3 慣性系SVD算法原理圖Fig.3 Schematic diagram of inertial frame SVD algorithm

改進(jìn)措施如下：

(1)隔離角晃動干擾

(2)隔離線振動干擾

a)比力積分

(23)

(24)

b)低通濾波器

選擇慣性系速度作為觀測向量，為進(jìn)一步消除線振動干擾的影響，增加一個數(shù)字濾波器以過濾掉噪聲干擾，保留正常信號。gn和fb為低頻信號，遠(yuǎn)低于Vn等干擾量的頻率，通過數(shù)字濾波器的方式，建立在對晃動干擾頻譜分析的基礎(chǔ)上。選擇二型結(jié)構(gòu)IIR低通濾波器，設(shè)計指標(biāo)為: 采樣頻率200Hz，通帶截止頻率10Hz，阻帶截止頻率20Hz，通帶衰減1dB，阻帶衰減80dB，10階IIR低通濾波器。具體系數(shù)由Matlab工具箱輔助獲得。

與方法1)相同條件下進(jìn)行試驗驗證，對加速度計數(shù)據(jù)做出頻譜特性分析，如圖5所示，濾波器處理前后航向角收斂對比圖如圖6所示。

圖4 車載怠速條件下加速度計輸出曲線圖Fig.4 Output curve of accelerometer under vehicle idling condition

圖5 車載怠速條件下加速度計幅頻特性圖Fig.5 Spectrum characteristic diagram of accelerometer under vehicle idling condition

圖6 濾波器處理前后航向角收斂對比圖Fig.6 Comparison of course angle convergence between velocity observation vector and force observation vector in inertial frame

經(jīng)過試驗可得，濾波器處理后的航向角收斂更加平滑。

3 車載試驗驗證

最后對三種抗擾動自對準(zhǔn)方法進(jìn)行車載對比試驗，試驗條件和過程與前文所述一致，每次對準(zhǔn)時間5min，一次上電測試20次。利用三種抗擾動自對準(zhǔn)方法對相同的實測數(shù)據(jù)進(jìn)行仿真分析，試驗車和慣導(dǎo)系統(tǒng)圖片如圖7和圖8所示，試驗結(jié)果如下。

圖7 試驗車照片F(xiàn)ig.7 Photo of test vehicle

圖8 捷聯(lián)慣導(dǎo)系統(tǒng)照片F(xiàn)ig.8 Photo of the strap-down inertial navigation system

1)對相同實測數(shù)據(jù)進(jìn)行仿真分析，三種自對準(zhǔn)方法航向角重復(fù)性精度對比圖如圖9所示，慣性系解析、Kalman濾波估計和慣性系SVD三種方法的航向角均值分別為-5.5078°、-5.5052°和-5.5074°。

圖9 三種自對準(zhǔn)方法重復(fù)性精度對比圖Fig.9 Comparison of repeatability accuracy of the three self alignment methods

2)為方便比較，將慣性系SVD解析對準(zhǔn)30s時刻的航向角作Kalman濾波自對準(zhǔn)方法的初始航向角，慣性系SVD繼續(xù)進(jìn)行解算，給出30～300s時間內(nèi)Kalman濾波估計自對準(zhǔn)和慣性系SVD解析自對準(zhǔn)航向角收斂速度對比圖如圖10所示。

圖10 SVD和Kalman航向角3min/4min/5min收斂對比圖Fig.10 Comparison of SVD and Kalman heading angle convergence for 3/4/5min

車載晃動基座條件下，對慣性系解析自對準(zhǔn)、Kalman濾波估計自對準(zhǔn)和慣性系SVD自對準(zhǔn)三種結(jié)果進(jìn)行對比分析。由圖9可知，后兩者重復(fù)性精度要優(yōu)于前者；由圖10可知，慣性系SVD與Kalman濾波估計相比對準(zhǔn)精度相當(dāng)，收斂速度相當(dāng)。

從慣性系解析自對準(zhǔn)、Kalman濾波估計自對準(zhǔn)和慣性系SVD自對準(zhǔn)三者的計算過程和試驗結(jié)果對比可得：

1)傳統(tǒng)慣性系解析自對準(zhǔn)的計算過程簡單，計算量小，但抗擾動能力相對弱，對準(zhǔn)精度相對低，對于對準(zhǔn)精度要求不是很高或者純靜態(tài)情況下可以采用；

2)Kalman濾波估計自對準(zhǔn)方法目前應(yīng)用最廣泛，計算量大于慣性系解析對準(zhǔn)，對準(zhǔn)精度高，抗擾動能力強(qiáng)，但需要姿態(tài)初值和濾波參數(shù)；

3)慣性系SVD自對準(zhǔn)的計算量和對準(zhǔn)精度與Kalman濾波估計自對準(zhǔn)相當(dāng)，抗擾動能力強(qiáng)，無需姿態(tài)初值且不需要建立模型和設(shè)置參數(shù)，可以實現(xiàn)短時間對準(zhǔn)。

4 結(jié)論

本文對慣性系解析、Kalman濾波估計和慣性系SVD三種車載抗擾動自對準(zhǔn)方法進(jìn)行對比研究，結(jié)果表明，三種方法各有優(yōu)點，慣性系解析自對準(zhǔn)方法計算簡單，適用于低精度要求或純靜態(tài)條件；慣性系SVD自對準(zhǔn)方法既可以利用陀螺輸出進(jìn)行姿態(tài)陣計算隔離外界角晃動，同時又具備濾波作用，可以達(dá)到與Kalman濾波方法相當(dāng)?shù)膶?zhǔn)精度，具有快速收斂性、無需先驗信息即可獲得最優(yōu)解、工程適用性高等優(yōu)點。