采用社會(huì)約束自適應(yīng)動(dòng)態(tài)窗口法的服務(wù)機(jī)器人路徑規(guī)劃

摘要：針對(duì)傳統(tǒng)動(dòng)態(tài)窗口法在行人密集環(huán)境下動(dòng)態(tài)路徑規(guī)劃存在靈活性差、效率低、安全性缺乏等問題，提出一種社會(huì)交互空間下基于社會(huì)約束自適應(yīng)動(dòng)態(tài)窗口法（social_DWA），并采用其解決服務(wù)機(jī)器人局部路徑規(guī)劃問題。首先，采用非對(duì)稱高斯公式對(duì)單行人以及多人群組交互空間進(jìn)行模型化描述；其次，在傳統(tǒng)動(dòng)態(tài)窗口法的基礎(chǔ)上，采用動(dòng)態(tài)行人方位角約束對(duì)動(dòng)態(tài)行人進(jìn)行避讓；改進(jìn)距離評(píng)價(jià)函數(shù)，分類決策與行人、多人群組、一般障礙物的安全距離；最后，提出速度權(quán)重自適應(yīng)調(diào)整策略，優(yōu)化服務(wù)機(jī)器人在途經(jīng)不同密集度社會(huì)交互區(qū)域時(shí)的移動(dòng)速度。為驗(yàn)證算法有效性，在兩種模擬社會(huì)場(chǎng)景下，先后開展了social_DWA算法與傳統(tǒng)DWA算法、FIDWA算法的路徑規(guī)劃仿真對(duì)比實(shí)驗(yàn)。結(jié)果表明：采用social_DWA算法所消耗的運(yùn)動(dòng)時(shí)間在場(chǎng)景1中較傳統(tǒng)DWA和FIDWA算法分別縮短了1.53、0.43 s，在場(chǎng)景2中較傳統(tǒng)DWA和FIDWA算法分別縮短了26.3、2.86 s;相較于傳統(tǒng)DWA算法和FIDWA算法，social_DWA算法能保持有效的行人安全距離，并使運(yùn)行軌跡更加合理。social_DWA算法在行人避讓、環(huán)境適應(yīng)能力等方面具有一定的優(yōu)越性。

關(guān)鍵詞：服務(wù)機(jī)器人；路徑規(guī)劃；動(dòng)態(tài)窗口法；參數(shù)自適應(yīng)；人性化

中圖分類號(hào)：TP242.6.文獻(xiàn)標(biāo)志碼：A

DOI：10.7652/xjtuxb202405005.文章編號(hào)：0253-987X（2024）05-0042-10

Path Planning of Service Robot Based on Social Dynamic Windows Approach Algorithm

Abstract：To address issues such as poor flexibility， low efficiency， and safety concerns in dynamic path planning in the crowded pedestrian environments， an adaptive dynamic window method based on social constraints （social_DWA） in the social interaction space is proposed， and it is used to solve local path planning problems of service robots. Firstly， the asymmetric Gaussian formula is employed to model interaction spaces of single pedestrian and groups. Secondly， based on the original dynamic window method， the dynamic pedestrian azimuth constraint is implemented to steer clear of dynamic pedestrians. Enhancements are made to the distance evaluation function to classify and ascertain the safety distances from pedestrians， pedestrian groups， and general obstacles. Finally， an adaptive adjustment strategy of speed weight is proposed to optimize the movement speed of service robots while passing through social interaction areas with varying densities. To verify the effectiveness of the algorithm， the path planning simulation experiments of the social_DWA algorithm， the traditional DWA algorithm， and the FIDWA algorithm are carried out in two simulated social scenarios featuring different complexities. The results show that the motion time consumed by the social_DWA algorithm is reduced by 1.53 and 0.43 s compared with the traditional DWA algorithms and FIDWA algorithms in scenario 1， and 26.3 and 2.86 s lower than the traditional DWA algorithms and FIDWA algorithms in scenario 2， respectively.Compared with the traditional DWA algorithm and FIDWA algorithm， the social_DWA algorithm maintains an effective pedestrian safety distance and ensures more rational running trajectories. The validation confirms the social_DWA algorithm’s superiority in pedestrian avoidance and environmental adaptability.

Keywords：service robots; path planning; dynamic windows approach; self-adaptation; humanization

在《中國制造2025》規(guī)劃中，機(jī)器人技術(shù)被確立為核心發(fā)展的優(yōu)先領(lǐng)域之一。與此同時(shí)，先進(jìn)的智能機(jī)器人技術(shù)正展現(xiàn)出強(qiáng)勁的發(fā)展勢(shì)頭，并已在醫(yī)療護(hù)理、家庭服務(wù)、國家安全防御以及工業(yè)生產(chǎn)等諸多領(lǐng)域?qū)崿F(xiàn)廣泛應(yīng)用。在這個(gè)過程中，路徑規(guī)劃作為機(jī)器人技術(shù)的關(guān)鍵領(lǐng)域，也迎來了蓬勃發(fā)展［1-3］。

服務(wù)機(jī)器人局部路徑規(guī)劃相較于全局路徑規(guī)劃，更側(cè)重于依據(jù)實(shí)時(shí)獲取的外部傳感器信息規(guī)劃出局部路徑，以適應(yīng)多變的動(dòng)態(tài)環(huán)境。在外部環(huán)境信息約束的基礎(chǔ)上，局部路徑規(guī)劃器可規(guī)劃出一條既滿足安全躲避局部環(huán)境中障礙物，又適合機(jī)器人移動(dòng)的路徑。傳統(tǒng)的局部路徑規(guī)劃方法包括D*［4］算法、人工勢(shì)場(chǎng)法［5-8］、動(dòng)態(tài)窗口法［9-12］及智能優(yōu)化算法［13-16］，其中動(dòng)態(tài)窗口法（dynamic windows approach，DWA）是一種成熟的機(jī)器人局部路徑規(guī)劃算法。DWA可通過評(píng)價(jià)函數(shù)在速度空間中篩選出最優(yōu)的速度組合作為控制指令進(jìn)一步傳遞到機(jī)器人底盤，控制機(jī)器人安全、高效地移動(dòng)。但是，傳統(tǒng)動(dòng)態(tài)窗口法在求解局部行人環(huán)境下的動(dòng)態(tài)路徑規(guī)劃問題時(shí)，存在靈活性差、效率低、缺乏安全性等缺陷［17］。

為了實(shí)現(xiàn)社會(huì)環(huán)境下服務(wù)機(jī)器人局部路徑規(guī)劃、及時(shí)規(guī)避行人、保證交互環(huán)境中人的安全性等導(dǎo)航任務(wù)，確定人類的活動(dòng)區(qū)域是至關(guān)重要的環(huán)節(jié)。在個(gè)人社會(huì)活動(dòng)區(qū)域中，將機(jī)器人或者其他行人不可隨便闖入的一部分范圍定義為個(gè)人社會(huì)空間，即社會(huì)舒適性交互空間。根據(jù)Hall提出的同心圓理論［18］，基于距離要素將個(gè)人周圍的特定的區(qū)域由近到遠(yuǎn)依次劃分為：親密空間，（0，0.45］ m；個(gè)人空間（0.45，1.20］ m；社交空間（1.20，3.6］ m；公共空間，大于3.60 m。社會(huì)空間的舒適性受行人直觀感受以及外界客觀原因影響，如可視范圍的大小、行走方向及速度、其他行人接近程度等。

此外，針對(duì)人類社會(huì)活動(dòng)區(qū)域的描述，Calderita等［19］提出了一個(gè)引入依賴時(shí)間社會(huì)映射的導(dǎo)航框架，使用非對(duì)稱高斯函數(shù)對(duì)個(gè)人交互空間建模，該函數(shù)由兩個(gè)方向的橢圓函數(shù)構(gòu)成。Vega等［20］提出了使用自適應(yīng)空間密度函數(shù)在人類靜態(tài)環(huán)境中的社會(huì)導(dǎo)航，根據(jù)人群的空間排列有效地聚類，并使用非對(duì)稱高斯函數(shù)和全局密度函數(shù)構(gòu)建數(shù)學(xué)模型定義場(chǎng)景中個(gè)人空間和每個(gè)物體對(duì)應(yīng)的空間。Zhang等［21］采用非物理的虛擬區(qū)域表示人類活動(dòng)范圍，實(shí)時(shí)調(diào)整機(jī)器人的導(dǎo)航方式。Daza等［22］引入近場(chǎng)理論，通過分析機(jī)器人與機(jī)器人、人與人、人與機(jī)器人不同狀態(tài)和彼此間社會(huì)距離，做出距離調(diào)整以預(yù)留合適的空間。

服務(wù)機(jī)器人在局部動(dòng)態(tài)社會(huì)環(huán)境下的路徑規(guī)劃，既要考慮行人社會(huì)行為及活動(dòng)區(qū)域，又得考慮合適的局部路徑規(guī)劃方法。近些年來，國內(nèi)外學(xué)者針對(duì)這兩個(gè)問題展開了一系列研究。Ngo等［23］提出建立維護(hù)人類心理安全的活動(dòng)區(qū)域框架，實(shí)時(shí)更新人的狀態(tài)到個(gè)人空間模型中，采用優(yōu)化動(dòng)態(tài)窗口法代價(jià)函數(shù)進(jìn)行社會(huì)交互環(huán)境的動(dòng)態(tài)導(dǎo)航。趙青等［24］利用橢圓區(qū)域描述行人潛在的活動(dòng)區(qū)域，對(duì)動(dòng)態(tài)窗口法中的評(píng)價(jià)函數(shù)進(jìn)行擴(kuò)展，將行人預(yù)測(cè)信息融入到動(dòng)態(tài)窗口法評(píng)價(jià)函數(shù)，實(shí)現(xiàn)機(jī)器人尊重行人意圖、對(duì)行人友好避讓的功能。

以上研究在動(dòng)態(tài)社會(huì)交互空間下服務(wù)機(jī)器人路徑規(guī)劃方面進(jìn)行了許多有益的探索，并取得了一定的進(jìn)展。但是，目前對(duì)社會(huì)空間模型的改進(jìn)研究，缺乏綜合性考慮人的社會(huì)行為分類，從而使人機(jī)交互舒適性較低。此外，對(duì)于動(dòng)態(tài)窗口法的改進(jìn)，仍然存在對(duì)動(dòng)態(tài)行人響應(yīng)速度慢的缺陷，面對(duì)室內(nèi)復(fù)雜性、隨機(jī)性較強(qiáng)的行人環(huán)境時(shí)，往往不能尊重環(huán)境中人類社會(huì)交互行為，影響行人的安全性和舒適性。為進(jìn)一步解決這些問題，本文采用基于非對(duì)稱高斯模型的行人建模方法，以實(shí)現(xiàn)單人、群組社交空間的分類描述，并提出基于社會(huì)約束的自適應(yīng)動(dòng)態(tài)窗口法（social dynamic windows approach，social_DWA）。在傳統(tǒng)動(dòng)態(tài)窗口法的基礎(chǔ)上，引入動(dòng)態(tài)行人方位角約束，實(shí)現(xiàn)機(jī)器人從移動(dòng)方向上及時(shí)規(guī)避動(dòng)態(tài)行人；提出分類決策的距離評(píng)價(jià)函數(shù)，實(shí)現(xiàn)機(jī)器人以不同的安全距離規(guī)避一般障礙物、單行人、多人群組，同時(shí)避免過多路徑消耗；采用速度權(quán)重值自適應(yīng)調(diào)整策略，以實(shí)時(shí)控制機(jī)器人行經(jīng)不同密集度人類社交區(qū)域時(shí)的速度，提高路徑執(zhí)行效率以及局部導(dǎo)航的接受度和舒適性；最后，通過仿真實(shí)驗(yàn)，驗(yàn)證social_DWA算法的有效性及環(huán)境適應(yīng)能力。

1.行人活動(dòng)區(qū)域建模

1.1.非對(duì)稱高斯公式的定義

基于非對(duì)稱高斯公式模型化表示社會(huì)交互空間，以更好地描述行人活動(dòng)區(qū)域。將社會(huì)交互空間模型內(nèi)的每一點(diǎn)賦予函數(shù)值，其中函數(shù)值越大的點(diǎn)，代表距離行人私密區(qū)域越近，當(dāng)機(jī)器人越接近此位置時(shí)，對(duì)人的安全性與舒適性影響越大。傳統(tǒng)二維對(duì)稱高斯公式數(shù)學(xué)表達(dá)式為

當(dāng)式（1）在X、Y軸方向都非對(duì)稱時(shí)，定義其為非對(duì)稱高斯函數(shù)，用于表示個(gè)人空間周圍距離出現(xiàn)變化的狀況，其中，將（xo，yo）定義為行人中心，建立的非對(duì)稱高斯函數(shù)公式為

式中：A為幅值；α=φ－φr，φr為旋轉(zhuǎn)角（表示人的正面朝向角度）；α∈［－π，π］，當(dāng)α屬于不同象限時(shí)，決定σx、σy的不同取值；定義φr正方向、反方向、正方向左側(cè)、正方向右側(cè)的高斯分布的標(biāo)準(zhǔn)差分別為σf、σb、σl、σr。

1.2.單個(gè)人社交模型的構(gòu)建

基于以上非對(duì)稱高斯模型建立個(gè)人社交模型，如圖1所示，描述個(gè)人在靜止、運(yùn)動(dòng)狀態(tài)下社交活動(dòng)區(qū)域。設(shè)定如下參數(shù)選取規(guī)則。

（1）如圖1（a）所示，單個(gè)人社會(huì)空間中，以0.5 m×0.5 m的紅色方形區(qū)域表示禁止機(jī)器人闖入的私密區(qū)域，設(shè)定此區(qū)域內(nèi)函數(shù)值為無窮大；定義Df、Db、Dl、Dr分別為前、后、左、右4個(gè)方向的個(gè)人空間最遠(yuǎn)距離。

（2）如圖1（b）所示，當(dāng)行人處于靜止?fàn)顟B(tài)時(shí)，依據(jù)Hall的親近性理論［18］，此時(shí)為個(gè)人空間。由于行人對(duì)正前面區(qū)域相對(duì)于身后區(qū)域更敏感，設(shè)個(gè)人空間正前方最遠(yuǎn)距離Df=1.2 m，正后方最遠(yuǎn)距離Db=1.1 m；此外，參考到行人一般有靠右行的社會(huì)習(xí)慣（在國內(nèi)），右方注意力略強(qiáng)于左方，設(shè)定人個(gè)人空間右方最遠(yuǎn)距離Dr=1.0 m，左方最遠(yuǎn)距離Dl=0.9 m。

（3）如圖1（c）所示，當(dāng)行人處于運(yùn)動(dòng)狀態(tài)時(shí)，依據(jù)行人的移動(dòng)注意力社會(huì)習(xí)慣，當(dāng)行人移動(dòng)速度越快時(shí)，對(duì)于人體前方區(qū)域的關(guān)注范圍會(huì)越來越大，因此定義Dvf為動(dòng)態(tài)值，取值變化如下

Dvf=Df+vΔt （3）

式中：Df=1.2 m；v表示行人移動(dòng)速度，取v為動(dòng)態(tài)速度；Δt=1 s，為行人決策時(shí)間。

設(shè)定行人所在的中心點(diǎn)坐標(biāo)（x，y）=（0，0）；幅值A(chǔ)取100；將非對(duì)稱高斯模型（個(gè)人空間區(qū)域）最外側(cè)邊緣位置點(diǎn)函數(shù)值設(shè)為0.01；依據(jù)Df、Db、Dl、Dr取值，采用式（4）最終可確定社交模型前、后、左、右4個(gè)方向上高斯分布的標(biāo)準(zhǔn)差σf、σb、σl、σr的取值。σi 的計(jì)算公式如下

式中：Do=0.5 m；下角i表示方向。

1.3.多人群組社交模型的構(gòu)建

服務(wù)機(jī)器人在社會(huì)環(huán)境中，除了會(huì)遇到處于靜止、運(yùn)動(dòng)狀態(tài)的單行人外，還會(huì)遇到兩人、多人群組交互的狀況。為此，本文在單個(gè)人社交模型的基礎(chǔ)上，建立群組的社會(huì)交互空間模型，如圖2所示。設(shè)定如下規(guī)則。

規(guī)則1：社會(huì)距離判斷。局部地圖中，在服務(wù)機(jī)器人當(dāng)前可達(dá)檢測(cè)范圍內(nèi)，行人i（i=1，2，3，…，n）的二維坐標(biāo)（xi，yi）逐一輸出，以此為中心位置，分別建立個(gè)人社會(huì)空間模型，定義第i個(gè)人空間區(qū)域內(nèi)任一點(diǎn)p的社會(huì)約束為fopi。由式（5）計(jì)算任意兩個(gè)行人間社交距離

基于社交距離式（6）對(duì)人群分類

規(guī)則2：群組交互區(qū)域社會(huì)約束。如圖2（a）所示，當(dāng)3個(gè)行人相互之間社交距離滿足多人交互社會(huì)距離時(shí)，進(jìn)行多人高斯模型建立，得到交互區(qū)域，如圖2（b）所示。在交互區(qū)域內(nèi)的點(diǎn)，越接近3個(gè)個(gè)體的中心位置，其安全性越低，等高線上的任意點(diǎn)，依據(jù)顏色不同，對(duì)應(yīng)不同的高斯函數(shù)值fgp。

最終，在整個(gè)群組的社會(huì)交互空間中，每一個(gè)點(diǎn)的社會(huì)代價(jià)函數(shù)值fgp，依據(jù)單個(gè)人社會(huì)空間函數(shù)值fo[KG-*5]pi、比例系數(shù)k和群組交互區(qū)域函數(shù)值fvpj求平均值，即fgp=[SX（]k（fop1+fop2+…+fopn+fvpj）[]n+1[SX）]。

2.傳統(tǒng)動(dòng)態(tài)窗口法

動(dòng)態(tài)窗口法的基本思想［10］如下：在已知機(jī)器人速度可達(dá)范圍和加減速度性能的基礎(chǔ)上，在允許的角速度、線速度約束范圍內(nèi)，提取多組速度，并依據(jù)機(jī)器人現(xiàn)有運(yùn)動(dòng)模型得出一定時(shí)間內(nèi)相應(yīng)的運(yùn)動(dòng)軌跡。然后，在目標(biāo)引導(dǎo)、安全避障、速度限制多重約束條件下，采用評(píng)價(jià)函數(shù)對(duì)軌跡組合進(jìn)行篩選，根據(jù)評(píng)價(jià)函數(shù)最優(yōu)原則得出下一時(shí)間段內(nèi)的最佳運(yùn)動(dòng)軌跡，并提取其速度組合。綜上可知，提取速度組合、評(píng)價(jià)函數(shù)是動(dòng)態(tài)窗口法的主要環(huán)節(jié)。

2.1.速度搜索空間

DWA算法速度搜索空間的定義由3部分構(gòu)成：依據(jù)機(jī)器人本身速度限制能達(dá)到的速度；機(jī)器人所處環(huán)境和確保避障安全性要求能達(dá)到的速度；機(jī)器人自身電機(jī)動(dòng)力約束能達(dá)到的速度，即加速度（線加速度、角加速度）范圍所能達(dá)到的速度。

（1）運(yùn)動(dòng)學(xué)約束。機(jī)器人本身所能達(dá)到的速度范圍為

vs={（v，ω）|v∈[vmin，vmax]， ω∈[ωmin，ωmax] （7）

式中：vmin、vmax、ωmin、ωmax分別為機(jī)器人的最低線速度、最高線速度、最低角速度、最高角速度。

（2）安全避障要求的速度限制。從避障安全性角度考慮，機(jī)器人行進(jìn)速度組合（v，ω）應(yīng)滿足在緊急情況下的制動(dòng)，即在最大加速度va和ωa作用下，在最短距離dist（v，ω）內(nèi)完成減速停車操作，公式為

（3）動(dòng)力學(xué)約束。機(jī)器人的電機(jī)性能（動(dòng)力學(xué)約束）是有限的，直接決定機(jī)器人的加減速度性能也是有限的，在下一時(shí)間段內(nèi)依據(jù)最大加速度（va和ωa）會(huì)有相應(yīng)的速度組合限制范圍

式中：vc、ωc、dt分別為機(jī)器人的當(dāng)前時(shí)刻線速度、當(dāng)前角速度、時(shí)間步長。

在動(dòng)態(tài)窗口范圍內(nèi)，綜合以上3部分速度限制，得到DWA算法最終的速度搜索空間應(yīng)為3種約束條件下速度集合的交集部分

vr=vs∩vb∩vd（10）

2.2.評(píng)價(jià)函數(shù)

在篩除以上不滿足運(yùn)動(dòng)學(xué)、碰撞、動(dòng)力約束采樣速度后，在符合要求的速度空間vr中仍然有較多的可行速度組合以及與其對(duì)應(yīng)的較多可行運(yùn)動(dòng)軌跡。采用評(píng)價(jià)函數(shù)進(jìn)一步篩選可行軌跡，以此得到可行且更優(yōu)的搜索速度空間vr。傳統(tǒng)DWA算法的評(píng)價(jià)函數(shù)由以下3部分權(quán)重組成：目標(biāo)點(diǎn)方位角、障礙物距離、速度評(píng)價(jià)，具體評(píng)價(jià)函數(shù)G（v，ω）為

G（v，ω）=η（λheading（v，ω）+βdist（v，ω）+γvel（v，ω））（11）

其中：η為歸一化處理的參數(shù)；目標(biāo)點(diǎn)方位角評(píng)價(jià)函數(shù)heading（v，ω）評(píng)價(jià)軌跡點(diǎn)與目標(biāo)點(diǎn)方向的偏離程度；障礙物間距函數(shù)dist（v，ω）［25］表示采樣運(yùn)動(dòng)軌跡上點(diǎn)與障礙物間的最小距離；速度評(píng)價(jià)函數(shù)vel（v，ω）表示機(jī)器人的運(yùn)行能力，即移動(dòng)速度；λ、β、γ分別表示目標(biāo)方位角函數(shù)權(quán)值、障礙物間距函數(shù)權(quán)值、速度函數(shù)權(quán)值。以上評(píng)價(jià)函數(shù)G（v，ω）中，3個(gè)評(píng)價(jià)分量需要進(jìn)一步做歸一化處理，使得篩選的最優(yōu)軌跡應(yīng)該同時(shí)滿足趨于目標(biāo)點(diǎn)、合理避開障礙物和保持較快的移動(dòng)速度這3個(gè)條件。

3.基于social_DWA算法的路徑規(guī)劃

基于上述對(duì)傳統(tǒng)動(dòng)態(tài)窗口法和相關(guān)改進(jìn)研究存在問題的分析，本文提出基于社會(huì)約束的自適應(yīng)動(dòng)態(tài)窗口法：在傳統(tǒng)動(dòng)態(tài)窗口法評(píng)價(jià)函數(shù)的基礎(chǔ)上，先后引入動(dòng)態(tài)行人方位角評(píng)價(jià)函數(shù)hum_angle（v，ω），基于分類決策的距離評(píng)價(jià)函數(shù)Dist（v，ω）和動(dòng)態(tài)自適應(yīng)速度權(quán)值γa，得到

G′（v，ω）=η（λheading（v，ω）+βDist（v，ω）+γavel（v，ω）+μhum_angle（v，ω））（12）

式中：γa為動(dòng)態(tài)自適應(yīng)速度權(quán)值；μ為行人方位角評(píng)價(jià)函數(shù)權(quán)值。

3.1.動(dòng)態(tài)行人方位角約束

在模擬周期內(nèi)，機(jī)器人除了趨近于目標(biāo)位置之外，對(duì)于環(huán)境中的動(dòng)態(tài)移動(dòng)的行人需要及時(shí)避讓，以尊重行人活動(dòng)空間以及人與人之間的社會(huì)交互行為。因此，本文算法引入動(dòng)態(tài)行人方位角評(píng)價(jià)函數(shù)hum_angle（v，ω），在模擬周期內(nèi)，獲得機(jī)器人生成的運(yùn)動(dòng)軌跡末端方向角與一定距離范圍內(nèi)動(dòng)態(tài)行人移動(dòng)軌跡方位角的偏差角ο，如圖3所示，偏差角ο角度越大，則hum_angle（v，ω）評(píng)價(jià)函數(shù)的值越大，表示機(jī)器人移動(dòng)方向與行人偏離程度越大，行人安全性、舒適性越好。

3.2.基于分類決策的距離評(píng)價(jià)函數(shù)

傳統(tǒng)動(dòng)態(tài)窗口法在局部避障時(shí)，將局部環(huán)境中出現(xiàn)的行人、人群都視作普通障礙物處理，導(dǎo)致機(jī)器人在人類活動(dòng)區(qū)域附近繞行時(shí)距離過近，影響行人、人群正常的交互活動(dòng)。為此，本文中提出基于分類決策的距離函數(shù)Dist（v，ω），表示模擬運(yùn)動(dòng)軌跡與附近一定范圍內(nèi)的一般障礙物、單個(gè)行人可擴(kuò)展區(qū)域、人群社交區(qū)域之間的最小距離，Dist（v，ω）越大，活動(dòng)區(qū)域越安全、機(jī)器人移動(dòng)行為對(duì)人類越友好。同時(shí)，Dist（v，ω）不可過大，避免目標(biāo)函數(shù)中距離要素過大。參照一般障礙物膨脹距離、行人舒適性社交規(guī)則，設(shè)定閾值半徑R∈［1.0，1.2］ m，Dist（v，ω）的取值過程為

情形1：dist1（v，ω）表示機(jī)器人在模擬周期內(nèi)采樣軌跡接近一般障礙物時(shí)的最小距離，閾值設(shè)為R，即dist1（v，ω）≤R。

情形2：dist2（v，ω）表示機(jī)器人在接近單個(gè)行人可擴(kuò)展社交區(qū)域時(shí)的最小距離，閾值設(shè)為3R，即dist2（v，ω）≤3R。

情形3：dist3（v，ω）表示機(jī)器人接近人群社會(huì)交互區(qū)域時(shí)的最小距離，閾值設(shè)置為3.5R，即dist3（v，ω）≤3.5R。

最小距離評(píng)價(jià)函數(shù)最終取三者的最大值，即

Dist（v，ω）=max{dist1（v，ω）， dist2（v，ω）， dist3（v，ω）}（14）

以上基于分類決策的距離評(píng)價(jià)函數(shù)Dist（v，ω），在模擬周期內(nèi)，所篩選的機(jī)器人運(yùn)動(dòng)軌跡能針對(duì)一般障礙物、單個(gè)行人、多個(gè)體人群等不同類型的對(duì)象，分別采取不同的距離評(píng)價(jià)函數(shù)以及不同的距離閾值。在接近一般障礙物時(shí)的最小距離dist1（v，ω），在滿足安全避障的前提下，減少不必要的路徑消耗；在接近單個(gè)行人時(shí)，距離評(píng)價(jià)要素dist2（v，ω），以及接近多個(gè)體群組時(shí)，距離評(píng)價(jià)要素dist3（v，ω），能有效規(guī)避行人活動(dòng)、群組交互潛在的區(qū)域，同時(shí)避免行人區(qū)域的過多繞行。

分類決策的改進(jìn)策略能有效提高社會(huì)環(huán)境中人類的安全性、舒適性，提高局部導(dǎo)航的靈活性；同時(shí)，有效縮短局部規(guī)劃路徑長度，避免機(jī)器人駛?cè)霃?fù)雜動(dòng)態(tài)環(huán)境中因距離權(quán)重過大而導(dǎo)致的停滯，或者過多繞行駛?cè)胱杂蓞^(qū)，偏離目標(biāo)點(diǎn)。

3.3.速度權(quán)重值自適應(yīng)調(diào)整策略

本文在傳統(tǒng)動(dòng)態(tài)窗口法的基礎(chǔ)上，先后引入動(dòng)態(tài)行人方位角評(píng)價(jià)函數(shù)、基于分類決策的距離評(píng)價(jià)函數(shù)。除此之外，速度評(píng)價(jià)函數(shù)作為同樣至關(guān)重要的評(píng)價(jià)分量，若繼續(xù)沿用傳統(tǒng)固定權(quán)值，會(huì)導(dǎo)致速度函數(shù)在整個(gè)評(píng)價(jià)體系中失衡［26］。同時(shí)，固定的速度權(quán)值對(duì)復(fù)雜、動(dòng)態(tài)行人環(huán)境的適應(yīng)性不足：當(dāng)機(jī)器人通過密集行人區(qū)域時(shí)，速度不能及時(shí)調(diào)整，則延長路徑繞行以確保安全性；當(dāng)機(jī)器人通過行人較少且相對(duì)曠闊區(qū)域時(shí)，速度又不能及時(shí)恢復(fù)，極大影響了路徑規(guī)劃效率。為此，本文中采用自適應(yīng)速度權(quán)值調(diào)整策略，根據(jù)機(jī)器人一定檢測(cè)范圍內(nèi)行人的疏密程度，更加靈活地動(dòng)態(tài)調(diào)整速度權(quán)重值，以適應(yīng)復(fù)雜的行人環(huán)境。

（2）自適應(yīng)權(quán)重值γa的設(shè)計(jì)。當(dāng)機(jī)器人處于行人密集區(qū)域內(nèi)通行時(shí)，鑒于安全性和行人舒適性，將檢測(cè)區(qū)域內(nèi)機(jī)器人與行人間的最短距離設(shè)為lmin。根據(jù)機(jī)器人的最大移動(dòng)速度和最大線加速度，設(shè)定制動(dòng)距離臨界值lb，其中，制動(dòng)性能越好，所需要的制動(dòng)距離臨界值越小，計(jì)算公式為

當(dāng)機(jī)器人在行人密集活動(dòng)區(qū)域移動(dòng)時(shí)，設(shè)置動(dòng)態(tài)的速度權(quán)重值γa。依據(jù)機(jī)器人與檢測(cè)范圍內(nèi)行人最小距離lmin與制動(dòng)距離臨界值lb間關(guān)系，實(shí)時(shí)調(diào)整權(quán)重值，以此動(dòng)態(tài)調(diào)整機(jī)器人通過行人區(qū)域的速度，具體過程為

式中：γm為機(jī)器人以最安全速度通過行人最密集區(qū)域時(shí)權(quán)重值；γM為機(jī)器人以最快速度通過行人區(qū)域時(shí)的權(quán)重值；ρ為調(diào)節(jié)指數(shù)，其取值范圍為［1，2］。

3.4.social_DWA算法流程

步驟1 輸入機(jī)器人初始狀態(tài)X{［x0，y0］，Δyaw，v，ω}；輸入機(jī)器人運(yùn)動(dòng)模型Kinematic［vmax，ωmax，va，ωa，dv，dω］參數(shù)，其中dv、dw分別為線速度分辨率和角速度分辨率；輸入目標(biāo)位置Goal［xg，yg］；輸入4個(gè)評(píng)價(jià)函數(shù)分量heading、Dist、vel、hum_angle的初始值。

步驟2 根據(jù)機(jī)器人傳感器（激光雷達(dá)、相機(jī)）信息獲取實(shí)時(shí)更新的障礙物、行人位置信息，由式（15）計(jì)算行人區(qū)域中相鄰個(gè)體間位置間距，并判斷機(jī)器人是否進(jìn)入密集行人區(qū)域。

步驟3 在劃定的半徑的圓環(huán)形檢測(cè)區(qū)域內(nèi)，計(jì)算最短行人間距l(xiāng)min，計(jì)算制動(dòng)距離臨界值lb，由式（17）計(jì)算動(dòng)態(tài)速度權(quán)重值γa。

步驟4 根據(jù)當(dāng)前運(yùn)動(dòng)狀態(tài)X和機(jī)器人運(yùn)動(dòng)模型Kinematic（運(yùn)動(dòng)、安全避障、動(dòng)力約束條件）計(jì)算搜索速度空間允許的速度范圍（v，ω）t。

步驟5 根據(jù)當(dāng)前運(yùn)動(dòng)狀態(tài)X和采樣速度空間（v，ω）t，計(jì)算出t～（t+1）時(shí)段的運(yùn)動(dòng)軌跡。

步驟6 根據(jù)下一時(shí)間周期內(nèi)預(yù)測(cè)的運(yùn)動(dòng)軌跡，分別計(jì)算更新評(píng)價(jià)函數(shù)heading（v，ω）、Dist（v，ω）、 vel（v，ω）、hum_angle（v，ω），并對(duì)4個(gè)評(píng)價(jià)分量進(jìn)行歸一化處理。

步驟7 根據(jù)綜合的評(píng)價(jià)函數(shù)G′（v，ω）計(jì)算出評(píng)價(jià)值最優(yōu)的速度組合（v，ω）t+1，令其作為（t+1）時(shí)刻機(jī)器人的移動(dòng)速度。

步驟8 執(zhí)行當(dāng)前評(píng)選出的最優(yōu)速度組合，判別是否抵達(dá)目標(biāo)位置，如果已抵達(dá)則結(jié)束循環(huán)；反之則返回執(zhí)行步驟2，繼續(xù)循環(huán)。

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

4.1.仿真實(shí)驗(yàn)環(huán)境

為驗(yàn)證本文所提social_DWA算法在局部動(dòng)態(tài)環(huán)境中的路徑尋優(yōu)能力、行人避讓能力（安全性）、對(duì)社會(huì)交互空間的尊重程度（舒適性），設(shè)定局部模擬環(huán)境由普通障礙物、靜態(tài)行人、動(dòng)態(tài)行人、多人群組（采用第1節(jié)中所述行人建模方法表示活動(dòng)區(qū)域）組成。仿真實(shí)驗(yàn)設(shè)定了2種模擬場(chǎng)景，具體情況如下。

場(chǎng)景1：靜態(tài)、動(dòng)態(tài)行人并存，障礙物密集隨機(jī)分布的復(fù)雜環(huán)境。

場(chǎng)景2：模擬多種社會(huì)交互行為并存的大型室內(nèi)環(huán)境。

為了評(píng)價(jià)本文所提social_DWA算法的有效性，仿真實(shí)驗(yàn)中基于以上2種場(chǎng)景參照對(duì)比傳統(tǒng)DWA算法和FIDWA算法［27］進(jìn)行局部路徑規(guī)劃驗(yàn)證，評(píng)價(jià)指標(biāo)如下。

（1）運(yùn)動(dòng)時(shí)間：機(jī)器人在不同的模擬場(chǎng)景中執(zhí)行局部路徑規(guī)劃所需的總時(shí)長，時(shí)間越短表示尋優(yōu)效率越高。

（2）迭代次數(shù)：算法在整個(gè)路徑規(guī)劃過程中總共需要執(zhí)行的主循環(huán)次數(shù)，算法迭代次數(shù)同樣反映算法的執(zhí)行效率。

（3）路徑長度：從局部環(huán)境中的起點(diǎn)（當(dāng)前位置）到目標(biāo)位置的總距離，路徑長度越短表示尋優(yōu)性能越好。

（4）安全距離：機(jī)器人在路徑執(zhí)行過程中距離行人中心位置、群組交互中心的最短距離。遵循行人舒適性社交規(guī)則：個(gè)人空間（0.45～1.20］ m、社交空間（1.20～3.60） m。安全距離越大，表示機(jī)器人避讓能力、社會(huì)交互區(qū)域的安全性越好。

仿真實(shí)驗(yàn)中采用的機(jī)器人主要參數(shù)及評(píng)價(jià)參權(quán)重值如表1所示。

4.2.仿真實(shí)驗(yàn)結(jié)果分析

基于2種仿真實(shí)驗(yàn)場(chǎng)景得到的傳統(tǒng)DWA算法、FIDWA算法、social_DWA算法路徑規(guī)劃圖如圖5～圖6所示，圖中淺綠色表示算法的預(yù)測(cè)軌跡，綠色圓圈代表安全舒適區(qū)域。表2為所有場(chǎng)景下的實(shí)驗(yàn)結(jié)果。

（1）場(chǎng)景1：靜態(tài)、動(dòng)態(tài)行人，復(fù)雜障礙物環(huán)境。場(chǎng)景1中，將模擬社會(huì)場(chǎng)景設(shè)為雙向移動(dòng)的單行人以及靜態(tài)個(gè)人，障礙物為密集且隨機(jī)分布的復(fù)雜場(chǎng)景。由圖5及表2可得：傳統(tǒng)DWA算法和FIDWA算法在目標(biāo)點(diǎn)的引導(dǎo)下，穿過密集障礙物后，繞過靜態(tài)個(gè)人空間但對(duì)動(dòng)態(tài)行人空間沒有避讓。相比之下，social_DWA算法由于引入動(dòng)態(tài)行人方位角約束和分類決策距離評(píng)價(jià)函數(shù)，在繞過靜態(tài)個(gè)人空間后，能有效避開潛在的動(dòng)態(tài)行人移動(dòng)區(qū)域，并預(yù)留安全空間（3.36 m）避讓動(dòng)態(tài)行人；social_DWA算法運(yùn)動(dòng)時(shí)間較DWA和FIDWA算法分別縮短1.53、0.43 s。由此可見，本文算法對(duì)于復(fù)雜障礙與行人并存環(huán)境有更強(qiáng)的適應(yīng)能力。

（2）場(chǎng)景2：模擬多種社會(huì)交互行為并存的大型室內(nèi)環(huán)境。在場(chǎng)景2中，模擬室內(nèi)環(huán)境下，機(jī)器人先后經(jīng)過走廊區(qū)域與單個(gè)動(dòng)態(tài)行人并行，遇到門口靜態(tài)交互的兩人群組、單個(gè)動(dòng)態(tài)行人、展板前零散的個(gè)人，而后是3人群組。由圖6及表2可以清晰看出：傳統(tǒng)DWA算法是機(jī)器人在目標(biāo)點(diǎn)的引導(dǎo)下，徑直駛?cè)雴蝹€(gè)行人潛在移動(dòng)區(qū)域后，又闖進(jìn)群組交互區(qū)域，導(dǎo)致暫時(shí)陷入死鎖狀態(tài)；FIDWA算法是機(jī)器人在目標(biāo)點(diǎn)的引導(dǎo)下，駛?cè)肓遂o態(tài)個(gè)人空間，導(dǎo)致速度損失較大；social_DWA算法由于引入了速度權(quán)值自適應(yīng)調(diào)整策略，機(jī)器人行經(jīng)不同密集度人類交互區(qū)域時(shí)靈活調(diào)節(jié)速度，運(yùn)行時(shí)間、迭代次數(shù)減少，提高了路徑尋優(yōu)效率，同時(shí)機(jī)器人先后及時(shí)避開個(gè)人空間、群組社交空間且保持安全距離（3.67 m），雖然路徑長度略有增加，但運(yùn)動(dòng)時(shí)間、迭代次數(shù)、安全距離等指標(biāo)保持了一定優(yōu)勢(shì)；social_DWA算法運(yùn)動(dòng)時(shí)間較DWA和FIDWA算法分別縮短26.3、2.86 s。由此可見，本文算法對(duì)多種社會(huì)交互行為并存的復(fù)雜環(huán)境具有一定的適應(yīng)能力。

5.結(jié).論

（1） 本文所提social_DWA算法通過引入動(dòng)態(tài)行人方位角約束有效防止機(jī)器人與社會(huì)環(huán)境中動(dòng)態(tài)行人的碰撞并預(yù)留安全空間避開潛在行人移動(dòng)區(qū)域，提高了機(jī)器人安全避讓能力；基于分類決策的距離評(píng)價(jià)函數(shù)，提高了機(jī)器人社會(huì)環(huán)境適應(yīng)能力；通過引入自適應(yīng)調(diào)整的速度權(quán)重值，提高了機(jī)器人遇到復(fù)雜社會(huì)環(huán)境（不同密集度交互區(qū)域）的速度調(diào)節(jié)能力，提高了路徑執(zhí)行效率。

（2） 局部路徑規(guī)劃仿真實(shí)驗(yàn)表明，在復(fù)雜度不同的兩組仿真實(shí)驗(yàn)場(chǎng)景下，social_DWA算法計(jì)算所得的路徑有效、合理，能有效避開潛在的動(dòng)態(tài)行人移動(dòng)區(qū)域。相比于傳統(tǒng)DWA算法和FIDWA算法，本文social_DWA算法具有較強(qiáng)的路徑尋優(yōu)能力、環(huán)境適應(yīng)能力和人機(jī)共融導(dǎo)航能力。

本文提出的social_DWA算法可以為室內(nèi)移動(dòng)服務(wù)機(jī)器人在大規(guī)模復(fù)雜工作環(huán)境中的導(dǎo)航和局部路徑規(guī)劃提供有效的解決方案，具有一定的應(yīng)用價(jià)值。此外，social_DWA算法還可以與全局路徑規(guī)劃方法相結(jié)合，提高移動(dòng)機(jī)器人的導(dǎo)航效率和穩(wěn)定性。

參考文獻(xiàn)：

［1］黃鶴， 李瀟磊， 楊瀾， 等. 引入改進(jìn)蝠鲼覓食優(yōu)化算法的水下無人航行器三維路徑規(guī)劃 ［J］. 西安交通大學(xué)學(xué)報(bào)， 2022， 56（7）： 9-18.

HUANG He， LI Xiaolei， YANG Lan， et al. Three dimensional path planning of unmanned underwater vehicle based on improved manta ray foraging optimization algorithm ［J］. Journal of Xi’an Jiaotong University， 2022， 56（7）： 9-18.

［2］劉志強(qiáng)， 何麗， 袁亮， 等. 采用改進(jìn)灰狼算法的移動(dòng)機(jī)器人路徑規(guī)劃 ［J］. 西安交通大學(xué)學(xué)報(bào)， 2022， 56（10）： 49-60.

LIU Zhiqiang， HE Li， YUAN Liang， et al. Path planning of mobile robot based on TGWO algorithm ［J］. Journal of Xi’an Jiaotong University， 2022， 56（10）： 49-60.

［3］王碩， 段蓉凱， 廖與禾. 機(jī)器人路徑規(guī)劃中快速擴(kuò)展隨機(jī)樹算法的改進(jìn)研究 ［J］. 西安交通大學(xué)學(xué)報(bào)， 2022， 56（7）： 1-8.

WANG Shuo， DUAN Rongkai， LIAO Yuhe. Research on improvement of rapidly exploring random tree algorithm in robot path planning ［J］. Journal of Xi’an Jiaotong University， 2022， 56（7）： 1-8.

［4］SADIQ A T， RAHEEM F A， ABBAS N A F. Ant colony algorithm improvement for robot arm path planning optimization based on D* strategy ［J］. International Journal of Mechanical amp; Mechatronics Engineering， 2021， 21（1）： 96-111.

［5］陳勁峰， 黃衛(wèi)華， 章政， 等. 動(dòng)態(tài)環(huán)境下基于改進(jìn)人工勢(shì)場(chǎng)法的路徑規(guī)劃算法 ［J］. 組合機(jī)床與自動(dòng)化加工技術(shù)， 2020（12）： 6-9.

CHEN Jinfeng， HUANG Weihua， ZHANG Zheng， et al. Path planning algorithm based on improved artificial potential field method in dynamic environment ［J］. Modular Machine Tool amp; Automatic Manufacturing Technique， 2020（12）： 6-9.

［6］王慶祿， 吳馮國， 鄭成辰， 等. 基于優(yōu)化人工勢(shì)場(chǎng)法的無人機(jī)航跡規(guī)劃 ［J］. 系統(tǒng)工程與電子技術(shù)， 2023， 45（5）： 1461-1468.

WANG Qinglu， WU Fengguo， ZHENG Chengchen， et al. UAV path planning based on optimized artificial potential field method ［J］. Systems Engineering and Electronics， 2023， 45（5）： 1461-1468.

［7］韓堯， 李少華. 基于改進(jìn)人工勢(shì)場(chǎng)法的無人機(jī)航跡規(guī)劃 ［J］. 系統(tǒng)工程與電子技術(shù)， 2021， 43（11）： 3305-3311.

HAN Yao， LI Shaohua. UAV path planning based on improved artificial potential field ［J］. Systems Engineering and Electronics， 2021， 43（11）： 3305-3311.

［8］LIU Weiheng， ZHENG Xin， DENG Zhihong. Dynamic collision avoidance for cooperative fixed-wing UAV swarm based on normalized artificial potential field optimization ［J］. Journal of Central South University， 2021， 28（10）： 3159-3172.

［9］FOX D， BURGARD W， THRUN S. The dynamic window approach to collision avoidance ［J］. IEEE Robotics amp; Automation Magazine， 1997， 4（1）： 23-33.

［10］LI Yue， ZHAO Jianyou， CHEN Zenghua， et al. A robot path planning method based on improved genetic algorithm and improved dynamic window approach ［J］. Sustainability， 2023， 15（5）： 4656.

［11］SUN Yicheng， ZHAO Xianliang， YU Yazhou. Research on a random route-planning method based on the fusion of the A*algorithm and dynamic window method ［J］. Electronics， 2022， 11（17）： 2683.

［12］程傳奇， 郝向陽， 李建勝， 等. 融合改進(jìn)A*算法和動(dòng)態(tài)窗口法的全局動(dòng)態(tài)路徑規(guī)劃 ［J］. 西安交通大學(xué)學(xué)報(bào)， 2017， 51（11）： 137-143.

CHENG Chuanqi， HAO Xiangyang， LI Jiansheng， et al. Global dynamic path planning based on fusion of improved A* algorithm and dynamic window approach ［J］. Journal of Xi’an Jiaotong University， 2017， 51（11）： 137-143.

［13］SHI Kun， WU Zhengtian， JIANG Baoping， et al. Dynamic path planning of mobile robot based on improved simulated annealing algorithm ［J］. Journal of the Franklin Institute， 2023， 360（6）： 4378-4398.

［14］AIT-SAADI A， MERAIHI Y， SOUKANE A， et al. A novel hybrid chaotic aquila optimization algorithm with simulated annealing for unmanned aerial vehicles path planning ［J］. Computers and Electrical Engineering， 2022， 104（Part B）： 108461.

［15］GAO Pan， ZHOU Li， ZHAO Xu， et al. Research on ship collision avoidance path planning based on modified potential field ant colony algorithm ［J］. Ocean amp; Coastal Management， 2023， 235： 106482.

［16］ZHANG He， GE Yun， SUN Chao， et al. Picking path planning method of dual rollers type safflower picking robot based on improved ant colony algorithm ［J］. Processes， 2022， 10（6）： 1213.

［17］SNCHEZ-IBEZ J R， PREZ-DEL-PULGAR C J， GARCA-CEREZO A. Path planning for autonomous mobile robots： a review ［J］. Sensors， 2021， 21（23）： 7898.

［18］HALL E T. The hidden dimension： man’s use of space in public and private ［M］. New York， USA： Anchor Books Press， 1966.

［19］CALDERITA L V， VEGA A， BUSTOS P， et al. A new human-aware robot navigation framework based on time-dependent social interaction spaces： an application to assistive robots in caregiving centers ［J］. Robotics and Autonomous Systems， 2021， 145： 103873.

［20］VEGA A， MANSO L J， MACHARET D G， et al. Socially aware robot navigation system in human-populated and interactive environments based on an adaptive spatial density function and space affordances ［J］. Pattern Recognition Letters， 2019， 118： 72-84.

［21］ZHANG Ying， ZHANG Cuihua， SHAO Xuyang. User preference-aware navigation for mobile robot in domestic via defined virtual area ［J］. Journal of Network and Computer Applications， 2021， 173： 102885.

［22］DAZA M， BARRIOS-ARANIBAR D， DIAZ-AMADO J， et al. An approach of social navigation based on proxemics for crowded environments of humans and robots ［J］. Micromachines， 2021， 12（2）： 193.

［23］NGO H Q T， LE V N， THIEN V D N， et al. Develop the socially human-aware navigation system using dynamic window approach and optimize cost function for autonomous medical robot ［J］. Advances in Mechanical Engineering， 2020， 12（12）： 1687814020979430.

［24］趙青， 陳勇， 羅斌， 等. 一種融合行人預(yù)測(cè)信息的局部路徑規(guī)劃算法 ［J］. 武漢大學(xué)學(xué)報(bào)（信息科學(xué)版）， 2020， 45（5）： 667-675.

ZHAO Qing， CHEN Yong， LUO Bin， et al. A local path planning algorithm based on pedestrian prediction information ［J］. Geomatics and Information Science of Wuhan University， 2020， 45（5）： 667-675.

［25］HOSSAIN T， HABIBULLAH H， ISLAM R， et al. Local path planning for autonomous mobile robots by integrating modified dynamic-window approach and improved follow the gap method ［J］. Journal of Field Robotics， 2022， 39（4）： 371-386.

［26］王永雄， 田永永， 李璇， 等. 穿越稠密障礙物的自適應(yīng)動(dòng)態(tài)窗口法 ［J］. 控制與決策， 2019， 34（5）： 927-936.

WANG Yongxiong， TIAN Yongyong， LI Xuan， et al. Self-adaptive dynamic window approach in dense obstacles ［J］. Control and Decision， 2019， 34（5）： 927-936.

［27］ABUBAKR O A， JARADAT M A， ABDELHAFEZ M F. Intelligent optimization of adaptive dynamic window approach for mobile robot motion control using fuzzy logic ［J］. IEEE Access， 2022， 10： 119368-119378.