一種基于視覺慣性里程計(jì)的微型無(wú)人機(jī)控制系統(tǒng)設(shè)計(jì)

孫藝東，戴天虹，羅飛揚(yáng)

（1.東北林業(yè)大學(xué) 機(jī)電工程學(xué)院，黑龍江哈爾濱，150000；2.中國(guó)民航大學(xué) 航空工程學(xué)院，天津，300000）

0 引言

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

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

目前針對(duì)無(wú)人機(jī)等微小型機(jī)器人，常用的室內(nèi)定位方法包括，光流法、短距基站定位法、并發(fā)建圖與定位法和視覺慣性里程計(jì)法[3]。其中，視覺慣性里程計(jì)法因其魯棒性高、響應(yīng)靈敏、有高度集成的商業(yè)解決方案等優(yōu)點(diǎn)，在無(wú)人機(jī)室內(nèi)應(yīng)用的研發(fā)中被廣泛使用。

1 視覺慣性里程計(jì)原理

視覺慣性里程計(jì)(VIO,Visual-Inertial Odometry)也稱視覺慣性系統(tǒng)(VINS，Visual-Inertial System)，是融合相機(jī)和IMU數(shù)據(jù)實(shí)現(xiàn)定位的方法。該算法由計(jì)算機(jī)視覺SLAM體系中的視覺里程計(jì)算法(VO，Visual Odometry)演化產(chǎn)生[4]。

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

表1 相機(jī)和IMU的缺點(diǎn)及互補(bǔ)性

視覺慣性里程計(jì)算法將視覺數(shù)據(jù)和慣性數(shù)據(jù)分別進(jìn)行預(yù)處理：將慣性數(shù)據(jù)進(jìn)行積分得出慣性估計(jì)的位姿序列，對(duì)圖像特征點(diǎn)進(jìn)行追蹤得出圖像估計(jì)的位姿序列。將上述兩種位置估計(jì)數(shù)據(jù)對(duì)齊并進(jìn)行融合，通過計(jì)算并最大化物體位置和姿態(tài)坐標(biāo)在視覺、慣性測(cè)量數(shù)據(jù)下的后驗(yàn)概率給出位姿坐標(biāo)的最優(yōu)解[5]，從而實(shí)時(shí)跟蹤目標(biāo)位置，可以估計(jì)出相機(jī)軌跡的真實(shí)尺度。

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

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

2.1 系統(tǒng)設(shè)計(jì)要求

設(shè)計(jì)一種基于四旋翼飛行器的模擬植保飛行器，能夠?qū)χ付ㄌ飰K完成“撒藥” 作業(yè)。如圖1所示作業(yè)區(qū)中，灰色部分是非播撒區(qū)域，綠色部分是待“播撒農(nóng)藥” 的區(qū)域，分成多個(gè) 50cm× 50cm 虛線格區(qū)塊，用 1~28 數(shù)字標(biāo)識(shí)，以全覆蓋飛行方式完成播撒作業(yè)。

圖1 作業(yè)區(qū)示意圖

圖1中，黑底白字的“十”字是飛行器起降點(diǎn)標(biāo)識(shí)，“A”標(biāo)識(shí)是播撒作業(yè)起點(diǎn)區(qū)塊，飛行器用啟閉可控的激光筆的閃爍光點(diǎn)表示播撒動(dòng)作，在作業(yè)區(qū)中放置一只高度為150cm桿塔，桿塔上套有圓環(huán)形條形碼。作業(yè)中或返航途中，飛行器識(shí)別條形碼，并用 LED 閃爍次數(shù)顯示數(shù)字。

圖2 系統(tǒng)模塊整體設(shè)計(jì)圖

2.2 主控系統(tǒng)設(shè)計(jì)

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

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

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

圖3 無(wú)人機(jī)實(shí)物圖

圖4 飛行控制器硬件電路實(shí)物圖

3 系統(tǒng)程序設(shè)計(jì)

3.1 視覺慣性里程計(jì)程序設(shè)計(jì)

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

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

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

其中歐拉角由四元數(shù)進(jìn)行求取，公式如下：

經(jīng)過上述旋轉(zhuǎn)過程后，將飛行控制器所需數(shù)據(jù)通過串口以數(shù)據(jù)包的形式進(jìn)行發(fā)送。飛行控制器接收數(shù)據(jù)并執(zhí)行位置控制邏輯，如圖5所示。

圖5 視覺慣性里程計(jì)的位置控制

3.2 任務(wù)控制邏輯

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

圖6 任務(wù)控制邏輯圖

3.3 視覺慣性里程計(jì)的校準(zhǔn)與在線矯正

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

圖7 繞場(chǎng)運(yùn)動(dòng)示意圖

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

圖8 誤差測(cè)試過程示意圖

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

4 測(cè)試方案與測(cè)試結(jié)果

4.1 測(cè)試方案

根據(jù)設(shè)計(jì)需求進(jìn)行分步測(cè)試，測(cè)試內(nèi)容包括：撒播點(diǎn)數(shù)測(cè)試，條形碼識(shí)別測(cè)試和降落精度測(cè)試。測(cè)試在自主搭建安全網(wǎng)空間內(nèi)進(jìn)行，使用的機(jī)型為帶防撞圈的250mm軸距無(wú)人機(jī)。

4.2 測(cè)試結(jié)果及分析

表2 基礎(chǔ)部分成功撒播點(diǎn)數(shù)記錄表（部分）

表3 基礎(chǔ)部分機(jī)降落位置距“十”字中心點(diǎn)距離（部分）

表4 發(fā)揮部分成功撒播點(diǎn)數(shù)記錄表（部分）

表5 openMV識(shí)別成功次數(shù)記錄表（部分）

表6 發(fā)揮部分機(jī)降落位置距“十”字中心點(diǎn)距離（部分）

模擬撒播過程中，漏灑點(diǎn)數(shù)，降落位置以及openMV識(shí)別條形碼的穩(wěn)定性與飛行器的穩(wěn)定性密切相關(guān)，飛行器越穩(wěn)定，漏灑點(diǎn)數(shù)越少，降落位置越符合規(guī)范，識(shí)別成功率也更大。實(shí)驗(yàn)數(shù)據(jù)表明，隨著對(duì)飛行器參數(shù)的調(diào)整，飛行器逐漸趨于穩(wěn)定狀態(tài)，基本達(dá)到預(yù)期效果。

5 總結(jié)

本文以基于視覺慣性里程計(jì)的無(wú)人機(jī)控制系統(tǒng)為設(shè)計(jì)對(duì)象，以2021年全國(guó)大學(xué)生電子設(shè)計(jì)大賽G題為設(shè)計(jì)目標(biāo)，從系統(tǒng)設(shè)計(jì)和軟件控制邏輯層面搭建出一個(gè)較為完整的運(yùn)行體系。并以視覺慣性里程計(jì)為研究對(duì)象，通過分析與測(cè)試，歸納闡明了里程計(jì)誤差產(chǎn)生的原因及誤差對(duì)系統(tǒng)控制的影響。通過改良系統(tǒng)初始化流程，并提出一種結(jié)合激光測(cè)距進(jìn)行在線校正的算法邏輯，使里程計(jì)數(shù)據(jù)更加穩(wěn)定魯棒。并通過實(shí)驗(yàn)獲得的實(shí)際數(shù)據(jù)，對(duì)無(wú)人機(jī)進(jìn)行微調(diào)，最終使其實(shí)現(xiàn)穩(wěn)定懸停，路徑規(guī)劃及避障功能，達(dá)到了設(shè)計(jì)目標(biāo)。