無人機(jī)數(shù)據(jù)采集EKF濾波懸停穩(wěn)定的研究與應(yīng)用

, ,

(長沙理工大學(xué),長沙 410114)

0 引言

近幾年，無人機(jī)技術(shù)越來越成為了一個(gè)高度集成的高科技，內(nèi)容涉及自動(dòng)控制技術(shù)、遙感技術(shù)、圖像處理技術(shù)、激光技術(shù)、信息技術(shù)、氣動(dòng)力設(shè)計(jì)技術(shù)等領(lǐng)域的成果應(yīng)用[1]。因無人機(jī)具有的垂直起降與空中懸停等飛行能力可以在任何環(huán)境下起降，這恰好與軍用、商業(yè)及民用的相關(guān)方面的需求相吻合，如偵查、信號(hào)傳輸中繼、航拍攝影、資源勘探、農(nóng)業(yè)植保、大氣監(jiān)測(cè)、交通監(jiān)控、邊境巡邏、電力線檢測(cè)、反恐偵查、森林防火等。在無人機(jī)能完成很多高難度任務(wù)的同時(shí)，要求飛行控制系統(tǒng)更加穩(wěn)定。利用導(dǎo)航技術(shù)、傳感技術(shù)和飛行控制律等使無人機(jī)趨向于小型化和高精度迫在眉睫，以減少工作任務(wù)中出現(xiàn)故障的機(jī)率[2]。四旋翼無人機(jī)是無人機(jī)研究中的一個(gè)熱門，與單旋翼相比，它結(jié)構(gòu)簡單，經(jīng)濟(jì)實(shí)惠。四旋翼無人機(jī)姿態(tài)和位置通過改變4個(gè)電機(jī)的轉(zhuǎn)速形成轉(zhuǎn)速差來控制。它機(jī)動(dòng)性強(qiáng)，可以在室內(nèi)相對(duì)狹小的空間飛行；效率高，一對(duì)電機(jī)順時(shí)針旋轉(zhuǎn)，另一對(duì)逆時(shí)針旋轉(zhuǎn)，由電機(jī)旋轉(zhuǎn)產(chǎn)生的旋轉(zhuǎn)力矩相互抵消，而不需像單旋翼一樣增加尾槳來抵消主漿的旋轉(zhuǎn)力矩。但這種無人機(jī)負(fù)載較小，因此對(duì)機(jī)載硬件的要求也更加嚴(yán)格。建立在已有的技術(shù)上通過對(duì)無人機(jī)實(shí)時(shí)數(shù)據(jù)采集，以ARM嵌入式系統(tǒng)作為上位機(jī)，通過Linux 下嵌入式 C++的編程，嚴(yán)謹(jǐn)合理的科研手段改善獲得無人機(jī)設(shè)備的運(yùn)行狀態(tài)[3]，對(duì)于提高無人機(jī)各方面性能和診斷具有重要的意義。

隨著智能先進(jìn)控制技術(shù)的發(fā)展，許多理論中先進(jìn)控制算法和濾波已應(yīng)用到無人機(jī)控制中。文獻(xiàn)[4]提到了模型控制的智能學(xué)習(xí)方法實(shí)現(xiàn)了無人直升機(jī)的倒飛懸?？刂?。Sessa S等人針對(duì)載體運(yùn)動(dòng)過程中產(chǎn)生的額外加速度提出了采用自適R陣來減小加速度的干擾[5-6]。文獻(xiàn)[7]提出無人機(jī)在水下懸停也可以得到較好的調(diào)節(jié)。文獻(xiàn)[8]在Matlab simulink環(huán)境下設(shè)計(jì)出了十字翼布局無人機(jī)非線性仿真，懸停階段的PID控制律設(shè)計(jì)。文獻(xiàn)[9]設(shè)計(jì)了懸停模式下的高度控制，在反饋線性代的基礎(chǔ)上采用PID控制設(shè)計(jì)。文獻(xiàn)[10]指出四旋翼懸?？刂颇Ｐ涂山埔粋€(gè)線性定常欠強(qiáng)耦合系統(tǒng)，系統(tǒng)的不穩(wěn)定利用二次型最優(yōu)控制對(duì)系統(tǒng)進(jìn)行鎮(zhèn)定。文獻(xiàn)[11-13 ]針對(duì)卡爾曼濾波及其實(shí)時(shí)應(yīng)用，提出使用新息判斷野值是否存在，并采用了最小二乘法結(jié)合UKF對(duì)野值進(jìn)行處理，提高了系統(tǒng)抗野值性能。本文借鑒文獻(xiàn)[10-13]的基礎(chǔ)上,設(shè)計(jì)了一種基于氣壓計(jì)值換算成高度自適應(yīng)擴(kuò)展卡爾曼濾波算法，并結(jié)合四旋翼無人機(jī)對(duì)高度進(jìn)行實(shí)時(shí)采集數(shù)據(jù)，有效增加了無人機(jī)懸停狀態(tài)下的穩(wěn)定性，大大降低了線性加速度和周圍測(cè)量

噪聲對(duì)測(cè)量結(jié)果的影響。

1 無人機(jī)擴(kuò)展Kalman濾波器模型建立

遞歸最小二乘(RLS)算法是最小二乘法的遞歸算法。它是由前一時(shí)刻濾波器權(quán)重向量的最小二乘解，根據(jù)當(dāng)前新的數(shù)據(jù)遞歸計(jì)算當(dāng)前權(quán)重向量的最新估計(jì)，因此是一種在線更新算[ 13]。濾波器是一種從混合信號(hào)恢復(fù)出所需信號(hào)的模塊或算法，在信號(hào)處理領(lǐng)域，噪聲的頻率與有用信號(hào)頻率的分布常常包含兩種情況。一種是兩者的頻率分布在不同的頻段區(qū)域，另一種是兩者的頻率分布在相同頻段范圍或者噪聲是隨機(jī)的。此時(shí)，有效的方法就是根據(jù)相應(yīng)噪聲統(tǒng)計(jì)特性在某一誤差準(zhǔn)則下設(shè)計(jì)相對(duì)于的自適應(yīng)濾波器。常用的誤差準(zhǔn)則包括最小均方誤差、最小二乘、最小方差等。

Kalman濾波器是一種以最小二乘法為基礎(chǔ)，核心原理在于預(yù)測(cè)和測(cè)量反饋，它由線性系統(tǒng)狀態(tài)預(yù)測(cè)方程和線性系統(tǒng)觀測(cè)方程組成。

考慮如下非線性離散狀態(tài)空間模型：

xk=f(xk-1)+w(k-1)

(1)

zk=h(xk)+vk

(2)

其中，狀態(tài)向量xk∈Rn是不可觀測(cè)的，zk∈Rm為可觀測(cè)的量測(cè)向量；f(·)和h(·)為已知的非線性函數(shù)；過程噪聲wk-1∈Rn和測(cè)量噪聲vk∈Rm為相互獨(dú)立的高斯白噪聲，其均值為零，協(xié)方差矩陣分別為Qk-1和Rk。

(3)

(4)

將式(3)、(4)代入(1)、(2)即可得到近似的線性狀態(tài)空間模型：

(5)

(6)

(7)

狀態(tài)預(yù)測(cè)誤差協(xié)方差矩陣：

(8)

Pk|k-1右下標(biāo)表示根據(jù)k-1時(shí)刻的P值來估算k時(shí)刻的值。

其中:Fk-1的雅克比(Jacobi)矩陣為：

卡爾曼增益：

(9)

狀態(tài)估計(jì)：

(10)

狀態(tài)估計(jì)誤差協(xié)方差矩陣：

Pk=(I-GkHk)Pk|k-1

(11)

飛控應(yīng)用中，擴(kuò)展卡爾曼濾波算法將從預(yù)先確定的初始條件集出發(fā)，在平穩(wěn)環(huán)境下，擴(kuò)展卡爾曼濾波收斂于某種統(tǒng)計(jì)意義下的最優(yōu)維納解；在非平穩(wěn)環(huán)境下，該算法提供了一種跟蹤能力，能夠跟蹤氣壓計(jì)特性變化緩慢的數(shù)據(jù)。

根據(jù)飛控?cái)?shù)據(jù)最優(yōu)估計(jì)實(shí)際情況，通過氣壓傳感器獲得裝換的高度計(jì)算真實(shí)的高度值，取加速度矢量，高度矢量和速度矢量建立擴(kuò)展kalman濾波器觀測(cè)方程：

(12)

其中:P(t)為需要測(cè)量的實(shí)時(shí)高度，V(t)為需要測(cè)量的速度，P(t-1)為上一刻時(shí)間的高度，V(t-1)為上一刻時(shí)間的速度值。

狀態(tài)估計(jì)：

(13)

狀態(tài)估計(jì)誤差協(xié)方差矩陣：

(14)

狀態(tài)估計(jì)：

x(k|k)=x(k|k-1)+kg(z(k)-[1 0]x(k|k-1)〗)

(15)

P(k|k-1)=(1-kg[1 0])P(k|k-1)

(16)

經(jīng)過反復(fù)實(shí)驗(yàn)得到最優(yōu)P、Q的初始矩陣:

其中:sa為卡爾曼濾波的加速度的系數(shù)，實(shí)驗(yàn)濾波中根據(jù)實(shí)物飛控系統(tǒng)參數(shù)加速度系數(shù)取為了0.03。

2 匿名科創(chuàng)地面站在無人機(jī)中的使用

目前市面上許多MiniFly都兼容一款強(qiáng)大功能的匿名科創(chuàng)地面站。匿名科創(chuàng)地面站支持許多通信方式，本文實(shí)驗(yàn)使用的是串口方式通信，以下介紹本實(shí)驗(yàn)如何使用匿名科創(chuàng)地面站查看飛控姿態(tài)，顯示波形圖。

第一步，根據(jù)電腦下載安裝驅(qū)動(dòng)cp210xvcpInstaller-x6或cp210xvcpInstaller-x86。

安裝成功后，將本實(shí)驗(yàn)所用的無線模塊一端cp2012 MODULES USB接入電腦，另一端連接飛控接受信號(hào)LC12S芯片。無線模塊實(shí)物圖如圖1。

圖1 飛控接受信號(hào)的無線模塊

第二步，在打開匿名科創(chuàng)地面站軟件中選擇串口通信。

點(diǎn)擊軟件左邊一列圖標(biāo)的程序設(shè)置，即可顯示程序設(shè)置界面，如圖2所示，然后根據(jù)設(shè)備串口選擇了COM5,設(shè)置飛控相應(yīng)的波特率38400，最后點(diǎn)擊右下角的打開連接，即可通信。

圖2 串口通信和波特率設(shè)置圖

第三步，查看飛控狀態(tài)。

點(diǎn)擊軟件左邊一列的飛控狀態(tài)即可顯示飛控狀態(tài)界面。如圖3所示。

圖3 飛控狀態(tài)圖

飛控狀態(tài)顯示主要包括了四軸姿態(tài)(PITROLYAW)、接收機(jī)、電機(jī)輸出(PWM)、傳感器原始數(shù)據(jù)(ACCGYRMAG)、氣壓高度。

第四步，點(diǎn)擊匿名科創(chuàng)地面站左邊一列圖標(biāo)的數(shù)據(jù)波形即可顯示波形界面，對(duì)波形界面下欄不同數(shù)據(jù)方框勾中可以在波形界面上顯示出來。如果進(jìn)行數(shù)據(jù)寫入，只需點(diǎn)擊其他功能程序設(shè)置，在對(duì)應(yīng)要寫入的數(shù)據(jù)欄勾中，點(diǎn)擊寫入，軟件會(huì)實(shí)時(shí)記錄飛控相應(yīng)的數(shù)據(jù)，電腦桌面會(huì)并自動(dòng)生成相應(yīng)的excel數(shù)據(jù)表。

3 實(shí)驗(yàn)仿真研究及結(jié)果分析

打開keil uvision5對(duì)建模好的卡爾曼濾波器進(jìn)行編程調(diào)用,銜接原程進(jìn)行編譯，以下是取自卡爾曼反復(fù)實(shí)驗(yàn)濾波編程得到最優(yōu)效果穩(wěn)定氣壓計(jì)程序：

if(SampleAltitude)

{z=-SampleAltitude; Y=Z-X[0];

R[0]=VEKF-R-BARO-INITIAL;//H=[1 0]//K=PH′/(HPH′+R)

K[0]=P[0][0]/(P[0][0]+R[0]);K[1]=P[1][0]/(P[0][0]+R[0]);

X[0]+=K[0]*Y;X[1]+=K[1]*Y;//K時(shí)刻最優(yōu)估計(jì)值//P=(I-KH)P=P-KHP

P[0][0]=tmP[0][0]-K[0]*tmP[0][0];P[0][1]=tmP[0][1]-K[0]*tmP[0][1];

P[1][0]=tmP[1][0]-K[1]*tmP[0][0];P[1][1]=tmP[1][1]-K[1]*tmP[0][1];

tmp[0][0]=P[0][0];tmp[0][1]=p[0][1];

tmP[1][0]=P[1][0];tmP[1][1]=P[1][1];

SampleAltitude=X[0];

點(diǎn)擊匿名科創(chuàng)地面站里面的打開鏈接，給無人機(jī)上升和下降不同懸停的高度，再點(diǎn)擊波形數(shù)據(jù)，打勾ACC-X，就可得到如圖4濾波前的波形.

圖4 濾波前的波形圖

點(diǎn)擊匿名科創(chuàng)地面站里面的其他功能，打勾ACC-X和ACC-Y,點(diǎn)擊數(shù)據(jù)寫入，桌面會(huì)自動(dòng)生成excel濾波前后數(shù)據(jù)表格，通過數(shù)據(jù)生成濾波前后對(duì)比圖，如圖5。

圖5 濾波前后數(shù)據(jù)采集圖形對(duì)比

為了觀察不同位置高度濾波是否具有良好效果，分別讓無人機(jī)懸停在相對(duì)中、低位置進(jìn)行了實(shí)時(shí)的數(shù)據(jù)采集，得到了如圖6～8。

圖6 懸停相對(duì)中端位置數(shù)據(jù)濾波前后對(duì)比圖

結(jié)果分析：從無人機(jī)飛行的不同高度數(shù)據(jù)采集濾波前后的對(duì)比圖和輸入濾波程序后的無人機(jī)現(xiàn)場(chǎng)飛行效果分析表明，飛控狀態(tài)估計(jì)的每一次更新由前一次估計(jì)和新的高度輸入數(shù)據(jù)計(jì)算得到，所以只需存儲(chǔ)前一次狀態(tài)估計(jì)，除了不需要存儲(chǔ)過去的所有量測(cè)數(shù)據(jù)外，擴(kuò)展卡爾曼濾波在計(jì)算效率方面比直接濾波過程中每一步的所有過去數(shù)據(jù)進(jìn)行估計(jì)的方法更加有效，高度新的數(shù)據(jù)輸入時(shí)，無人機(jī)實(shí)時(shí)數(shù)據(jù)采樣會(huì)存在一些滯后，最大可能原因是程序執(zhí)行需要時(shí)間。本次以ARM嵌入式系統(tǒng)作為上位機(jī)，設(shè)計(jì)的擴(kuò)展卡爾曼濾波在飛控中取得的良好的去噪聲效果，懸停不同高度位置中，穩(wěn)定運(yùn)行增強(qiáng)。

圖7 懸停相對(duì)中端位置數(shù)據(jù)濾波前后對(duì)比圖

圖8 懸停相對(duì)低端位置數(shù)據(jù)濾波前后對(duì)比圖

4 結(jié)論

懸停穩(wěn)定性具有重大價(jià)值推廣，因其實(shí)際飛控操作還可以運(yùn)用到民用、商業(yè)場(chǎng)合，本文針對(duì)增加懸停穩(wěn)定性的需要，通過以校園合作公司提供的良好開發(fā)平臺(tái)，無人機(jī)豐富的數(shù)據(jù)采集，設(shè)計(jì)的擴(kuò)展卡爾曼模型，軟件使用，軟件編程調(diào)試完成了階段性無人機(jī)數(shù)據(jù)采集擴(kuò)展卡爾曼濾波增加懸停穩(wěn)定性研究與應(yīng)用。在今后的無人機(jī)研究中將更進(jìn)一步完善，運(yùn)用更智能先進(jìn)的算法提高無人機(jī)懸停運(yùn)行穩(wěn)定性。

[1] Plan Y,Vershynin R.One-bit compressed sensing-by-linear-programming[J].Communication-on-Pure&Applied-Mathematics,2013,66(8):1275-1297.

[2] Hayashi K,Nagahara M,Tanaka T.A user’s guide to compressed sensing for communications systems [J].IEEE Transactions on Communications,2013,96(3):687-712.

[3] Blumensath T.Compressed sensing with nonlinear observations and related nonlinear optimization problems [J].IEEE Transactions on Information Theory,2013,59(6-7):3466-3474.

[4] 李慶鑫，宗 群，王 芳,等.基于魯棒自適應(yīng)的無人機(jī)直升機(jī)懸停控制[J].控制工程,2014,2(2):253-257.

[5] Seassa S,Zecca M,LinZ,et al.A Methodology for the performance Evaluation of Inertial Measurement Units[J].Journal of Intelligent&Robotic Systems,2013,71(2):143-157.

[6] 馬正華,賀小棒.基于預(yù)估測(cè)量值的EKF在手臂測(cè)資中的應(yīng)用[J].計(jì)算機(jī)測(cè)量與控制,2016,24(11).

[7] 盧 浩,倪洪啟,趙艷春,等.無人機(jī)水下定點(diǎn)懸停平衡模塊設(shè)計(jì)[J].機(jī)械工程師,2017(6):31-33.

[8] 都基焱，張 振.十字翼布局無人機(jī)懸停階段控制律設(shè)計(jì)[J].電子設(shè)計(jì)工程,2014,22(3):94-96.

[9] 王海洋,路 平,江 濤.三旋翼構(gòu)型傾轉(zhuǎn)旋翼無人機(jī)建模與懸?？刂蒲芯縖J].電光與控制,2015,22(10):51-55.

[10] 王大鵬,王茂森,戴勁松,等.四旋翼飛行器懸停建模及控制[J].兵工自動(dòng)化,2017(5):36(5).

[11] 申文斌,裴海龍.改進(jìn)的Unscented kalman濾波算法[J].計(jì)算機(jī)工程與科學(xué),2011,33(4):143-157.

[12] Charles K.Chui,CHEN Guanrong.卡爾曼濾波及其實(shí)時(shí)應(yīng)用(第四版)[M].北京：清華大學(xué)出版社,2013.

[13] 王世元,黃錦旺,謝智剛,等.非線性卡爾曼濾波器[M].北京：電子工業(yè)出版社,2015.