李聰,胡超芳,沈同圣,趙德鑫

(1.天津大學 電氣自動化與信息工程學院,天津 300072;2.國防科技創(chuàng)新研究院,北京 100071)

近些年,隨著自主水下航行器(Autonomous underwater vehicle, AUV)自主平臺、傳感器、通信以及智能算法的飛速發(fā)展,使得AUV在水下爆炸物等危險物品探測領域發(fā)揮著越來越重要的作用,而路徑規(guī)劃和實時避障能力是AUV的兩項關鍵技術,也是完成AUV水下作業(yè)任務的重要基礎[1-2]。AUV路徑規(guī)劃任務通常分為全局路徑規(guī)劃和局部路徑規(guī)劃,當AUV進行水下目標探測作業(yè)時,通常需要根據待搜索區(qū)域水下環(huán)境的先驗知識,預先對AUV進行全局路徑規(guī)劃,而后再根據感知到的環(huán)境信息、自身的健康狀況以及目標信息,采用局部路徑修復技術,使AUV可有效跟蹤水下目標,保證其高效的水下作業(yè)[3-4]。學者們在局部路徑修復研究方面做了許多的工作,文獻[5]提出利用基于行為的控制體系結構,通過對行為進行矢量求和獲得移動機器人的控制決策,有效解決了傳統(tǒng)的感知-計劃-行動控制回路維護單一世界模型的不足。文獻[6]基于行為體系結構,通過對每個可能的控制動作進行投票,選擇每個變量的單獨決策來確定移動機器人的路徑。但是對于移動機器人的耦合控制變量,例如速度和航向變量,對其進行解耦控制容易造成較差的整體行為,存在控制決策選擇的困難。為了解決這一問題,通過對自主機器人所有可能的動作進行評估,每個行為都將在公共的決策空間上產生一個目標函數,將自主機器人動作選擇問題視為一個多目標優(yōu)化問題[7-8],但是對自主機器人所有可能的動作進行評估是一個計算量非常大的工作,決策空間也會隨著決策變量的增加呈指數增長,難以及時產生控制決策。

針對上述方法的不足,本文設計了在MOOS體系下利用區(qū)間優(yōu)化(Interval programming, IVP)模型進行AUV行為的協調。IVP模型是一種表示和求解多目標優(yōu)化問題的數學模型,在IVP模型中每個行為都會產生一個分段線性定義的目標函數,是對基本線性函數的一個近似,通過各個塊(每個函數一個)的組合空間而不是實際的決策空間來進行搜索的算法,并且在每個塊中,分段線性函數隱式的對塊中包含的所有動作進行了評估[9-10]。根據AUV路徑規(guī)劃中實時避障的要求,設計路徑點跟蹤行為、實時避障行為,并將環(huán)境約束融入目標函數中,利用IVP模型協調行為,完成了指定區(qū)域內設定點的路徑規(guī)劃,分析了IVP模型的有效性。

1 路徑規(guī)劃問題建模

1.1 分布式控制體系結構MOOS-IVP

MOOS-IVP體系結構主要用于對無人駕駛的水面航行器和水下機器人等進行自主控制,由Paul Newman和Mike Benjamin共同維護和開發(fā),包括MOOS(Mission oriented operating suite)和IVP Helm兩個部分。如圖1所示,采用模塊化設計和分布式星型拓撲結構將整體功能分離成單獨的不同的模塊,MOOSDB負責協調所有模塊,并且不同模塊代碼之間完全解耦[11-12]。

圖1 MOOS應用程序及其連接

基于模塊的設計思想為自主系統(tǒng)的進一步發(fā)展提供了更高的可靠性和更短的開發(fā)時間[13]。其中MOOS的核心是MOOSDB數據庫,主要用于協調MOOS體系中運行的獨立的MOOS應用程序,所有的MOOS應用程序模塊彼此之間不能相互通信,必須通過MOOSDB數據庫以“發(fā)布-訂閱”的方式來獲取各自所需的數據信息。

1.2 行為控制下的IVP Helm

IVP Helm作為一個獨立的MOOS應用程序主要用于協調它自身內部運行的多個自治行為,它的核心是用于管理行為的多目標優(yōu)化求解器,用于生成行為集的最終控制決策。AUV航跡的控制決策所需要的信息由MOOS-IVP體系中的各個應用程序提供,并將產生的信息以郵件的形式發(fā)送到其他應用程序。如圖2所示,IVP Helm首先從MOODB獲取信息,并放在數據緩沖區(qū)中供行為查詢使用,為了使行為有效的產生目標函數,需要對行為模式進行聲明,通常情況下,AUV任務結構包括一組任務模式M,即

圖2 行為求解過程

M={m1,m2,…,mn}

(1)

每個模式都由唯一名稱及一組邏輯條件定義,即

mi={n,c1,c2,…,cp}

(2)

行為根據權重信息產生一個目標函數,最后IVP求解器對多個行為目標函數執(zhí)行多目標優(yōu)化產生AUV航跡的控制決策。

(3)

2 IVP模型中的路徑規(guī)劃行為

2.1 路徑點跟蹤行為

這里假設AUV工作在同一深度,路徑點跟蹤行為主要用于在x-y平面上過渡到一組指定的路徑點,其中路徑點可以以多邊形頂點或者割草機路線點的形式在任務啟動前直接指定,也可以在AUV運行時動態(tài)提供。這里我們以目標為中心設定了一組環(huán)繞路徑點,并認為以這樣方式航行的AUV能夠掌握足夠多的目標信息,從而間接實現對目標的探測。定義每個路徑點周圍內徑圓和外徑圓來確定AUV實際到達某個路徑點需要滿足的條件,具體思想如圖3所示。

圖3 路徑點的到達條件

圖3a)中AUV通過路徑點的內徑圓,此時宣布到達路徑點;圖3b)中AUV錯過了內徑圓,但是當AUV在外徑圓范圍內,并且到航路點的距離開始增加時,同樣宣布到達;圖3c)中AUV既不通過內徑圓,也錯過了外徑圓的范圍,此時AUV無法到達當前路徑點,也就無法獲取路徑點的信息。

在兩個路徑點之間,既可以使AUV簡單的轉向下一個航路點,也可以根據任務情況驅動AUV轉向航跡上的任意一點,具體轉向點由跟蹤控制引線參數L決定,思想如圖4所示。它是從垂直交點d到下一個路徑點的距離,定義了軌道線上一個假想的路徑點,該行為根據這個假想的轉向點輸出一個航向,如果引線參數L大于沿著軌線到下一個航路點的距離,則假想的點就是下一個航路點。同時,通過控制AUV與跟蹤控制引線的距離h。可以使AUV在有效距離內被拉伸到下一個航路點,以減少軌跡線的超調。

圖4 路徑點之間的過渡

路徑點跟蹤行為在IVP Helm的每個迭代中產生一個定義在變量速度和航向上的目標函數,如圖5所示,它驅動AUV以接近2 m/s的速度航行到位于90°方向的路徑點,圓圈表示航向變量,半徑表示速度變量,距離中心越遠的位置表示更大的速度值,并且藍色部分表示不可取值,深紅色部分表示行為偏好的控制決策。

圖5 路徑點跟蹤行為目標函數

2.2 實時避障行為

在AUV路徑規(guī)劃過程中,并非所有的障礙物及其位置在任務加載時就預先知道,只有當AUV及其傳感器逐漸接近時才被發(fā)現,通過對原路徑進行局部路徑修復,可以實時有效的處理障礙。文中通過傳感器模塊標記的動態(tài)點簇來表示輸入的障礙物,障礙管理器維護一個凸多邊形表示來自傳感器模塊的每一個點簇,并只保留傳入的最新的N個動態(tài)特征點,即隨著新感知點的出現,舊感知點將從障礙管理器的內存中消失。通常情況下,當障礙物的幾何形狀未知時,采用不規(guī)則的凸多邊形維護每一個點簇,并且隨著障礙物的漂移,多邊形也會隨之繼續(xù)增大,從而增加了計算的復雜性。為了簡化處理,采用指定徑向距離和頂點數的規(guī)則多邊形維護單個動態(tài)障礙點簇,其中多邊形以給定點簇中點的平均值為中心,如圖6所示。

圖6 規(guī)則多邊形近似的障礙物

AUV沿著一組路徑點表示的路徑航行,當障礙物進入傳感器范圍時,障礙管理器提供有關障礙物的信息,并促進IVP Helm生成避障行為從而形成經過該區(qū)域的局部路徑修復。避障行為作為模板存在于IVP Helm 中,在收到關于障礙物的信息之前不會產生任何避障行為,它被配置為在其到障礙物的距離減小時增加優(yōu)先級,定義了如圖7所示的兩個距離參數D1和D2,D2直接影響避障行為是否應處于活躍的模式,當AUV和障礙物的距離大于D2時,優(yōu)先級權重為零,該行為是不活躍的,當AUV在D1范圍內時,配置的優(yōu)先級權重為100%,行為是完全活躍的,而在D1和D2距離之間,優(yōu)先級權重呈線性方式下降。

圖7 避障行為優(yōu)先級權重變化過程

考慮到傳感器在感知障礙物的大小及其位置時的不確定性以及在利用規(guī)則多邊形近似實際障礙物的誤差,定義了圖8中用虛線框表示的緩沖多邊形, AUV在避障過程中從緩沖多邊形的一邊繞到另一邊實現避碰,有效的實現了AUV機動安全性和可行路徑之間的平衡。

圖8 避障實現過程,灰色多邊形表示障礙物

避障行為的目標函數定義在速度v和航向θ的笛卡爾積組成的控制決策空間上,其中:

(4)

函數g(x,y,θ,P)為真表示AUV從當前位置(x,y)沿著θ方向航行時與緩沖多邊形P不相交。避障行為目標函數形式如下:

(5)

式中:函數h(x,y,v,θ,P)表示AUV從當前位置(x,y)沿著θ方向以速度v航行時與緩沖多邊形相交所用時間;tf表示當避障行為活躍時,從當前位置直接過渡到障礙物所能容忍的最大秒數。避障行為產生的目標函數如圖9所示,它輸出航向大約160°、速度大約為2 m/s的AUV航行控制決策,其中深紅色區(qū)域表示AUV控制決策的偏好值。

圖9 實時避障行為目標函數

3 仿真分析

為了說明上述IVP模型及其控制算法的有效性,采用實際的AUV動力學模型,通過PID控制器進行航行控制,并在MOOS-IVP仿真環(huán)境下進行實驗驗證。圖10給出了仿真結果,AUV從初始點(0,0)以期望速度v=2 m/s,初始航向θ=180°出發(fā),最大加速度a1=0.1 m/s2,最大減速度a2=0.1 m/s2,需要跟蹤的路徑點是分別以目標A=(90,-105)和目標B=(200,-58)為中心,徑向距離為30 m的正八邊形頂點。為每個路徑點定義的參數如下:L=30 m;h=2 m;內徑圓半徑r1=5 m;外徑圓半徑r2=10 m。

圖10 實驗仿真結果圖

實驗中利用徑向距離為6 m,頂點數為8的正多邊形維護了5個障礙物,配置警報距離參數h1=35 m,當AUV與障礙物的距離進入這個范圍時,生成避障行為,定義的其他障礙物參數如下:D1=12 m;D2=20 m;D3=11 m;tf=5 s;N=50個。其中N表示感知并保留最近的障礙物的特征點數,而D3表示圖8中緩沖多邊形的徑向距離。

為了更好地了解避障過程,圖11中分別列出了t1、t2、t3時刻對應的目標函數,圖中深紅色表示AUV偏好的控制決策,而藍色表示不可取值。

圖11 不同時刻目標函數產生的控制決策

圖10表明,AUV根據到障礙物的距離范圍實時生成避障行為,成功避開障礙物實現對目標A和目標B周圍的多邊形路徑點的跟蹤,證明了文中設計的IVP模型和控制算法在AUV路徑點跟蹤以及實時避障方面的有效性和可行性,圖12是對應的速度、航向時間序列圖。

圖12 速度航向時間序列圖

4 結束語

本文研究了AUV的路徑規(guī)劃和實時避障控制問題,介紹了一種新的基于行為的控制方法,將路徑規(guī)劃任務劃分為多個具體的行為,設計路徑點跟蹤行為對以指定位置目標為中心的規(guī)則多邊形的頂點進行順時針方式的路徑點跟蹤,設計實時避障行為避開障礙,并搭建MOOS-IVP體系結構模型,利用IVP模型的多目標優(yōu)化求解算法進行控制決策選擇。最后在仿真分析中,AUV繞開障礙物實現對規(guī)則多邊形的路徑點跟蹤說明了本文設計的基于IVP模型的體系結構及其控制算法的可行性和有效性,降低了計算的復雜程度,并保證及時獲得最優(yōu)的結果。