四旋翼飛行器懸停算法設(shè)計(jì)與實(shí)現(xiàn)

張國(guó)云 ?k李亞斌 ++向爛群 ++涂兵 ++吳健輝

摘要：以自制四旋翼飛行器為研究對(duì)象，對(duì)四旋翼飛行器懸停算法進(jìn)行詳細(xì)研究。控制中心通過(guò)傳感器MPU6050獲取飛行數(shù)據(jù)，通過(guò)獲取的數(shù)據(jù)計(jì)算得到飛行姿態(tài)，然后結(jié)合氣壓傳感器和攝像頭相結(jié)合進(jìn)行懸停算法的設(shè)計(jì)。程序算法主要包括四元數(shù)、PID、卡爾曼濾波和視覺(jué)定位等算法，計(jì)算得出相應(yīng)的數(shù)據(jù)，最后通過(guò)控制電子調(diào)節(jié)系統(tǒng)驅(qū)動(dòng)無(wú)刷電機(jī)完成其懸停與控制。

關(guān)鍵詞：四旋翼飛行器；懸停技術(shù)；四元素算法；卡爾曼濾波算法

中圖分類號(hào)：TM277文獻(xiàn)標(biāo)識(shí)碼：A

1引言

四旋翼飛行器是以四個(gè)電機(jī)作為動(dòng)力心臟，通過(guò)調(diào)節(jié)電機(jī)轉(zhuǎn)速來(lái)控制飛行的系統(tǒng)[1-2]。國(guó)內(nèi)外眾多學(xué)者對(duì)四旋翼飛行器硬件設(shè)計(jì)和軟件算法都做了大量的工作，比如文獻(xiàn)[3]中提出了PID控制算法和LQR控制算法，實(shí)驗(yàn)結(jié)果表明其控制性能非常優(yōu)異。文獻(xiàn)[4]設(shè)計(jì)了基于嵌入式ARM的控制系統(tǒng)，給出了其硬件詳細(xì)過(guò)程，遺憾的作者沒(méi)有給出其驗(yàn)證其優(yōu)良性能的實(shí)驗(yàn)數(shù)據(jù)。文獻(xiàn)[5]設(shè)計(jì)了基于DSP的控制系統(tǒng)，給出了其設(shè)計(jì)過(guò)程，但是其控制算法比較復(fù)雜，不易實(shí)現(xiàn)。因此本文為了實(shí)現(xiàn)一種性價(jià)比比較高的四旋翼飛行器，以C8051F020單片機(jī)為計(jì)算控制單元，完成了飛行控制系統(tǒng)的硬件設(shè)計(jì)，并制作出的四旋翼飛行器實(shí)物系統(tǒng)，該飛行器具有重量輕、體積小以及功耗低的優(yōu)點(diǎn)。為了實(shí)現(xiàn)飛行器的懸停控制，將四元素、PID、卡爾曼濾波和視覺(jué)定位等算法融合在一起，實(shí)現(xiàn)對(duì)四旋翼飛行器的懸停控制。經(jīng)過(guò)多次室外試驗(yàn)，該飛行器硬件設(shè)計(jì)可靠，軟件算法能精確地實(shí)現(xiàn)飛行器起飛、懸停和降落等飛行模態(tài)的控制要求。

2四旋翼飛行器結(jié)構(gòu)組成及實(shí)現(xiàn)

四旋翼飛行器系統(tǒng)設(shè)計(jì)采用模塊化的設(shè)計(jì)思想，主要包括電源模塊、飛行器系統(tǒng)模塊、電機(jī)驅(qū)動(dòng)模塊、航姿參考系統(tǒng)模塊、輔助定高傳感器模塊、主控制器模塊以及視覺(jué)定位系統(tǒng)模塊七大模塊，其飛行器的系統(tǒng)總體設(shè)計(jì)框圖如圖1所示，主要分為硬件部分和軟件部分，本文主要對(duì)四旋翼飛行器懸停算法進(jìn)行設(shè)計(jì)和分析，對(duì)硬件設(shè)計(jì)只做簡(jiǎn)要介紹。四旋翼飛行器硬件系統(tǒng)包含有遙控器接收頭、系統(tǒng)主控電路、電子調(diào)速器、氣壓計(jì)MS5611、鋰電池電源模塊、視覺(jué)圖像處理電路和OV7670攝像頭模塊。系統(tǒng)主控制電路上擁有MPU6050傳感器，通過(guò)采集其數(shù)據(jù)計(jì)算出飛行器航姿。遙控器接收頭接收到控制信號(hào)，傳輸給系統(tǒng)主控制電路，控制電子調(diào)速器調(diào)整目標(biāo)航姿。氣壓計(jì)MS5611獲取到氣壓數(shù)據(jù)作為懸停參考。視覺(jué)圖像處理電路從OV7670攝像頭模塊中采集圖像幀信息，通過(guò)分析圖像信息得出位圖傳輸給系統(tǒng)主控制電路，進(jìn)而根據(jù)其位圖調(diào)整姿態(tài)，飛行器系統(tǒng)硬件設(shè)計(jì)框圖如圖2所示[5-7]。整個(gè)硬件設(shè)計(jì)完成后制作實(shí)物如圖3所示，四旋翼飛行器系統(tǒng)能夠自主飛行關(guān)鍵在于各種算法的成功應(yīng)用，因此程序設(shè)計(jì)是關(guān)鍵，接下來(lái)將對(duì)幾種算法做詳細(xì)介紹。

3四旋翼飛行器懸停算法設(shè)計(jì)與實(shí)現(xiàn)

懸停算法設(shè)計(jì)是基于視覺(jué)定位技術(shù)與基于氣壓計(jì)的高度定位技術(shù)而成的，它是以器件數(shù)據(jù)為出發(fā)點(diǎn)，中間加入各個(gè)算法與計(jì)算，以關(guān)鍵數(shù)據(jù)為轉(zhuǎn)折點(diǎn)，最后再?gòu)目刂破鬏敵隹刂菩盘?hào)到電調(diào)裝置，最后控制電機(jī)轉(zhuǎn)速，進(jìn)而控制飛行器的飛行。飛行器系統(tǒng)軟件設(shè)計(jì)時(shí)按照數(shù)據(jù)流的去向進(jìn)行設(shè)計(jì)的，大體由飛行器姿態(tài)計(jì)算、攝像頭位圖處理、姿態(tài)PID控制、高度參考系數(shù)計(jì)算和高度PID控制組成，懸停算法設(shè)計(jì)總體框圖如圖4所示。

3.1四元素姿態(tài)算法設(shè)計(jì)與實(shí)現(xiàn)

3.1.1四元素算法介紹

四元素算法由一個(gè)標(biāo)量以及一個(gè)矢量?jī)刹糠纸M成，標(biāo)量和矢量構(gòu)成超復(fù)數(shù)，它具有實(shí)數(shù)和復(fù)數(shù)的所有特征。它作為已知定位參數(shù)，首先可以準(zhǔn)確判斷剛體的某種姿態(tài)，根據(jù)姿態(tài)可以明確剛體的具體位置信息，最終可以計(jì)算出剛體的準(zhǔn)確位姿。如果把它當(dāng)作是變換算子，它可以方便地構(gòu)成可以逆轉(zhuǎn)的矩陣算子，同時(shí)這種算子既可以構(gòu)成單邊算子，也可以構(gòu)成雙邊算子。另外對(duì)于任何一種算子它既可以應(yīng)用于半角，也可以應(yīng)用全角，因此使用非常方便，而且使用范圍非常寬廣。

3.1.2四元素算法實(shí)現(xiàn)

首先把傳感器所測(cè)量到的三維向量轉(zhuǎn)成單位向量，繼而把四元素?fù)Q算成《方向余弦矩陣》中的第三列的三個(gè)元素。根據(jù)余弦矩陣和歐拉角的定義，地理坐標(biāo)系的重力向量變換到機(jī)體坐標(biāo)系，正好是這三個(gè)元素。其中ax、ay、az是加速度傳感器實(shí)際測(cè)量得到的重力向量，而vx、vy、vz是陀螺積分后的姿態(tài)來(lái)推算出的重力向量，它們都是機(jī)體坐標(biāo)參照系上的重力向量。它們之間的誤差向量，就是陀螺積分后的姿態(tài)和加速度傳感器測(cè)出來(lái)的姿態(tài)之間的誤差。向量間的誤差，可以用向量叉積來(lái)表示，ex、ey、ez表示為兩個(gè)重力向量的叉積。經(jīng)過(guò)運(yùn)算后的向量叉積仍舊是位于機(jī)體坐標(biāo)系上的，而陀螺積分誤差也位于機(jī)體坐標(biāo)系中，而且叉積的大小與陀螺積分誤差是成正比例的關(guān)系，因此可以用來(lái)修正陀螺。由于陀螺是對(duì)機(jī)體的直接積分，因此對(duì)陀螺的修正量會(huì)直接體現(xiàn)在對(duì)機(jī)體坐標(biāo)系的修正上。然后可以利用叉積誤差來(lái)做PI修正陀螺的零偏，最終可以解出四元素微分方程，這就是四元素算法的整個(gè)編程思路[8-9]。

3.1.3四元素算法結(jié)果分析

為了將四元素姿態(tài)算法應(yīng)用于四軸飛行器，首先得驗(yàn)證四元素姿態(tài)算法的正確性和有效性。由于本系統(tǒng)的數(shù)據(jù)采樣，必須在系統(tǒng)運(yùn)動(dòng)的時(shí)候計(jì)算角度通過(guò)串口顯示在上位機(jī)上，并記錄數(shù)據(jù)。其算法測(cè)試原理如圖5所示。主控芯片在計(jì)算出三個(gè)方向的角度后，馬上通過(guò)串口，以一定的格式傳遞給上位機(jī)，這樣就可以記錄數(shù)據(jù)，并驗(yàn)證數(shù)據(jù)。

其四元素姿態(tài)算法測(cè)試結(jié)果如表1所示，從表1可以看出所有角度經(jīng)過(guò)四元素算法融合之后，三個(gè)方向上的角度都完全正確，完全能夠滿足四軸飛行器對(duì)動(dòng)態(tài)平衡控制的要求。

3.2改進(jìn)型卡爾曼濾波算法與實(shí)現(xiàn)

3.2.1卡爾曼濾波算法原理

引入一個(gè)離散控制系統(tǒng)的過(guò)程，該系統(tǒng)可用線性微分方程進(jìn)行描述如下：

X（k）=FX（k）+BU（k）+W（k）（1）endprint

加上系統(tǒng)測(cè)量值，可以寫成：

Z（k）=HX（k）+V（k）（2）

其中X（k）表示k時(shí)刻系統(tǒng)的狀態(tài)，U（k）對(duì)系統(tǒng)在k時(shí)刻的控制量。F和B為系統(tǒng)參數(shù)，針對(duì)多模型的系統(tǒng)，他們?yōu)榫仃嚒（k）是k時(shí)刻測(cè)量值，H是測(cè)量系統(tǒng)其參數(shù)，針對(duì)多測(cè)量的系統(tǒng)，H為矩陣。

W（k）和V（k）分別表述過(guò)程及測(cè)量噪聲。假設(shè)他們?yōu)楦咚拱自肼暎敲矗鼈兊膮f(xié)方差分別是Q和R。

時(shí)間更新（預(yù)測(cè)）

（1）計(jì)算先驗(yàn)狀態(tài)估計(jì)值

x（k|k-1）=Fx（k-1|k-1）+Bu（k）+w（k）（3）

（2）計(jì)算先驗(yàn)狀態(tài)估計(jì)值的協(xié)方差

P（k|k-1）=FP（k-1|k-1）F′+Q（4）

測(cè)量更新（修正）

（1）計(jì)算加權(quán)矩陣（卡爾曼增益）

Kg（k）=P（k|k-1）H′/（HP（k|k-1）H′+R）（5）

（2）對(duì)預(yù)測(cè)值進(jìn)行修正

x（k|k）=x（k|k-1）+Kg（k）（Z（k）-

HX（k|k-1））（6）

（3）更新修正值的協(xié)方差

P（k|k）=（I-Kg（k）H）P（k|k-1）（7）

3.2.2改進(jìn)的卡爾曼濾波算法原理

對(duì)于四旋翼飛行器系統(tǒng)而言，傳感器數(shù)據(jù)在飛行器的不同運(yùn)動(dòng)姿態(tài)中，所測(cè)量數(shù)據(jù)的噪聲是大不一樣的。這些噪聲即是系統(tǒng)噪聲，傳感器的測(cè)量誤差是一定的，為常量。而在一般的模型系統(tǒng)中，系統(tǒng)參數(shù)F、B均為單位量常數(shù)，卡爾曼濾波器在長(zhǎng)時(shí)間計(jì)算過(guò)后會(huì)有記憶特性，增益Kg將會(huì)恒為常量，因此濾波器對(duì)動(dòng)態(tài)數(shù)據(jù)的跟隨性變差。

為滿足四旋翼飛行器系統(tǒng)的應(yīng)用需要，在卡爾曼的五個(gè)公式中加入系統(tǒng)誤差更新，從給出改進(jìn)型卡爾曼濾波算法。

Qk=Axk-1k-12-xk-2k-22A≤1（8）

A為常數(shù)，參數(shù)A的越小，濾波器的靜態(tài)平均誤差值越小，濾波器對(duì)數(shù)據(jù)跟隨遲滯周期變長(zhǎng)。反之，濾波器的靜態(tài)平均誤差值越小，濾波器對(duì)數(shù)據(jù)跟隨遲滯周期變短。

將式（1）至（7）簡(jiǎn)化，其中F=1，B=1，w（k）=0，H=0，因此得到以下公式：

X0=X1+U0（9）

P=P+Q（10）

Kg=P/（P+R）（11）

X0=X0+Kg×（Z-X0）（12）

P=（1-Kg）×P（13）

Q=A×|X1-X0|（14）

X1=X0（15）

式（9）中U0為控制量，式（13）中的Z為測(cè)量真實(shí)值。

3.2.3改進(jìn)的卡爾曼濾波算法實(shí)驗(yàn)驗(yàn)證

為驗(yàn)證改進(jìn)的卡爾曼濾波器的濾波效果，引入隨機(jī)數(shù)模擬測(cè)量誤差，U0=0，R=0.3，A=0.5，實(shí)驗(yàn)數(shù)據(jù)記錄如圖7所示。引入另外一組隨機(jī)數(shù)模擬測(cè)量誤差，當(dāng)U0=0，R=0.3，A=0.3時(shí)，實(shí)驗(yàn)數(shù)據(jù)記錄如圖8所示。

圖7卡爾曼濾波器實(shí)驗(yàn)1

圖8卡爾曼濾波器實(shí)驗(yàn)2

根據(jù)圖7和圖8所示的卡爾曼濾波實(shí)驗(yàn)可以給出卡爾曼濾波實(shí)驗(yàn)分析表如表2所示。表2中的卡爾曼濾波結(jié)果表明：改進(jìn)的卡爾曼濾波器能夠有效的減少系統(tǒng)誤差。在參數(shù)A為0.3的情況下，經(jīng)過(guò)估計(jì)后的誤差值只有13.284，相比第一次實(shí)驗(yàn)參數(shù)A為0.5的情況減少了約百分之四十五。參數(shù)A越大，靜態(tài)誤差累計(jì)值越小，濾波器的動(dòng)態(tài)性能越差。

表2卡爾曼濾波實(shí)驗(yàn)結(jié)果分析表

實(shí)驗(yàn)次數(shù)常數(shù)A參數(shù)R真實(shí)誤差累計(jì)真實(shí)誤差累計(jì)10.50.367.22821.51720.30.367.22813.284

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

在本文中，使用四旋翼飛行器作為研究平臺(tái)進(jìn)行懸停算法的研究與設(shè)計(jì)，懸停內(nèi)容包含飛行器航姿角度計(jì)算、空間高度定位和空間地理坐標(biāo)系定位，經(jīng)過(guò)多次實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如下所示：首先對(duì)四旋翼飛行器的姿態(tài)角度進(jìn)行實(shí)驗(yàn)分析，本系統(tǒng)使用MPU6050作為姿態(tài)計(jì)算的數(shù)據(jù)源，通過(guò)卡爾曼濾波器和四元數(shù)的計(jì)算，得出飛行器的航姿角度。實(shí)驗(yàn)結(jié)果表明，飛行器的航姿角度計(jì)算正確，實(shí)時(shí)性好，精確度高，且不會(huì)存在死角問(wèn)題。

其次，四旋翼飛行器的空間高度定位進(jìn)行實(shí)驗(yàn)分析，本系統(tǒng)使用的是氣壓計(jì)和加速度計(jì)Z軸數(shù)據(jù)的累積量，再進(jìn)行加權(quán)操作，其中，加速度使用了旋轉(zhuǎn)矩陣進(jìn)行處理。實(shí)驗(yàn)結(jié)果表明，在相對(duì)高度空間不變地情況下，飛行器進(jìn)行姿態(tài)角度變換操作，其加權(quán)值基本上沒(méi)有變化，完全滿足設(shè)計(jì)要求。

視覺(jué)定位技術(shù)是使用攝像頭采集并分析圖片幀信息，最后得出姿態(tài)動(dòng)作指示圖。飛行器根據(jù)姿態(tài)動(dòng)作指示圖調(diào)整飛行姿態(tài)，以實(shí)現(xiàn)飛行器的空間平面坐標(biāo)定位步驟。實(shí)驗(yàn)結(jié)果表明，經(jīng)過(guò)圖像處理系統(tǒng)處理后，能夠得出正確的動(dòng)作指示圖。

綜上所述，本文所設(shè)計(jì)的無(wú)參考系的飛行器懸停控制系統(tǒng)能夠很好的滿足懸停控制要求。

5結(jié)論

以自制四旋翼無(wú)人飛行器為研究對(duì)象，對(duì)于無(wú)參考系飛行器的懸停進(jìn)行了研究與設(shè)計(jì)，可以得出如下結(jié)論：

第一：采用四元數(shù)和卡爾曼融合技術(shù)參與計(jì)算，可以提高精度和準(zhǔn)確性。實(shí)驗(yàn)結(jié)果表示，卡爾曼濾波器進(jìn)行數(shù)據(jù)融合能夠得到更加準(zhǔn)確的航姿估計(jì)值，四元素能夠很好的解決旋轉(zhuǎn)矩陣計(jì)算所帶來(lái)的死角問(wèn)題。

第二：采用氣壓計(jì)與視覺(jué)定位系統(tǒng)相結(jié)合的懸停方案。氣壓計(jì)作為空間高度定位的參考數(shù)據(jù)，與加速度增量積分而來(lái)的數(shù)據(jù)累積量進(jìn)行加權(quán)處理，作為高度定位的參考標(biāo)準(zhǔn)。采用視覺(jué)定位技術(shù)可以輔助空間高度定位，使其更加穩(wěn)定，同時(shí)，也能防止飛行器長(zhǎng)期懸停所帶來(lái)的地理位置偏移的情況。

參考文獻(xiàn)

[1]劉杰，四軸飛行器的研究與設(shè)計(jì)[D].南京：南京郵電大學(xué)，2013.

[2]侯永鋒，陸連山，高尚德，等.基于PD算法的四旋翼飛行器控制系統(tǒng)研究[J].機(jī)械科學(xué)與技術(shù)，2012，31（3）：359-362.

[3]TommasoBresciani.Modelling，IdentificateionandControlofaQuadrotorHelicopter[D].DepartmentofAutomaticControlLundUniversity，2008.

[4]劉乾，孫志鋒.基于ARM的四旋翼無(wú)人飛行器控制系統(tǒng)[J].機(jī)電工程，2011，28（10）：1237-1240.

[5]呂強(qiáng)，郭善亮，王冬來(lái)，等.基于DSP四旋翼飛行器姿態(tài)控制系統(tǒng)硬件設(shè)計(jì)[J].計(jì)算機(jī)與數(shù)字工程，2011，39（7）：144-146，156.

[6]朱江樂(lè)，蔣東方，李俊華.基于虛擬儀器的低頻掃頻儀設(shè)計(jì)[J].科學(xué)技術(shù)與工程，2008，8（6）：1580-1582.

[7]王偉，馬浩，孫長(zhǎng)銀.四旋翼飛行器設(shè)計(jì)與平穩(wěn)控制仿真研究[J].科學(xué)技術(shù)與工程，2013，13（19）：5513-5519.

[8]王史春.四旋翼飛行器力學(xué)模型與控制系統(tǒng)設(shè)計(jì)[J].中北大學(xué)學(xué)報(bào)：自然科學(xué)版，2014，35（2）：218-214.

[9]江杰，豈偉楠.四旋翼飛行器建模與PID控制器設(shè)計(jì)[J].電子設(shè)計(jì)工程，2013，23（21）：147-150.

[10]易先軍，周敏，謝亞奇.四旋翼飛行器控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[J].武漢工程大學(xué)學(xué)報(bào)，2014，26（11）：59-62，69.

第35卷第1期2016年3月計(jì)算技術(shù)與自動(dòng)化ComputingTechnologyandAutomationVol35，No1Mar.2016第35卷第1期2016年3月計(jì)算技術(shù)與自動(dòng)化ComputingTechnologyandAutomationVol35，No1Mar.2016endprint