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

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

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

DOI：10.7652/xjtuxb202405005.文章編號：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ī)劃中，機器人技術(shù)被確立為核心發(fā)展的優(yōu)先領(lǐng)域之一。與此同時，先進的智能機器人技術(shù)正展現(xiàn)出強勁的發(fā)展勢頭，并已在醫(yī)療護理、家庭服務(wù)、國家安全防御以及工業(yè)生產(chǎn)等諸多領(lǐng)域?qū)崿F(xiàn)廣泛應(yīng)用。在這個過程中，路徑規(guī)劃作為機器人技術(shù)的關(guān)鍵領(lǐng)域，也迎來了蓬勃發(fā)展［1-3］。

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

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

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

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

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

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

1.1.非對稱高斯公式的定義

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

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

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

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

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

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

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

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

Dvf=Df+vΔt （3）

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

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

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

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

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

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

基于社交距離式（6）對人群分類

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

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

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

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

2.1.速度搜索空間

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

（1）運動學(xué)約束。機器人本身所能達到的速度范圍為

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

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

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

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

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

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

vr=vs∩vb∩vd（10）

2.2.評價函數(shù)

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

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

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

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

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

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

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

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

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

3.2.基于分類決策的距離評價函數(shù)

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

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

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

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

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

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

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

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

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

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

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

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

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

3.4.social_DWA算法流程

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

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

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

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

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

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

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

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

4.仿真實驗結(jié)果與分析

4.1.仿真實驗環(huán)境

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

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

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

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

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

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

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

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

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

4.2.仿真實驗結(jié)果分析

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

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

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

5.結(jié).論

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

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

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

參考文獻：

［1］黃鶴， 李瀟磊， 楊瀾， 等. 引入改進蝠鲼覓食優(yōu)化算法的水下無人航行器三維路徑規(guī)劃 ［J］. 西安交通大學(xué)學(xué)報， 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］劉志強， 何麗， 袁亮， 等. 采用改進灰狼算法的移動機器人路徑規(guī)劃 ［J］. 西安交通大學(xué)學(xué)報， 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］王碩， 段蓉凱， 廖與禾. 機器人路徑規(guī)劃中快速擴展隨機樹算法的改進研究 ［J］. 西安交通大學(xué)學(xué)報， 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)華， 章政， 等. 動態(tài)環(huán)境下基于改進人工勢場法的路徑規(guī)劃算法 ［J］. 組合機床與自動化加工技術(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)化人工勢場法的無人機航跡規(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］韓堯， 李少華. 基于改進人工勢場法的無人機航跡規(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］程傳奇， 郝向陽， 李建勝， 等. 融合改進A*算法和動態(tài)窗口法的全局動態(tài)路徑規(guī)劃 ［J］. 西安交通大學(xué)學(xué)報， 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ù)測信息的局部路徑規(guī)劃算法 ［J］. 武漢大學(xué)學(xué)報（信息科學(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)動態(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.