四旋翼飛行器懸停算法設計與實現

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

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

關鍵詞：四旋翼飛行器；懸停技術；四元素算法；卡爾曼濾波算法

中圖分類號：TM277文獻標識碼：A

1引言

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

2四旋翼飛行器結構組成及實現

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

3四旋翼飛行器懸停算法設計與實現

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

3.1四元素姿態算法設計與實現

3.1.1四元素算法介紹

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

3.1.2四元素算法實現

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

3.1.3四元素算法結果分析

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

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

3.2改進型卡爾曼濾波算法與實現

3.2.1卡爾曼濾波算法原理

引入一個離散控制系統的過程，該系統可用線性微分方程進行描述如下：

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

加上系統測量值，可以寫成：

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

其中X（k）表示k時刻系統的狀態，U（k）對系統在k時刻的控制量。F和B為系統參數，針對多模型的系統，他們為矩陣。Z（k）是k時刻測量值，H是測量系統其參數，針對多測量的系統，H為矩陣。

W（k）和V（k）分別表述過程及測量噪聲。假設他們為高斯白噪聲，那么，它們的協方差分別是Q和R。

時間更新（預測）

（1）計算先驗狀態估計值

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

（2）計算先驗狀態估計值的協方差

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

測量更新（修正）

（1）計算加權矩陣（卡爾曼增益）

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

（2）對預測值進行修正

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

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

（3）更新修正值的協方差

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

3.2.2改進的卡爾曼濾波算法原理

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

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

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

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

將式（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為測量真實值。

3.2.3改進的卡爾曼濾波算法實驗驗證

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

圖7卡爾曼濾波器實驗1

圖8卡爾曼濾波器實驗2

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

表2卡爾曼濾波實驗結果分析表

實驗次數常數A參數R真實誤差累計真實誤差累計10.50.367.22821.51720.30.367.22813.284

4實驗結果及分析

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

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

視覺定位技術是使用攝像頭采集并分析圖片幀信息，最后得出姿態動作指示圖。飛行器根據姿態動作指示圖調整飛行姿態，以實現飛行器的空間平面坐標定位步驟。實驗結果表明，經過圖像處理系統處理后，能夠得出正確的動作指示圖。

綜上所述，本文所設計的無參考系的飛行器懸停控制系統能夠很好的滿足懸停控制要求。

5結論

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

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

第二：采用氣壓計與視覺定位系統相結合的懸停方案。氣壓計作為空間高度定位的參考數據，與加速度增量積分而來的數據累積量進行加權處理，作為高度定位的參考標準。采用視覺定位技術可以輔助空間高度定位，使其更加穩定，同時，也能防止飛行器長期懸停所帶來的地理位置偏移的情況。

參考文獻

[1]劉杰，四軸飛行器的研究與設計[D].南京：南京郵電大學，2013.

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

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

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

[5]呂強，郭善亮，王冬來，等.基于DSP四旋翼飛行器姿態控制系統硬件設計[J].計算機與數字工程，2011，39（7）：144-146，156.

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

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

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

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

[10]易先軍，周敏，謝亞奇.四旋翼飛行器控制系統的設計與實現[J].武漢工程大學學報，2014，26（11）：59-62，69.

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