基于改進(jìn)人工勢場的避障路徑規(guī)劃策略研究

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

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

0 引 言

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

路徑規(guī)劃的任務(wù)就是在已知的約束條件下找出一條從當(dāng)前位置到目標(biāo)位置的最優(yōu)或次優(yōu)的可行路徑。路徑規(guī)劃方法又可分為經(jīng)典算法和智能算法。智能算法中有遺傳算法、粒子群優(yōu)化算法、神經(jīng)網(wǎng)絡(luò)算法等。大多數(shù)論文認(rèn)為經(jīng)典算法中有3 類主要方法：細(xì)胞分解法、人工勢場法以及路線圖方法[1]。

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

1 人工勢場法原理

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

目標(biāo)位置所形成的引力勢場可表示為：

式中：katt為引力增益系數(shù)；X為航行器的位置矢量；Xd為目標(biāo)的位置矢量。由力場產(chǎn)生的引力為引力勢能的負(fù)梯度：

當(dāng)航行器到達(dá)目標(biāo)位置時引力為0。

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

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

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

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

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

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

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

1.2 改進(jìn)人工勢場法

針對傳統(tǒng)人工勢場法的缺陷，Khatib 提出的FIRAS 函數(shù)是障礙物形成的斥力勢場常用的表達(dá)方法：

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

合力F的方向即為航行器的運(yùn)動方向。

采用式（4）的傳統(tǒng)斥力勢場，當(dāng)目標(biāo)點在礙航物附近時，UUV 無法到達(dá)目標(biāo)點。因此引入新的斥力函數(shù)在原有的斥力場的基礎(chǔ)上，加入了目標(biāo)和物體距離的影響。強(qiáng)化目標(biāo)點附近引力場，弱化目標(biāo)點附近斥力場，改進(jìn)勢場如下：

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

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

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

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

2 仿真結(jié)果及分析

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

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

圖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 顯示目標(biāo)距離障礙物很近時，由于強(qiáng)化了目標(biāo)點附近的水下無人航行器并沒有停止在障礙物周圍，同時UUV 能夠安全穿過障礙物到達(dá)目標(biāo)點。

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

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

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

表1 改進(jìn)人工勢場法對照試驗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 結(jié) 語

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