一種旋翼無人機(jī)組合導(dǎo)航系統(tǒng)設(shè)計(jì)及應(yīng)用*

劉洪劍，王耀南*，譚建豪，李樹帥，鐘 杭

(1.湖南大學(xué)電氣與信息工程學(xué)院，長(zhǎng)沙410082;2.機(jī)器人視覺感知與控制技術(shù)國(guó)家工程實(shí)驗(yàn)室，長(zhǎng)沙410082)

一種旋翼無人機(jī)組合導(dǎo)航系統(tǒng)設(shè)計(jì)及應(yīng)用*

劉洪劍1，2，王耀南1，2*，譚建豪1，2，李樹帥1，2，鐘 杭1，2

(1.湖南大學(xué)電氣與信息工程學(xué)院，長(zhǎng)沙410082;2.機(jī)器人視覺感知與控制技術(shù)國(guó)家工程實(shí)驗(yàn)室，長(zhǎng)沙410082)

針對(duì)傳統(tǒng)組合導(dǎo)航算法相對(duì)復(fù)雜，對(duì)系統(tǒng)硬件要求太高，實(shí)時(shí)性不好的缺點(diǎn)，設(shè)計(jì)了一種基于線性卡爾曼濾波器的簡(jiǎn)單實(shí)用組合導(dǎo)航算法。考慮到傳統(tǒng)卡爾曼濾波器在系統(tǒng)噪聲變化時(shí)濾波精度變差甚至發(fā)散以及模糊控制器計(jì)算量大的缺陷，設(shè)計(jì)了線性迭代調(diào)整觀測(cè)噪聲參數(shù)的方法對(duì)濾波器進(jìn)行自適應(yīng)改進(jìn)，提高了算法的魯棒性和實(shí)時(shí)性。利用STM32微控制器和MEMS慣性單元以及UBLOX GPS定位模塊搭建硬件平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證。結(jié)果表明:在168 MHz時(shí)鐘頻率下，一次姿態(tài)數(shù)據(jù)讀取和解算共耗時(shí)3.27 ms，一次組合導(dǎo)航濾波耗時(shí)2.18 ms，二者分別運(yùn)行在100 Hz和5 Hz頻率下。最終組合導(dǎo)航結(jié)果可以滿足無人機(jī)在1 m的精度范圍內(nèi)自動(dòng)懸停的需求，驗(yàn)證了所設(shè)計(jì)的組合導(dǎo)航算法的可靠性和實(shí)用性。

無人機(jī);卡爾曼濾波;自適應(yīng);組合導(dǎo)航

如今旋翼無人機(jī)已經(jīng)成為一個(gè)研究熱點(diǎn)，位姿信息獲取及位姿控制是無人機(jī)自主式飛行面臨的基本問題。目前旋翼無人機(jī)的姿態(tài)解算及控制研究已經(jīng)比較成熟，而導(dǎo)航控制問題仍處于研究階段。相比于傳統(tǒng)監(jiān)測(cè)等飛行任務(wù)，本課題研究的面向任務(wù)的旋翼飛行機(jī)械臂自主作業(yè)與控制系統(tǒng)則需要更加準(zhǔn)確的位置信息，以實(shí)現(xiàn)無人機(jī)懸停模式下準(zhǔn)確的空中抓取作業(yè)［1］。

應(yīng)用于旋翼無人機(jī)上集加速度計(jì)和陀螺儀于一體的微型慣性測(cè)量單元(MIMU)，不僅可以通過姿態(tài)解算得到姿態(tài)信息。還可以作為慣性導(dǎo)航系統(tǒng)(INS)，提供自主式無人機(jī)導(dǎo)航與制導(dǎo)所需的位置信息。但是其導(dǎo)航誤差會(huì)隨時(shí)間快速積累，不能單獨(dú)用于長(zhǎng)時(shí)間位置導(dǎo)航。在室外環(huán)境下，GPS能提供準(zhǔn)確的位置信息，但是其測(cè)量誤差不可避免，短時(shí)間測(cè)量精度較差。INS和GPS各有所長(zhǎng)，優(yōu)勢(shì)互補(bǔ)，利用卡爾曼濾波器對(duì)INS與GPS數(shù)據(jù)進(jìn)行融合，獲得導(dǎo)航參數(shù)的最優(yōu)估計(jì)值，可以得到高精度、高可靠的導(dǎo)航信息［2］。

目前對(duì)基于卡爾曼濾波器的組合導(dǎo)航算法的研究已有很多［3－4］。主要研究熱點(diǎn)集中在針對(duì)系統(tǒng)模型非線性或系統(tǒng)噪聲統(tǒng)計(jì)特性不明確引起的濾波器精度降低甚至發(fā)散這一問題的改進(jìn)上。具體改進(jìn)措施有:針對(duì)系統(tǒng)非線性模型的擴(kuò)展卡爾曼濾波(EKF)算法［5－6］、粒子濾波(PF)算法［7－8］和無跡卡爾曼濾波(UKF)算法［9－11］等;針對(duì)系統(tǒng)噪聲不確定性的自適應(yīng)衰減記憶法卡爾曼濾波算法［12］、Sage-Husa自適應(yīng)卡爾曼濾波算法［13－14］、模糊邏輯自適應(yīng)卡爾曼濾波算法［15－16］等。

EKF算法將非線性函數(shù)直接忽略高階項(xiàng)進(jìn)行線性化，勢(shì)必存在高階項(xiàng)截?cái)嗾`差，而且雅可比矩陣求解的計(jì)算量比較大。PF算法大量粒子的隨機(jī)產(chǎn)生，很難滿足導(dǎo)航系統(tǒng)實(shí)時(shí)性需求。UKF算法對(duì)初始值的取值比較敏感，系統(tǒng)噪聲不確定性會(huì)對(duì)濾波精度產(chǎn)生較大的影響;目前對(duì)于記憶衰減因子的選擇沒有完善的理論，只能根據(jù)經(jīng)驗(yàn)進(jìn)行確定。Sage-Husa自適應(yīng)算法可以在一定程度上降低模型誤差、提高濾波精度，但是計(jì)算量大，且對(duì)于階次較高的系統(tǒng)可靠性不高。模糊邏輯的思想和其他自適應(yīng)方法是一致的，都是用權(quán)值調(diào)整噪聲參數(shù)。但是模糊邏輯將調(diào)整過程根據(jù)經(jīng)驗(yàn)分為幾個(gè)模糊空間，而其他方法可以在每一點(diǎn)上調(diào)整，所以模糊邏輯調(diào)整的精確性不如其他方法好。而且模糊控制作為一種人工智能技術(shù)，對(duì)系統(tǒng)硬件要求比較高。

以上方法雖在精度上都達(dá)到了較理想的效果，但是一個(gè)共同的缺點(diǎn)就是算法復(fù)雜導(dǎo)致實(shí)時(shí)性不理想，對(duì)系統(tǒng)硬件要求太高，嚴(yán)重限制了其在實(shí)際中的應(yīng)用，大部分算法只能停留在仿真階段。本文從工程應(yīng)用角度出發(fā)，提出一種簡(jiǎn)潔有效的實(shí)時(shí)自適應(yīng)卡爾曼濾波算法。首先設(shè)計(jì)INS/GPS組合導(dǎo)航系統(tǒng)模型框架，然后建立線性系統(tǒng)的離散卡爾曼濾波模型。使用線性迭代的方式代替模糊控制器來克服系統(tǒng)噪聲的不確實(shí)性。最后設(shè)計(jì)實(shí)驗(yàn)平臺(tái)對(duì)算法進(jìn)行驗(yàn)證并得出結(jié)論。

1 組合導(dǎo)航系統(tǒng)模型設(shè)計(jì)

INS/GPS組合導(dǎo)航系統(tǒng)一般有兩種信息融合方式:速度、位置組合(松耦合)和偽距、偽距率組合(緊耦合)，本文采用前一種方式，這種方法的測(cè)量模型是線性的，可以用標(biāo)準(zhǔn)卡爾曼濾波器進(jìn)行信息的融合。

設(shè)計(jì)的INS/GPS組合導(dǎo)航系統(tǒng)框圖如圖1所示。慣性導(dǎo)航系統(tǒng)(INS)以牛頓力學(xué)為基礎(chǔ)，首先由姿態(tài)解算得到姿態(tài)矩陣Tnb，建立起導(dǎo)航坐標(biāo)系。然后利用加速度計(jì)來測(cè)量無人機(jī)的實(shí)時(shí)加速度，將實(shí)時(shí)加速度通過Tnb轉(zhuǎn)換到導(dǎo)航坐標(biāo)系中，經(jīng)過一次積分運(yùn)算得到實(shí)時(shí)速度，再一次積分運(yùn)算得出實(shí)時(shí)位置。然后利用自適應(yīng)卡爾曼濾波器結(jié)合GPS提供的速度和位置信息對(duì)上述預(yù)測(cè)值進(jìn)行校正。這里采用輸出校正法，利用估計(jì)值直接校正組合導(dǎo)航系統(tǒng)最終輸出的速度和位置，工程上實(shí)現(xiàn)簡(jiǎn)單。

圖1 組合導(dǎo)航系統(tǒng)框圖

圖1中INS為公共子系統(tǒng)，GPS為輔助子系統(tǒng)。兩個(gè)子系統(tǒng)分別進(jìn)行定位運(yùn)算，然后將兩者結(jié)果融合到一起。該算法的好處是:不需對(duì)子系統(tǒng)內(nèi)部做任何改動(dòng)，方便系統(tǒng)集成，比如在不方便使用GPS的情況下，可以使用其他定位系統(tǒng)如視覺傳感器直接替換掉GPS模塊，而程序不需做太大的改動(dòng)。而且姿態(tài)解算是組合導(dǎo)航的基礎(chǔ)，雖然兩個(gè)過程同時(shí)進(jìn)行，但是組合導(dǎo)航算法不會(huì)對(duì)姿態(tài)算法產(chǎn)生任何影響。

為了充分發(fā)揮慣性傳感器更新速率快的優(yōu)勢(shì)，INS子系統(tǒng)運(yùn)行在100 Hz的高頻率上，而組合導(dǎo)航卡爾曼濾波器運(yùn)行在5 Hz的GPS低定位頻率上。當(dāng)GPS出現(xiàn)故障時(shí)，系統(tǒng)可以直接輸出INS子系統(tǒng)的定位結(jié)果，在短時(shí)間內(nèi)可提供較準(zhǔn)確的位置信息。

2 卡爾曼濾波器設(shè)計(jì)

步驟1 狀態(tài)空間表達(dá)式的建立

旋翼無人機(jī)的運(yùn)行狀態(tài)可以用精簡(jiǎn)的6個(gè)狀態(tài)變量來描述:3個(gè)速度分量和3個(gè)位置分量。為了簡(jiǎn)化計(jì)算量，本文選取狀態(tài)向量為:

式中:vn，ve，vd分別表示北東地方向的速度分量，pn，pe，pd分別北東地方向的位置分量。則可以建立系統(tǒng)的狀態(tài)方程:

式中:Tnb為已知的機(jī)體坐標(biāo)系b到導(dǎo)航坐標(biāo)系n的姿態(tài)矩陣:

dT為卡爾曼濾波周期，［dvx，dvy，dvz］T表示周期dT內(nèi)無人機(jī)在機(jī)體坐標(biāo)系b中測(cè)得的速度增量。由于慣導(dǎo)系統(tǒng)數(shù)據(jù)更新速率比GPS模塊數(shù)據(jù)更新速率要快得多，而且在GPS數(shù)據(jù)更新時(shí)間間隔內(nèi)，系統(tǒng)加速度不可能為一定值。為減小狀態(tài)預(yù)測(cè)誤差，本文采用在GPS更新周期內(nèi)對(duì)加速度計(jì)多次采樣求積分的方式得到速度增量:

式中:dt表示INS的更新周期。G表示導(dǎo)航坐標(biāo)系n中的重力加速度值。過程噪聲ωk=［ωdvx，ωdvy，ωdvz］T。

取GPS模塊輸出的速度和位置信息作為觀測(cè)變量，則系統(tǒng)觀測(cè)向量為:

觀測(cè)方程為:

測(cè)量噪聲υk=［υvn，υve，υvd，υpn，υpe，υpd］T。

到此，建立了系統(tǒng)的線性離散狀態(tài)空間模型。

步驟2 濾波條件的建立

將式(2)寫成線性離散狀態(tài)方程的形式:

式中:“(－)”表示對(duì)應(yīng)量的預(yù)測(cè)值，“(+)”表示對(duì)應(yīng)量的估計(jì)值，下文表示方法類似。則系統(tǒng)一步狀態(tài)轉(zhuǎn)移矩陣和過程噪聲分布矩陣分別為:

式中:I3×3表示3×3的單位矩陣，03×3表示3×3的0矩陣，下文表示方法類似。

將狀態(tài)向量Xk代入式(6)觀測(cè)方程，可知輸出矩陣和觀測(cè)噪聲分布矩陣分別為:

卡爾曼濾波器是一個(gè)循環(huán)迭代過程，迭代需要一個(gè)初始條件。本文以無人機(jī)起飛點(diǎn)處作為位置坐標(biāo)原點(diǎn)，為了讓濾波器盡快收斂，此處初始條件設(shè)為:

為了簡(jiǎn)化模型，通常我們假設(shè)過程噪聲ωk和觀測(cè)噪聲υk為零均值高斯白噪聲，且其統(tǒng)計(jì)特性不隨時(shí)間和環(huán)境而改變:

過程噪聲協(xié)方差矩陣Qk和觀測(cè)噪聲協(xié)方差矩陣Rk分別代表了預(yù)測(cè)的不確定性和測(cè)量的不確定性。即使在上述假設(shè)成立的條件下，其值也是無法直接測(cè)量或者根據(jù)先驗(yàn)知識(shí)得到的，需要在實(shí)際應(yīng)用時(shí)根據(jù)濾波效果不斷調(diào)整得到一個(gè)合理值。根據(jù)本文調(diào)試經(jīng)驗(yàn)，可初步設(shè)定:

步驟3 濾波器的運(yùn)行

利用式(7)～式(17)，可以建立系統(tǒng)的卡爾曼濾波模型如下:

狀態(tài)預(yù)測(cè):

協(xié)方差預(yù)測(cè):

卡爾曼增益:

狀態(tài)修正:

協(xié)方差修正:

3 測(cè)量噪聲自適應(yīng)調(diào)整

對(duì)由式(2)和式(6)描述的系統(tǒng)，卡爾曼濾波器給出了最小協(xié)方差意義下狀態(tài)向量的最優(yōu)無偏估計(jì)。但是在實(shí)際中，過程噪聲、測(cè)量噪聲以及殘差一般情況下不再是高斯白噪聲［12，17］，此時(shí)用傳統(tǒng)的卡爾曼濾波器很容易導(dǎo)致濾波發(fā)散，一旦產(chǎn)生發(fā)散則信息融合失敗。

通常的解決方法是通過殘差方差匹配技術(shù)，使濾波器根據(jù)環(huán)境的改變自適應(yīng)調(diào)整統(tǒng)計(jì)參數(shù)，增強(qiáng)殘差的理論協(xié)方差陣與實(shí)際協(xié)方差陣的一致性［17］。文獻(xiàn)［14，17］介紹的方法都是通過模糊控制器來實(shí)現(xiàn)的。雖然仿真效果都不錯(cuò)，但是進(jìn)行模糊化、模糊推理和去模糊化等復(fù)雜過程嚴(yán)重影響了實(shí)際應(yīng)用時(shí)算法的實(shí)時(shí)性。所以本文設(shè)計(jì)如下的線性迭代自適應(yīng)過程:

定義殘差為:

殘差的理論協(xié)方差陣計(jì)算公式［17］:

實(shí)際協(xié)方差陣計(jì)算公式:

式中:N為用來計(jì)算實(shí)際協(xié)方差陣窗口的大小，N取值過大會(huì)影響自適應(yīng)調(diào)整的實(shí)時(shí)性，取值過小會(huì)降低Cr(k)的準(zhǔn)確性，都不利于自適應(yīng)性能的改善。根據(jù)調(diào)試結(jié)果，本系統(tǒng)取5＜N＜30較合適，本文取N=10進(jìn)行實(shí)驗(yàn)。

從式(24)可以看出，改變觀測(cè)噪聲協(xié)方差陣Rk的大小，可以改變殘差協(xié)方差的理論值Pr(k)。所以可以通過調(diào)整Rk來改變Pr(k)從而增強(qiáng)Pr(k)與Cr(k)的一致性。

用式(26)來表示理論值與實(shí)際值的一致程度:

設(shè)計(jì)Rk的線性迭代調(diào)整公式為:

式中:Ri(k)表示Rk主對(duì)角線上第i個(gè)元素。則根據(jù)上述分析可得如下關(guān)系:若DOMi(k)＞0，即理論值大于實(shí)際值，應(yīng)減小Ri(k)，所以ΔRi＜0;若DOMi(k)=0，即理論值等于實(shí)際值，保持Ri(k)不變，所以ΔRi=0;若DOMi(k)＜0，即理論值小于實(shí)際值，應(yīng)增大Ri(k)，所以ΔRi＞0。

|ΔRi|的大小可根據(jù)實(shí)際情況進(jìn)行調(diào)整。取得太大，Rk的調(diào)整力度過大導(dǎo)致震蕩;取得太小，Rk的自適應(yīng)調(diào)整效果不明顯，退化為常規(guī)卡爾曼濾波。一般可以參考式(17)，取Ri(k)×10－1數(shù)量級(jí)合適。如需進(jìn)一步提高自適應(yīng)性能，可以根據(jù)DOM(k)的大小，自動(dòng)調(diào)整|ΔRi|，使兩者成正向關(guān)關(guān)系，但是參數(shù)整定過程會(huì)變得繁瑣。權(quán)衡利弊本文取:

進(jìn)行參數(shù)自適應(yīng)調(diào)整后，卡爾曼濾波器具體步驟需要進(jìn)行修改，在上述濾波模型后面需加上如下步驟:① 計(jì)算式(20)時(shí)輸出式(24)，得到Pr(k);②計(jì)算式(21)時(shí)輸出式(23)，得到rk;③將rk代入式(25)，得到 Cr(k);④將Pr(k)和Cr(k)代入式(26)，得到DOM(k);⑤根據(jù)DOMi(k)求ΔRi;⑥將ΔRi代入式(27)，求得Rk作為下一次濾波迭代中式(20)的噪聲參數(shù)。

圖2 無人機(jī)實(shí)驗(yàn)平臺(tái)

4 實(shí)驗(yàn)結(jié)果及分析

4.1 實(shí)驗(yàn)平臺(tái)

圖2為本文設(shè)計(jì)的無人機(jī)實(shí)驗(yàn)平臺(tái)，軸距60 cm。飛行控制器采用STM32F405(Cortex－M4內(nèi)核)為中央處理器，工作頻率168 MHz。主要完成傳感器數(shù)據(jù)采集、姿態(tài)解算、組合導(dǎo)航、實(shí)驗(yàn)數(shù)據(jù)傳送、控制量輸出等功能。慣性測(cè)量單元(IMU)MPU6050固連于載體上，該芯片內(nèi)置一個(gè)三軸陀螺儀和一個(gè)三軸加速度計(jì)，實(shí)驗(yàn)使用±2 000(°)/s和±2 gn的量程，16 bit數(shù)字輸出，采樣數(shù)據(jù)最高輸出速率可達(dá)1 kHz。GPS接收機(jī)使用UBLOX NEO－M8N定位模塊，該模塊定位精度PDOP數(shù)值能達(dá)到2.0 m左右。采用NMEA－0183協(xié)議通過串口輸出GPS定位數(shù)據(jù)，并可通過UBX協(xié)議對(duì)模塊進(jìn)行配置。NMEA－0183協(xié)議利用ASCⅡ碼傳遞GPS定位數(shù)據(jù)，一幀的最大長(zhǎng)度可達(dá)600 byte左右，數(shù)據(jù)量比較大。為了數(shù)據(jù)傳輸?shù)姆€(wěn)定性和快速性，本設(shè)計(jì)使用主控制器的DMA串口功能接收GPS數(shù)據(jù)。模塊最高更新頻率可達(dá)到10 Hz，但數(shù)據(jù)不夠穩(wěn)定，故本文使用5 Hz更新頻率。經(jīng)測(cè)試，一次姿態(tài)解算耗時(shí)約3.27 ms，一次組合導(dǎo)航濾波耗時(shí)2.18 ms，為留冗余，設(shè)計(jì)姿態(tài)解算頻率為100 Hz。即每10 ms進(jìn)行一次姿態(tài)解算和INS導(dǎo)航，每200 ms進(jìn)行一次GPS數(shù)據(jù)讀取和位置卡爾曼濾波計(jì)算。

4.2 實(shí)驗(yàn)測(cè)試與結(jié)果分析

使用搭建的硬件平臺(tái)對(duì)算法進(jìn)行實(shí)驗(yàn)驗(yàn)證，通過無線串口模塊將數(shù)據(jù)實(shí)時(shí)傳送到電腦端得到測(cè)試數(shù)據(jù)。實(shí)驗(yàn)結(jié)果如圖3～圖11所示。其中，圖3～圖8為靜態(tài)濾波結(jié)果圖，即電機(jī)靜止?fàn)顟B(tài)下，手持無人機(jī)在水平面內(nèi)快速移動(dòng)時(shí)的位置濾波結(jié)果。圖9～圖11為動(dòng)態(tài)濾波結(jié)果圖，實(shí)驗(yàn)中給定無人機(jī)一個(gè)三維路徑，在其自動(dòng)飛行的過程中，實(shí)時(shí)傳回濾波數(shù)據(jù)。

圖3 靜態(tài)東向位置殘差曲線

圖4 靜態(tài)北向位置殘差曲線

由圖3和圖4位置濾波的殘差曲線可以看出，整個(gè)實(shí)驗(yàn)過程，殘差一直保持著0均值白噪聲特性不變。系統(tǒng)穩(wěn)定后，殘差最大值小于1.5 m。圖5和圖6分別表示東向和北向位置濾波的殘差協(xié)方差曲線，實(shí)線表示實(shí)際值，虛線表示理論值。結(jié)果顯示，測(cè)量噪聲Rk的自適應(yīng)調(diào)整，使殘差協(xié)方差的理論值緊跟實(shí)際值變化，提高了濾波器在系統(tǒng)噪聲統(tǒng)計(jì)特性改變時(shí)的濾波精度。圖7和圖8中實(shí)線和虛線分別表示靜態(tài)位置濾波曲線的測(cè)量值和估計(jì)值，可以看出，濾波器對(duì)GPS的輸出數(shù)據(jù)起到了良好的平滑和跟蹤效果。為了更好地驗(yàn)證算法的實(shí)際應(yīng)用效果，使用最優(yōu)估計(jì)值對(duì)無人機(jī)進(jìn)行定位實(shí)驗(yàn)。

圖5 靜態(tài)東向位置殘差協(xié)方差曲線

圖6 靜態(tài)北向位置殘差協(xié)方差曲線

圖7 靜態(tài)東向位置濾波曲線

圖8 靜態(tài)北向位置濾波曲線

圖9和圖10的實(shí)驗(yàn)結(jié)果證明，無人機(jī)可以平穩(wěn)地執(zhí)行起飛、按軌跡飛行、懸停和降落等任務(wù)，動(dòng)態(tài)濾波殘差在1.5 m以內(nèi)。以圖11所示的視角觀察無人機(jī)的懸停效果和漂移范圍，結(jié)果顯示，在風(fēng)力較小的環(huán)境下，無人機(jī)的空間位置漂移范圍可在1 m以內(nèi)。

圖9 動(dòng)態(tài)三維位置濾波曲線

圖10 動(dòng)態(tài)三維位置殘差曲線

圖11 無人機(jī)組合導(dǎo)航自動(dòng)懸停實(shí)驗(yàn)

5 結(jié)束語

本文首先建立了組合導(dǎo)航系統(tǒng)模型，然后按照模型的思路，選取了精簡(jiǎn)的6個(gè)狀態(tài)變量設(shè)計(jì)出系統(tǒng)線性離散卡爾曼濾波器。不僅避免了系統(tǒng)的非線性帶來的問題，也減少了計(jì)算量，保證了算法的實(shí)時(shí)性要求。為克服系統(tǒng)噪聲統(tǒng)計(jì)特性的不確定性給濾波器帶來的負(fù)面影響，對(duì)系統(tǒng)測(cè)量噪聲進(jìn)行了自適應(yīng)調(diào)整。使用線性迭代的方式代替模糊控制器，思路簡(jiǎn)單且易于實(shí)現(xiàn)。最后設(shè)計(jì)并制作了實(shí)驗(yàn)驗(yàn)證平臺(tái)，結(jié)果證明，利用濾波器輸出的最優(yōu)位置估計(jì)信息，無人機(jī)可以實(shí)現(xiàn)長(zhǎng)時(shí)間的穩(wěn)定自動(dòng)懸停，精度在1 m以內(nèi)。說明本系統(tǒng)可以為無人機(jī)在室外環(huán)境下的自主作業(yè)與控制提供準(zhǔn)確有效的位置信息。

［1］ 譚建豪，王耀南，王媛媛，等.旋翼飛行機(jī)器人研究進(jìn)展［J］.控制理論與應(yīng)用，2015，32(10):1278－1286.

［2］ 董緒榮，張守信，華仲春.GPS/INS組合導(dǎo)航定位及其應(yīng)用［M］.北京:國(guó)防科技大學(xué)出版社，1998.

［3］ 秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航系統(tǒng)原理［M］.西安:西北工業(yè)大學(xué)出版社，1998.

［4］ 董健康，安東.自適應(yīng)卡爾曼濾波在組合導(dǎo)航中的應(yīng)用研究［J］.計(jì)算機(jī)技術(shù)與發(fā)展，2011，21(10):183－185.

［5］ 張品秀，黃操軍，喬相偉.基于自適應(yīng)擴(kuò)展Kalman濾波的SINS/GPS深組合研究［J］.傳感技術(shù)學(xué)報(bào)，2010，23(3):408－412.

［6］ Psiaki M L.The Super-Iterated Extended Kalman Filter［C］// AIAA Guidance，Navigation，and Control Conference，2004(8): AIAA－04－5418.

［7］ Rigatos G G.Nonlinear Kalman Filters and Particle Filters for integrated Navigation of Unmanned Aerial Vehicles［J］.Robotics and Autonomous Systems，2012，60(7):978－995.

［8］ 陳延軍，梁新華，潘泉，等.兩級(jí)量測(cè)更新的粒子濾波檢測(cè)前跟蹤算法［J］.傳感技術(shù)學(xué)報(bào)，2015，28(1):115－120.

［9］ 楊波，秦永元，柴艷.UKF在INS/GPS直接法卡爾曼濾波中的應(yīng)用［J］.傳感技術(shù)學(xué)報(bào)，2007，20(4):842－846.

［11］歐縣華，武憲青，何熊熊.基于AIUKF的WSN節(jié)點(diǎn)定位算法［J］.傳感技術(shù)學(xué)報(bào)，2015，28(2):234－238.

［12］馬忠孝，劉宗玉，陳明.基于模糊邏輯的自適應(yīng)卡爾曼濾波在GPS/INS組合導(dǎo)航中的應(yīng)用［J］.信息與控制，2006，35(4): 457－461.

［13］王沖，曾慶軍.基于Sage-Husa自適應(yīng)濾波算法的AUV組合導(dǎo)航系統(tǒng)設(shè)計(jì)［J］.電子設(shè)計(jì)工程，2013，21(15):75－77.

［14］張少軍，王宏麗，蔡宗平.MIMU/GPS組合導(dǎo)航模糊自適應(yīng)卡爾曼濾波研究［J］.電光與控制，2008，15(8):84－86.

［15］Sasiadek J Z，Wang Q，Zeremba M B.Fuzzy Adaptive Kalman Filtering For INS/GPS Data Fusion［C］//Proceedings of the 15th IEEE International Symposium on Intelligent Control.［s.l.］:［s.n.］.2000:181－186.

［16］段宇，徐曉蘇.模糊邏輯自適應(yīng)卡爾曼濾波技術(shù)在GPS/INS組合導(dǎo)航系統(tǒng)中的仿真研究［J］.傳感技術(shù)學(xué)報(bào)，2003，16(2): 165－169.

［17］柏菁，劉建業(yè)，袁信.模糊自適應(yīng)卡爾曼濾波技術(shù)研究［J］.信息與控制，2002，31(3):193－197.

劉洪劍(1991－)，男，湖南大學(xué)電氣與信息工程學(xué)院碩士研究生，主要研究方向?yàn)闊o人機(jī)數(shù)據(jù)融合與控制，hnlhj @hnu.edu.cn;

王耀南(1957－)，男，湖南大學(xué)教授，博士生導(dǎo)師，主要研究方向?yàn)橹悄芸刂评碚撆c智能信息處理，yaonan@hnu.edu.cn。

A Kind of Design and Application of Integrated Navigation Systems for UAVs*

LIU Hongjian1，2，WANG Yaonan1，2*，TAN Jianhao1，2，LI Shushuai1，2，ZHONG Hang1，2
(1.College of Electrical and Information Engineering，Hunan University，Changsha410082，China; 2.National Engineering Laboratory for Robot Visual Perception and Control Technology，Changsha410082，China)

Aiming at the disadvantages that the traditional integrated navigation algorithm has high complexity，high hardware requirements，and poor real-time performance，a simple and practical integrated navigation algorithm based on linear Kalman filter is designed.Considering the problems that the filtering accuracy will become terrible or even divergence when the system noise varies，and great computational burden of the fuzzy controller，a method to adjust observation noise parameters adaptively based on linear iteration is proposed to improve the adaptive performance and robustness of the filter.Then，the proposed method is verified in an experimental system that consists of the STM32 microcontroller，the MEMS inertial unit and the UBLOX GPS positioning module.The experimental results indicate that，the time of reading attitude data and attitude solution costs 3.27 ms in total running at 100 Hz，and the time of integrated navigation filtering costs 2.18 ms in total running at 5 Hz，both under 168 MHz clock frequency.The final integrated navigation results can meet the needs of the UAVs automatic hover in the precision of 1 m and verify the reliability and practicability of the designed integrated navigation algorithm.

UAV;Kalman filter;self-adaptive;integrated navigation

U666.11

A

1004－1699(2017)02－0331－06

C:6140B;6330;7220

10.3969/j.issn.1004－1699.2017.02.028

項(xiàng)目來源:國(guó)家自然科學(xué)基金重點(diǎn)項(xiàng)目(61433016);國(guó)家自然科學(xué)基金面上項(xiàng)目(61573134)

2016－07－03 修改日期:2016－09－06