基于一種改進(jìn)A*算法的智能輪椅路徑規(guī)劃研究

田志宏，韓雪潔，田 碩，王家旺

(1.天津科技大學(xué)電子信息與自動(dòng)化學(xué)院，天津 300222；2.展訊通信(天津)有限公司，天津 300000)

隨著人口老齡化問(wèn)題的加重，需要看護(hù)的老年人數(shù)量逐漸增加，已成為現(xiàn)階段亟待解決的社會(huì)問(wèn)題.隨著科技的發(fā)展，先進(jìn)的代步工具正在逐步改善老年人的生活質(zhì)量，智能輪椅[1]作為一種服務(wù)機(jī)器人，已經(jīng)被國(guó)家列為新時(shí)代康復(fù)工程的主要項(xiàng)目之一.路徑規(guī)劃算法作為智能輪椅開(kāi)發(fā)中的核心環(huán)節(jié)，發(fā)揮極其重要的作用，常用的路徑搜索算法包括深度優(yōu)先搜索(depth-first search，DFS)算法、廣度優(yōu)先搜索(breadth-first search，BFS)算法以及最佳優(yōu)先搜索(best-first search)算法.深度優(yōu)先搜索算法和廣度優(yōu)先搜索算法屬于無(wú)信息搜索的算法，一般只適用于求解比較簡(jiǎn)單的問(wèn)題，經(jīng)典路徑規(guī)劃算法之一的迪杰斯特拉算法(Dijkstra’s algorithm)屬于廣度優(yōu)先算法.最佳優(yōu)先搜索算法是一種啟發(fā)式搜索算法，它是廣度優(yōu)先搜索算法的一種改進(jìn).A*(A star)算法是一種最佳優(yōu)先搜索算法，是目前經(jīng)典、流行的尋路算法之一，廣泛應(yīng)用于各種需要尋路的場(chǎng)景中.A*算法在迪杰斯特拉算法的基礎(chǔ)上引入了當(dāng)前節(jié)點(diǎn)的代價(jià)函數(shù)，并對(duì)代價(jià)函數(shù)加以限制，避免陷入局部最優(yōu)[2].在大規(guī)模地圖中進(jìn)行路徑尋優(yōu)時(shí)，A*算法比迪杰斯特拉算法更加精巧、高效[3]，但由于其為了尋找更短路徑往往會(huì)犧牲路徑的平滑度、搜索時(shí)間，因此智能輪椅在面對(duì)復(fù)雜的家居運(yùn)行場(chǎng)景時(shí)，應(yīng)對(duì) A*算法加以改進(jìn)，減少路徑的拐點(diǎn)和搜索時(shí)間，使輪椅的行駛路徑更加平滑，轉(zhuǎn)彎半徑更小.

1 改進(jìn)A*算法

1.1 A*算法的代價(jià)函數(shù)

A*算法在路徑搜索過(guò)程中，通過(guò)代價(jià)函數(shù)計(jì)算當(dāng)前節(jié)點(diǎn)向周圍拓展節(jié)點(diǎn)的代價(jià)值，然后選擇代價(jià)值最小的節(jié)點(diǎn)作為下一節(jié)點(diǎn)，重復(fù)此過(guò)程使其沿著目標(biāo)點(diǎn)方向前進(jìn).因此，代價(jià)函數(shù)的選擇會(huì)決定A*算法的效率和最終得到的路徑.

A*算法的代價(jià)函數(shù)[4]表示為

其中：n表示柵格地圖中當(dāng)前節(jié)點(diǎn)周圍任意方向的拓展節(jié)點(diǎn)；g (n)表示當(dāng)前節(jié)點(diǎn)到 n節(jié)點(diǎn)所需要的實(shí)際代價(jià)；h(n)為啟發(fā)函數(shù)，表示節(jié)點(diǎn)n到目標(biāo)點(diǎn)所需要的實(shí)際代價(jià).通過(guò)啟發(fā)函數(shù)能夠避免一些無(wú)關(guān)節(jié)點(diǎn)的搜索，h(n)在實(shí)際路徑規(guī)劃中有多種可選擇的函數(shù)，應(yīng)根據(jù)實(shí)際情況選擇相應(yīng)的啟發(fā)函數(shù)[5].本文選擇曼哈頓距離函數(shù)作為啟發(fā)函數(shù)，設(shè)任意節(jié)點(diǎn)n在柵格地圖中的坐標(biāo)為(xn, yn)，目標(biāo)點(diǎn)(goal)在柵格地圖中的坐標(biāo)為(xg,yg).可得到對(duì)應(yīng)的啟發(fā)函數(shù)為

1.2 A*算法流程

首先創(chuàng)建兩個(gè)集合open list和close list，A*算法核心是從 open list中選擇最優(yōu)〔 f (n)值小的最優(yōu)，或 f (n)值相同時(shí) h (n)值小的最優(yōu)〕的節(jié)點(diǎn)到 close list集中；然后將其后繼節(jié)點(diǎn)放入open list中，重復(fù)操作選取最優(yōu)節(jié)點(diǎn)，直至到達(dá)目標(biāo)點(diǎn)或者open list為空時(shí)停止.A*算法流程圖如圖1所示.

圖1 A*算法流程圖Fig.1 A* algorithm flow chart

1.3 代價(jià)函數(shù)的改進(jìn)

傳統(tǒng) A*算法的代價(jià)函數(shù)〔式(1)〕存在一些不足.在柵格地圖中存在一些 f (n)值比較小的節(jié)點(diǎn)，h(n)值在其中占比過(guò)小，導(dǎo)致算法的方向性不明確，因此可以將代價(jià)函數(shù)改進(jìn)為

其中α為權(quán)重，α＞1.α可以影響代價(jià)函數(shù)的值，通過(guò)改變?chǔ)恋拇笮∵M(jìn)而改變路徑搜索過(guò)程中 h (n)對(duì)A*算法的影響.α越大，h(n)在代價(jià)函數(shù)中所占的比重就越大，路徑搜索需要搜索的節(jié)點(diǎn)數(shù)量越少，用時(shí)也越短.但是，α越大，其搜索方式越趨向于 BFS算法，對(duì)于某些特定的場(chǎng)景會(huì)出現(xiàn)搜索路徑不是最優(yōu)路徑的情況，因此需要根據(jù)不同的場(chǎng)景選取特定的α值.使用 MATLAB仿真軟件建立柵格地圖并仿真，不同α的路徑曲線對(duì)比見(jiàn)表1.

表1 不同α 的路徑曲線對(duì)比Tab.1 Comparison of path curves of differentα

由于權(quán)重α影響 h (n)在代價(jià)函數(shù)中所占的比重，這使動(dòng)態(tài)衡量啟發(fā)式 A*算法在路徑搜索過(guò)程中具有更強(qiáng)的方向性，節(jié)點(diǎn)的搜索更具有選擇性.因此，動(dòng)態(tài)衡量啟發(fā)式 A*算法搜索節(jié)點(diǎn)的數(shù)量遠(yuǎn)遠(yuǎn)小于傳統(tǒng)A*算法，提高了算法的運(yùn)行效率.

1.4 引入人工勢(shì)場(chǎng)改進(jìn)A*算法

由于 A*算法是在路網(wǎng)中求解最短路徑的算法，在搜尋路徑的過(guò)程中，往往會(huì)為了尋找更短路徑而犧牲路徑的平滑度.智能輪椅在移動(dòng)過(guò)程中，如果轉(zhuǎn)彎的次數(shù)過(guò)多則會(huì)減小智能輪椅的移動(dòng)效率，同時(shí)也會(huì)影響使用者的舒適程度.因此，應(yīng)當(dāng)盡可能減少搜索路徑的拐點(diǎn).拐點(diǎn)的出現(xiàn)是由于在路徑搜索過(guò)程中，算法不知道障礙物的位置，因此當(dāng)前節(jié)點(diǎn)的選取總是選擇代價(jià)值最小的節(jié)點(diǎn)，這也導(dǎo)致出現(xiàn)了當(dāng)前節(jié)點(diǎn)選擇不合理的情況.

通過(guò) A*算法的規(guī)則可以看出，修改 g (n)值可以對(duì)路徑進(jìn)行轉(zhuǎn)向約束，當(dāng)移動(dòng)方向發(fā)生變化時(shí)，增加額外的 g (n)值來(lái)提高路徑的轉(zhuǎn)向成本.采用表1中的仿真場(chǎng)景對(duì)不同 g (n)值進(jìn)行仿真，不同 g (n)值對(duì)路徑曲線的影響如圖2所示.

圖2 不同 g (n)值對(duì)路徑曲線的影響Fig.2 Effect of different g(n) values on the path curve

由圖2(a)可以看出，g (n)值的增大會(huì)使路徑更平滑，但是路徑的拐點(diǎn)數(shù)達(dá)到最小值后并不會(huì)隨著g(n)值的增大而遞減.圖2(b)為 g (n)值對(duì)搜索時(shí)間的影響曲線圖，g (n)值越大，搜索路徑需要的時(shí)間越長(zhǎng)，搜索的節(jié)點(diǎn)數(shù)越多，所以 g (n)值不可一味地增大，需要根據(jù)不同的環(huán)境需求進(jìn)行修改.對(duì)于智能輪椅，在常規(guī)情況下需要保證安全性與舒適性，所以適當(dāng)增大 g (n)值可以獲取更平滑的路徑.

智能輪椅行走的環(huán)境復(fù)雜多變，每種環(huán)境下人為調(diào)整 g (n)參數(shù)使路徑達(dá)到最優(yōu)并不現(xiàn)實(shí)，因此將人工勢(shì)場(chǎng)法的概念引入A*算法.在人工勢(shì)場(chǎng)法中，障礙物會(huì)對(duì)智能輪椅產(chǎn)生斥力，通過(guò)對(duì)斥力的分析就可以使算法提前得到障礙物的方位.

傳統(tǒng)人工勢(shì)場(chǎng)法的計(jì)算方式如下：設(shè)智能輪椅在柵格地圖中的位置為 X，則目標(biāo)位置(Xg)與智能輪椅之間的引力場(chǎng)為

引力場(chǎng)所產(chǎn)生的引力向量為

斥力場(chǎng)定義為[6]

斥力場(chǎng)所產(chǎn)生的斥力向量為

其中：d0為障礙物的作用距離，當(dāng)與障礙物的距離大于d0時(shí)，障礙物將不會(huì)產(chǎn)生影響；X -X0為智能輪椅到障礙物的距離；Ka和Kr分別為引力系數(shù)和斥力系數(shù).智能輪椅在環(huán)境中受到的總勢(shì)場(chǎng)為

對(duì)于 A*算法，只需要使智能輪椅知道障礙物的方位和斥力場(chǎng)，通過(guò)計(jì)算斥力場(chǎng)的值便可以在 A*算法拓展節(jié)點(diǎn)時(shí)得出斥力對(duì) g (n)的作用.當(dāng)拓展方向與障礙物的斥力方向垂直時(shí)，其斥力并未作用在拓展節(jié)點(diǎn)上，因此 g (n)值的大小不受斥力的影響，此時(shí)可看作智能輪椅與障礙物同向前進(jìn).當(dāng)拓展方向與障礙物方向相反時(shí)，斥力同樣不作用在 g (n)上，節(jié)點(diǎn)的拓展是算法中的正常拓展，此時(shí)智能輪椅遠(yuǎn)離障礙物.當(dāng)拓展方向靠近障礙物方向時(shí)，此時(shí)智能輪椅有靠近障礙物的趨勢(shì)，這也無(wú)形中增加了拐點(diǎn)出現(xiàn)的概率；此時(shí)斥力作用在 g (n)上，受到斥力的影響，g (n)的值增加，因此這個(gè)節(jié)點(diǎn)的代價(jià)值增加，這會(huì)降低此節(jié)點(diǎn)被選中的概率.改進(jìn)的A*算法的代價(jià)函數(shù)為

其中：X -X0表示當(dāng)前節(jié)點(diǎn)的位置與障礙物之間的距離，Xt- X0表示拓展節(jié)點(diǎn)的位置與障礙物之間的距離.當(dāng) X - X0＞ Xt-X0時(shí)，即拓展方向朝著障礙物方向靠近，此時(shí)障礙物產(chǎn)生的斥力作用于 g (n)上，這個(gè)節(jié)點(diǎn)的代價(jià)值也隨之增加；當(dāng) X - X0≤ Xt-X0時(shí)，該節(jié)點(diǎn)的代價(jià)值不受障礙物斥力的影響.

2 智能輪椅的模型設(shè)計(jì)

Webots仿真平臺(tái)可用于多個(gè)領(lǐng)域，包括工業(yè)手臂、多足機(jī)器人、模塊化機(jī)器人、無(wú)人駕駛汽車、無(wú)人機(jī)、水下機(jī)器人、履帶式機(jī)器人和航空航天器等.Webots可以在 Windows、Linux、Mac OS系統(tǒng)上使用.使用 Webots創(chuàng)建機(jī)器人原型，開(kāi)發(fā)和測(cè)試控制算法，并能夠通過(guò) 3D動(dòng)畫形象生動(dòng)地展示出最終結(jié)果[8].智能輪椅的開(kāi)發(fā)比較復(fù)雜、難度較大，采用Webots仿真平臺(tái)對(duì)智能輪椅進(jìn)行建模與仿真可以降低開(kāi)發(fā)難度，縮短開(kāi)發(fā)進(jìn)程.

2.1 智能輪椅的底層設(shè)計(jì)

對(duì)底層運(yùn)動(dòng)模塊進(jìn)行建模，在 Webots仿真窗口通過(guò)場(chǎng)景樹對(duì)運(yùn)動(dòng)模塊進(jìn)行搭建.本研究的智能輪椅運(yùn)動(dòng)底層采用 0.5m×0.1m×0.6m 的長(zhǎng)方體，后側(cè)驅(qū)動(dòng)輪采用直徑0.26m、高度0.05m的圓柱體，前側(cè)萬(wàn)向輪采用直徑0.15m、高度0.02m的圓柱體.智能輪椅的底層結(jié)構(gòu)如圖3所示，wheel1和wheel2為后側(cè)驅(qū)動(dòng)輪，wheel3和wheel4為導(dǎo)向輪.

圖3 智能輪椅的底層結(jié)構(gòu)Fig.3 Bottom structure of intelligent wheelchair

2.2 智能輪椅的模型建立

在 Webots仿真平臺(tái)中，模型的主體結(jié)構(gòu)是由多個(gè) solid(固體)節(jié)點(diǎn)組成的樹狀結(jié)構(gòu)，其根節(jié)點(diǎn)為robot節(jié)點(diǎn).首先建立一個(gè) robot節(jié)點(diǎn)，然后對(duì)節(jié)點(diǎn)的physics(物理特性)設(shè)置質(zhì)量和重心位置，從而使物理引擎賦予車身結(jié)構(gòu)一個(gè)現(xiàn)實(shí)的重力.隨后在 robot節(jié)點(diǎn)的children(子)節(jié)點(diǎn)中添加solid節(jié)點(diǎn)，在該模型中智能輪椅的 solid節(jié)點(diǎn)共有 5個(gè)，分別為 body(車身)、兩個(gè)驅(qū)動(dòng)輪和兩個(gè)導(dǎo)向輪.

超聲波傳感器和紅外傳感器用于感知solid節(jié)點(diǎn)的 BoundingObject(碰撞邊界)，獲取與 solid節(jié)點(diǎn)之間的距離，并返回函數(shù)值.智能輪椅的超聲波傳感器和紅外傳感器通過(guò) DistanceSensor(距離傳感器節(jié)點(diǎn))進(jìn)行建模，該節(jié)點(diǎn)可用于模擬紅外傳感器、超聲波傳感器和激光測(cè)距儀.超聲波傳感器只接收射線發(fā)射角度大于 22.5°的射線.當(dāng)智能輪椅距離障礙物 0m時(shí)，超聲波傳感器返回值為 1000；當(dāng)智能輪椅距離障礙物 0.5m 時(shí)，超聲波傳感器返回值為 200；當(dāng)智能輪椅距離障礙物 1m時(shí)，超聲波傳感器返回值為100.未直接指定的距離將直接線性內(nèi)插.本文設(shè)計(jì)的智能輪椅有6個(gè)超聲波傳感器和6個(gè)紅外傳感器，左右腳踏板兩邊各安裝2個(gè)超聲波傳感器，用于檢測(cè)輪椅前側(cè)的障礙物；智能輪椅的上部左右兩邊各安裝1個(gè)超聲波傳感器，用于檢測(cè)智能輪椅上方的障礙物；紅外傳感器安裝在與超聲波傳感器相同的位置，當(dāng)超聲波傳感器進(jìn)入測(cè)量盲區(qū)時(shí)，與其互補(bǔ)使用，避免發(fā)生碰撞.

2.3 室內(nèi)環(huán)境建模

按照一般室內(nèi)場(chǎng)景建立了20m×15m的仿真環(huán)境.對(duì)仿真環(huán)境的基本參數(shù)設(shè)置完成后，需要對(duì)家居場(chǎng)景進(jìn)行構(gòu)建.Webots軟件內(nèi)集成了大量的對(duì)象集合，在其中選擇了門、床、沙發(fā)、桌子等，組成一般的家居場(chǎng)景.

3 實(shí)驗(yàn)結(jié)果與分析

3.1 A*算法的改進(jìn)效果

為了更好地展示改進(jìn)后的 A*算法在室內(nèi)環(huán)境中路徑搜索的效果，同時(shí)與傳統(tǒng) A*算法進(jìn)行對(duì)比，在MATLAB軟件中采用建好的室內(nèi)環(huán)境柵格地圖進(jìn)行路徑搜索測(cè)試，改進(jìn)前后的效果對(duì)比見(jiàn)表2，表中翠綠色節(jié)點(diǎn)為起始點(diǎn)，紅色節(jié)點(diǎn)為目標(biāo)點(diǎn)，曲線周圍淡綠色節(jié)點(diǎn)為路徑搜索過(guò)的節(jié)點(diǎn).測(cè)試結(jié)果表明，改進(jìn)的 A*算法比傳統(tǒng)的 A*算法在路徑規(guī)劃中的拐點(diǎn)更少，同時(shí)選擇合適的值也可以提高算法的運(yùn)行效率.

表2 改進(jìn)前后的效果對(duì)比Tab.2 Comparison of before and after improvement

3.2 輪椅避障

當(dāng)傳感器檢測(cè)到有障礙物存在時(shí)，智能輪椅根據(jù)傳感器返回的數(shù)值判斷障礙物的方位，依據(jù)障礙物存在的方位改變移動(dòng)方向.在現(xiàn)實(shí)生活中，障礙物并不都是固定的，智能輪椅前行時(shí)可能會(huì)遇到移動(dòng)障礙物，所以在此次仿真中除了設(shè)置固定的家居環(huán)境外，還設(shè)置了可移動(dòng)的障礙物，以驗(yàn)證智能輪椅的動(dòng)態(tài)避障效果.圖4為智能輪椅避開(kāi)移動(dòng)障礙物的運(yùn)行效果圖.

圖4 智能輪椅避開(kāi)移動(dòng)障礙物的運(yùn)行效果圖Fig.4 Operation effect diagram of intelligent wheelchair avoiding moving obstacles

3.3 輪椅的路徑規(guī)劃

不同目標(biāo)點(diǎn)下 A*算法改進(jìn)前后對(duì)比如圖5所示，圖中圓形框內(nèi)的藍(lán)色小車為移動(dòng)障礙物，用以測(cè)試智能輪椅的動(dòng)態(tài)避障效果.障礙物處于不同位置時(shí) A*算法改進(jìn)前后對(duì)比如圖6所示，圖中圓形框內(nèi)的棕色盒子為臨時(shí)障礙物，用以檢驗(yàn)障礙物在不同位置時(shí)智能輪椅的避障效果.圖5和圖6中三角形區(qū)域?yàn)橹悄茌喴嗡诘某跏嘉恢?，綠色方框中智能輪椅所在的位置為目標(biāo)點(diǎn)，黑色線條為智能輪椅的行動(dòng)軌跡，綠色線條為移動(dòng)障礙物的行動(dòng)軌跡.

由圖5和圖6可以看出，無(wú)論是遇到移動(dòng)障礙物還是臨時(shí)障礙物，智能輪椅都可以避開(kāi)并向目標(biāo)點(diǎn)方向前進(jìn).A*算法改進(jìn)后智能輪椅的運(yùn)動(dòng)軌跡較改進(jìn)前轉(zhuǎn)彎角度更小，拐點(diǎn)更少，路徑更加平滑，可以使其舒適度更高，使用者的體驗(yàn)感更好.

圖5 不同目標(biāo)點(diǎn)下A*算法改進(jìn)前后對(duì)比Fig.5 Comparison of A* algorithm before and after improvement under different target points

圖6 障礙物處于不同位置時(shí)A*算法改進(jìn)前后對(duì)比Fig.6 Comparison of A* algorithm before and after improvement when obstacles are in different positions

4 結(jié) 語(yǔ)

路徑規(guī)劃是智能輪椅研究的熱點(diǎn)問(wèn)題，本文以用戶使用舒適度為導(dǎo)向，對(duì)經(jīng)典A*算法進(jìn)行了改進(jìn)，通過(guò) MATLAB軟件以及Webots仿真平臺(tái)進(jìn)行對(duì)比驗(yàn)證.結(jié)果表明：將改進(jìn)A*算法應(yīng)用于智能輪椅上減少了路徑的拐點(diǎn)，減小了轉(zhuǎn)彎半徑，使路徑更加平滑，智能輪椅也可以在室內(nèi)環(huán)境下實(shí)現(xiàn)從起始點(diǎn)到目標(biāo)點(diǎn)的移動(dòng)和動(dòng)態(tài)避障.