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

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

0 引言

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

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

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

1 自對(duì)準(zhǔn)方法

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

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

(1)

(2)

(3)

(4)

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

(5)

(6)

可表示為

(7)

其中

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

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

(8)

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

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

Kalman濾波模型如下

(9)

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

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

(10)

濾波觀測(cè)量為

(11)

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

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

bi=Ari+Δbi

(12)

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

(13)

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

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

(14)

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

(15)

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

(16)

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

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

(17)

(18)

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

ΔB=bk(rk)T

(19)

Bk=Bk-1+ΔBk

(20)

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

2 抗擾動(dòng)處理

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

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

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

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

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

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

(21)

(22)

式中

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

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

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

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

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

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

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

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

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

(1)隔離角晃動(dòng)干擾

(2)隔離線振動(dòng)干擾

a)比力積分

(23)

(24)

b)低通濾波器

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

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

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

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

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

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

3 車載試驗(yàn)驗(yàn)證

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

圖7 試驗(yàn)車照片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)對(duì)相同實(shí)測(cè)數(shù)據(jù)進(jìn)行仿真分析，三種自對(duì)準(zhǔn)方法航向角重復(fù)性精度對(duì)比圖如圖9所示，慣性系解析、Kalman濾波估計(jì)和慣性系SVD三種方法的航向角均值分別為-5.5078°、-5.5052°和-5.5074°。

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

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

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

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

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

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

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

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

4 結(jié)論

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