基于高效模板法與動(dòng)態(tài)窗口法的服務(wù)機(jī)器人全覆蓋路徑規(guī)劃方法①

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

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

0 引 言

隨著科學(xué)技術(shù)的進(jìn)步和社會(huì)的發(fā)展，服務(wù)機(jī)器人逐漸進(jìn)入人們的生活。清掃機(jī)器人作為服務(wù)機(jī)器人的典型代表，能夠節(jié)省人力減輕勞動(dòng)負(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)用價(jià)值。

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

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

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

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

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

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

圖1 柵格化地圖

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

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

(1)

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

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

原模板算法共引入5個(gè)模板，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)致在某些情形下會(huì)出現(xiàn)2種模板皆可使用的狀況，這使得算法陷入“僵局”；SS模板產(chǎn)生了較高的重復(fù)路徑；SCOO模板的避障方式容易使機(jī)器人遺漏掉障礙物的邊緣區(qū)域；對(duì)于機(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個(gè)方向，在進(jìn)行遍歷時(shí)，每個(gè)模板都有著確定的優(yōu)先級(jí)順序，本文制定的模板優(yōu)先級(jí)從高到低依次為TL、TD、TU、TR。相較于原模板算法，高效模板算法僅通過4種模板就可以完成地圖的全遍歷，不需要單獨(dú)的避障模板就可以避開中間障礙物，制定的優(yōu)先級(jí)策略更是保證了遍歷路徑的規(guī)律性和有效性。

圖4 高效模板示意圖

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

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

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

回溯列表中存儲(chǔ)的是符合準(zhǔn)則1、2的未遍歷自由柵格，在機(jī)器人遍歷過程中，陷入到4種高效模板都無法處理的死區(qū)位置，則啟動(dòng)回溯法來逃離死區(qū)，前往未遍歷區(qū)域繼續(xù)執(zhí)行區(qū)域覆蓋。選擇回溯列表中的哪一個(gè)回溯點(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分別是對(duì)原模板算法和高效模板算法進(jìn)行地圖全覆蓋的描述圖。在圖5所示的原模板算法中，UT模板的主導(dǎo)地位使得機(jī)器人的遍歷方向是沿著障礙物的邊緣向左側(cè)推進(jìn)，從而導(dǎo)致了機(jī)器人陷入了更靠近角落的死區(qū)A。而在圖6所示的高效模板算法中，機(jī)器人按照TL、TD、TU、TR的優(yōu)先級(jí)順序執(zhí)行模板，它傾向于沿著障礙物邊緣移動(dòng)，避免陷入障礙物與墻壁邊界的角落位置，達(dá)到一個(gè)離未覆蓋區(qū)域更近的C點(diǎn)。最后，由SS模板沿邊移動(dòng)到B點(diǎn)產(chǎn)生路徑AB，由回溯法從C點(diǎn)移動(dòng)到B點(diǎn)產(chǎn)生路徑CB，而CB路徑長度明顯小于AB路徑長度，這意味著高效模板算法的重復(fù)覆蓋率更低，并且回溯法能夠處理任何位置的死區(qū)問題。因此，相較于原模板算法，使用高效模板算法對(duì)靜態(tài)區(qū)域進(jìn)行覆蓋的整體效率更高。

圖5 原模板算法

圖6 高效模板算法

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

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

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

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

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

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

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

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

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

3.1 動(dòng)態(tài)窗口法

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

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

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

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

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

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

(5)

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

(6)

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

Vr=Vs∩Va∩Vd

(7)

對(duì)于采樣的速度空間Vr，每一組采樣速度對(duì)應(yīng)的預(yù)測軌跡都滿足約束條件，需引入評(píng)價(jià)函數(shù)評(píng)估軌跡的優(yōu)劣性。最優(yōu)軌跡應(yīng)使清掃機(jī)器人在避開障礙物的情況下更快更準(zhǔn)地向目標(biāo)移動(dòng)，因此本文采用的評(píng)價(jià)函數(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ū)域的全覆蓋，但對(duì)于環(huán)境中未記錄或突然出現(xiàn)的障礙物沒有應(yīng)對(duì)策略。因此，本節(jié)將高效模板算法的靜態(tài)規(guī)劃能力與DWA算法的局部避障能力結(jié)合起來，提出一種適用動(dòng)態(tài)環(huán)境下的全覆蓋路徑規(guī)劃方法。該方法可讓清掃機(jī)器人在已知環(huán)境中靜態(tài)規(guī)劃出全局覆蓋路徑后再進(jìn)行清掃，即使遇到環(huán)境中的動(dòng)態(tài)障礙物也能順利避開，大大提高了清掃機(jī)器人的工作效率。

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

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

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

(2)高效模板算法對(duì)先驗(yàn)靜態(tài)地圖規(guī)劃出可行的全局覆蓋路徑。首先確定清掃機(jī)器人在柵格地圖的起始位置S，然后根據(jù)地圖信息匹配相應(yīng)的模板，并按照TL、TD、TU、TR的優(yōu)先級(jí)執(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)時(shí)表示遍歷完成。

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

圖8 總路徑分解圖

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

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

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

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

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

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

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

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

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

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

表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，且整個(gè)場地被墻體隔斷成多個(gè)連通區(qū)域，具有代表性和普遍性。實(shí)驗(yàn)中將通過行人來模擬地圖中的高動(dòng)態(tài)障礙物，以驗(yàn)證算法的局部動(dòng)態(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)下的一個(gè)小型仿真器，能夠模擬激光雷達(dá)數(shù)據(jù)且具備物理引擎，因此，本文將stage模擬器作為仿真平臺(tái)。將SLAM地圖導(dǎo)入stage平臺(tái)中，配置機(jī)器人相關(guān)參數(shù)，得到仿真環(huán)境如圖13所示，圖中位于地圖下方的方塊表示機(jī)器人的初始位置，位于地圖中間的方塊模擬地圖中未記錄的動(dòng)態(tài)障礙物。選取地圖中間的一個(gè)通道來驗(yàn)證算法的局部動(dòng)態(tài)性能，假定模板算法與本文所提算法規(guī)劃的靜態(tài)覆蓋路徑同為MN。從圖14可以看出，當(dāng)清掃機(jī)器人前方突然出現(xiàn)地圖中未記錄的障礙物時(shí)，模板算法直接撞向障礙物，而本文提出的算法根據(jù)傳感器信息檢測到障礙物，依靠DWA算法重新規(guī)劃路徑，并調(diào)整速度避開該障礙物。

圖13 Stage仿真地圖

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

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

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

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

通常情況下，覆蓋率和重復(fù)率是衡量全覆蓋算法性能的重要指標(biāo)。覆蓋率是機(jī)器人已覆蓋面積占總面積的百分比，重復(fù)率是指機(jī)器人執(zhí)行覆蓋任務(wù)時(shí)，重復(fù)覆蓋面積占總面積的百分比。表2是本文算法在仿真環(huán)境下的覆蓋實(shí)驗(yàn)結(jié)果，通過表中的數(shù)據(jù)可知在發(fā)生2次避障的情況下，本文所提的全覆蓋算法的覆蓋率高達(dá)98.3%，重復(fù)率僅為2.6%，該結(jié)果表明本文算法在高動(dòng)態(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ū)的問題；然后利用高效模板算法對(duì)先驗(yàn)靜態(tài)地圖規(guī)劃全局覆蓋路徑，將全局覆蓋路徑分解成子目標(biāo)路徑后與傳感器信息融合構(gòu)成動(dòng)態(tài)窗口，使機(jī)器人沿著子目標(biāo)路徑進(jìn)行清掃任務(wù)，窗口檢測到地圖上未記錄的動(dòng)態(tài)障礙物時(shí)調(diào)用DWA算法規(guī)劃局部路徑進(jìn)行避障；最后，通過仿真和實(shí)驗(yàn)驗(yàn)證了本文算法在動(dòng)態(tài)環(huán)境下全覆蓋路徑規(guī)劃的正確性和有效性。目前，本文提出的全覆蓋路徑規(guī)劃方法在逃離死區(qū)時(shí)會(huì)產(chǎn)生一定的重復(fù)路徑，以后會(huì)針對(duì)該問題進(jìn)一步提高算法的性能。