基于混合勢場法的無人潛航器路徑規(guī)劃及編隊方法研究

王健，張華，徐令令，曹園山，陳偉，顧媛媛

(中國船舶科學(xué)研究中心，江蘇 無錫 214082)

0 引 言

水下無人潛航器（UUV）是一種無人化能夠自主在水下作業(yè)完成特定功能的水下無人裝備[1]。UUV的精確航跡跟蹤能力對于UUV 來說是一項很關(guān)鍵的行為能力，它是完成UUV 作業(yè)任務(wù)（如海底地形調(diào)查，遠程航行等）的一種重要技術(shù)手段。

對于一般UUV 來說，通常采用螺旋槳進行驅(qū)動，以尾舵來控制航向。在進行路徑跟蹤時需要通過尾舵同時進行航向以及航跡控制，這種單尾舵的控制方式屬于欠驅(qū)動的方式，需要設(shè)計合理的潛航器航行控制算法滿足潛航器這種欠驅(qū)動的方式。

1 混合勢場法路徑跟蹤及避障架構(gòu)

對于路徑規(guī)劃方法，通常是針對從起始點航行到目標(biāo)點，自主尋找到一條最優(yōu)的無障礙航線，一般應(yīng)用方法有A*算法、蟻群算法[2]、粒子群算法[3]、勢場法[4]等。而水下潛航器一般在實際作業(yè)時都有一條預(yù)規(guī)劃的航線，潛航器在一般情況下需要按照預(yù)設(shè)航線運動，在遇到障礙等特殊情況需要進行規(guī)避，規(guī)避動作也必須在預(yù)設(shè)航線的趨勢上進行規(guī)避。

本文提出一種混合勢場架構(gòu)，將潛航器避障算法與路徑跟蹤算法進行混合，合并為一種統(tǒng)一的廣義勢場。該種方法將避障、航向保持、路徑跟蹤等多驅(qū)動問題進行統(tǒng)一，輸出為單舵控制的方式，適合欠驅(qū)動潛航器。混合勢場法控制架構(gòu)如圖1 所示。針對潛航器單驅(qū)動問題，算法最終的輸出層必須為單目標(biāo)的方式，以目標(biāo)航向的形式輸送給潛航器執(zhí)行層。潛航器的目標(biāo)航向來源于潛航器本體所受各抽象力的合力，主要有引力合力和斥力合力。潛航器的引力合力來源于潛航器的航線引力以及航向引力，潛航器的斥力來源于各障礙斥力。

圖 1 混合勢場法控制架構(gòu)圖Fig. 1Control architecture of hybrid potential field method

2 混合勢場法的無人潛航器路徑規(guī)劃方法

2.1 勢場法下路徑跟蹤方法

目前勢場法的理論應(yīng)用很廣，所謂人工勢場法是將實體環(huán)境中的UUV 以及障礙物都抽象為三維空間中的微粒，在微粒間構(gòu)建虛擬力。障礙物形成斥力、目標(biāo)點形成引力，通過多種力的合力實現(xiàn)UUV 的正確運動方向[5]。

圖 2 勢場法路徑跟蹤圖示Fig. 2Diagram of path tracking based on potential field method

參照勢場法理論[6]，潛航器在進行水下路徑跟蹤時，在潛航器周圍構(gòu)建虛擬力場，如圖2 所示。由潛航器偏離航線產(chǎn)生的航線偏差構(gòu)建對潛航器的吸引力吸引力的方向由潛航器指向預(yù)設(shè)定的航線。同時潛航器預(yù)設(shè)定的保持航向?qū)摵狡饕喈a(chǎn)生吸引力航器，實吸際引目力標(biāo)的航方向向由由吸設(shè)引定力航線的A 點指合向力B方點向。決潛定。對于吸引力的構(gòu)建方法有多種，本文采用距離值構(gòu)建法。距離構(gòu)建法中，建立引力勢能函數(shù)見式（1），

其中： Ka為 標(biāo)量控制增益； Rgk為航行器和目標(biāo)中心點之間的距離；dgoal為引力影響半徑，在該值范圍內(nèi)引力影響消失。由此產(chǎn)生的吸引力為吸引勢能場的負的梯度，具有如下形式：

則潛航器目標(biāo)航向 ψd，為航線航向 αk與合力夾角Ψr的合力值：

2.2 混合勢場法下路徑跟蹤及避障方法

一般在研究UUV 避障中，通常需要一條從起始點到目標(biāo)點的無障礙路徑。而通常一般UUV 在進行作業(yè)時需要按照實際的路徑進行作業(yè)，在遇到障礙后進行自主規(guī)避，規(guī)避完成后回歸預(yù)設(shè)定的路線。

在建立勢場法路徑跟蹤方法后，在勢場法全面的算法架構(gòu)下，進行自主避障只需要遵循勢場法的理念在潛航器本體上添加虛擬的障礙物斥力使水下無人潛航器在任務(wù)空間中遠離障礙物，即可實現(xiàn)自主避障[7]。建立潛航器障礙斥力場同樣采用距離值的方法進行構(gòu)建。在建立排斥勢能場時，同時考慮了障礙物對航行器的作用以及目標(biāo)對航行器的作用，可用來解決目標(biāo)在障礙物影響范圍內(nèi)的情況。建立的排斥勢能函數(shù)為如下式：

排斥勢能場負的梯度，即由排斥勢能場產(chǎn)生的排斥力，潛航器障礙物斥力函數(shù)見式（8），斥力的方向由障礙物指向潛航器本體。

其中：rat為當(dāng)前點和吸引目標(biāo)距離； rre為當(dāng)前點和障礙距離， R rei=rre2； Po為障礙物影響距離； m為斥力增益系數(shù)。

在增加相關(guān)斥力場后，潛航器的受力以及運動圖解如圖3 所示。圖中航線以及航向的引力合力FNatt對潛航器產(chǎn)生吸引力，障礙物對潛航器產(chǎn)生斥力FRep，2 種力的綜合作用產(chǎn)生力 FNSJ，力FNSJ將指引潛航器產(chǎn)生航向偏轉(zhuǎn)作用，指引潛航器繞開障礙物進行規(guī)避運動。

圖 3 混合勢場法避障受力示意圖Fig. 3Force diagram of obstacle avoidance with mixed potential field method

3 實航測試

采用中國船舶科學(xué)研究中心研制的“海翔-H”混合動力水下無人潛航器進行試驗，“海翔-H”無人潛航器具備螺旋槳以及尾舵可用于進行有動力推進航行。

3.1 無障礙路徑跟蹤測試

進行無障礙路徑跟蹤測試，給潛航器預(yù)設(shè)計一條1 km 長的弧形折線預(yù)設(shè)軌跡，如圖4 虛線所示。潛航器從圖中起點開始運動，到達終點停止，圖中實線為潛航器實際軌跡，從圖中可以看出潛航器能夠很好的沿預(yù)設(shè)定路線進行運動，在受到實際環(huán)境的風(fēng)浪流干擾后偶爾會偏離預(yù)設(shè)定的航線但很快能夠自主回歸，軌跡跟蹤誤差小于5 m。

圖 4 軌跡跟蹤路線Fig. 4Trajectory tracking

3.2 有障礙路徑跟蹤測試

在進行潛航器避障試驗時，分別設(shè)計潛航器單障礙避障、雙障礙避障、多障礙連續(xù)避障工況，考驗方法的可靠性。水下單障礙避障如圖5 所示，主要是考驗算法控制器在路徑跟蹤以及障礙規(guī)避、回航的能力；雙障礙避障如圖6 所示，主要是考驗算法控制器在按照預(yù)設(shè)定的航線運動過程中受到來自雙重障礙的夾擊作用，滑翔器是否能夠從兩障礙物中間找到最優(yōu)路徑通過，不能出現(xiàn)堵死的狀況；多障礙連續(xù)避障如圖7 所示，主要是考驗算法控制器在航行路線有多個障礙，進行連續(xù)避障、而不發(fā)散的能力。

圖 5 單障礙避障路線Fig. 5Single obstacle avoidance route

圖 6 雙障礙避障路線Fig. 6Double obstacle avoidance route

圖中虛線為預(yù)設(shè)運動軌跡，圓圈為7 m 直徑圓形障礙，實線為實際軌跡。從圖中可以看出，在未接近障礙前潛航器按照設(shè)定路線航行，靠近障礙后潛航器開始自主的偏離航線進行自主規(guī)避，規(guī)避完成后回到預(yù)設(shè)定路線。同時在雙障礙夾擊作用能夠找到最優(yōu)路徑通過，在多個障礙連續(xù)作用能夠連續(xù)避障不發(fā)散。

4 結(jié) 語

本文從應(yīng)用實際出發(fā)，針對水下無人潛航器避障、航向保持、航線跟蹤等多輸出、單尾舵輸入欠驅(qū)動控制問題，提出一種統(tǒng)一架構(gòu)的混合勢場法。該方法巧妙利用虛擬力的理念，在避障控制、航向保持控制、航線跟蹤控制等全部建立虛擬力場，通過多種力場的綜合作用生成單一合力場指引尾舵操縱。同時本文還利用“海翔-H”水下無人潛航器進行實際水域的航線跟蹤、單障礙避障、雙障礙避障、多障礙連續(xù)避障測試。結(jié)果表明，混合勢場法能夠很好的兼顧潛航器實際作業(yè)時的航向保持、航線跟蹤、避障等工況，具備良好的魯棒性。另外，通過本文建立起的統(tǒng)一勢場法增加虛擬力還可以很快的拓展到無人潛航器的編隊控制中。

圖 7 多障礙避障路線Fig. 7Obstacle avoidance route