視覺與慣性融合的多旋翼飛行機(jī)器人室內(nèi)定位技術(shù)

張懷捷，馬靜雅，劉浩源，郭品，鄧慧超，*，徐坤，丁希侖

1．北京航空航天大學(xué) 機(jī)器人研究所，北京 100191

2．北京空間飛行器總體設(shè)計(jì)部，北京 100094

多旋翼飛行機(jī)器人具有結(jié)構(gòu)簡單、功能性強(qiáng)、動(dòng)態(tài)特性好的特點(diǎn)，被廣泛應(yīng)用于環(huán)境監(jiān)測、災(zāi)難救援、邊境巡檢等公共安全領(lǐng)域［1-2］。在面向城市或者森林的搜索與救援任務(wù)時(shí)，多旋翼飛行機(jī)器人機(jī)動(dòng)性高，可以觀測到大范圍的視野，但由于森林和建筑物的遮擋，依賴于全球定位系統(tǒng)（Global Position System， GPS）的定位方法失效，對目標(biāo)的搜索與救援任務(wù)也變得困難。因此，實(shí)現(xiàn)多旋翼飛行機(jī)器人無GPS 環(huán)境下的自主跟蹤及降落任務(wù)，具有較強(qiáng)的應(yīng)用價(jià)值和現(xiàn)實(shí)意義。

對于自主跟蹤及降落問題，傳統(tǒng)導(dǎo)航系統(tǒng)采用慣性導(dǎo)航系統(tǒng)（Inertial Navigation System， INS）與GPS 來實(shí)現(xiàn)［3］。但是，INS 會(huì)因?yàn)榉e分導(dǎo)致累積誤差，GPS 也會(huì)由于衛(wèi)星信號被遮擋而失效，因此，基于視覺定位［4-6］的方式來實(shí)現(xiàn)自主跟蹤及降落可以讓飛行機(jī)器人適應(yīng)大多數(shù)復(fù)雜環(huán)境下的情況。

而對于自主定位問題，室外開闊環(huán)境下，主要通過GPS 全球定位系統(tǒng)來進(jìn)行定位，在森林、室內(nèi)等環(huán)境下，衛(wèi)星信號被遮擋會(huì)導(dǎo)致機(jī)器人定位失效，從而影響飛行機(jī)器人自主功能的實(shí)現(xiàn)。目前，國內(nèi)外提出了一些室內(nèi)定位的方法［7-8］，例如基于無線保真（Wireless Fidelity， WiFi）、射頻識(shí) 別 技 術(shù)（Radio Frequency Identification， RFID）［9］、超 寬 帶 技 術(shù)（Ultra Wide Band， UWB）、紅外等，但這些方法需要依賴于外部設(shè)備使得飛行機(jī)器人的應(yīng)用場景受限，不具備在未知環(huán)境下完全自主的功能。因此，采用同步定位與建圖（Simultaneous Localization and Mapping， SLAM）技術(shù)擁有更大的優(yōu)勢。

視覺SLAM 中的視覺前端也叫做視覺里程計(jì)［10］，主要包括特征點(diǎn)提取和匹配，數(shù)據(jù)關(guān)聯(lián)，位姿估計(jì)幾部分。按其數(shù)據(jù)關(guān)聯(lián)方式不同，通常將其分為特征點(diǎn)法和直接法。特征點(diǎn)法是在關(guān)鍵點(diǎn)提取的基礎(chǔ)上，通過描述子匹配建立數(shù)據(jù)關(guān)聯(lián)［11-12］；直接法是在關(guān)鍵點(diǎn)提取的基礎(chǔ)上，基于灰度不變假設(shè)的光流跟蹤實(shí)現(xiàn)數(shù)據(jù)關(guān)聯(lián)［13-14］。此外，有學(xué)者將卷積神經(jīng)網(wǎng)絡(luò)的深度估計(jì)用于直接法中，與單目SLAM 中的深度估計(jì)融合，為位姿估計(jì)提供絕對的尺度信息［15］。

由于飛行機(jī)器人運(yùn)動(dòng)變化較快，僅依靠視覺定位方式會(huì)使得定位精度低及穩(wěn)定性差。目前，研究人員普遍采用多傳感器融合定位的方式來實(shí)現(xiàn)位姿估計(jì)以減小飛行機(jī)器人運(yùn)動(dòng)過程中的定 位 偏 差，例 如MSCKF［16］、VINS-Mono［17］、ORB-SLAM3［18］等。

為實(shí)現(xiàn)室內(nèi)場景下的自主跟蹤及降落任務(wù)，設(shè)計(jì)了基于視覺定位的地面平臺(tái)自主跟蹤及降落系統(tǒng)，在Gazebo 中仿真驗(yàn)證其有效性。面向室內(nèi) 定 位 的 場 景 需 求，借 鑒 了VINS-Mono［17］中 的融合方法，針對室內(nèi)場景圖像對比度較低的問題，在視覺前端加入圖像增強(qiáng)算法處理從而減少光流跟蹤中的誤匹配點(diǎn)數(shù)以提高定位精度。為解決飛行機(jī)器人單目視覺慣性定位中的漂移問題，本文提出了一種基于圖像信息的特征點(diǎn)提取和圖像幀發(fā)布策略，以提高飛行機(jī)器人的定位精度及穩(wěn)定性。將改進(jìn)后的視覺前端數(shù)據(jù)與慣性數(shù)據(jù)融合，在EuRoC 數(shù)據(jù)集中對比評估算法結(jié)果，設(shè)計(jì)飛行機(jī)器人平臺(tái)，完成了真實(shí)場景下的室內(nèi)定位實(shí)驗(yàn)，基于運(yùn)動(dòng)捕捉系統(tǒng)獲取的定位真值評估定位結(jié)果，完成了室內(nèi)自主跟蹤及降落實(shí)驗(yàn)，驗(yàn)證了系統(tǒng)有效性。

1 系統(tǒng)框架

設(shè)計(jì)的多旋翼飛行機(jī)器人平臺(tái)系統(tǒng)總體框架如圖1 所示，分為4 個(gè)子模塊：飛控模塊、室內(nèi)定位模塊、自主導(dǎo)航模塊及地面站模塊。飛控模塊搭載氣壓計(jì)和慣性測量單元（Inertial Measurement Unit， IMU）姿態(tài)傳感器，運(yùn)行實(shí)時(shí)性較高的姿態(tài)解算和位置控制，實(shí)現(xiàn)穩(wěn)定飛行；定位模塊包括機(jī)載處理器、相機(jī)、IMU，獲取圖像信息和姿態(tài)數(shù)據(jù)，在機(jī)載處理器中運(yùn)行定位算法，為飛行機(jī)器人提供實(shí)時(shí)的位姿估計(jì)結(jié)果；自主導(dǎo)航模塊包括機(jī)載處理器和俯視相機(jī)兩部分，運(yùn)行自主跟蹤及降落算法，為飛控提供自主導(dǎo)航指令；地面站實(shí)時(shí)監(jiān)控系統(tǒng)運(yùn)行結(jié)果。

圖1 系統(tǒng)總體框架Fig.1 System overall framework

定位系統(tǒng)框架如圖2 所示。為實(shí)現(xiàn)室內(nèi)場景下飛行機(jī)器人定位飛行任務(wù)，在前端視覺里程計(jì)中加入圖像增強(qiáng)處理以提高圖像灰度對比。為解決后端視覺慣性融合優(yōu)化中的漂移問題，提出了一種基于圖像信息的特征點(diǎn)提取和圖像幀發(fā)布策略以提高定位精度及穩(wěn)定性。之后，基于Perspective-n-Point（PnP）實(shí)現(xiàn)了連續(xù)幀幀間位姿估計(jì)。最后，對IMU 數(shù)據(jù)進(jìn)行預(yù)積分，將改進(jìn)后的視覺前端數(shù)據(jù)與慣性數(shù)據(jù)初始化對齊后，利用非線性優(yōu)化方法求解飛行機(jī)器人位姿。

圖2 視覺慣性融合定位系統(tǒng)框架Fig.2 Visual-inertial fusion positioning system framework

針對飛行機(jī)器人自主跟蹤及降落任務(wù)中任務(wù)場景復(fù)雜、手動(dòng)降落不穩(wěn)定及現(xiàn)有GPS 定位精度不高的情況，設(shè)計(jì)基于視覺定位的自主跟蹤及降落系統(tǒng)，總體流程如圖3 所示：用戶通過人機(jī)交互模塊（上位機(jī)或遙控器）輸入指令；飛行機(jī)器人自主起飛，搜索地面目標(biāo)；通過懸停校準(zhǔn)、水平接近、自主降落三種狀態(tài)機(jī)的切換自主執(zhí)行跟蹤及降落任務(wù)。

圖3 自主跟蹤及降落流程圖Fig.3 Flowchart of autonomous tracking and landing

2 基于直接法的單目視覺里程計(jì)

2.1 圖像增強(qiáng)

由于應(yīng)用場景為室內(nèi)場景，對于相機(jī)獲取的圖像信息，需進(jìn)行圖像增強(qiáng)處理以提高灰度對比用于減少光流跟蹤中的誤匹配點(diǎn)。區(qū)別于OpenCV 中的全局直方圖均衡化思想，采用圖像塊分塊插值的思想來實(shí)現(xiàn)。首先，計(jì)算灰度直方圖，對圖像以塊為單位，統(tǒng)計(jì)并存儲(chǔ)每個(gè)塊中不同灰度像素的數(shù)量計(jì)算灰度直方圖；其次，裁剪直方圖，對灰度直方圖縱軸設(shè)置一個(gè)標(biāo)準(zhǔn)閾值，統(tǒng)計(jì)其像素灰度平均值，對超出閾值，超出平均值或者低于平均值的情況分別裁剪，計(jì)算累計(jì)直方圖并對其歸一化；最后，對圖像塊間進(jìn)行分塊插值以減少塊狀效應(yīng)。圖像塊角點(diǎn)像素保持原灰度，角點(diǎn)以外的部分采用雙線性插值方法，通過循環(huán)遍歷圖像中所有像素點(diǎn)計(jì)算其圖像增強(qiáng)處理后的灰度值。如圖4 所示，插值節(jié)點(diǎn)選擇待求像素點(diǎn)相鄰4 個(gè)圖像塊的中心點(diǎn)像素坐標(biāo)Q11(x1，y1)、Q12(x1，y2)、Q21(x2，y1)、Q22(x2，y2)，P點(diǎn)為插值點(diǎn)。

圖4 雙線性插值Fig.4 Bilinear interpolation

雙線性插值的公式為

式中：f(Q)為該點(diǎn)像素灰度值；f(x，y)為經(jīng)雙線性插值處理后待求點(diǎn)的像素灰度值。

2.2 圖像角點(diǎn)提取

面向SLAM 領(lǐng)域的局部特征提取問題，特征點(diǎn)以其計(jì)算量較小，對于場景中光照、遮擋、旋轉(zhuǎn)的魯棒性較高等優(yōu)勢被廣泛應(yīng)用。角點(diǎn)作為特征點(diǎn)中重要的一種，主要應(yīng)用于目標(biāo)跟蹤和視覺定位等場合中。表1綜合比較Shi-Tomasi［19］、Fast［20］、SIFT［21］3種角點(diǎn)，結(jié)合多旋翼飛行機(jī)器人室內(nèi)定位的實(shí)時(shí)性和準(zhǔn)確性要求，決定采用提取速度較快，且同時(shí)具備光照魯棒和旋轉(zhuǎn)不變性的Shi-Tomasi角點(diǎn)，Shi-Tomasi角點(diǎn)提取示意圖如圖5 所示。

表1 角點(diǎn)性能綜合比較分析Table 1 Comprehensive comparative analysis of corner performance

圖5 Shi-Tomasi 角點(diǎn)提取示意圖Fig.5 Schematic diagram of Shi-Tomasi corner extraction

設(shè)當(dāng)前窗口中心像素坐標(biāo)為P(x，y)，其像素灰度值為I(x，y)，對窗口沿X、Y軸正向移動(dòng)微小位移(u，v)，中心點(diǎn)像素灰度值為I(x+u，y+v)，則窗口沿各個(gè)方向移動(dòng)所造成的像素灰度值的變化為

式中：

對當(dāng)前圖像所有窗口設(shè)定一個(gè)標(biāo)準(zhǔn)閾值，以M矩陣中最小的特征值作為當(dāng)前窗口得分，當(dāng)?shù)梅殖^設(shè)定閾值時(shí)，即認(rèn)為當(dāng)前像素點(diǎn)是一個(gè)角點(diǎn)。

2.3 基于圖像信息的特征點(diǎn)提取和圖像幀發(fā)布策略

飛行機(jī)器人在視覺與慣性融合定位過程中常會(huì)出現(xiàn)朝一個(gè)方向持續(xù)漂移的問題。經(jīng)過實(shí)驗(yàn)分析，其主要原因是飛行機(jī)器人的速度變化過快，在不同運(yùn)動(dòng)場景下采用相同的圖像幀發(fā)布頻率和特征點(diǎn)發(fā)布數(shù)量會(huì)導(dǎo)致在機(jī)器人速度較慢、場景變化不大的情況下有相似的圖像幀冗余，而在機(jī)器人速度較快，或機(jī)身抖動(dòng)較為嚴(yán)重的情況下又會(huì)因?yàn)榘l(fā)布的圖像幀不足使得用于優(yōu)化的圖像數(shù)據(jù)不足，導(dǎo)致視覺定位失效。此時(shí)，融合過程更多依賴于IMU 的數(shù)據(jù)，而僅依靠IMU 實(shí)現(xiàn)定位又會(huì)受到IMU 噪聲的影響產(chǎn)生累計(jì)漂移，導(dǎo)致定位失效。因此，本文提出了一種應(yīng)用于飛行機(jī)器人的基于圖像信息的特征點(diǎn)提取和圖像幀發(fā)布策略，以提高飛行機(jī)器人定位性能，如圖6所示。

圖6 基于圖像信息的特征點(diǎn)提取和圖像幀發(fā)布策略Fig.6 Feature point extraction and image frame publishing strategy based on image information

對相鄰兩幀圖像建立圖像金字塔，采用計(jì)時(shí)器獲取相鄰兩幀圖像的時(shí)間間隔，基于金字塔光流跟蹤［22-23］得到對應(yīng)特征點(diǎn)的匹配關(guān)系，做差得到像素速度，對滑動(dòng)窗口內(nèi)所有像素點(diǎn)求和平均，根據(jù)窗口內(nèi)所有像素點(diǎn)的平均速度來控制視覺前端發(fā)布給后端用于優(yōu)化的特征點(diǎn)數(shù)量和圖像幀頻率，考慮到飛行機(jī)器人調(diào)姿和飛行過程中抖動(dòng)的影響，對數(shù)據(jù)做了延時(shí)和移動(dòng)平均處理，通過平滑數(shù)據(jù)來達(dá)到防抖的效果。

之后，基于隨機(jī)抽樣一致算法（Random Sample Consensus， RANSAC）實(shí)現(xiàn)誤匹配點(diǎn)剔除，基于PnP 實(shí)現(xiàn)連續(xù)幀的位姿估計(jì)，將位姿估計(jì)結(jié)果與慣性數(shù)據(jù)融合，優(yōu)化求解飛行機(jī)器人位姿估計(jì)。

3 視覺慣性融合定位

融合思路借鑒了VINS-Mono［17］中的融合思想，將改進(jìn)后前端視覺里程計(jì)中的相機(jī)初始位姿估計(jì)與IMU 預(yù)積分結(jié)果初始化對齊，利用相機(jī)旋轉(zhuǎn)約束標(biāo)定陀螺儀偏置，同時(shí)初始化IMU 速度、重力向量以及相機(jī)位姿估計(jì)時(shí)的尺度因子。

在原有優(yōu)化方程的基礎(chǔ)上，將改進(jìn)后的視覺前端連續(xù)幀估計(jì)結(jié)果與IMU 狀態(tài)數(shù)據(jù)融合加入方程，計(jì)算噪聲項(xiàng)的協(xié)方差矩陣，歸一化兩者量綱，基于滑動(dòng)窗口法優(yōu)化求解飛行機(jī)器人位姿，優(yōu)化方程表示為

式中：rp為先驗(yàn)殘差，通過被邊緣化的某幀對窗口內(nèi)其他幀的約束關(guān)系來構(gòu)建；B、C為IMU 測量值和視覺測量值的集合；rB為IMU 預(yù)積分殘差，通過IMU 預(yù)積分結(jié)果變式求得；rC為視覺重投影殘差；ρ為柯西核函數(shù)；優(yōu)化變量χ為滑動(dòng)窗口中IMU狀態(tài)信息，包含位置、速度、旋轉(zhuǎn)角度、相機(jī)外參、加速度計(jì)和陀螺儀偏置ba、bg及三角化恢復(fù)出的特征點(diǎn)逆深度λ信息；可以表示為

除原有的圖像數(shù)據(jù)外，將改進(jìn)后的視覺前端增加的特征點(diǎn)和圖像幀信息投影至參考幀下，與參考幀中的測量值構(gòu)建重投影殘差。Σp、ΣB、ΣC為3 種誤差項(xiàng)的協(xié)方差矩陣。采用列文伯格-馬夸爾特方法，調(diào)用Ceres 庫完成求解［17］。

4 自主跟蹤及降落策略

自主降落系統(tǒng)包含5 個(gè)坐標(biāo)系，如圖7 所示，分別為世界坐標(biāo)系Ow、機(jī)器人本體坐標(biāo)系Ob、相機(jī)坐標(biāo)系Oc、Marker 坐標(biāo)系Om、相機(jī)像素平面坐標(biāo)系Op。其中，飛行機(jī)器人初始坐標(biāo)系原點(diǎn)與世界坐標(biāo)系Ow原點(diǎn)重合，Xw、Yw、Zw的正方向采用飛行機(jī)器人坐標(biāo)表示的通用方法，分別指向北、東、地心3 個(gè)方向。飛行機(jī)器人本體坐標(biāo)系Ob以其質(zhì)心為原點(diǎn)，Xb、Yb、Zb分別指向機(jī)頭、機(jī)身右側(cè)、垂直機(jī)身向下3 個(gè)方向。相機(jī)坐標(biāo)系Oc以其光心為原點(diǎn)，Xc、Yc、Zc分別指向相機(jī)右側(cè)、下方、垂直相機(jī)平面向外側(cè)3 個(gè)方向。Marker 坐標(biāo)系Om以Aruco Marker 幾何中心為原點(diǎn)，Xm、Ym、Zm分別指向北、東、地心3 個(gè)方向。其中，Ow到Ob的位姿變換關(guān)系Tw b可以由定位算法得到。由于選用了RealSenseD435i 相機(jī)固連在機(jī)身本體上，Ob到Oc的位姿變換關(guān)系Tb c由相機(jī)安裝位置決定。相機(jī)坐標(biāo)系下的目標(biāo)位置Pc由Aruco Marker 的位姿估計(jì)結(jié)果得到。由此，可以通過坐標(biāo)轉(zhuǎn)換關(guān)系不斷更新目標(biāo)Marker 在世界坐標(biāo)系下的位姿信息Pw用于位姿校正：

圖7 坐標(biāo)轉(zhuǎn)換關(guān)系Fig.7 Coordinate transformation relationship

根據(jù)任務(wù)需求，為飛行機(jī)器人自主任務(wù)建立狀態(tài)機(jī)，包含初始化、起飛、目標(biāo)搜索、懸停校準(zhǔn)、水平接近、自主降落共6 個(gè)狀態(tài)機(jī)標(biāo)志位。其中，目標(biāo)識(shí)別部分采用了Aruco Marker 模塊提供地面目標(biāo)相對于機(jī)身本體的三維位置信息。由于俯視相機(jī)固連于機(jī)身本體，通過目標(biāo)搜索、懸停校準(zhǔn)、水平接近3 個(gè)狀態(tài)機(jī)之間的狀態(tài)切換實(shí)現(xiàn)對于地面目標(biāo)的跟蹤，滿足許可水平誤差閾值要求后執(zhí)行自主降落，算法框架如圖8 所示。

圖8 自主跟蹤及降落Fig.8 Autonomous tracking and landing

算法主要需要解決以下4 個(gè)關(guān)鍵點(diǎn)：

1） 俯視相機(jī)固連在飛行機(jī)器人本體上，飛行機(jī)器人運(yùn)動(dòng)或者調(diào)姿過程會(huì)影響標(biāo)志物在視野中的位置，同時(shí)考慮到通信延時(shí)的影響，采用懸停校準(zhǔn)的方式來不斷更新標(biāo)志物在世界坐標(biāo)系下的位置。

2） 飛行機(jī)器人相比于地面移動(dòng)機(jī)器人具有高動(dòng)態(tài)、移動(dòng)范圍大的特性，飛行過程會(huì)影響目標(biāo)檢測的穩(wěn)定性，所以需要設(shè)立目標(biāo)初檢測、穩(wěn)定檢測及檢測丟失幾個(gè)標(biāo)志位來處理檢測丟失的情況。其中，目標(biāo)初檢測以識(shí)別模塊的回調(diào)函數(shù)觸發(fā)作為判斷標(biāo)準(zhǔn)，持續(xù)觸發(fā)回調(diào)函數(shù)一定時(shí)間即視為穩(wěn)定檢測。對于檢測丟失標(biāo)志位，考慮到飛行過程中抖動(dòng)對檢測穩(wěn)定性造成的影響，以距離上次觸發(fā)識(shí)別模塊的回調(diào)函數(shù)間隔超過1 s作為檢測丟失標(biāo)志位的判斷依據(jù)。

3） 飛行機(jī)器人在執(zhí)行自主任務(wù)時(shí)，由機(jī)載處理器執(zhí)行控制指令輸入，需要在外環(huán)開發(fā)遙控失控保護(hù)功能以保證安全性。

4） 飛行機(jī)器人對目標(biāo)的自主降落需要建立在對自身準(zhǔn)確定位的基礎(chǔ)上。但是，無論在室外采用GPS 全球定位系統(tǒng)還是室內(nèi)采用SLAM 算法，定位結(jié)果都會(huì)存在一定程度的漂移。所以，需要以當(dāng)前漂移位置作為基準(zhǔn)，可以有效減弱飛行機(jī)器人自身定位精度對于自主降落系統(tǒng)本身的影響。此外，對機(jī)器人到達(dá)目標(biāo)位置的判斷要給予一定的容錯(cuò)估計(jì)，當(dāng)實(shí)測位置誤差小于某一閾值且持續(xù)一定時(shí)間，認(rèn)為機(jī)器人已經(jīng)到達(dá)其目標(biāo)位置。

對于初始化狀態(tài)機(jī)（initialization），需要執(zhí)行傳感器校準(zhǔn)，傳感器狀態(tài)監(jiān)控，通信連接狀態(tài)確認(rèn)。在所有狀態(tài)就緒后，使能模式切換，由遙控指令輸入切換回機(jī)載傳感器指令輸入，完成手動(dòng)模式向自主模式的切換，使能解鎖服務(wù)，一切就緒后，進(jìn)入起飛狀態(tài)機(jī)。

對于起飛狀態(tài)機(jī)（take_off），考慮到定位數(shù)據(jù)的漂移影響，不以[0，0，0]作為初始位姿，而記錄當(dāng)前實(shí)測位姿數(shù)據(jù)，以該數(shù)據(jù)作為初始點(diǎn)。同時(shí)，開啟俯視相機(jī)，開啟目標(biāo)檢測功能，不斷更新標(biāo)志物在相機(jī)視野中的位姿，到達(dá)初始起飛位置。

對于目標(biāo)搜索狀態(tài)機(jī)（target_search），結(jié)合飛行高度和RealSense 相機(jī)視場角范圍，給定初始目標(biāo)所在矩形區(qū)域大小，按一定策略遍歷區(qū)域內(nèi)目標(biāo)位置，識(shí)別到目標(biāo)后，切換懸停校準(zhǔn)狀態(tài)機(jī)。倘若遍歷區(qū)域內(nèi)所有位置之后未識(shí)別到目標(biāo)，返回初始點(diǎn)降落。

對于懸停校準(zhǔn)狀態(tài)機(jī)（hold_calibration），首先記錄初始懸停位置，根據(jù)位姿估計(jì)結(jié)果進(jìn)行懸停校準(zhǔn)。如果在懸停校準(zhǔn)狀態(tài)機(jī)下目標(biāo)丟失，切換目標(biāo)搜索狀態(tài)機(jī)。如果目標(biāo)相對機(jī)身水平位置精度在一定時(shí)間內(nèi)穩(wěn)定滿足誤差要求且目標(biāo)滿足穩(wěn)定檢測許可判斷，切換自主降落狀態(tài)機(jī)。若未達(dá)到精度要求但目標(biāo)滿足穩(wěn)定檢測許可判斷，切換水平接近狀態(tài)機(jī)。

對于水平接近狀態(tài)機(jī)（horizon_approach），要以當(dāng)前實(shí)測水平位置作為基準(zhǔn)，若到達(dá)目標(biāo)位置且滿足穩(wěn)定檢測許可判斷，切換懸停校準(zhǔn)狀態(tài)機(jī)，若到達(dá)目標(biāo)位置但檢測丟失，切換目標(biāo)搜索狀態(tài)機(jī)。

對于自主降落狀態(tài)機(jī)（auto_landing），本算法采用的定高設(shè)備為氣壓計(jì)，考慮到飛行機(jī)器人的近地效應(yīng)會(huì)對氣壓計(jì)的估計(jì)精度產(chǎn)生較大影響，自主降落任務(wù)完成的判斷標(biāo)準(zhǔn)由IMU 數(shù)據(jù)得出。當(dāng)Z軸速度穩(wěn)定小于某一閾值且持續(xù)0.5 s 以上，判斷飛行機(jī)器人已降落，執(zhí)行電機(jī)上鎖，退出自主模式，由遙控接管指令輸入。

5 仿真驗(yàn)證分析

5.1 自主跟蹤及降落Gazebo 仿真

為驗(yàn)證所編寫的基于視覺定位的地面平臺(tái)自主跟蹤及降落算法可靠性，在Gazebo 中建立飛行機(jī)器人仿真模型進(jìn)行硬件在環(huán)仿真，如圖9所示。

圖9 自主跟蹤及降落仿真Fig.9 Simulation of autonomous tracking and landing

終端為程序輸出，左上角為俯視相機(jī)機(jī)載視野，右下角為地面站狀態(tài)監(jiān)測。使用筆記本電腦替代機(jī)載處理器發(fā)布任務(wù)指令，地面站負(fù)責(zé)監(jiān)控當(dāng)前機(jī)器人狀態(tài)，對算法進(jìn)行優(yōu)化設(shè)計(jì)驗(yàn)證其準(zhǔn)確有效性。

5.2 公開數(shù)據(jù)集驗(yàn)證

在EuRoC 數(shù)據(jù)集［24］上測試定位算法結(jié)果，該數(shù)據(jù)集基于微型飛行器的室內(nèi)飛行實(shí)驗(yàn)，包含了相機(jī)和IMU 的信息，采用的數(shù)據(jù)集序列為MH_01～MH_05、V1_01～V1_03 共8 個(gè) 數(shù) 據(jù)集，每個(gè)數(shù)據(jù)集基于2 種改進(jìn)思路的不同組合進(jìn)行了5 次對比實(shí)驗(yàn)，得到40 組實(shí)驗(yàn)結(jié)果。其中，MH_01_easy 數(shù)據(jù)集場景偏暗，V1_02_medium數(shù)據(jù)集飛行速度變化較快，適合本文所改進(jìn)的定位算法應(yīng)用場景，故著重分析這2 個(gè)數(shù)據(jù)集的實(shí)驗(yàn)結(jié)果。采用里程計(jì)評測（Evaluation of Odometry， EVO）工具評估改進(jìn)前后的算法估計(jì)值與真實(shí)軌跡的差異，計(jì)算每對位姿之間的絕對軌跡誤差，通過計(jì)算均方根誤差（Root Mean Square Error， RMSE）、平 均 誤 差（Mean Absolute Error， MAE）和 方 差（Sum of Squares for Error， SSE）3 項(xiàng)指標(biāo)來評估定位精度。MH_01_easy 數(shù)據(jù)集評估結(jié)果如表2 所示，V1_02_medium 數(shù)據(jù)集評估結(jié)果如表3 所示。

表2 MH_01_easy 數(shù)據(jù)集評估結(jié)果Table 2 MH_01_easy dataset evaluation results

表3 V1_02_medium 數(shù)據(jù)集評估結(jié)果Table 3 V1_02_medium dataset evaluation results

改進(jìn)方案相比于原有方案，策略1 中僅加入了圖像增強(qiáng)算法，策略2 中僅加入了本文提出的一種基于圖像信息的特征點(diǎn)提取和圖像幀發(fā)布策略，改進(jìn)后的方法同時(shí)加入了以上2 種改進(jìn)思路，用以提高飛行機(jī)器人飛行過程中的定位精度。實(shí)驗(yàn)結(jié)果表明，平均誤差和均方根誤差在改進(jìn)后均減小，其中，MH_01_easy 數(shù)據(jù)集場景偏暗，飛行速度變化較慢，僅引入圖像增強(qiáng)算法處理?xiàng)l件下，平均誤差減少了7%，均方根誤差減少了5%，方差減少了10%；僅引入一種基于圖像信息的特征點(diǎn)提取和圖像幀發(fā)布策略后，平均誤差減少了2%，均方根誤差減少了1%，方差減少了5%；同時(shí)引入兩種改進(jìn)思路后，平均誤差減小了10%，均方根誤差減少了7%，方差減小了13%。V1_02_medium 數(shù)據(jù)集場景圖像對比度較高，飛行速度變化較快，僅引入圖像增強(qiáng)算法處理?xiàng)l件下，平均誤差減少了3%，均方根誤差減少了2%，方差減少了4%；僅引入一種基于圖像信息的特征點(diǎn)提取和圖像幀發(fā)布策略后，平均誤差減少了12%，均方根誤差減少了9%，方差減少了18%；同時(shí)引入兩種改進(jìn)思路后，平均誤差減小了14%，均方根誤差減少了10%，方差減小了20%。測試結(jié)果表明，在場景整體較暗使得圖像對比度較低時(shí)引入圖像增強(qiáng)算法效果明顯，而在飛行機(jī)器人運(yùn)動(dòng)速度較快使得相機(jī)視野中圖像特征變化較快時(shí)引入本文提出的一種基于圖像信息的特征點(diǎn)提取和圖像幀發(fā)布策略效果明顯。同時(shí)加入2 種改進(jìn)思路后，改進(jìn)后的算法相比于原有方案，定位精度均有較大的提升。

6 飛行實(shí)驗(yàn)驗(yàn)證

6.1 多旋翼飛行機(jī)器人平臺(tái)

基于任務(wù)需求設(shè)計(jì)并搭建多旋翼飛行機(jī)器人平臺(tái)，硬件系統(tǒng)包含F(xiàn)550 機(jī)架及動(dòng)力系統(tǒng)，Pixhawk 飛控及配套飛行設(shè)備一套。傳感設(shè)備為RealSenseD455 與RealSenseD435i 相 機(jī)，板 載 處理設(shè)備選用Jetson Xavier NX，飛行機(jī)器人平臺(tái)如圖10 所示。

圖10 多旋翼飛行機(jī)器人平臺(tái)Fig.10 Multi rotor flying robot platform

6.2 圖像增強(qiáng)及角點(diǎn)提取實(shí)驗(yàn)

采用飛行機(jī)器人機(jī)載相機(jī)獲取當(dāng)前環(huán)境中圖像信息，應(yīng)用本文圖像增強(qiáng)算法實(shí)驗(yàn)處理后的結(jié)果如圖11 所示，左側(cè)為輸入RGB 圖像，中間為灰度圖，右側(cè)為引入圖像增強(qiáng)算法處理后的結(jié)果。

圖11 圖像增強(qiáng)算法處理結(jié)果Fig.11 Image enhancement algorithm processing results

在室內(nèi)實(shí)驗(yàn)室環(huán)境下，旋轉(zhuǎn)飛行機(jī)器人機(jī)載相機(jī)每隔15°拍攝一組圖像，如圖12 所示。測試結(jié)果表明，Shi-Tomasi 具有良好的旋轉(zhuǎn)不變性，對L 型角點(diǎn)檢測效率較高，提取速度較快，4 幅圖像提取角點(diǎn)數(shù)量的平均值為158，平均耗時(shí)0.076 ms，總耗時(shí)12.008 ms，RealSense 相機(jī)彩色圖像輸出頻率為30 Hz，可以滿足實(shí)時(shí)處理的要求。

圖12 角點(diǎn)提取實(shí)驗(yàn)Fig.12 Corner extraction experiment

6.3 室內(nèi)定位實(shí)驗(yàn)

在機(jī)載處理器中運(yùn)行室內(nèi)定位算法，完成室內(nèi)懸停實(shí)驗(yàn)，如圖13 所示。基于Motion Analysis運(yùn)動(dòng)捕捉系統(tǒng)提供的定位真值評估定位結(jié)果，三軸定位數(shù)據(jù)比較如圖14 所示，橫軸表示時(shí)間，縱軸表示定位誤差。實(shí)驗(yàn)結(jié)果表明，在懸停過程中，定位誤差保持在10 cm 以內(nèi)，可以在室內(nèi)環(huán)境下實(shí)現(xiàn)穩(wěn)定懸停，滿足要求。

圖13 飛行機(jī)器人室內(nèi)懸停Fig.13 Indoor hovering of flying robot

圖14 三軸定位數(shù)據(jù)對比Fig.14 Comparison of three-axis positioning data

完成室內(nèi)懸停實(shí)驗(yàn)后，為進(jìn)一步驗(yàn)證本文引入的圖像增強(qiáng)算法及一種基于圖像信息的特征點(diǎn)提取和圖像幀發(fā)布策略的改進(jìn)效果，在暗光環(huán)境及明亮環(huán)境下完成對比實(shí)驗(yàn)。實(shí)驗(yàn)場景如圖15所示，圖15中A為暗光環(huán)境，B為明亮環(huán)境。

圖15 飛行實(shí)驗(yàn)場景Fig.15 Flight experiment scene

采用遙控器輸入位置指令，在室內(nèi)場景下完成飛行機(jī)器人封閉軌跡實(shí)驗(yàn)，飛行實(shí)驗(yàn)選擇在以實(shí)驗(yàn)室中心位置為區(qū)域中心，高度為0.7 m，水平區(qū)域?yàn)? m×2.5 m 的范圍內(nèi)進(jìn)行，定位結(jié)果見圖16 和圖17。其中，圖16 為暗光環(huán)境下改進(jìn)前方法與本文方法定位數(shù)據(jù)對比，圖17 為明亮環(huán)境及暗光環(huán)境兩種不同光照條件下本文方法定位數(shù)據(jù)與動(dòng)捕系統(tǒng)的真值數(shù)據(jù)對比。

圖16 暗光環(huán)境下水平定位數(shù)據(jù)對比Fig.16 Comparison of horizontal positioning data in dark environment

圖17 不同光照條件下水平定位數(shù)據(jù)對比Fig.17 Comparison of horizontal positioning data under different lighting conditions

封閉軌跡實(shí)驗(yàn)結(jié)果表明，在靠近窗口的位置即X軸位置超過3.5 m 沿Y軸正方向飛行時(shí)，相機(jī)視野中圖像特征更新較快，參與優(yōu)化的圖像幀較多，同時(shí)，窗簾的抖動(dòng)被視為動(dòng)態(tài)目標(biāo)，對算法精度造成了比較大的影響。通過計(jì)算平均誤差，在室內(nèi)實(shí)驗(yàn)室較暗環(huán)境下，改進(jìn)前算法定位平均誤差為0.26 m，本文方法定位平均誤差為0.17 m，有效提高了定位精度。此外，在室內(nèi)實(shí)驗(yàn)室明亮環(huán)境下重復(fù)實(shí)驗(yàn)，本文方法定位平均誤差為0.14 m，滿足室內(nèi)自主跟蹤及降落實(shí)驗(yàn)的需求。

6.4 室內(nèi)自主跟蹤及降落實(shí)驗(yàn)

在完成飛行機(jī)器人的室內(nèi)定位實(shí)驗(yàn)后，在6 m×6 m 的區(qū)域內(nèi)隨意放置靶標(biāo)，在機(jī)載處理器上同時(shí)運(yùn)行本文所編寫的基于視覺定位的地面平臺(tái)自主跟蹤及降落算法，進(jìn)行飛行機(jī)器人室內(nèi)自主跟蹤及降落實(shí)驗(yàn)，實(shí)驗(yàn)分為以下6 個(gè)部分：系統(tǒng)初始化、自主起飛、區(qū)域搜索、懸停校準(zhǔn)、水平接近、自主降落，圖18 為實(shí)驗(yàn)結(jié)果。

圖18 中左下角為自主定位算法輸出的位置估計(jì)值，右上角為機(jī)載俯視相機(jī)視野。A 為系統(tǒng)初始化：運(yùn)行自主定位算法和自主導(dǎo)航算法，等待遙控器給予模式切換指令；B 為自主起飛：遙控器給予模式切換指令，使能自主導(dǎo)航任務(wù)中的失控保護(hù)；C、D 為區(qū)域搜索：在6 m×6 m 的區(qū)域內(nèi)以一定策略遍歷區(qū)域內(nèi)所有位置，搜索地面靶標(biāo)位置；E 為懸停校準(zhǔn)：識(shí)別到靶標(biāo)位置后，飛行機(jī)器人切換懸停模式，解算地面目標(biāo)相對于機(jī)身本體的位姿并且持續(xù)更新；F 為水平接近：根據(jù)懸停校準(zhǔn)的結(jié)果來執(zhí)行水平迫近；G 為自主降落：當(dāng)靶標(biāo)位置相對于機(jī)身本體的水平位置誤差達(dá)到允許閾值，執(zhí)行自主降落；H 為上鎖保護(hù)：由于近地效應(yīng)的影響，氣壓計(jì)的高度估計(jì)會(huì)存在比較大的誤差，因此，通過讀取IMU 的Z軸速度來執(zhí)行上鎖保護(hù)，當(dāng)Z軸速度小于0.2 m/s 且持續(xù)0.5 s 以上時(shí)，飛控及電機(jī)上鎖，實(shí)驗(yàn)結(jié)束。

圖18 室內(nèi)自主跟蹤及降落實(shí)驗(yàn)Fig.18 Indoor autonomous tracking and landing experiment

實(shí)驗(yàn)結(jié)果表明，本文設(shè)計(jì)的室內(nèi)條件下飛行機(jī)器人自主跟蹤及降落算法存在一定的降落誤差，造成誤差的原因有以下3 個(gè)方面：識(shí)別模塊的精度，算法中自主降落狀態(tài)機(jī)中允許降落時(shí)設(shè)定的水平位置誤差閾值，以及最后執(zhí)行自主降落過程時(shí)的水平定位誤差。經(jīng)過多次迭代實(shí)驗(yàn)并優(yōu)化算法中各項(xiàng)指標(biāo)參數(shù)，最終確定在降落過程進(jìn)行兩次懸停校準(zhǔn)，同時(shí)設(shè)置降落狀態(tài)機(jī)初始許可水平誤差閾值為7 cm，實(shí)驗(yàn)相關(guān)數(shù)據(jù)如表4所示。

表4 自主跟蹤及降落實(shí)驗(yàn)結(jié)果Table 4 Results of autonomous tracking and landing experiments

重復(fù)實(shí)驗(yàn)表明，所設(shè)計(jì)的飛行機(jī)器人室內(nèi)自主降落系統(tǒng)誤差維持在10 cm 以內(nèi)，有效地減弱了室內(nèi)定位實(shí)驗(yàn)中飛行機(jī)器人自身定位精度誤差對于室內(nèi)自主降落系統(tǒng)本身的影響。

7 結(jié) 論

1）為實(shí)現(xiàn)自主跟蹤及降落任務(wù)，編寫基于視覺定位的飛行機(jī)器人自主跟蹤及降落算法，在Gazebo 中搭建飛行機(jī)器人模型完成仿真驗(yàn)證。

2）針對飛行機(jī)器人室內(nèi)定位需求，在視覺前端加入圖像增強(qiáng)處理，提出了一種基于圖像信息的特征點(diǎn)提取和圖像幀發(fā)布策略以提高定位精度，在EuRoC 數(shù)據(jù)集下采用EVO 工具評估定位效果。結(jié)果表明，改進(jìn)的算法可以有效提高機(jī)器人定位精度。其中，MH_01_easy 數(shù)據(jù)集場景下平均誤差減小了10%，均方根誤差減少了7%，方差減小了13%，V1_02_medium 數(shù)據(jù)集場景下平均誤差減小了14%，均方根誤差減少了10%，方差減小了20%。

3）真實(shí)場景下搭建了一套多旋翼飛行機(jī)器人平臺(tái)，完成室內(nèi)定位實(shí)驗(yàn)，基于運(yùn)動(dòng)捕捉系統(tǒng)評估真實(shí)場景下飛行機(jī)器人定位精度。根據(jù)任務(wù)需求，完成室內(nèi)場景下對地面平臺(tái)的自主跟蹤及降落實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明，室內(nèi)實(shí)驗(yàn)室場景下，封閉軌跡實(shí)驗(yàn)的定位平均誤差不超過0.17 m，所設(shè)計(jì)的飛行機(jī)器人室內(nèi)自主降落系統(tǒng)誤差保持在0.1 m 以內(nèi)，有效地減弱了室內(nèi)定位實(shí)驗(yàn)中飛行機(jī)器人自身定位精度誤差對于室內(nèi)自主降落系統(tǒng)本身的影響，滿足室內(nèi)場景下的自主跟蹤及降落任務(wù)需求。

未來將基于本文的研究工作組合地面異構(gòu)機(jī)器人目標(biāo)實(shí)現(xiàn)空地協(xié)同任務(wù)，拓展應(yīng)用場景，提升機(jī)器人在復(fù)雜場景下的適應(yīng)能力。