視覺導航的四軸飛行器控制系統設計

錢紅亮 成怡 郭強 趙越

【摘 要】本文以四軸飛行器為基礎來研究無人機（UAV）以成為當前研究熱點，為實現四軸飛行器的能夠自主飛行，設計了相應的視覺導航系統。飛行控制部分采用四元數解算姿態，運用經典的PID控制設計了X、Y、Z三個軸的PID控制器進行整個系統的飛行控制。導航方面則融合了慣性導航與視覺導航，提高導航精度。

【關鍵詞】四軸飛行器;視覺導航;四元數

0引言

四軸飛行器是一種能夠垂直起降的小型低空四旋翼無人飛行器，和固定翼飛行器相比，它具有機械結構簡單、成本較低以及飛行穩定性好的特點。它可代替人來完成在海上、廢墟等不適合人員進入的環境提供偵察、救援、繪圖等服務。目前UAV廣泛使用的是GPS或慣性導航系統，然而其使用具有一定的環境局限性，且慣性導航系統誤差隨時間積累，因而僅僅需要一些輔助導航來實現飛行器的精確導航。視覺導航方法，即由機載的視覺傳感器獲得周圍環境的圖像信息，然后通過圖像算法及相機的位置標定解算出載體的位置，由于視覺傳感器是無源的，而且在成本、重量、功率消耗和尺寸上都具有優勢，易于實現。視覺導航方法主要有兩種方法，一類是基于視覺傳感器以視覺導航為主，直接利用圖像特征控制飛行器到指定位置，一般有兩個控制環，外環將實時特征與目標特征相比較，從而估計飛行器的飛行速度，然后通過內環控制電機轉速使達到理想速度，最后使飛行器到達目標位置。另一類是視覺輔助導航，一般利用Kalman濾波方法，融合視覺傳感器和慣性傳感器給出載體位置，從而控制飛行。

1 系統總體設計

四軸飛行器視覺導航系統主要由飛行控制系統和視覺導航系統組成。飛行控制系統主控芯片采用STM32嵌入式處理器，實現對加速度計MPU6050和陀螺儀HMC5883L的數據采集，從而控制飛行姿態。飛行控制部分采用四元數算法，對傳感器采集的數值進行轉化處理，進行姿態解算，利用PID算法設計了相應的控制規律，實現對飛行器的閉環控制[1]。視覺導航系統主要由機載攝像頭實時捕獲周圍環境圖像，以無線的方式傳輸到視覺計算機并經過SIFT算法定位及相機的位置標定解算出載體位置。設計的四軸飛行器如圖1所示。

圖1 四軸飛行器實物圖

2 系統硬件設計

2.1 飛行器控制系統結構

四軸飛行器的控制系統如圖2所示。飛行控制器是系統的核心部分，所設計的控制器要能夠通過采集處理三軸加速度計和陀螺儀傳感器數據解算姿態，并根據導航指令和任務要求，結合相應的PID控制律給出適當的控制信號，控制飛行器的執行機構，改變飛行器的姿態和位置等[2]。整個控制系統包括電源模塊，傳感器模塊，電機驅動模塊和控制模塊。傳感器模塊采用三軸加速度計和陀螺儀實時采集飛行器的飛行狀態，并將飛行器的姿態傳輸給控制器模塊，控制器采用的是ST公司生產的基于ARM Cortex-M3內核的STM32F103RBT6處理器，由于其能耗低，性能高，接口豐富，具有很強的數據處理能力，使得飛控系統能夠穩定精確快速的處理數據。控制器模塊接收到傳感器傳輸的數據，對采集到的三軸角速率，三軸加速度通過四元數算法解算出姿態角，按所設計的的控制規律計算出控制量，轉化為相應的PWM信號，經由驅動電路來控制四個電機工作，保持四軸飛行器穩定飛行。

圖2 飛行控制系統結構圖

2.1.1 姿態測量系統

四軸飛行器飛行器在某個時刻的狀態由6個物理量來描述，即三維坐標中的3個位置量和沿3個軸的姿態量。本次設計的四軸飛行器使用整合性6軸運動處理組件MPU-6050，它整合了三軸加速度和三軸陀螺儀，通過串行總線IIC進行數據訪問和傳感器初始化配置控制，傳感器檢測數據經過四元數算法可得姿態角，此外還可通過慣性導航算法得到飛行器的慣導位置，將慣導位置與視覺位置融合可估算最優位置，從而提高導航的精度。它是實現自動檢測和自動控制的首要環節。在測量過程中由于陀螺儀存在溫漂導致測得的姿態信息并不準確，因此將陀螺儀、加速度計結合起來獲取準確的滾轉角、俯仰角信息。

加速度傳感器：加速度傳感器用于測量機身相對于水平面的傾斜角度，利用了地球萬有引力，把重力加速度投影到X，Y，Z軸上，測量出物體的姿勢。

陀螺儀：利用旋轉物體的旋轉軸所指的方向在不受外力影響時的不變性，測量外力對物體的影響。旋轉物體的旋轉軸方向是不確定的，因而角速度傳感器只能用來測量位置改變，而無法像加速度傳感器和地磁傳感器那樣，測量出物體的絕對角度和姿勢。

2.1.2 電機驅動模塊

四軸飛行器使用無刷電機取代傳統的有刷電機，因為無刷電機具有效率高、轉速快、噪音低、穩態轉速誤差小、沒有電火花產生等優點。此次使用的是新西達A2212無刷電機，配以好盈30A商品電調，簡化了系統設計，而且滿足四軸飛行器快速響應的要求。主控芯片根據飛行姿態信息，結合導航指令和所設計的控制律計算輸出PWM控制量協調控制4個電機，實現穩定飛行。通過控制PWM可以實現對加在兩端實際等效電壓的控制從而實現控制速度，PWM 占空比越高，等效電壓就越高，占空比越低，等效電壓就越低。STM32F103RBT6的定時器有4個捕獲比較寄存器TIMx_CCR1-4，在輸出模式下，該寄存器的值與計數器TIMx_CNT1-4的值比較，根據比較結果產生相應的動作。所以在程序設計中改變TIMx_CCR1-4的值就可以控制PWM的輸出脈寬，從而控制電機轉速。

2.2 視覺導航系統設計

視覺導航系統是將慣性傳感器解算的位置和經由攝像頭采集的圖像經過圖像算法解算的位置融合，找到飛行器精確位置，并將生成的導航指令實時傳送至飛行控制系統，從而指引四軸飛行器進行相應的動作[3～4]。視覺導航系統依然使用STM32F103RBT6處理器作為導航控制器，在圖像解算上利用SIFT算法構建圖像的多尺度空間，然后精確定位特征點的位置。然后根據圖像中提取的最穩定特征點采用最近/次近鄰距離匹配方法對特征點進行匹配，利用已經匹配的特征點對求解基礎矩陣再進一步解算本質矩陣從而求解了飛行器的姿態變換并實施控制。

3 系統軟件分析

本控制器的軟件部分采用C語言進行模塊化開發，主要完成硬件平臺各模塊初始化、傳感器數據采集處理、姿態角解算以及控制律的實現。系統上電后首先是定時器、中斷、串口、傳感器等的初始化及自檢，自檢成功才運行系統并進入任務調度。其中數據采集過程中一方面飛行控制器對MEMS傳感器數據進行采集并處理，另一方面導航控制器控制機載攝像頭采集環境視頻圖像并由無線模塊傳回地面PC上位機。在姿態控制中，采用四元數法解算姿態獲得所需的姿態角，然后結合控制律改變PWM輸出，從而控制4個電機轉速，完成姿態調整。然后導航控制器融合慣導位置和地面PC上位機解算的視覺位置給出飛行器最優位置估計，對控制律解算后生成導航指令反饋給飛行控制系統，指引飛行器下一步動作。

3.1 飛行控制流程

飛行的控制流程步驟分解如下：

1）停在水平地面上，開機自檢，電池電量情況，若電量過低則無法飛行;

2）定電機轉速一致，機身也會發生傾斜。檢測當前航向信息，加速度計測到向上的加速度（積分后得到速度）;

3）和預期航向信息（俯仰角和滾轉角為0，偏航角保持初始值不變）進行比較，當前航向角和預期值出現誤差時，調整電機轉速，以使其達到預期值;

4）控制電機使飛行器速度為0，達到懸停狀態，動態平衡;

5）機載攝像頭對周圍環境進行拍攝，獲取當前位置信息并且尋找特征點，并且通過無線傳輸給上位機;

6）通過SIFT算法對特征點進行匹配，并且將處理后的信息傳輸給控制器，生成導航指令指導飛行器飛行。

四旋翼無人直升機導航到達特征位置成功后，系統進入等待狀態。若有上位機控制命令，則MSU對命令進行解析，執行相關操作。系統執行完命令后再次進入等待狀態，等待新的命令到來。

3.2 SIFT算法圖像匹配

3.2.1 SIFT算法定位特征點的位置

首先利用高斯核函數與圖像進行卷積，得到經高斯模糊變化的圖像，通過改變尺度空間因子生成一系列不同空間尺度的圖像，然后對高斯模糊變化后的圖像利用差分求取尺度空間的極值得到圖像的高斯差分尺度空間，由此構建了圖像的多尺度空間。在構建了圖像的多尺度空間后，對圖像點計算每個尺度下的高斯差分尺度的響應值，將這些響應值連接起來得到特征尺度的軌跡曲線。計算此特征曲線同時在二維空間和尺度空間中的極值點作為該位置點的特征尺度，由此的局部特征點的精確定位。在確定局部特征點的位置后，使用關鍵點的及領域像素的梯度信息，來作為特征點的方向參數，由此每個特征點都包括位置、尺度和方向3個信息。

3.2.2 最近/次近鄰距離匹配方法對特征點進行匹配

取兩幅圖像，分別獲得128維向量的特征集來描述圖片的特征信息，以兩特征集的歐氏距離作為距離函數。由于遍歷算法耗時嚴重，以特征集的每一同維數據與其均值的差方根大小為索引建立Kd-Tree，采用BBF搜素算法 。然后通過歐氏距離以判斷最近距離和次近距離的比值進行匹配。

3.2.3 求解了無人機的姿態變換

首先利用RANSAC作為搜索引擎，基于7點法的解算思想求解基礎矩陣。在獲得基礎矩陣后，對原匹配點進行錯誤匹配點的剔除，保留屬于內點的特征點。同時在利用Camera Caliration Toolbox for Matlab工具包進行攝像機內參標定，最終基礎矩陣結合攝像機內參求得本質矩陣，最終結算解算得到載體的運動信息。

3.3 控制計算

為了提高飛行器控制可靠性，需要得到飛行器的俯仰和滾轉角作為姿態校正，為提高其飛行穩定性，需加入角速率反饋以增加阻尼。飛行器的飛行姿態通過慣性測量單元來獲取，包括陀螺儀、加速度傳感器。由于加速度傳感器測得量為線運動量，角速率陀螺測得量為角運動量，在此將二者都轉化為飛行器的姿態角然后再進行數據融合。角速度傳感器瞬時數據十分精確，但積分后存在較大累積誤差。而加速度傳感器的靜態分量能夠較準確地確定姿態角，但是由于飛行過程中飛行器自身存在加速度，同時加速度計對機械振動十分敏感，其瞬時測量值不能用于反映飛行器當前的姿態。

4 結束語

本研究不僅詳細敘述了四軸飛行器視覺組合導航系統的軟、硬件設計方法，并且利用機載攝像頭將視覺導航和慣性導航相融合的組合導航方式，提高了整個系統的導航精度。該系統不僅能實現起飛、懸停等控制要求，并且能夠自主尋找特征點并且實現飛行控制和定點降落，因此本研究適合近地面的偵查、監視和航拍等任務，具有廣泛的應用范圍。

【參考文獻】

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

[2]楊明志，王敏.四旋翼微型飛行器控制系統設計[J].計算機測量與控制，2008.

[責任編輯：許麗]