陳玉權，翟學鋒，宋 煜，吳 媚，張星煒，曹世鵬

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

0 引言

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

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

1 相關理論

(1) SLAM跟蹤

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

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

圖1 圖像金字塔

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

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

(1)

N層金字塔的總面積為：

(2)

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

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

圖2 FAST角點提取

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

(2)局部地圖構建

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

1) 關鍵幀之間要間隔20幀以上；

2) 當前幀與其他關鍵幀的匹配點數(shù)要達到50以上；

3) 當前幀與關鍵幀的共視點達到90%以上；

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

(1) 有三個以上關鍵幀可以檢測到該點；

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

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

2 基于RTK的多傳感器融合

針對無人機精準定位導航的巡檢要求，本文研究使用EKF建立無人機對應的運動學模型，將通過RTK技術提高精度后的定位信息融入到基于視覺信息的SLAM方案中。

2.1 人機運動模型設計

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

圖3 無人機運動學模型

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

2.2 EKF參數(shù)設定

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

當t時刻SLAM更新時，對應X,Y,Z與位姿信息都會更新。矩陣H表示為：

Hk=(I6×6O6×9)

(3)

濾波器預測流程與標準的EKF一致，下一刻的狀態(tài)估計為：

(4)

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

(5)

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

(6)

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

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

(7)

對測量殘差進行更新：

(8)

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

(9)

對卡爾曼增益進行更新：

(10)

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

(11)

對EKF方差進行更新：

(12)

2.3 導航系統(tǒng)構成

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

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

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

3 實驗驗證及分析

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

圖5 設定航點圖和測試軌跡圖

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

表1 EKF與RTK航跡坐標與設定航跡坐標測量誤差

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

圖6 XYZ軸誤差圖像

4 結(jié)論

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