基于視覺的無人機定點飛行控制技術研究

陳崟松，吳 芬，吳 妍

（1.南京理工大學教育實驗學院，江蘇南京 210094；2.南京理工大學自動化院，江蘇南京 210094）

無人機（Unmanned Aerial Vehicles，UAV）是利用無線電遙控和自備的程序控制裝置操縱的不載人飛機[1]。目前無人機主要可分為固定翼無人機，旋翼無人機以及撲翼式無人機[2]。而旋翼無人機因其機動性好，起落受限制少，能適應多種復雜環(huán)境[3]，而被廣泛運用于軍事偵察、目標跟蹤、地形勘探、輸電線路維護[4]等領域，而四旋翼無人機則是其中最流行的一種。

無人機的導航通常采用慣性導航系統(tǒng)[5]（Inertial Navigation System，INS）和全球定位系統(tǒng)（Global Position System，GPS），但是導航過程中慣性器件具有積累誤差，對初始值過于敏感的缺點，而GPS又受環(huán)境約束，在室內(nèi)和復雜環(huán)境下會難以獲取信號，同時精度也往往滿足不了無人機的要求。

近年來，隨著計算機視覺的飛速發(fā)展，使運用視頻圖像處理的方法實現(xiàn)無人機導航成為可能。首先，視覺傳感器具有價格便宜、采集信息豐富的優(yōu)點。同時，視覺傳感器可以與慣性導航和GPS結合，提升導航精度[6]。此外，相比于無線電與GPS，視覺導引抗干擾性能更好[7]。

在無人機視覺方面，世界很多研究機構已經(jīng)做出很多嘗試?？突仿〈髮W的研究人員使用雙目視覺、模板匹配、特征跟蹤的方法實現(xiàn)四旋翼無人機目標識別與跟蹤[8]。近年來國內(nèi)也出現(xiàn)了基于視覺的無人機飛行控制研究成果。王翔鵬[9]提出了在GPS失效情況下農(nóng)用無人機視覺導航算法，主要利用田間直線等信息進行無人機被動定位。

文中從硬件選擇，圖像處理，飛行控制3個方面介紹基于視覺的定點飛行控制技術。

1 硬件平臺搭建

基于視覺的四旋翼無人機定點飛行系統(tǒng)由無人機硬件平臺以及PC（Personal Compute，個人計算機）上位機兩部分組成，系統(tǒng)總體硬件組成及通信結構如圖1所示。為了減小無人機負載，方便我們對算法執(zhí)行過程進行監(jiān)測，我們將視頻圖像處理工作交給PC機處理，為此將攝像機獲得的視頻信息通過無線圖像傳輸設備發(fā)送給PC機，當PC機完成視覺處理后將偏差信息通過無線數(shù)據(jù)傳輸設備發(fā)送給無人機，作為導航指令輸入飛行控制器。

圖1 系統(tǒng)結構圖

1.1 無人機硬件平臺

我們選用了Pixhawk飛控板，Gopro Hero4運動攝像機，XL-MaxSonar-EZ4聲吶，XBEE pro無線數(shù)傳，大疆公司的5.8G圖傳以及無人機自穩(wěn)云臺等器件搭建無人機硬件平臺。理由如下：

1）Pixhawk飛控板配有MPU6000三軸加速度計/陀螺儀、LSM303D磁力計和MS5611氣壓計，性能優(yōu)越，同時具備進行二次開發(fā)便捷的優(yōu)點。

2）XL-MaxSonar-EZ4聲吶，其內(nèi)嵌了距離讀數(shù)實時噪聲抑制算法，即便在電氣噪聲源存在的環(huán)境下依然能夠正常工作，能準確采集無人機實時高度。

3）無線圖傳與數(shù)傳用于無人機與PC之間的數(shù)據(jù)與圖像傳輸交互，我們選用的XBEE pro無線數(shù)傳與大疆公司的5.8G圖傳都具有實時穩(wěn)定且小巧堅固的優(yōu)點。

4）自穩(wěn)云臺則用于使得攝像機的鏡頭始終豎直向下，隔離了視覺信息采集與四旋翼無人機姿態(tài)抖動，方便之后的圖像處理。

5）Gopro Hero4攝像機可以在2.7 K和1 080 P的畫質(zhì)下連續(xù)拍攝1小時40分鐘左右，充分滿足了我們的需求。圖2為搭建完成的四旋翼無人機。

圖2 四旋翼無人機

1.2 PC上位機

PC機采用Intel Core i3-2310M處理器，工作頻率為2.1 GHz，內(nèi)存為4 G，安裝32位操作系統(tǒng)，使用Samsung SSD固態(tài)硬盤，處理器性能優(yōu)越，圖像處理速度快。

2 圖像處理

為了實現(xiàn)對地面標識點的跟蹤，我們要對攝像機采集到的圖像進行處理，主要分為3個步驟：1）圖像預處理；2）目標跟蹤；3）上位機設計。

2.1 圖像預處理

在無人機飛行過程中由于機體的機械振動和無線傳輸模塊導致的噪聲會對攝像頭采集的圖像造成干擾，因此對圖像進行預處理是很重要的。本文中我們采取了中值濾波[10]的方法對原始圖像進行處理，將每一像素點的灰度值設置為該點某鄰域窗口內(nèi)的所有像素點灰度值的中值，從而消除孤立噪聲點，同時又可以保持圖像的邊緣特性[11]。

2.2 目標跟蹤算法

本文采用基于卡爾曼濾波的Camshift（Continuously Adaptively Mean Shift，連續(xù)自適應均值偏移）算法[12]，即對于當前一幀而言，已知當前目標物重心的位置，利用卡爾曼濾波預測下一幀目標物的位置，并與Camshift算法解算出的位置進行最優(yōu)估計，得到最優(yōu)的目標位置實現(xiàn)最優(yōu)跟蹤。

2.2.1 Camshift算法組成

Camshift算法[13]可以分為3個部分：反向投影計算、Mean Shift算法和Camshift算法。算法流程圖如圖3所示，虛線框內(nèi)為Mean shift算法。

圖3 Camshift算法流程圖

2.2.2 反向投影

RGB顏色空間對亮度變化較為敏感，為了降低此變化對目標跟蹤效果的影響，首先將圖像從RGB空間轉(zhuǎn)換到HSV空間（Hue，Saturation，Value色調(diào)，飽和度，亮度）。然后對其中的H分量進行直方圖統(tǒng)計得到H分量的直方圖，再對圖像中每個像素的值用其顏色出現(xiàn)的概率對替換，就獲得了顏色概率分布圖，即反向投影圖。原圖像中每一個像素值反映的是在這一點的顏色信息，在反向投影后，圖像中每一個像素值轉(zhuǎn)換成了該顏色信息出現(xiàn)在此處的一種離散化概率度量，為之后的跟蹤提供了線索。

RGB顏色空間模型與HSV顏色空間模型轉(zhuǎn)化公式如下：

1）計算max=max（R，G，B），min=min（R，G，B）；

2）如果R=max；H=（G-B）/（max-min）；如果G=max；H=2+（B+R）/（max-min）；如果B=max，H=4+（R-G）/（max-min）；

3）將上一步中計算得到的H乘以60得到角度，若此時H小于0，則H=H+360；

4）V=max（R，G，B），S=（max-min）/max。

2.2.3 Mean Shift算法

Mean Shift算法（圖3中虛線框部分）在已知目標圖像的反向投影和初始搜索窗口位置的情況下，用迭代法尋找跟蹤目標的質(zhì)心，當搜索窗口中心的移動距離小于固定閾值或者循環(huán)達到某一個固定次數(shù)，停止迭代。

設I（x，y）為反向投影圖中（x，y）處的像素值，x與y的變化范圍即搜索窗的范圍。

零階距：

一階距：

搜索窗口質(zhì)心：

2.2.4 Camshift算法實現(xiàn)

Camshift算法的基本原理是在Mean Shift上實現(xiàn)了搜索窗口的自適應，即在每次進行搜索之前，根據(jù)當前鎖定目標的形狀與尺寸確定下一幀窗口的尺寸。

對于當前圖像，若以橢圓形狀鎖定此時跟蹤的目標，則計算鎖定窗口的二階距：

，經(jīng)過質(zhì)心且相互垂直的長軸l與短軸w如下所示：

根據(jù)上式，在每次進行搜索前都自適應調(diào)整搜索窗口的窗口，從而解決由于目標物發(fā)生外形變化而導致跟不上的情況。

2.2.5 卡爾曼濾波

卡爾曼濾波是一種以均方誤差最小作為估計時的準則從而得到一套遞推估計的濾波方法。其基本思想是：建立信號與噪聲的狀態(tài)空間模型，通過前一時刻的估計值和當前時刻的觀測值實現(xiàn)對狀態(tài)變量的更新與估計，從而得到當前時刻的估計值。對于滿足狀態(tài)和測量都是高斯噪聲且系統(tǒng)可描述為線性隨機微分方程（如式（8）所示），我們可以采取卡爾曼濾波，系統(tǒng)可表述如下：

狀態(tài)方程：

觀測方程：

其中，k∈N是時間指標，Xk表示k時刻的狀態(tài)向量，Zk為k時刻系統(tǒng)狀態(tài)的觀測向量，Ak與Hk分別表示系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣與觀測矩陣，Uk-1為k-1時刻的外部控制量，在本文中取0。Wk與Vk表示k時刻的狀態(tài)與觀測噪聲，通?？醋髡龖B(tài)分布。矩陣Q表示狀態(tài)噪聲的協(xié)方差矩陣，R表示測量時噪聲的協(xié)方差矩陣，即Wk～N(0,Q)，Vk～N(0,R)。

卡爾曼濾波公式如下：

預測值和預測誤差的協(xié)方差矩陣分別為：

獲取新的觀測Zk后，最優(yōu)估算值、卡爾曼增益和更新的濾波誤差協(xié)方差矩陣分別為：

卡爾曼濾波公式可以利用正交定理推導，推導過程可參考文獻[14]。

2.2.6 基于卡爾曼濾波的Camshift算法

在目標跟蹤過程中，由于相鄰兩幀圖像間隔時間較短，目標運動狀態(tài)變化較小，可以假設目標在兩幀時間間隔內(nèi)做勻速運動。定義目標運動狀態(tài)向量Xk=(xk,yk,vxk,vyk)T，觀測狀態(tài)向量Zk=(xk,yk)。xk,yk表示目標質(zhì)心在圖像中位置，vxk,vyk表示目標運動的速度。故由2.2.2可知：

主要流程可以概括為：1）初始化。選擇0時刻的窗口大小位置作為Camshift算法的輸入，同時對0時刻窗口狀態(tài)向量X0進行初始化。2）迭代。在時刻k，將上一時刻最優(yōu)估計k-1|k-1代入式（11）得到|k-1，將Camshift算法給出的目標質(zhì)心位置作為觀測值Zk，把Zk代入式（13）對k|k-1進行修正得到Xk的最優(yōu)估計|k，再將|k中的目標質(zhì)心的位置分量作為下一時刻Camshift算法的輸入。

在跟蹤過程中，Camshift算法可依據(jù)目標大小自主調(diào)節(jié)搜索窗口大小，在目標突然跟蹤失敗無法繼續(xù)跟蹤的情況下，基于卡爾曼濾波的Camshift算法會產(chǎn)生一個可擴大窗口進一步進行目標跟蹤。此外，搜索窗口的位置精度也有所提高。

2.3 上位機設計

本文中上位機主要完成以下事件：1）對傳輸過來的圖像進行處理，包括目標跟蹤與求取位置偏差兩個部分，又由于本文中攝像頭的位置相對于無人機是固定不變的，我們可以利用圖像中目標物位置與圖像中心的偏差來比擬實際無人機與目標物的位置偏差；2）將位置偏差實時返回。

對事件1我們采用Visual Studio編寫上位機人機交互界面同時加載Open CV的庫文件輔助圖像處理實現(xiàn)。對事件2我們采用串口通信，主要包括了串口通信的初始化設置（包括串口號的選擇，波特率的設定，數(shù)據(jù)位的選擇以及奇偶校驗位的設置），串口通信的發(fā)送（由于在本文中，只需要上位機界面給下位機飛行控制板發(fā)送位置偏差，因此不需要設計串口接收的部分）。

通信時采用數(shù)據(jù)發(fā)送格式如表1所示。

表1 數(shù)據(jù)發(fā)送格式

上表中，0xFA與0xAF均為設計的校驗位。在下位機飛控板端，通過不斷接收6個字符，判斷第一位與最后一位是否為設計的校驗位，從而保證發(fā)送正確。最終設計的上位機界面如圖4所示。

圖4 上位機界面

3 飛行控制

3.1 控制系統(tǒng)建立與仿真模型搭建

PID算法是一種不依賴被控對象模型，只和系統(tǒng)輸入輸出數(shù)據(jù)有關的控制方法，又因為四旋翼無人機有4個控制輸入量與6個狀態(tài)輸出量，是一個欠驅(qū)動系統(tǒng)，很難準確建模，所以我們選用PID控制方法。

文獻[15]向我們展示了關于四旋翼無人機比較成熟的動力學建模方法，本文基于該方法結合圖像處理返回的x與y方向的偏差利用simulink仿真工具搭建控制模型。又由于無人機在根據(jù)偏差進行橫滾與俯仰校準，將控制量輸送給電機時，會導致無人機高度上的變化。因此，在飛行過程中需要考慮保持無人機的飛行高度，而且飛行高度的穩(wěn)定也有利于攝像頭返回穩(wěn)定的圖像。最終得到控制框圖如圖5所示。

圖5 控制模型框圖

根據(jù)本文所搭建的無人機，結合相關文獻，得到相關變量的仿真參數(shù)如表2。

表2 simulink仿真參數(shù)表

3.2 PID參數(shù)整定與仿真結果

PID控制器參數(shù)整定，是指在控制器規(guī)律已經(jīng)確定為PID形式的情況下，通過調(diào)整控制器的參數(shù)，使得由控制對象、控制器等組成的控制回路的動態(tài)特性滿足期望的指標要求，達到理想的控制目標。

圖6 X，Y，Z 方向跟蹤仿真圖（縱軸單位：m，橫軸單位：s）

文中在PID參數(shù)整定時參照了ZN臨界比度法[16]，即在閉環(huán)的情況下，將控制器的積分和微分作用先去掉，僅留下比例作用，然后在系統(tǒng)中加入一個擾動，反復調(diào)試得出臨界增益Ku和臨界振蕩周期Tu，最后利用Ku和Tu通過經(jīng)驗公式得到PID參數(shù)整定值。最終位置PID參數(shù)：kpx=kpy=0.8，kdx=kdy=1.3，kix=10-4，kiy=3×10-4，高度PID參數(shù)：kpz=1.2，kdz=2，kiz=10-6。進行仿真實驗得到如圖5所示的位置輸出與高度輸出曲線圖。

圖6給出了系統(tǒng)仿真模型對設定點（3，4，5）（單位：m）的跟蹤性能。通過不停的反饋位置偏差調(diào)整無人機的姿態(tài)，最終無人機穩(wěn)定在（3，4，5）這個點。根據(jù)上述仿真給出的結果圖，可以判斷出這種控制算法是可行的。

3.3 定點實驗

為了測試定點裝置的實際應用效果，通過一定量的實驗測試其性能具體測試內(nèi)容如下：當無人機定點裝置，處于目標點位置飛行較為穩(wěn)定時，通過將遙控器撥到手動通道，人為的將無人機拉到遠離目標點的位置，再將遙控器迅速撥回自動通道，觀察無人機定點裝置的運動，并利用Vicon室內(nèi)目標定位系統(tǒng)記錄下無人機XY通道的運動數(shù)據(jù)，如圖7所示。

通過實驗數(shù)據(jù)可以看到，在開始位置將無人機拉到距離目標為（210，-200）位置處，通過控制律的調(diào)節(jié)，在誤差允許的范圍內(nèi)，可以視為將無人機拉回原點，即目標位置，經(jīng)測試能較好的實現(xiàn)定點功能。

圖7 XY通道跟蹤誤差曲線（縱軸單位：cm，橫軸單位：ms）

4 結束語

本文通過在simulink平臺上搭建四旋翼無人機模型，和實際中搭建無人機硬件平臺和地面站，分別從仿真與實驗的角度證明了基于視覺的無人機在定點飛行時的優(yōu)良性能。

參考文獻：

[1]李雷，陳桂琴，田園.無人機的發(fā)展歷程與展望[J].科技論壇，2011（32）：6.

[2]吳瑤瑤，白云飛，曹科才.撲翼飛行器硬件平臺設計與實現(xiàn)[J].信息化研究，2016，42（3）：38-42.

[3]吳顯亮，石宗英，鐘宜生.無人機視覺導航研究綜述[J].系統(tǒng)仿真學報，2010，22（1）：62-65.

[4]郭佳琪，吳杰，蔣一琦.高壓線巡線飛行器的研究與設計[J].南京工程學院學報：自然科學版，2012，10（1）：39-42.

[5]徐東升.慣性導航中加速度計和陀螺儀性能研究[J].佳木斯大學學報：自然科學版，2012，30（3）：334-337.

[6]劉暢，楊良軍，劉峰.一種基于慣性/視覺信息融合的無人機自主著陸導航算法[J].導航定位與授時，2016，3（6）：7-11.

[7]盛哲.基于視覺導航的無人機位姿控制與自動返航技術[D].上海：上海交通大學，2015.

[8]Zhang X，Xian B.Autonomous flight control of a nano quadrotor helicopter in a GPS-Denied envi?ronment using on-Board vision[J].IEEE Transac?tions on Industrial Electronics，2015：1-11.

[9]王翔鵬.GPS失效情況下農(nóng)用無人機視覺導航關鍵算法研究[D].青島：青島科技大學，2014.

[10]劉國宏，郭文明.改進的中值濾波去噪算法應用分析[J].計算機工程與應用，2010，46（10）：187-189.

[11]趙高長，張磊，武風波.改進的中值濾波算法在圖像去噪中的應用[J].應用光學，2011，32（4）：678-682.

[12]黃忠主.面向監(jiān)視視頻的運動軌跡提取方法研究[D].長沙：國防科技大，2011.

[13]閆鈞華，陳少華，艾淑芳.基于Kalman預測器的改進的CAMShift目標跟蹤[J].中國慣性技術學報，2014，22（4）：536-542.

[14]韓崇昭，朱洪艷，段戰(zhàn)勝.多源信息融合[M].北京：清華大學出版社，2006.

[15]李俊，李運堂.四旋翼飛行器的動力學建模及PID控制[J].遼寧工程技術大學學報：自然科學版，2012，31（1）：114-117.

[16]付瑞玲，樂麗琴.基于MATLAB/Simulink的PID參數(shù)整定[J].工業(yè)控制計算機，2013，26（8）：75-76.