室內(nèi)移動機器人路徑規(guī)劃

李建紅

(湖北工業(yè)大學(xué)機械工程學(xué)院，湖北 武漢 430068)

路徑規(guī)劃是自主移動機器人的核心技術(shù)，根據(jù)對環(huán)境信息認(rèn)知的不同，可以分為全局路徑規(guī)劃和局部路徑規(guī)劃[1]。全局路徑規(guī)劃主要有Dijkstra算法[2]，最佳優(yōu)先搜索算法[3]，A*算法[4]等。其中A*算法應(yīng)用最廣，文獻(xiàn)[5]通過判斷路徑上相鄰兩點方向是否一致，刪除多余的冗余點，路徑更加簡短。文獻(xiàn)[6]通過在拐點處，增加五次多項式計算，路徑更加平滑。局部路徑規(guī)劃主要有時間彈性帶法[7]，動態(tài)窗口法(Dynamical Window Approach，DWA[8])，人工勢場法[9]等。其中DWA算法應(yīng)用最廣，文獻(xiàn)[10]通過增加機器人與障礙物的空間關(guān)系，實現(xiàn)U型環(huán)境下避障。文獻(xiàn)[11]通過使用評價函數(shù)估計機器人路徑過程中的節(jié)點，避免陷入局部無解。

針對單一算法不能有效的面對復(fù)雜環(huán)境，提出了融合A*算法和DWA算法的路徑規(guī)劃算法。由A*算法規(guī)劃出全局路徑，結(jié)合機器人模型和視覺傳感器對動態(tài)障礙物進(jìn)行膨脹化處理，然后由DWA算法規(guī)劃局部避障路徑，最后機器人自主運動到目標(biāo)點。

1 全局路徑規(guī)劃算法

Dijkstra算法使用遍歷搜索方式，選擇離起點最近的點進(jìn)行搜索，到目標(biāo)點停止得到最短路徑。但是遍歷搜索需要訪問所有節(jié)點，因此時間復(fù)雜度非常高，運行效率也非常低。BFS算法基于貪心策略，在節(jié)點搜索空間中，每次選擇離最近目標(biāo)的點為下一次的搜索起點，依次循環(huán)下去得到移動軌跡。但是忽略了當(dāng)前節(jié)點的實際代價，因此無法得到從起點到目標(biāo)點的最短路徑。

A*算法綜合Dijkstra和BFS 2種算法的思想，加入評價函數(shù)對軌跡上的每個點進(jìn)行評估，得到一條從起點到目標(biāo)點的最優(yōu)路徑。評價函數(shù)為

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

(1)

其中，F(xiàn)(n)表示從初始點到目標(biāo)點的總體代價估計，G(n)表示從初始點到當(dāng)前點(xn，yn)的實際代價估計。H(n)表示從當(dāng)前點(xn,yn)到目標(biāo)點的代價估計。H(n)能夠影響整個路徑規(guī)劃的搜索區(qū)域。設(shè)當(dāng)前點(xn,yn)到目標(biāo)點的實際距離為h(n)，當(dāng)H(n)，需要搜索的區(qū)域較大，但能獲得最短路徑；當(dāng)h(n>)h(n)，需要搜索的區(qū)域較小，但是不能獲得最短路徑。A*算法實現(xiàn)步驟如下：

(1)創(chuàng)建OPEN表和CLOS表，將起始點放入OPEN表中。

(2)遍歷OPEN表，選取F值對應(yīng)最小的節(jié)點，放入CLOSE表中，同時將其從OPEN表中刪除。

(3)對該節(jié)點的8個相鄰格子進(jìn)行判斷，如果格子是不可到達(dá)或者已經(jīng)在CLOSE表中，則跳過它。如果相鄰格子不在OPEN表中，則把它加入CLOSE表中。如果相鄰格子在OPEN表中，計算g值，選取g值最小的作為下一節(jié)點，并更新f，g，h值。

(4)重復(fù)步驟(2)和步驟(3)，直到目標(biāo)點在OPEN表中，表示找到整個路徑。

2 局部路徑規(guī)劃算法

動態(tài)窗口算法(Dynamic Windows Approach，DWA)是最常用的局部路徑規(guī)劃算法。其主要思想是在速度空間(v,ω)中，根據(jù)移動機器人運動模型，采樣出能滿足自身條件的速度組合。然后模擬這些速度組合對應(yīng)的運動軌跡，使用評價函數(shù)對運動軌跡進(jìn)行評價，選取最優(yōu)軌跡下對應(yīng)的速度組合控制機器人運動。

2.1 機器人運動模型

由于DWA算法需要模擬速度空間下移動機器人運動軌跡。因此，要知道移動機器人運動模型。對于兩輪差速機器人，如圖1所示。

圖1 移動機器人運動模型

在時間Δt內(nèi)，運動軌跡可以看作是一段圓弧，則有機器人運動模型如下：

(2)

2.2 速度采樣

移動機器人在速度空間(v,ω)運動中存在著無限的速度組合。考慮到自身的特點和周圍環(huán)境，速度采樣被限制在一定的范圍內(nèi)。

首先，移動機器人的自身速度有上下界。

Vm={(v,ω)|v∈[vmin,vmax],ω∈[ωmin,ωmax]}

(3)

其次，移動機器人還受到電機影響，有最大的加減速極限。

Vd={(v,ω)|v∈[v0-vxΔt,v0+vyΔt],ω∈[ω0-ωxΔt,ω0+ωyΔt]}

(4)

式中，v0表示初始時刻的線速度，ω0表示初始時刻的角速度，vx表示線速度最大加速度，vy表示線速度的最大減速度，ωx表示角速度的最大加速度，ωy表示角速度的最大減速度。

最后，移動機器人應(yīng)與障礙物保持足夠安全距離。

(5)

式中，dist(v,ω)表示在速度空間(v,ω)中，機器人與障礙物之間的距離。

綜合式(3)(4)(5)3種約束條件，可以得出實際DWA算法可搜索的速度空間(v,ω)范圍如下：

V=Vm∩Vd∩Va

(6)

2.3 評價函數(shù)

由于速度空間中可搜索的速度組合依然很多，因此有必要引入一個評估函數(shù)來評價速度空間中生成的軌跡。評價函數(shù)為：

G(v,ω)=σ(α?heading(v,ω)+β?dist(v,ω)+γ?velcity(v,ω))

(7)

其中，heading(v,ω)是用來評價當(dāng)前點機器人軌跡末端的方向和目標(biāo)點方向的角度大小。dist(v,ω)代表當(dāng)前軌跡與障礙物之間的距離，velocity(v,ω)用來評價當(dāng)前軌跡的速度。α，β，γ為每一項的權(quán)值系數(shù)。

3 融合算法

基于全局路徑規(guī)劃的A*算法，只能在已知靜態(tài)地圖中找到最優(yōu)路徑，如果環(huán)境發(fā)生變化，比如在規(guī)劃的路徑上出現(xiàn)一個行人，則A*算法規(guī)劃的移動機器人軌跡無法進(jìn)行有效避開。通過加入動態(tài)窗口算法，根據(jù)移動機器人視覺傳感器采集局部環(huán)境信息，完成對局部動態(tài)障礙物的安全躲避。具體步驟如下：

(1)加載實驗室環(huán)境的柵格地圖，將地圖中已知障礙物進(jìn)行膨脹化處理，得到全局靜態(tài)代價地圖，并指定移動機器人起點和目標(biāo)點。

(2)全局路徑規(guī)劃。根據(jù)目標(biāo)點位姿信息，在全局靜態(tài)代價地圖上，通過A*算法規(guī)劃一條從起點到目標(biāo)點的全局路徑。

(3)未知障礙物處理。移動機器人運動過程中發(fā)現(xiàn)未知障礙物，通過視覺傳感器采集未知的障礙物信息，上位機獲得信息數(shù)據(jù)并進(jìn)行處理，將動態(tài)障礙物信息加載到全局靜態(tài)代價地圖中，得到局部動態(tài)代價地圖。

(4)局部路徑規(guī)劃。根據(jù)視覺傳感器信息，在局部動態(tài)代價地圖上，通過DWA算法進(jìn)行局部路徑規(guī)劃，移動機器人按照評價函數(shù)最優(yōu)的軌跡進(jìn)行運動，完成局部避障。

(5)重新規(guī)劃全局路徑。根據(jù)目標(biāo)點位姿信息，調(diào)整移動機器人角度，到達(dá)目標(biāo)點。

融合算法實現(xiàn)框圖如下：

圖2 融合算法框圖

4 真實環(huán)境下實驗與分析

4.1 實驗硬件平臺搭建

實驗硬件平臺使用英特爾NUC作為整個系統(tǒng)的控制中心，通過USB端口連接Turtlebot3移動底盤和Kinect視覺傳感器。如圖3所示。

圖3 移動機器人平臺

4.2 實驗軟件平臺

實驗軟件平臺是在Ubuntu16.04下安裝的ROS系統(tǒng)，ROS是開源的機器人操作系統(tǒng)，能夠為機器人提供標(biāo)準(zhǔn)化的系統(tǒng)框架[12]。為了方便代碼調(diào)試，ROS提供了三維可視化工具Rviz，三維仿真工具Gazebo來查看整個機器人系統(tǒng)的運行過程。

4.3 代價地圖的建立

選擇實驗室為實驗環(huán)境，如圖3所示。控制機器人行走一圈，通過視覺傳感器獲取周圍環(huán)境數(shù)據(jù)，對數(shù)據(jù)進(jìn)行處理得到柵格地圖。為了使機器人能夠進(jìn)行更加安全的路徑規(guī)劃，對柵格地圖進(jìn)行膨脹處理，得到代價地圖，如圖4所示。

圖4 實驗環(huán)境

圖5 代價地圖

4.4 融合算法的實現(xiàn)

利用搭建的基于ROS的移動機器人平臺，在代價地圖上進(jìn)行融合A*算法和DWA算法測試實驗。通過ROS中的三維可視化工具Rviz，指定目標(biāo)點，可以觀察到機器人在代價地圖中的實時位置。整個過程如圖4所示：

圖6 導(dǎo)航過程中Rviz界面截圖

圖6(a)為移動機器人在起始點，A*算法開始規(guī)劃全局路徑，圖6(b)為移動機器人檢測到突然闖進(jìn)的陌生人，DWA算法開始規(guī)劃局部路徑。圖6(c)為移動機器人避開動態(tài)障礙物，重新生成全局路徑。圖6(d)為移動機器人到達(dá)目標(biāo)點。從整個過程中可以看出，移動機器人成功地避開了地圖中突然出現(xiàn)的陌生人，驗證了本文提出的融合算法的可行性。

5 結(jié)語

全局路徑規(guī)劃對比分析Dijkstra、BFS和A*算法，選擇A*算法作為全局路徑規(guī)劃算法，局部路徑規(guī)劃算法則選取DWA算法，并進(jìn)行理論推導(dǎo)與分析。然后融合A*算法和DWA算法，選取實驗室環(huán)境進(jìn)行實驗驗證。實驗表明，融合后的算法能夠避開地圖中突然出現(xiàn)的陌生人。與A*算法相比，融合算法可以對地圖中未知的障礙物進(jìn)行實時避障，更能適應(yīng)復(fù)雜多變的室內(nèi)環(huán)境。