基于雙目視覺(jué)的無(wú)人機(jī)室內(nèi)定位研究

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

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

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

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

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

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

1 理論分析

1.1 雙目立體視覺(jué)測(cè)距模型

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

(1)

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

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

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

圖1 平行式雙目視覺(jué)測(cè)距模型

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

(2)

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

(3)

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

(4)

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

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

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

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

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

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

(2)無(wú)人機(jī)定位算法

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

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

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

根據(jù)無(wú)人機(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)中第一個(gè)等式可以求出縱坐標(biāo)為

(8)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.3 基于SURF算法的垂直距離測(cè)量

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

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

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

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

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

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

④計(jì)算所有匹配點(diǎn)的最小距離，按匹配距離誤差從小到大生成特征點(diǎn)匹配向量；

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

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

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

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

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

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

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

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

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

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

(9)

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

圖10 垂直距離估計(jì)值絕對(duì)誤差隨距離的變化

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

(10)

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

(11)

(12)

(13)

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

表6 兩種距離估計(jì)方式相對(duì)誤差均值比較

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

3 室內(nèi)定位仿真實(shí)驗(yàn)

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

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

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

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

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

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

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

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

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

(14)

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

表7 定位均方根誤差 cm

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

4 結(jié)論

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