何 玲

（蘇州工業(yè)園區(qū)職業(yè)技術(shù)學院，江蘇 蘇州 215000）

多旋翼無人機自主導(dǎo)航系統(tǒng)主要包含三個部分：1）基于關(guān)鍵幀的單目SLAM，用于無人機的姿態(tài)估計；2）擴展卡爾曼濾波（EKF），利用EKF融合視覺姿態(tài)以及其他傳感器的測量姿態(tài)，實現(xiàn)不同數(shù)據(jù)源的同步，并補償因無線局域網(wǎng)傳輸及其他大量計算等原因造成的時延；3）PID控制器，用于控制無人機的姿態(tài)，實現(xiàn)無人機飛向或保持在目標位置。本研究基于視覺-慣性，以Parrot公司的AR.Drone 2.0無人機為載體，對四旋翼無人機自主導(dǎo)航系統(tǒng)的PID控制進行了研究。

1 研究內(nèi)容

1）利用一臺筆記本運行控制算法，通過無線局域網(wǎng)實現(xiàn)對Parrot公司的AR.Drone 2.0無人機的自主導(dǎo)航?？刂扑惴ㄖ饕齻€部分：同步定位與地圖構(gòu)建（Simultaneous Localization And Mapping,SLAM）、擴展卡爾曼濾波（Extended Kalman Filter,EKF）和PID控制。三者彼此之間的關(guān)系如圖1所示。

圖1 系統(tǒng)示意圖

2）SLAM通過相機獲取的視頻流計算出無人機的姿態(tài)估計值。通過底部光流傳感器及超聲波傳感器得到的信息與該估計值比較，計算出地圖的尺度。

3）EKF融合SLAM估計的姿態(tài)、光流傳感器及超聲波傳感器觀測數(shù)據(jù)以及上次發(fā)送的控制命令的可預(yù)測效果3種數(shù)據(jù)，計算出更為精準的無人機姿態(tài)及速度的估計值，并預(yù)測下個命令對將來狀態(tài)的預(yù)期影響，從而補償了因通信造成的時延。

4）基于EKF提供的無人機位置和速度的估計值，PID控制用于計算適宜的控制命令，使無人機飛行到或保持在指定的目標位置[1]。

2 PID控制

控制理論目標是控制動態(tài)系統(tǒng)的行為。動態(tài)系統(tǒng)是指一個狀態(tài)隨時間變化的系統(tǒng)，其中狀態(tài)可以被一個或多個輸入值控制。通常目標是計算系統(tǒng)輸入值u(t)使系統(tǒng)達到并維持在期望的狀態(tài)。換言之，給出的設(shè)置點w(t)和系統(tǒng)的測量輸出y(t)間的測量誤差被最小化。特別地，要求系統(tǒng)能快速到達并維持在設(shè)置點，穩(wěn)定無震蕩，能抵消環(huán)境引入的各種干擾[2]。整個PID控制過程的示意圖如圖2所示。

圖2 PID控制過程的示意圖

PID控制，即比例—積分—微分控制一種通用的閉環(huán)反饋控制機制?？刂菩盘栍?個獨立控制項加權(quán)和決定：比例項取決于當前的誤差e(t)；積分項取決于累積誤差微分項取決于預(yù)測將來的誤差項，基于誤差導(dǎo)數(shù)如果積分和微分項無法通過測量得出，可近似為PID控制器通過公式（1）計算系統(tǒng)輸入值：

其中：Kp、Ki、Kd是可調(diào)參數(shù)，通常通過試錯實驗或經(jīng)驗獲得，但也有一些啟發(fā)式方法和準則來幫助這個過程。控制系統(tǒng)的質(zhì)量由收斂時間tconv來衡量，即e(t)收斂到接近0的指定的閾值內(nèi)經(jīng)過的時間。

3 AR.Drone 2.0無人機軟件結(jié)構(gòu)的建立

本設(shè)計的主要目標是建立一個可靠的、可容錯的軟件系統(tǒng)來控制AR.Drone 2.0?？扇蒎e性是一個巨大的挑戰(zhàn)，比如：要求系統(tǒng)能快速偵測到連接的丟失并迅速恢復(fù)，以防內(nèi)部控制狀態(tài)的丟失。為了解決這個問題，需將AR.Drone 2.0 SDK代碼封裝在獨立的線程里，稱之為無人機代理進程，即第一個進程。該進程提供一套清晰的本地接口實現(xiàn)對視頻和導(dǎo)航數(shù)據(jù)的訪問以及無人機控制命令的發(fā)送。除此之外，它還提供對無人機發(fā)出的所有數(shù)據(jù)錄制和重放的功能，從而模擬真實飛行來實現(xiàn)問題的快速定位和解決[3]。

第二個進程，負責檢測無人機代理進程的狀態(tài)，當連接斷開或其他錯誤發(fā)生時，自動重啟該進程。該進程為用戶界面進程。它也提供通過鍵盤手動控制無人機、管理無人機連接、飛行的錄制與回放等功能。

第三個進程，運行主要算法，包括SLAM、EKF和PID控制器以及視頻和地圖的可視化。該進程為控制進程。

三個進程通過消息管道、共享內(nèi)存區(qū)、互斥和事件來實現(xiàn)視頻、導(dǎo)航數(shù)據(jù)及控制數(shù)據(jù)的交互。整個軟件結(jié)構(gòu)如圖3所示。

圖3 軟件框圖

4 AR.Drone 2.0無人機的PID控制

上式中，cΦp表示PID系數(shù)，其中下標Φp表示橫滾角Φ的比例項系數(shù)，其他以此類推。

5 PID控制在視覺追蹤恢復(fù)中的應(yīng)用

視覺追蹤丟失在很多條件下會發(fā)生，比如轉(zhuǎn)動太快、飛行太快等等都會導(dǎo)致攝像模糊，致使追蹤丟失。本研究的測試方法是先讓無人機懸停在目標位置，然后快速推動無人機，讓其迅速離開目標位置并伴有旋轉(zhuǎn)動作，使得視覺追蹤丟失，再觀測其是否能回退到原始目標位置。理論上，當視覺追蹤丟失后，無人機仍可用IMU等信息回退到接近目標點的位置。因為IMU有偏移，所以無法回到原始姿態(tài)，但因接近原始姿態(tài)，視覺會追蹤到原始場景，從而實現(xiàn)追蹤恢復(fù)，并對IMU的偏移做出補償，進一步回到精確的原始姿態(tài)。該過程中4個控制信號波形記錄如圖4所示。

圖4 視覺追蹤恢復(fù)過程中的控制曲線

從圖中可以看出，在0.5 s時，無人機被人為推移，并發(fā)生轉(zhuǎn)動，導(dǎo)致視覺追蹤丟失，如圖豎線一處。隨后無人機通過IMU嘗試回到最初的位置。在3.2 s時視覺追蹤恢復(fù)，如圖豎線二處，無人機進一步調(diào)整并回到目標位置[4-5]。

6 結(jié)論

從測試結(jié)果看，該系統(tǒng)具有一定的魯棒性，可以在視覺追蹤丟失的情況下，借助IMU等信息實現(xiàn)追蹤恢復(fù)。同時，視覺姿態(tài)估計又可以反過來補償IMU的時間累積誤差，限制飛行軌跡出現(xiàn)較大的偏移。