郭關(guān)有，姚仁和，杜白雨，趙志萍,院老虎

(1.沈陽航空航天大學(xué) 航空宇航學(xué)院，沈陽 110136;2.上海航天技術(shù)研究院 上海宇航系統(tǒng)工程研究所，上海 201109)

隨著技術(shù)更新和需求增長，無人機產(chǎn)業(yè)在軍民兩用方面得到了快速發(fā)展。對于無人機系統(tǒng)，能夠準(zhǔn)確定位的先進(jìn)導(dǎo)航系統(tǒng)是實現(xiàn)其自主導(dǎo)航的基礎(chǔ)。針對室內(nèi)環(huán)境，多旋翼無人機因其明顯優(yōu)勢成為大多數(shù)科研人員的研究對象。目前室內(nèi)定位方式主要有基于測距儀的定位、基于聲納傳感器的定位、基于視覺的定位、基于慣性導(dǎo)航和視覺融合系統(tǒng)的定位，以及其他多智能傳感器的組合定位。但是這些方法尚未形成完善的體系，室內(nèi)環(huán)境的復(fù)雜性、無人機狀態(tài)的不確定性更是增加了自主定位的難度[1]。多種定位方式中，基于視覺的定位技術(shù)以其價格低廉、部署簡單且不存在信號屏蔽等優(yōu)點被研究人員關(guān)注[2-4]。

國外最早開展了有關(guān)基于視覺的無人機室內(nèi)定位研究，從1966年開始，美國的麻省理工學(xué)院便開始了室內(nèi)定位導(dǎo)航的研究[2]。AHRENS S.等人[5]開發(fā)了一種四旋翼無人機的系統(tǒng)結(jié)構(gòu)和核心算法，通過視覺傳感器，實現(xiàn)了無GPS室內(nèi)環(huán)境下的無人機導(dǎo)航和避障。MUSTAFAH Y.M.[6]等人利用雙目立體視覺傳感器設(shè)計了一種無人機室內(nèi)定位系統(tǒng)，實現(xiàn)了實時獲取無人機的位置信息功能。最近，SANTOS M.C.P.等人[7]研發(fā)了一種低成本的定位系統(tǒng)，該系統(tǒng)利用RGB-D傳感器、慣性測量單元(IMU)和超聲傳感器，基于光流算法實時估計無人機的速度數(shù)據(jù)。

在國內(nèi)，何芳[8]設(shè)計了一套適用于無人機室內(nèi)自主定位的半直接法視覺SLAM系統(tǒng)，能夠高效地確定空間三維點的準(zhǔn)確位置。陳曄[2]搭建了無人機雙目視覺定位測試系統(tǒng)，以SIFT算法作為特征點提取與匹配的主要方法，實現(xiàn)了四旋翼的室內(nèi)定位。張澤群[9]采用雙目立體視覺，對四旋翼無人機在普通室內(nèi)環(huán)境下的定位和避障進(jìn)行了研究。最近，余莎莎等人[10]通過RSS(Received Signal Strength)定位算法進(jìn)行了無人機自主三維定位研究，仿真實驗表明，該方法具有精度高、無誤差積累和適應(yīng)性強的特點。

本文針對室內(nèi)場景下的無人機定位問題，采用雙目視覺傳感器，利用簡單地標(biāo)和雙目測距技術(shù)實現(xiàn)的RSS定位算法來實現(xiàn)無人機的室內(nèi)自主三維定位。該方案的定位計算均由傳感器采集的圖像和完整的算法流程完成，因而具有定位自主性；相較于單目視覺定位系統(tǒng)，雙目立體視覺系統(tǒng)定位精度更高，視角更廣闊，通過采用RSS定位算法，避免了定位誤差因航行距離增大而積累的問題；同時，該方案大大減少了圖像數(shù)據(jù)和定位計算的處理時間，這對于實現(xiàn)無人機的實時定位非常重要。

1 理論分析

1.1 雙目立體視覺測距模型

在計算機視覺研究中常用的坐標(biāo)系有世界坐標(biāo)系、相機坐標(biāo)系、圖像像素坐標(biāo)系和圖像物理坐標(biāo)系。根據(jù)四個坐標(biāo)系之間的轉(zhuǎn)換關(guān)系和成像原理，可得到相機成像模型。

(1)

式中：Mi——攝像頭的內(nèi)部參數(shù)矩陣；

Me——攝像頭的外部參數(shù)矩陣。

本文采用平行雙目立體視覺系統(tǒng)，其簡易模型如圖1所示。該模型中兩個攝像頭的光軸彼此平行，成像平面共面，其優(yōu)點是能最大程度保證測量數(shù)據(jù)的穩(wěn)定[11]。

圖1 平行式雙目視覺測距模型

圖1中，兩個攝像頭之間的距離定義為基線距B，焦距均為f，相機坐標(biāo)系原點為左攝像頭的光心點Ol(0，0，0)。假定左右攝像頭在同一時刻對相機坐標(biāo)系下的空間點P(XC，YC，ZC)進(jìn)行拍照，分別在左右視圖上得到投影點的圖像像素坐標(biāo)為pl=(ul，vl)和pr=(ur，vr)，經(jīng)過立體校正后兩個光心原點將處于同一水平高度，即vl=vr。根據(jù)攝像機的投影幾何關(guān)系可以得到式(2)。

(2)

定義視差d=u1-ur，聯(lián)立式(2)中兩項并代入vl=vr，可以得到式(3)。

(3)

在已知攝像頭內(nèi)參矩陣和匹配特征點視差的前提下，空間點在相機坐標(biāo)下的三維坐標(biāo)可由式(3)唯一確定。于是，雙目同步攝像頭模組基線的中點M(即質(zhì)心)距離空間點P的相對距離為

(4)

1.2 無人機室內(nèi)自主定位算法

本文基于RSS定位算法，通過設(shè)計地標(biāo)進(jìn)行輔助定位來求解預(yù)設(shè)世界坐標(biāo)系中無人機的三維坐標(biāo)。

(1)地標(biāo)設(shè)計

根據(jù)地標(biāo)設(shè)計的一般要求[12-13]，以及本文利用地標(biāo)實現(xiàn)輔助定位的需求，設(shè)計地標(biāo)如圖2所示。此地標(biāo)的設(shè)計形狀非常簡單，具有易于視覺算法識別的主體部分(除字母區(qū)域之外的部分，實物設(shè)計為紅色)，且地標(biāo)之間可利用特征明顯的字母做區(qū)分(實物設(shè)計為黃色)，適宜于在室內(nèi)環(huán)境下進(jìn)行識別定位。

圖2 地標(biāo)設(shè)計(字母為A、B、C、D的情況)

此地標(biāo)的設(shè)計能夠使其與周圍的地面環(huán)境區(qū)別開來，易于圖像處理算法對其識別，能在一定程度上克服室內(nèi)光線不足的問題；矩形當(dāng)中的字母可作為SURF算法的特征點提取部分，能夠很好地簡化特征點提取算法，提高算法的運算速度和特征點匹配的準(zhǔn)確性。

(2)無人機定位算法

將平行雙目攝像頭安裝在無人機的底部，左右攝像頭光軸互相平行且關(guān)于無人機機軸對稱，并使其相對無人機的位置固定。當(dāng)無人機在室內(nèi)飛行時，應(yīng)保證三個地標(biāo)都能被雙目攝像頭拍到(地標(biāo)呈三角形狀布置)，如圖3所示。

圖3中D1、D2、D3分別為點M到三個地標(biāo)形心點的距離，由式(4)求解得到。為研究方便，假定：1預(yù)設(shè)世界坐標(biāo)系中三個地標(biāo)的形心坐標(biāo)分別為P1(XW1,YW1,ZW1)、P2(XW2,YW2,ZW2)和P3(XW3,YW3,ZW3)，為已知量；2當(dāng)無人在室內(nèi)上空飛行時，可以抽象為其質(zhì)心CM的運動，其機載雙目同步攝像頭基線中點M的三維位置就可以近似為無人機當(dāng)前的位置，記為M(XW,YW,ZW)，為未知量。

圖3 無人機定位幾何關(guān)系

根據(jù)無人機定位的幾何關(guān)系，有：

(5)

式(5)中，由于地標(biāo)處于同一平行地面，因此令ZW1=ZW2=ZW3=0，將式(5)整理成未知量為XW、YW矩陣方程：

AX=B

(6)

式(6)為二元非齊次線性方程組，根據(jù)最小二乘法原理，可求得未知量橫縱坐標(biāo)X為

(7)

將式(7)代入式(5)中第一個等式可以求出縱坐標(biāo)為

(8)

至此，可以得到基于雙目視覺測距算法的無人機位置坐標(biāo)估計值M(XW,YW,ZW)。

1.3 雙目攝像頭模組的標(biāo)定

通過考慮攝像頭畸變的徑向分量和切向分量，基于經(jīng)典的張氏標(biāo)定法[14]對雙目攝像頭進(jìn)行標(biāo)定。

本文采用的雙目同步攝像頭模組型號為樹莓派HBV-1714-2 S1.0。利用其提供的USB接口，選擇模組分辨率為1 280×480，基于MATLAB GUI開發(fā)了圖像采集系統(tǒng)。設(shè)計8×11的黑白棋盤格標(biāo)定板，每個方格的邊長均為50 mm。

基于圖像采集系統(tǒng)和棋盤格標(biāo)定板，利用MATLAB立體相機標(biāo)定APP對采集到的16對棋盤格圖像進(jìn)行標(biāo)定。通過修正標(biāo)定參數(shù)的重投影誤差，最終使其全部小于0.14，得到雙目同步攝像頭模組的標(biāo)定參數(shù)見表1至表3所示。由于模組兩個攝像頭焦距不變，相對位姿也已固定，因此這些標(biāo)定參數(shù)可作為已知量直接用于后面的計算。

表1 左攝像頭主要參數(shù)標(biāo)定結(jié)果

2 地標(biāo)識別與定位

2.1 地標(biāo)的識別與分割

在室內(nèi)環(huán)境下，本文設(shè)計的地標(biāo)(實物需要彩色打印)對于圖像的分割是一個很有用的線索，可以通過對所拍攝圖像進(jìn)行顏色的K-means聚類分析實現(xiàn)對地標(biāo)的識別，這樣可以有效地避免光線不足對圖像分割造成的干擾。

表2 右攝像頭主要參數(shù)標(biāo)定結(jié)果

表3 雙目攝像頭模組的相對位姿標(biāo)定結(jié)果

圖4 地標(biāo)識別算法流程

地標(biāo)識別過程的程序步驟如圖4所示。采用K-means聚類分析圖像分割算法，將彩色圖像(包含地標(biāo))每個像素的RGB值作為樣本數(shù)據(jù)集X進(jìn)行劃分，再根據(jù)對含地標(biāo)圖像的預(yù)先認(rèn)知，指定圖樣的點集(聚類數(shù)目)數(shù)量K?；贙均值的顏色分類將會把樣本數(shù)X轉(zhuǎn)換成xy色度坐標(biāo)，然后使用K-means算法找出xy色度平面上的聚類點集，Peter Corke對此方法做了研究[15]。根據(jù)對圖像的預(yù)先認(rèn)知，地標(biāo)區(qū)域的像素數(shù)目應(yīng)遠(yuǎn)小于非地標(biāo)區(qū)域的像素數(shù)目，因此可通過比較兩個點集中像素的數(shù)目之和的大小來進(jìn)行判別，較小者的即為地標(biāo)區(qū)域所對應(yīng)的點集。在實際拍攝的圖像中，字母所占區(qū)域很小，因此只考慮主體區(qū)域和字母區(qū)域進(jìn)行顏色分類，指定圖像的聚類中心數(shù)量為K=2對地標(biāo)進(jìn)行識別。

基于上述算法，對一幅480×640×3實例圖像進(jìn)行地標(biāo)識別試驗，圖中地標(biāo)的尺寸為(9×13)cm，樣本數(shù)據(jù)集X=480×640。算法可將地標(biāo)區(qū)域成功地從圖像中分離，將其顯示為地標(biāo)二值圖像，可看到圖像中存在毛刺、小點和小裂縫等噪聲，如圖5所示。

圖5 地標(biāo)識別結(jié)果及缺陷標(biāo)記

圖5所示的地標(biāo)二值圖像具有明顯的矩形特征，基于形態(tài)學(xué)的形狀濾波器可利用形態(tài)學(xué)算子有效地濾除噪聲，保留圖中原有信息[16]。本文利用二值形態(tài)分析方法中的基本運算，即腐蝕、膨脹以及由它們組合得到的開閉運算實現(xiàn)圖像的去噪。根據(jù)圖像開運算和閉運算特點，對圖5先執(zhí)行閉操作，再執(zhí)行開操作，得到結(jié)果如圖6所示。對比發(fā)現(xiàn)，圖像中的噪聲點已經(jīng)消失，得到了完整的矩形地標(biāo)。

本文采用基于統(tǒng)計二值圖像不規(guī)則連通區(qū)域的八連通方法，通過檢查圖像中每個像素與相鄰像素的連通性，使用MATLAB分別求取每個地標(biāo)的形心坐標(biāo)和最小外接矩形，結(jié)果如圖7所示。

圖6 地標(biāo)二值圖像去噪

圖7 地標(biāo)形心和最小外接矩形計算結(jié)果

可以看出地標(biāo)的形心和最小外接矩形能被很好地識別出來，并得到了三個地標(biāo)在圖像像素坐標(biāo)系下的形心坐標(biāo)和最小外接矩形坐標(biāo)范圍。

2.2 地標(biāo)形心定位計算與誤差分析

根據(jù)模組參數(shù)標(biāo)定結(jié)果，模組基線的實際距離為B=11.999 cm，將其和表1、表2的相應(yīng)數(shù)值和地標(biāo)形心坐標(biāo)代入式(3)計算得到相機坐標(biāo)系下的坐標(biāo)估計值(XC,YC,ZC)，與真實值進(jìn)行比較如表4所示。

表4 地標(biāo)形心坐標(biāo)真實值與估計值(單位：cm)

通過計算絕對誤差，發(fā)現(xiàn)相機坐標(biāo)系下地標(biāo)形心坐標(biāo)XC和YC的實際值與估計值之間的最大誤差為3.26 cm，其余誤差都在3 cm以內(nèi)，基本上能夠滿足定位精度需求。但是坐標(biāo)ZC值的誤差過大，都在6 cm以上，屬于不可接受的范圍，因此后面將利用SURF算法進(jìn)行更精確垂直距離的測量。

2.3 基于SURF算法的垂直距離測量

SURF算法又稱為加速魯棒性特征，許多研究的實驗證明其運算速度要比SIFT算法快很多，綜合性能優(yōu)異，被廣泛應(yīng)用于計算機視覺、導(dǎo)彈制導(dǎo)時的目標(biāo)識別和機器人導(dǎo)航等領(lǐng)域[17-18]。

(1)基于立體校正和地標(biāo)區(qū)域匹配約束的SURF立體匹配

本文針對其匹配誤差和計算速度問題，先基于Bouquet校正原理對圖像進(jìn)行立體校正預(yù)處理，并通過地標(biāo)最小外接矩形確定的地標(biāo)范圍添加特征點匹配約束。針對某一圖像對的立體匹配步驟如下。

①讀取RGB格式的圖像對，分別進(jìn)行圖像中地標(biāo)的識別、分割和最小外接矩形的計算；

②根據(jù)地標(biāo)最小外接矩形數(shù)據(jù)，確定地標(biāo)區(qū)域圖像像素坐標(biāo)值的范圍，將其作為地標(biāo)區(qū)域的判斷條件；

③讀取原始圖像，進(jìn)行立體圖像校正、SURF特征點檢索和描述算子生成；

④計算所有匹配點的最小距離，按匹配距離誤差從小到大生成特征點匹配向量；

⑤指定需要的特征點個數(shù)，按匹配距離誤差從小到大讀取。每讀取一對特征點，就判斷其像素坐標(biāo)值是否在第二步所設(shè)定的地標(biāo)區(qū)域內(nèi)，若是，則作為最佳匹配輸出；否則，將其從最佳匹配中剔除。

基于上述算法步驟，指定目標(biāo)特征點匹配對數(shù)為30，對2.1節(jié)中的圖像對進(jìn)行立體匹配計算，結(jié)果如圖8所示?？梢钥闯?，所有特征點匹配對全部落在地標(biāo)區(qū)域內(nèi)，無誤匹配現(xiàn)象。

將對SURF立體匹配算法改進(jìn)前后的相關(guān)數(shù)據(jù)比較如表5所示(準(zhǔn)確率：落在地標(biāo)區(qū)域內(nèi)匹配對數(shù)和總匹配對數(shù)之間的比值)。對比發(fā)現(xiàn)，基于立體校正和地標(biāo)區(qū)域匹配約束的SURF立體匹配已經(jīng)很好地達(dá)到了目標(biāo)。

圖8 基于改進(jìn)SURF算法的圖像立體匹配

參數(shù)未校正校正校正+區(qū)域約束誤匹配對數(shù)310準(zhǔn)確率83.3%90%100%

(2)基于多幅圖像的垂直距離計算及數(shù)據(jù)擬合

為了確定雙目攝像頭模組能夠準(zhǔn)確測量的距離范圍，以及獲得更精確的垂直距離，采集不同距離的多幅實驗圖像進(jìn)行視差和垂直距離計算，并擬合真實距離與圖像視差數(shù)據(jù)。通過比較擬合多項式模型計算結(jié)果和式(3)第三項計算結(jié)果，選出誤差較小的作為本文的垂直距離計算模型。

本次試驗共利用三個地標(biāo)(A、B、D)，將其平行于地面布置。假定無人機在室內(nèi)的飛行高度在(1～3)m之間，結(jié)合模組的有效測量距離，設(shè)計三個地標(biāo)的尺寸均為(18×13)cm，它們的相對距離如圖9所示，三個地標(biāo)的形心構(gòu)成一個等腰三角形。

圖9 預(yù)設(shè)世界坐標(biāo)系及地標(biāo)布置

根據(jù)模組的有效測量距離，從104 cm處開始采集圖像，采集的高度范圍為104～204 cm(增幅為10 cm，利用精準(zhǔn)測距儀測得)。利用基于立體校正和地標(biāo)區(qū)域匹配約束的SURF立體匹配方法進(jìn)行匹配計算，指定最佳匹配對數(shù)為30，得到只屬于地標(biāo)區(qū)域的最佳匹配特征點并計算其視差，按照匹配距離誤差從小到大的原則取前4組視差數(shù)據(jù)，將其平均值作為該地標(biāo)形心的視差數(shù)據(jù)。對所有11組圖像進(jìn)行視差計算，結(jié)果代入式(8)得到垂直距離ZC估計值如式(9)所示。

(9)

計算真實值和估計值的絕對誤差，如圖10所示?？梢钥闯?，大部分誤差都在3 cm以內(nèi)，隨著測量距離的增加，絕對誤差有增大趨勢?？紤]到超過200 cm以后，誤差會進(jìn)一步增大，因此去掉200 cm以后部分的視差數(shù)據(jù)，確定本文采用模組在當(dāng)前地標(biāo)布置情況下的有效測量距離為1～2 m。

圖10 垂直距離估計值絕對誤差隨距離的變化

根據(jù)式(9)所示的測距模型，垂直距離ZC和視差d之間為反比例關(guān)系，Bα為標(biāo)定常數(shù)。利用經(jīng)驗公式，可引入誤差距離e改進(jìn)垂直測距模型如式(10)所示。

(10)

將真實垂直距離ZC作為因變量，視差的倒數(shù)1/d作為自變量，通過多項式擬合來求解未知常數(shù)。用MATLAB分別對三個地標(biāo)的數(shù)據(jù)進(jìn)行多項式擬合，通過比較均方根誤差(RMSE)，選擇一次多項式擬合，分別得到地標(biāo)A、B和D垂直距離擬合表達(dá)式如式(11)～(13)所示。

(11)

(12)

(13)

基于這些擬合函數(shù)和視差數(shù)據(jù)計算得到擬合函數(shù)模型距離估計值ZCF，計算其與真實值之間的平均相對誤差A(yù)RE_CF，與未引入誤差限e得到的距離估計值ZC的平均相對誤差A(yù)RE_C進(jìn)行比較，如表6所示。

表6 兩種距離估計方式相對誤差均值比較

可明顯看出，通過擬合一次多項式模型求解得到的距離估計值的平均相對誤差普遍較低，故本文將利用擬合多項式模型計算垂直距離。至此，通過圖像處理已經(jīng)可以得到相機坐標(biāo)下地標(biāo)的三維坐標(biāo)(XC,YC,ZC)。利用式(4)至式(8)就可求解得到無人機的三維坐標(biāo)(XW,YW,ZW)。

3 室內(nèi)定位仿真實驗

本文采用的是平行雙目攝像頭模組立體視覺定位系統(tǒng)，根據(jù)對無人機定位算法的假定條件，可將雙目同步攝像頭基線中點的三維位置近似為無人機當(dāng)前的位置。因此，可用雙目攝像頭模組模擬無人機飛行，并拍攝一系列圖像數(shù)據(jù)。

實驗仿真平臺的硬件包括樹莓派HBV-1714-2 S1.0雙目攝像頭模組(模擬無人機)、USB圖像傳輸線，以及圖像數(shù)據(jù)處理平臺戰(zhàn)神K650D-i7 D3筆記本電腦(配置為CPU核處理器i7-4710MQ，內(nèi)存8G)。根據(jù)前面的圖像處理和無人機定位算法得出無人機整體定位流程框圖如圖11所示?；跓o人機定位流程結(jié)構(gòu)框圖和MATLAB GUI設(shè)計應(yīng)用程序如圖12所示。

圖11 基于雙目視覺的無人機室內(nèi)定位流程框圖

圖12 無人機定位仿真系統(tǒng)軟件界面設(shè)計

采用和圖9一樣的世界坐標(biāo)系和地標(biāo)布置，可得到地標(biāo)在預(yù)設(shè)世界坐標(biāo)系中的坐標(biāo)。讓雙目攝像頭模組模擬無人機在室內(nèi)地面上空(1～2)m的范圍內(nèi)沿一條不規(guī)則路徑飛行，拍攝得到23組圖像對序列，并利用精準(zhǔn)測距儀測量得到了真實軌跡點的坐標(biāo)。利用定位仿真系統(tǒng)，計算得到無人機室內(nèi)仿真軌跡如圖13所示。

三個坐標(biāo)值仿真結(jié)果的絕對誤差如圖14所示，可直觀看出誤差在可接受范圍之內(nèi)。

均方根誤差(RMSE)對一組數(shù)據(jù)中的最大和最小誤差反映非常敏感，可以很好地反映出室內(nèi)定位的精度。本文將其作為定位誤差大小地評價標(biāo)準(zhǔn)，如式(14)所示。

圖13 真實軌跡和仿真軌跡比較(單位：cm)

圖14 仿真結(jié)果的絕對誤差

(14)

式中，n為測量數(shù)據(jù)個數(shù)，Xobs,i為真實值，Xmodel,i為模型測量值。利用此公式分別計算三個坐標(biāo)值的均方根誤差，結(jié)果如表7所示。

表7 定位均方根誤差 cm

可以看出，三個坐標(biāo)值的定位精度都在3 cm以內(nèi)。此定位精度基本滿足預(yù)定要求，說明在室內(nèi)飛行高度為1～2 m且拍攝圖像中包含地標(biāo)的范圍內(nèi)，本文的定位方案能夠?qū)崿F(xiàn)比較精確的無人機室內(nèi)定位。

4 結(jié)論

本文提出了一種基于K均值圖像分割、SURF立體匹配和RSS定位算法的無人機室內(nèi)定位流程。仿真實驗表明，該流程在一定的空間范圍內(nèi)能有效實現(xiàn)無人機的定位，且定位精度較高。通過設(shè)計簡單的輔助定位地標(biāo)，充分利用了K均值和SURF算法的優(yōu)勢，對圖像的地標(biāo)區(qū)域進(jìn)行處理，最終得到了目標(biāo)特征點，可以較快的速度計算出無人機位置。此定位方案具有精度高、成本低、計算速度快和不受電磁干擾等特點，但是定位范圍十分有限，可通過布置三個以上地標(biāo)實現(xiàn)更廣范圍的室內(nèi)定位，存在一定的局限性，還需要進(jìn)一步改進(jìn)。