基于視覺-慣性的四旋翼無人機自主導航系統PID控制的研究

何 玲

（蘇州工業園區職業技術學院，江蘇 蘇州 215000）

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

1 研究內容

1）利用一臺筆記本運行控制算法，通過無線局域網實現對Parrot公司的AR.Drone 2.0無人機的自主導航。控制算法主要包含三個部分：同步定位與地圖構建（Simultaneous Localization And Mapping,SLAM）、擴展卡爾曼濾波（Extended Kalman Filter,EKF）和PID控制。三者彼此之間的關系如圖1所示。

圖1 系統示意圖

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

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

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

2 PID控制

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

圖2 PID控制過程的示意圖

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

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

3 AR.Drone 2.0無人機軟件結構的建立

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

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

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

三個進程通過消息管道、共享內存區、互斥和事件來實現視頻、導航數據及控制數據的交互。整個軟件結構如圖3所示。

圖3 軟件框圖

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

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

5 PID控制在視覺追蹤恢復中的應用

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

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

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

6 結論

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