山西中北大學(xué)機(jī)電工程學(xué)院　閆鵬杰　張亞

一種移動(dòng)機(jī)器人路徑規(guī)劃新方法

山西中北大學(xué)機(jī)電工程學(xué)院閆鵬杰張亞

路徑規(guī)劃是自主移動(dòng)機(jī)器人導(dǎo)航的基本任務(wù)，其精度取決于地圖構(gòu)建和定位。一些路徑規(guī)劃算法已經(jīng)應(yīng)用于精確的路勁規(guī)劃過程，比如Dijkstra算法，A*算法等。如果機(jī)器人的體積大于柵格地圖的柵格單元，傳統(tǒng)A*算法路徑規(guī)劃就會(huì)不精確，在這種情況下移動(dòng)機(jī)器人很難安全通過門和狹窄的通道。本文提出的新的路徑規(guī)劃方法在障礙物周圍虛擬增加障礙柵格單元，有效降低了發(fā)生碰撞的可能性。

路徑規(guī)劃；A*算法；自主移動(dòng)機(jī)器人

1　引言

路徑規(guī)劃是自主導(dǎo)航移動(dòng)機(jī)器人的關(guān)鍵技術(shù)之一。準(zhǔn)確、最優(yōu)的路徑規(guī)劃一方面需要機(jī)器人通過傳感器獲取準(zhǔn)確的環(huán)境信息和里程計(jì)信息；另一方面移動(dòng)機(jī)器人必須避開障礙物安全到達(dá)目標(biāo)點(diǎn)，并且路徑必須是最短的。最短路徑是節(jié)省時(shí)間方面的約束，最安全路徑是移動(dòng)機(jī)器人安全方面的約束［1-2］。

Dijkstra算法是一種移動(dòng)機(jī)器人最短路徑規(guī)劃算法。Peter E. Har等人在dijkstra算法基礎(chǔ)上提出了A*算法［3］，加快了搜索效率。但是A*一直存在一個(gè)問題：當(dāng)機(jī)器人的外形尺寸大于柵格地圖中柵格的尺寸時(shí)，當(dāng)機(jī)器人沿著規(guī)劃的最短路徑行走時(shí)存在撞到障礙物的可能性，這對(duì)機(jī)器人是不安全的。本文就是在傳統(tǒng)A*算法的基礎(chǔ)上，將機(jī)器人的外形尺寸因素引入到路徑搜索的過程中，從而尋找到最短路徑。

2　路徑規(guī)劃

2.1A*路徑規(guī)劃算法

A*算法應(yīng)用在柵格環(huán)境地圖中，是一種啟發(fā)式搜索算法，啟發(fā)函數(shù)h（n）是算法的核心，也是區(qū)別于Dijkstra算法的關(guān)鍵。啟發(fā)函數(shù)的作用是給出當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的估計(jì)距離，這個(gè)距離不考慮路徑中的障礙物。啟發(fā)函數(shù)使算法可有效得到更多關(guān)于環(huán)境地圖的信息。A*算法從起始節(jié)點(diǎn)開始搜索，并把起始節(jié)點(diǎn)周圍的節(jié)點(diǎn)加入到一個(gè)列表中，我們稱之為Open列表。Open列表中的節(jié)點(diǎn)按f（n）值由小到大排列，啟發(fā)函數(shù)h（n）是f （n）的組成部分。Open列表中f（n）值最小的節(jié)點(diǎn)就是下一次的擴(kuò)展節(jié)點(diǎn)，這個(gè)過程一直循環(huán)直到擴(kuò)展到目標(biāo)節(jié)點(diǎn)，這樣最終路徑就被搜索出來了。每個(gè)柵格節(jié)點(diǎn)的代價(jià)函數(shù)如下式：

f（n）=g（n）+h（n）（1）

上式中，假設(shè)當(dāng)前的搜索節(jié)點(diǎn)為n（xn，yn），f（n）是從起始節(jié)點(diǎn)S經(jīng)過當(dāng)前節(jié)點(diǎn)n到達(dá)目標(biāo)節(jié)點(diǎn)G的最小代價(jià)估計(jì)值，g （n）是從起始節(jié)點(diǎn)S到達(dá)當(dāng)前節(jié)點(diǎn)n的實(shí)際最小代價(jià)值，h（n）是從當(dāng)前節(jié)點(diǎn)n到達(dá)目標(biāo)點(diǎn)G的最小估計(jì)代價(jià)值。g（n）的值表示如式（2）：

當(dāng)prev（n）=Sk時(shí)，即當(dāng)前節(jié)點(diǎn)為Sk時(shí)：

式（4）中的h（n）值是從當(dāng)前節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)G的歐式距離。啟發(fā)函數(shù)h（n）的值也可以按Manhattan距離計(jì)算或是其他相對(duì)應(yīng)的距離計(jì)算方法。

通過以上計(jì)算方法和搜索策略就可以找到最短路徑。從目標(biāo)點(diǎn)依次向前追蹤到起始點(diǎn)S就是所求最短路徑。傳統(tǒng)A*算法的路徑規(guī)劃仿真結(jié)果如圖1所示。

2.2缺陷

仿真實(shí)驗(yàn)?zāi)M的是室內(nèi)的環(huán)境，包括一些虛擬的障礙物如門、墻、桌子等。占有柵格地圖是虛擬室內(nèi)環(huán)境的映射，障礙物柵格用‘1’表示，可行區(qū)域用‘0’表示。仿真實(shí)驗(yàn)中起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)在柵格地圖中的位置是事先給定的。起始位置設(shè)置為‘-1’目標(biāo)位置設(shè)置為‘-2’。按照公式（1）-（4）的計(jì)算方法，從起始點(diǎn)到目標(biāo)點(diǎn)用傳統(tǒng)A*算法進(jìn)行路徑搜索就會(huì)找到路徑。為了獲得更精確的最短路徑，必須增大柵格地圖的分辨率，即機(jī)器人的外形尺寸會(huì)大于柵格大小，這就會(huì)使傳統(tǒng)A*算法規(guī)劃出的路徑存在安全問題，如圖1所示，機(jī)器人行走路徑會(huì)很接近障礙物，這就會(huì)導(dǎo)致實(shí)際移動(dòng)機(jī)器人在行走中會(huì)撞上墻等障礙物。另一方面，如果門等通道是很狹窄的，機(jī)器人可能被夾住。

同理，式（1）中的h（n）估計(jì)值如式（4）：

圖1　傳統(tǒng)A*算法

2.3改進(jìn)方法

為了解決上述問題，本文對(duì)A*算法進(jìn)行了改進(jìn)。在新方法中，我們按照公式（5），在障礙物的周圍增加了虛擬的障礙柵格所有的障礙物柵格被定義為‘1’。虛擬障礙物的設(shè)置規(guī)則如下：

代價(jià)值被標(biāo)記為‘1’和‘2’的所有柵格都放在關(guān)閉列表中。關(guān)閉列表是機(jī)器人下一次搜索不能選的單元列表。代價(jià)值為‘2’的柵格單元就被認(rèn)定為虛擬障礙物。現(xiàn)在傳統(tǒng)的A*算法按式（5）進(jìn)行了改進(jìn)。改進(jìn)后的A*算法更適應(yīng)于機(jī)器人外型等于柵格大小的情況。

A*算法的過程：

A.生成可行節(jié)點(diǎn)和不可行節(jié)點(diǎn)的地圖。

B.創(chuàng)建“Open”和“Closed”列表，初始狀態(tài)都為空。

C.將所有的不可行節(jié)點(diǎn)放入“關(guān)閉”列表。

D.創(chuàng)建起始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)。將起始節(jié)點(diǎn)放在開啟列表中

E.如果開啟列表不是空的并且沒有找到路徑，按如下步驟：

a）將開啟列表中f（n）值最小的節(jié)點(diǎn)移出，這個(gè)節(jié)點(diǎn)就作為當(dāng)前節(jié)點(diǎn)。

b）將當(dāng)前節(jié)點(diǎn)的狀態(tài)設(shè)置為關(guān)閉狀態(tài)。

c）對(duì)于當(dāng)前節(jié)點(diǎn)周圍相鄰的8個(gè)節(jié)點(diǎn)做如下處理：

·如果該節(jié)點(diǎn)是地圖范圍內(nèi)可以行走的，并且不在關(guān)閉列表中，做如下處理：

·如果該節(jié)點(diǎn)不在Open列表中，把它放入Open列表中，并存儲(chǔ)其f、g、h值，同時(shí)將它的狀態(tài)設(shè)置為開啟狀態(tài)。

·如果該節(jié)點(diǎn)已經(jīng)在Open列表中（也就是說它的狀態(tài)為開啟狀態(tài)），重新計(jì)算它的g值，如果新的g值小于之前的g值，則將它的父輩節(jié)點(diǎn)改為當(dāng)前節(jié)點(diǎn)，并且在Open列表中存儲(chǔ)新的f 和g值。

d）當(dāng)目標(biāo)節(jié)點(diǎn)的狀態(tài)被放到開啟列表中時(shí)，意味著找到了路徑。

F.當(dāng)開啟列表為空時(shí)，意味著路徑搜索失敗。

以此類比，當(dāng)機(jī)器人的外形尺寸是柵格尺寸的2～3倍時(shí)，從安全角度考慮，我們將虛擬障礙物再向外擴(kuò)展一層，如圖3所示。

3　仿真實(shí)驗(yàn)

本實(shí)驗(yàn)假設(shè)環(huán)境地圖為已知的柵格地圖，地圖大小分為100×100格。柵格地圖的表示方法為：‘1’表示為障礙物（不可行節(jié)點(diǎn)），‘0’表示可通行區(qū)域（可行節(jié)點(diǎn)），起始位置都預(yù)先在地圖中設(shè)置。本文提出的改進(jìn)策略也在算法中實(shí)現(xiàn)。

傳統(tǒng)A*算法的搜索結(jié)果見圖1，機(jī)器人在移動(dòng)過程中會(huì)離障礙物很近。經(jīng)過公式（5）改進(jìn)，從圖2中可以很明顯的看到，改進(jìn)后的A*算法搜索出的路徑對(duì)機(jī)器人更安全。如果機(jī)器人外形尺寸變?yōu)闁鸥癯叽绲?～3倍時(shí)，繼續(xù)擴(kuò)大虛擬障礙物范圍，此時(shí)的搜索結(jié)果如圖3所示。對(duì)比圖1與圖3可以發(fā)現(xiàn)，改進(jìn)后的A*算法可以解決機(jī)器人在移動(dòng)過程中碰到障礙物的問題。

圖2　改進(jìn)A*算法：機(jī)器人尺寸等于柵格尺寸

圖3　改進(jìn)A*算法：機(jī)器人外形尺寸等于2～3倍柵格尺寸

4　結(jié)論

為了避免機(jī)器人與障礙物發(fā)生碰撞，在傳統(tǒng)算法基礎(chǔ)上改進(jìn)的A*算法通過添加虛擬障礙物來適應(yīng)不同外形尺寸的機(jī)器人，仿真實(shí)驗(yàn)結(jié)果驗(yàn)證了新方法的有效性。改進(jìn)后的A*算法搜索出的路徑或許不是最短的，但是對(duì)于機(jī)器人來說會(huì)更加安全。

［1］王麗，徐德民.移動(dòng)機(jī)器人路徑規(guī)劃方法研究［J］.西安：西北工業(yè)大學(xué)航海學(xué)院，2007.

［2］黎紅.自主移動(dòng)機(jī)器人路徑規(guī)劃中的主要方法［J］.中國(guó)電力教育，2010（S1）：814-816.

［3］王殿君.基于改進(jìn)A*算法的室內(nèi)移動(dòng)機(jī)器人路徑規(guī)劃［J］.清華大學(xué)學(xué)報(bào)，2012，52（8）：1085-1089.

［4］譚寶成，王培.A＊路徑規(guī)劃算法的改進(jìn)及實(shí)現(xiàn)［J］.西安工業(yè)大學(xué)學(xué)報(bào)，2012，32（04）：325-328.

閆鵬杰，1990出生，男，山西運(yùn)城人，在讀碩士研究生，機(jī)械電子工程。