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

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

0 引 言

水下無(wú)人航行器需要自主完成多種人類無(wú)法完成的任務(wù)，在復(fù)雜環(huán)境中的任務(wù)規(guī)劃更具挑戰(zhàn)性。目前智能的自主任務(wù)規(guī)劃受到了廣泛的關(guān)注，而路徑規(guī)劃是實(shí)現(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ì)胞分解法、人工勢(shì)場(chǎng)法以及路線圖方法[1]。

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

1 人工勢(shì)場(chǎng)法原理

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

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

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

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

1.1 傳統(tǒng)人工勢(shì)場(chǎng)法缺陷

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

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

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

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

圖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)人工勢(shì)場(chǎng)法

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

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

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

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

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

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

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

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

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

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

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

圖3 障礙物無(wú)斥力場(chǎng)范圍時(shí)航行器軌跡曲線與航行器距障礙物距離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)距離障礙物很近時(shí)，由于強(qiáng)化了目標(biāo)點(diǎn)附近的水下無(wú)人航行器并沒(méi)有停止在障礙物周圍，同時(shí)UUV 能夠安全穿過(guò)障礙物到達(dá)目標(biāo)點(diǎn)。

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

圖6 目標(biāo)點(diǎn)附近存在障礙物時(shí)航行器距障礙物距離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)點(diǎn)的情況下，給定相同的引力和斥力系數(shù)和障礙物斥力場(chǎng)范圍，水下航行器會(huì)根據(jù)實(shí)時(shí)速度判斷障礙物在航行器的左側(cè)，右上側(cè)或者右側(cè)，進(jìn)而選擇最近的一側(cè)作為旋轉(zhuǎn)方向，并不停調(diào)整旋轉(zhuǎn)方向進(jìn)行避障。因而航行器有更短的航行距離，能更快到達(dá)目標(biāo)點(diǎn)。

表1 改進(jìn)人工勢(shì)場(chǎng)法對(duì)照試驗(yà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é) 語(yǔ)

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