基于四旋翼組合導(dǎo)航系統(tǒng)中卡爾曼濾波技術(shù)的應(yīng)用設(shè)計

北京聯(lián)合大學(xué) 崔利申 熊 磊 王夕巖 許匯冬

基于四旋翼組合導(dǎo)航系統(tǒng)中卡爾曼濾波技術(shù)的應(yīng)用設(shè)計

北京聯(lián)合大學(xué) 崔利申 熊 磊 王夕巖 許匯冬

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

卡爾曼濾波；組合導(dǎo)航

1.引言

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

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

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

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

2.利用卡爾曼濾波對組合導(dǎo)航數(shù)據(jù)進(jìn)行融合

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

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

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

X(k)是k時刻的系統(tǒng)狀態(tài)，U(k)是k時刻對系統(tǒng)的控制量。A和B是系統(tǒng)參數(shù)，此系統(tǒng)中，他們?yōu)槎S矩陣。W(k)表示過程的噪聲（且為高斯白噪聲），協(xié)方差為Q（假設(shè)不隨系統(tǒng)狀態(tài)的改變而改變）。

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

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

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

首先我們要利用系統(tǒng)的過程模型，來預(yù)測下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是X(k)，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測出現(xiàn)在狀態(tài):

X(k|k-1)是利用上一狀態(tài)預(yù)測的結(jié)果，X(k-1|k-1)是上一狀態(tài)最優(yōu)估計的結(jié)果，U(k)為現(xiàn)在狀態(tài)的控制量，如果沒有控制量，可以設(shè)為0。

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

P(k|k-1)是X(k|k-1)狀態(tài)下對應(yīng)的協(xié)方差，P(k-1|k-1)是X(k-1|k-1)對應(yīng)的協(xié)方差，AT表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過程的協(xié)方差。式子(1)，(2)就是卡爾曼濾波器5個公式當(dāng)中的前兩個，也就是對系統(tǒng)的預(yù)測。

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

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

在我們利用計算機程序?qū)崿F(xiàn)時，我們應(yīng)該將Kg的計算在(3)式實現(xiàn)之前實現(xiàn)；

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

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

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

圖1 卡爾曼濾波處理數(shù)據(jù)流程圖

圖2 采用GPS與采用組合導(dǎo)航卡爾曼濾波后經(jīng)度效果對比圖

3.計算機仿真

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

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

4.結(jié)論

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

圖4 安裝四旋翼卡爾曼濾波組合導(dǎo)航系統(tǒng)的實物圖

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

[2]干國強．定位與導(dǎo)航—-現(xiàn)代戰(zhàn)爭的北斗星[M]．北京:國防工業(yè)出版社,2000．

[3]陸愷．最優(yōu)估計理論及其在導(dǎo)航中的應(yīng)用[M]．上海:上海交通大學(xué)出版社,1990．

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

鳴謝：該項目的研究成果基于北京聯(lián)合大學(xué)“啟明星”大學(xué)生科技創(chuàng)新項目經(jīng)費資助，項目編號201611417SJ082。

許匯冬