姚道金 殷雄 羅真 溫銳 陳智宇 鄒鴻昊

（華東交通大學(xué) 電氣與自動(dòng)化工程學(xué)院，江西 南昌 330013）

近年來(lái)，自動(dòng)導(dǎo)向搬運(yùn)車系統(tǒng)（AGVS）已成為研究的熱點(diǎn)，其中AGVS 路徑規(guī)劃是研究熱點(diǎn)之一。AGVS 是由多個(gè)同構(gòu)或異構(gòu)機(jī)器人組成的自動(dòng)導(dǎo)向搬運(yùn)車系統(tǒng)，通過(guò)控制機(jī)器人協(xié)同完成作業(yè)任務(wù)［1］。相比單機(jī)器人，AGVS 具有可同時(shí)執(zhí)行多個(gè)任務(wù)、工作范圍大、作業(yè)集群化和規(guī)?；葍?yōu)點(diǎn)。但隨著機(jī)器人數(shù)量的增加，路徑規(guī)劃算法的復(fù)雜度也呈指數(shù)上升。目前，AGVS 路徑規(guī)劃大多使用分層分布式框架，分為下層路徑規(guī)劃和上層協(xié)同規(guī)劃，下層路徑規(guī)劃為各機(jī)器人尋找最優(yōu)路徑，而上層協(xié)同規(guī)劃考慮機(jī)器人之間優(yōu)先級(jí)，完成機(jī)器人等待、避讓等任務(wù)［2］。

路徑規(guī)劃算法通?？煞譃閳D搜索算法和智能優(yōu)化算法，其中圖搜索算法對(duì)地圖柵格化后進(jìn)行篩選，生成最優(yōu)路徑，包括Dijkstar 算法［3］、A*算法［4］、JPS 算法［5］等；智能優(yōu)化算法包括人工勢(shì)場(chǎng)法［6］、蟻群算法［7］、灰狼算法［8］等。

針對(duì)路徑規(guī)劃問(wèn)題，許多學(xué)者進(jìn)行了大量研究，文獻(xiàn)［9］提出了一種雙層路徑規(guī)劃算法，算法第一層為融合路徑信息動(dòng)態(tài)權(quán)重因子和Bezier 曲線的改進(jìn)A*算法，第二層引入時(shí)間維度，建立機(jī)器人路徑時(shí)間圖，從而預(yù)測(cè)機(jī)器人之間的沖突。文獻(xiàn)［10］提出了一種AGVS協(xié)同規(guī)劃策略，首先使用協(xié)同進(jìn)化粒子群優(yōu)化算法部署傳感器位置，然后使用改進(jìn)K-means 方法劃分任務(wù)區(qū)域，最后以傳感器位置為路徑點(diǎn)規(guī)劃路徑。文獻(xiàn)［11］提出了一種多機(jī)器人多任務(wù)點(diǎn)協(xié)同巡檢方法，首先使用改進(jìn)的生物激勵(lì)神經(jīng)網(wǎng)絡(luò)算法對(duì)多任務(wù)點(diǎn)進(jìn)行巡檢，然后將電站區(qū)域分解成若干不含障礙物的子區(qū)域，各移動(dòng)機(jī)器人分別在子區(qū)域內(nèi)利用優(yōu)先級(jí)啟發(fā)式算法選擇路徑。文獻(xiàn)［12］提出了一種多機(jī)器人協(xié)同路徑規(guī)劃方法，首先生成預(yù)約表，然后生成加權(quán)地圖，最后通過(guò)分布式控制方法對(duì)多機(jī)器人協(xié)同路徑進(jìn)行規(guī)劃。文獻(xiàn)［13］提出了一種基于改進(jìn)DWA的多機(jī)器人編隊(duì)控制算法，首先對(duì)傳統(tǒng)DWA算法進(jìn)行改進(jìn)，然后為機(jī)器人設(shè)計(jì)不同的行為，最后基于改進(jìn)DWA和社會(huì)力模型設(shè)計(jì)行為控制策略，使領(lǐng)航者在未知環(huán)境下能夠規(guī)劃適合整體編隊(duì)運(yùn)行的路徑，跟隨者能夠根據(jù)編隊(duì)的不同狀態(tài)自適應(yīng)地切換跟隨方式。以上算法均取得了較好的規(guī)劃效果，但沒(méi)有同時(shí)考慮靜態(tài)地圖中出現(xiàn)的臨時(shí)靜態(tài)障礙物和動(dòng)態(tài)障礙物，未能有效地解決復(fù)雜環(huán)境下AGVS的路徑規(guī)劃問(wèn)題。

為此，文中提出了一種分層分布式框架下改進(jìn)的AGVS 路徑規(guī)劃算法，該算法分為兩層，第一層引入混合改進(jìn)A*算法和DWA算法，實(shí)現(xiàn)單機(jī)器人的路徑規(guī)劃；第二層引入?yún)f(xié)同規(guī)劃，考慮機(jī)器人之間的協(xié)調(diào)運(yùn)動(dòng)，實(shí)現(xiàn)AGVS 路徑規(guī)劃。最后在Matlab中進(jìn)行仿真實(shí)驗(yàn)，以驗(yàn)證所提算法的有效性。

1 算法整體框架

AGVS路徑規(guī)劃框架主要有3類：集中式框架、分布式框架和分層分布式框架［14］。文中算法使用的分層分布式框架具有集中式框架和分布式框架的優(yōu)點(diǎn)［15］，保留了單個(gè)移動(dòng)機(jī)器人路徑規(guī)劃的獨(dú)立性，同時(shí)各機(jī)器人接受統(tǒng)一規(guī)劃任務(wù)控制。算法整體框架如圖1所示。

圖1 分層分布式框架Fig.1 Hierarchical distributed framework

2 路徑規(guī)劃

2.1 改進(jìn)的A*算法

傳統(tǒng)A*算法的搜索步驟如下：首先將起始點(diǎn)Xstart加入open 列表，同時(shí)將起始點(diǎn)Xstart作為父節(jié)點(diǎn)加入close 列表；然后搜索起始點(diǎn)Xstart的相鄰（非障礙物）節(jié)點(diǎn)并加入open 列表，依據(jù)評(píng)價(jià)函數(shù)f（n）=g（n）+h（n）計(jì)算open 列表中節(jié)點(diǎn)代價(jià)，選取代價(jià)最低節(jié)點(diǎn)作為下一個(gè)父節(jié)點(diǎn)，并將其放入close 列表；最后搜索父節(jié)點(diǎn)的可達(dá)節(jié)點(diǎn)并計(jì)算代價(jià)，重復(fù)上述過(guò)程，直至父節(jié)點(diǎn)為目標(biāo)點(diǎn)Xgoal為止［16］。其中g(shù)（n）為當(dāng)前節(jié)點(diǎn)到起始點(diǎn)Xstart的距離，h（n）為當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)Xgoal的距離，采用歐幾里得距離，即

式中，nx、ny為當(dāng)前節(jié)點(diǎn)在x、y軸上的坐標(biāo)值，gx、gy為目標(biāo)點(diǎn)在x、y軸上的坐標(biāo)值。

2.1.1 改進(jìn)的評(píng)價(jià)函數(shù)

由于評(píng)價(jià)函數(shù)估計(jì)值與柵格地圖實(shí)際值相差很大，導(dǎo)致傳統(tǒng)A*算法的搜索效率低。因此，可對(duì)傳統(tǒng)A*算法的評(píng)價(jià)函數(shù)進(jìn)行改進(jìn)，由于h（n）為歐幾里得距離，且h（n）始終小于實(shí)際距離，故可為h（n）增加權(quán)重系數(shù)，得到改進(jìn)后的評(píng)價(jià)函數(shù)為

式中，r為當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的距離，R為起始點(diǎn)到目標(biāo)點(diǎn)的距離。

2.1.2 雙向Floyd算法

傳統(tǒng)A*算法生成的路徑中含有大量的冗余點(diǎn)，路徑不平滑。目前，常用的路徑優(yōu)化算法有Floyd算法、貝塞爾曲線和B 樣條等。Floyd 算法作為一種基于動(dòng)態(tài)規(guī)劃方法的最短路徑算法，又被稱為插點(diǎn)法［17］。傳統(tǒng)Floyd 算法僅對(duì)起始點(diǎn)Xstart至目標(biāo)點(diǎn)Xgoal的路徑進(jìn)行優(yōu)化。為此，文中提出了一種雙向Floyd 算法，增加了對(duì)目標(biāo)點(diǎn)Xgoal至起始點(diǎn)Xstart的反向軌跡進(jìn)行優(yōu)化，實(shí)現(xiàn)雙向軌跡優(yōu)化，以進(jìn)一步提高路徑平滑度，具體原理如圖2所示。

圖2 改進(jìn)Floyd算法去除路徑冗余點(diǎn)原理Fig.2 Principle of the improved Floyd algorithm to remove path redundancy points

圖2 中，改進(jìn)Floyd 算法先去除路徑冗余點(diǎn)，路徑變?yōu)閄start→X1→X2→X3→Xgoal；然后進(jìn)行正向軌跡優(yōu)化，路徑變?yōu)閄start→X4→Xgoal；最后采用與正向軌跡優(yōu)化相同的方式進(jìn)行反向軌跡優(yōu)化，路徑變?yōu)閄start→X5→Xgoal。

雙向Floyd算法加入了障礙檢測(cè)，計(jì)算障礙物中心與路徑間的垂直距離，避免路徑與障礙物相交。障礙物距離計(jì)算方法如圖3所示，設(shè)路徑起始點(diǎn)Xstart的坐標(biāo)為（xA，yA），路徑目標(biāo)點(diǎn)Xgoal的坐標(biāo)為（xB，yB），XstartXgoal與x軸之間的夾角為α，障礙物中心點(diǎn)C的坐標(biāo)為（xC，yC），d為點(diǎn)C至點(diǎn)Xstart的垂直距離，e為點(diǎn)C至點(diǎn)Xgoal的縱向距離，則障礙物與路徑XstartXgoal之間的距離d可由點(diǎn)Xstart、Xgoal、C的坐標(biāo)表示為

圖3 障礙物距離示意圖Fig.3 Schematic diagram of obstacle distance

2.2 改進(jìn)的DWA算法

DWA 算法是一種常見(jiàn)的移動(dòng)機(jī)器人局部路徑規(guī)劃算法，將移動(dòng)機(jī)器人建立在速度空間中，具體步驟如下：首先對(duì)移動(dòng)機(jī)器人建立運(yùn)動(dòng)學(xué)模型，采集多組速度，通過(guò)建立的運(yùn)動(dòng)學(xué)和速度模型對(duì)其一段時(shí)間內(nèi)的運(yùn)動(dòng)軌跡進(jìn)行分析和預(yù)測(cè)；然后使用DWA算法的評(píng)價(jià)函數(shù)選出最優(yōu)軌跡及對(duì)應(yīng)的速度；最后根據(jù)選出的最優(yōu)軌跡段及其對(duì)應(yīng)的速度組進(jìn)行局部路徑規(guī)劃［18］。

2.2.1 機(jī)器人初始位姿

針對(duì)目標(biāo)點(diǎn)方位，可以改進(jìn)起始點(diǎn)機(jī)器人的初始位姿［19］。若機(jī)器人的初始位姿方向與目標(biāo)點(diǎn)方向相差太多，則需要機(jī)器人繞幅度較大的彎來(lái)完成位姿矯正，這將極大增加路徑轉(zhuǎn)彎次數(shù)，同時(shí)耗費(fèi)時(shí)間。因此，文中對(duì)起始點(diǎn)位姿進(jìn)行優(yōu)化，若機(jī)器人起始點(diǎn)位姿θ大于45°，則給定一個(gè)角加速度a進(jìn)行位姿優(yōu)化：

式中，θ為初始航向與起始點(diǎn)到目標(biāo)點(diǎn)連線的夾角（如圖4所示），ΔT為位姿優(yōu)化時(shí)間。

圖4 機(jī)器人起始位姿調(diào)整Fig.4 Robot starting posture adjustment

2.2.2 機(jī)器人運(yùn)動(dòng)學(xué)模型

移動(dòng)機(jī)器人運(yùn)動(dòng)模型分為差速驅(qū)動(dòng)機(jī)器人模型和全向移動(dòng)機(jī)器人模型。文中建立差速驅(qū)動(dòng)機(jī)器人模型，通過(guò)調(diào)節(jié)左、右輪驅(qū)動(dòng)速度控制機(jī)器人運(yùn)動(dòng)。運(yùn)動(dòng)學(xué)建模過(guò)程中，對(duì)線速度和角速度進(jìn)行采樣，得到機(jī)器人在一段時(shí)間內(nèi)進(jìn)行勻速運(yùn)動(dòng)的模型：

式中，v（t）為當(dāng)前線速度，ω（t）為當(dāng)前角速度，Δt為計(jì)算時(shí)間，θt為機(jī)器人當(dāng)前位姿角度。

2.2.3 機(jī)器人運(yùn)動(dòng)速度范圍

移動(dòng)機(jī)器人在速度空間中，由于自身模型限制、硬件上限和環(huán)境約束，需要對(duì)理論上存在的無(wú)窮多速度組進(jìn)行約束。設(shè)移動(dòng)機(jī)器人受自身?xiàng)l件制約的最大線速度為vmax、最大角速度為ωmax、最小線速度為vmin和最小角速度為ωmin，則機(jī)器人線速度v和角速度ω組成的速度空間可表示為

差速機(jī)器人靠電機(jī)驅(qū)動(dòng)，存在速度約束。在DWA算法中，機(jī)器人模型受加速度影響的線速度v和角速度ω范圍為

式中，vt為t時(shí)刻的線速度，為加速度，ωt為t時(shí)刻的角速度，為角加速度。

DWA 算法中評(píng)價(jià)函數(shù)選取最優(yōu)軌跡和對(duì)應(yīng)的速度組，選擇標(biāo)準(zhǔn)為躲避障礙物、距離最短、靠近目標(biāo)點(diǎn)，具體公式為

式中，dist（v，ω）為模擬軌跡與最近障礙物的距離。

2.2.4 改進(jìn)的DWA算法的評(píng)價(jià)函數(shù)

DWA 算法需要為機(jī)器人建立評(píng)價(jià)函數(shù)，對(duì)模擬出的機(jī)器人若干運(yùn)動(dòng)軌跡進(jìn)行評(píng)估擇優(yōu)。傳統(tǒng)DWA算法的評(píng)價(jià)函數(shù)head（v，ω）是軌跡終點(diǎn)方向與目標(biāo)點(diǎn)之間的角度差，但文中DWA 算法是在全局路徑規(guī)劃基礎(chǔ)上運(yùn)行的，因此，需要改進(jìn)角度差評(píng)價(jià)函數(shù)，增加一個(gè)當(dāng)前目標(biāo)加權(quán)系數(shù)P，改進(jìn)的DWA算法的評(píng)估函數(shù)為

式中，vel（v，ω）為速度評(píng)價(jià)子函數(shù)，σ為平滑函數(shù)，δ、β和γ為各評(píng)價(jià)子函數(shù)的加權(quán)系數(shù)。

2.3 算法融合

兩種改進(jìn)算法融合的具體步驟為：首先，使用改進(jìn)的A*算法生成移動(dòng)機(jī)器人的全局最優(yōu)路徑；然后，將最優(yōu)路徑中的關(guān)鍵節(jié)點(diǎn)分配給改進(jìn)的DWA算法，由改進(jìn)的DWA算法進(jìn)行局部路徑規(guī)劃；最后，移動(dòng)機(jī)器人沿著最優(yōu)路徑，有效躲避靜態(tài)障礙和動(dòng)態(tài)障礙。

3 協(xié)同規(guī)劃

AGVS 協(xié)同規(guī)劃是減少機(jī)器人之間發(fā)生沖突的關(guān)鍵。協(xié)同規(guī)劃主要包括設(shè)置優(yōu)先級(jí)和設(shè)置沖突協(xié)調(diào)策略兩部分［20］，以提高AGVS規(guī)劃算法的魯棒性。

3.1 設(shè)置優(yōu)先級(jí)

優(yōu)先級(jí)法是指在AGVS 系統(tǒng)中為移動(dòng)機(jī)器人進(jìn)行等級(jí)排序［21］。當(dāng)機(jī)器人之間發(fā)生沖突時(shí)，優(yōu)先級(jí)高的機(jī)器人擁有指揮權(quán)和調(diào)配權(quán)，從而控制優(yōu)先級(jí)低的機(jī)器人，避免機(jī)器人之間發(fā)生沖突。優(yōu)先級(jí)法中優(yōu)先級(jí)分配是關(guān)鍵，直接影響沖突協(xié)調(diào)結(jié)果。文中根據(jù)3 個(gè)機(jī)器人（AGV1、AGV2、AGV3）執(zhí)行任務(wù)順序，將優(yōu)先級(jí)設(shè)置為AGV1 ≥AGV2 ≥AGV3。

3.2 設(shè)置沖突協(xié)調(diào)策略

沖突協(xié)調(diào)策略包括等待行為和避讓行為。等待行為指機(jī)器人之間距離小于危險(xiǎn)距離時(shí)，低優(yōu)先級(jí)機(jī)器人將保持原位置等候，直至沖突結(jié)束。避讓行為指機(jī)器人之間距離小于危險(xiǎn)距離時(shí)，低優(yōu)先級(jí)機(jī)器人在原位置等候，而高優(yōu)先級(jí)機(jī)器人則使用改進(jìn)的DWA算法重新規(guī)劃路徑，避讓低優(yōu)先級(jí)機(jī)器人，直至沖突結(jié)束。具體沖突協(xié)調(diào)策略如下：

（1）協(xié)調(diào)策略1 針對(duì)全局路徑規(guī)劃，通過(guò)優(yōu)先級(jí)策略判斷沖突機(jī)器人的優(yōu)先級(jí)大小。高優(yōu)先級(jí)機(jī)器人完成路徑搜索后，再由低優(yōu)先級(jí)機(jī)器人搜索，最后生成所有機(jī)器人的全局最優(yōu)路徑。

（2）協(xié)調(diào)策略2 針對(duì)局部路徑規(guī)劃，將機(jī)器人危險(xiǎn)距離設(shè)置為3 m，當(dāng)AGV1航向角與AGV2航向角的夾角小于100°時(shí)，低優(yōu)先級(jí)機(jī)器人發(fā)生等待行為，高優(yōu)先級(jí)機(jī)器人按照原路徑前進(jìn)，直至沖突結(jié)束；當(dāng)AGV1 航向角與AGV2 航向角的夾角大于100°時(shí)，低優(yōu)先級(jí)機(jī)器人發(fā)生等待行為，高優(yōu)先級(jí)機(jī)器人發(fā)生避讓行為，直至沖突結(jié)束。

4 算法仿真與結(jié)果分析

為驗(yàn)證文中改進(jìn)算法的可行性和有效性，在30 m×30 m 含障礙物地圖環(huán)境中進(jìn)行路徑規(guī)劃。運(yùn)行環(huán)境為：Matlab R2022a、Windows10 64bit、處理器（Intel）i5-12400F 12代、內(nèi)存32 GB。

傳統(tǒng)A*算法與改進(jìn)A*算法規(guī)劃的路徑及其長(zhǎng)度對(duì)比如圖5 和表1 所示。從圖中可以看出，改進(jìn)A*算法的路徑冗余點(diǎn)明顯減少，且路徑與障礙物保持安全距離。由表中可知，由改進(jìn)的A*算法生成的路徑長(zhǎng)度平均縮短了2.26%。

表1 兩種算法生成的路徑長(zhǎng)度對(duì)比Table 1 Length comparison of paths generated by two algorithms

圖5 兩種算法生成的路徑比較Fig.5 Comparison of paths generated by two algorithms

圖6 為文中算法的AGVS 完整路徑規(guī)劃結(jié)果，圖中有3個(gè)圓形機(jī)器人、數(shù)個(gè)黑色靜態(tài)障礙物、3個(gè)移動(dòng)障礙物和1 個(gè)灰色臨時(shí)障礙物。由圖中可知，AGVS可在簡(jiǎn)單環(huán)境中完成無(wú)碰撞路徑規(guī)劃。

為進(jìn)一步驗(yàn)證文中算法的魯棒性，更改地圖環(huán)境，再進(jìn)行AGVS路徑規(guī)劃，仿真結(jié)果如圖7所示。由圖中可知，3 個(gè)移動(dòng)機(jī)器人均能順利到達(dá)目標(biāo)位置，未與障礙物發(fā)生碰撞，這表明，文中算法可以適應(yīng)地圖環(huán)境的變化，具有較強(qiáng)的魯棒性。

文中算法的AGVS 位姿、角速度和線速度曲線如圖8 所示。由圖8（a）可知，改進(jìn)的DWA 算法可以有效地調(diào)整機(jī)器人的初始位姿，使機(jī)器人沿著全局最優(yōu)路徑前進(jìn)。由圖8（b）和圖8（c）可知，AGVS運(yùn)動(dòng)過(guò)程中，機(jī)器人的角速度始終保持在-0.4～0.4 rad/s 區(qū)域內(nèi)，AGVS 線速度保持在0.6～1.2 m/s區(qū)域內(nèi)，符合機(jī)器人運(yùn)動(dòng)學(xué)特性。文中提出的AGVS 路徑規(guī)劃算法可以使多個(gè)移動(dòng)機(jī)器人平滑運(yùn)動(dòng)到目標(biāo)位置，在避免AGVS間沖突的同時(shí)，生成最優(yōu)的平滑路徑。

圖8 文中算法的AGVS位姿、角速度和線速度曲線Fig.8 Pose，angular velocity and linear velocity curves of AGVS by the proposed algorithm

圖9 為AGVS 沖突協(xié)調(diào)策略的作用效果。當(dāng)AGV1和AGV2發(fā)生沖突，根據(jù)協(xié)調(diào)策略，AGV2速度預(yù)測(cè)軌跡消失，暫停等待AGV1，當(dāng)AGV1離開后，AGV2 開始運(yùn)動(dòng)，證明了文中提出的算法可以有效地解決機(jī)器人之間的沖突問(wèn)題。

圖9 AGVS沖突協(xié)調(diào)策略的作用效果Fig.9 Effect of AGVS conflict coordination strategy

5 結(jié)論

文中提出了一種分層分布框架下改進(jìn)AGVS 路徑規(guī)劃算法，以解決復(fù)雜環(huán)境下AGVS的路徑規(guī)劃問(wèn)題。該算法分為下層路徑規(guī)劃和上層協(xié)同規(guī)劃，下層路徑規(guī)劃為融合改進(jìn)A*算法的全局規(guī)劃和改進(jìn)DWA算法的局部規(guī)劃；上層協(xié)同規(guī)劃在機(jī)器人優(yōu)先級(jí)分配的基礎(chǔ)上加入沖突協(xié)調(diào)策略，以實(shí)現(xiàn)AGVS在復(fù)雜環(huán)境下的路徑規(guī)劃。仿真結(jié)果表明，文中改進(jìn)算法可得到復(fù)雜環(huán)境下機(jī)器人的無(wú)碰撞最優(yōu)路徑，路徑長(zhǎng)度比傳統(tǒng)A*算法的機(jī)器人路徑縮短2.26%。

文中提出的算法實(shí)現(xiàn)了復(fù)雜環(huán)境下AGVS 的路徑規(guī)劃，但缺少任務(wù)分配和作業(yè)區(qū)域自主分配。未來(lái)工作主要集中在：①對(duì)AGVS任務(wù)進(jìn)行自主分配，增加單機(jī)器人執(zhí)行任務(wù)的數(shù)量；②對(duì)機(jī)器人作業(yè)區(qū)域進(jìn)行有效分配，對(duì)分配區(qū)域進(jìn)行自主路徑規(guī)劃。