基于改進人工勢場的避障路徑規劃策略研究

郭凱紅，李博昊，宗曉瑞，侯冬冬

(1.海軍裝備部裝備審價中心，北京 100071；2.中國船舶集團有限公司第七一三研究所，河南 鄭州 450000；3.河南省水下智能裝備重點實驗室，河南 鄭州 450000)

0 引 言

水下無人航行器需要自主完成多種人類無法完成的任務，在復雜環境中的任務規劃更具挑戰性。目前智能的自主任務規劃受到了廣泛的關注，而路徑規劃是實現任務規劃的必要前提以及重要組成部分。

路徑規劃的任務就是在已知的約束條件下找出一條從當前位置到目標位置的最優或次優的可行路徑。路徑規劃方法又可分為經典算法和智能算法。智能算法中有遺傳算法、粒子群優化算法、神經網絡算法等。大多數論文認為經典算法中有3 類主要方法：細胞分解法、人工勢場法以及路線圖方法[1]。

人工勢場法因其表達簡潔，原理簡單，對硬件要求低，規劃速度快等優點而廣泛應用于機器人導航與避障中。Khatib 首次將人工勢場法引入實時路徑規劃中[2]。人工勢場法是構造一個勢力場，將在勢力場中的航行器看作電子，目標位置對其產生吸引力以保證朝著目標位置前進，障礙物對其產生斥力以保證避開障礙物。然而，人工勢場法在路徑規劃的應用中如果障礙物距離目標點過近、U 型障礙物等復雜問題時則會出現無法到達目標不可達等問題。為了克服這些問題，研究人員嘗試去改進傳統的人工勢場法。S.S.Ge 等[3]提出當目標附近存在障礙物而使目標不可達時，利用航行器與目標之間的距離來構造新的斥力函數來確保目標位置是全局最小。朱華健等[4]提出通過直接尋找一系列勢能極小點來構成航行器的行進路徑可解決航行器目標不可達以及震蕩問題。林欣等[5]提出利用決策樹改進人工勢場法，根據斥力增益系數、合力偏轉角度和速度對不同障礙分布做出合理的決策。本文提出一種基于速度矢量判斷旋轉方向的改進人工勢場法，在水下無人航行器的局部路徑規劃中實現安全快速的避障。

1 人工勢場法原理

人工勢場中水下無人航行器受到由障礙物產生的斥力場以及目標物產生的引力場的影響，水下航行器所受到的引力以及斥力的合力方向即為航行器的移動方向，這一運動方向使得航行器避過障礙物并朝著目標位置移動。

目標位置所形成的引力勢場可表示為：

式中：katt為引力增益系數；X為航行器的位置矢量；Xd為目標的位置矢量。由力場產生的引力為引力勢能的負梯度：

當航行器到達目標位置時引力為0。

1.1 傳統人工勢場法缺陷

1）容易陷入局部極小值。當水下無人航行器所受引力與斥力大小相等、方向相反時，航行器所受合力為0，此時航行器陷入了局部極小值點。在局部極小值點周圍的各個位置的合力都指向局部極小值，導致航行器在局部極小值點周圍振蕩，無法自行走出該區域。

2）傳統人工勢場法引力部分與水下無人航行器和目標點位置距離成正比，當水下無人航行器距離目標點較遠時，引力部分過大，可能導致水下無人航行器與障礙物相撞。

3）當航行器離目標點比較遠時，引力將變的特別大，相對較小的斥力在甚至可以忽略的情況下，航行路徑上可能會碰到障礙物。

4）水下無人航行器在三維空間中，通常將航行器和障礙物看作在同一水平面內。采用水平面二維人工勢場法避障，且使用二維平面固定左旋轉方向或者右旋轉方向進行避障，如圖1 和圖2 所示。對于障礙物長寬比懸殊、復雜海底巡航，扁平型UUV 編隊協同等特殊情況，且需要航行器快速繞過障礙物時二維平面固定速度旋轉方向人工勢場法不能很好滿足實際工程的全部需求。

圖1 固定速度左旋轉避障Fig.1 Left-turn obstacle avoidance at fixed speed

圖2 固定速度右旋轉避障Fig.2 Right-turn obstacle avoidance at fixed speed

1.2 改進人工勢場法

針對傳統人工勢場法的缺陷，Khatib 提出的FIRAS 函數是障礙物形成的斥力勢場常用的表達方法：

式中：krep為斥力增益系數；ρ0為障礙物斥力場的影響范圍，只有當障礙物與航行器間的距離ρ(X,Xobs)小于ρ0時航行器才會受到該障礙物的影響。有斥力場產生的斥力為斥力勢能的負梯度：

合力F的方向即為航行器的運動方向。

采用式（4）的傳統斥力勢場，當目標點在礙航物附近時，UUV 無法到達目標點。因此引入新的斥力函數在原有的斥力場的基礎上，加入了目標和物體距離的影響。強化目標點附近引力場，弱化目標點附近斥力場，改進勢場如下：

當水下無人航行器越靠近目標點時，斥力勢能越小，引力勢能越大。這可解決當目標點周圍出現障礙物的局部極小問題。

針對水下無人航行器固定旋轉方向的缺陷問題提出基于速度矢量判定的旋轉方法。

叉積是2 個三維向量之間的叉積生成一個與這2 個向量都垂直的新向量。對于A，B兩個向量：

在三維環境中，采用速度矢量a與障礙物與當前位置矢量b的叉積得到垂直于二者的向量c，通過向量c與速度矢量a的又一次叉積得到旋轉矢量d，通過旋轉矢量d判斷UUV 在障礙物的一側（左側、右側、上側、下側），進而選擇近的一側作為旋轉方向，使速度方向旋轉至合力方向離開障礙物。

2 仿真結果及分析

本文利用 Matlab 軟件針對傳統人工勢場法的缺陷和水下航行器二維平面固定旋轉方向的問題采用改進人工勢場法進行仿真，以下仿真結果均是在靜態障礙物的前提下進行的。水下無人航行器采用尾十字舵和單主推驅動，仿真中記錄機器人重心的軌跡和距離障礙物的距離。仿真結果表明，改進后的人工勢場法具有更強的環境適應性，避障更加快速安全，達到了預期的避障效果。

由圖3 和圖4 可以看出在距離目標點很遠的情況下，航跡上有多個障礙物，由于存在障礙物勢力場范圍，UUV 仍然能夠進行安全避障。

圖3 障礙物無斥力場范圍時航行器軌跡曲線與航行器距障礙物距離Fig.3 When the obstacle has no repulsion field range,the trajectory curve of the aircraft and the distance between the aircraft and the obstacle

圖5 和圖6 顯示目標距離障礙物很近時，由于強化了目標點附近的水下無人航行器并沒有停止在障礙物周圍，同時UUV 能夠安全穿過障礙物到達目標點。

圖5 目標點附近存在障礙物航行器軌跡曲線Fig.5 The trajectory curve of the aircraft when there is an obstacle near the target point

圖6 目標點附近存在障礙物時航行器距障礙物距離Fig.6 The distance between the aircraft and the obstacle when there is an obstacle near the target point

由圖7 和表1 可以看出，三維環境中，在不同障礙物和特殊目標點的情況下，給定相同的引力和斥力系數和障礙物斥力場范圍，水下航行器會根據實時速度判斷障礙物在航行器的左側，右上側或者右側，進而選擇最近的一側作為旋轉方向，并不停調整旋轉方向進行避障。因而航行器有更短的航行距離，能更快到達目標點。

表1 改進人工勢場法對照試驗Tab.1 Improved artificial potential field method control test

圖7 基于速度矢量判斷航行器三維空間航行軌跡Fig.7 The three-dimensional space navigation trajectory of aircraft based on velocity vector judgment

3 結 語

本文針對 UUV 在未知環境自主航行中的局部路徑規劃問題，提出一種改進人工勢場算法，根據障礙物的形狀，目標點的遠近，選擇合理的障礙物斥力范圍和引力斥力系數，再根據速度矢量判定速度旋轉方向，這種改進的人工勢場法解決了傳統人工勢場法在三維平面內固定旋轉方向的避障缺陷，能很好地應對復雜障礙物和特殊目標點，保持和障礙物的距離，快速且安全到達目標點。但這種方法同樣具有局限性，基于速度矢量判斷方法會使水下無人航行器控制變得更加復雜，增加航行的控制難度。因此未來在航行器中因增加避障策略器，在不同障礙物和目標點選擇不同的避障策略。