動態(tài)環(huán)境下自主機器人的雙機制切向避障

章一鳴，姚文廣，陳海進

(1.南通大學(xué) 江蘇省專用集成電路設(shè)計重點實驗室，江蘇 南通，226001；2.傲拓科技股份有限公司，江蘇 南京，210012)

路徑規(guī)劃是指機器人根據(jù)環(huán)境或傳感器信息并在一系列約束下，規(guī)劃一條從起點到目標(biāo)點的無碰撞路徑.隨著服務(wù)型機器人在智能家居應(yīng)用中的普及，人們對于減少人為干預(yù)、提升服務(wù)型機器人自主性提出更高要求，廣大學(xué)者在該領(lǐng)域進行了大量研究并發(fā)展出許多可行算法[1-2].室內(nèi)環(huán)境具有動態(tài)隨機性的特點，局部路徑規(guī)劃算法可以感知外部環(huán)境變化的特點，彌補了全局路徑規(guī)劃算法的不足，因此該算法成為學(xué)者的研究重點之一.

Molinos 等[3]提出動態(tài)窗口樹、運動曲線環(huán)境自適應(yīng)以及多算法融合等方法，以改進動態(tài)窗口法（dynamic window approach,DWA）缺乏全局指導(dǎo)、局部最優(yōu)的缺陷；代婉玉等[4]提出沿邊轉(zhuǎn)彎、速度與轉(zhuǎn)向角消息轉(zhuǎn)換機制以及區(qū)域分級策略等方法，改進時間彈性帶法（time elastic band,TEB）控制不穩(wěn)定的缺陷.相較于DWA 和TEB，人工勢場法[5-6]的數(shù)學(xué)模型更簡單、計算復(fù)雜度更小，在實際動態(tài)環(huán)境中的實時性更優(yōu)，但存在目標(biāo)不可達(dá)、局部極小值陷阱問題.解決傳統(tǒng)人工勢場法的局部極小值陷阱問題主要采用如下3 種思路.1）改變勢場受力情況[7-8]：通過打破受力平衡狀態(tài)使機器人脫離陷阱.2）多算法融合[9]：切換其他路徑規(guī)劃算法引導(dǎo)機器人脫離陷阱.3）提前預(yù)測陷阱[10]：邊移動邊對外部環(huán)境進行預(yù)測，或者在機器人移動前提前堵住易陷入局部極小值陷阱的路徑.Yao 等[11]基于思路1）、3），提出黑洞勢場強化Q 學(xué)習(xí)的方法，在原有勢場中疊加黑洞勢場，通過黑洞力打破部分局部極小值陷阱處的受力平衡，同時通過強化學(xué)習(xí)使機器人自動學(xué)習(xí)跳出陷阱.該方法在不同環(huán)境下具有極強的適應(yīng)能力，但額外添加的黑洞勢場會使力多重疊加，并且需要預(yù)先進行大量訓(xùn)練.Min 等[12]基于思路3），提出經(jīng)驗混合人工勢場法，將基礎(chǔ)案例推理與人工勢場法結(jié)合，創(chuàng)新性地將障礙物與線型、角度型2 種內(nèi)置障礙物模型進行配準(zhǔn)，并將過去躲避障礙物的經(jīng)驗存儲在經(jīng)驗庫中.該方法適用于未知與非結(jié)構(gòu)化環(huán)境中實時避障，但無論是進行障礙物的檢索與匹配，還是進行避障經(jīng)驗的查找都要耗費大量時間.

為了有效解決局部極小值陷阱，避免使用高性能GPU 進行訓(xùn)練，實現(xiàn)動態(tài)環(huán)境下快速自主避障，本研究結(jié)合3 種思路對傳統(tǒng)人工勢場法進行改進，提出以靜態(tài)避障為主、動態(tài)避障為輔的雙機制切向避障.同時，為了獲取在亟需避障時及時避障、無須避障時快速返回最優(yōu)路徑的效果，將基于狀態(tài)決策進行全局規(guī)劃模塊與局部規(guī)劃模塊的高效結(jié)合.

1 改進人工勢場法

1.1 傳統(tǒng)人工勢場法

1986 年Khatib[13]提出人工勢場法，該方法實現(xiàn)簡單，表達(dá)式為

式中：U(p) 為合力勢場函數(shù)，Uatt(p) 為引力勢場函數(shù)，Urep(p) 為斥力勢場函數(shù)，η 為引力增益系數(shù)，ρ(p,pgoal) 為機器人與目標(biāo)點的歐氏距離，k為斥力增益系數(shù)，ρ(p,pobs) 為機器人與障礙物的歐氏距離，ρo為障礙物的斥力影響范圍.機器人對目標(biāo)點生成引力，對障礙物生成斥力，對引力與斥力勢場函數(shù)求解負(fù)梯度后即可得到引力與斥力的數(shù)值.在勢場中，機器人通過疊加引力和斥力的方式得到合力，依據(jù)合力的大小與方向判斷下一步的移動軌跡.傳統(tǒng)人工勢場法存在以下缺陷：1）陷入局部極小值陷阱，如圖1 所示；2）躲避動態(tài)障礙物效果差.

圖1 傳統(tǒng)人工勢場法的局部極小值陷阱Fig.1 Local minimum trap of traditional artificial potential field method

1.2 人工勢場法下雙機制切向避障機制

1.2.1 陷阱預(yù)測 機器人在勢場中沿梯度下降方向移動，出現(xiàn)局部極小值陷阱的原因：引力與斥力共線反向，導(dǎo)致合力為0，運動陷入停滯.Hessian矩陣是對二階導(dǎo)數(shù)的矩陣描述，可以用來檢查梯度下降方向上的點是不是局部極小值點.本研究將通過Hessian 矩陣在全局地圖中預(yù)測局部極小值陷阱.

當(dāng)函數(shù)定義在實數(shù)域且二階偏導(dǎo)數(shù)連續(xù)時，矩陣H為實對稱矩陣，進行特征分解：

式中：λ1、λ2為特征值；D=[d1,d2]，其中d1、d2為特征值所對應(yīng)的特征向量，同時也是標(biāo)準(zhǔn)正交向量.函數(shù)的方向二階導(dǎo)數(shù)表示為

在二元函數(shù)的臨界點處，二階方向?qū)?shù)的正負(fù)僅與特征值的正負(fù)相關(guān).當(dāng)特征值均為正時，Hessian矩陣正定，該點為局部極小值點；當(dāng)特征值均為負(fù)時，Hessian 矩陣負(fù)定，該點為局部極大值點.假設(shè)選取的位置坐標(biāo)與目標(biāo)點的距離為x、與障礙物的距離為y、機器人所受合力勢場為U(x,y)，那么合力勢場函數(shù)表達(dá)式為

對式（4）求偏導(dǎo)數(shù)組成二階Hessian 矩陣：

將選取的位置坐標(biāo)代入式（5）進行特征分解后，依據(jù)特征值正負(fù)判斷當(dāng)前采樣點是不是局部極小值點.

1.2.2 柵格地圖預(yù)處理 柵格地圖的分辨率大小影響地圖的精細(xì)度.當(dāng)柵格分辨率設(shè)置較大，建圖時將丟失部分細(xì)節(jié)；當(dāng)分辨率設(shè)置較小，需要更多的存儲空間和計算資源，但表示的地圖信息更加詳細(xì).為了減輕當(dāng)全局地圖相對較大時陷阱預(yù)測的計算壓力，在不影響地圖精細(xì)度的情況下，在對柵格地圖進行縮放處理后再進行陷阱預(yù)測，如圖2 所示.建圖時按圖2(a)（分辨率為0.05 m）進行柵格映射.地圖預(yù)處理時，當(dāng)20×20 柵格內(nèi)存在占據(jù)柵格時，縮放至10×10 柵格后標(biāo)注為占據(jù)柵格，否則為空閑柵格，從而將20×20 柵格內(nèi)的不規(guī)則障礙物縮放為圖2（b）側(cè)1 號占據(jù)柵格（分辨率為1 m）.經(jīng)過縮放后的柵格地圖資源占用減少0.75，需要遍歷的空閑柵格坐標(biāo)大大減少.

圖2 柵格地圖金字塔縮放Fig.2 Grid map pyramid scaling

縮放后，柵格地圖的右半部分占據(jù)柵格密度較大，相較于空閑柵格密度較大的左半部分更易陷入局部極小值陷阱.為了進一步減少陷阱預(yù)測的計算量，通過區(qū)域搜索，選取存在局部極小值陷阱概率更高的障礙物密集區(qū)域，遍歷該區(qū)域中梯度下降方向上的空閑坐標(biāo)點，計算Hessian 矩陣特征值檢測是否存在局部極小值陷阱，如圖3 所示.以縮放后全局地圖原點為起始點構(gòu)建長度為2t+1的搜索框，按從左向右、從下往上的順序?qū)⒌貓D分成數(shù)塊小區(qū)域，計算每塊小區(qū)域內(nèi)障礙物的疏密度：

式中：a、b為占據(jù)概率與非連通障礙物占比的權(quán)重系數(shù)，NG為搜索框中占據(jù)柵格的數(shù)量，RG為柵格分辨率，NNO為搜索框中非連通障礙物的數(shù)量，NT為搜索框中連通與非連通障礙物的總數(shù).連通障礙物的判定條件如下.條件1：縮放后的柵格為占據(jù)柵格.條件2：縮放后占據(jù)柵格的八鄰域柵格內(nèi)存在占據(jù)柵格.定義同時滿足2 個判定條件的障礙物為連通障礙物（圖3 中2、3、5 號障礙物），其他的均為非連通障礙物（圖3 中1、4 號障礙物）.得到非連通障礙物坐標(biāo)后，無論形狀大小，都視為以該坐標(biāo)為圓心、半徑固定的虛擬圓，斥力方向跟隨機器人位置的變化而變化；連通障礙物是占據(jù)區(qū)域且形狀不固定，因此固定其斥力方向.

如圖4 所示，機器人在不同位置僅受到來自一個方向的斥力作用，具體判定規(guī)則如下.

圖4 連通障礙物斥力方向判定Fig.4 Direction determination of repulsive force for connected obstacles

規(guī)則1（條件3 與條件4 與條件5）：機器人所受單個連通障礙物的斥力，方向垂直于優(yōu)先級最高的占據(jù)柵格.條件3：機器人周圍僅一個連通障礙物.條件4：機器人位于該連通障礙物的斥力影響范圍內(nèi).條件5：機器人左側(cè)占據(jù)柵格優(yōu)先級＞右側(cè)占據(jù)柵格優(yōu)先級＞上側(cè)占據(jù)柵格優(yōu)先級＞下側(cè)占據(jù)柵格優(yōu)先級.

規(guī)則2（條件6 與條件7 與條件8）：機器人所受多個連通障礙物的斥力，方向來自距離最近的連通障礙物，斥力方向選擇遵循規(guī)則1.條件6：機器人周圍存在多個連通障礙物.條件7：機器人位于多個連通障礙物的斥力影響范圍內(nèi).條件8：機器人與多個連通障礙物的距離差較大且存在最小值.

規(guī)則3（條件6 與條件7 與條件9 與條件10）：機器人所受多個連通物的斥力，方向來自優(yōu)先級最高的連通障礙物，斥力方向選擇遵循規(guī)則1.條件9：機器人與多個連通障礙物的距離差較小.條件10：左側(cè)連通障礙物優(yōu)先級＞右側(cè)連通障礙物優(yōu)先級＞上側(cè)連通障礙物優(yōu)先級＞下側(cè)連通障礙物優(yōu)先級.

1.2.3 靜態(tài)切向避障機制 對于連通障礙物，通過勢場法結(jié)合斥力方向判定規(guī)則，機器人在不同位置僅受到來自一個方向的斥力，減少了因在C 形、U 形連通障礙物區(qū)域受到來自不同方向斥力的疊加而陷入局部極小值陷阱的問題.

針對非連通障礙物，提出切向避障的機制，避免機器人面對非連通障礙物時陷入局部極小值陷阱.機器人只需在障礙物影響移動路徑時進行避障，為此進行非連通障礙物的重要性評估，將非連通障礙物分成重要障礙物與普通障礙物.當(dāng)機器人與障礙物的距離小于2 倍 ρo且機器人與障礙物的夾角小于60°時，該障礙物為重要障礙物；當(dāng)機器人與障礙物的距離大于等于2 倍 ρo或機器人與障礙物的夾角大于等于60°時，該障礙物為普通障礙物.如圖5 所示，非連通障礙物1 為重要障礙物，非連通障礙物2 為普通障礙物，在同一時刻僅有一個重要障礙物，機器人圍繞非連通障礙物1完成避障后才進行下一輪重要性評估.

圖5 非連通障礙物重要性評估Fig.5 Importance evaluation of non-connected obstacles

針對重要障礙物，將其斥力影響范圍劃分為人工勢場區(qū)域（寬度r3）、切向避障區(qū)域（寬度r2）以及安全距離區(qū)域（寬度r1），如圖6 所示.陷阱預(yù)測后，若存在一組局部極小值點 (xm,ym)，3 個區(qū)域的劃分寬度為

圖6 非連通障礙物斥力影響范圍劃分Fig.6 Influence range division of repulsive force for non-connected obstacles

式中：robs為非連通障礙物半徑；為局部極小值點與周圍障礙物最小距離的平均值，為局部極小值點與周圍障礙物最大距離的平均值，

若陷阱預(yù)測后未檢測到局部極小值點，3 個區(qū)域的劃分寬度為

式中：rinf為非連通障礙物虛擬圓向外膨脹的寬度.通過區(qū)域劃分，將局部極小值陷阱所在位置限制在切向避障區(qū)域內(nèi)，該區(qū)域不受原勢場的控制，從而避免陷入局部極小值陷阱.機器人進入安全距離區(qū)域后采取緊急制動，避免與障礙物發(fā)生碰撞；在人工勢場區(qū)域保持原人工勢場控制；在切向避障區(qū)域不斷將斥力分力Frep1的方向調(diào)整為避障切線方向，如圖7 所示.

圖7 非連通障礙物靜態(tài)切向避障Fig.7 Static tangential obstacle avoidance for non-connected obstacles

將斥力分力調(diào)整為避障切線方向原因如下：假設(shè)機器人為1 個質(zhì)點，那么完成避障的極限角度為機器人當(dāng)前所在位置與重要障礙物的切線方向，如圖8(c)合力1；當(dāng)斥力和向逆時針方向旋轉(zhuǎn)，合力與障礙物存在交集或向偏離引力和方向移動，導(dǎo)致與障礙物碰撞或繞行；斥力和方向順時針旋轉(zhuǎn)，合力貼近引力和方向，實現(xiàn)對重要障礙物的最優(yōu)避障.當(dāng)旋轉(zhuǎn)角度小于避障切線方向、大于極限角度，合力方向如圖8(e)合力3，機器人與重要障礙物的距離較小；旋轉(zhuǎn)角度大于避障切線方向時，合力方向如圖8(f)合力4，與普通障礙物存在碰撞風(fēng)險.結(jié)合上述分析并開展多組仿真實驗，斥力分力調(diào)整為避障切線方向，如圖8(d)合力2，避障效果較優(yōu)，靜態(tài)切向避障的過程如圖9 所示.

圖8 斥力調(diào)整方向后的合力方向分析Fig.8 Analysis of resultant force direction after repulsive force adjustment

圖9 靜態(tài)切向避障流程圖Fig.9 Flowchart of static tangential obstacle avoidance

1.2.4 動態(tài)切向避障機制 傳統(tǒng)人工勢場法的勢場函數(shù)僅考慮機器人與障礙物的相對距離，未考慮機器人與動態(tài)障礙物間相對速度.由于不同的動態(tài)障礙物具有不同的速度，有時動態(tài)障礙物的出現(xiàn)與機器人無碰撞風(fēng)險，有時機器人周圍會出現(xiàn)多個動態(tài)障礙物.在障礙物斥力影響范圍內(nèi)，將式（1）中斥力勢場函數(shù)改進為

式中：Urep(p,v) 為適用于動態(tài)環(huán)境的斥力勢場；Urep(v) 為障礙物速度產(chǎn)生的斥力勢場；kwar為碰撞風(fēng)險系數(shù)，當(dāng)機器人與障礙物的相對速度小于等于0 m/s，若機器人與障礙物的距離大于等于2 m且合力與障礙物的夾角大于等于60°，設(shè)置kwar=-1，為最低避障優(yōu)先級，代表機器人無需進行避障.當(dāng)機器人與障礙物的相對速度大于0 m/s，若機器人與障礙物的距離小于2 m 或合力與障礙物的夾角小于60°，設(shè)置kwar=1，為次低避障優(yōu)先級；若機器人與障礙物的距離小于2 m 且合力與障礙物的夾角小于60°，設(shè)置kwar=2，為次高避障優(yōu)先級；若機器人與障礙物的距離小于0.4 m，設(shè)置kwar=3，為最高避障優(yōu)先級.設(shè)置kwar=1、2、3 代表機器人需要避障且必須選擇目前避障優(yōu)先級最高的動態(tài)障礙物進行避障.

靜態(tài)切向避障機制是在障礙物靜止不動的前提下進行切向避障，對于動態(tài)障礙物，該機制須改進，如圖10 所示.圖中，速度為Vobs的動態(tài)障礙物與機器人相向而行且距離較近，存在碰撞風(fēng)險.因此在原合力與動態(tài)障礙物的夾角 γ 的基礎(chǔ)上補償角度 θ，得到新航向角 | |γ|+θ|，機器人沿新航向角進行避障，計算式為

圖10 動態(tài)切向避障補償角的補償原理Fig.10 Compensation principle of compensation angle for dynamic tangential obstacle avoidance

式中：m為新航向角正反偏系數(shù).當(dāng)夾角 γ 為負(fù)角時，新航向角正偏（航向角為正），否則負(fù)偏（航向角為負(fù)），角度正負(fù)關(guān)系以逆時針方向為正.補償角 θ 由模糊控制器依據(jù)機器人與動態(tài)障礙物間的情況給出，

式中：ε 為避障補償角模糊控制器的輸出，{PB，MD，PS，ZE}為補償角的模糊語言變量.

2 基于雙機制切向避障的混合路徑規(guī)劃系統(tǒng)設(shè)計

2.1 系統(tǒng)設(shè)計

如圖11 所示為本研究方法的系統(tǒng)框架.全局路徑規(guī)劃單元與經(jīng)過地圖預(yù)處理后的局部路徑規(guī)劃單元通過狀態(tài)決策決定控制狀態(tài).若規(guī)劃的全局路徑與膨脹后的障礙物仍存在一定距離或未檢測到初始靜態(tài)地圖中不存在的障礙物，由全局路徑規(guī)劃單元控制；反之，由局部路徑規(guī)劃單元控制.狀態(tài)決策的轉(zhuǎn)移過程如圖12 所示.

圖11 混合路徑規(guī)劃模塊系統(tǒng)框圖Fig.11 Block diagram of mixed path planning module system

圖12 混合路徑規(guī)劃狀態(tài)決策過程Fig.12 Decision process of mixed path planning state

基于上述狀態(tài)決策思想統(tǒng)籌全局路徑規(guī)劃與雙機制切向避障，實現(xiàn)混合路徑規(guī)劃，流程如圖13所示.1）初始化勢場法參數(shù)：輸入起點、終點坐標(biāo)、小車線速度、角速度等，轉(zhuǎn)至步驟2）.2）全局路徑規(guī)劃：進行一次全局路徑與局部路徑規(guī)劃，在該狀態(tài)下全局路徑優(yōu)先級高于局部路徑，機器人沿全局路徑移動，移動一段距離后進行優(yōu)先級判斷，轉(zhuǎn)至步驟3）.3）路徑優(yōu)先級判斷：計算機器人與全局路徑前進方向上障礙物的距離，檢測是否存在距離全局路徑較近的障礙物或臨時出現(xiàn)的靜態(tài)、動態(tài)障礙物，影響機器人移動安全性.若存在這樣的障礙物，進入局部路徑規(guī)劃，轉(zhuǎn)至步驟4）；若不存在，則輸出全局路徑軌跡，轉(zhuǎn)至步驟5）.4）局部路徑規(guī)劃：根據(jù)機器人與障礙物的相對速度決定進行靜態(tài)或動態(tài)避障，動態(tài)障礙物在勢場下進行動態(tài)切向避障，非連通靜態(tài)障礙物在勢場下進行靜態(tài)切向避障，連通靜態(tài)障礙物在勢場下固定斥力方向，優(yōu)先級調(diào)整為局部路徑高于全局路徑，并以當(dāng)前位置為起點重新進行一次全局規(guī)劃；選取全局路徑前進方向上一點作為臨時目標(biāo)點，機器人沿局部避障路徑向該點移動；輸出局部避障軌跡，轉(zhuǎn)至步驟5）.5）判斷是否到達(dá)臨時目標(biāo)點：若已到達(dá)，轉(zhuǎn)至步驟6）；否則轉(zhuǎn)至步驟4）.6）判斷機器人是否完成避障：若完成避障，轉(zhuǎn)至步驟7），否則保持局部路徑規(guī)劃狀態(tài)直至到達(dá)臨時目標(biāo)點；如果選取的臨時目標(biāo)點附近存在突發(fā)障礙物時，則重新進行一次全局路徑規(guī)劃并選取一點作為新臨時目標(biāo)點.若避障失敗，機器人陷入停滯，則復(fù)位至步驟1）開始一輪全新的規(guī)劃.7）結(jié)束：完成本輪路徑規(guī)劃.

圖13 混合路徑規(guī)劃算法流程圖Fig.13 Flowchart of mixed path planning algorithms

2.2 局部規(guī)劃模糊控制器設(shè)計

本研究設(shè)計的雙機制切向避障通過判斷機器人與障礙物的距離、夾角后確定是否避障，這與模糊控制不需要過程的精確模型的特點較為貼合，因此將模糊控制與雙機制切向避障相結(jié)合來實現(xiàn)局部規(guī)劃模糊控制器設(shè)計.Mamdani 法是機器人控制中常用的模糊方法，中心去模糊法是重心法與最大隸屬度法去模糊的折中，因此本研究采用這2 種方法進行模糊化與解模糊.

將切向避障模糊控制器設(shè)計成三輸入一輸出，模糊變量語義對照如表1 所示.將機器人與障礙物的距離d、合力與障礙物的角度ω、引力與調(diào)整后斥力分力Frep1的夾角 δ作為輸入，共制定30 條模糊規(guī)則.機器人根據(jù)輸出 output 判斷在勢場中受人工勢場控制、切向避障、緊急制動，如當(dāng)距離d為近（DN）、角度ω 為負(fù)（NS）、夾角 δ 為大（QB）時，機器人進行向右切向避障（RQM）.避障補償角模糊控制器以機器人與動態(tài)障礙物的距離d、夾角 |γ| 以及相對速度Vrel為輸入，以動態(tài)補償角θ的大小 ε 作為輸出，共制定125 條模糊規(guī)則，模糊變量語義對照如表2 所示.

表1 切向避障語義對照表Tab.1 Semantic comparison table of tangential obstacle avoidance

表2 避障角補償語義對照表Tab.2 Semantic comparison table of obstacle avoidance angle compensation

3 仿真驗證與實物平臺測試

3.1 仿真環(huán)境下算法有效性驗證

為了驗證所提算法的性能，開展機器人使用不同算法在易陷入局部極小值陷阱、狹窄可行區(qū)域避障的對比實驗，參與對比的算法包括本研究算法、沿邊走法、虛擬目標(biāo)點法和虛擬障礙法.如圖14 所示，搭建60 m×60 m 的二維實驗環(huán)境，起點與目標(biāo)點分別在（0,0）和（50,50），引力增益系數(shù)與斥力增益系數(shù)設(shè)置為20、30，機器人的移動步長為0.7 m，非連通障礙物的斥力影響范圍設(shè)置為障礙物的半徑加5 m.在如圖14(a)的實驗環(huán)境中，4 種算法都到達(dá)目標(biāo)點，但本研究方法路徑更短、更平滑.如圖14(b)的實驗環(huán)境為易陷入局部極小值陷阱與產(chǎn)生路徑震蕩現(xiàn)象的狹長U 形區(qū)域.虛擬目標(biāo)點法[14]規(guī)劃失敗，在U 形陷阱內(nèi)陷入循環(huán)狀態(tài)；沿邊走法[15]進入U 陷阱后，沿著連通障礙物的邊界移動，逃脫了局部較小值陷阱，路徑明顯較長；虛擬障礙物法[16]未陷入局部極小值陷阱，但在連通障礙物間狹窄通道存在路徑震蕩現(xiàn)象；本研究方法未產(chǎn)生其他參與對比算法的缺陷.在不同復(fù)雜度的實驗環(huán)境中統(tǒng)計4 種算法的規(guī)劃結(jié)果，如表3 所示，表中，C為規(guī)劃成功次數(shù)，T為完成規(guī)劃運行時間，L為規(guī)劃路徑的總長，S為規(guī)劃路徑中的拐點數(shù).可以看出，在不同環(huán)境復(fù)雜度下，本研究方法完成規(guī)劃的穩(wěn)定性較高，平均運行時間相較于沿邊走法、虛擬目標(biāo)點法、虛擬障礙物法分別提高21%、20%、19%；在復(fù)雜環(huán)境下的提升效果尤佳，平均運行時間分別提高33%、23%、20%.

表3 不同環(huán)境復(fù)雜度時不同算法的規(guī)劃結(jié)果統(tǒng)計Tab.3 Statistics of different algorithm planning results for different environmental complexity

圖14 不同仿真環(huán)境下改進人工勢場法性能對比Fig.14 Performance comparison of improved artificial potential field method under different simulation environments

在Gazebo 仿真平臺搭建如圖15(a)所示仿真環(huán)境，其中t=2.0、η=10.0、k=6.0、ρo=1.8，算法與機器人的其他相關(guān)參數(shù)如表4 所示.利用Rviz 觀測機器人運動狀態(tài).機器人接收到目標(biāo)點后，進入全局規(guī)劃狀態(tài)，淺色為全局路徑，深色為局部路徑.當(dāng)全局路徑與障礙物距離較小時，考慮到機器人轉(zhuǎn)向半徑，路徑安全性較低，須增大與障礙物的距離，進入局部避障狀態(tài)，此時局部路徑優(yōu)先級高于全局路徑，機器人沿局部路徑進行避障，如圖15（b）所示.

表4 仿真環(huán)境下混合路徑規(guī)劃模塊實驗參數(shù)Tab.4 Experimental parameters of mixed path planning module in simulation environment

圖15 Gazebo 仿真環(huán)境的避障驗證Fig.15 Obstacle avoidance verification of Gazebo simulation environment

3.2 實物平臺測試

為了進一步驗證所提算法在真實環(huán)境中的性能，搭建全向移動平臺.機器人的激光雷達(dá)數(shù)據(jù)由鐳神N10P 提供；機器人操作系統(tǒng)主要平臺為樹莓派4B 系統(tǒng)核心板；采用底盤控制器主控（型號為STM32F407ZET6）接收并解析上位機發(fā)布的速度指令驅(qū)動電機轉(zhuǎn)動.

3.2.1 室內(nèi)環(huán)境下靜態(tài)障礙物避障測試 在不同的實驗環(huán)境中對全向移動平臺開展避障測試.圖16（a）、（b）中，機器人在地圖1 中由于固定斥力方向，受到優(yōu)先級最高的上側(cè)連通障礙物1 與左側(cè)連通障礙物2 的斥力作用，固定斥力方向垂直向下與垂直向右，不再疊加其他連通障礙物斥力，深色避障路徑相較于淺色全局路徑，明顯與障礙物保持充裕的安全距離，避免發(fā)生碰撞.圖16（c）、（d）中，在地圖2 中以易產(chǎn)生受力平衡的C 形、U 形區(qū)域為起點進行規(guī)劃，機器人未陷入停滯狀態(tài)并成功到達(dá)目標(biāo)點.圖16（e）、（f）中機器人對存在影響的非連通障礙物2 與4 進行靜態(tài)切向避障，對不存在影響的非連通障礙物3 不進行避障，并且未在多個非連通障礙物間陷入停滯.測試結(jié)果表明，本研究的靜態(tài)避障機制在不同環(huán)境中實現(xiàn)了自主避障，在幾種典型的易陷入局部極小值陷阱的連通或非連通障礙物環(huán)境中，也有效避免了機器人陷入停滯狀態(tài).

圖16 室內(nèi)環(huán)境下靜態(tài)避障結(jié)果Fig.16 Static obstacle avoidance results in indoor environment

將局部規(guī)劃器采用DWA、TEB 在圖16 的3 幅地圖中進行25 次多點導(dǎo)航測試，并逐步向地圖中添加障礙物改變環(huán)境復(fù)雜度，結(jié)果如圖17（a）所示，TN為平均導(dǎo)航耗時，c為路徑規(guī)劃成功次數(shù).在起點與目標(biāo)點相同的情況下，本研究方法成功完成路徑規(guī)劃73 次，比DWA 多22 次，比TEB 多2 次；平均導(dǎo)航耗時比DWA 提升了39%、比TEB 提升了22%.圖17（b）中，TOA為平均完成一次避障的用時.將3 種算法完成對連通、非連通障礙物以及易陷入局部極小值陷阱的U 形障礙物區(qū)域的平均避障時間進行對比，本研究方法所耗費的時間最小，比DWA 提升了55%，比TEB 提升了40%.

圖17 全向移動平臺測試結(jié)果比對Fig.17 Test results comparison of omnidirectional mobile platforms

3.2.2 室內(nèi)環(huán)境下動態(tài)障礙物避障測試 如圖18所示，進行本研究方法躲避行人的有效性驗證實驗.實驗結(jié)果表明，機器人進行避障角補償后能夠躲避行人.可以看出，1 號人員與機器人的相對距離較近、夾角較小，風(fēng)險系數(shù)為次高避障優(yōu)先級，2 號人員與機器人的夾角較大，風(fēng)險系數(shù)為次低避障優(yōu)先級，對1 號人員進行避障角補償符合規(guī)則.

圖18 室內(nèi)環(huán)境下動態(tài)避障結(jié)果Fig.18 Dynamic obstacle avoidance results in indoor environment

4 結(jié)語

本研究提出基于人工勢場法的雙機制切向避障算法.所提算法將障礙物分成連通與非連通障礙物，并針對阻礙實現(xiàn)自主避障的局部極小值陷阱采取不同解決策略.采用人工勢場法并固定連通障礙物斥力方向，減少了不同方向力的疊加，避免機器人陷入停滯；通過陷阱預(yù)測得到的局部極小值點，對非連通障礙物的斥力影響范圍進行劃分，將局部極小值陷阱限制在切向避障區(qū)域內(nèi)，機器人進入該區(qū)域后進行切向避障避免陷入停滯.在此基礎(chǔ)上，針對室內(nèi)環(huán)境中可能出現(xiàn)的動態(tài)障礙物，通過避障補償角實現(xiàn)動態(tài)切向避障.實驗結(jié)果表明，在不同的地圖中，相比主流算法，所提算法能夠在不同環(huán)境復(fù)雜度下有效避免陷入局部極小值陷阱，在避障耗時與整體導(dǎo)航耗時上有較大提升，在復(fù)雜環(huán)境中提升效果尤佳.同時，實物測試平臺的避障自主性較優(yōu)，減少了人為干預(yù).本研究須在規(guī)劃前對已完成建模的地圖環(huán)境進行陷阱預(yù)測，而在實際應(yīng)用中地圖往往是未知的，未來計劃在本研究的基礎(chǔ)上進行未知環(huán)境下的實時陷阱預(yù)測研究.