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

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

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

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

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

1引言

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

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

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

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

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

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

3.1.1四元素算法介紹

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

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

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

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

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

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

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

3.2.1卡爾曼濾波算法原理

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

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

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

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

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

W（k）和V（k）分別表述過程及測量噪聲。假設(shè)他們?yōu)楦咚拱自肼?，那么，它們的協(xié)方差分別是Q和R。

時間更新（預(yù)測）

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

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

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

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

測量更新（修正）

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

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

（2）對預(yù)測值進(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)的卡爾曼濾波算法原理

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

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

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

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

將式（1）至（7）簡化，其中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為測量真實(shí)值。

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

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

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

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

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

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

實(shí)驗次數(shù)常數(shù)A參數(shù)R真實(shí)誤差累計真實(shí)誤差累計10.50.367.22821.51720.30.367.22813.284

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

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

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

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

綜上所述，本文所設(shè)計的無參考系的飛行器懸停控制系統(tǒng)能夠很好的滿足懸?？刂埔?。

5結(jié)論

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

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

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

參考文獻(xiàn)

[1]劉杰，四軸飛行器的研究與設(shè)計[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的四旋翼無人飛行器控制系統(tǒng)[J].機(jī)電工程，2011，28（10）：1237-1240.

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

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

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

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

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

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

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