勞彩蓮 李 鵬 馮 宇

(1.中國(guó)農(nóng)業(yè)大學(xué)現(xiàn)代精細(xì)農(nóng)業(yè)系統(tǒng)集成研究教育部重點(diǎn)實(shí)驗(yàn)室， 北京 100083；2.中國(guó)農(nóng)業(yè)大學(xué)農(nóng)業(yè)農(nóng)村部農(nóng)業(yè)信息獲取技術(shù)重點(diǎn)實(shí)驗(yàn)室， 北京 100083)

0 引言

隨著我國(guó)農(nóng)業(yè)的發(fā)展，溫室技術(shù)變得高度智能化和高效化。在植株高度密集、高溫和高濕度甚至有毒的溫室環(huán)境下，不利于長(zhǎng)時(shí)間進(jìn)行人工作業(yè)，再加上勞動(dòng)力不足和成本增加的影響，溫室機(jī)器人應(yīng)用越來(lái)越多[1]。利用移動(dòng)機(jī)器人完成溫室自主化作業(yè)符合精細(xì)化農(nóng)業(yè)的需求，路徑規(guī)劃是移動(dòng)機(jī)器人在溫室作業(yè)自主導(dǎo)航的重要前提[2-4]。根據(jù)機(jī)器人的作業(yè)范圍，可以將路徑規(guī)劃分為基于靜態(tài)全局環(huán)境的全局路徑規(guī)劃和基于局部障礙的局部路徑規(guī)劃[5]。常見(jiàn)的全局路徑規(guī)劃算法包括以Dijkstra[6-7]、A*[8-9]為代表的搜索規(guī)劃算法，以RRT[10]為代表的采樣規(guī)劃算法和以遺傳算法[11]為代表的啟發(fā)式規(guī)劃算法。依據(jù)實(shí)時(shí)傳感器數(shù)據(jù)的局部路徑規(guī)劃算法有人工勢(shì)場(chǎng)法[12]和動(dòng)態(tài)窗口法[13]。

Dijkstra算法采用遍歷搜索方式，規(guī)劃節(jié)點(diǎn)數(shù)較多，節(jié)點(diǎn)網(wǎng)絡(luò)非常龐大，算法效率低下[14]。在Dijkstra算法的基礎(chǔ)上，A*算法引入目標(biāo)點(diǎn)到當(dāng)前點(diǎn)的估計(jì)代價(jià)，根據(jù)估計(jì)代價(jià)決定路徑搜索方向，提高了算法效率[15]。A*算法在已知環(huán)境下能快速實(shí)現(xiàn)移動(dòng)機(jī)器人無(wú)碰、最短全局路徑規(guī)劃，主要通過(guò)節(jié)點(diǎn)狀態(tài)檢測(cè)和簡(jiǎn)單的估值功能規(guī)劃出一條最佳的安全道路。但是，該算法忽略了機(jī)器人的運(yùn)動(dòng)約束，不能保證曲率連續(xù)，導(dǎo)致運(yùn)動(dòng)參量在拐點(diǎn)處發(fā)生跳變。文獻(xiàn)[16]動(dòng)態(tài)計(jì)算出機(jī)器人的旋轉(zhuǎn)方向及角度，簡(jiǎn)化了路徑,但增加了運(yùn)行時(shí)間，實(shí)時(shí)性不高。文獻(xiàn)[17]使用跳點(diǎn)搜索減小了算法搜索面積，加快了速度，但是規(guī)劃出來(lái)的路徑拐點(diǎn)過(guò)多。文獻(xiàn)[18]采用直線-圓弧策略對(duì)路徑進(jìn)行平滑處理,消除了鋸齒效應(yīng)。文獻(xiàn)[19]使用微分方法減少了拐點(diǎn)數(shù)，但數(shù)學(xué)計(jì)算過(guò)多，比較耗時(shí)。文獻(xiàn)[20]優(yōu)化了A*算法的啟發(fā)函數(shù)，改進(jìn)了關(guān)鍵點(diǎn)的選取策略，減少了冗余的路徑點(diǎn)。文獻(xiàn)[21]提出對(duì)鄰域進(jìn)行擴(kuò)展，將8鄰域擴(kuò)展到24鄰域，使機(jī)器人可以小角度行進(jìn)，從而軌跡更加平滑。但這些改進(jìn)方法沒(méi)有考慮未知的障礙物，不具備動(dòng)態(tài)避障能力。

動(dòng)態(tài)窗口法[13，22-23](Dynamic window approach， DWA)通過(guò)結(jié)合傳感器的數(shù)據(jù)進(jìn)行在線實(shí)時(shí)局部路徑規(guī)劃，具有良好的避障能力，適用于動(dòng)態(tài)環(huán)境中的機(jī)器人路徑規(guī)劃，但并不能滿足全局路徑的最優(yōu)。將局部路徑規(guī)劃的算法結(jié)合到全局路徑規(guī)劃之中，在實(shí)際作業(yè)過(guò)程中，既考慮到環(huán)境中的障礙物，又能保證路徑規(guī)劃的完備性、實(shí)時(shí)性，滿足機(jī)器人的運(yùn)動(dòng)約束。

本文提出基于改進(jìn)A*算法與動(dòng)態(tài)窗口法結(jié)合的溫室機(jī)器人路徑規(guī)劃算法，利用選取策略對(duì)路徑進(jìn)行優(yōu)化。通過(guò)結(jié)合動(dòng)態(tài)窗口算法與超聲波信息進(jìn)行局部避障。在一維弦機(jī)器人操作系統(tǒng)上對(duì)改進(jìn)A*算法與Dijkstra算法、A*算法、RRT算法進(jìn)行仿真對(duì)比，驗(yàn)證改進(jìn)A*算法的有效性?？紤]機(jī)器人的實(shí)際尺寸，在真實(shí)環(huán)境下對(duì)柵格地圖進(jìn)行膨脹化處理，并結(jié)合超聲波進(jìn)行局部避障，完成真實(shí)環(huán)境下的機(jī)器人自主導(dǎo)航任務(wù)，并對(duì)算法的可行性進(jìn)行驗(yàn)證。

1 基于改進(jìn)A*算法的路徑規(guī)劃

1.1 傳統(tǒng)A*算法

A*算法采用啟發(fā)式搜索與廣度優(yōu)先算法結(jié)合，是靜態(tài)環(huán)境中用于求解最優(yōu)路徑最有效的直接搜索算法。A*算法通過(guò)一個(gè)代價(jià)函數(shù)F(n)，選擇搜索方向，從起點(diǎn)開(kāi)始向周圍擴(kuò)展，通過(guò)啟發(fā)函數(shù)H(n)計(jì)算得到周圍每個(gè)節(jié)點(diǎn)的代價(jià)值，選擇最小代價(jià)值作為下一個(gè)擴(kuò)展點(diǎn)，重復(fù)這個(gè)過(guò)程，直到到達(dá)終點(diǎn)，生成從起點(diǎn)到終點(diǎn)的路徑。在搜索過(guò)程中，由于路徑上的每一個(gè)節(jié)點(diǎn)都是具有最小代價(jià)節(jié)點(diǎn)，得到的路徑代價(jià)是最小的。A*算法的代價(jià)函數(shù)為

F(n)=G(n)+H(n)

(1)

式中G(n)——從起點(diǎn)到當(dāng)前節(jié)點(diǎn)的實(shí)際路徑代價(jià)

F(n)——當(dāng)前節(jié)點(diǎn)所在路徑從起點(diǎn)到終點(diǎn)預(yù)估的路徑代價(jià)總和

A*算法在執(zhí)行路徑規(guī)劃時(shí)主要用2個(gè)表實(shí)現(xiàn)節(jié)點(diǎn)的擴(kuò)展和最優(yōu)點(diǎn)的選擇，即通過(guò)Openlist表和Closelist表來(lái)記錄節(jié)點(diǎn)。其中Openlist表作用是保存搜索過(guò)程中遇到的擴(kuò)展節(jié)點(diǎn)，同時(shí)將這些節(jié)點(diǎn)按代價(jià)進(jìn)行排序，取出F(n)值最小的節(jié)點(diǎn)，作為當(dāng)前節(jié)點(diǎn)；然后將當(dāng)前節(jié)點(diǎn)的所有鄰節(jié)點(diǎn)按鄰節(jié)點(diǎn)規(guī)則加入到Openlist表中。

1.2 A*算法優(yōu)化策略

傳統(tǒng)A*算法根據(jù)載入的地圖，按照8鄰域搜索得到一系列的點(diǎn)，規(guī)劃出的路徑轉(zhuǎn)折次數(shù)太多，包含了一部分冗余點(diǎn)。如果轉(zhuǎn)折點(diǎn)過(guò)多，移動(dòng)機(jī)器人每次只能移動(dòng)很短的距離，會(huì)造成機(jī)器人卡頓現(xiàn)象。在實(shí)際的機(jī)器人移動(dòng)過(guò)程中，單次指令下，機(jī)器人的運(yùn)動(dòng)距離越長(zhǎng)，機(jī)器人移動(dòng)流暢度越好，產(chǎn)生的誤差也會(huì)越少。這些問(wèn)題造成在機(jī)器人的運(yùn)動(dòng)中平滑性欠缺，因此本文在傳統(tǒng)A*算法基礎(chǔ)上進(jìn)行關(guān)鍵點(diǎn)的路徑優(yōu)化策略。該策略能夠在柵格地圖上求解出更優(yōu)的路徑，移動(dòng)機(jī)器人路徑執(zhí)行具有了更高效率。

傳統(tǒng)A*算法規(guī)劃的路徑如圖1a所示，其中起始點(diǎn)為綠色方格，終點(diǎn)為紅色方格，灰色方格為規(guī)劃路徑，從圖中可以看出，路徑轉(zhuǎn)折點(diǎn)過(guò)多。

本文先從A*算法規(guī)劃出的軌跡獲得一系列的點(diǎn)，然后選取當(dāng)前節(jié)點(diǎn)運(yùn)動(dòng)方向相同的點(diǎn)，最終優(yōu)化成一條線段。通過(guò)相鄰節(jié)點(diǎn)的斜率對(duì)機(jī)器人的運(yùn)動(dòng)方向進(jìn)行判斷。從路徑規(guī)劃的第2個(gè)節(jié)點(diǎn)開(kāi)始，如果當(dāng)前節(jié)點(diǎn)與前一個(gè)節(jié)點(diǎn)的運(yùn)動(dòng)方向一致，則認(rèn)為前一個(gè)節(jié)點(diǎn)為冗余的，刪除前一個(gè)節(jié)點(diǎn)，依次遍歷所有路徑的節(jié)點(diǎn)，刪除所有的冗余點(diǎn)。機(jī)器人只需執(zhí)行包含起點(diǎn)、拐點(diǎn)的一系列路徑點(diǎn)，減少向機(jī)器人下達(dá)運(yùn)動(dòng)命令的次數(shù)，保證了機(jī)器人執(zhí)行路徑平滑性。

經(jīng)過(guò)關(guān)鍵點(diǎn)選取策略處理后的路徑如圖1b所示，其中點(diǎn)A到點(diǎn)B有18格子，機(jī)器人的運(yùn)動(dòng)方向都是朝下，將A與B之間的點(diǎn)優(yōu)化成2個(gè)點(diǎn)，即點(diǎn)A和點(diǎn)B，刪除AB之間的冗余點(diǎn)，更新路徑，使得機(jī)器人運(yùn)動(dòng)較為流暢。使用相同的優(yōu)化策略，形成BC、CD、DE段路徑，最終將路徑優(yōu)化成沒(méi)有冗余點(diǎn)的最優(yōu)路徑。經(jīng)過(guò)改進(jìn)A*算法的路徑平滑后，圖1a中的多余轉(zhuǎn)折點(diǎn)被消除，得到如圖1b所示的路徑。

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

在結(jié)構(gòu)化溫室環(huán)境中，作物按照一定的規(guī)則進(jìn)行放置，其相對(duì)位置已知，但存在一些易變因素，很難獲得完整的環(huán)境信息。為避免損害植物、保證機(jī)器人安全性，本文在全局路徑規(guī)劃的基礎(chǔ)上，采用動(dòng)態(tài)窗口法進(jìn)行局部路徑規(guī)劃。動(dòng)態(tài)窗口法根據(jù)機(jī)器人超聲傳感器檢測(cè)到局部環(huán)境問(wèn)題，進(jìn)行實(shí)時(shí)的路徑規(guī)劃，具有良好避障能力。載入全局地圖后，通過(guò)改進(jìn)A*算法進(jìn)行全局路徑規(guī)劃，下達(dá)給機(jī)器人運(yùn)動(dòng)命令開(kāi)始運(yùn)動(dòng)。由于機(jī)器人本身的運(yùn)動(dòng)學(xué)約束和環(huán)境因素影響，造成機(jī)器人在運(yùn)動(dòng)過(guò)程中產(chǎn)生誤差。動(dòng)態(tài)窗口法主要是在速度空間內(nèi)采樣線速度和角速度，并根據(jù)機(jī)器人的運(yùn)動(dòng)學(xué)計(jì)算預(yù)測(cè)其下一時(shí)間間隔的軌跡。對(duì)其獲得的待評(píng)價(jià)軌跡進(jìn)行評(píng)分，從而獲得更加安全、平滑的最優(yōu)局部路徑。

2.1 機(jī)器人運(yùn)動(dòng)模型

動(dòng)態(tài)窗口法將移動(dòng)機(jī)器人的位置控制轉(zhuǎn)換為速度控制[24]。在利用速度模式對(duì)機(jī)器人運(yùn)動(dòng)軌跡進(jìn)行預(yù)測(cè)時(shí)，首先需要對(duì)機(jī)器人的運(yùn)動(dòng)模型進(jìn)行分析。移動(dòng)機(jī)器人采用的是兩輪差速模型，v(t)和w(t)分別代表機(jī)器人在世界坐標(biāo)系下的平移速度與角速度，反映了機(jī)器人的運(yùn)動(dòng)軌跡。在機(jī)器人的編碼器采樣周期Δt內(nèi)，位移較小，機(jī)器人作勻速直線運(yùn)動(dòng)，則機(jī)器人運(yùn)動(dòng)模型為

(2)

式中x(t)、y(t)、θ(t)——t時(shí)刻機(jī)器人在世界坐標(biāo)下的位姿

2.2 速度采樣

動(dòng)態(tài)窗口法將避障問(wèn)題描述為速度空間中帶約束的優(yōu)化問(wèn)題，其中約束主要包括差速機(jī)器人的非完整約束、環(huán)境障礙物的約束以及機(jī)器人結(jié)構(gòu)的動(dòng)力學(xué)約束。DWA算法的速度矢量空間示意圖如圖2所示，橫坐標(biāo)為機(jī)器人角速度w，縱坐標(biāo)為機(jī)器人線速度v，其中vmax、vmin為機(jī)器人最大、最小線速度，wmax、wmin為機(jī)器人最大、最小角速度；整個(gè)區(qū)域?yàn)閂s，所有白色區(qū)域Va為機(jī)器人安全區(qū)域，Vd為考慮電機(jī)扭矩在控制周期內(nèi)限制的機(jī)器人可達(dá)速度范圍，Vr為上述3個(gè)集合的交集最終確定的動(dòng)態(tài)窗口。

根據(jù)機(jī)器人的速度限制，定義Vs為機(jī)器人線速度與角速度的集合，即動(dòng)態(tài)窗口算法搜索求解的最大范圍，滿足

Vs={(v,w)|vmin≤v≤vmax,wmin≤w≤wmax}

(3)

整個(gè)機(jī)器人的運(yùn)動(dòng)軌跡，可以細(xì)分為若干個(gè)直線或圓弧運(yùn)動(dòng)，為保證機(jī)器人安全區(qū)域，在最大減速度條件下，當(dāng)前速度應(yīng)能在撞擊障礙物之前減速為0，則定義機(jī)器人碰撞可行區(qū)域的線速度與角速度集合Va滿足

(4)

dist(v,w)——軌跡上與障礙物最近的距離

由于機(jī)器人電機(jī)轉(zhuǎn)矩的限制，采樣周期Δt內(nèi)存在機(jī)器人最大、最小可到達(dá)的速度v和角速度w范圍，需要進(jìn)一步縮小動(dòng)態(tài)窗口。在給定當(dāng)前線速度vc和角速度wc條件下，下一時(shí)刻動(dòng)態(tài)窗口Vd滿足

(5)

最終的速度范圍為上述3個(gè)速度的合集，定義動(dòng)態(tài)窗口Vr滿足

Vr=Vs∩Va∩Vd

(6)

在速度矢量空間Vr中，根據(jù)線速度、角速度采樣點(diǎn)數(shù)，將連續(xù)的速度矢量空間Vr離散化，得到離散的采樣點(diǎn)(v,w)。對(duì)于每一個(gè)采樣點(diǎn)，根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)模型預(yù)測(cè)下一時(shí)刻機(jī)器人的多個(gè)運(yùn)動(dòng)軌跡生成，如圖3所示。

2.3 評(píng)價(jià)函數(shù)

通過(guò)設(shè)計(jì)評(píng)價(jià)函數(shù)，在速度空間內(nèi)有選取最優(yōu)軌跡。在局部規(guī)劃中，保證最優(yōu)軌跡靠近A*算法的全局路徑，完成避障任務(wù)，朝向目標(biāo)快速運(yùn)動(dòng)。定義評(píng)價(jià)函數(shù)為

G(v,w)=k(αHeading(v,w)+βGoal(v,w)+
γPath(v,w)+σOcc(v,w))

(7)

式中k——平滑函數(shù)

α、β、γ、σ——各子函數(shù)的加權(quán)系數(shù)

Path(v,w)——路徑評(píng)價(jià)子函數(shù)，計(jì)算軌跡末端點(diǎn)到全局路徑的距離，距離越短說(shuō)明軌跡越靠近全局路徑

Heading(v,w)——方位角不斷地朝向終點(diǎn)位置函數(shù)

Goal(v,w)——評(píng)價(jià)機(jī)器人局部路徑末端點(diǎn)到終點(diǎn)的距離函數(shù)，其目的是不斷縮短與終點(diǎn)的距離

Occ(v,w)——評(píng)價(jià)機(jī)器人軌跡到障礙物距離函數(shù)

當(dāng)G(v,w)值最小時(shí)，獲得最佳路徑。本文中通過(guò)改進(jìn)A*算法路徑規(guī)劃獲得全局路徑，如圖4中黑色實(shí)線所示，虛線為局部規(guī)劃路徑，之間的距離計(jì)算公式為

(8)

式中 (x1,y1)——機(jī)器人根據(jù)運(yùn)動(dòng)學(xué)模型估計(jì)的局部路徑末端點(diǎn)坐標(biāo)

(x2,y2)——全局路徑規(guī)劃的節(jié)點(diǎn)坐標(biāo)

在移動(dòng)過(guò)程中，Head(v,w)函數(shù)用于使機(jī)器人的朝向不斷趨向終點(diǎn)方向，θ越小，說(shuō)明與終點(diǎn)的方位角越小，其示意圖如圖5所示，計(jì)算公式為

Head(v,w)=180°-θ

(9)

Goal(v,w)函數(shù)用于評(píng)價(jià)機(jī)器人局部路徑末端與終點(diǎn)的距離，通過(guò)函數(shù)不斷縮短與終點(diǎn)的距離，其示意圖如圖6所示。

Occ(v,w)函數(shù)用于評(píng)價(jià)機(jī)器人軌跡到障礙物的距離，體現(xiàn)了機(jī)器人的避障能力，如果機(jī)器人的軌跡到障礙物的距離大于機(jī)器人半徑，則沒(méi)有發(fā)生碰撞的危險(xiǎn)；反之，就說(shuō)明碰撞風(fēng)險(xiǎn)大，舍棄這條軌跡。其示意圖如圖7所示，R為機(jī)器人底盤(pán)半徑，H為機(jī)器人軌跡到障礙物的距離。

3 融合算法

移動(dòng)機(jī)器人超聲波檢測(cè)溫室局部環(huán)境信息，結(jié)合動(dòng)態(tài)窗口法進(jìn)行在線實(shí)時(shí)局部路徑規(guī)劃，檢測(cè)窗口滾動(dòng)前進(jìn)，具有避障能力，獲取局部的最優(yōu)路徑。通過(guò)全局路徑規(guī)劃與局部路徑規(guī)劃結(jié)合，將改進(jìn)A*算法進(jìn)行全局路徑規(guī)劃與動(dòng)態(tài)窗口算法融合，以保證動(dòng)態(tài)規(guī)劃路徑的全局最優(yōu)性。

在已知地圖上進(jìn)行全局路徑規(guī)劃，根據(jù)當(dāng)前速度、角速度、線速度計(jì)算并發(fā)布軌跡，通過(guò)評(píng)價(jià)標(biāo)準(zhǔn)下發(fā)各個(gè)軌跡，拋棄不合理的軌跡和遇到障礙的軌跡，運(yùn)用4個(gè)評(píng)價(jià)函數(shù)對(duì)某一路徑進(jìn)行評(píng)價(jià)，將各個(gè)評(píng)價(jià)函數(shù)累加，將成本最低的軌跡設(shè)定為最佳軌跡。如果當(dāng)前最佳軌跡是可通過(guò)的，依照速度模式根據(jù)最佳軌跡的速度移動(dòng)；如果遇到障礙，根據(jù)相關(guān)規(guī)則進(jìn)行避障，融合算法流程如圖8所示。

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

4.1 改進(jìn)A*算法仿真

Moro機(jī)器人是中國(guó)一維弦公司研發(fā)的移動(dòng)機(jī)器人，其配套EwayOS機(jī)器人操作系統(tǒng)包含了SDK接口、運(yùn)動(dòng)算法等模塊，能夠?qū)oro機(jī)器人路徑規(guī)劃算法進(jìn)行離線仿真，驗(yàn)證其算法的有效性。本文構(gòu)建5個(gè)尺寸一致的柵格地圖場(chǎng)景1～5，其水平方向上32個(gè)方格，豎直方向上29個(gè)方格，分辨率為10 cm×10 cm，其中黑色方塊代表環(huán)境中的障礙物信息，白色區(qū)域?yàn)榭尚袇^(qū)域。本文將改進(jìn)A*算法與傳統(tǒng)A*、Dijkstra、RRT算法進(jìn)行對(duì)比仿真實(shí)驗(yàn)，設(shè)定相同的起點(diǎn)坐標(biāo)為(0,0)，終點(diǎn)坐標(biāo)為(31,28)，其仿真實(shí)驗(yàn)結(jié)果如圖9～13所示。

5個(gè)柵格地圖場(chǎng)景障礙物的覆蓋率逐漸增大，復(fù)雜程度也逐漸增大，其中包含了規(guī)則和不規(guī)則障礙物，具有一定代表性。針對(duì)每個(gè)柵格地圖場(chǎng)景，本文使用4種路徑規(guī)劃算法運(yùn)行10次，初始設(shè)置參數(shù)一致，得到10組路徑規(guī)劃算法的路徑長(zhǎng)度、計(jì)算時(shí)間以及拐點(diǎn)數(shù)，取其平均值，結(jié)果如表1所示。

從表1來(lái)看，在5個(gè)不同柵格地圖場(chǎng)景中，4種路徑規(guī)劃算法都能最終規(guī)劃出路徑，但其路徑長(zhǎng)度、計(jì)算時(shí)間、拐點(diǎn)數(shù)存在一定差異。RRT算法的計(jì)算時(shí)間最短，但RRT算法是由隨機(jī)樹(shù)節(jié)點(diǎn)組成，導(dǎo)致生成的路線轉(zhuǎn)折點(diǎn)過(guò)多，并非理想的平滑曲線，這對(duì)于機(jī)器人運(yùn)動(dòng)來(lái)說(shuō)不利。當(dāng)機(jī)器人運(yùn)動(dòng)是從一個(gè)節(jié)點(diǎn)走到另一個(gè)節(jié)點(diǎn)，首先需要在原地完成轉(zhuǎn)向后，轉(zhuǎn)向下一個(gè)節(jié)點(diǎn)再繼續(xù)行走，拐點(diǎn)過(guò)多或路徑過(guò)長(zhǎng)，會(huì)造成機(jī)器人時(shí)間和能量的消耗。而改進(jìn)A*算法、傳統(tǒng)A*算法、Dijkstra算法獲得的全局路徑較優(yōu)。Dijkstra算法是一種遍歷算法，相對(duì)于改進(jìn)A*算法更加耗時(shí)，尤其面對(duì)不規(guī)則障礙時(shí)，改進(jìn)A*算法規(guī)劃的路徑是最優(yōu)的。改進(jìn)A*算法相對(duì)于傳統(tǒng)A*算法在計(jì)算時(shí)間、路徑長(zhǎng)度和拐點(diǎn)數(shù)都有較大的改善。從實(shí)驗(yàn)數(shù)據(jù)可以看出，改進(jìn)A*算法有較好的實(shí)時(shí)性，路徑更短，路徑平滑性更好，便于機(jī)器人執(zhí)行路徑軌跡達(dá)到目標(biāo)點(diǎn)。

表1 不同路徑規(guī)劃算法的路徑長(zhǎng)度、計(jì)算時(shí)間與拐點(diǎn)數(shù)對(duì)比Tab.1 Comparison of length, calculation time and number of inflection points of different path planning algorithms

4.2 環(huán)境模型處理

在算法仿真實(shí)驗(yàn)中機(jī)器人理想化成一點(diǎn)，忽略了機(jī)器人本體尺寸，在真實(shí)環(huán)境中還要考慮機(jī)器人尺寸,避免機(jī)器人邊緣和障礙物發(fā)生碰撞，執(zhí)行路徑失敗。用柵格圖構(gòu)建機(jī)器人的工作環(huán)境，移動(dòng)機(jī)器人在柵格間運(yùn)動(dòng)時(shí)，將固定障礙和機(jī)器人運(yùn)動(dòng)過(guò)程中超聲波檢測(cè)到的障礙設(shè)置為不可行區(qū)域。在此基礎(chǔ)上對(duì)地圖的障礙物進(jìn)行膨脹化處理，保證機(jī)器人與障礙物間的安全區(qū)域，提高路徑的可行性。如圖14所示，膨脹后的路徑，設(shè)有安全區(qū)域，更利于移動(dòng)機(jī)器人安全執(zhí)行規(guī)劃后的路徑。

在已知溫室環(huán)境地圖基礎(chǔ)上，移動(dòng)機(jī)器人通過(guò)超聲波獲取未知的動(dòng)態(tài)障礙信息。本文使用的Moro機(jī)器人底盤(pán)前方的4個(gè)超聲波雷達(dá)，檢測(cè)范圍為0～250 cm。移動(dòng)機(jī)器人在地圖中的位置已知，超聲波雷達(dá)將檢測(cè)到與動(dòng)態(tài)障礙物的距離轉(zhuǎn)換到世界坐標(biāo)系下，增加局部障礙物，將其加入全局地圖，機(jī)器人再根據(jù)融合算法進(jìn)行局部路徑規(guī)劃。在實(shí)際環(huán)境中，如圖15a所示，Moro機(jī)器人遇到動(dòng)態(tài)障礙物，超聲檢測(cè)到障礙物如圖15b所示，白色表示障礙，檢測(cè)到障礙物與機(jī)器人坐標(biāo)原點(diǎn)的距離為80 cm,實(shí)際測(cè)量值為82 cm,測(cè)量結(jié)果誤差較小，可以滿足實(shí)際的精度要求。如圖16a所示,機(jī)器人對(duì)局部障礙進(jìn)行膨脹處理，生成局部地圖，重新規(guī)劃路徑，避開(kāi)障礙物；圖16b中障礙物厚度增加，綠色的線即為重新規(guī)劃出的路徑。

通過(guò)構(gòu)建如圖17a所示環(huán)境，驗(yàn)證機(jī)器人運(yùn)動(dòng)的定位精度，起點(diǎn)為綠色，終點(diǎn)為紅色。EwayOS機(jī)器人操作系統(tǒng)實(shí)時(shí)記錄融合算法的路徑，即圖17a中的灰色曲線與實(shí)際運(yùn)動(dòng)軌跡和圖17a中的黃色曲線。Moro機(jī)器人按照規(guī)劃路徑成功抵達(dá)終點(diǎn)，路線中存在不重合的地方，分析實(shí)時(shí)的跟蹤數(shù)據(jù)，如圖17b所示，機(jī)器人實(shí)際跟蹤誤差始終小于0.22 m,基本滿足實(shí)際需求。

5 真實(shí)環(huán)境下的實(shí)時(shí)路徑規(guī)劃

本文的融合算法適用于小范圍結(jié)構(gòu)化環(huán)境，其環(huán)境具有一定的特殊性，選擇的3個(gè)與實(shí)驗(yàn)相似的結(jié)構(gòu)化環(huán)境1～3，對(duì)算法可行性進(jìn)行驗(yàn)證。利用Moro機(jī)器人分別在樓層走廊、模擬溫室環(huán)境和真實(shí)的溫室環(huán)境進(jìn)行實(shí)驗(yàn)。3個(gè)實(shí)驗(yàn)環(huán)境地面光滑程度相似，其柵格地圖中黑色為機(jī)器人可以到達(dá)的無(wú)障礙空間，白色為障礙物信息，融合算法的實(shí)驗(yàn)步驟描述為：①加載環(huán)境柵格地圖，設(shè)定機(jī)器人作業(yè)起點(diǎn)與目標(biāo)點(diǎn)。②改進(jìn)A*算法全局路徑規(guī)劃。通過(guò)機(jī)器人定位方法，獲取機(jī)器人實(shí)時(shí)位置信息，然后通過(guò)改進(jìn)A*算法規(guī)劃?rùn)C(jī)器人當(dāng)前位置至目標(biāo)點(diǎn)的全局路徑。③障礙物信息處理。機(jī)器人運(yùn)動(dòng)過(guò)程中通過(guò)超聲波雷達(dá)實(shí)時(shí)檢測(cè)環(huán)境障礙信息，并將傳感器信息轉(zhuǎn)換為機(jī)器人地圖上的障礙信息，得到膨脹后的地圖。④動(dòng)態(tài)窗口法局部路徑規(guī)劃。根據(jù)機(jī)器人的當(dāng)前位置信息，結(jié)合實(shí)時(shí)超聲波信息，在全局路徑的基礎(chǔ)上進(jìn)行動(dòng)態(tài)窗口法局部路徑規(guī)劃，獲得機(jī)器人最優(yōu)運(yùn)動(dòng)軌跡，機(jī)器人按照最優(yōu)軌跡對(duì)應(yīng)的線速度與角速度執(zhí)行運(yùn)動(dòng)。⑤根據(jù)機(jī)器人當(dāng)前位置到目標(biāo)點(diǎn)的距離判斷是否到達(dá)。如果未到達(dá)終點(diǎn)，則返回步驟③繼續(xù)運(yùn)動(dòng)；如果到達(dá)了目標(biāo)點(diǎn)，機(jī)器人導(dǎo)航任務(wù)結(jié)束。

機(jī)器人的實(shí)驗(yàn)參數(shù)設(shè)置如下：最大線速度為0.6 m/s,最大角速度為0.3 rad/s,線速度采樣個(gè)數(shù)為10，角速度采樣個(gè)數(shù)為20，采樣周期為0.2 s，子函數(shù)的加權(quán)系數(shù)α=0.1、β=10.0、γ=0.1、σ=0.2。

環(huán)境1為中國(guó)農(nóng)業(yè)大學(xué)信息與電氣工程學(xué)院樓層走廊,地面為光滑地板。圖18a所示走廊實(shí)際長(zhǎng)為10.5 m，寬為6.5 m,圖18b所示柵格地圖分辨率為10 cm×10 cm，起點(diǎn)為(1.0 m,1.0 m)，終點(diǎn)設(shè)為(9.1 m,4.2 m)，實(shí)際機(jī)器人路線如圖18b中紅線所示，終點(diǎn)為(9.3 m，4.0 m)，誤差為0.22 m。

環(huán)境2為模擬溫室盆栽環(huán)境，如圖19a所示，設(shè)置2排10個(gè)盆栽花盆，地面為光滑地板。場(chǎng)景實(shí)際長(zhǎng)為6 m，寬為4 m, 圖19b所示柵格地圖分辨率為10 cm×10 cm，起點(diǎn)為(0.5 m,0.5 m)，終點(diǎn)設(shè)為(5.5 m,3.5 m)，實(shí)際機(jī)器人路線如圖19b中紅線所示，終點(diǎn)為(5.5 m,3.4 m)，誤差為0.1 m。

環(huán)境3為實(shí)際溫室環(huán)境，如圖20a所示，為復(fù)合薄膜型結(jié)構(gòu)化溫室，地面為光滑的地板，有4排植物生長(zhǎng)金屬框架。場(chǎng)景的實(shí)際長(zhǎng)為8 m，寬為6 m，圖20b所示柵格地圖分辨率為10 cm×10 cm，起點(diǎn)為(0.5 m,0.5 m)，終點(diǎn)設(shè)為(7.0 m,2.5 m)，實(shí)際機(jī)器人路線如圖20b中紅線所示，終點(diǎn)為(7.1 m,2.3 m)，誤差為0.28 m。

從實(shí)驗(yàn)結(jié)果來(lái)看，本文的融合算法能夠?qū)崿F(xiàn)避障，繞過(guò)障礙物前進(jìn)到目標(biāo)點(diǎn)，且規(guī)劃的路徑能夠保持全局最優(yōu)，機(jī)器人基本按照規(guī)劃路徑抵達(dá)目標(biāo)。隨著場(chǎng)景面積變大和復(fù)雜程度增高，機(jī)器人到達(dá)的實(shí)際目標(biāo)點(diǎn)與設(shè)定的目標(biāo)點(diǎn)之間的誤差逐漸增大，主要是由于機(jī)器人轉(zhuǎn)彎時(shí)與地面打滑以及超聲波出現(xiàn)噪聲點(diǎn)造成的，其誤差不大于0.28 m，滿足實(shí)際需求。

6 結(jié)論

(1)根據(jù)環(huán)境信息已知的結(jié)構(gòu)化溫室環(huán)境進(jìn)行機(jī)器人路徑規(guī)劃，從路徑的平滑性、實(shí)時(shí)性和局部避障出發(fā)，提出一種基于改進(jìn)A*算法與動(dòng)態(tài)窗口法相結(jié)合的融合算法，在全局最優(yōu)路徑的基礎(chǔ)上，進(jìn)行實(shí)時(shí)避障，保證了路徑的平滑性和實(shí)時(shí)性。

(2)仿真結(jié)果表明，相較于傳統(tǒng)A*算法、Dijkstra算法和RRT算法，改進(jìn)A*算法在兼顧實(shí)時(shí)性基礎(chǔ)上采取的關(guān)鍵點(diǎn)策略，使路徑更為平滑，符合實(shí)際機(jī)器人運(yùn)動(dòng)需求。

(3)通過(guò)對(duì)環(huán)境模型處理，保證機(jī)器人運(yùn)動(dòng)的安全性。首先，對(duì)柵格地圖的障礙進(jìn)行膨脹化處理，結(jié)合超聲波傳感器對(duì)動(dòng)態(tài)障礙進(jìn)行檢測(cè)，并將膨脹化處理添加到全局地圖中。在仿真環(huán)境中對(duì)融合算法的有效性進(jìn)行了驗(yàn)證，結(jié)果表明，其跟蹤誤差保持在0.22 m以內(nèi)。

(4)在3個(gè)相似的結(jié)構(gòu)化環(huán)境下進(jìn)行了Moro機(jī)器人實(shí)驗(yàn)，結(jié)果表明，測(cè)量定位誤差不大于0.28 m，能夠滿足實(shí)際需求。驗(yàn)證了融合算法在溫室機(jī)器人路徑規(guī)劃中的可行性。