劉梅 劉任平 陳賽清

摘 要： 為使清潔機(jī)器人在清掃的房間中能快速確定自己的位置，根據(jù)廣角攝像頭采集到的墻角畫面圖像中墻角的三條棱線對應(yīng)的角度關(guān)系，來確定清潔機(jī)器人離墻面的距離，進(jìn)而確定在房間中的位置。首先對廣角攝像頭采集到的畸變圖像進(jìn)行校正，再識別提取出墻角棱線，最后依據(jù)測量出來的房間的高度和位置信息建立三維模型并利用神經(jīng)網(wǎng)絡(luò)模擬數(shù)學(xué)關(guān)系實現(xiàn)定位。仿真與實際數(shù)據(jù)對比表明，此算法穩(wěn)定，能夠滿足清潔機(jī)器人的定位需求。

關(guān)鍵詞： 清潔機(jī)器人； 三維建模； 墻角棱線夾角； 定位

中圖分類號：TP391 文獻(xiàn)標(biāo)志碼：A 文章編號：1006-8228（2013）05-21-04

Research of location algorithm based on three-dimensional models

Liu Mei， Liu Renping， Chen Saiqing

（College of information engineering， North China University of Technology， Beijing 100144， China）

Abstract： In order to make the clean robot quickly position itself in the room， the distance of robot to the wall is determined based on angles of three lines from a corner in the picture collected by wide-angle camera on the robot and then its own position in the room can be calculated. The first step is to correct the distorted graph collected by camera. The second step is to identify the lines from corner. Finally the positioning is realized by 3-dim mathematical model constructed based on the height of the room and coordinate. Simulation results show that this algorithm is stable and can meet the needs of positioning.

Key words： cleaning robot； three-dimensional models； angle between corner lines； positioning

0 引言

隨著人們生活水平的提高和智能家居技術(shù)的成熟，清潔機(jī)器人已經(jīng)成為較為普遍使用的日常家電產(chǎn)品。在清潔機(jī)器人路徑規(guī)劃過程中，清潔機(jī)器人的導(dǎo)航定位是路徑規(guī)劃算法中的一個難點。清潔機(jī)器人目前有多種導(dǎo)航方式，根據(jù)環(huán)境信息的完整程度，導(dǎo)航指示信號類型，導(dǎo)航地域等因素的不同，可以分為地圖導(dǎo)航，陸標(biāo)導(dǎo)航，視覺導(dǎo)航，感知器導(dǎo)航等[1]。

本文基于視覺導(dǎo)航進(jìn)行研究。視覺導(dǎo)航的優(yōu)點在于探測范圍廣，能最大采集周圍環(huán)境信息，缺點是處理圖像所需計算量大。我們采用廣角攝像頭采集到的墻角圖像信息及超聲波檢測到的房間高度信息來建立一個三維模型，從而確定清潔機(jī)器人在房間中所處的位置，為之后尋找效率最快的全覆蓋路徑規(guī)劃作好準(zhǔn)備。

1 廣角鏡頭圖像畸變校正

1.1 畸變校正原理

廣角鏡頭光學(xué)系統(tǒng)由于攝像機(jī)本身的構(gòu)造，以及制造、安裝、工藝等因素導(dǎo)致的各種非線性畸變，如徑向畸變，離心畸變和薄棱鏡畸變等，其空間點與其圖像對應(yīng)點之間是一種復(fù)雜的非線性關(guān)系，一般分為徑向畸變與切向畸變[2]，Tsai已經(jīng)證明，對于大多數(shù)計算機(jī)視覺應(yīng)用來說，切向畸變可以忽略[3]。因此本文只考慮徑向畸變。

本文采用平面網(wǎng)絡(luò)模板法對攝像機(jī)圖像進(jìn)行非線性畸變較正。平面模板法的原理是利用一個特定模板，如平面模板或同心圓模板，描述理想圖和畸變圖之間的地址映射，以實現(xiàn)對圖像的非線性校正。

1.2 平面網(wǎng)格模板法

平面網(wǎng)格模板法一般有三個步聚：制定模板，特征點提取和鏡頭畸變系數(shù)求解。

網(wǎng)格標(biāo)定模板可以通過程序自動生成點陣圖案來制作，如圖1所示，圖1中各網(wǎng)格線選用不同的灰度值。

圖1 網(wǎng)格標(biāo)定板

在攝像頭顯示器窗口，增加一個可視十字架。移動標(biāo)定模板使模板上的十字架和顯示器上觀察窗口的十字架重合，此時捕獲一副圖像。其對應(yīng)的畸變圖如圖2所示。

圖2 畸變圖像

特征點提取步驟如下。

理想網(wǎng)格點（x，y）計算：圖像中心附近小范圍內(nèi)畸變很小，可以忽略，以中心四鄰域的四個網(wǎng)格交點作為無畸變的理想點，由于網(wǎng)格是等間距的，可以推算出其他網(wǎng)格交點的理想坐標(biāo)。

實際網(wǎng)格點（，）提?。河捎诰W(wǎng)格線平行交叉，以網(wǎng)格交點為頂點形成一個解析曲面。遠(yuǎn)離標(biāo)定模板中心的網(wǎng)格交點，在成像后，由于鏡頭的畸變，曲面頂點將發(fā)生偏移。假設(shè)解析曲面用二元二次多項式擬合，如公式：

（1.1）

其中，p表示曲面上的灰度值，在像素交點及周圍附近尋找6個點（x，y，p），即可以求出多項式的六個系數(shù)。

對此曲面進(jìn)行求導(dǎo)，可得極值點坐標(biāo)如下：

（1.2）

（1.3）

根據(jù)以上兩個公式，可以求得K對圖像理想點與畸變點的映射表。根據(jù)求得的K對映射關(guān)系，下一步就可以求解鏡頭畸變系數(shù)。

鏡頭畸變系數(shù)求解：假設(shè)點（，）是校正前圖像的一點（畸變圖像點），那么采用多項式變換，此點（，）與校正后圖像的對應(yīng)點（x，y）（圖像理想點）的關(guān)系為：

（1.4）

（1.5）

其中n為多項式的次數(shù)。一般來講，n取到3次，畸變校正效果就很好了。

使用最小二乘法，使擬合誤差平方和ε為最小，也就是使：

（1.6）

最小，則需要求：

（1.7）

由此得到：

（1.8）

同理可得出：

（1.9）

其中，K為控制點對的個數(shù)，s=0，1，2，…，n；t=0，1，…，n-s；s+t≤n。分別求解公式（1.8）（1.9），即可求出aij，bij，再將其代入（1.4）（1.5）就可實現(xiàn)兩個坐標(biāo)系之間的變換。

2 墻角檢測及識別

清潔機(jī)器人的工作環(huán)境多為室內(nèi)，采集到的墻角信息多為直線信息，可采用Hough變換從圖像中提取墻角等直線信息，然后對計算出的直線信息進(jìn)行模式匹配，找出房間的墻角，進(jìn)而利用三維模型計算清潔機(jī)器人離該墻角兩邊墻面的距離來確定自己所在的位置。

目前直線提取最成熟、應(yīng)用最廣泛的算法仍然是Hough變換。Hough變換有非常多的優(yōu)點，比如提取穩(wěn)定，精度高。但其缺點仍然非常顯著，就是實時性太差。Hough變換檢測直線的原理是首先得到邊緣點，對每一邊緣點得到過邊緣點的所有直線，直線參數(shù)為極坐標(biāo)下的（γ，δ），而每一個（γ，δ）都對應(yīng)極坐標(biāo)中一個點，對每一條直線對應(yīng)的點記為投票一次，最終記錄投票數(shù)，投票數(shù)大于某一域值的即可看作一條直線。本文采用文獻(xiàn)[4]提出的一種子像素級圖像測量直線提取算法，采用改進(jìn)的Hough變換對線狀特征進(jìn)行提取，獲得具有確定長度和端點的直線段，以此直線段為初始值自適應(yīng)生成直線模板，利用最小二乘方法進(jìn)行模板與圖像特征匹配，實現(xiàn)圖像線特征的子像素提取。

2.1 邊緣檢測

邊緣是圖像最基本的特征，為人們識別目標(biāo)提供了重要的參數(shù)信息[4]。Robert算子提取邊緣容易丟失灰度值變化緩慢的局部邊緣，平滑噪聲作用較小，對噪聲較為敏感。Sobel算子對噪聲有一定的平滑作用，產(chǎn)生較好的邊緣效果，但是對于幅值較小的邊緣點，容易丟失其邊緣，降低了檢測定位精度。LOG算子的抗干擾能力強(qiáng)，且能提取出對比度較弱的邊緣。Canny算子具有信噪比高，檢測精度高和單邊緣響應(yīng)的優(yōu)點。

2.2 Hough變換

Hough變換自從1962年由Hough提出以后，已經(jīng)成為直線檢測的重要工具，也是其他參數(shù)化形狀檢測的重要工具，其本質(zhì)是從圖像空間到參數(shù)空間的映射。Hough變換是對圖像進(jìn)行某種形式的坐標(biāo)變換，它將原始圖像中給定形伏的直線或曲線變換成變換空間的一個點，即原始圖像中給定形狀的直線或曲線上的所有點都集中到變換空間的某個點上形成峰值。這樣，就把原始圖像中給定形狀的直線或曲線的檢測問題變成了尋找變換空間中的峰點問題，也即把檢測整體特性（給定直線的點集）變成檢測局部特性的問題。

圖像空間中的任意解析曲線可以表述為：

f（（γ1γ2…γn），（x，y））=0 （2.1）

其中，γ1γ2…γn為該曲線的特征參數(shù)。如果將上式中的特征參數(shù)與變量的角色調(diào)換，那么，上式等價于：

g（（x，y），（γ1γ2…γn））=0 （2.2）

也就是說，圖像空間中屬于同一解析曲線的點經(jīng)過式（1.2）變換后，都映射到參數(shù)空間中，且相交于由參數(shù)確定的點。從而說明，只要圖像空間中屬于同一解析曲線的數(shù)據(jù)點足夠多，就可以通過判斷參數(shù)空間中各參數(shù)點的積累值來確定此解析曲線的描述。

2.3 二維模式匹配

二維模式匹配，就是在一個n×m二維矩陣T中尋找一個p×q二維矩陣P（稱為模板）。假定文本T的大小為n×n，模板P的大小為m×m，m

2.4 角度測度

在墻角二值圖像中要測量出墻角三條棱線的夾角大小作為后面三維模型的已知參數(shù)。最小二乘法就是用誤差的平方和作為測量總誤差的標(biāo)準(zhǔn)，求所有誤差平方和最小的解進(jìn)而求出夾角大小的方法。

空間直線可由空間兩相交平面表示，平面方程的一般形式為：

ax+by+cz+d=0 （2.3）

其中，（x，y，z）為所表示平面上的任一點坐標(biāo)，用齊次坐標(biāo)形式表示為（x'，y'，z'，t）（t≠0），則（2.3）式可寫為

ax'+by'+cz'+dt=0 （2.4）

其中，下面用（x'，y'，z'，t）表示直線或點的齊次坐標(biāo)。

設(shè)相機(jī)內(nèi)部參數(shù)矩陣K，旋轉(zhuǎn)矩陣R和位移向量T分別為

（2.5）

方向為（vx，vy，vz）的直線族，其無窮遠(yuǎn)點的齊次坐標(biāo)為（vx，vy，vz，0）。

由相機(jī)成像方程可知，齊次坐標(biāo)所示的無窮遠(yuǎn)點與其在相機(jī)成像平面上的圖像坐票（u，v）有以下關(guān)系：

（2.6）

由（vx，vy，vz）為單位向量可推出：

（2.7）

將相機(jī)內(nèi)部參數(shù)矩陣K代入（2.7）式，可計算出|ρ1|和|ρ2|。則兩直線夾角θ的余弦

（2.8）

利用（2.8）式可求出墻角三條棱線夾角θ1和θ2。

3 三維環(huán)境模型的建立

在一個矩形房間中清潔機(jī)器所處的位置不同，所采集到的墻角信息即兩夾角是不同的；將攝像頭放置在不同的高度，所采集到的墻角信息，也會不同，由此猜測清潔機(jī)器人在某個地方采集到的墻角信息，與房間的高度和離墻面的距離有關(guān)。

分離出墻角的三條主要棱，再計算出三條棱之間的角度。檢測出的三條棱中，有一條始終是垂直于地面的，將該棱兩邊的夾角記為θ1和θ2，利用超聲波采集到房間的高度為h，清潔機(jī)器人距離該墻角相鄰墻面的距離為x，y。

本文采用BP神經(jīng)網(wǎng)絡(luò)[6]擬合方法來找出墻角棱線夾角與距離的映射關(guān)系。BP網(wǎng)絡(luò)模型處理信息的基本原理是：輸入信號Xi通過中間節(jié)點（隱層點）作用于輸出節(jié)點，經(jīng)過非線形變換，產(chǎn)生輸出信號Yk，網(wǎng)絡(luò)訓(xùn)練的每個樣本包括輸入向量X和期望輸出量t，網(wǎng)絡(luò)輸出值Y與期望輸出值t之間的偏差，通過調(diào)整輸入節(jié)點與隱層節(jié)點的聯(lián)接強(qiáng)度取值Wij和隱層節(jié)點與輸出節(jié)點之間的聯(lián)接強(qiáng)度Tjk以及閾值，使誤差沿梯度方向下降，經(jīng)過反復(fù)學(xué)習(xí)訓(xùn)練，確定與最小誤差相對應(yīng)的網(wǎng)絡(luò)參數(shù)（權(quán)值和閾值），訓(xùn)練即告停止。此時經(jīng)過訓(xùn)練的神經(jīng)網(wǎng)絡(luò)即能對類似樣本的輸入信息，自行處理輸出誤差最小的經(jīng)過非線形轉(zhuǎn)換的信息。

隱節(jié)點輸出模型：

Oj=f（∑Wij×Xi-q j） （3.1）

輸出節(jié)點輸出模型：

Yk=f（∑Tjk×Oj-qk） （3.2）

f-非線形作用函數(shù)；q -神經(jīng)單元閾值。輸入信號Xi包括棱兩邊夾角θ1和θ2及房間的高度h。

作用函數(shù)是反映下層輸入對上層節(jié)點刺激脈沖強(qiáng)度的函數(shù)又稱刺激函數(shù)，一般取為（0，1）內(nèi)連續(xù)取值Sigmoid函數(shù)：

f（x）=1/（1+e-x） （3.3）

誤差計算模型是反映神經(jīng)網(wǎng)絡(luò)期望輸出與計算輸出之間誤差大小的函數(shù)：

Ep=1/2×∑（tpi-Opi）2 （3.4）

tpi-i節(jié)點的期望輸出值；Opi-i節(jié)點計算輸出值。

神經(jīng)網(wǎng)絡(luò)的學(xué)習(xí)過程，即連接下層節(jié)點和上層節(jié)點之間的權(quán)重拒陣Wij的設(shè)定和誤差修正過程。BP網(wǎng)絡(luò)有師學(xué)習(xí)方式-需要設(shè)定期望值和無師學(xué)習(xí)方式-只需輸入模式之分。自學(xué)習(xí)模型為

ΔWij（n+1）= h ×Фi×Oj+a×ΔWij（n） （3.5）

h -學(xué)習(xí)因子；Фi-輸出節(jié)點i的計算誤差；Oj-輸出節(jié)點j的計算輸出；a-動量因子。

在matlab中實驗，由BP神經(jīng)網(wǎng)絡(luò)算出的離墻面的距離與實際距離對比情況如圖3所示。

圖3 測驗數(shù)據(jù)與實際數(shù)據(jù)對比圖

4 結(jié)束語

本文提出了基于墻角棱線信息的提取來定位的問題，建立了房間三維模型，進(jìn)而確定位置的方法。該算法適用于天花板較簡單無復(fù)雜線條且形狀為矩形的房間。仿真實驗結(jié)果表明，該方法是有效且穩(wěn)定的。因為清潔機(jī)器人本身大小在30-40cm之間，所以誤差控制在該數(shù)據(jù)范圍內(nèi)都是可行的。利用三維建模來定位的方法為后續(xù)清潔機(jī)器人在房間中實時確定其位置提供了可靠依據(jù)。但該算法受環(huán)境影響較大，主要影響因素是房間亮度、是否有較大遮擋物等。

參考文獻(xiàn)：

[1] 陸新華，張桂林.自主移動面器人智能導(dǎo)航研究進(jìn)展[J].機(jī)器人

Robot，2003.1.

[2] Lin J M，Xing M L，Sha D G，et al. Distortion measurement of CCD

imaging system with short focal length and large-field objective[J]. Optics and Lasers in Engineering，2005.43：1137-1144

[3] Tsai R Y.A versatile camera calibration technique for high-accuracy

3D machine vision metrology using off-the-shelf TV cameras and lenses[J].IEEE Journal of Robotics and Automation，1987.3（4）：323-344

[4] 張春森，姜楠.一種子像素圖像測量直線提取算法[J].中國礦業(yè)大學(xué)

學(xué)報，2009.10.

[5] 陳海峰.數(shù)字圖像中基本幾何形狀檢測算法的研究與應(yīng)用[D].浙江大

學(xué)，2007.

[6] 唐萬梅.BP神經(jīng)網(wǎng)絡(luò)網(wǎng)絡(luò)結(jié)構(gòu)優(yōu)化問題的研究[J].系統(tǒng)工程理論與

實踐，2005.10.