張大偉，王 佳，孟森森，鄧計(jì)才

(鄭州大學(xué) 信息工程學(xué)院,河南 鄭州 450001)

0 引 言

移動(dòng)機(jī)器人導(dǎo)航是機(jī)器人領(lǐng)域的一個(gè)研究熱點(diǎn)[1]。視覺(jué)傳感器作為最接近人類(lèi)的環(huán)境感知工具，具有其他傳感器無(wú)法比擬的優(yōu)勢(shì)。根據(jù)視覺(jué)傳感器的數(shù)目，視覺(jué)導(dǎo)航分為3種：?jiǎn)文恳曈X(jué)、雙目視覺(jué)、多目視覺(jué)[2]。單目視覺(jué)具有結(jié)構(gòu)簡(jiǎn)單、適用范圍廣等優(yōu)點(diǎn)，單目視覺(jué)導(dǎo)航分為3類(lèi)：基于單視圖[3]、雙視圖[4]、多視圖[5]。雙視圖根據(jù)兩幀圖像所包含的目標(biāo)物體的視差信息完成定位，兩幀圖像中對(duì)應(yīng)點(diǎn)間存在一定的約束關(guān)系，根據(jù)這種約束關(guān)系可實(shí)現(xiàn)基礎(chǔ)矩陣求解，三維重建，運(yùn)動(dòng)估計(jì)等，并且計(jì)算量較小。因此，在成本控制、性?xún)r(jià)比、實(shí)時(shí)性要求高的移動(dòng)機(jī)器人領(lǐng)域，單目視覺(jué)中的雙視圖導(dǎo)航方式是一個(gè)較佳的選擇。

本文以雙視圖定位為基礎(chǔ)，提出了基于單目視頻路徑的導(dǎo)航方法。

1 導(dǎo)航原理

原理流程如圖1所示。

圖1 導(dǎo)航原理流程

1)人工驅(qū)動(dòng)機(jī)器人沿參考路徑移動(dòng)，并通過(guò)自身的前置攝像機(jī)沿途拍攝視頻序列，從中提取關(guān)鍵圖像，并用一組有序的關(guān)鍵圖像代表參考路徑的視頻序列；2)當(dāng)機(jī)器人在參考路徑附近移動(dòng)時(shí)，可拍攝當(dāng)前圖像，利用攝像機(jī)中心間的最短歐氏距離，從關(guān)鍵圖像序列中找出與當(dāng)前圖像最近似的圖像，并提取其特征點(diǎn)，進(jìn)行圖像匹配，求出基礎(chǔ)矩陣并優(yōu)化，得到位姿參數(shù)，完成機(jī)器人定位；3)結(jié)合參考路徑的視頻序列，找出當(dāng)前圖像和關(guān)鍵圖像共同包含的特征點(diǎn)，以其形心橫坐標(biāo)為參照，使機(jī)器人沿參考路徑行走，實(shí)現(xiàn)自主導(dǎo)航。

1.1 定 位

1.1.1 關(guān)鍵圖像獲取

視頻路徑拍攝后，需要從視頻序列中提取一組有序關(guān)鍵圖像，目的是為機(jī)器人的定位及后續(xù)導(dǎo)航提供引導(dǎo)[6]。將視頻序列的第一幀圖像，始終作為第一幀關(guān)鍵圖像，記作I1，然后，提取下一幀關(guān)鍵圖像I2。關(guān)鍵圖像提取原則如下：

1)關(guān)鍵圖像In和In+1間存在盡可能多的視頻圖像；

2)關(guān)鍵圖像In和In+1間至少有M個(gè)共同的特征點(diǎn)；

3)關(guān)鍵圖像In-1和In+1間至少有N個(gè)共同的特征點(diǎn)，以保證圖像的連續(xù)性。

1.1.2 最近似關(guān)鍵圖像

(1)

從關(guān)鍵圖像序列中，選擇歐氏距離最短的圖像，作為與當(dāng)前圖像I最近似的關(guān)鍵圖像，并記作Ik，最近似關(guān)鍵圖像的選擇是一次粗匹配，為圖像匹配奠定基礎(chǔ)。

1.1.3 特征點(diǎn)提取

采用Harris 角點(diǎn)檢測(cè)算法，尋找圖像的特征點(diǎn)。算法思路：在圖像中設(shè)置一個(gè)局部測(cè)試窗口，當(dāng)該窗口沿任意方向作微小移動(dòng)時(shí)，窗口的灰度值將發(fā)生變化。引入微分運(yùn)算和自相關(guān)矩陣來(lái)檢測(cè)灰度的變化情況，當(dāng)該窗口的灰度變化值超過(guò)設(shè)定的閾值時(shí)，即將窗口的中心像素點(diǎn)作為角點(diǎn)。角點(diǎn)響應(yīng)函數(shù)CFR定義為

CFR=det(M)-k(traceM)2

(2)

式中

(3)

矩陣M為像素點(diǎn)的自相關(guān)矩陣；Wu,v為高斯平滑因子；X，Y分別為像素點(diǎn)在水平、垂直方向上的一階導(dǎo)數(shù)；k為經(jīng)驗(yàn)值常數(shù)，通常k=0.04。

1.1.4 圖像匹配

(4)

1.1.5 對(duì)極幾何與基礎(chǔ)矩陣

當(dāng)前圖像I與最近似關(guān)鍵圖像Ik為針對(duì)同一目標(biāo)、不同角度拍攝得的2幀圖像，其對(duì)極幾何關(guān)系如圖2所示。D為空間任一點(diǎn)，C和Ck分別為兩攝像機(jī)的中心，C和Ck連接的直線(xiàn)稱(chēng)為基線(xiàn)，與圖像I和Ik的交點(diǎn)分別為e，ek；d和dk為空間中點(diǎn)D分別在2幀圖像上的投影，為一對(duì)匹配點(diǎn)；平面DCCk與圖像I的交線(xiàn)為h，與圖像Ik的交線(xiàn)為hk，d和dk分別在各自的交線(xiàn)上，稱(chēng)h為匹配點(diǎn)dk對(duì)應(yīng)的極線(xiàn)，同理，hk為匹配點(diǎn)d對(duì)應(yīng)的極線(xiàn)。

圖2 對(duì)極幾何

基礎(chǔ)矩陣是對(duì)極幾何的代數(shù)表示，可以由兩幀圖像之間的匹配點(diǎn)對(duì)進(jìn)行求解。假設(shè)：d?dk是當(dāng)前圖像I和最近似關(guān)鍵圖像Ik之間的一對(duì)匹配點(diǎn)，坐標(biāo)分別為d=(u,v,1)T,dk=(uk,vk,1)T，d和dk之間約束關(guān)系為

(5)

將式(5)整理可得線(xiàn)性方程組

Af=0

(6)

式中f為基礎(chǔ)矩陣F的展開(kāi)變形

f=(f11f12f13f21f22f23f31f32f33)T

(7)

(8)

求解基礎(chǔ)矩陣F需要有9個(gè)參數(shù)，歸一化，變?yōu)?個(gè)。因此，需要8對(duì)匹配點(diǎn)代入線(xiàn)性方程組(6)，即可以完全確定基礎(chǔ)矩陣。

1.1.6 基礎(chǔ)矩陣優(yōu)化

算法思路：兩幀圖像的一對(duì)匹配點(diǎn)，其中一個(gè)特征點(diǎn)對(duì)應(yīng)的另一幀圖像上的匹配點(diǎn)，必然在該特征點(diǎn)對(duì)應(yīng)的極線(xiàn)上，利用這一特點(diǎn)，將匹配點(diǎn)到該極線(xiàn)的垂直距離作為選擇最優(yōu)匹配點(diǎn)對(duì)的標(biāo)準(zhǔn)。

具體做法：第一次迭代，任意選取8對(duì)匹配點(diǎn)，求出基礎(chǔ)矩陣F1，然后分別計(jì)算8個(gè)特征點(diǎn)到極線(xiàn)F1d的垂直距離，剔除其中距離最大的特征點(diǎn)。第二次迭代，從眾多剩余的特征點(diǎn)中任意選取一個(gè)特征點(diǎn)，求出基礎(chǔ)矩陣F2，再分別計(jì)算這8個(gè)特征點(diǎn)到極線(xiàn)F2d的垂直距離，再次剔除其中距離最大的特征點(diǎn)。重復(fù)操作，直到獲得最終8個(gè)特征點(diǎn)，由于該8個(gè)特征點(diǎn)在所有特征點(diǎn)中匹配精度最優(yōu)，所以利用該8對(duì)特征點(diǎn)所求出的基礎(chǔ)矩陣的準(zhǔn)確性最好。

1.1.7 目標(biāo)定位

空間中任一點(diǎn)D在當(dāng)前圖像I和最近似關(guān)鍵圖像Ik中的投影點(diǎn)分別為d和dk，點(diǎn)d和dk是一對(duì)匹配點(diǎn)。設(shè)目標(biāo)點(diǎn)D在圖像I和Ik中的攝像機(jī)坐標(biāo)分別為X=(x,y,z),Xk=(xk,yk,zk)，圖像坐標(biāo)分別為(u,v),(uk,vk)，像素。攝像機(jī)由最近似關(guān)鍵圖像Ik到當(dāng)前圖像I，相當(dāng)于做了一次運(yùn)動(dòng)。假設(shè)攝像機(jī)運(yùn)動(dòng)的旋轉(zhuǎn)矩陣為R、平移向量為t，在攝像機(jī)坐標(biāo)系下，兩個(gè)坐標(biāo)的關(guān)系可表示為

X=RXk+t

(9)

假設(shè)t=(tx,ty,tz)T，G為攝像機(jī)內(nèi)參數(shù)矩陣，根據(jù)圖像坐標(biāo)系和攝像機(jī)坐標(biāo)系間的轉(zhuǎn)換關(guān)系有

(10)

式中αx,αy分別為u軸和v軸上的歸一化焦距；u0，v0為圖像的中心。

式(10)整理可得

(11)

式中

(12)

式中G,R,t為可求量;zk為可測(cè)量;z為目標(biāo)點(diǎn)在當(dāng)前攝像機(jī)坐標(biāo)系下的深度值，可由最小二乘法計(jì)算得到，圖像坐標(biāo)(u,v)和(uk,vk)可以通過(guò)特征點(diǎn)的提取和匹配獲得。將z代入式(10)，可以求得x和y

(13)

點(diǎn)X=(x,y,z)即為攝像機(jī)坐標(biāo)系下目標(biāo)點(diǎn)的當(dāng)前三維坐標(biāo)，由此可以實(shí)現(xiàn)目標(biāo)定位。

1.2 路徑規(guī)劃

具體做法為：首先，當(dāng)前圖像記為I，與當(dāng)前圖像最近似的關(guān)鍵圖像記為Ik，記圖像Ik的下一張關(guān)鍵圖像為Ik+1∈{I1，I2，I3,…,In}。使用Harris算法分別提取圖像I和Ik+1的特征點(diǎn)，求出2幀圖像共同包含的特征點(diǎn)集的形心，用形心的橫坐標(biāo)X作為定位的參照[7,8]。當(dāng)機(jī)器人沿參考路徑移動(dòng)，圖像I與Ik+1共同包含的特征點(diǎn)集的形心橫坐標(biāo)X發(fā)生變化，由圖像I的形心橫坐標(biāo)X1，不斷趨向于圖像Ik+1的形心橫坐標(biāo)Xk+1，當(dāng)?shù)竭_(dá)Xk+1時(shí)，即機(jī)器人到達(dá)關(guān)鍵圖像Ik+1的位置。此時(shí)，下一張關(guān)鍵圖像Ik+2變?yōu)槟繕?biāo)位置，以此類(lèi)推，直到到達(dá)關(guān)鍵圖像In位置，完成自主導(dǎo)航，如圖3(a)所示。

圖3 視覺(jué)導(dǎo)航與移動(dòng)機(jī)器人平臺(tái)

2 實(shí) 驗(yàn)

實(shí)驗(yàn)驗(yàn)證在自主開(kāi)發(fā)移動(dòng)機(jī)器人平臺(tái)上進(jìn)行如圖3(b)。該機(jī)器人采用基于4個(gè)麥克納姆輪的全向移動(dòng)機(jī)構(gòu)，內(nèi)部配備有一個(gè)微型電腦(英特爾Intel NUC 6i5SYH，處理器i5，內(nèi)存8 GB)，頭部安裝了一個(gè)用于單目視覺(jué)定位導(dǎo)航的攝像機(jī)(微軟Kinect V2內(nèi)部彩色攝像機(jī))。所開(kāi)發(fā)的定位導(dǎo)航算法基于Visual Studio 2015 和 OpenCV 3.0開(kāi)發(fā)。

實(shí)驗(yàn)步驟如下：1)人工驅(qū)動(dòng)機(jī)器人沿參考路徑行走，用攝像機(jī)記錄視頻路徑;2)提取圖像特征點(diǎn)，從視頻序列中提取一組有序關(guān)鍵圖像{…,Ik,Ik+1,Ik+2,…}作為代表，算法如前所述，實(shí)驗(yàn)中,設(shè)定M=400,N=300；3)拍攝當(dāng)前圖像I，根據(jù)圖像之間歐氏距離最短，從關(guān)鍵圖像序列中選取最近似關(guān)鍵圖像Ik，將圖像I和Ik進(jìn)行圖像匹配，求出基礎(chǔ)矩陣并優(yōu)化，實(shí)現(xiàn)目標(biāo)定位；4)根據(jù)已有路線(xiàn)，通過(guò)單目攝像機(jī)動(dòng)態(tài)攝取路面圖像，經(jīng)計(jì)算機(jī)處理識(shí)別出關(guān)鍵圖像，并判斷關(guān)鍵圖像特征點(diǎn)集形心的橫坐標(biāo)位置，使機(jī)器人的實(shí)際行駛路線(xiàn)與參考路線(xiàn)的偏差保持在允許的范圍內(nèi)，以此實(shí)現(xiàn)自主視覺(jué)導(dǎo)航。如圖3(a)所示，機(jī)器人根據(jù)當(dāng)前圖像I和最近似的關(guān)鍵圖像Ik，實(shí)現(xiàn)定位，并完成從Ik到Ik+1，Ik+2的導(dǎo)航。

機(jī)器人運(yùn)動(dòng)軌跡與參考路徑如圖4所示，其置信區(qū)間如圖5所示。

圖5 置信區(qū)間

3 結(jié) 論

實(shí)驗(yàn)結(jié)果表明:機(jī)器人實(shí)際軌跡與參考路徑的誤差始終控制在一定允許范圍內(nèi)(小于2 m)。在實(shí)驗(yàn)過(guò)程中，影響精度的原因主要有3個(gè)：1)鏡頭本身的畸變；2)標(biāo)定誤差;3)匹配誤差；隨著工藝技術(shù)的發(fā)展和各種算法的不斷改進(jìn)，誤差的影響會(huì)越來(lái)越小。

[1] 譚 民,王 碩.機(jī)器人技術(shù)研究進(jìn)展[J].自動(dòng)化學(xué)報(bào),2013,39(7): 963-972.

[2] 敬 澤,薛方正,李祖樞.基于單目視覺(jué)的空間目標(biāo)位置測(cè)量[J].傳感器與微系統(tǒng),2011,30(3):125-131.

[3] 余厚云,張為公.基于單目視覺(jué)傳感器的車(chē)距測(cè)量與誤差分析[J].傳感器與微系統(tǒng),2012,31(9):10-13.

[4] 于天慧,孫金瑋.面向復(fù)雜場(chǎng)景的未標(biāo)定兩視圖三維重構(gòu)方法[J].計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào),2016,28(8):1232-1241.

[5] 姜翰青,趙長(zhǎng)飛,章國(guó)鋒,等.基于多視圖深度采樣的自然場(chǎng)景三維重建[J].計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào),2015,27(10):1805-1815.

[6] Cherubini A,Chaumette F.Visual navigation of a mobile robot with laser-based collision avoidance[J].The International Journal of Robotics Research,2013,32(2):189-205.

[7] Charmette B,Royer E,Chausse F.Vision-based robot localization based on the efficient matching of planar features[J].Machine vision and Applications,2016,27(4): 415-436.

[8] Cherubini A,Spindler F,Chaumette F.Autonomous visual navigation and laser-based moving obstacle avoidance[J].IEEE Transactions on Intelligent Transportation Systems,2014,15(5):2101-2110.

鄧計(jì)才(1974-)，男，通訊作者，博士，教授，主要從事競(jìng)技機(jī)器人領(lǐng)域研究工作，E—mail：iejcdeng@zzu.edu.cn。