基于四旋翼組合導航系統中卡爾曼濾波技術的應用設計

北京聯合大學 崔利申 熊 磊 王夕巖 許匯冬

基于四旋翼組合導航系統中卡爾曼濾波技術的應用設計

北京聯合大學 崔利申 熊 磊 王夕巖 許匯冬

利用單一一種導航技術無法對四旋翼進行精確、穩定地導航，使用多種導航技術（GPSINS）對四旋翼進行導航時又必須將其中必要的參數進行融合，在對多個數據進行融合的算法中，卡爾曼濾波技術能達到很好的融合效果，因此在四旋翼組合導航中利用卡爾曼濾波算法能達到對四旋翼精確、穩定導航的目的。

卡爾曼濾波；組合導航

1.引言

四旋翼組合導航系統之所以不能使用單一的導航技術，是因為現有的導航系統均有其局限性，即慣性導航系統由于其快速性、獨立性和不易受干擾等優點而在現代商業和軍事領域中獲得了廣泛應用。但另一方面,慣性導航系統會產生隨時間無界增加的位置誤差。解決這個問題的一種有效辦法是利用外部信息對慣導系統進行周期性的更新，重設慣性導航的誤差，使誤差值回到初始值[1]；衛星導航（GPS）的誤差不隨時間增長，但易受電子干擾等影響，提高單一系統的導航精度，技術難、造價高，還難以滿足要求[2]。

隨著計算機、人工智能等相關技術領域的發展，微型機器人、微型直升飛機等微小型自主式載體將廣泛運用到未來的生產以及人類生活中，而微小型、低成本的導航系統是其關鍵技術之一，這對慣性傳感器提出了新的要求。MEMS (Micro Electro Mechanical System)慣性傳感器采用微電子加工技術制造的芯片級慣性傳感器，是由微傳感器、微執行器、信號處理和控制電路、通訊接口和電源等部件組成的一體化的微型器件系統。其目標是把信息的獲取、處理和執行集成在一起，組成具有多功能的微型系統集成于大尺寸系統中，從而大幅度地提高系統的自動化、智能化和可靠性水平。MEMS 器件具有體積小、質量輕、成本低、抗沖擊、可靠性高等優點，在汽車、電子、家電、機電等行業以及軍事領域有著極為廣闊的應用前景[4]。

盡管目前MEMS器件的精度還未達到極限，可以通過改進硬件設計和制造方法可提高其精度，但是通過硬件制造高精度陀螺儀不僅技術難，而且其成本也極其高。但是通過軟件算法，同樣可以達到比較理想的精度，而且成本比單一提升硬件精度要低很多，因此利用卡爾曼濾波技術，對INS與GPS的導航數據進行處理，可以起到優勢互補的作用，進而滿足對四旋翼飛行設備進行精確、穩定地導航控制。

卡爾曼濾波是一種有著相當廣泛應用的濾波方法，但它既需要假定系統是線性的，又需要認為系統中的各個噪聲與狀態變量均呈高斯分布，而這兩條并不總是確切的假設限制了卡爾曼濾波器在現實生活中的應用。擴展卡爾曼濾波器（EKF）極大地拓寬了卡爾曼濾波的適用范圍。EKF的基本思路是，假定卡爾曼濾濾對當前系統狀態估計值非常接近于其真實值，于是將非線性函數在當前狀態估計值處進行臺勞展開并實現線性化。另一種非線性卡爾曼濾波叫線性化卡爾曼濾波。它與EKF的主要區別是前者將非線函數在濾波器對當前系統狀態的最優估計值處線性化，而后者因為預先知道非線性系統的實際運行狀態大致按照所要求、希望的軌跡變化，所以這些非線性化函數在實際狀態處的值可以表達為在希望的軌跡處的臺勞展開式，從而完成線性化。

2.利用卡爾曼濾波對組合導航數據進行融合

卡爾曼濾波是一種遞推線性最小方差估計，因為上一時刻的估計是利用上一時刻和以前的量測得到的，所以這種遞推的實時估計就是利用所有的量測數據得到的；其次，卡爾曼濾波把被估計量作為系統的狀態，用系統狀態方程來描述狀態的轉移過程，因此，各時刻之間的狀態相關函數，就可以根據狀態方程的轉移特性來描述，解決非平穩隨機過程估計的困難。卡爾曼濾波的最優準則與線性最小方差估計一樣，每一時刻的估計都使估計均方誤差最小。由于采用了狀態方程，進行卡爾曼濾波的系統必須是線性的；要用卡爾曼濾波來估計非線性系統的狀態，必須對系統進行線性化。

應用卡爾曼濾波的GPS/INS組合導航系統，通過把GPS和INS兩種導航系統的數據融合在一起，做到優勢互補，克服各自缺點，提高導航精度。具體表現為：可以實現對慣性傳感器的校準及空中對準，提高精度：INS的輔助可以實現GPS完整性的檢測，從而提高可靠性。在隨機干擾情況下，采用卡爾曼濾波技術設計GPS/INS組合導航系統可以獲得統計最優的組合導航系統[3]。換句話說，GPS在長期的定位中，穩定性好，可以利用GPS的這個優勢來矯正INS在長期的定位中累積的誤差；而INS定位的動態性能又能彌補單一GPS定位在動態環境中信號失鎖的問題，而卡爾曼濾波算法可以將兩者的數據進行很好的融合，使兩者的數據能更好的反應物體真實的位置。

為了我們能夠更好的研究這個問題，同時也為了我們能方便的描述，我們引入一個離線控制系統。我們用一個線性隨機微分方程來描述我們要解決的問題：

X(k)是k時刻的系統狀態，U(k)是k時刻對系統的控制量。A和B是系統參數，此系統中，他們為二維矩陣。W(k)表示過程的噪聲（且為高斯白噪聲），協方差為Q（假設不隨系統狀態的改變而改變）。

然后我們用下面的方程來表示GPS/INS的測量值：

Z(k)是k時刻的測量值，H 是測量系統的參數，此系統中，H為二維矩陣。V(k)表示過程測量的噪聲（且為高斯白噪聲），協方差為R（假設不隨系統狀態的改變而改變）。

對于滿足上面的條件(線性隨機微分系統，過程和測量都是高斯白噪聲)的系統，卡爾曼濾波器是最優的信息處理器。下面我們來用他們結合他們的協方差來估算系統的最優化輸出。

首先我們要利用系統的過程模型，來預測下一狀態的系統。假設現在的系統狀態是X(k)，根據系統的模型，可以基于系統的上一狀態而預測出現在狀態:

X(k|k-1)是利用上一狀態預測的結果，X(k-1|k-1)是上一狀態最優估計的結果，U(k)為現在狀態的控制量，如果沒有控制量，可以設為0。

經過上面的步驟，系統的結果已經得到了更新，但是與X(k|k-1)對應的協方差并沒有得到更新。我們可以用P表示對應的協方差：

P(k|k-1)是X(k|k-1)狀態下對應的協方差，P(k-1|k-1)是X(k-1|k-1)對應的協方差，AT表示A的轉置矩陣，Q是系統過程的協方差。式子(1)，(2)就是卡爾曼濾波器5個公式當中的前兩個，也就是對系統的預測。

通過上面的兩個式子，我們得到了此時的預測結果，然后我們通過傳感器傳回的此時的狀態的測量值。結合預測值和測量值，我們就可以得到現在狀態的最優化估算值X(k|k)：

Kg我們稱之為卡爾曼增益，其計算公式如下：

在我們利用計算機程序實現時，我們應該將Kg的計算在(3)式實現之前實現；

此時X(k|k)就是k時刻的最優估計值，但是在系統運行的過程中，卡爾曼濾波一般不會只運行一次，為了使卡爾曼濾波器能夠持續運行直到系統停止，我們必須得更新此時（k）狀態X(k|k)的協方差：

(5)式中，I為單位矩陣；隨著系統的運行，數據的更新，便可以得到下一時刻的P(k+1|k+1)。

以上五個式子即為隨機線性離散系統卡爾曼濾波基本方程。只要給定的初值，根據k時刻的觀測值Z(k)，就可以遞推計算得k時刻的狀態估計值。在一個濾波周期內，從卡爾曼濾波在使用信息和觀測信息的先后順序來看，卡爾曼濾波具有兩個明顯的信息更新過程：時間更新過程和觀測更新過程。如圖1所示：

圖1 卡爾曼濾波處理數據流程圖

圖2 采用GPS與采用組合導航卡爾曼濾波后經度效果對比圖

3.計算機仿真

為了體現出經過卡爾曼濾波的組合導航的優勢，我們將其與單一的GPS定位做對比，由于硬件條件的限制，我們進行matlab軟件進行仿真，來模擬真實環境中的不同導航方式的導航情況。對比效果圖如圖2、圖3所示。

圖3 采用GPS與采用組合導航卡爾曼濾波后的緯度效果對比圖

4.結論

通過仿真的結果，可以清晰明了的看出，運用了卡爾曼濾波的組合導航系統的導航效果更佳，無論是在精度還是在穩定性方面，都要比采用單一傳感器（GPS）的導航效果好很多。因此，在四旋翼這種對成本有著嚴格要求的飛行器上面，采用卡爾曼濾波的組合導航系統系統，不僅可以大大降低購買更高精度傳感器的花費，并且能夠達到很好的導航效果。下面圖4即為安裝了四旋翼卡爾曼濾波組合導航系統的實物圖。

圖4 安裝四旋翼卡爾曼濾波組合導航系統的實物圖

[1]秦永元,張洪鉞,汪叔華．卡爾曼濾波與組合導航原理[M]．西安:西北工業大學出版社,1998．

[2]干國強．定位與導航—-現代戰爭的北斗星[M]．北京:國防工業出版社,2000．

[3]陸愷．最優估計理論及其在導航中的應用[M]．上海:上海交通大學出版社,1990．

[4]許江寧,朱濤,卞鴻巍．慣性傳感技術發展與展望[J]．海軍工程大學學報,2007,19(3):1-5

鳴謝：該項目的研究成果基于北京聯合大學“啟明星”大學生科技創新項目經費資助，項目編號201611417SJ082。

許匯冬