基于優(yōu)化動態(tài)窗口算法的智能機器人路徑規(guī)劃設(shè)計

摘 要：針對普通動態(tài)窗口算法進(jìn)行動態(tài)避障后規(guī)劃路徑長度增加、在復(fù)雜環(huán)境中無法進(jìn)行規(guī)劃的問題，本文研究一種優(yōu)化智能機器人路徑規(guī)劃算法。本文采取設(shè)置動態(tài)函數(shù)、調(diào)整運動機制和規(guī)定運動航向等策略，保證算法可規(guī)劃最短路徑；利用軌跡評價函數(shù)構(gòu)建最佳路徑的目標(biāo)函數(shù)，提升機器人的避障性能，安全到達(dá)終點。使用蜂鳥云驗證，結(jié)果表明，與傳統(tǒng)哈里斯鷹算法相比，本文算法在復(fù)雜環(huán)境中規(guī)劃路徑更短，路徑質(zhì)量更佳，運動時間更少，證明算法在路徑規(guī)劃中適用性較好。

關(guān)鍵詞：優(yōu)化動態(tài)窗口算法；智能機器人；路徑規(guī)劃；傳統(tǒng)哈里斯鷹算法

中圖分類號：TP 242" " " " " " " " " " 文獻(xiàn)標(biāo)志碼：A

路徑規(guī)劃的目的是使機器人利用行進(jìn)準(zhǔn)則搜索最佳的無碰撞路徑，根據(jù)環(huán)境掌握程度，機器人路徑規(guī)劃可分為全局規(guī)劃與局部規(guī)劃。全局規(guī)劃是了解環(huán)境后求解靜態(tài)環(huán)境中的最短路徑，局部規(guī)劃一般用于解決動態(tài)環(huán)境中的避障問題[1]。路徑規(guī)劃算法包括普通算法與優(yōu)化算法，普通算法須提前載入信息，優(yōu)化算法在既定環(huán)境中全局性較好。對路徑規(guī)劃優(yōu)化算法研究較多，楊帆等[2]引入距離信息，在優(yōu)化函數(shù)后得到最優(yōu)路徑；黃勇強等[3]提出融合優(yōu)化A*算法和動態(tài)窗口算法的路徑規(guī)劃算法，完成了全局修正；常新中等[4]將動態(tài)權(quán)重引入混合金豺優(yōu)化算法，增強全局探索能力。

雖然上述研究提升了算法效率，但是存在依賴參數(shù)設(shè)定、在復(fù)雜地形中無法規(guī)劃等問題。優(yōu)化動態(tài)窗口算法具有參數(shù)少、尋優(yōu)速度快等優(yōu)勢，因此選取該算法進(jìn)行路徑規(guī)劃。在靜態(tài)全局規(guī)劃的基礎(chǔ)上進(jìn)行實時動態(tài)規(guī)劃，使算法能夠有效應(yīng)用于動態(tài)環(huán)境中，提升智能機器人規(guī)劃路徑的順滑度。

1 普通動態(tài)窗口算法

當(dāng)應(yīng)用于路徑規(guī)劃時，動態(tài)窗口算法起到有效避障的作用。在計算過程中，機器人速度約束條件如公式（1）所示。

（1）

式中：xt為機器人處于t時刻的線速度；yt為機器人處于t時刻的角速度；ymax為角速度最大值；xt+1為機器人處于t+1時刻的線速度；yt+1為機器人處于t+1時刻的角速度；mxmax為線加速度最大值；mymax為角加速度最大值；λt為時間步長。對速度空間進(jìn)行采樣，獲得速度集合，在該過程中生成預(yù)測軌跡，使用評估函數(shù)評分后發(fā)現(xiàn)最佳運動軌跡與相應(yīng)速度集合。評估函數(shù)g如公式（2）所示。

（2）

式中：x為機器人的線速度；y為機器人的角速度；k1、k2和k3為評估函數(shù)權(quán)重；r為歸一化過程；difference函數(shù)用于計算預(yù)測軌跡終端方位角與目標(biāo)方位角的差值，差值越大，函數(shù)值越小；minimum函數(shù)用于計算預(yù)測軌跡所有坐標(biāo)到障礙物的最佳距離，其評估預(yù)測軌跡與障礙物的距離，函數(shù)值越大，距離越遠(yuǎn)；speed函數(shù)用于評估線速度與角速度，線速度越大，評分越高，角速度越慢。

如果需要算法全局搜索能力更強，則可增加k1數(shù)值；如果需要機器人避障能力更強，則可增加k2數(shù)值；如果需要機器人運動更順滑，則可增加k3數(shù)值。在實踐中，算法有2個需要調(diào)整的問題。1）避障后規(guī)劃路徑長度偶爾會增加。2）當(dāng)靠近障礙物時，k2數(shù)值增加，搜索性能顯著降低。如果降低k2數(shù)值，就會觸碰障礙物，還會出現(xiàn)無法規(guī)劃路徑的問題。因此，本文提出優(yōu)化動態(tài)窗口算法解決上述問題。

2 優(yōu)化動態(tài)窗口算法

2.1 設(shè)置動態(tài)函數(shù)

在了解全局信息的情況下，優(yōu)化動態(tài)窗口算法計算靜態(tài)最佳路徑，在沿該路徑運動的過程中，機器人不斷掃描環(huán)境，當(dāng)發(fā)現(xiàn)動態(tài)障礙物時會主動避障，調(diào)整路徑?？赡艹霈F(xiàn)規(guī)劃路徑距離最佳路徑較遠(yuǎn)的問題，為了使路徑更短，可在優(yōu)化過程中添加dynamic動態(tài)函數(shù)，將其評估函數(shù)權(quán)重標(biāo)記為k4。動態(tài)函數(shù)能夠使算法更快發(fā)現(xiàn)最短路徑，優(yōu)化后的評估函數(shù)g如公式（3）、公式（4）所示。

（3）

式中：dynamic（x，y）為動態(tài)函數(shù)。

（4）

式中：l1、l2、l3、l4和l5為預(yù)估軌跡中5個均分點至坐標(biāo)（a，b）

的最佳距離；ω為這5個點至最佳距離的平均加權(quán)數(shù)。當(dāng)ω為最小值時，預(yù)估路徑最接近最佳距離，算法會優(yōu)先選擇該路徑，因此算法規(guī)劃的路徑更接近最佳距離。

2.2 調(diào)整運動機制

當(dāng)規(guī)避障礙物時，算法需要有較強的搜索能力和避障能力，當(dāng)進(jìn)行規(guī)劃時，普通動態(tài)窗口算法易出現(xiàn)權(quán)重系數(shù)不變的問題。如果動態(tài)障礙物速度較快，就會導(dǎo)致避障遲滯。因此，引入動態(tài)調(diào)整機制，當(dāng)機器人靠近障礙物時，動態(tài)調(diào)整數(shù)值k2開始增加，避障能力顯著提升。計算機器人位置與障礙物的最佳距離，如公式（5）～公式（7）所示。

（5）

（6）

（7）

式中：wdo為機器人與障礙物之間的距離；（a1，b1）為機器人此時的坐標(biāo)；（a2，b2）為障礙物此時的坐標(biāo)；wdl為起點與終點之間的距離；（a3，b3）為起點坐標(biāo)；（a4，b4）為終點坐標(biāo)；xt為機器人處于t時刻的線速度；yt為機器人處于t時刻的角速度。k2數(shù)值與角速度成正比，與wdo成反比，距障礙物越近，k2數(shù)值越大，避障能力越好。

2.3 設(shè)定動態(tài)航向

在運動過程中，如果機器人與目標(biāo)柵格存在角度差，則可能會繞行，因此本文提出以起點與首個目標(biāo)點的連線水平夾角來設(shè)定動態(tài)航向。為避免錯誤選擇子目標(biāo)點導(dǎo)致角度差，應(yīng)在篩選周圍節(jié)點后根據(jù)距離設(shè)置評估函數(shù)。設(shè)（a3，b3）為起點坐標(biāo)，（a5，b5）為無障礙物可行節(jié)點坐標(biāo)，（a6，b6）為目標(biāo)節(jié)點坐標(biāo)。如果起點附近沒有障礙物，那么均為可通過區(qū)域，h為下一個可通過節(jié)點，根據(jù)可通過節(jié)點距離來計算函數(shù)u的具體數(shù)值，如公式（8）所示。

（8）

式中：n1為可通過節(jié)點距目標(biāo)節(jié)點與當(dāng)前節(jié)點之和的最佳路徑；n2為可通過節(jié)點距目標(biāo)節(jié)點與當(dāng)前節(jié)點之和的最長路徑。h越接近目標(biāo)點，函數(shù)u越大；h越接近終點，函數(shù)u越小。算法優(yōu)化后機器人會直接移動至下一個可通過節(jié)點，來避免路徑冗余。

3 算法路徑規(guī)劃過程

在既定環(huán)境中，優(yōu)化動態(tài)窗口算法負(fù)責(zé)全局規(guī)劃，提取關(guān)鍵點作為局部目標(biāo)點，當(dāng)出現(xiàn)新增障礙物時進(jìn)行局部規(guī)劃，保證全局路徑最佳。算法路徑規(guī)劃過程包括以下5個步驟。1）采用柵格法在蜂鳥云創(chuàng)建模擬地圖，對各個柵格進(jìn)行編號，區(qū)分障礙物柵格與自由柵格，設(shè)置機器人的運動起止點。2）初始化處理算法參數(shù)，設(shè)時間步長λt、動態(tài)因子β、最大迭代次數(shù)Nmax、最大線速度xmax、最大角速度ymax、最大線加速度axmax和最大角加速度aymax等。3）令最大迭代次數(shù)Nmax=200，開始迭代，如果迭代次數(shù)達(dá)到最多，就可得到全局最佳路徑。4）提取局部子目標(biāo)點，使用算法完成采樣，使用線速度與角速度（x，y）預(yù)測運動軌跡，根據(jù)結(jié)果選出最佳組合規(guī)劃局部路徑。5）判定局部子目標(biāo)點，如果是全局目標(biāo)點，則說明已到達(dá)終點，在輸出最佳路徑后結(jié)束運算。

4 仿真試驗與分析

為驗證算法有效性，采用不同算法進(jìn)行仿真試驗。仿真平臺選用蜂鳥云，操作系統(tǒng)為Windows 11。采用柵格法對環(huán)境進(jìn)行建模，將優(yōu)化動態(tài)窗口算法與傳統(tǒng)哈里斯鷹算法進(jìn)行比較，再將優(yōu)化動態(tài)窗口算法放置在新增障礙物的模擬環(huán)境中進(jìn)行試驗。

4.1 環(huán)境建模

采用柵格法構(gòu)建節(jié)點能夠精確表現(xiàn)墻體、路面以及障礙物的位置，雖然存在部分傾斜與重影，但是不會對后續(xù)規(guī)劃產(chǎn)生影響。將機器人處理為小質(zhì)點來忽略體積，降低誤差。柵格地圖環(huán)境大小為20 m×20 m，白色區(qū)域為自由區(qū)域，黑色柵格為障礙物，相鄰柵格距離為1 m。坐標(biāo)（0，0）為原點，從左上至右下依次計數(shù)，坐標(biāo)（1，19）為起點，坐標(biāo)（19，1）為終點。

4.2 設(shè)置參數(shù)

傳統(tǒng)哈里斯鷹算法參數(shù)設(shè)置如下。粒子總數(shù)為50，動態(tài)因子β1=β2=2，最大迭代次數(shù)Nmax=200；優(yōu)化動態(tài)窗口算法的參數(shù)設(shè)置如下。線加速度最大值axmax=0.2 m/s2，角加速度最大值aymax=20 rad/s，膨脹半徑Robstacle=0.2 m，評價函數(shù)權(quán)重系數(shù)分別為γ1=0.05，γ2=0.2，γ3=0.2，根據(jù)參數(shù)觀察軌跡。

4.3 靜態(tài)路徑規(guī)劃試驗

為檢驗在靜態(tài)布局地圖中路徑規(guī)劃算法的有效性，對2種算法在同一張地圖上進(jìn)行試驗，智能機器人靜態(tài)路徑規(guī)劃比較如圖1所示，黑色實線為2種不同算法計算得到的運動軌跡，統(tǒng)計搜索節(jié)點數(shù)量、路徑長度、轉(zhuǎn)折次數(shù)、路徑節(jié)點數(shù)量以及時間，2種算法性能比較見表1。由表1可知，傳統(tǒng)哈里斯鷹算法規(guī)劃的路徑冗余點與轉(zhuǎn)折次數(shù)較多，當(dāng)運動時會搜索許多節(jié)點，導(dǎo)致運行效率顯著降低；優(yōu)化動態(tài)窗口算法規(guī)劃的路徑冗余點與轉(zhuǎn)折次數(shù)較少，與傳統(tǒng)哈里斯鷹算法相比，其運行效率具有一定優(yōu)勢。

雖然2種算法目標(biāo)點相同，但是優(yōu)化動態(tài)窗口算法參與搜索的節(jié)點數(shù)量減少了18個，時間也縮短了1.33 s；對關(guān)鍵節(jié)點進(jìn)行深度篩選后，路徑節(jié)點數(shù)量減少了11個，轉(zhuǎn)折次數(shù)減少了10次。優(yōu)化動態(tài)窗口算法能夠減少轉(zhuǎn)折次數(shù)和搜索節(jié)點數(shù)量，節(jié)約計算時間。

4.4 動態(tài)路徑規(guī)劃試驗

在柵格地圖的優(yōu)化動態(tài)窗口算法的運動軌跡上放置灰色模塊作為新增障礙物（如圖2所示），并將2種算法進(jìn)行對比試驗，由此驗證優(yōu)化動態(tài)窗口算法的避障能力，優(yōu)化動態(tài)窗口算法運動軌跡如圖3所示。

由圖3可知，如果新增障礙物處于全局最佳路徑，那么傳統(tǒng)哈里斯鷹算法不能有效避障，動態(tài)性能欠佳；優(yōu)化動態(tài)窗口算法所得路徑在規(guī)避了所有新增障礙的基礎(chǔ)上更順滑，符合雙輪差速機械結(jié)構(gòu)的運動特點，獲得了優(yōu)質(zhì)運動軌跡，證明了算法的有效性。

5 結(jié)語

綜上所述，當(dāng)優(yōu)化動態(tài)窗口算法應(yīng)用于動態(tài)環(huán)境時，可顯著減少節(jié)點數(shù)量，提高運行效率，對關(guān)鍵節(jié)點進(jìn)行深度篩選后能夠減少轉(zhuǎn)折次數(shù)，解決了當(dāng)運動時無法躲避新增障礙物的問題，提升了軌跡順滑度，保證在最佳全局路徑上，機器人可實時避障。本研究僅在新增障礙物的環(huán)境下進(jìn)行仿真試驗，未過多關(guān)注移動障礙物，在后續(xù)研究中可將其作為參照，提升應(yīng)用效果。

參考文獻(xiàn)

[1]黃敬堯，劉洪宇，武慧慧，等.基于改進(jìn)的五行環(huán)算法的移動機器人路徑規(guī)劃[J].傳感器與微系統(tǒng)，2023，42（10）：143-146.

[2]楊帆，李瑋，嚴(yán)天宏.基于梯度優(yōu)化的移動機器人路徑規(guī)劃算法[J].現(xiàn)代電子技術(shù)，2023，46（21）：99-104.

[3]黃勇強，劉硯菊，宋建輝，等.基于改進(jìn)A*算法與優(yōu)化DWA的差速移動機器人路徑規(guī)劃[J]. 沈陽理工大學(xué)學(xué)報，2023，42（6）：25-33.

[4]常新中，岳哲鵬，郜海超，等.基于改進(jìn)粒子群的混合金豺優(yōu)化算法的移動機器人路徑規(guī)劃[J].中原工學(xué)院學(xué)報，2023，34（4）：24-29.