基于超聲雷達(dá)的室內(nèi)清潔機(jī)器人自主行走控制方法研究

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

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

0 引言

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

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

1.1 機(jī)器人運動控制建模

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

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

圖1 機(jī)器人模型

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

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

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

圖2 超聲波傳感器模型

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

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

2.1 墻面檢測

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

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

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

圖3 墻面探測流程

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

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

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

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

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

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

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

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

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

圖5 噪點去除后的探測結(jié)果

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

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

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

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

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

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

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

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

b.機(jī)器人與基準(zhǔn)的理論距離為Dj，測量的實際距離為dj，設(shè)置一個閾值ε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 障礙物的探測及繞行

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

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

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

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

表1 地圖表示方式特點比較

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

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

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

圖8 直線行程地圖的表示

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

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

圖9 清掃算法流程

在圖9中，有：

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

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

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

3 機(jī)器人運動實例

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

圖10 簡單實驗環(huán)境

圖11 障礙物情況下的控測

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

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

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

4 結(jié)束語

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

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

［2］王衛(wèi)華，陳衛(wèi)東，席裕庚.移動機(jī)器人地圖創(chuàng)建中的不確定傳感信息處理［J］.自動化學(xué)報，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ī)測量與控制，2007，15（2）：241-244.

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

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

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