基于貪心算法的自動(dòng)路徑規(guī)劃送藥小車

徐小小 周啟銀 婁成龍 夏 宇

（貴州師范大學(xué)物理與電子科學(xué)學(xué)院，貴州 貴陽(yáng) 550025）

0 引言

隨著無(wú)人駕駛技術(shù)的不斷創(chuàng)新發(fā)展，小車自動(dòng)規(guī)劃路徑成為當(dāng)今社會(huì)的熱點(diǎn)。為了實(shí)現(xiàn)醫(yī)院無(wú)接觸配送藥品，該文進(jìn)行了以無(wú)人小車為主的精細(xì)化、智慧化送藥服務(wù)[1]的研究。

黃匯華等[2]采用K210、OpenMV 和STM32 共同組成的智能送藥小車能按規(guī)定的路線完成送藥任務(wù)，但是在送藥之前需要布置小車軌道。閉世管等[3]基于MSP-423E401Y 單片機(jī)、LV8548MC 芯片、OpenMV 識(shí)別模塊和NRF24L01 通信模塊實(shí)現(xiàn)不同藥房的送藥、取藥功能。萬(wàn)瑞豐[4]基于OpenArt和TC377 設(shè)計(jì)的送藥小車雖然降低了小車送藥的操作復(fù)雜度，但是在尋跡過程中的識(shí)別受光線的影響較大。肖光亞[5]基于Arduino 的智能送藥小車通過識(shí)別地面的安置路線自動(dòng)避障，以實(shí)現(xiàn)往返送藥的功能。

在總結(jié)國(guó)內(nèi)學(xué)者對(duì)智能送藥小車相關(guān)研究的基礎(chǔ)上，針對(duì)送藥小車在二維平面下如何規(guī)劃出最優(yōu)路徑，以提高送藥效率，該文提出了一種基于貪心算法的改進(jìn)RRT 算法。該算法可以建立醫(yī)院內(nèi)部的坐標(biāo)圖，并在獲取目標(biāo)位置的坐標(biāo)點(diǎn)后，根據(jù)醫(yī)院內(nèi)部各個(gè)部門的坐標(biāo)位置動(dòng)態(tài)規(guī)劃出1 條最優(yōu)的路徑。

1 貪心算法和動(dòng)態(tài)規(guī)劃法

貪心算法是將一個(gè)問題分成多個(gè)子問題，自頂向下以循環(huán)遞歸的方式快速地進(jìn)行貪心選擇，從而求出子問題的最優(yōu)解，進(jìn)而簡(jiǎn)化數(shù)學(xué)模型的規(guī)模[6]，并把迭代求解出的局部問題最優(yōu)解集合為整個(gè)問題的最終解[7]。貪心算法常運(yùn)用于子問題具有最優(yōu)解的問題中，其算法流程為建立能描述問題的數(shù)學(xué)模型、把問題分成多個(gè)子問題、對(duì)子問題進(jìn)行求解、獲得子問題的最優(yōu)解以及將集合子問題的最優(yōu)解作為整個(gè)問題的解。問題的求解關(guān)鍵是貪心策略，當(dāng)選擇貪心策略時(shí)須注意策略應(yīng)具有無(wú)后效性，即當(dāng)前狀態(tài)與前一個(gè)狀態(tài)沒有統(tǒng)計(jì)學(xué)意義，單獨(dú)節(jié)點(diǎn)的問題單獨(dú)求解，第一次選擇不需要前一個(gè)時(shí)刻子問題的解。無(wú)后效性是貪心算法區(qū)別于動(dòng)態(tài)規(guī)劃法的本質(zhì)特點(diǎn)。

動(dòng)態(tài)規(guī)劃法是自底向上地求解子問題的解，這是一種與前一個(gè)狀態(tài)有關(guān)聯(lián)的求解方法，當(dāng)?shù)谝淮芜x擇時(shí)，需要求解前一個(gè)時(shí)刻子問題的解，只有求解出前一個(gè)時(shí)刻子問題的解才能求解下一個(gè)子問題。該算法具有重疊子問題的性質(zhì)，在循環(huán)遞歸時(shí)會(huì)解決相同的問題，但是不會(huì)一直生成新的子問題。

2 貪心算法

2.1 貪心算法的基本步驟

貪心算法的求解過程為讀入問題、制定貪心策略、問題排序以及處理問題[8]。貪心算法從問題的初始解出發(fā)，不斷向目標(biāo)點(diǎn)逼近，每步都是不可回溯的決策，根據(jù)貪心選擇的性質(zhì)，歸并一系列的局部最優(yōu)解，以得到所求問題的解。該算法的核心思想是待求解問題的最優(yōu)解依賴于子問題的最優(yōu)解。該思想縮小了解決問題需要考慮的范圍，將全局變成局部，有利于降低算法的復(fù)雜度。

2.2 貪心算法的適用性

貪心算法具有最優(yōu)子結(jié)構(gòu)的性質(zhì)，即整體的解取決于局部子問題的解。該特性使貪心算法的適用范圍受到限制，即只有局部子問題最優(yōu)解能決定全局最優(yōu)解的情況才適用貪心算法。貪心算法的可信度可以通過數(shù)學(xué)方法證明，通過該算法能夠得到解決整個(gè)問題趨于最優(yōu)的解。

2.3 貪心算法實(shí)例——Prim 算法、Kruskal 算法

Prim 算法和Kruskal 算法是貪心算法中常用的算法，2 種算法的運(yùn)行機(jī)制不同，因此其執(zhí)行過程也存在差異，但這2 種算法都是求連通網(wǎng)的最小生成樹，可以將每條邊線逐步添加到?jīng)]有任何邊線的狀態(tài)的樹中。

2.3.1 Prim 算法

文獻(xiàn)[9]中，該算法是從任一根節(jié)點(diǎn)出發(fā)，利用貪心算法把離頂點(diǎn)最近的頂點(diǎn)添加到生成樹，不斷擴(kuò)大所含頂點(diǎn)的個(gè)數(shù)，當(dāng)最后一個(gè)節(jié)點(diǎn)被添加到生成樹時(shí)結(jié)束。與Kruskal 算法相比，該算法不受網(wǎng)絡(luò)圖稠密的影響。Prim 算法隨機(jī)選取1 個(gè)頂點(diǎn)，在圖1 中選取A為初始根節(jié)點(diǎn)，計(jì)算其與無(wú)向圖中的其他節(jié)點(diǎn)的距離，選取最小的邊添加到生成樹，循環(huán)上述流程，將節(jié)點(diǎn)添加到生成樹，直到終點(diǎn)被添加到生成樹。Prim 算法在無(wú)向圖中的執(zhí)行過程如圖1 所示。由圖1 可知，Prim 算法得到的最小生成樹是權(quán)值最小、有n個(gè)頂點(diǎn)且有n-1 條邊的帶權(quán)無(wú)向完全圖。該算法是對(duì)所有節(jié)點(diǎn)進(jìn)行直接搜索的，涉及多次迭代。

圖1 Prim 算法執(zhí)行圖

圖1 以A點(diǎn)為起點(diǎn)，利用Prim 算法得到的最小生成樹的每條邊上的數(shù)值為兩點(diǎn)之間的權(quán)重，算法通過判斷權(quán)重生成“ABDGEC”的擴(kuò)展樹，其中不包括F點(diǎn)，比較連接F點(diǎn)的權(quán)重可以得到G指向F的擴(kuò)展樹。

2.3.2 Kruskal 算法

Kruskal 算法可以選擇任意的起點(diǎn)，主要應(yīng)用在互斥集合數(shù)據(jù)結(jié)構(gòu)中。該算法的貪心策略是將所有節(jié)點(diǎn)與當(dāng)前節(jié)點(diǎn)的權(quán)值進(jìn)行比較，第一條邊是最小權(quán)值的邊線，再在其余邊線中任意選取一條邊線放在已有的邊線中，按照此方法循環(huán)取出邊線，最后得到最小生成樹。

Kruskal 算法執(zhí)行圖如圖 2 所示，選擇D點(diǎn)作為起點(diǎn)，對(duì)邊線的權(quán)值進(jìn)行排序，將權(quán)值最小的邊線添加到最小生成樹。橙色的粗線代表被添加到生成樹的邊線，即將圖2 中DGEC先放入生成樹，再將A、F的邊線進(jìn)行排序，把B到A、D到F加入樹中。該算法當(dāng)將邊線添加到最小生成樹時(shí)還須注意其是否會(huì)與生成樹中其他邊線產(chǎn)生回路，并將會(huì)產(chǎn)生回路的邊線刪除。要判斷是否產(chǎn)生回路，需要對(duì)生成樹進(jìn)行深度優(yōu)先搜索，判斷逆向邊是否存在。按照上述方式將所有的邊線添加到生成樹。

圖2 Kruskal 算法執(zhí)行圖

由上述原理介紹可知，網(wǎng)絡(luò)圖邊線數(shù)量對(duì)Kruskal 算法的影響較大。

對(duì)比Kruskal 算法與Prim 算法建立生成樹的方法可知，Kruskal 算法是先對(duì)權(quán)重進(jìn)行排序，選取最小的權(quán)重添加到生成樹；Prim 算法則是直接對(duì)其他的節(jié)點(diǎn)進(jìn)行循環(huán)查找，搜索權(quán)值最小的節(jié)點(diǎn)放入最小生成樹。因此當(dāng)邊線繁雜時(shí)選用Prim算法，如果考慮時(shí)間復(fù)雜度，那么Kruskal 算法的效率比Prim算法效率高。

3 二維坐標(biāo)系下貪心算法結(jié)合動(dòng)態(tài)規(guī)劃規(guī)劃最優(yōu)路徑

3.1 RRT 算法

RRT 算法是基于隨機(jī)采樣的快速擴(kuò)展樹，通過與障礙物產(chǎn)生碰撞檢測(cè)是否將該節(jié)點(diǎn)添加到擴(kuò)展樹，多次循環(huán)檢測(cè)直到到達(dá)目標(biāo)點(diǎn)，再?gòu)哪繕?biāo)點(diǎn)回溯到起始點(diǎn)，就可以找到路徑。與Dijkstra 算法、粒子群優(yōu)化算法和人工勢(shì)場(chǎng)法相比，該算法的搜索能力強(qiáng)、參數(shù)少且搜索路徑的速度快，適用于高維復(fù)雜環(huán)境下的路徑優(yōu)化。在擴(kuò)展生成樹的過程中，擴(kuò)展的節(jié)點(diǎn)是隨機(jī)采樣的，因此其搜索的路徑只能找到兩點(diǎn)之間的路徑，并不能得到最優(yōu)路徑并且冗余節(jié)點(diǎn)的計(jì)算量大、規(guī)劃時(shí)間響應(yīng)時(shí)間太長(zhǎng)，容易導(dǎo)致路徑規(guī)劃失敗。

3.2 貪心算法融合改進(jìn)的RRT 算法規(guī)劃流程

為了解決傳統(tǒng)的RRT 算法帶來的路徑規(guī)劃非最優(yōu)、搜索速度慢的問題，該文引入貪心算法中的Kruskal 算法選擇每個(gè)子問題的最優(yōu)解并舍棄其他解，以減少冗余點(diǎn)的計(jì)算量。RRT算法路徑規(guī)劃的優(yōu)化主要進(jìn)行以下5 個(gè)步驟：1）隨機(jī)樹初始化。2）進(jìn)行隨機(jī)采樣。3）擴(kuò)展節(jié)點(diǎn)。4）終點(diǎn)停止擴(kuò)展。5）處理冗余點(diǎn)。

3.2.1 隨機(jī)樹初始化

當(dāng)該算法在讀取坐標(biāo)系時(shí)，以當(dāng)前位置為快速擴(kuò)展樹的搜索起點(diǎn)，以接收到的目的地坐標(biāo)點(diǎn)為快速擴(kuò)展樹的終點(diǎn)。隨機(jī)樹的初始化包括根節(jié)點(diǎn)和帶閾值的終點(diǎn)。

3.2.2 進(jìn)行隨機(jī)采樣

隨機(jī)樹的擴(kuò)展采用隨機(jī)采樣檢測(cè)節(jié)點(diǎn)是否存在碰撞，從而判斷該點(diǎn)是否存在障礙物。利用歐氏距離計(jì)算空間中兩點(diǎn)的距離，二維環(huán)境下的歐氏距離、三維坐標(biāo)系上的歐式距離如公式（1）、公式（2）所示。

式中：（x1，y1）為直線一側(cè)端點(diǎn)的坐標(biāo)點(diǎn)；（x2，y2）為直線另一側(cè)端點(diǎn)的坐標(biāo)點(diǎn)；z1、z2為2 個(gè)點(diǎn)在z坐標(biāo)軸上的坐標(biāo)。

可以將公式（2）推廣到三維坐標(biāo)中，還可以將公式（1）推廣到n維坐標(biāo)系上。在對(duì)計(jì)算的距離進(jìn)行排序后，選擇距離最短的節(jié)點(diǎn)進(jìn)行碰撞檢測(cè)（判斷2 個(gè)節(jié)點(diǎn)之間是否存在障礙物）。如果無(wú)障礙物發(fā)生碰撞，那么將該節(jié)點(diǎn)添加到快速擴(kuò)展樹；如果該節(jié)點(diǎn)發(fā)生碰撞事件，算法判斷該點(diǎn)存在障礙物，那么重新生成隨機(jī)點(diǎn)，再循環(huán)檢測(cè)歐氏距離的計(jì)算和判斷。

3.2.3 擴(kuò)展節(jié)點(diǎn)

節(jié)點(diǎn)的擴(kuò)展須基于上一步驟檢測(cè)碰撞事件，將沒有發(fā)生碰撞事件的節(jié)點(diǎn)添加到隨機(jī)樹，以擴(kuò)展隨機(jī)樹。

3.2.4 終點(diǎn)停止擴(kuò)展

隨機(jī)樹初始化時(shí)需要設(shè)置一個(gè)帶閾值的終點(diǎn)。當(dāng)擴(kuò)展樹的節(jié)點(diǎn)與終點(diǎn)的歐式距離小于設(shè)置的閾值時(shí)，程序判斷該節(jié)點(diǎn)是終點(diǎn)，停止隨機(jī)樹的擴(kuò)展。完成上述步驟后就可以得到1條從起點(diǎn)到終點(diǎn)的路徑。

3.2.5 處理冗余點(diǎn)

冗余點(diǎn)刪除前后對(duì)比圖如圖3 所示。由圖3 可以得到冗余點(diǎn)處理表，見表 1。

圖3 冗余點(diǎn)刪除前后對(duì)比

圖4 二維環(huán)境路徑優(yōu)化對(duì)比圖

貪心算法結(jié)合改進(jìn)的RRT 算法在二維環(huán)境下的路徑優(yōu)化對(duì)比圖如圖 4 所示。

4 結(jié)語(yǔ)

針對(duì)傳統(tǒng)送藥小車按軌道尋徑存在靈活性差、工作效率低和適用范圍容易被限制等問題，該文提出將Kruskal 算法與RRT 算法結(jié)合，建立醫(yī)院內(nèi)部的坐標(biāo)圖，自動(dòng)規(guī)劃最優(yōu)路徑，以完成送藥任務(wù)。該算法使送藥小車能夠靈活選擇醫(yī)院的各個(gè)位置，不再局限于只在軌道上運(yùn)動(dòng)，且該算法不用識(shí)別地面上的軌道，外界的亮度就無(wú)法對(duì)其產(chǎn)生影響。利用貪心算法提取最優(yōu)解，可以減少路徑冗余點(diǎn)的計(jì)算量，結(jié)合改進(jìn)的RRT 算法能夠快速找到最優(yōu)路徑，提高送藥小車的工作效率。但是該文研究算法的試驗(yàn)背景是在道路上沒有行人的場(chǎng)所，如果要將該算法投入使用，還需要實(shí)現(xiàn)送藥小車的實(shí)時(shí)避障功能。將最優(yōu)路徑算法結(jié)合實(shí)時(shí)避障算法，最終使送藥小車能夠靈活避開行人，快速完成送藥任務(wù)。

表1 冗余點(diǎn)處理表