基于碰撞預(yù)測的改進(jìn)A*路徑規(guī)劃算法研究

陶 智 梁獻(xiàn)霞 路俊維 趙 慶

（1、邢臺職業(yè)技術(shù)學(xué)院信息工程系，河北 邢臺054000 2、河北機(jī)電職業(yè)技術(shù)學(xué)院機(jī)電工程系，河北 邢臺054000）

移動機(jī)器人的路徑規(guī)劃問題是當(dāng)前移動機(jī)器人領(lǐng)域所研究的熱點(diǎn)問題，其主要目標(biāo)是在已知、未知或者部分未知的環(huán)境中規(guī)劃出從起始點(diǎn)到目標(biāo)點(diǎn)的安全無碰撞路徑。

在已知的靜態(tài)環(huán)境，所用的全局路徑規(guī)劃算法通常有柵格法、RRT 算法、A*算法、神經(jīng)網(wǎng)絡(luò)算法、遺傳算法、蟻群算法等等。文獻(xiàn)[1]改進(jìn)A*算法中使用了正反向搜索的機(jī)制，范圍仍然局限于靜態(tài)障礙物環(huán)境下。對于動態(tài)障礙物環(huán)境下的路徑規(guī)劃，文獻(xiàn)[2]采用了基本避障策略和啟發(fā)式動態(tài)規(guī)劃法相結(jié)合的思路，但是關(guān)于A*算法在動態(tài)障礙物下的路徑規(guī)劃研究文獻(xiàn)卻幾乎空白[1-2]。

本文結(jié)合前人的研究成果，對動態(tài)障礙物環(huán)境下的路徑規(guī)劃進(jìn)行研究探討，在A*算法的基礎(chǔ)上進(jìn)行改進(jìn)，通過引入逃逸距離策略解決機(jī)器人與動態(tài)障礙物相遇的問題，并對新算法進(jìn)行了仿真驗(yàn)證。實(shí)驗(yàn)結(jié)果表明，采用逃逸距離策略后的算法，可提高機(jī)器人路徑規(guī)劃的安全效率。

1 柵格化網(wǎng)絡(luò)

為了便于分析研究，將機(jī)器人、路徑系統(tǒng)和障礙物進(jìn)行網(wǎng)絡(luò)柵格化處理，將不規(guī)則障礙物按照規(guī)則化處理，并設(shè)定路徑網(wǎng)格的最小單位正方形，單位為1cm，如圖1 所示。

圖1 環(huán)境網(wǎng)格

2 A*算法引入

A*算法的評價函數(shù)表示為：f（n）=g（n）+h（n）。其中f（n）是從初始站點(diǎn)經(jīng)由當(dāng)前站點(diǎn)到目標(biāo)站點(diǎn)的估計值，在尋路過程中，總是選擇f（n）值最小的下一站點(diǎn)作為最優(yōu)路徑中的站點(diǎn)，g（n）表示從初始站點(diǎn)到當(dāng)前站點(diǎn)已經(jīng)付出的代價，h（n）表示從當(dāng)前站點(diǎn)到目標(biāo)站點(diǎn)的估計代價。

A*算法中g(shù)（n）值由起點(diǎn)開始所經(jīng)過的距離累加而得，h（n）值則通過距離預(yù)估方法求得，通常有曼哈頓距離或者歐幾里得距離[3-5]。

3 逃逸范圍策略

實(shí)際情況下，移動障礙物的尺寸大于機(jī)器人尺寸，并且在相互運(yùn)動過程中有沖突的可能，需要設(shè)置有效避障范圍。預(yù)測機(jī)器人和動態(tài)障礙物相遇的邊界，由此確定此時碰撞的矩形范圍，機(jī)器人可以安全逃逸到達(dá)目標(biāo)點(diǎn)，可以防止運(yùn)行中被損壞。

3.1 最小矩形圖

在實(shí)際環(huán)境中，障礙物有規(guī)則和不規(guī)則形狀，為了方便處理，提出了最小矩形圖的概念，即能夠正好將整個障礙物包括在內(nèi)的矩形，如圖2 所示的障礙物都包含在長10cm 和寬為4cm 的矩形框內(nèi)。

圖2 中凸形障礙物

3.2 逃逸范圍數(shù)學(xué)建模

假設(shè)機(jī)器人車體r，移動速度為vr，機(jī)器人坐標(biāo)為r（rx, ry）。動態(tài)障礙物為o，運(yùn)動速度為vo，障礙物的坐標(biāo)為o（ox, oy），其橫向長度為oh，縱向長度為ow，且在運(yùn)動過程中不改變方向，機(jī)器人與障礙物的運(yùn)動夾角為θ。

在沒有遇到障礙物的情況下，機(jī)器人r 搜索路徑的方向夾角為θ，因此機(jī)器人r 移動到障礙物o 邊界的橫向距離等同于縱向距離，可以推出此時機(jī)器人的坐標(biāo)為（ox，ry+ox-rx），固逃逸范圍是以（ox，ry+ox-rx）為坐標(biāo)，橫向長度為oh，縱向長度為ow的矩形。同理可以根據(jù)移動機(jī)器人r 和動態(tài)障礙物o 的相對位置和運(yùn)動方向推出機(jī)器人r 移動到障礙物o 右邊界和上下邊界時的逃逸范圍[6]。

3.3 逃逸范圍計算模型

本文只考慮單動態(tài)障礙物的情況，對不規(guī)則的動態(tài)障礙物做了柵格化處理，統(tǒng)一為最小矩形圖。機(jī)器人和障礙物在協(xié)同運(yùn)動過程中可能同時占用路徑網(wǎng)格資源[7-8]。圖3、圖4、圖5 和圖6 展示了在柵格型路徑運(yùn)動過程中可能出現(xiàn)的沖突類型。

當(dāng)機(jī)器人移動到邊界逃逸范圍，即計算逃逸范圍，其確定的方法，不同方向運(yùn)動的計算各有差異，概括為四種情況，分別討論如下[9-10]。

（1）機(jī)器人r 和障礙物o 右斜向相向運(yùn)動

圖3 右斜向相向運(yùn)動逃逸

圖4 左斜向相向運(yùn)動逃逸

圖5 左斜向反向運(yùn)動逃逸

圖6 右斜向反向運(yùn)動逃逸

3.4 改進(jìn)A*算法流程

引入逃逸范圍策略，改進(jìn)后A*算法的整體流程如圖7 所示[11-12]。

其算法的流程可以簡述為：

（1）加載路徑網(wǎng)格坐標(biāo)信息；

（2）判斷有無障礙物，有則判斷障礙物的狀態(tài)，如果是動態(tài)，則執(zhí)行改進(jìn)A*算法，否則執(zhí)行固有的A*算法；

（3）如果沒有障礙物，則直接計算路徑長度；

（4）更新機(jī)器人的坐標(biāo)位置，并判斷是否到達(dá)目的坐標(biāo)，如果沒有，則循環(huán)執(zhí)行（2）、（3）和（4）步驟，否則，循環(huán)終止，流程結(jié)束。

圖7 路徑規(guī)劃算法流程

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

圖8 斜向相向運(yùn)動

不考慮障礙物之間的碰撞，且機(jī)器人和障礙物的移動速度均為勻速。網(wǎng)格中紅色方框代表目標(biāo)點(diǎn)，黑色邊框代表動態(tài)障礙物的移動軌跡，黑色箭頭表示障礙物的移動方向，綠色方框表示機(jī)器人的移動軌跡[13]。實(shí)驗(yàn)?zāi)M對比了斜向相向運(yùn)動情況下A*算法和改進(jìn)A*算法后的情況，圖8 中的（a）表示機(jī)器人r和障礙物o 剛好邊界相遇的狀態(tài)，（b）表示設(shè)置邊界逃逸范圍后的運(yùn)動情況。

5 結(jié)論

通過改進(jìn)A*算法，引入逃逸范圍策略，通過對比仿真實(shí)驗(yàn)，證明此策略可以有效防止機(jī)器人與動態(tài)障礙物發(fā)生碰撞，避免了造成機(jī)器人的損傷，可以安全到達(dá)目標(biāo)點(diǎn)。

本文探究了單動態(tài)障礙物下，機(jī)器人的路徑規(guī)劃，改進(jìn)算法具有優(yōu)越性，對于復(fù)雜環(huán)境中的多動態(tài)障礙物，以及多動態(tài)障礙物和靜態(tài)障礙物同時存在的情況，將是下一步要研究的方向。