視覺導(dǎo)航的四軸飛行器控制系統(tǒng)設(shè)計(jì)

錢紅亮 成怡 郭強(qiáng) 趙越

【摘 要】本文以四軸飛行器為基礎(chǔ)來研究無人機(jī)（UAV）以成為當(dāng)前研究熱點(diǎn)，為實(shí)現(xiàn)四軸飛行器的能夠自主飛行，設(shè)計(jì)了相應(yīng)的視覺導(dǎo)航系統(tǒng)。飛行控制部分采用四元數(shù)解算姿態(tài)，運(yùn)用經(jīng)典的PID控制設(shè)計(jì)了X、Y、Z三個(gè)軸的PID控制器進(jìn)行整個(gè)系統(tǒng)的飛行控制。導(dǎo)航方面則融合了慣性導(dǎo)航與視覺導(dǎo)航，提高導(dǎo)航精度。

【關(guān)鍵詞】四軸飛行器;視覺導(dǎo)航;四元數(shù)

0引言

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

1 系統(tǒng)總體設(shè)計(jì)

四軸飛行器視覺導(dǎo)航系統(tǒng)主要由飛行控制系統(tǒng)和視覺導(dǎo)航系統(tǒng)組成。飛行控制系統(tǒng)主控芯片采用STM32嵌入式處理器，實(shí)現(xiàn)對(duì)加速度計(jì)MPU6050和陀螺儀HMC5883L的數(shù)據(jù)采集，從而控制飛行姿態(tài)。飛行控制部分采用四元數(shù)算法，對(duì)傳感器采集的數(shù)值進(jìn)行轉(zhuǎn)化處理，進(jìn)行姿態(tài)解算，利用PID算法設(shè)計(jì)了相應(yīng)的控制規(guī)律，實(shí)現(xiàn)對(duì)飛行器的閉環(huán)控制[1]。視覺導(dǎo)航系統(tǒng)主要由機(jī)載攝像頭實(shí)時(shí)捕獲周圍環(huán)境圖像，以無線的方式傳輸?shù)揭曈X計(jì)算機(jī)并經(jīng)過SIFT算法定位及相機(jī)的位置標(biāo)定解算出載體位置。設(shè)計(jì)的四軸飛行器如圖1所示。

圖1 四軸飛行器實(shí)物圖

2 系統(tǒng)硬件設(shè)計(jì)

2.1 飛行器控制系統(tǒng)結(jié)構(gòu)

四軸飛行器的控制系統(tǒng)如圖2所示。飛行控制器是系統(tǒng)的核心部分，所設(shè)計(jì)的控制器要能夠通過采集處理三軸加速度計(jì)和陀螺儀傳感器數(shù)據(jù)解算姿態(tài)，并根據(jù)導(dǎo)航指令和任務(wù)要求，結(jié)合相應(yīng)的PID控制律給出適當(dāng)?shù)目刂菩盘?hào)，控制飛行器的執(zhí)行機(jī)構(gòu)，改變飛行器的姿態(tài)和位置等[2]。整個(gè)控制系統(tǒng)包括電源模塊，傳感器模塊，電機(jī)驅(qū)動(dòng)模塊和控制模塊。傳感器模塊采用三軸加速度計(jì)和陀螺儀實(shí)時(shí)采集飛行器的飛行狀態(tài)，并將飛行器的姿態(tài)傳輸給控制器模塊，控制器采用的是ST公司生產(chǎn)的基于ARM Cortex-M3內(nèi)核的STM32F103RBT6處理器，由于其能耗低，性能高，接口豐富，具有很強(qiáng)的數(shù)據(jù)處理能力，使得飛控系統(tǒng)能夠穩(wěn)定精確快速的處理數(shù)據(jù)?？刂破髂K接收到傳感器傳輸?shù)臄?shù)據(jù)，對(duì)采集到的三軸角速率，三軸加速度通過四元數(shù)算法解算出姿態(tài)角，按所設(shè)計(jì)的的控制規(guī)律計(jì)算出控制量，轉(zhuǎn)化為相應(yīng)的PWM信號(hào)，經(jīng)由驅(qū)動(dòng)電路來控制四個(gè)電機(jī)工作，保持四軸飛行器穩(wěn)定飛行。

圖2 飛行控制系統(tǒng)結(jié)構(gòu)圖

2.1.1 姿態(tài)測(cè)量系統(tǒng)

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

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

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

2.1.2 電機(jī)驅(qū)動(dòng)模塊

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

2.2 視覺導(dǎo)航系統(tǒng)設(shè)計(jì)

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

3 系統(tǒng)軟件分析

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

3.1 飛行控制流程

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

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

2）定電機(jī)轉(zhuǎn)速一致，機(jī)身也會(huì)發(fā)生傾斜。檢測(cè)當(dāng)前航向信息，加速度計(jì)測(cè)到向上的加速度（積分后得到速度）;

3）和預(yù)期航向信息（俯仰角和滾轉(zhuǎn)角為0，偏航角保持初始值不變）進(jìn)行比較，當(dāng)前航向角和預(yù)期值出現(xiàn)誤差時(shí)，調(diào)整電機(jī)轉(zhuǎn)速，以使其達(dá)到預(yù)期值;

4）控制電機(jī)使飛行器速度為0，達(dá)到懸停狀態(tài)，動(dòng)態(tài)平衡;

5）機(jī)載攝像頭對(duì)周圍環(huán)境進(jìn)行拍攝，獲取當(dāng)前位置信息并且尋找特征點(diǎn)，并且通過無線傳輸給上位機(jī);

6）通過SIFT算法對(duì)特征點(diǎn)進(jìn)行匹配，并且將處理后的信息傳輸給控制器，生成導(dǎo)航指令指導(dǎo)飛行器飛行。

四旋翼無人直升機(jī)導(dǎo)航到達(dá)特征位置成功后，系統(tǒng)進(jìn)入等待狀態(tài)。若有上位機(jī)控制命令，則MSU對(duì)命令進(jìn)行解析，執(zhí)行相關(guān)操作。系統(tǒng)執(zhí)行完命令后再次進(jìn)入等待狀態(tài)，等待新的命令到來。

3.2 SIFT算法圖像匹配

3.2.1 SIFT算法定位特征點(diǎn)的位置

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

3.2.2 最近/次近鄰距離匹配方法對(duì)特征點(diǎn)進(jìn)行匹配

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

3.2.3 求解了無人機(jī)的姿態(tài)變換

首先利用RANSAC作為搜索引擎，基于7點(diǎn)法的解算思想求解基礎(chǔ)矩陣。在獲得基礎(chǔ)矩陣后，對(duì)原匹配點(diǎn)進(jìn)行錯(cuò)誤匹配點(diǎn)的剔除，保留屬于內(nèi)點(diǎn)的特征點(diǎn)。同時(shí)在利用Camera Caliration Toolbox for Matlab工具包進(jìn)行攝像機(jī)內(nèi)參標(biāo)定，最終基礎(chǔ)矩陣結(jié)合攝像機(jī)內(nèi)參求得本質(zhì)矩陣，最終結(jié)算解算得到載體的運(yùn)動(dòng)信息。

3.3 控制計(jì)算

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

4 結(jié)束語

本研究不僅詳細(xì)敘述了四軸飛行器視覺組合導(dǎo)航系統(tǒng)的軟、硬件設(shè)計(jì)方法，并且利用機(jī)載攝像頭將視覺導(dǎo)航和慣性導(dǎo)航相融合的組合導(dǎo)航方式，提高了整個(gè)系統(tǒng)的導(dǎo)航精度。該系統(tǒng)不僅能實(shí)現(xiàn)起飛、懸停等控制要求，并且能夠自主尋找特征點(diǎn)并且實(shí)現(xiàn)飛行控制和定點(diǎn)降落，因此本研究適合近地面的偵查、監(jiān)視和航拍等任務(wù)，具有廣泛的應(yīng)用范圍。

【參考文獻(xiàn)】

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

[2]楊明志，王敏.四旋翼微型飛行器控制系統(tǒng)設(shè)計(jì)[J].計(jì)算機(jī)測(cè)量與控制，2008.

[責(zé)任編輯：許麗]