未知環境下機器人避障及動態目標追蹤*

崔寶俠， 宋佳瑞

(沈陽工業大學 信息科學與工程學院， 沈陽 110870)

機器人避障及動態目標追蹤是智能機器人技術的重要分支和研究熱點[1].避障算法共分為已知環境下的全局避障算法和未知環境下的局部避障算法兩類[2]，已知環境下的機器人避障通過對已知的障礙物分布信息進行地圖建模就可以實現，目前較為成熟的已知環境下的全局避障算法主要有以下幾種：可視圖法、柵格法、自由空間法和蟻群算法等[3-6]，這些算法在要求必須求出全局最優解的問題中是可以考慮的，獲取的環境信息越詳細、準確，規劃出的路徑越貼近最優，通常這些算法的實時性非常不好，并且計算過程繁瑣、步驟多，對于障礙物位置會發生變化的環境適應性差，導致速率和效率很低.而未知環境下機器人避障的具體過程就是首先由探測器探測障礙物信息，之后將探測到的信息交給處理器處理，根據探測到的不同信息選擇不同的避障策略.

目前應用于未知環境下且被研究最為廣泛的局部避障算法有以下幾種：神經網絡算法、模糊邏輯算法、人工勢場法和遺傳算法等[7-10]，雖然這些算法避障的實時性相對于全局避障算法要好很多，對未知環境的適應性較強，但是依然存在著不同方面的局限性.人工勢場法由于實時性好、反應速度快、運算量小、建模容易、對未知環境適應性強等優點而被廣泛使用.但同時人工勢場法也存在局部最小點和無法到達目標點等缺陷.

1 基于人工勢場法的避障方法分析

人工勢場法最初是由Khatib于1985年提出的，其核心思想是將機器人的運動假想成在抽象的合力勢場中的運動.在機器人工作空間中假想出無數條引力和斥力的場強線，使得機器人在類似于充滿“磁場”的空間中運動，目標點對機器人產生的作用力就像“磁鐵”一樣吸引著機器人，把它想象成引力.而障礙物與機器人的作用力就好像磁鐵同極之間的作用力一樣，把它想象成斥力.機器人就是在這種引力和斥力的合力驅動下進行運動，每行走到一個位置就去計算當前位置機器人所受的合力大小以及合力的方向，邊行走邊去探測周圍的環境信息，屬于反應式機器人避障算法.該方法的提出讓機器人的避障規劃問題與代數問題和幾何學問題相結合，將抽象的問題具體化，便于專家研究和學者分析，為機器人避障規劃研究工作做出了巨大貢獻.圖1為人工勢場法的受力分析示意圖.

圖1 人工勢場法受力分析示意圖Fig.1 Schematic diagram of force analysis inartificial potential field method

傳統人工勢場法的引力勢場函數和斥力勢場函數分別為

(1)

(2)

對式(1)、(2)分別求負梯度可得引力計算公式和斥力計算公式，即

(3)

(4)

相比其他算法，人工勢場法的優點主要表現在實時性強、便于分析和建模，可以在未知環境下應用，無需先驗知識[11].而正是由于其反應式的避障特性導致其忽略了大部分的全局信息，人工勢場法的局限性主要表現在目標不可達問題和局部極小值問題.

1) 目標不可達問題.目標不可達問題是指機器人在行進至距離目標點較近的位置時，目標點附近存在一個或多個障礙物，并且目標點和機器人同時處在障礙物的影響范圍之內時所產生的機器人到達目標點失敗的現象.隨著機器人與目標點之間的距離縮短，機器人所受的引力越來越小，機器人所受的斥力大小與障礙物的距離成反比.目標點附近的障礙物會導致在機器人所受引力減小的過程中，對機器人產生的斥力越來越大，機器人很可能會被“推”回來，向遠離目標點的方向運動.而在反方向運動的同時，機器人在這個瞬間再一次遠離了障礙物和目標點，機器人所受的引力又在增加，斥力又在減小，因此，就出現了機器人由于所受合力為零且速度不完全為零，在目標點附近不停地徘徊震蕩，卻永遠不會剛好停止在目標點上的現象.圖2為機器人在陷入目標不可達問題時的受力分析圖.

圖2 人工勢場法目標不可達示意圖Fig.2 Schematic diagram of unreachable targetin artificial potential field method

2) 局部極小值問題.所謂的傳統人工勢場法的局部極小值問題是指機器人在行走的過程中，在距離目標點較遠的情況下，機器人所受的單個障礙物的斥力或者多個障礙物的斥力合力與目標點對機器人的引力大小相等，方向相反，機器人在未到達目標點之前所受合力為零，機器人會誤以為當前的路徑結點為全局勢場最小點，即目標點.當機器人陷入局部極小值時，存在以下兩種情況.

第一種情況：機器人和障礙物都距離目標點較遠時，機器人、障礙物和目標點在同一條直線上且障礙物位于機器人和目標點中間，此時機器人所受的目標點的引力還是非常大的，而由于機器人距離障礙物很近，機器人所受的斥力也非常大，因此產生了引力和斥力大小相等，方向相反的情況，機器人陷入局部極小值點，示意圖如圖3所示.

第二種情況：機器人所在位置的附近存在兩個或者兩個以上的障礙物，障礙物之間具有一定距離，機器人與目標點的連線上不存在影響范圍包含機器人的障礙物，目標點對機器人的引力會和機器人所在位置附近的每個影響范圍都包含機器人的障礙物的斥力合力大小相等，方向相反，同樣會形成局部極小值問題，導致機器人在速度不完全為零的情況下，由于慣性在障礙物通道中徘徊震蕩不前進.在機器人的避障環境中，相對于第一種局部極小值情況，第二種更加常見，幾乎存在于大部分地圖中，機器人在第二種局部極小值情況下的受力示意圖如圖4所示.

圖3 人工勢場法局部極小值示意圖1Fig.3 Schematic diagram 1 of local minimum valuein artificial potential field method

圖4 人工勢場法局部極小值示意圖2Fig.4 Schematic diagram 2 of local minimum valuein artificial potential field method

2 算法改進與應用

2.1 重新定義斥力方向

文獻[12]對傳統人工勢場法進行了改進，在斥力場函數中加入了機器人與目標點距離這一項，使得機器人在靠近目標點過程中，目標點附近的障礙物對機器人產生的斥力場逐漸減小，保證了目標點為機器人所在勢場的全局最小點.改進公式為

(5)

Frep= -Urep(W)=

(6)

改進后的斥力由兩個分力組成，分別為

(7)

(8)

式中：Frep1為機器人所受斥力的第一個分量，其方向由障礙物指向機器人，作為機器人遠離障礙物的動力；Frep2為斥力的第二個分量，其方向由機器人指向目標點，和引力的方向相同；n為機器人與目標點距離的指數因子.

雖然改進斥力場函數的人工勢場法解決了目標不可達問題，但僅僅是調整了目標點附近障礙物在機器人靠近目標點過程中對機器人產生斥力的大小，對解決機器人在距離目標點較遠的障礙物周邊陷入局部極小值點的問題是無效的.

當機器人在陷入目標不可達問題時，機器人距離目標點較近，由于目標點附近的障礙物對機器人的斥力合力和目標點對機器人的引力大小相等、方向相反，引力方向與各個障礙物的斥力合力方向夾角為180°.在機器人陷入局部極小值點時，距離目標點較遠，機器人附近的單個障礙物對機器人的斥力或者多個障礙物對機器人的斥力合力與目標點對機器人的引力大小相等、方向相反，機器人所受的引力和各個障礙物斥力合力的夾角同樣為180°.實際上引力與斥力分力的夾角是多種情況的，并不一定是固定值，但是通過分析在改進斥力場函數的人工勢場法條件下障礙物的分布情況，可以總結出機器人受力情況規律，機器人所受的每個障礙物的斥力的第一個分力Frep1與引力Fatt的夾角大于90°的情況下可能會產生目標不可達問題或者局部極小值問題.為了使機器人在運行過程中每一時刻所受的引力和每個障礙物給予的斥力的每一個分力的夾角都不大于90°，重新定義了人工勢場法斥力分力的方向.

將工作空間中的障礙物假設為圓形，改進原理為斥力分力的大小采用式(7)、(8)求解，但僅僅是模的大小相等，方向卻不同.將每個障礙物對機器人產生的斥力Frep分解為Frep1和Frep2兩個方向的分力，Frep2的方向與改進斥力場函數的人工勢場法相同，為從機器人指向目標點，與引力的方向一致，不同的是將Frep1的方向重新定義為與機器人和障礙物圓心連線垂直的方向，單純的垂線是沒有方向的，改進算法中Frep1的方向為與引力方向夾角不大于90°的方向.重新定義斥力分力方向的目的就是從根本上消除產生每種類型的局部極小值問題和目標不可達問題的可能性，Frep2的方向與引力方向同向，則夾角為0°，將Frep1又重新定義為與引力方向夾角不大于90°的方向，則機器人在到達目標點前永遠不會出現引力方向與斥力分力方向夾角大于90°的情況.同理引力Fatt與兩個斥力分力的合力Frep的夾角也不大于90°，也就是說機器人在到達目標點前不可能會出現局部極小值和目標不可達兩種問題.圖5為在重新定義了斥力分力的方向后，機器人的受力分析圖.

圖5 斥力改進示意圖1Fig.5 Schematic diagram 1 for improvementof repulsive force

但有以下兩種特殊情況除外：

1) 當機器人、障礙物和目標點位于同一條直線上，并且機器人位于障礙物與目標點之間時，定義Frep1與Frep2同向，均指向目標點，示意圖如圖6所示.

圖6 斥力改進示意圖2Fig.6 Schematic diagram 2 for improvementof repulsive force

2) 當機器人、障礙物和目標點在同一直線上，且目標點位于機器人與障礙物之間時，定義Frep1與Frep2同向，均指向目標點，示意圖如圖7所示.

圖7 斥力改進示意圖3Fig.7 Schematic diagram 3 for improvementof repulsive force

上述兩種狀況斥力分力之一Frep1所定義的方向與引力方向一致，其目的是為了防止當機器人、障礙物和目標點位于同一條直線上時，機器人在避障的過程中以及行走至目標點的過程中產生不必要的轉彎和抖動，增加行走的路徑長度和避障的時間.由圖5～7可知，機器人在重新定義斥力分力方向的人工勢場法條件下所受的引力和各個斥力的分力夾角永遠不會大于90°.

2.2 重新定義人工勢場引力公式

由于在實際應用中，目標點大部分是具有速度和加速度的，例如導彈的跟蹤攔截.在重新定義了斥力分力方向后，為了完成機器人在避障的同時進行動態目標追蹤的工作，在引力勢場函數上加入機器人與目標速度之差和加速度之差的因子，使得加速度之差的變化也會導致引力函數的變化，不再僅僅討論目標加速度為0的勻速運動.為了預防引力的大小瞬時突變，產生與障礙物碰撞，會一直保持對機器人產生適宜的牽引力，使得機器人路徑更加平滑，長度短，機器人行走路線更加合理.重新定義人工勢場引力表達式為

(9)

Fatt= -grad(Uatt)=

-Kaqq-qgoal-Kavv-vgoal-

Kaaa-agoal

(10)

式中：Kaq、Kav、Kaa為三個增益系數；Uatt和Fatt分別為引力勢場函數和引力函數；q和qgoal分別為機器人和目標點的位移大小；v和vgoal分別為機器人和目標點的速度大小；a與agoal分別為機器人和目標點的加速度大小.根據不同的環境地形、障礙物的分布以及密集狀況靈活地選擇引力和斥力增益系數以及障礙物影響范圍.改進算法流程如圖8所示.

3 仿真結果分析

對不同障礙物分布的環境分別進行仿真，與現有的改進人工勢場法進行對比，并將地圖中的障礙物數量不斷增加，使得障礙物越來越密集.通過對不同障礙物分布情況的仿真結果進行對比，從而體現該算法的可行性和優勢.

圖9為多障礙物環境，目標勻速直線運動，并且避障空間較大.利用文獻[12]中的改進人工勢 場法進行避障及動態目標追蹤會使得機器人陷入局部極值點停止不前，并且由于目標在不斷向右運動，機器人不會靈活調整方向，導致停止在該點不追蹤動態目標以及避障.圖9中的曲線為機器人運動軌跡，直線為動態目標點運動軌跡，黑白相間的圓為障礙物，仿真為動態圖，本文中的圖示為到達規定步數停止之后的最后狀態圖.

圖8 算法流程圖Fig.8 Flow chart of algorithm

圖9 改進人工勢場法的機器人動態目標追蹤及其避障

Fig.9Dynamictargettrackingandobstacleavoidanceofrobotwithimprovedartificialpotentialfieldmethod

圖10～12分別為利用本文改進思想的人工 勢場法在障礙物逐漸密集以及數量越來越多的仿真圖示.相對于圖9，圖10的障礙物更加密集，而且目標改變為做勻加速運動，該環境下目標的運動要考慮加速度，所以利用式(9)、(10)計算引力，利用重新定義斥力分力方向的人工勢場法去 計算合力的大小和方向，仿真結果顯示該算法可行.

圖10加入優化思想后的機器人避障及其目標追蹤過程仿真圖示1

Fig.10Simulationdiagram1fordynamictargettrackingandobstacleavoidanceofrobotafteraddingoptimizationidea

圖11加入優化思想后的機器人避障及其目標追蹤過程仿真圖示2

Fig.11Simulationdiagram2fordynamictargettrackingandobstacleavoidanceofrobotafteraddingoptimizationidea

圖11中不僅目標做變加速運動，而且在圖10的基礎上將障礙物位置稍加變動，在機器人追蹤目標的關鍵路徑點上設置障礙物，而且目標加速度一直在變化，增加避障物及其目標追蹤的難度系數，仿真結果顯示該改進算法依然可行.

圖12顯示在機器人的起點處，障礙物就開始盡力封堵機器人的路線，使得機器人無法從左側直接繞過障礙物追蹤動態目標，并且目標做變加速運動，仿真結果顯示該算法依然可行.

通過對比不同環境下的仿真結果，不斷改變環境、障礙物的分布以及密集程度，增加機器人避障及其追蹤的難度系數，驗證了該算法的可行性. 該算法的優勢在于障礙物可以隨機設置，在極小的避障空間內也可以追蹤到動態運動的目標，同時解決了傳統人工勢場法產生的局部極小值問題和目標不可達問題.

圖12加入優化思想后的機器人避障及其目標追蹤過程仿真圖示3

Fig.12Simulationdiagram3fordynamictargettrackingandobstacleavoidanceofrobotafteraddingoptimizationidea

4 結 論

本文將改進人工勢場法應用到機器人動態目標追蹤及避障中，并且將機器人受到的斥力分力方向重新定義，提高避障的靈活性，使得機器人可以靈活調整方向，消除了機器人在某一點不停抖動停滯不前的現象.把二者加速度之差加入公式中，提高了引力控制的精確性，使得機器人受到的引力不會過大而導致避障失敗，并且根據不同障礙物分布情況靈活選擇人工勢場法的計算參數、引力斥力增益系數和障礙物影響范圍.綜合多方面優化改進人工勢場法，在前人優秀成果中加入了優化改進思想，可以用來解決靜態目標環境下的避障問題，只需將目標點的速度和加速度設置為0，引力公式變為傳統人工勢場法引力公式即可.在多障礙物以及障礙物密集的復雜地形下，該方法的避障實時性強、靈活，不用預知地圖情況，障礙物可隨機設置，只要有避障空間和避障的可能性，該算法完全可以實現機器人在未知環境中高效、獨立、自主地避障及其動態目標追蹤.

參考文獻(References)：

[1] 胡小平，謝珂，左富勇.基于改進人工勢場法的機械手避障規劃 [J].測控技術，2012，31(10)：109-111.

(HU Xiao-ping，XIE Ke，ZUO Fu-yong.Obstacle avoidance path planning for manipulator based on improved artificial potential field [J].Measurement & Control Technology，2012，31(10)：109-111.)

[2] 李奕銘.基于人工勢場法的移動機器人避障研究 [D].合肥：合肥工業大學，2013.

(LI Yi-ming.Research on obstacle avoidance of mobile robot based on artificial potential field method [D].Hefei：Hefei University of Technology，2013.)

[3] 陳超，唐堅，靳祖光，等.一種基于可視圖法導盲機器人路徑規劃的研究 [J].機械科學與技術，2014，33(4)：490-495.

(CHEN Chao，TANG Jian，JIN Zu-guang，et al.A path planning algorithm for seeing eye robots based on V-graph [J].Mechanical Science and Technology for Aerospace Engineering，2014，33(4)：490-495.)

[4] 朱磊，樊繼壯，趙杰，等.基于柵格法的礦難搜索機器人全局路徑規劃與局部避障 [J].中南大學學報(自然科學版)，2011，42(11)：3421-3428.

(ZHU Lei，FAN Ji-zhuang，ZHAO Jie，et al.Global path planning and local obstacle avoidance of searching robot in mine disasters based on grid method [J].Journal of Central South University(Science and Technology)，2011，42(11)：3421-3428.)

[5] 李剛.自由空間激光通信網路由最優路徑選擇方法研究 [J].激光雜志，2017，38(2)：132-136.

(LI Gang.Research on routing of free space laser communication networks optimal path selection method [J].Laser Journal，2017，38(2)：132-136.)

[6] 蒲云花，陳世平.改進蟻群算法在移動自組網中的研究 [J].計算機應用研究，2015(2)：574-578.

(PU Yun-hua，CHEN Shi-ping.Research of improved ant algorithm for mobile Ad Hoc networks [J].Application Research of Computers，2015(2)：574-578.)

[7] 楊忠，劉華春.基于BP神經網絡的掃地機器人尋路算法 [J].電腦知識與技術，2017，13(10)：156-158.

(YANG Zhong，LIU Hua-chun.A sweeping robot path pathfinding algorithm based on BP neural network [J].Computer Knowledge and Technology，2017，13(10)：156-158.)

[8] Kim C J，Chwa D.Obstacle avoidance method for wheeled mobile robots using interval type-2 fuzzyneural network [J].IEEE Transactions on Fuzzy Systems，2015，23(3)：677-687.

[9] Yu Z Z，Yan J，Zhao J，et al.Mobile robot path plan-ning based on improved artificial potential field method [J].Journal of Harbin Institute of Technology，2011，43(1)：349-354.

[10]Zhou L F，Hong B R.A knowledge based genetic algorithm for path planning of a mobile robot [J].Acta Electronica Sinica，2006，34(5)：911-914.

[11]崔寶俠，周鈺雨，段勇.機器人在未知環境條件下的動態避障 [J].沈陽工業大學學報，2016，38(6)：657-661.

(CUI Bao-xia，ZHOU Yu-yu，DUAN Yong.Dynamic obstacle avoidance of robot in unknown environment [J].Journal of Shenyang University of Technology，2016，38(6)：657-661.)

[12]王會麗，傅衛平，方宗德，等.基于改進的勢場函數的移動機器人路徑規劃 [J].機床與液壓，2002(6)：67-68.

(WANG Hui-li，FU Wei-ping，FANG Zong-de，et al.Path planning of mobile robot based on improved potential field function [J].Machine Tool and Hydraulic，2002(6)：67-68.)