張 琪，羅福源，肖 灑，焦曉進(jìn)

（1.南京航空航天大學(xué)機(jī)電學(xué)院，江蘇 南京 210016；2.南京航空航天大學(xué)金城學(xué)院，江蘇 南京 211156）

0 引言

隨著社會(huì)的發(fā)展和生活水平的提高，人們迫切需要從繁重的清潔工作中解放出來。在傳統(tǒng)的吸塵器越來越無法滿足快節(jié)奏現(xiàn)代生活需要的時(shí)候，全自動(dòng)室內(nèi)清潔機(jī)器人應(yīng)運(yùn)而生。全自動(dòng)清潔器人裝載有探測(cè)傳感器，可以在前方遇到障礙物的時(shí)候自動(dòng)改變方向。但是，目前市場(chǎng)上現(xiàn)有的清潔機(jī)器人的智能水平與清潔效率還較低，原因在于無法自主定位與導(dǎo)航。機(jī)器人在室內(nèi)移動(dòng)過程中，實(shí)際上是在沿墻模式、折線式、繞柱模式、自由模式、左螺旋和右螺旋等幾種行走模式中隨機(jī)切換，并通過大量路徑的累積效應(yīng)達(dá)到遍歷清潔整個(gè)室內(nèi)地面的目的。在自主移動(dòng)機(jī)器人的行走中，實(shí)現(xiàn)機(jī)器人自身的準(zhǔn)確定位是一個(gè)基本而又非常重要的功能，也是移動(dòng)機(jī)器人具有自主性的先決條件之一。作為移動(dòng)機(jī)器人的研究熱點(diǎn)，現(xiàn)階段國(guó)內(nèi)外研究主要集中在同時(shí)定位與地圖構(gòu)建（SLAM）方面［1］。地圖的構(gòu)建與存儲(chǔ)方式是SLAM的核心問題。為此，提出了一種基于直線行程地圖的表示和存儲(chǔ)方法，在此基礎(chǔ)上進(jìn)行清潔機(jī)器人的路徑規(guī)劃。

1 清潔機(jī)器人建模

1.1 機(jī)器人運(yùn)動(dòng)控制建模

清潔機(jī)器人簡(jiǎn)化模型如圖1所示，由2個(gè)驅(qū)動(dòng)輪和1個(gè)萬向輪組成。驅(qū)動(dòng)輪上裝有左右2個(gè)編碼器測(cè)量速度，車身上載有電子羅盤測(cè)量機(jī)器人的位姿。

在全局坐標(biāo)系XOY中，設(shè)t時(shí)刻的機(jī)器人位姿為（Xt，Yt，θt）T，速度為vt，角速度為ωt。所以經(jīng)過時(shí)間t1后清潔機(jī)器人的位姿為：

圖1 機(jī)器人模型

通過式（1），理論上可以計(jì)算出任意t+t1時(shí)刻機(jī)器人的位姿。在實(shí)際運(yùn)動(dòng)過程中，可能遇到車輪打滑、電子羅盤受干擾、速度估計(jì)不準(zhǔn)和位置計(jì)算累積誤差等問題。需要用程序算法進(jìn)一步修正。

1.2 超聲雷達(dá)的構(gòu)建

超聲波傳感器因其低廉的價(jià)格而廣泛運(yùn)用于移動(dòng)機(jī)器人中，但是超聲波傳感器存在掃描速度慢、感應(yīng)信息具有不確定性等問題［2-3］。系統(tǒng)用4個(gè)超聲波傳感器按照十字形排列方式固定在一塊底板上，并在底板下方安裝一個(gè)舵機(jī)驅(qū)動(dòng)其轉(zhuǎn)動(dòng)，從而構(gòu)建出一個(gè)能夠?qū)κ覂?nèi)環(huán)境進(jìn)行實(shí)現(xiàn)旋轉(zhuǎn)掃描探測(cè)的超聲雷達(dá)。如圖2所示。

圖2 超聲波傳感器模型

4個(gè)超聲波傳感器成正交分布，這樣可以相互不產(chǎn)生干擾，提高測(cè)量準(zhǔn)確性。使用多個(gè)超聲波傳感器可以提高掃描探測(cè)效率。舵機(jī)每旋轉(zhuǎn)1°后暫停，這時(shí)4個(gè)超聲傳感器分別進(jìn)行1次超聲探測(cè)。這樣，只要舵機(jī)旋轉(zhuǎn)90°，就可以測(cè)出清潔機(jī)器人周圍360°的障礙物信息。

2 室內(nèi)環(huán)境探測(cè)及路徑規(guī)劃

2.1 墻面檢測(cè)

通常墻壁是較為平整且長(zhǎng)度較長(zhǎng)的障礙物，因此將其作為機(jī)器人行走的基準(zhǔn)是比較合適的。

為了便于說明和分析問題，搭建了3m×2m的矩形房間模型。

機(jī)器人利用舵機(jī)旋轉(zhuǎn)超聲波傳感器依次探測(cè)，依照如圖3所示的程序流程進(jìn)行探測(cè)。

圖3 墻面探測(cè)流程

圖4為仿真環(huán)境中超聲波傳感器探測(cè)到的一組數(shù)據(jù)。可見，在4個(gè)墻角的地方數(shù)據(jù)發(fā)生了明顯的失真。這是超聲波本身的特性決定的：超聲波在墻角處會(huì)發(fā)生偏折現(xiàn)象，導(dǎo)致了測(cè)得的墻壁通常是較為平整且長(zhǎng)度較長(zhǎng)的障礙物，因此將其作為機(jī)器人行走的基準(zhǔn)是比較合適的。

圖4 超聲波探測(cè)結(jié)果

數(shù)據(jù)跳躍性失真，表現(xiàn)在數(shù)據(jù)出現(xiàn)非線性和不連續(xù)增大。針對(duì)這個(gè)問題，提出了一種基于最小二乘法的直線擬合算法。

依據(jù)式（2）把以機(jī)器人為中心的極坐標(biāo)轉(zhuǎn)化為笛卡爾坐標(biāo)系。

（xi，yi）為第i個(gè)點(diǎn)的坐標(biāo)；i=0，1，2，…，359，代表角度；ρ為機(jī)器人到障礙物的距離。

在直角坐標(biāo)系中，為了剔除“墻角”的失真點(diǎn)，先把數(shù)據(jù)分段。算法如下：

b.設(shè)置一個(gè)閾值ε，如果Di＜ε，判斷為連續(xù)的點(diǎn)，則i=i+1，轉(zhuǎn)到步驟a。如果Di＞ε，說明此處為斷點(diǎn)，則把i以前的連續(xù)點(diǎn)加入一個(gè)連續(xù)點(diǎn)集的集合Cj。

c.判斷點(diǎn)集Cj中點(diǎn)的個(gè)數(shù)，如果少于一定值，判斷為噪點(diǎn)，將其去除。轉(zhuǎn)到步驟a。

通過上述的算法，將數(shù)據(jù)劃分成幾段連續(xù)的數(shù)據(jù)點(diǎn)。如圖5所示，四周的失真點(diǎn)已經(jīng)被剔除，剩下的點(diǎn)被劃分成了四段數(shù)據(jù)點(diǎn)集。

圖5 噪點(diǎn)去除后的探測(cè)結(jié)果

最小二乘法進(jìn)行直線擬合：最小二乘法可以這樣理解，假定要將點(diǎn)集Cj中的n個(gè)點(diǎn)（X1，Y1），（X2，Y2），…，（Xn，Yn）擬合一條直線，擬合的直線是Y=aX+b。只要確定2個(gè)參數(shù)a和b即可［4］，則有：

經(jīng)過直線擬合的直線L1，L2，L3，L4如圖6所示。為了方便觀察，圖上并沒有將原來的點(diǎn)去除。

圖6 擬合后的探測(cè)結(jié)果

2.2 行走方式及路徑規(guī)劃

與巡線機(jī)器人不同，清潔機(jī)器人在運(yùn)動(dòng)的過程中，要求盡量覆蓋每一塊無障礙區(qū)域，同時(shí)，盡可能不重復(fù)覆蓋同一塊區(qū)域，減少能耗。

為了完全覆蓋，現(xiàn)有2種比較好的行走方式，直線式和內(nèi)螺旋式。經(jīng)過比較研究，內(nèi)螺旋式的行走方式算法復(fù)雜轉(zhuǎn)角更多，且需要建立柵格地圖與之配合。所以采用來回直線式行走方式，同時(shí)“由線及面”表示地圖。

機(jī)器人根據(jù)以上所述的基準(zhǔn)前進(jìn)，如圖7所示。利用機(jī)器人的右側(cè)超聲波探測(cè)與基準(zhǔn)的距離為dj。需要讓機(jī)器人保持與基準(zhǔn)特定的距離沿直線前進(jìn)，利用以下算法實(shí)現(xiàn)：

a.機(jī)器人從起始點(diǎn)出發(fā)，平行基準(zhǔn)墻壁前進(jìn)。

b.機(jī)器人與基準(zhǔn)的理論距離為Dj，測(cè)量的實(shí)際距離為dj，設(shè)置一個(gè)閾值εj。判斷若｜dj-Dj｜＜εr，轉(zhuǎn)到步驟a。否則進(jìn)行下一步。

c.若dj-Dj＞εr，說明距離偏大，機(jī)器人向右旋轉(zhuǎn)αr=arctan，L為右側(cè)超聲波到車尾的距離。若dj-Dj＜-εr，說明距離偏小，機(jī)器人向左旋轉(zhuǎn)αl= arctan。

d.沿平行基準(zhǔn)墻壁的方向前進(jìn)，返回步驟b。

圖7 直線行走模型

2.3 障礙物的探測(cè)及繞行

機(jī)器人沿直線運(yùn)動(dòng)的時(shí)候，遠(yuǎn)離基準(zhǔn)的那一個(gè)超聲波傳感器進(jìn)行障礙物的探測(cè)工作。通過超聲波傳感器記錄下障礙物的起始位置s1和終止位置s2。機(jī)器人就探測(cè)到了和障礙物的距離。機(jī)器人接近障礙物，前置超聲波傳感器探測(cè)障礙物距離小于εd時(shí)，停止前進(jìn)，啟動(dòng)探測(cè)程序。

當(dāng)探測(cè)到的數(shù)據(jù)有一個(gè)明顯的躍遷的時(shí)候，判定為障礙物的邊角。判斷完障礙物的邊角，就可以用緊貼基準(zhǔn)墻行走的程序完成繞障。

2.4 基于直線行程的地圖建立

在SLAM中，地圖的表示方法現(xiàn)階段主要有柵格地圖、拓?fù)涞貓D和特征地圖［5］。這3種方式的特點(diǎn)如表1所示［6］。

表1 地圖表示方式特點(diǎn)比較

從表1可以看出，特征地圖和拓?fù)涞貓D算法難度較大，惟一比較符合要求的柵格法需要大量的存儲(chǔ)空間，所以不適合創(chuàng)建信息量大的地圖［7］。

基于直線行走方式，提出了一種直線行程地圖。類似于柵格地圖的“點(diǎn)陣”，直線行程地圖是利用“線陣”來組成一個(gè)平面的。實(shí)驗(yàn)仿真了一個(gè)室內(nèi)的簡(jiǎn)易環(huán)境如圖8a所示。類似于柵格地圖，把這個(gè)空間分化成圖8b所示的矩形條狀，其條寬一般是取機(jī)器人本身的寬度。寬度取的越小，地圖的精度相應(yīng)的就越高。

在程序中設(shè)置一個(gè)動(dòng)態(tài)數(shù)組Map［m］［n］記錄下這個(gè)機(jī)器人探測(cè)過的地圖數(shù)據(jù)。其中，m=1，2，3，…，代表第m個(gè)機(jī)器人走過的行程編號(hào)，n代表每個(gè)行程中被障礙物打斷的直線號(hào)。如圖8c按照其行走方式走完一個(gè)直線后，發(fā)現(xiàn)沒有遇到障礙物，此時(shí)，在Map［1］［1］中記錄下直線兩端的坐標(biāo)。如圖8d和圖8e所示，當(dāng)遇到障礙物時(shí)，線被切割成幾段。此時(shí)分別在 Map［3］［1］，Map［3］［2］和 Map［4］［1］，Map［4］［2］，Map［4］［3］中記錄下始末的坐標(biāo)。以此類推，機(jī)器人掃完這個(gè)空間，就完成了地圖建立。如圖8f所示。

圖8 直線行程地圖的表示

2.5 機(jī)器人清掃算法流程

清潔機(jī)器人在運(yùn)動(dòng)的過程中不斷建立直線行程地圖，同時(shí)也很好地覆蓋了所有沒有障礙物的區(qū)域。其實(shí)現(xiàn)流程如圖9所示。

圖9 清掃算法流程

在圖9中，有：

a.機(jī)器人旋轉(zhuǎn)90°，4個(gè)超聲波傳感器測(cè)出360°的信息，將測(cè)出的這些點(diǎn)擬合成直線。所有直線中最長(zhǎng)的一條默認(rèn)為墻壁。

b.障礙物可能是墻壁或室內(nèi)其他物體，由上一個(gè)循環(huán)的超聲波探測(cè)得出。

c.進(jìn)入繞障子程序。

3 機(jī)器人運(yùn)動(dòng)實(shí)例

為了驗(yàn)證提出的行走控制方法，在實(shí)驗(yàn)環(huán)境中搭建了一個(gè)簡(jiǎn)單的室內(nèi)模擬環(huán)境如圖10所示。超聲波傳感器探測(cè)周圍360°環(huán)境。通過算法程序處理后如圖11所示。通過算法將點(diǎn)擬合成了6條直線。根據(jù)算法，取最長(zhǎng)的3號(hào)直線為基準(zhǔn)墻壁。

圖10 簡(jiǎn)單實(shí)驗(yàn)環(huán)境

圖11 障礙物情況下的控測(cè)

基準(zhǔn)確定以后，進(jìn)入直線往復(fù)清掃，實(shí)驗(yàn)環(huán)境尺寸為3m×2m，測(cè)得機(jī)器人寬度為0.2m，作為直線行程的寬度。機(jī)器人在行走的過程中記錄下行程數(shù)據(jù)，存儲(chǔ)數(shù)組Map［m］［n］，并繪制地圖。最后的地圖如圖12所示。

圖12 簡(jiǎn)單環(huán)境下的直線行程地圖

機(jī)器人的速度設(shè)定為50mm／s，完成清掃所用時(shí)間為659s。利用隨機(jī)遍歷的方式清掃，每次的用時(shí)都不確定，但是最少也要1000s。利用這種方法可以有效地提高機(jī)器人的清潔效率。

4 結(jié)束語(yǔ)

在墻壁基準(zhǔn)探測(cè)方面，提出了一種基于超聲雷達(dá)的探測(cè)方法和數(shù)據(jù)擬合算法，并通過實(shí)驗(yàn)驗(yàn)證了可行性。對(duì)清潔機(jī)器人的地圖建立與路徑規(guī)劃進(jìn)行了研究，提出了一種路徑規(guī)劃方法。這種表示方法具有柵格地圖的所有優(yōu)點(diǎn)，但其需要的存儲(chǔ)空間大大減小，是一種比較理想的地圖表示方法。實(shí)驗(yàn)證明，新的地圖表示及路徑規(guī)劃方法減少了遍歷時(shí)間，提高了清潔效率。

［1］孫紅新，葉小嶺，胡 凱.基于超聲波的移動(dòng)機(jī)器人的同時(shí)定位和地圖構(gòu)建［J］.計(jì)算機(jī)測(cè)量與控制，2011，19（11）：2769-2771.

［2］王衛(wèi)華，陳衛(wèi)東，席裕庚.移動(dòng)機(jī)器人地圖創(chuàng)建中的不確定傳感信息處理［J］.自動(dòng)化學(xué)報(bào)，2003，29（2）：267-274.

［3］Tardos J D，Neira J，Newman P M，et al.Robust mapping and localization in indoor environments using sonar［J］.International Journal of Robotics Research，2002，21（4）：311-330.

［4］黃明登，肖曉明，蔡自興，等.機(jī)器人局部環(huán)境特征提取方法的研究［J］.計(jì)算機(jī)測(cè)量與控制，2007，15（2）：241-244.

［5］王衛(wèi)華，陳衛(wèi)東，席裕庚.基于不確定信息的移動(dòng)機(jī)器人地圖創(chuàng)建研究進(jìn)展［J］.機(jī)器人，2001，23（6）：563-568.

［6］趙一路.移動(dòng)機(jī)器人SLAM問題研究［D］.上海：復(fù)旦大學(xué)，2010.

［7］鮑菁丹.室內(nèi)未知環(huán)境下幾何地圖構(gòu)建及機(jī)器人定位方法研究［D］.天津：天津大學(xué)，2007.