基于地磁輔助慣性導航的狀態(tài)反饋系統(tǒng)研究

唐志勇，艾文娟，劉巖

（1.長春理工大學　光電工程學院，長春　130022；2.長春理工大學　機電工程學院，長春　130022）

TANG Zhiyong1，AI Wenjuan2，LIU Yan2（1.School of Optoelectronic Engineering，Changchun University of Science and Technology，Changchun 130022；2.School of Mechatronical Engineering，Changchun University of Science and Technology，Changchun 130022）

基于地磁輔助慣性導航的狀態(tài)反饋系統(tǒng)研究

唐志勇1，艾文娟2，劉巖2

（1.長春理工大學光電工程學院，長春130022；2.長春理工大學機電工程學院，長春130022）

針對MEMS技術(shù)的慣性傳感器低精度的特點，提出一種高精度的組合導航算法，采用地磁輔助導航進行閉環(huán)校正，并對地磁的自差和航向角進行觀測，以達到較高的慣性導航精度和更高的動態(tài)性能。實驗證明，狀態(tài)反饋的慣性導航系統(tǒng)能很好地克服慣導的誤差累積，大幅度提高了地磁導航的抗干擾能力，得到較好的導航精度。

地磁導航；MEMS技術(shù)；組合濾波；自差校正

吊艙等偵察設(shè)備經(jīng)常在敵后縱深、戰(zhàn)場或水下深潛等條件下工作，工作環(huán)境比較復(fù)雜，要求具有成本低廉、體積小、抗沖擊力強、可靠性高的穩(wěn)定精確導航系統(tǒng)，微機械（MEMS）導航元器件［1］在小型飛行器如無人機偵查設(shè)備上有廣闊的應(yīng)用前景，但是微機械慣性導航元件存在導航精度低，時間漂移大等缺點，微機械磁力計檢測的信號微弱，更容易受到載體磁化磁場或強磁的影響，從而大大降低導航的精度。

1　微機械導航元器件的誤差模型

ADIS16405是基于MEMS技術(shù)的導航元器件，集成三軸陀螺儀、三軸加速計、三軸磁力計為一體，微機械元件的主要誤差來源包括零偏誤差［2］、隨機漂移誤差和噪聲等，根據(jù)誤差來源和相關(guān)誤差的特性，可以建立誤差模型。在本文的研究中，將誤差等效成隨機常數(shù)誤差補償、一階馬爾科夫過程和高斯白噪聲。

微機械陀螺儀的誤差模型如下：

其中，δω為陀螺儀的測量誤差，nb為陀螺的隨機誤差常數(shù)，nr為一階馬爾科夫過程，nw為高斯白噪聲，Tg為陀螺儀漂移的相關(guān)時間，wr為陀螺儀的漂移噪聲。

微機械加速計的誤差模型如下：

其中，?為加速計測量誤差，?b為加速計隨機誤差常數(shù)，?r為一階馬爾科夫過程，na為高斯白噪聲，Ta為加速計漂移的相關(guān)時間，wa為加速計的漂移噪聲。

微機械磁傳感器的誤差模型如下：

其中，δH為磁傳感器［3］的測量誤差，εb為磁力計隨機誤差常數(shù)，εr為一階馬爾科夫過程，εw為高斯白噪聲，Th為磁傳感器漂移的相關(guān)時間，wh為磁傳感器的漂移噪聲。

磁力計又稱為磁羅經(jīng)，廣泛應(yīng)用于船舶、航空航天等的導航系統(tǒng)中?，F(xiàn)代的機體多采用鋼鐵架構(gòu)，在受到地磁磁化后具有磁場，由于磁羅經(jīng)早期應(yīng)用于船舶的導航，所以地磁磁化產(chǎn)生的磁場稱為船磁［4］。在船磁的作用下，羅經(jīng)偏離磁北Nm所指的方向為羅北Nc，磁北和羅北之間的夾角稱為磁羅經(jīng)自差δ。自差的示意圖如圖1所示。

圖1　磁羅經(jīng)自差示意圖

2　地磁輔助慣性組合導航的數(shù)學模型

2.1地磁輔助慣性導航的狀態(tài)方程

狀態(tài)方程的標準形式如下：

在當?shù)氐乩碜鴺讼迪碌膶Ш椒匠倘缦拢?/p>

其中：

可以根據(jù)導航方程推導出誤差模型如下：

式中，Φ=［ψ，θ，φ］T分別表示俯仰角、橫滾角、航向角［6］。εn為陀螺儀的漂移角速度誤差，?fb為加速計的測量誤差。

其中，σ和θ分別是慣導元件和磁強計的白噪聲漂移誤差的均方差。

2.2地磁輔助慣性導航的觀測方程

位置觀測方程如下：

式中，LINS、λINS、hINS為慣性導航系統(tǒng)的輸出位置信息，LGNS、λGNS、hGNS為地磁導航系統(tǒng)的輸出位置信息，Hp為位置觀測方程的觀測矩陣，ΔVp為位置觀測噪聲。

由于地磁導航的磁羅經(jīng)自差不能通過觀測信息的組合導航濾波對ΔVp進行消除，所以組合導航的觀測方程應(yīng)包括地磁導航的磁羅經(jīng)自差［7］δ，地磁導航的狀態(tài)模型如下：

式中，X′、Y′、Z′為帶誤差的磁場強度在載體坐標系上的投影，X、Y、Z為真實磁場強度在載體坐標系上的投影，a、b、c、d、e、f、g、h、k分別為軟鐵系數(shù)，w為系統(tǒng)白噪聲。

自差觀測方程如下：式中，HB為地磁自差觀測方程的觀測矩陣，ΔVB為地磁自差觀測噪聲。

將X′，Y′，Z′旋轉(zhuǎn)到水平坐標系，即東北天坐標系得：

其中θ是飛行器的橫滾角，φ是飛行器的俯仰角。則系統(tǒng)的測量磁航向角φ可由下式計算：

航向角觀測方程如下：

其中φINS，φGNS分別是慣性導航的航向角輸出信息和地磁導航的航向角輸出信息，Hφ是航向角觀測方程的觀測參數(shù)，ΔVφ是航向角觀測噪聲。

整理成標準觀測模型的形式如下：

其中，H（t）是7×12維的狀態(tài)觀測矩陣，HP（t）是3× 12維的狀態(tài)觀測矩陣，HB（t）是3×12維的狀態(tài)觀測矩陣，Hφ（t）是1×12維的狀態(tài)觀測矩陣。觀測噪聲v（t）是隨機噪聲向量，它服從均值為零的高斯正態(tài)分布，可寫作v（t）～N（0，R），R表示隨機向量的方差。

3　組合導航模型的性能分析及優(yōu)化算法

3.1組合導航的控制算法研究

經(jīng)典卡爾曼濾波算法是根據(jù)觀測向量、觀測模型和系統(tǒng)模型來獲取狀態(tài)向量的最優(yōu)估計，是一種被廣泛應(yīng)用的工程誤差估計算法。但是經(jīng)典卡爾曼濾波要求觀測量是與時間無關(guān)的噪聲，也就是白噪聲［8］，而本文的自差和姿態(tài)等都是與時間相關(guān)的噪聲，卡爾曼濾波器會把這類噪聲變量歸因于狀態(tài)的改變，可能會影響誤差的傳遞過程，破壞狀態(tài)的估計。狀態(tài)反饋可以配置極點、實現(xiàn)系統(tǒng)解耦控制、構(gòu)成最優(yōu)調(diào)節(jié)器、改善跟蹤系統(tǒng)的穩(wěn)態(tài)特性等特點，但是狀態(tài)變量一般都是不易直接測量的，而且狀態(tài)反饋雖然不會改變系統(tǒng)的可控性，但可能會影響系統(tǒng)的可觀測性，所以可以采用狀態(tài)觀測器［9］，對不可觀測的狀態(tài)進行狀態(tài)重構(gòu)，并采用重構(gòu)的狀態(tài)代替系統(tǒng)的真實狀態(tài)，進行有效的狀態(tài)估計。同時考慮不到組合導航過程中存在很多干擾和噪聲，因此提出了如下的消除誤差和干擾的方法，即利用系統(tǒng)輸出來估計系統(tǒng)狀態(tài)，以實現(xiàn)狀態(tài)反饋控制，帶有觀測器的組合導航反饋系統(tǒng)如圖2所示。其中，∑0表示給定的系統(tǒng)狀態(tài)空間，∑?表示新的狀態(tài)空間表達式，X?表示狀態(tài)空間的估計狀態(tài)向量。設(shè)狀態(tài)反饋的狀態(tài)空間表達式為：

通過反饋控制器K可以使得系統(tǒng)內(nèi)部穩(wěn)定，而且達到一定的穩(wěn)態(tài)精度，能夠提高系統(tǒng)的魯棒性［13］。

狀態(tài)觀測器采用卡爾曼濾波進行狀態(tài)估計，采用降維觀測器只是對位置、地磁和航向角進行狀態(tài)估計。

圖2　狀態(tài)反饋示意圖

首先研究系統(tǒng)的狀態(tài)觀測器是否存在，由于狀態(tài)觀測器存在的條件是系統(tǒng)狀態(tài)完全可觀，所以進行可觀測性驗證。線性時變系統(tǒng)的能控性判據(jù)為：

如果矩陣F（t）、H（t）滿足n-1次連續(xù)可微的條件，在時間區(qū)間［t0，t1］內(nèi)，又有：

則系統(tǒng)是完全能觀測的。其中分塊矩陣為：

由系統(tǒng)的狀態(tài)方程和觀測方程整理化簡得F（t）、H（t）如下：

HP（t）可以參考文獻［6］和文獻［8］，HB（t）可由上文求解，Hφ（t）計算如下：，當用坐標轉(zhuǎn)換矩陣和歐拉角描述一個小角度的擾動時，考慮到小角度近似：

計算可得到N0（t），N1（t），…，N11（t）線性無關(guān)，其秩為滿秩，滿足狀態(tài)完全可觀測，可以建立狀態(tài)觀測器，對相關(guān)狀態(tài)進行估計。

3.2組合導航模型的反饋校正

采用組合導航濾波得到的誤差估計，對慣導系統(tǒng)進行閉環(huán)校正［10］，也就是前面所說的帶有狀態(tài)觀測器的反饋系統(tǒng)，當狀態(tài)變量不便直接測量得到時，可以采用狀態(tài)觀測器獲得狀態(tài)向量X（t）的估計值（t），并用?（t）代替X（t）完成慣導系統(tǒng)的閉環(huán)反饋任務(wù)。采用地磁慣性組合導航的閉環(huán)反饋系統(tǒng)結(jié)構(gòu)圖3所示。

圖3　閉環(huán)反饋結(jié)構(gòu)圖

4　組合導航的性能仿真驗證

4.1基于Matlab的組合導航仿真實驗

組合導航的性能還需要進行驗證，才能夠判斷算法的可行性和算法特性，所以該節(jié)進行仿真實驗，通過標準傳感器對ADIS16405元件的誤差進行標定如表1所示。

表1　導航元件的標定誤差

其它仿真參數(shù)：系統(tǒng)狀態(tài)初值X0、狀態(tài)估計均方誤差矩陣初值P0、系統(tǒng)噪聲均方誤差初始矩陣Q0、系統(tǒng)觀測噪聲均方誤差初始矩陣R0，設(shè)置如下：

進行仿真的條件如下：設(shè)置載體的初始位置為東經(jīng)125°，北緯43°，并以140km/h的速度勻速航行，仿真的頻率為20Hz。進行狀態(tài)估計的仿真如圖4至圖7所示，分別為經(jīng)典卡爾曼濾波和帶狀態(tài)反饋的優(yōu)化卡爾曼濾波的仿真結(jié)果。

圖4　組合導航的經(jīng)度誤差仿真

圖5　組合導航的緯度誤差仿真

圖6　組合導航的高度誤差仿真

圖7　組合導航的航向角誤差仿真

4.2優(yōu)化校正后的組合導航性能驗證

通過卡爾曼濾波對組合導航的誤差值進行狀態(tài)估計，比較初始狀態(tài)和濾波后的狀態(tài)，可以看出卡爾曼濾波可以很好的消除隨機干擾，但是系統(tǒng)的響應(yīng)速度較慢，穩(wěn)定性較差，而且建立的數(shù)學模型常常采用簡化的模型，存在較大的模型誤差［11，12］，由仿真圖像可以看出系統(tǒng)的跟隨性能存在缺陷，位置和地磁的導航精度較高，但是航向角仍然存在較大的誤差，經(jīng)過前面的分析，可以采用H∞濾波進行系統(tǒng)的優(yōu)化，得到更好的動態(tài)特性、魯棒鎮(zhèn)定和干擾抑制能力。

5　結(jié)論

由組合導航的狀態(tài)觀測反饋，通過對地磁慣性組合的狀態(tài)向量進行狀態(tài)估計，設(shè)計狀態(tài)觀測器，并采用估計狀態(tài)代替實際的狀態(tài)變量進行反饋［13］，能夠有效的提高組合導航的精度，避免了直接采用卡爾曼濾波的各種復(fù)雜要求，能更加簡單有效地實現(xiàn)組合導航的噪聲和干擾的濾除，為導航提供了更加方便的控制方法。但是采用狀態(tài)估計反饋，增加了狀態(tài)的階次，也就是提高了系統(tǒng)的維數(shù)，而且并不能改善系統(tǒng)的動態(tài)特性，反而，帶觀測器的狀態(tài)反饋系統(tǒng)在魯棒性上，要比真實狀態(tài)的反饋系統(tǒng)更差。所以還有待進一步優(yōu)化算法，或者進行系統(tǒng)狀態(tài)反饋的魯棒性鎮(zhèn)定問題進行更深入的研究。

［1］ 張嗣灜，高立群.現(xiàn)代控制理論［M］.北京：清華大學出版社，2006，27-189.

［2］ 戰(zhàn)德軍，戴東凱，張忠華.單軸旋轉(zhuǎn)INS/GPS組合導航中重力垂線偏差引起的姿態(tài)誤差分析［J］.中國慣性技術(shù)學報，2014，2（11）：301-305.

［3］ 姚一飛，李增科，高井祥.GPS/INS緊耦合導航中多路徑效應(yīng)改正算法應(yīng)用［J］.中國慣性技術(shù)學報，2014，5 （8）：782-787.

［4］ 王鵬，張迎春.一種小衛(wèi)星高精度自主定軌/定姿一體化新方法［J］.中國慣性技術(shù)學報，2014，12（3）：741-747.

［5］ 楊曉東，王煒.地磁導航原理［M］.北京：國防工業(yè)出版社，2009.

［6］ 段本印.地磁輔助慣性導航系統(tǒng)技術(shù)研究［D］.哈爾濱：哈爾濱工業(yè)大學，2012.

［7］ 晏登洋.慣性地磁組合導航原理［D］.西安：西北工業(yè)大學，2007.

［8］ 伊程毅.基于地磁和微慣性器件組合姿態(tài)測量系統(tǒng)研究［D］.哈爾濱：哈爾濱工業(yè)大學，2013.

［9］ 丁君.基于微慣性傳感器的姿態(tài)算法研究［D］.上海：上海交通大學，2013.

［10］ 董超，錢峰.微型姿態(tài)解算系統(tǒng)的抗磁干擾算法研究［J］.電子測量系統(tǒng)，2011，12（5）：59-62.

［11］ 楊宇彬，湯國建，包衛(wèi)國.地磁姿態(tài)探測器的實時標定算法研究［J］.飛行力學，2014，19（7）：93-96.

［12］ 龍達峰，李杰，劉俊.地磁傳感器誤差參數(shù)估計與補償方法［J］.傳感技術(shù)學報，2014，9（10）：1649-1653.

［13］ 劉曉光，胡靜濤，王鶴.基于自適應(yīng)濾波的組合導航系統(tǒng)研究［J］.儀器儀表學報，2014，12（7）：1013-1020.

Based on the Geomagnetic-aided Inertial Navigation System of State Feedback

According to the characteristics of low precision of the inertial sensor of MEMS technology，a high precision of integrated navigation algorithm is put forward，adopt geomagnetic aided navigation with closed-loop correction，and the deviation of the magnetic and heading Angle of the observation is in order to achieve high precision of inertial navigation and a higher dynamic performance.Experiments show that state feedback of inertial navigation system can overcome the error accumulation of inertial navigation，greatly improve the anti-interference ability of the geomagnetic navigation，get better navigation accuracy.

geomagnetic navigation；MEMS technology；composite filtering；deviation correction

TP273

A

1672-9870（2015）06-0127-06

TANG Zhiyong1，AI Wenjuan2，LIU Yan2
（1.School of Optoelectronic Engineering，Changchun University of Science and Technology，Changchun 130022；2.School of Mechatronical Engineering，Changchun University of Science and Technology，Changchun 130022）

2015-09-07

唐志勇（1976-），男，碩士，講師，E-mail：3551086@qq.com