IVP模型在AUV路徑規(guī)劃與實(shí)時(shí)避障中的應(yīng)用研究

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

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

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

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

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

1.1 分布式控制體系結(jié)構(gòu)MOOS-IVP

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

圖1 MOOS應(yīng)用程序及其連接

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

1.2 行為控制下的IVP Helm

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

圖2 行為求解過程

M={m1,m2,…,mn}

(1)

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

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

(2)

行為根據(jù)權(quán)重信息產(chǎn)生一個(gè)目標(biāo)函數(shù),最后IVP求解器對多個(gè)行為目標(biāo)函數(shù)執(zhí)行多目標(biāo)優(yōu)化產(chǎn)生AUV航跡的控制決策。

(3)

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

2.1 路徑點(diǎn)跟蹤行為

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

圖3 路徑點(diǎn)的到達(dá)條件

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

在兩個(gè)路徑點(diǎn)之間,既可以使AUV簡單的轉(zhuǎn)向下一個(gè)航路點(diǎn),也可以根據(jù)任務(wù)情況驅(qū)動(dòng)AUV轉(zhuǎn)向航跡上的任意一點(diǎn),具體轉(zhuǎn)向點(diǎn)由跟蹤控制引線參數(shù)L決定,思想如圖4所示。它是從垂直交點(diǎn)d到下一個(gè)路徑點(diǎn)的距離,定義了軌道線上一個(gè)假想的路徑點(diǎn),該行為根據(jù)這個(gè)假想的轉(zhuǎn)向點(diǎn)輸出一個(gè)航向,如果引線參數(shù)L大于沿著軌線到下一個(gè)航路點(diǎn)的距離,則假想的點(diǎn)就是下一個(gè)航路點(diǎn)。同時(shí),通過控制AUV與跟蹤控制引線的距離h?？梢允笰UV在有效距離內(nèi)被拉伸到下一個(gè)航路點(diǎn),以減少軌跡線的超調(diào)。

圖4 路徑點(diǎn)之間的過渡

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

圖5 路徑點(diǎn)跟蹤行為目標(biāo)函數(shù)

2.2 實(shí)時(shí)避障行為

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

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

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

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

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

圖8 避障實(shí)現(xiàn)過程,灰色多邊形表示障礙物

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

(4)

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

(5)

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

圖9 實(shí)時(shí)避障行為目標(biāo)函數(shù)

3 仿真分析

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

圖10 實(shí)驗(yàn)仿真結(jié)果圖

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

為了更好地了解避障過程,圖11中分別列出了t1、t2、t3時(shí)刻對應(yīng)的目標(biāo)函數(shù),圖中深紅色表示AUV偏好的控制決策,而藍(lán)色表示不可取值。

圖11 不同時(shí)刻目標(biāo)函數(shù)產(chǎn)生的控制決策

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

圖12 速度航向時(shí)間序列圖

4 結(jié)束語

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