陳超勇 熊禾根 陶 永* 鄒 遇 任 帆 江 山

(*武漢科技大學(xué)冶金裝備及其控制教育部重點(diǎn)實(shí)驗(yàn)室 武漢 430081)(**武漢科技大學(xué)機(jī)械傳動與制造工程湖北省重點(diǎn)實(shí)驗(yàn)室 武漢 430081)(***北京航空航天大學(xué)機(jī)械工程及自動化學(xué)院 北京 100191)(****北京航空航天大學(xué)生物醫(yī)學(xué)工程高精尖創(chuàng)新中心 北京 100191)

0 引 言

隨著科學(xué)技術(shù)的進(jìn)步和社會的發(fā)展，服務(wù)機(jī)器人逐漸進(jìn)入人們的生活。清掃機(jī)器人作為服務(wù)機(jī)器人的典型代表，能夠節(jié)省人力減輕勞動負(fù)擔(dān)，已成為智能機(jī)器人研究的熱點(diǎn)[1]。清掃機(jī)器人的核心技術(shù)是全覆蓋路徑規(guī)劃，通過多傳感器融合來估計(jì)自身位姿和檢測環(huán)境信息，然后尋找一條在工作區(qū)域內(nèi)從起始點(diǎn)到終點(diǎn)且經(jīng)過所有可達(dá)區(qū)域的連續(xù)無碰撞路徑，并要保證所規(guī)劃路徑的最優(yōu)性或合理性[2]。全覆蓋路徑規(guī)劃的研究在排雷機(jī)器人[3]、割草機(jī)[4]、水下機(jī)器人[5]、噴漆機(jī)器人[6]等方面同樣具有重大的應(yīng)用價值。

目前，全覆蓋路徑規(guī)劃技術(shù)總體可分為隨機(jī)式全覆蓋和規(guī)劃式全覆蓋[7]。隨機(jī)式全覆蓋即機(jī)器人直行，發(fā)生碰撞后隨機(jī)旋轉(zhuǎn)角度繼續(xù)直行，該方法雖然簡單，但是存在重復(fù)率高、效率低的缺點(diǎn)[8]。因此，國內(nèi)外的研究人員主要對規(guī)劃式全覆蓋進(jìn)行研究。Bochkarev等人[9]以高度最小為準(zhǔn)則將地圖分割成2部分，在每個區(qū)域內(nèi)利用往復(fù)運(yùn)動實(shí)現(xiàn)全覆蓋，但該方法僅作用于非凸多邊形區(qū)域。Gajjar等人[10]提出了一種基于改進(jìn)Boustrophedon單元分解法的全覆蓋路徑規(guī)劃算法，為柵格地圖定義了障礙代價函數(shù)，并隨著機(jī)器人的移動自動更新。Ma等人[11]提出了一種基于生物激勵神經(jīng)網(wǎng)絡(luò)的全覆蓋路徑規(guī)劃算法，采用高分辨率柵格定位、低分辨率柵格路徑規(guī)劃的策略，提高了算法的執(zhí)行效率。Wang等人[12]將蟻群算法和禁忌搜索算法進(jìn)行融合規(guī)劃最佳覆蓋路徑，取得了不錯的效果。為了快速逃離死區(qū)，曹翔等人[13]提出了一種基于柵格信度函數(shù)的全覆蓋規(guī)劃算法。針對掃地機(jī)器人局部區(qū)域覆蓋過程中存在遺漏區(qū)域未覆蓋的問題，李楷等人[14]提出了一種基于回溯法的全覆蓋規(guī)劃算法。

以上方法在已知的靜態(tài)環(huán)境中能發(fā)揮作用，但無法處理存在動態(tài)障礙的環(huán)境。最新的研究和應(yīng)用表明基于傳感器的方法對動態(tài)環(huán)境有著較好的效果。Li等人[15]結(jié)合有限狀態(tài)機(jī)(finite state machine, FSM)和滾動窗口法實(shí)現(xiàn)了在未知環(huán)境中的全覆蓋。Liu等人[16]提出了一種適用于多機(jī)器人系統(tǒng)的全覆蓋算法，該算法大大減少了區(qū)域覆蓋所需的時間。Zhou等人[17]提出了一種基于滾動窗口法和距離變換法的全覆蓋路徑規(guī)劃算法，使用傳感器檢測動態(tài)環(huán)境并更新局部柵格地圖，算法性能優(yōu)于生物激勵神經(jīng)網(wǎng)絡(luò)法。梁嘉俊等人[18]提出了一種改進(jìn)的勢場柵格算法，解決了人工勢場法中清潔機(jī)器人在障礙物附近震蕩而無法到達(dá)目標(biāo)點(diǎn)、臨近的障礙物之間不能發(fā)現(xiàn)路徑等問題。但是基于滾動窗口法的全覆蓋算法受到其窗口大小的限制，對于全局地圖的整體路徑規(guī)劃表現(xiàn)并不好。而在改進(jìn)勢場柵格法中，機(jī)器人面對復(fù)雜的環(huán)境容易陷入死區(qū)，影響覆蓋效率。

針對以上算法存在的問題，本文提出一種基于高效模板法與動態(tài)窗口法(dynamic window approach, DWA)相結(jié)合的服務(wù)機(jī)器人全覆蓋路徑規(guī)劃方法。高效模板法通過預(yù)定義的模板與先驗(yàn)地圖匹配，對整個環(huán)境進(jìn)行全覆蓋路徑規(guī)劃，同時引入回溯法解決高效模板法中機(jī)器人陷入死區(qū)的問題。運(yùn)用2D激光雷達(dá)傳感器信息與全局覆蓋路徑信息構(gòu)建環(huán)境檢測的動態(tài)窗口，遇到運(yùn)動障礙物時調(diào)用動態(tài)窗口法實(shí)現(xiàn)局部的路徑規(guī)劃。清掃服務(wù)機(jī)器人的實(shí)驗(yàn)結(jié)果表明該方法簡單有效，實(shí)現(xiàn)了動態(tài)環(huán)境下的全覆蓋路徑規(guī)劃。

1 柵格法環(huán)境建模

在清掃機(jī)器人開始清潔任務(wù)之前，必須建立其工作空間的地圖，即環(huán)境建模。柵格法可以很好地描述任何形式的障礙物，并且產(chǎn)生的二值信息特征便于計(jì)算機(jī)的存儲和更新，所以選用柵格法進(jìn)行環(huán)境建模具有顯著的優(yōu)勢。

清掃機(jī)器人的工作區(qū)域存在很多障礙物，在地圖中以灰色圖形表示障礙物。柵格法將機(jī)器人的工作空間分為大小相同的柵格，其中灰色柵格表示被障礙物占據(jù)，白色柵格表示自由空間，如圖1所示。

圖1 柵格化地圖

以2維的柵格地圖為例建立其數(shù)學(xué)模型。圖2顯示的是一個2維的柵格地圖，工作區(qū)域被分成了9個柵格，在該地圖中，機(jī)器人的移動方向不再是任意方向，而是用八叉樹所表示的8個行進(jìn)方向。每個柵格都存儲著一個值，這些值就組成了一個2維數(shù)組，此2維數(shù)組即工作環(huán)境的柵格模型。將障礙柵格賦值為1，可行域柵格賦值為0，則工作區(qū)域柵格模型如式(1)所示。

圖2 工作環(huán)境示意圖

(1)

2 基于回溯法的高效模板算法

模板算法是一種利用模板對靜態(tài)已知環(huán)境進(jìn)行遍歷的算法，因其簡單易實(shí)現(xiàn)的優(yōu)點(diǎn)被廣泛使用。1995年Hofner等人[19]提出模板算法，但文章中提出的模板算法不能應(yīng)用于有障礙的環(huán)境，只對于該文中列舉的一些環(huán)境具有實(shí)用性。隨后De Carvalho等人[20]對模板算法進(jìn)行改進(jìn)，成功應(yīng)用在有障礙的環(huán)境中，但是在一些特殊情況如遇到存在凹陷的障礙物時會陷入無法處理的死循環(huán)狀態(tài)。針對現(xiàn)有模板算法存在效率低、陷入死區(qū)無法逃離等問題，本文提出了一種基于回溯法的高效模板算法，極大地改善了原算法中存在的問題。

原模板算法共引入5個模板，TM(toward marker)，UT(u turn)，SS(side shift)，UTI(u turn interlaced)和SCOO(steer clear of obstacle)，如圖3所示。原模板算法簡單易實(shí)現(xiàn)，但也存在一些缺點(diǎn)。例如：5種模板之間沒有確立明顯的優(yōu)劣關(guān)系，導(dǎo)致在某些情形下會出現(xiàn)2種模板皆可使用的狀況，這使得算法陷入“僵局”；SS模板產(chǎn)生了較高的重復(fù)路徑；SCOO模板的避障方式容易使機(jī)器人遺漏掉障礙物的邊緣區(qū)域；對于機(jī)器人陷入死區(qū)沒有相應(yīng)的解決策略。

圖3 原模板示意圖

為了解決原模板算法中存在的問題，提高算法的效率，本文重新定義了4種高效模板并引入回溯法來逃離死區(qū)。4種高效模板分別是TU(toward up)、TD(toward down)、TL(toward left)以及TR(toward right)，如圖4所示。TU、TD、TL、TR模板表示上下左右4個方向，在進(jìn)行遍歷時，每個模板都有著確定的優(yōu)先級順序，本文制定的模板優(yōu)先級從高到低依次為TL、TD、TU、TR。相較于原模板算法，高效模板算法僅通過4種模板就可以完成地圖的全遍歷，不需要單獨(dú)的避障模板就可以避開中間障礙物，制定的優(yōu)先級策略更是保證了遍歷路徑的規(guī)律性和有效性。

圖4 高效模板示意圖

高效模板的執(zhí)行原理如下，對柵格地圖進(jìn)行區(qū)域遍歷之前，要確定遍歷的起始位置，一般選為地圖的邊界處。按照模板的優(yōu)先級首先判斷TL模板是否與當(dāng)前環(huán)境匹配，即查看Left方向是否存在障礙物，如果Left方向是自由區(qū)域，則執(zhí)行TL模板沿著Left方向移動直到被障礙物、已遍歷區(qū)域或環(huán)境邊界等阻擋。在環(huán)境與TL模板不匹配的情況下，會選擇次優(yōu)模板TD，若Down方向不存在障礙物，則執(zhí)行模板TD沿著Down方向進(jìn)行遍歷。同時在移動的過程中會時刻檢測TL與環(huán)境的匹配情況，若Left方向不存在障礙物，算法將會停止執(zhí)行TD模板轉(zhuǎn)而執(zhí)行TL模板。TU、TR模板的執(zhí)行則排在TL與TD之后。因此，高效模板在進(jìn)行地圖遍歷的過程中，將按照指定的優(yōu)先級順序進(jìn)行遍歷，并且高優(yōu)先級模板具有搶斷低優(yōu)先級模板執(zhí)行的能力。

上述提出的4種高效模板對于靜態(tài)區(qū)域的覆蓋雖然具有良好的性能，但是無法解決機(jī)器人陷入死區(qū)的問題。原模板算法中的SS模板僅能解決機(jī)器人陷入角落而側(cè)面有可行區(qū)域這一種情況，對于更為復(fù)雜的情況如周圍是由已遍歷區(qū)域和障礙物組成的死區(qū)，SS模板并不能逃離出去。因此，本文在高效模板中引入回溯法解決死區(qū)問題，以實(shí)現(xiàn)一次完成所有可行區(qū)域的全覆蓋。

回溯法中至關(guān)重要的一點(diǎn)是回溯列表的構(gòu)建。回溯列表是根據(jù)高效模板在執(zhí)行過程中積累的信息構(gòu)建的，在地圖的遍歷過程中，如果將地圖中的所有未遍歷自由柵格作為回溯點(diǎn)存儲到回溯列表中，將導(dǎo)致回溯列表占用的內(nèi)存極大。因此，本文為回溯列表中存放的回溯點(diǎn)制定了2條準(zhǔn)則。準(zhǔn)則1：距離最短原則?；厮蔹c(diǎn)與死區(qū)之間的距離要盡可能小，以此降低二者之間的重復(fù)覆蓋。準(zhǔn)則2：靠邊原則。回溯點(diǎn)應(yīng)位于未遍歷子區(qū)域的邊界角落，以及減少機(jī)器人到達(dá)該點(diǎn)后產(chǎn)生新的未遍歷子區(qū)域和回溯點(diǎn)。通過以上2條準(zhǔn)則，可以大大減少回溯列表中回溯點(diǎn)的數(shù)量。

回溯列表中存儲的是符合準(zhǔn)則1、2的未遍歷自由柵格，在機(jī)器人遍歷過程中，陷入到4種高效模板都無法處理的死區(qū)位置，則啟動回溯法來逃離死區(qū)，前往未遍歷區(qū)域繼續(xù)執(zhí)行區(qū)域覆蓋。選擇回溯列表中的哪一個回溯點(diǎn)作為未遍歷區(qū)域的起始點(diǎn)成為了影響回溯機(jī)制效率的主要因素。為了提高回溯法的效率，本文將機(jī)器人陷入的死區(qū)位置與回溯點(diǎn)之間的歐式距離作為貪心算法的選擇策略，采用貪心算法在回溯列表中選擇距離死區(qū)位置最近的回溯點(diǎn)，其數(shù)學(xué)描述為

(2)

(3)

其中，ns表示當(dāng)前狀態(tài)選擇的回溯點(diǎn)，lback表示回溯列表，n表示lback中的回溯點(diǎn)，ndp表示死區(qū)位置，d(n,ndp)表示死區(qū)位置與回溯點(diǎn)之間的歐式距離。

圖5和圖6分別是對原模板算法和高效模板算法進(jìn)行地圖全覆蓋的描述圖。在圖5所示的原模板算法中，UT模板的主導(dǎo)地位使得機(jī)器人的遍歷方向是沿著障礙物的邊緣向左側(cè)推進(jìn)，從而導(dǎo)致了機(jī)器人陷入了更靠近角落的死區(qū)A。而在圖6所示的高效模板算法中，機(jī)器人按照TL、TD、TU、TR的優(yōu)先級順序執(zhí)行模板，它傾向于沿著障礙物邊緣移動，避免陷入障礙物與墻壁邊界的角落位置，達(dá)到一個離未覆蓋區(qū)域更近的C點(diǎn)。最后，由SS模板沿邊移動到B點(diǎn)產(chǎn)生路徑AB，由回溯法從C點(diǎn)移動到B點(diǎn)產(chǎn)生路徑CB，而CB路徑長度明顯小于AB路徑長度，這意味著高效模板算法的重復(fù)覆蓋率更低，并且回溯法能夠處理任何位置的死區(qū)問題。因此，相較于原模板算法，使用高效模板算法對靜態(tài)區(qū)域進(jìn)行覆蓋的整體效率更高。

圖5 原模板算法

圖6 高效模板算法

在高效模板中引入回溯法可以使清掃機(jī)器人逃離死區(qū)，實(shí)現(xiàn)所有可行區(qū)域的全覆蓋，完整的基于回溯法的高效模板算法流程如下所示。

輸入：環(huán)境模型M。

輸出：一條遍歷所有自由柵格的全局路徑L。

步驟1柵格化環(huán)境模型，確定遍歷的初始位置；

步驟2依照地圖中的環(huán)境特征匹配對應(yīng)的高效模板進(jìn)行運(yùn)動，直到陷入無法逃離的死區(qū)位置；

步驟3根據(jù)環(huán)境信息更新回溯列表lback；

步驟4以死區(qū)位置和回溯點(diǎn)之間的歐式距離為貪心策略，利用貪心算法選擇未覆蓋區(qū)域的回溯點(diǎn)，并移動到該回溯點(diǎn)。

步驟5重復(fù)步驟2～4，直至回溯列表lback中元素為空，結(jié)束算法。

3 動態(tài)環(huán)境下全覆蓋路徑規(guī)劃方法

3.1 動態(tài)窗口法

動態(tài)窗口法(DWA)將局部路徑規(guī)劃問題描述為速度矢量空間上的約束優(yōu)化問題，是一種有效的局部避障算法[21]。DWA算法的主要思想是，根據(jù)機(jī)器人速度、加速度以及環(huán)境障礙約束，將速度矢量空間(v,ω)限制在一個動態(tài)窗口中，對此窗口內(nèi)的線速度v和角速度ω進(jìn)行多組采樣；然后，預(yù)測每個采樣速度在下一個周期內(nèi)對應(yīng)的機(jī)器人運(yùn)動軌跡；最后，引入一個評價函數(shù)對預(yù)測的運(yùn)動軌跡進(jìn)行評估，選擇得分最高的軌跡對應(yīng)的速度來控制機(jī)器人運(yùn)動。DWA算法的核心在于動態(tài)窗口的建立以及評價函數(shù)的選擇，下面根據(jù)清掃機(jī)器人自身性能和環(huán)境的約束建立速度采樣動態(tài)窗口。

清掃機(jī)器人受硬件性能的約束，其最大、最小速度都被限制在一定范圍內(nèi)，所以清掃機(jī)器人允許的極限速度矢量集合Vs為：

Vs={(v,ω)|v∈[vmin,vmax],

ω∈[ωmin,ωmax]} (4)

式中，vmax、vmin為清掃機(jī)器人的最大、最小線速度，ωmax、ωmin為清掃機(jī)器人的最大、最小角速度。

為了保護(hù)自身的安全，清掃機(jī)器人要與障礙物保持一定距離，并且在碰到障礙物前能夠及時停止，因此，清掃機(jī)器人的安全速度矢量集合Va可根據(jù)下式確定：

(5)

根據(jù)清掃機(jī)器人的最大加減速能力，可以計(jì)算出某時刻采樣速度在下一個周期Δt內(nèi)的變化范圍，用Vd表示：

(6)

將滿足Vs、Va、Vd的速度矢量空間構(gòu)建成動態(tài)窗口，可得到最終速度搜索空間Vr：

Vr=Vs∩Va∩Vd

(7)

對于采樣的速度空間Vr，每一組采樣速度對應(yīng)的預(yù)測軌跡都滿足約束條件，需引入評價函數(shù)評估軌跡的優(yōu)劣性。最優(yōu)軌跡應(yīng)使清掃機(jī)器人在避開障礙物的情況下更快更準(zhǔn)地向目標(biāo)移動，因此本文采用的評價函數(shù)如下：

G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)

+γ·vel(v,ω))

(8)

式中，heading(v,ω)為清掃機(jī)器人軌跡終點(diǎn)的航向與目標(biāo)點(diǎn)的夾角，dist(v,ω)為軌跡與障礙物的最小距離，vel(v,ω)為采樣速度，α、β、γ為權(quán)重比。

3.2 基于高效模板法與DWA算法的全覆蓋路徑規(guī)劃方法

前面提出了一種基于回溯法的高效模板算法，該算法能夠?qū)崿F(xiàn)已知地圖下所有可行區(qū)域的全覆蓋，但對于環(huán)境中未記錄或突然出現(xiàn)的障礙物沒有應(yīng)對策略。因此，本節(jié)將高效模板算法的靜態(tài)規(guī)劃能力與DWA算法的局部避障能力結(jié)合起來，提出一種適用動態(tài)環(huán)境下的全覆蓋路徑規(guī)劃方法。該方法可讓清掃機(jī)器人在已知環(huán)境中靜態(tài)規(guī)劃出全局覆蓋路徑后再進(jìn)行清掃，即使遇到環(huán)境中的動態(tài)障礙物也能順利避開，大大提高了清掃機(jī)器人的工作效率。

本節(jié)將結(jié)合如圖7所示的算法流程圖，詳細(xì)描述提出的全覆蓋路徑規(guī)劃方法的具體實(shí)現(xiàn)步驟。

圖7 基于高效模板法與DWA算法的全覆蓋路徑規(guī)劃流程圖

(1)建立清掃機(jī)器人工作區(qū)域的環(huán)境模型。利用激光雷達(dá)傳感器掃描環(huán)境，生成包含障礙物的代價地圖，然后根據(jù)機(jī)器人的實(shí)際尺寸確定劃分的柵格大小，用于全局的遍歷路徑規(guī)劃。

(2)高效模板算法對先驗(yàn)靜態(tài)地圖規(guī)劃出可行的全局覆蓋路徑。首先確定清掃機(jī)器人在柵格地圖的起始位置S，然后根據(jù)地圖信息匹配相應(yīng)的模板，并按照TL、TD、TU、TR的優(yōu)先級執(zhí)行模板，最后得到一條能夠遍歷所有自由柵格的全局總路徑L。

(3)總路徑分解。按照關(guān)鍵拐點(diǎn)將全局總路徑L分解成若干條子路徑集合P，這些子路徑皆由簡單的直線組成，易于清掃機(jī)器人的執(zhí)行，而子路徑的首末端點(diǎn)則組成了子目標(biāo)點(diǎn)集合Q。P和Q的數(shù)學(xué)描述分別如式(9)、(10)所示。

P={L1,L2,L3, …,Ln}

(9)

Q={O0,O1,O2, …,On}

(10)

其中n的值與全局總路徑L的長短有關(guān)，O0和On分別為全局總路徑L的起點(diǎn)和終點(diǎn)，當(dāng)清掃機(jī)器人到達(dá)On點(diǎn)時表示遍歷完成。

全局總路徑L分解得到子路徑集合P以及子目標(biāo)點(diǎn)集合Q，如圖8所示。

圖8 總路徑分解圖

(4)將子目標(biāo)集合Q中的第一個點(diǎn)O0設(shè)為清掃機(jī)器人的工作起始點(diǎn)，則O0的下一個點(diǎn)Oi作為機(jī)器人在當(dāng)前位置需要到達(dá)的子目標(biāo)點(diǎn)。在步驟2、3中，已通過高效模板算法得到全局遍歷路徑L并將其分解為子路徑集合P，因此集合P中的子路徑Li作為機(jī)器人在當(dāng)前位置規(guī)劃出的子路徑。以速度v0驅(qū)動機(jī)器人沿著子路徑方向移動，并根據(jù)機(jī)器人目前姿態(tài)和相關(guān)速度約束構(gòu)建的動態(tài)窗口。動態(tài)窗口是連續(xù)的，實(shí)時接收傳感器數(shù)據(jù)并在地圖上更新動態(tài)障礙物信息。當(dāng)沿著子路徑Li方向的速度v0不滿足動態(tài)窗口要求(即Li與障礙物將發(fā)生碰撞)時，DWA算法會重新在動態(tài)窗口中采樣，選擇最優(yōu)軌跡對應(yīng)的速度v驅(qū)動機(jī)器人進(jìn)行局部路徑規(guī)劃避開障礙物，其原理如圖9所示。

圖9 DWA局部路徑規(guī)劃避障圖

在圖9中，DWA算法的關(guān)鍵在于動態(tài)窗口實(shí)時檢測環(huán)境信息，并判斷子路徑Li在動態(tài)窗口中是否與動態(tài)障礙物發(fā)生碰撞，若是，則規(guī)劃局部路徑進(jìn)行避障，否則繼續(xù)沿著子路徑Li移動，這樣的優(yōu)點(diǎn)在于避免了用DWA算法進(jìn)行長距離的全局規(guī)劃，可完美發(fā)揮其局部規(guī)劃的能力。

(5)當(dāng)清掃機(jī)器人到達(dá)子目標(biāo)點(diǎn)Oi時，將當(dāng)前位置設(shè)為Oi，令i值加1，作為下一個子目標(biāo)點(diǎn)。

(6)判斷清掃機(jī)器人當(dāng)前位置是否已到達(dá)集合Q中最終目標(biāo)點(diǎn)On，若否，則繼續(xù)迭代移動到一下個子目標(biāo)點(diǎn)。若是，則算法結(jié)束，表示清掃機(jī)器人已經(jīng)完成了全覆蓋的清潔任務(wù)。

以上是本文所提的基于高效模板與DWA算法全覆蓋路徑規(guī)劃方法的全過程，大致可以分為全局靜態(tài)規(guī)劃與局部動態(tài)規(guī)劃。高效模板算法在進(jìn)行全遍歷的同時也解決了清掃機(jī)器人陷入死區(qū)的問題，生成的簡單路徑易于機(jī)器人執(zhí)行，提高了算法的效率。與DWA算法的融合有效改善了當(dāng)前全覆蓋算法對未知動態(tài)障礙物無法避障的能力，增加了機(jī)器人的安全性能。接下來將通過仿真和實(shí)驗(yàn)聯(lián)合驗(yàn)證本文所提全覆蓋算法的正確性和有效性。

4 仿真與實(shí)驗(yàn)

4.1 實(shí)驗(yàn)平臺

本文自主搭建的雙輪差速清掃機(jī)器人實(shí)驗(yàn)平臺如圖10所示。機(jī)器人底盤的左右主動輪分別由帶有減速器和編碼器的直流電機(jī)獨(dú)立驅(qū)動，構(gòu)成雙輪差分系統(tǒng)，前后再輔以2個從動萬向輪。在機(jī)器人平臺的中間層安裝有2維激光雷達(dá)傳感器、STM32控制板、電源模塊以及Intel NUC迷你主機(jī)，上層放置1塊用于人機(jī)交互的顯示屏。在本實(shí)驗(yàn)平臺中，上位機(jī)通過RS232串口與STM32控制板進(jìn)行通訊，控制左右電機(jī)的轉(zhuǎn)速并獲取編碼器的里程信息，而激光雷達(dá)的數(shù)據(jù)則直接傳給上位機(jī)處理。實(shí)驗(yàn)中清掃機(jī)器人規(guī)格及運(yùn)動參數(shù)如表1所示。

圖10 清掃機(jī)器人平臺

表1 清掃機(jī)器人參數(shù)配置表

4.2 全覆蓋路徑規(guī)劃仿真與實(shí)驗(yàn)分析

本文在如圖11所示的實(shí)驗(yàn)環(huán)境驗(yàn)證所提出的基于高效模板與DWA算法的全覆蓋路徑規(guī)劃方法。該實(shí)驗(yàn)場地由多條走廊和過道組成，可行區(qū)域面積約為230 m2，且整個場地被墻體隔斷成多個連通區(qū)域，具有代表性和普遍性。實(shí)驗(yàn)中將通過行人來模擬地圖中的高動態(tài)障礙物，以驗(yàn)證算法的局部動態(tài)性能。

圖11 多走廊和過道組成的大面積實(shí)驗(yàn)環(huán)境

本文使用安裝在機(jī)器人上的2維激光雷達(dá)傳感器掃描圖11中的環(huán)境，通過SLAM[22]技術(shù)建立環(huán)境的2維柵格地圖，如圖12所示。圖12中數(shù)字標(biāo)記的區(qū)域是門或電梯口等地方，限于機(jī)器人尺寸，本文將這7處區(qū)域從可行區(qū)域中除去，作為清掃機(jī)器人的工作空間。

圖12 實(shí)驗(yàn)場景的SLAM地圖

Stage是機(jī)器人操作系統(tǒng)(ROS)下的一個小型仿真器，能夠模擬激光雷達(dá)數(shù)據(jù)且具備物理引擎，因此，本文將stage模擬器作為仿真平臺。將SLAM地圖導(dǎo)入stage平臺中，配置機(jī)器人相關(guān)參數(shù)，得到仿真環(huán)境如圖13所示，圖中位于地圖下方的方塊表示機(jī)器人的初始位置，位于地圖中間的方塊模擬地圖中未記錄的動態(tài)障礙物。選取地圖中間的一個通道來驗(yàn)證算法的局部動態(tài)性能，假定模板算法與本文所提算法規(guī)劃的靜態(tài)覆蓋路徑同為MN。從圖14可以看出，當(dāng)清掃機(jī)器人前方突然出現(xiàn)地圖中未記錄的障礙物時，模板算法直接撞向障礙物，而本文提出的算法根據(jù)傳感器信息檢測到障礙物，依靠DWA算法重新規(guī)劃路徑，并調(diào)整速度避開該障礙物。

圖13 Stage仿真地圖

圖14 模板算法與本文算法動態(tài)性能對比圖

本文所提算法對靜態(tài)地圖規(guī)劃的全局覆蓋路徑如圖15所示。根據(jù)關(guān)鍵拐點(diǎn)將其拆解成若干條子目標(biāo)路徑，然后清掃機(jī)器人按照子目標(biāo)路徑進(jìn)行清掃任務(wù)，圖16是機(jī)器人工作時的覆蓋軌跡圖。圖16(b)、(d)分別模擬了行人從門內(nèi)、電梯口突然出現(xiàn)的情況，由圖16(c)、(e)可知機(jī)器人檢測到了行人，立即調(diào)用DWA算法規(guī)劃出了一條局部路徑來繞開行人，隨后回歸到全局子路徑上。圖16(f)中機(jī)器人陷入死區(qū)，此時回溯法起作用，選擇最近的回溯點(diǎn)并移動到該點(diǎn)，如圖16(g)所示。

圖15 靜態(tài)全局覆蓋路徑

圖16 清掃機(jī)器人覆蓋軌跡圖

通常情況下，覆蓋率和重復(fù)率是衡量全覆蓋算法性能的重要指標(biāo)。覆蓋率是機(jī)器人已覆蓋面積占總面積的百分比，重復(fù)率是指機(jī)器人執(zhí)行覆蓋任務(wù)時，重復(fù)覆蓋面積占總面積的百分比。表2是本文算法在仿真環(huán)境下的覆蓋實(shí)驗(yàn)結(jié)果，通過表中的數(shù)據(jù)可知在發(fā)生2次避障的情況下，本文所提的全覆蓋算法的覆蓋率高達(dá)98.3%，重復(fù)率僅為2.6%，該結(jié)果表明本文算法在高動態(tài)環(huán)境下的全覆蓋路徑規(guī)劃中有著良好的表現(xiàn)。

表2 清掃機(jī)器人覆蓋實(shí)驗(yàn)結(jié)果

以上是在stage仿真器中的測試結(jié)果，下面進(jìn)行實(shí)際場景中清掃機(jī)器人的工作演示。圖17是清掃機(jī)器人在圖11環(huán)境中的實(shí)驗(yàn)結(jié)果圖，圖中的覆蓋軌跡以及避障情況與仿真結(jié)果一致，再次驗(yàn)證本文所提算法的正確性和有效性。

圖17 清掃機(jī)器人實(shí)驗(yàn)結(jié)果圖

5 結(jié) 論

本文提出了基于高效模板算法和DWA算法的清掃服務(wù)機(jī)器人的全覆蓋路徑規(guī)劃方法。該方法首先提出基于回溯法的高效模板算法，解決了原模板算法中效率低、無法逃離死區(qū)的問題；然后利用高效模板算法對先驗(yàn)靜態(tài)地圖規(guī)劃全局覆蓋路徑，將全局覆蓋路徑分解成子目標(biāo)路徑后與傳感器信息融合構(gòu)成動態(tài)窗口，使機(jī)器人沿著子目標(biāo)路徑進(jìn)行清掃任務(wù)，窗口檢測到地圖上未記錄的動態(tài)障礙物時調(diào)用DWA算法規(guī)劃局部路徑進(jìn)行避障；最后，通過仿真和實(shí)驗(yàn)驗(yàn)證了本文算法在動態(tài)環(huán)境下全覆蓋路徑規(guī)劃的正確性和有效性。目前，本文提出的全覆蓋路徑規(guī)劃方法在逃離死區(qū)時會產(chǎn)生一定的重復(fù)路徑，以后會針對該問題進(jìn)一步提高算法的性能。