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

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

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

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

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

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

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

（1）

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

（2）

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

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

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

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

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

（3）

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

（4）

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

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

2.2 調(diào)整運(yùn)動(dòng)機(jī)制

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

（5）

（6）

（7）

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

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

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

（8）

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

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

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

4 仿真試驗(yàn)與分析

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

4.1 環(huán)境建模

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

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

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

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

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

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

4.4 動(dòng)態(tài)路徑規(guī)劃試驗(yàn)

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

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

5 結(jié)語

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

參考文獻(xiàn)

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

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

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

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