陳玉權(quán)，翟學(xué)鋒，宋 煜，吳 媚，張星煒，曹世鵬

(1. 江蘇方天電力技術(shù)有限公司, 南京 211102; 2. 眾芯漢創(chuàng)(北京)科技有限公司, 北京 100080)

0 引言

輸電線路是電網(wǎng)系統(tǒng)的重要組成部分，是支撐國民生產(chǎn)和生活的運(yùn)輸命脈[1-3]。由于我國地形復(fù)雜多樣，部分輸電線路處于較極端的地理環(huán)境中，如有受損則會影響電力的正常運(yùn)輸，因此需要線路巡檢來保證電力穩(wěn)定傳輸。傳統(tǒng)的人工巡檢有著耗時(shí)長，投入大，且容易受環(huán)境條件影響的缺點(diǎn)，無人機(jī)電力巡檢技術(shù)的引入很大程度上解決了這一難題。但是手動操控?zé)o人機(jī)需要巡檢工作人員具備較高的無人機(jī)操控技術(shù)，因此無人機(jī)全自主巡檢成為新的可選方案，可以保證巡檢的質(zhì)量以及采集數(shù)據(jù)的規(guī)范，提高巡檢效率。其中，計(jì)算機(jī)視覺、定位技術(shù)的發(fā)展為自主巡檢提供了技術(shù)保障[3]。RTK定位技術(shù)[4-5]可以消除信號傳遞過程中造成的定位誤差，不僅可以通過相機(jī)來拍攝線路巡檢狀態(tài)，還可以通過圖像處理獲得自身位置信息，實(shí)現(xiàn)自主的導(dǎo)航與拍攝，SLAM則能在未知的環(huán)境中，利用其搭載的傳感器建立地圖，進(jìn)行定位與導(dǎo)航，比較常用的算法是ORB-SLAM[6-8]。然而無人機(jī)自主巡檢的方案還不成熟，僅使用相機(jī)定位導(dǎo)航的無人機(jī)自主巡檢現(xiàn)在仍面臨在障礙物集中，視線較差的環(huán)境下難以做到合理避障的問題。比如基于相機(jī)拍攝的SLAM容易受光照條件影響，在光照較暗的情況下精度不佳，因而不利于無人機(jī)在室外導(dǎo)航使用[9-11]。因此需要使用多傳感器搭配SLAM技術(shù)，來彌補(bǔ)單一傳感器的不足，提高無人機(jī)的定位導(dǎo)航能力與穩(wěn)定性?；诙鄠鞲衅鞯腟LAM定位方案中，比較常用的是IMU與視覺傳感器融合的方案。

本文研究引入位置信息融合即時(shí)定位與地圖構(gòu)建算法，使用擴(kuò)展卡爾曼濾波器作為數(shù)據(jù)融合方法，采用實(shí)時(shí)動態(tài)技術(shù)，結(jié)合了高精度的定位信息與其他傳感器的獲得的參數(shù)，在飛行控制中進(jìn)行邏輯數(shù)據(jù)融合，將濾波器輸出的位姿信息反饋到無人機(jī)飛行控制終端，來提高無人機(jī)的定位精度與導(dǎo)航能力, 以此調(diào)整飛行控制方式與飛行機(jī)制，做到準(zhǔn)確避障，實(shí)現(xiàn)自主飛行。

1 相關(guān)理論

(1) SLAM跟蹤

跟蹤是SLAM系統(tǒng)中的主進(jìn)程，通過相機(jī)提取圖像特征并進(jìn)行匹配，跟蹤每一幀相機(jī)，對無人機(jī)進(jìn)行位姿估計(jì)與優(yōu)化。與SURF，SIFT等圖像特征相比較，ORB特征運(yùn)算量較少，可以通過CPU計(jì)算獲得同等于GPU的特征效果，并且ORB特征具有尺度不變性與旋轉(zhuǎn)不變性，因此在SLAM需要實(shí)時(shí)信息來計(jì)算的情況下，常用ORB作為圖像特征。ORB角點(diǎn)提取流程主要如下：

構(gòu)建層數(shù)為N的特征金字塔，根據(jù)金字塔層數(shù)的增加獲取圖像特征數(shù)量減少，如圖1所示。

圖1 圖像金字塔

對每層圖像特征分配采用如下策略。設(shè)金字塔底層圖像像素寬度為W，高度為H，尺度因子為s，第n層金字塔的面積S。

S=W×H×(s2)n-1

(1)

N層金字塔的總面積為：

(2)

本文中使用的ORB-SLAM設(shè)定提取的特征金字塔高度為8，尺度因子為1.2。提取ORB特征的流程主要如下：

提取FAST角點(diǎn)。設(shè)置閾值T，以待確定的像素點(diǎn)p(像素值Ip)為圓心，繪制半徑為3像素的圓(如圖2所示)。如果圓周上n個(gè)點(diǎn)的像素值大于Ip+T或者小于Ip-T，則確定該點(diǎn)為角點(diǎn)。

圖2 FAST角點(diǎn)提取

計(jì)算rBRIEF描述子[12]。以角點(diǎn)為中心，選取31×31的圖像鄰域，每個(gè)像素使用其領(lǐng)域中5×5的平均灰度值代替，因此對應(yīng)獲得n=(31-5+1)×(31-5+1)個(gè)對比的像素塊。像素塊兩兩之間進(jìn)行比較，獲得長度為m=C(N,2)的二進(jìn)制編碼。根據(jù)均方差最大原理，篩選獲得256對，即為rBRIEF描述子。提取特征后，需要進(jìn)行算法初始化，對前后兩幀圖像進(jìn)行特征匹配。當(dāng)前若匹配特征點(diǎn)數(shù)較少，則認(rèn)為跟蹤丟失，此時(shí)算法會進(jìn)行重定位，尋找新的相鄰圖像幀進(jìn)行初始化。初始化成功后，以前一幀圖像為原點(diǎn)建立坐標(biāo)系。在運(yùn)動過程中，將從當(dāng)前圖像幀提取的圖像特征與原點(diǎn)匹配，獲得后一幀圖像的旋轉(zhuǎn)角度與位移量。通過多視角幾何算法，可以解算出當(dāng)前無人機(jī)的位姿。同時(shí)還以三角測量獲得的三維坐標(biāo)設(shè)為地圖的初始點(diǎn)，并使用全集束調(diào)整對位姿和地圖點(diǎn)進(jìn)行優(yōu)化。

(2)局部地圖構(gòu)建

局部地圖構(gòu)建主要是在跟蹤過程中插入新的關(guān)鍵幀，并對局部地圖進(jìn)行局部BA優(yōu)化[13]。判斷當(dāng)前幀是否為關(guān)鍵幀，若是關(guān)鍵幀，則插入局部地圖。關(guān)鍵幀篩選主要有以下幾個(gè)原則：

1) 關(guān)鍵幀之間要間隔20幀以上；

2) 當(dāng)前幀與其他關(guān)鍵幀的匹配點(diǎn)數(shù)要達(dá)到50以上；

3) 當(dāng)前幀與關(guān)鍵幀的共視點(diǎn)達(dá)到90%以上；

插入關(guān)鍵幀到地圖中還需要剔除部分地圖點(diǎn)。地圖點(diǎn)是通過相鄰的關(guān)鍵幀多視角幾何計(jì)算得到的，保留地圖點(diǎn)不被剔除有如下的原則：

(1) 有三個(gè)以上關(guān)鍵幀可以檢測到該點(diǎn)；

(2) 跟蹤時(shí)，有四分之一的可視幀可以匹配到該點(diǎn)。

得到保留地圖點(diǎn)的集合后，計(jì)算保留地圖點(diǎn)的空間位置，將新產(chǎn)生的地圖點(diǎn)添加到地圖中，使用局部集束優(yōu)化(Local Bundle Adjustment, Local BA)調(diào)整局部地圖點(diǎn)和位姿，對相鄰的關(guān)鍵幀中重復(fù)的地圖點(diǎn)進(jìn)行融合，然后調(diào)整當(dāng)前處理的關(guān)鍵幀，檢測去除冗余的關(guān)鍵幀。

2 基于RTK的多傳感器融合

針對無人機(jī)精準(zhǔn)定位導(dǎo)航的巡檢要求，本文研究使用EKF建立無人機(jī)對應(yīng)的運(yùn)動學(xué)模型，將通過RTK技術(shù)提高精度后的定位信息融入到基于視覺信息的SLAM方案中。

2.1 人機(jī)運(yùn)動模型設(shè)計(jì)

本文采用的無人機(jī)模型狀態(tài)數(shù)量主要參考ROS的開發(fā)包，選用維度為15的狀態(tài)向量。濾波器主要有4個(gè)輸入模塊，分別為IMU，RTK、氣壓計(jì)(測量高度)以及超聲模塊。RTK模塊提供修正后的水平方向上位置信息，氣壓計(jì)提供高度信息，IMU提供姿態(tài)角信息。各個(gè)傳感器傳入數(shù)據(jù)構(gòu)建狀態(tài)變量的示意圖如圖3所示。

圖3 無人機(jī)運(yùn)動學(xué)模型

其中，X,Y,Z表示位置坐標(biāo)信息，θ,Ψ,Φ表示姿態(tài)角中的俯仰角、偏航角、翻滾角。

2.2 EKF參數(shù)設(shè)定

擴(kuò)展卡爾曼濾波算法，在無人機(jī)SLAM中，將無人機(jī)定位坐標(biāo)信息，其余傳感器的數(shù)據(jù)按照圖3所示組合，構(gòu)成狀態(tài)向量的初始值和濾波器協(xié)方差矩陣以及噪聲協(xié)方差。

當(dāng)t時(shí)刻SLAM更新時(shí)，對應(yīng)X,Y,Z與位姿信息都會更新。矩陣H表示為：

Hk=(I6×6O6×9)

(3)

濾波器預(yù)測流程與標(biāo)準(zhǔn)的EKF一致，下一刻的狀態(tài)估計(jì)為：

(4)

對系統(tǒng)函數(shù)求導(dǎo)(計(jì)算雅克比矩陣)：

(5)

下一時(shí)刻的均方誤差預(yù)測結(jié)果為：

(6)

最后的預(yù)測結(jié)果為：

輸出結(jié)果維度為6，即預(yù)測得到下一時(shí)刻的位姿信息。對測量向量進(jìn)行更新：

(7)

對測量殘差進(jìn)行更新：

(8)

對測量殘差協(xié)方差進(jìn)行更新：

(9)

對卡爾曼增益進(jìn)行更新：

(10)

對狀態(tài)估計(jì)值進(jìn)行更新：

(11)

對EKF方差進(jìn)行更新：

(12)

2.3 導(dǎo)航系統(tǒng)構(gòu)成

在實(shí)際情況下，如果應(yīng)用SLAM于整個(gè)系統(tǒng)，計(jì)算量十分龐大。因此將相機(jī)作為視覺里程計(jì)，僅使用SLAM處理圖像數(shù)據(jù)[14-17]。處理后數(shù)據(jù)與傳感器觀測數(shù)據(jù)使用EKF算法進(jìn)行融合，最終得到下一時(shí)刻的預(yù)測值(位姿信息)，輸出到無人機(jī)控制系統(tǒng)中。導(dǎo)航系統(tǒng)的構(gòu)成如圖4所示。

圖4 導(dǎo)航系統(tǒng)流程圖

控制系統(tǒng)根據(jù)當(dāng)前位置與預(yù)測值的距離，以及姿態(tài)的差值判斷是否在設(shè)定的閾值范圍內(nèi)，來決定飛行是否需要調(diào)整路線與飛行姿態(tài)。

3 實(shí)驗(yàn)驗(yàn)證及分析

在室外場地驗(yàn)證導(dǎo)航系統(tǒng)性能，主要方式如下：設(shè)定飛行軌跡，在無人機(jī)處理后端上運(yùn)行擴(kuò)展卡爾曼濾波器，將融合數(shù)據(jù)通過藍(lán)牙發(fā)送至無人機(jī)控制芯片，在無人機(jī)飛行到距離預(yù)計(jì)位置在設(shè)定閾值0.5 m范圍以內(nèi)時(shí)，輸入下一刻位置。無人機(jī)通過PID算法來對飛行位置進(jìn)行調(diào)整。本次測試以繞行電塔為例，從地面起飛，圍繞電塔一周進(jìn)行拍攝，設(shè)定軌跡，對比使用RTK進(jìn)行導(dǎo)航飛行的軌跡與使用融合數(shù)據(jù)的EKF導(dǎo)航飛行軌跡如圖5所示。

圖5 設(shè)定航點(diǎn)圖和測試軌跡圖

從表1可以看出，表示使用融合數(shù)據(jù)的EKF得到航跡的坐標(biāo)與設(shè)定軌跡的最大誤差值和平均誤差值均小于僅使用RTK導(dǎo)航的誤差，可見融合視覺SLAM的數(shù)據(jù)通過EKF輸出，能提高導(dǎo)航系統(tǒng)的飛行精度與穩(wěn)定性。但在圖5中可以看到，在起飛降落過程中，在水平面方向上的定位會出現(xiàn)較大偏差。這是因?yàn)樵趯?shí)際過程中，無人機(jī)起飛與降落受空氣阻力等影響較大的原因?qū)е?。由于無人機(jī)在飛行中易受到風(fēng)擾，在z軸方向上，無人機(jī)誤差存在持續(xù)的抖動，因此平均誤差相較于水平方向上更大。

表1 EKF與RTK航跡坐標(biāo)與設(shè)定航跡坐標(biāo)測量誤差

對無人機(jī)兩種導(dǎo)航定位方式的航行坐標(biāo)進(jìn)行采樣，得到各個(gè)坐標(biāo)軸上波形圖，如圖6所示。其中在x軸、y軸圖像上出現(xiàn)較大振蕩，與圖5無人機(jī)在起飛降落階段航跡呈現(xiàn)的誤差保持一致。

圖6 XYZ軸誤差圖像

4 結(jié)論

本文提出一種將RTK定位信息與SLAM數(shù)據(jù)融合的無人機(jī)高精度定位和視覺自動跟蹤方法，核心思想是利用SLAM方法提供的數(shù)據(jù)與RTK模塊獲得的位置數(shù)據(jù)，使用EKF進(jìn)行融合，對無人機(jī)進(jìn)行狀態(tài)估計(jì)，輸出位姿信息，后利用無人機(jī)飛行控制模塊對位姿信息進(jìn)行解析，以此控制硬件，調(diào)整當(dāng)前的飛行姿態(tài)，進(jìn)行準(zhǔn)確的導(dǎo)航，完成電力設(shè)備的巡檢。本文方法提高了無人機(jī)定位精度，相比較普通的GPS定位，誤差可以縮小到cm級。