一種基于視覺慣性里程計的微型無人機控制系統設計

孫藝東，戴天虹，羅飛揚

（1.東北林業大學 機電工程學院，黑龍江哈爾濱，150000；2.中國民航大學 航空工程學院，天津，300000）

0 引言

近年來，微型無人機系統的應用范圍越來越廣，適用途徑也從傳統的航拍，巡邏等應用，逐漸向室內外建圖、農業播撒、特種偵察等專業的行業應用轉變[1]。針對無人機本身設計難度適中及愈發豐富的行業應用背景，越來越多的高校將無人機控制系統設計納入大學本科和研究生階段重要的創新環節。各類關于無人機的學科競賽也層出不窮，其中全國大學生電子設計競賽是教育部和工業和信息化部共同發起的大學生學科競賽之一，組委會于2021年比賽中，提出了面向行業應用的“植保無人機”賽題。

賽題要求設計一基于四旋翼飛行器的模擬植保飛行器，完成自主飛行、循跡和控制激光筆的閃爍功能，以此來模擬對指定田塊完成“撒藥”作業。這類賽題要求無人機能夠在室內無GPS場景下進行位置和姿態控制[2]，并完成指定的路徑的規劃和目標搜索和識別功能。對無人機姿態控制系統、機載傳感器和機載圖像識別模塊都有了更復雜的需求。本文針對上述賽題需求，提出了一種基于視覺慣性里程計的微型無人機控制系統設計方案，解決了無人機在室內無GPS有效定位條件下的無人機位姿控制和路徑規劃問題，結合對各類視覺識別傳感器的分析，最終提出了一套針對“植保無人機”賽題的解決方案。

目前針對無人機等微小型機器人，常用的室內定位方法包括，光流法、短距基站定位法、并發建圖與定位法和視覺慣性里程計法[3]。其中，視覺慣性里程計法因其魯棒性高、響應靈敏、有高度集成的商業解決方案等優點，在無人機室內應用的研發中被廣泛使用。

1 視覺慣性里程計原理

視覺慣性里程計(VIO,Visual-Inertial Odometry)也稱視覺慣性系統(VINS，Visual-Inertial System)，是融合相機和IMU數據實現定位的方法。該算法由計算機視覺SLAM體系中的視覺里程計算法(VO，Visual Odometry)演化產生[4]。

在傳統的定位方法中，利用視覺元件和慣性元件可以分別對目標進行定位。利用雙目相機，于像素平面進行目標特征點識別和匹配、確定相差從而計算出目標物體的三維坐標，該方法對于低速運動的特征點豐富的目標能夠進行較為準確的定位。然而對于高速或運動狀態劇烈改變的物體，由于其相機幀率和快門時間等內參因素，這種定位方式難以運行。利用搭載在目標物體上的捷聯慣性部件，如加速度計和陀螺儀，能夠高速、實時地測量物體的姿態和加速度。相對于視覺方法，慣性部件可以估計快速運動狀態或運動狀態劇烈變化等情況，具有良好的普適性。數據和慣性數據融合有很好的互補性。利用這種互補性，將視覺數據和慣性部件數據進行融合，將能夠大幅度提升定位的準確性和穩定性。

表1 相機和IMU的缺點及互補性

視覺慣性里程計算法將視覺數據和慣性數據分別進行預處理：將慣性數據進行積分得出慣性估計的位姿序列，對圖像特征點進行追蹤得出圖像估計的位姿序列。將上述兩種位置估計數據對齊并進行融合，通過計算并最大化物體位置和姿態坐標在視覺、慣性測量數據下的后驗概率給出位姿坐標的最優解[5]，從而實時跟蹤目標位置，可以估計出相機軌跡的真實尺度。

隨著MEMS器件的快速發展，利用智能手機、深度傳感器等設備可以便捷地獲取IMU數據和攝像頭拍攝數據，融合IMU 和視覺信息的VIO 算法可以很大程度地提高視覺SLAM 算法性能[6]，是一種低成本高性能的導航方案。

2 系統總體設計

2.1 系統設計要求

設計一種基于四旋翼飛行器的模擬植保飛行器，能夠對指定田塊完成“撒藥” 作業。如圖1所示作業區中，灰色部分是非播撒區域，綠色部分是待“播撒農藥” 的區域，分成多個 50cm× 50cm 虛線格區塊，用 1~28 數字標識，以全覆蓋飛行方式完成播撒作業。

圖1 作業區示意圖

圖1中，黑底白字的“十”字是飛行器起降點標識，“A”標識是播撒作業起點區塊，飛行器用啟閉可控的激光筆的閃爍光點表示播撒動作，在作業區中放置一只高度為150cm桿塔，桿塔上套有圓環形條形碼。作業中或返航途中，飛行器識別條形碼，并用 LED 閃爍次數顯示數字。

圖2 系統模塊整體設計圖

2.2 主控系統設計

系統以STM32F405RG作為飛行控制器的主控芯片，飛行控制器集成MPU6050陀螺儀和加速度計傳感器，IST8310磁羅盤傳感器，SPL06氣壓計。同時預留了PMW9301光流傳感器和VL53Lx激光測距傳感器的安裝插座。系統模塊的整體設計如圖3所示。

系統以Raspberry Pi 4B作為飛行計算機，用以處理來自Intel Realsense T265視覺慣性里程計的數據。Raspberry Pi 4B采用博通BCM2711型1.5GHz四核架構CPU，并最高配備8GB內存，體積小重量輕，并配備USB3.0接口。

視覺慣性里程計模塊被安裝在機身上層前方無遮擋區域，保證傳感器收集的圖像準確無干擾。飛行計算機安裝在機身上層后方區域，在機身縱向方向與前方傳感器進行配平，保證無人機的重心不偏移。飛行控制器被安裝在機身下層，使用斜拉橡膠球減震板與機身進行軟連接，使集成在飛行控制器上的光流模塊和激光測距模塊能無遮擋的對地照射，同時保證無人機飛行時產生的高頻震動不易傳至下方飛行控制器。這種下置式飛行控制器有區別于傳統的飛控安裝方式，可以極大的減小重量及系統布線壓力。無人機實物圖如圖3所示，飛行控制器硬件電路實物圖如圖4所示。

圖3 無人機實物圖

圖4 飛行控制器硬件電路實物圖

3 系統程序設計

3.1 視覺慣性里程計程序設計

本系統使用Intel RealSenseT265作為視覺慣性里程計傳感器，該傳感器通過USB3.0協議與飛行計算機，即樹莓派連接。利用傳感器供應商提供的SDK程序。我們可以便捷的獲取到T265所輸出的三軸位置信息、速度信息和四元數。

其中，位置信息及速度信息均為地面坐標系下的三維向量(Xv,Yv,Zv)及(Vvx,Vvy,Vzv)。而在無人機位置控制系統中，我們需要使用到地面坐標系下的三維位置坐標(Xv,Yv,Zv)作為位置外環輸入，使用機體坐標系下的三軸速度(Vbx,Vby,Vbv)作為速度內環輸入。因此需使用坐標變換方法，將地面坐標系下(Vvx,Vvy,Vzv)轉換為機體坐標系下(Vbx,Vby,Vbv)，即求飛行速度矢量在集體軸系各軸投影。其公式如下：

其中(φ,θ,)ψ為機體三軸歐拉角，即俯仰，橫滾及偏航。對應的旋轉公式如下：

其中歐拉角由四元數進行求取，公式如下：

經過上述旋轉過程后，將飛行控制器所需數據通過串口以數據包的形式進行發送。飛行控制器接收數據并執行位置控制邏輯，如圖5所示。

圖5 視覺慣性里程計的位置控制

3.2 任務控制邏輯

無人機在執行前后兩次任務的過程中，根據視覺慣性里程計提供的當前坐標進行航線飛行，在航線飛行過程中通過K210圖像傳感器反饋的下方顏色信息，控制激光筆進行打點。通過前方的Openmv圖像傳感器進行，桿體和條形碼的識別，從而判斷桿體是否在飛行路徑上，防止任務中撞桿。任務控制邏輯如圖6所示。

圖6 任務控制邏輯圖

3.3 視覺慣性里程計的校準與在線矯正

視覺慣性里程計在初始化過程中，使用圖像和加速度信息進行內參估計。這種估計依賴在初始化過程中圖像和慣性數據的質量，因此視覺慣性里程計在每次使用時，需提供一個較好的外部環境以幫助里程計更好的初始化，從而獲得更好的重復精度。同時，在里程計工作過程中，將通過詞袋法進行閉環修正。修正過程將產生速度及位置數據的跳變，對無人機飛行產生影響。因此在初始化過程中，通過攜帶飛機繞場平穩運動，可以有效減少里程計因閉環修正產生的跳變。繞場運動示意圖如圖7所示。

圖7 繞場運動示意圖

視覺慣性模塊因其制造工藝、出場前內參校準、工作前校準等不確定因素，很難保證其尺度的一致性。經如圖8所示的方法進行多次測量，確定這種由尺度不確定性產生的誤差滿足一定規律。如圖8所示，多次使無人機按折線飛行產生的尺度誤差成比例規律分布。

圖8 誤差測試過程示意圖

針對上述視覺慣性模塊存在的普遍尺度誤差，提出一種利用飛控機載激光測距模塊進行的在線校正算法，進一步提高位姿控制精度。即在無人機起飛過程中，使無人機在里程計數據1M處懸停，多次使用激光測距對地進行測距，測定的平均實際距離X2即為實際高度（米），即可求得比例系數K=X2/100。將比例系數K乘入里程計數據，即可得校正后的數據。經驗證，這種方式可以有效的補償T265的定位誤差。

4 測試方案與測試結果

4.1 測試方案

根據設計需求進行分步測試，測試內容包括：撒播點數測試，條形碼識別測試和降落精度測試。測試在自主搭建安全網空間內進行，使用的機型為帶防撞圈的250mm軸距無人機。

4.2 測試結果及分析

表2 基礎部分成功撒播點數記錄表（部分）

表3 基礎部分機降落位置距“十”字中心點距離（部分）

表4 發揮部分成功撒播點數記錄表（部分）

表5 openMV識別成功次數記錄表（部分）

表6 發揮部分機降落位置距“十”字中心點距離（部分）

模擬撒播過程中，漏灑點數，降落位置以及openMV識別條形碼的穩定性與飛行器的穩定性密切相關，飛行器越穩定，漏灑點數越少，降落位置越符合規范，識別成功率也更大。實驗數據表明，隨著對飛行器參數的調整，飛行器逐漸趨于穩定狀態，基本達到預期效果。

5 總結

本文以基于視覺慣性里程計的無人機控制系統為設計對象，以2021年全國大學生電子設計大賽G題為設計目標，從系統設計和軟件控制邏輯層面搭建出一個較為完整的運行體系。并以視覺慣性里程計為研究對象，通過分析與測試，歸納闡明了里程計誤差產生的原因及誤差對系統控制的影響。通過改良系統初始化流程，并提出一種結合激光測距進行在線校正的算法邏輯，使里程計數據更加穩定魯棒。并通過實驗獲得的實際數據，對無人機進行微調，最終使其實現穩定懸停，路徑規劃及避障功能，達到了設計目標。