李 港，賴欽偉，蔣 林,1b，雷 斌,1b，梁鏵杰

(1.武漢科技大學(xué)a.機(jī)器人與智能系統(tǒng)研究院；b.冶金裝備及其控制教育部重點(diǎn)實(shí)驗(yàn)室，武漢 430081；2.珠海市一微半導(dǎo)體有限公司，珠海 519000)

0 引言

室內(nèi)機(jī)器人的定位求解計(jì)算與傳感器有著重要的關(guān)聯(lián)，本質(zhì)上是對(duì)傳感器數(shù)據(jù)的處理。目前，室內(nèi)移動(dòng)機(jī)器人常用的定位傳感器可以分為激光傳感器與視覺(jué)傳感器。激光傳感器是通過(guò)測(cè)量與周?chē)矬w的相對(duì)距離實(shí)現(xiàn)自身位置的計(jì)算[1]。激光傳感器定位精度較高，但存在價(jià)格昂貴、體積大等問(wèn)題[2]。

視覺(jué)傳感器相較于激光傳感器數(shù)據(jù)更加直觀豐富且成本較低，因此基于視覺(jué)傳感器的室內(nèi)定位算法更容易進(jìn)行推廣應(yīng)用。TAREEN等[3]在大量數(shù)據(jù)集的基礎(chǔ)上對(duì)比傳統(tǒng)特征提取匹配算法，分析得出其中實(shí)時(shí)性最優(yōu)的算法為ORB[4]算法，因此ORB常用于各種實(shí)時(shí)場(chǎng)景。各種圖像變換下SIFT算法準(zhǔn)確度最高，但SIFT算法復(fù)雜度過(guò)高，實(shí)時(shí)性較差[3]。BAY等[5]基于SIFT提出SURF，在計(jì)算關(guān)鍵點(diǎn)主方向以及關(guān)鍵點(diǎn)周邊像素方向的方法上，使用哈爾小波轉(zhuǎn)換。相比于SIFT算法，實(shí)時(shí)性有所提高，提取匹配效果有所下降。譚光興等[6]在SIFT的基礎(chǔ)上簡(jiǎn)化算法，通過(guò)建立十字形分區(qū)的特征描述子，將描述子的維數(shù)從128降低至64維，但仍無(wú)法到達(dá)實(shí)時(shí)的效果。

隨著深度學(xué)習(xí)技術(shù)不斷的發(fā)展[7]，也出現(xiàn)了一些基于深度學(xué)習(xí)框架的特征檢測(cè)與匹配算法，如LOFTR[8]、D2-NET[9]、SUPERPOINT[10]等。其中DANIEL提出的基于自監(jiān)督框架模型SUPERPOINT是非常經(jīng)典的端到端學(xué)習(xí)特征，模型會(huì)輸出全像素級(jí)別的特征點(diǎn)和特征描述符，結(jié)合特征匹配SUPERGLUE算法[11]，較其他基于深度學(xué)習(xí)框架，有著很高的匹配準(zhǔn)確率與光照尺度不變性。其算法存在主要的問(wèn)題就是復(fù)雜的深度學(xué)習(xí)框架與包含大量參數(shù)的模型對(duì)移動(dòng)端配置要求過(guò)高，且在移動(dòng)端過(guò)于耗時(shí)。

結(jié)合以上分析，考慮到單目視覺(jué)傳感器計(jì)算量小，成本低且適合在嵌入式系統(tǒng)上實(shí)現(xiàn)，因此在機(jī)器人實(shí)驗(yàn)平臺(tái)上搭載單目廣角視覺(jué)傳感器，采用頂視室內(nèi)天花板的單目視覺(jué)定位方案。針對(duì)天花板特征穩(wěn)定且不存在大的尺度變換的特點(diǎn)進(jìn)一步改進(jìn)SIFT，壓縮尺度空間、降低描述子維度。描述子的初匹配采用基于權(quán)重的曼哈頓距離結(jié)合FLANN算法，誤匹配篩除采用基于漸近樣本一致性算法加速迭代過(guò)程，并利用IMU數(shù)據(jù)進(jìn)一步約束匹配。在改進(jìn)SIFT算法的基礎(chǔ)上構(gòu)建實(shí)時(shí)的前端慣性視覺(jué)里程計(jì)，基于權(quán)重融合IMU信息，進(jìn)一步提高移動(dòng)機(jī)器人室內(nèi)視覺(jué)定位精度?；丨h(huán)檢測(cè)利用慣導(dǎo)位姿為初值局部搜索SIFT特征點(diǎn)實(shí)現(xiàn)重定位。

1 天花板視覺(jué)

1.1 機(jī)器視覺(jué)模型

相比于朝前的相機(jī)，天花板特征較為穩(wěn)定，不易受動(dòng)態(tài)環(huán)境的干擾。結(jié)合機(jī)器人的運(yùn)動(dòng)特性分析，機(jī)器運(yùn)動(dòng)主要為沿Y軸的移動(dòng)與繞Z軸的轉(zhuǎn)動(dòng)，均為平面運(yùn)動(dòng)且與天花板平面始終平行，因此用朝上的相機(jī)布局方式能避免大部分的尺度變換[12]。朝上的安裝方式使得相機(jī)拍攝的圖像變換方式主要為仿射變換，提高了定位精度。因此采用機(jī)器人視覺(jué)傳感器頂視室內(nèi)天花板，可視化模型如圖1所示。

(a) 機(jī)器人模型 (b) 機(jī)器人觀測(cè)模型圖1 機(jī)器人模型與機(jī)器人觀測(cè)模型

在單目相機(jī)的具體配置上，選用120°廣角相機(jī)，最大幀率為30 Hz。廣角相機(jī)通過(guò)擴(kuò)大相機(jī)視角的方式使拍攝畫(huà)面盡可能全的包含天花板信息。相機(jī)成像的具體效果如圖2所示。由圖可以看出，即使機(jī)器在房間較邊緣區(qū)域，拍攝圖像也能基本包含整個(gè)天花板。在圖像用于室內(nèi)定位時(shí)，單個(gè)房間僅需數(shù)張圖像就可以包含該房間全部的天花板特征，這個(gè)特點(diǎn)使移動(dòng)機(jī)器人在同一房間的拍攝畫(huà)面始終具有很高的特征點(diǎn)重復(fù)率，簡(jiǎn)化了視覺(jué)定位系統(tǒng)的數(shù)據(jù)量與復(fù)雜度。

(a) 客廳 (b) 走廊圖2 天花板視覺(jué)示意圖

1.2 坐標(biāo)系轉(zhuǎn)換

視覺(jué)定位的本質(zhì)是求解圖像中的事物的位置與實(shí)際坐標(biāo)之間存在著的某種特定的關(guān)系，也就是坐標(biāo)系的轉(zhuǎn)換，具體包括圖像坐標(biāo)系、相機(jī)坐標(biāo)系、機(jī)器坐標(biāo)系與世界坐標(biāo)系。而首先需要求解的就是相機(jī)的內(nèi)參，內(nèi)參包含了圖像坐標(biāo)系到相機(jī)坐標(biāo)系的轉(zhuǎn)換關(guān)系。每個(gè)攝像機(jī)的內(nèi)參都是固定且不同的，需要通過(guò)構(gòu)建攝像機(jī)模型然后經(jīng)過(guò)實(shí)驗(yàn)算法求解出攝像機(jī)的參數(shù)，該過(guò)程為相機(jī)標(biāo)定[13]。

在實(shí)際算法中用第一個(gè)時(shí)刻的機(jī)器坐標(biāo)系為世界坐標(biāo)系?？臻g中一點(diǎn)在世界坐標(biāo)系下的坐標(biāo)為(xw,yw,zw)，結(jié)合機(jī)器位姿轉(zhuǎn)換到此刻的機(jī)器坐標(biāo)系下為(xr,yr,zr)，機(jī)器坐標(biāo)系以機(jī)器的幾何中心為原點(diǎn)，再利用機(jī)器安裝數(shù)據(jù)轉(zhuǎn)換到相機(jī)坐標(biāo)系下為(xc,yc,zc)，經(jīng)針孔相機(jī)模型成像，在圖像像素坐標(biāo)系下為(u,v)。具體轉(zhuǎn)換關(guān)系的表達(dá)式為：

(1)

式中，K為相機(jī)的內(nèi)參矩陣，由相機(jī)標(biāo)定算出；Rcr為相機(jī)坐標(biāo)系與機(jī)器坐標(biāo)系的旋轉(zhuǎn)矩陣；tcr為平移向量；Tcr為Rcr與tcr組成的變換矩陣；Trw為機(jī)器的位姿，會(huì)隨著機(jī)器運(yùn)動(dòng)發(fā)生改變，即機(jī)器人的運(yùn)動(dòng)軌跡，也是定位任務(wù)中要求解的目標(biāo)。

針對(duì)廣角相機(jī)存在較大畸變，需要利用相機(jī)標(biāo)定計(jì)算得到的畸變參數(shù)對(duì)像素點(diǎn)做反畸變。為了不丟失天花板信息，采用魚(yú)眼模型做不丟失像素的反畸變?？梢暬Ч鐖D3所示。

(a) 客廳 (b) 走廊圖3 魚(yú)眼反畸變效果

2 改進(jìn)SIFT算法

2.1 壓縮尺度空間

原SIFT算法在特征檢測(cè)階段會(huì)建立高斯差分金字塔(difference of gaussian，DOG)。首先利用原圖生成六層高斯金字塔，再利用中間層下采樣得到下一組第一層，如此往復(fù)，得到多個(gè)尺度的高斯金字塔。利用高斯金字塔同組相鄰層做差得到高斯差分金字塔DOG，定義為：

D(x,y,σ)=[G(x,y,kσ)-G(x,y,σ)]·I(x,y)

(2)

在DOG中，紋理豐富與邊緣等區(qū)域具有比較大的值。SIFT利用DOG多尺度的特性使提取的特征具有尺度不變性，但也因此加大了算法復(fù)雜度，增加了提取特征點(diǎn)的用時(shí)與后續(xù)匹配的錯(cuò)誤率。

結(jié)合SIFT的多尺度原理分析移動(dòng)機(jī)器人的運(yùn)動(dòng)，機(jī)器在運(yùn)動(dòng)時(shí)只有Y軸的移動(dòng)與繞Z軸的旋轉(zhuǎn)，在運(yùn)動(dòng)方式的基礎(chǔ)上相機(jī)朝上的安裝方式使得天花板特征點(diǎn)不存在尺度變換，因此原算法的多尺度差分金字塔信息存在大量冗余信息，通過(guò)壓縮多尺度差分金字塔層數(shù)以降低算法耗時(shí)，加速算法，同時(shí)也能提高后續(xù)匹配準(zhǔn)確率。通過(guò)對(duì)比DOG金字塔配置與提取特征數(shù)目、算法耗時(shí)、匹配準(zhǔn)確率之間的關(guān)系得到最合理的DOG層數(shù)。經(jīng)過(guò)驗(yàn)證DOG為14層時(shí)在天花板數(shù)據(jù)集具有最優(yōu)的平均匹配準(zhǔn)確率為61%，且算法時(shí)間為原算法時(shí)間62%。壓縮后的可視化DOG金字塔如圖4所示。

圖4 DOG金字塔

2.2 描述子改進(jìn)

原SIFT通過(guò)計(jì)算關(guān)鍵點(diǎn)鄰域內(nèi)像素的梯度幅值和梯度方向，疊加構(gòu)成128維特征點(diǎn)描述子。

一張圖的特征點(diǎn)數(shù)往往成百上千，高維度的描述子導(dǎo)致算法花費(fèi)時(shí)間長(zhǎng)，計(jì)算成本大，匹配計(jì)算復(fù)雜度高，同時(shí)特征描述子包含了冗余數(shù)據(jù)，因此，對(duì)描述子進(jìn)行降維，可以提高匹配效率。簡(jiǎn)化后描述子的具體劃分如圖5所示，采用3個(gè)“回”字子區(qū)域[14]，3個(gè)區(qū)域用不同灰度值填充表示，每個(gè)區(qū)域依舊分為8個(gè)梯度方向，疊加幅值得到24維描述子。

圖5 描述子鄰域

另外，由于距離特征點(diǎn)越近的像素點(diǎn)對(duì)特征點(diǎn)的特征描述影響越大這一特點(diǎn)，3個(gè)區(qū)域分別取不同權(quán)重進(jìn)行匹配。

w=[w1,w2,w3]

(3)

傳統(tǒng)匹配方式為歐氏距離，歐氏距離定義為：

(4)

本文采用曼哈頓距離替換歐式距離，結(jié)合位置權(quán)重的曼哈頓距離為：

(5)

相比于歐氏距離，曼哈頓距離可明顯縮短運(yùn)算時(shí)間，提高了算法的匹配效率，結(jié)合位置權(quán)重也能提高匹配的準(zhǔn)確率。

2.3 PROSAC算法刪除誤匹配

在匹配階段在利用匹配距離選取最近匹配之后利用FLANN初步篩選匹配，之后常用RANSAC算法進(jìn)一步刪除離群點(diǎn)。RANSAC采取隨機(jī)采樣作為初值，這使得迭代過(guò)程具有隨機(jī)性，且迭代次數(shù)不穩(wěn)定，這種不穩(wěn)定會(huì)隨著數(shù)據(jù)錯(cuò)誤率的提高而大幅度提升。本文采用PROSAC算法去除誤匹配。相比隨機(jī)匹配點(diǎn)作為初值，PROSAC先將數(shù)據(jù)排序，再按照質(zhì)量和的高低來(lái)選取數(shù)據(jù)[15]，選取質(zhì)量最高的數(shù)據(jù)作為迭代初值，因此具有更高的效率和準(zhǔn)確性。

利用最近鄰與次近鄰比值作為質(zhì)量評(píng)價(jià)依據(jù)，表達(dá)式為：

(6)

μ=(β1,β2,…,βn)

(7)

式中，lf為特征點(diǎn)最近曼哈頓距離；ls為次近曼哈頓距離；μ為最近鄰與次近鄰比值合集；β數(shù)值越低匹配質(zhì)量越高，利用最優(yōu)部分β數(shù)值作為PROSAC初值。

3 天花板視覺(jué)定位算法

在改進(jìn)SIFT算法的基礎(chǔ)上構(gòu)建天花板視覺(jué)定位系統(tǒng)。由于單目視覺(jué)存在尺度不確定性，因此結(jié)合IMU信息獲取真實(shí)尺度，目前視覺(jué)與IMU融合定位方法主要分為松耦合式融合方法和緊耦合式融合方法[16]。松耦合方法將視覺(jué)傳感器和 IMU分別計(jì)算得到的位姿直接進(jìn)行融合，算法復(fù)雜度低，時(shí)效性高，并在定位精度上取得了一定的成效。緊耦合方法則是將圖像數(shù)據(jù)和IMU數(shù)據(jù)聯(lián)合進(jìn)行位姿估計(jì)，考慮到相機(jī)和IMU之間的約束關(guān)系，共同構(gòu)建優(yōu)化模型，算法精度高，復(fù)雜度高?？紤]到算法實(shí)時(shí)性，本文采用基于松耦合數(shù)據(jù)的融合方案，實(shí)現(xiàn)慣性數(shù)據(jù)和視覺(jué)數(shù)據(jù)的融合并計(jì)算移動(dòng)機(jī)器人的軌跡，算法整體框架如圖6所示。

圖6 SIFT定位框架

首先在相鄰幀之間利用改進(jìn)的SIFT特征匹配，PROSAC篩出錯(cuò)誤匹配點(diǎn)。利用IMU短時(shí)定位準(zhǔn)確的特性約束匹配。在跟蹤階段利用IMU數(shù)值為初值，根據(jù)重投影特征點(diǎn)位置，設(shè)定一定閾值約束匹配點(diǎn)范圍去除誤匹配。對(duì)篩選出的正確匹配特征點(diǎn)做反畸變，在利用反畸變后的點(diǎn)對(duì)結(jié)合對(duì)極幾何求解旋轉(zhuǎn)矩陣和平移向量。由于單目相機(jī)的尺度不確定性，得到的平移向量與真實(shí)場(chǎng)景像素之間存在一個(gè)不確定縮放尺度，需要借助IMU數(shù)據(jù)估計(jì)。

IMU數(shù)據(jù)首先需要進(jìn)行初始化操作，消除零偏，再利用預(yù)積分對(duì)齊圖像，求得對(duì)應(yīng)時(shí)刻的圖像位姿。

視覺(jué)里程計(jì)會(huì)得到平移向量tv，存在縮放尺度為s，IMU通過(guò)加速度預(yù)積分輸出位移為ti，通過(guò)最小化向量誤差估計(jì)最優(yōu)的尺度因子s*：

(8)

在得到視覺(jué)尺度后利用視覺(jué)估計(jì)與慣性估計(jì)的加權(quán)和[17]作為真實(shí)位移的估計(jì)量t*：

t*=λs*tv+(1-λ)ti

(9)

再通過(guò)每?jī)蓭g的平移向量求和為里程信息Lk：

(10)

在得到位姿之后利用最小化重投影誤差進(jìn)一步優(yōu)化位姿。最小化重投影誤差表達(dá)式為：

(11)

利用優(yōu)化后的位姿為初值給定一定閾值局部搜索SIFT特征點(diǎn)進(jìn)行回環(huán)檢測(cè)，消除累計(jì)誤差。

4 算法對(duì)比實(shí)驗(yàn)

4.1 實(shí)驗(yàn)環(huán)境

本文使用改進(jìn)后的SIFT分別在室內(nèi)天花板數(shù)據(jù)集上進(jìn)行了特征提取與匹配實(shí)驗(yàn)，驗(yàn)證算法提取匹配效率與精度。考慮實(shí)際定位過(guò)程，周?chē)h(huán)境光線會(huì)發(fā)生變化，拍攝的圖像也可能會(huì)產(chǎn)生光照變換、旋轉(zhuǎn)、遮擋視角等現(xiàn)象，故實(shí)驗(yàn)數(shù)據(jù)來(lái)自于多個(gè)不同場(chǎng)景，且挑選了多種特殊情況的圖像對(duì)分別驗(yàn)證。在對(duì)比提取匹配的基礎(chǔ)上與開(kāi)源的視覺(jué)定位方案對(duì)比跟蹤定位精度。

實(shí)驗(yàn)場(chǎng)景為普通家居環(huán)境，相機(jī)圖像的分辨率為640×480。實(shí)驗(yàn)設(shè)備采用處理器為3.40 GHz的四核、內(nèi)存為8 GB的個(gè)人計(jì)算機(jī)與移動(dòng)機(jī)器采用ARM-A7嵌入式處理器，機(jī)器外觀如圖7所示。

圖7 機(jī)器外形

4.2 特征提取與匹配實(shí)驗(yàn)

首先在PC端與ARM端分別運(yùn)行算法，并在程序中記錄用時(shí)，在大規(guī)模運(yùn)行后記錄平均用時(shí)。

表1為5種算法在不同平臺(tái)的平均單幀提取特征點(diǎn)與描述子用時(shí)運(yùn)行時(shí)間，可以看出改進(jìn)后的SIFT耗時(shí)較原算法提高了58%，在PC端可以達(dá)到30 Hz左右的幀率，在ARM端也可達(dá)到8 Hz左右的幀率。

表1 算法提取用時(shí)對(duì)比 (ms)

在天花板數(shù)據(jù)集多個(gè)場(chǎng)景上對(duì)比驗(yàn)證各個(gè)算法，選取改進(jìn)SIFT算法部分匹配效果做可視化如圖8所示。

(a) 相鄰平移 (b) 相鄰旋轉(zhuǎn)

在對(duì)比各個(gè)算法在不同場(chǎng)景的特征提取的效果之后，分析整理詳細(xì)提取數(shù)據(jù)對(duì)比表如表2所示。

表2 算法各場(chǎng)景準(zhǔn)確率對(duì)比

由表2可以對(duì)比分析，在天花板數(shù)據(jù)上改進(jìn)SIFT較原算法各個(gè)場(chǎng)景匹配成功率均有明顯提高。在相鄰幀與光照變化幀，改進(jìn)SIFT與SUPERPOINT+SUPERGLUE算法匹配準(zhǔn)確率接近；在大角度選擇時(shí)，基于深度學(xué)習(xí)方案準(zhǔn)確率大幅下降，改進(jìn)SITF依舊保持高的準(zhǔn)確率。

4.3 視覺(jué)定位實(shí)驗(yàn)

在提取匹配實(shí)驗(yàn)的基礎(chǔ)上，進(jìn)一步分析改進(jìn)SIFT的視覺(jué)定位精度效果。天花板數(shù)據(jù)集作為實(shí)驗(yàn)場(chǎng)景，對(duì)比SIFT構(gòu)建的視覺(jué)慣性里程計(jì)與ORB-SLAM2、PL-SLAM開(kāi)源算法的定位精度，給其提供IMU真實(shí)尺度。3個(gè)算法對(duì)比兩個(gè)場(chǎng)景定位效果如圖9所示，圖9a和圖9b為ORB-SLAM2與PL-SLAM的定位軌跡與真實(shí)軌跡對(duì)比，圖9c為本文算法實(shí)現(xiàn)定位效果。

(a) ORB-SLAM2跟蹤效果

通過(guò)絕對(duì)軌跡誤差的均方根誤差對(duì)實(shí)驗(yàn)結(jié)果進(jìn)行量化評(píng)估，結(jié)果如表3所示。其中RMSE為軌跡的均方根誤差，單位為m。ORB-SLAM2在兩個(gè)場(chǎng)景平均RMSE為0.137 5 m；PL-SLAM為0.147 5 m；本文算法平均RMSE為0.076 m。

表3 算法RMSE對(duì)比

5 結(jié)論

本文提出了一種基于SIFT的室內(nèi)移動(dòng)機(jī)器人視覺(jué)定位系統(tǒng)。在PC端與ARM端分別驗(yàn)證本文算法。實(shí)驗(yàn)結(jié)果表明，改進(jìn)后的 SIFT算法較其他算法與原算法在時(shí)效性與準(zhǔn)確性均有明顯提升。提出的定位方法平均絕對(duì)軌跡誤差的均方根誤差為0.076 m。分析實(shí)驗(yàn)結(jié)果得出結(jié)論如下：

(1)提出的改進(jìn)SIFT方法提高了算法實(shí)時(shí)性，在算力受限設(shè)備上達(dá)到實(shí)時(shí)性要求，同時(shí)保證了特征點(diǎn)質(zhì)量。

(2)多組天花板場(chǎng)景下的實(shí)驗(yàn)結(jié)果表明，改進(jìn)算法提取的STFT特征點(diǎn)具有較高的唯一性，相較于ORB需要依賴龐大的詞袋文件，SIFT特征點(diǎn)可以用于進(jìn)行回環(huán)檢測(cè)，且具有較高準(zhǔn)確率。

(3)提出的慣性視覺(jué)定位算法在算力有限的平臺(tái)可以實(shí)時(shí)運(yùn)行跟蹤定位與回環(huán)檢測(cè)，且較開(kāi)源算法具有更高的定位精度。