基于地磁輔助慣性導航的狀態反饋系統研究

唐志勇，艾文娟，劉巖

（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）

基于地磁輔助慣性導航的狀態反饋系統研究

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖1　磁羅經自差示意圖

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

2.1地磁輔助慣性導航的狀態方程

狀態方程的標準形式如下：

在當地地理坐標系下的導航方程如下：

其中：

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

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

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

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

位置觀測方程如下：

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

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

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

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

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

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

航向角觀測方程如下：

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

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

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

3　組合導航模型的性能分析及優化算法

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

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

通過反饋控制器K可以使得系統內部穩定，而且達到一定的穩態精度，能夠提高系統的魯棒性［13］。

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

圖2　狀態反饋示意圖

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

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

則系統是完全能觀測的。其中分塊矩陣為：

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

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

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

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

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

圖3　閉環反饋結構圖

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

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

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

表1　導航元件的標定誤差

其它仿真參數：系統狀態初值X0、狀態估計均方誤差矩陣初值P0、系統噪聲均方誤差初始矩陣Q0、系統觀測噪聲均方誤差初始矩陣R0，設置如下：

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

圖4　組合導航的經度誤差仿真

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

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

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

4.2優化校正后的組合導航性能驗證

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

5　結論

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

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

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

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

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

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

［6］ 段本印.地磁輔助慣性導航系統技術研究［D］.哈爾濱：哈爾濱工業大學，2012.

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

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

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

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

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

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

［13］ 劉曉光，胡靜濤，王鶴.基于自適應濾波的組合導航系統研究［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