王震, 桑宏強(qiáng), 孫秀軍,2,3*
(1. 天津工業(yè)大學(xué)機(jī)械工程學(xué)院, 天津 300387; 2. 中國海洋大學(xué)物理海洋教育部重點(diǎn)實(shí)驗(yàn)室, 青島 266100; 3. 青島海洋科學(xué)與技術(shù)國家實(shí)驗(yàn)室海洋動(dòng)力過程與氣候功能實(shí)驗(yàn)室, 青島 266237)
波浪滑翔器(wave glider, WG)作為一種新型的海洋探測(cè)平臺(tái)[1],通過波浪能產(chǎn)生向前推力,同時(shí)攜帶太陽能電池板,為長(zhǎng)期的海洋觀測(cè)任務(wù)提供了保障。波浪滑翔器具有續(xù)航時(shí)間長(zhǎng)、控制容易等優(yōu)點(diǎn)。然而單臺(tái)波浪滑翔器觀測(cè)區(qū)域有限,無法實(shí)現(xiàn)大范圍的海洋數(shù)據(jù)采集,因此需要多臺(tái)波浪滑翔器組成編隊(duì)來完成更加復(fù)雜的任務(wù)。波浪滑翔器編隊(duì)如何防止內(nèi)部的碰撞,又能安全躲避障礙物,最終順利地完成一定區(qū)域內(nèi)的觀測(cè)任務(wù),顯得尤為重要。
為了實(shí)現(xiàn)期望的編隊(duì)行為[2-3],學(xué)者們提出了多種思路,按照編隊(duì)控制思想可分為領(lǐng)航跟隨者法[4]、虛擬結(jié)構(gòu)法、基于一致性算法、基于行為法[5]、基于勢(shì)函數(shù)法[6-7]和圖論法[8]等。Cui等[9]應(yīng)用參考路徑和虛擬水下航行器的思想,同時(shí)通過反步法來控制編隊(duì)隊(duì)形。潘無為等[10]通過結(jié)合人工勢(shì)場(chǎng)法和虛擬結(jié)構(gòu)法,實(shí)現(xiàn)了智能水下機(jī)器人的編隊(duì)控制。Choi等[11]則應(yīng)用自適應(yīng)技術(shù)通過對(duì)領(lǐng)航者的速度進(jìn)行估計(jì),然后利用位置信息完成編隊(duì)控制。而針對(duì)避障問題,常見的避障控制算法包括遺傳算法、動(dòng)態(tài)窗口法、人工勢(shì)場(chǎng)法等。許多研究人員采用人工勢(shì)場(chǎng)法來實(shí)現(xiàn)避障控制,因?yàn)樵摲椒ň哂杏?jì)算簡(jiǎn)單、避障安全性高、實(shí)時(shí)性強(qiáng)等優(yōu)點(diǎn)。韓君[12]通過將北斗定位信息和全球定位系統(tǒng)(global positioning system,GPS)定位信息相融合,同時(shí)結(jié)合人工勢(shì)場(chǎng)法,實(shí)現(xiàn)了無人船的高精度自動(dòng)化智能航行。Wang等[13]提出了一種基于人工勢(shì)場(chǎng)算法的波浪滑翔器避障方法,通過引入一個(gè)新的斥力函數(shù)解決了局部最小域問題,實(shí)現(xiàn)了波浪滑翔器避障,但未考慮編隊(duì)問題。張強(qiáng)等[14]通過結(jié)合蟻群算法與人工勢(shì)場(chǎng)法,解決了自主機(jī)器人的路徑規(guī)劃問題。Zhang等[15]通過增設(shè)虛擬目標(biāo)點(diǎn)的方法,解決了局部最小域問題,實(shí)現(xiàn)了無人機(jī)的編隊(duì)避障控制。
由上述分析可知,雖然大多數(shù)改進(jìn)的人工勢(shì)場(chǎng)法解決了局部最小域問題,但現(xiàn)有的研究主要集中在機(jī)器人個(gè)體的控制上,未考慮編隊(duì)內(nèi)部的碰撞及編隊(duì)的保持問題,同時(shí)研究對(duì)象大多具有一定驅(qū)動(dòng)能力的自主水下機(jī)器人,針對(duì)波浪滑翔器這種欠驅(qū)動(dòng)無人船的研究較少。針對(duì)上述問題,現(xiàn)通過結(jié)合領(lǐng)航跟隨者法與人工勢(shì)場(chǎng)法,將波浪滑翔器編隊(duì)視為多體系統(tǒng),利用人工勢(shì)場(chǎng)法實(shí)現(xiàn)編隊(duì)的避障以及編隊(duì)內(nèi)部的防撞與保持,為波浪滑翔器編隊(duì)的應(yīng)用提供理論研究基礎(chǔ)。
準(zhǔn)確的波浪滑翔器運(yùn)動(dòng)學(xué)模型,有利于分析和控制波浪滑翔器編隊(duì)和避障控制系統(tǒng)。由于海洋環(huán)境多變,所以波浪滑翔器的運(yùn)動(dòng)情況較為復(fù)雜。為了保證編隊(duì)避障方法研究的可靠性,對(duì)波浪滑翔器運(yùn)動(dòng)模型做出如下的簡(jiǎn)化。
(1)因?yàn)樗聽恳龣C(jī)在水下足夠重,無論是浮體船隨波浪上升階段還是下降階段,臍帶纜始終處于拉緊狀態(tài)。因此,在將臍帶纜視為剛體的前提下,推導(dǎo)了波浪滑翔器的運(yùn)動(dòng)參數(shù)。
(2)當(dāng)考慮波浪滑翔器的整體運(yùn)動(dòng)時(shí),水面浮體船和水下牽引機(jī)的形狀忽略不計(jì)。
(3)波浪滑翔器的質(zhì)量分布均勻,且原點(diǎn)位于重心。
所研究的波浪滑翔器主要是在海平面上運(yùn)動(dòng),只考慮水面浮體船的橫蕩、縱蕩、垂蕩和艏搖4個(gè)自由度就可以模擬出波浪滑翔器的運(yùn)動(dòng)狀態(tài)。因此基于大地坐標(biāo)系(E)和船體坐標(biāo)系(F)(圖1),建立波浪滑翔器運(yùn)動(dòng)學(xué)模型為
(1)
(2)
圖1 坐標(biāo)系建立Fig.1 Establishment of coordinate system
采用領(lǐng)航跟隨者方法控制波浪滑翔器的編隊(duì)。領(lǐng)航跟隨者編隊(duì)策略中需要設(shè)定一臺(tái)波浪滑翔器為領(lǐng)航者,其余波浪滑翔器作為跟隨者,該編隊(duì)策略下的整體編隊(duì)行動(dòng)都是基于領(lǐng)航波浪滑翔器的行動(dòng)軌跡,領(lǐng)航跟隨者編隊(duì)策略示意圖如圖2所示。
圖2 領(lǐng)航跟隨者編隊(duì)示意圖Fig.2 The formation schematic of the leader and followers
由多個(gè)波浪滑翔器組成的集群網(wǎng)絡(luò),可以通過編隊(duì)協(xié)同控制共同完成單臺(tái)波浪滑翔器無法完成的復(fù)雜海洋任務(wù),同時(shí)降低任務(wù)對(duì)單臺(tái)設(shè)備性能的要求及波浪滑翔器的制造成本。波浪滑翔器的協(xié)同控制主要體現(xiàn)在對(duì)多臺(tái)波浪滑翔器進(jìn)行整體編隊(duì)和安全避障。
現(xiàn)有的多機(jī)器人編隊(duì)系統(tǒng)絕大多數(shù)是通過控制單體的各種局部行為,從而實(shí)現(xiàn)整體編隊(duì)系統(tǒng)的協(xié)同工作。在基于領(lǐng)航跟隨者編隊(duì)策略的基礎(chǔ)上,根據(jù)波浪滑翔器編隊(duì)的特殊海洋工作環(huán)境,考慮到各波浪滑翔器單體間信息異步傳輸?shù)膹?fù)雜性,以及各個(gè)參數(shù)獲取的不確定性等因素,引入人工勢(shì)場(chǎng)算法的思想,在波浪滑翔器編隊(duì)間建立間距勢(shì)能場(chǎng),利用間距勢(shì)能場(chǎng)所產(chǎn)生的作用力使波浪滑翔器個(gè)體之間形成互聯(lián),成為一個(gè)“虛擬的多波浪滑翔器系統(tǒng)”,最終實(shí)現(xiàn)編隊(duì)隊(duì)形的保持。
在相鄰兩波浪滑翔器間建立間距勢(shì)能場(chǎng),來控制相鄰兩波浪滑翔器的間距,使它們保持在一定的距離位置上。定義間距勢(shì)能函數(shù)為
(3)
式(3)中:rij為相鄰波浪滑翔器之間的距離;d0為相鄰波浪滑翔器間的最小期望距離;d1為相鄰波浪滑翔器發(fā)生吸引作用還是排斥作用的臨界距離;kI為間距勢(shì)能場(chǎng)系數(shù)。
當(dāng)相鄰波浪滑翔器間距rij
間距勢(shì)能場(chǎng)產(chǎn)生的作用力為它的負(fù)梯度,可表示為
圖3 三角形波浪滑翔器編隊(duì)Fig.3 Triangular formation of wave gliders
(4)
人工勢(shì)場(chǎng)法具有計(jì)算簡(jiǎn)單、實(shí)時(shí)性好等特點(diǎn),采用人工勢(shì)場(chǎng)法實(shí)現(xiàn)波浪滑翔器的編隊(duì)控制,為了方便研究,障礙物都近似為圓形。
人工勢(shì)場(chǎng)法通過勢(shì)場(chǎng)函數(shù)來表達(dá)波浪滑翔器、目標(biāo)點(diǎn)以及障礙物之間的約束關(guān)系,勢(shì)場(chǎng)函數(shù)的負(fù)梯度即為相應(yīng)的受力表達(dá)式。傳統(tǒng)人工勢(shì)場(chǎng)法中引力勢(shì)場(chǎng)的數(shù)學(xué)模型和斥力勢(shì)場(chǎng)的數(shù)學(xué)模型分別表示為
(5)
(6)
式中:(x,y)為波浪滑翔器的位置坐標(biāo);δ為吸引力系數(shù);ρgoal為波浪滑翔器與目標(biāo)點(diǎn)之間的距離;η為排斥力系數(shù);ρob為波浪滑翔器與障礙物之間的距離;ρ0為障礙物的影響距離,意味著只有波浪滑翔器在障礙物影響距離內(nèi)才會(huì)受到障礙物的排斥力。
人工勢(shì)場(chǎng)法實(shí)現(xiàn)波浪滑翔器編隊(duì)避障的過程中通常存在局部最小域問題,表現(xiàn)為引力和排斥力的合力為零。滿足以下任意條件則波浪滑翔器陷入局部最小域。
條件(1):Uatt(x)+Urep(x)+UI(x)=0。
條件(2):|xa-xb|≤αsab。
條件(1)成立表示波浪滑翔器的合勢(shì)能為零,其中,Uatt為目標(biāo)點(diǎn)的引力勢(shì)能,Urep為障礙物的排斥勢(shì)能,UI為相鄰波浪滑翔器的間距勢(shì)能。條件(2)成立表示波浪滑翔器移動(dòng)了較長(zhǎng)距離,但是位移很小,|xa-xb|表示兩點(diǎn)之間的位移大小,sab表示兩點(diǎn)間的路程,α是一個(gè)很小的正數(shù)。
針對(duì)上述局部最小域問題,通過引入一個(gè)沿障礙物切線方向的引力Uatt2來幫助波浪滑翔器編隊(duì)逃出局部最小域,如圖4所示。
根據(jù)幾何知識(shí),從波浪滑翔器的位置(x,y)到圓心為(xo,yo)半徑為r的障礙物可做一條切線,切線的斜率可以表示為
圖4 切線方向的新輸入引力Fig.4 The new input attracted force along tangent direction
(7)
切點(diǎn)的坐標(biāo)可以表示為
(8)
最終,沿切線方向的新輸入引力Uatt2的大小可以表示為
(9)
浪滑翔器編隊(duì)的控制體系主要分為3個(gè)層次,分別是任務(wù)協(xié)調(diào)層、任務(wù)執(zhí)行層和編隊(duì)避障層,如圖5所示。任務(wù)協(xié)調(diào)層的主要任務(wù),即所有波浪滑翔器控制系統(tǒng)初始化,主要包括編隊(duì)隊(duì)形信息初始化操作、編隊(duì)任務(wù)分配初始化、設(shè)備搭載傳感器初始化操作。任務(wù)執(zhí)行層根據(jù)分配好的任務(wù)需求同時(shí)參考搭載傳感器獲得的環(huán)境信息,按照既定的虛擬領(lǐng)航者編隊(duì)方式行進(jìn)同時(shí)通過間距勢(shì)能場(chǎng)實(shí)現(xiàn)編隊(duì)隊(duì)形保持。編隊(duì)避障層在波浪滑翔器編隊(duì)行進(jìn)的過程中,根據(jù)改進(jìn)人工勢(shì)場(chǎng)法對(duì)障礙物進(jìn)行躲避,保證波浪滑翔器編隊(duì)的安全。
圖5 波浪滑翔器編隊(duì)控制體系Fig.5 Control architecture of wave glider formation
運(yùn)用所提出的新輸入引力的方法給出具有局部最小域下的編隊(duì)避障仿真。假設(shè)由3臺(tái)波浪滑翔器組成三角形的編隊(duì)隊(duì)形,要求波浪滑翔器編隊(duì)在保持隊(duì)形的情況下,自主地躲避障礙物抵達(dá)目標(biāo)位置。區(qū)域內(nèi)有一個(gè)半徑為50 m的圓形障礙物,圓心位于(230,230)m處。相鄰兩波浪滑翔器之間的期望距離范圍為16~18 m。波浪滑翔器的初始位置分別為WG1(50,50)m,WG2(50,30)m和WG3(30,50)m,目標(biāo)點(diǎn)位置為(400,400)m。波浪滑翔器編隊(duì)避障仿真圖如圖6所示。編隊(duì)在(80,80)m處陷入局部最小域,此時(shí)引入沿切線方向的新輸入引力幫助編隊(duì)逃離局部最小域。
圖7為波浪滑翔器和障礙物之間的距離,可以看出隨著波浪滑翔器朝向目標(biāo)點(diǎn)運(yùn)動(dòng),距離障礙物越來越近并在75 s時(shí)距離障礙物最近,但始終與障礙物保持著安全距離,沒有發(fā)生碰撞。
圖8為波浪滑翔器編隊(duì)內(nèi)部?jī)蓛稍O(shè)備之間的距離,可以看出由于受間距勢(shì)能場(chǎng)和障礙物排斥勢(shì)場(chǎng)的共同影響,相鄰波浪滑翔器之間的距離越來越小,直到75 s時(shí),因?yàn)槭艿秸系K物的排斥勢(shì)場(chǎng)影響最大,此時(shí)相鄰波浪滑翔器之間的間距最小。當(dāng)波浪滑翔器編隊(duì)逐漸遠(yuǎn)離障礙物后,障礙物的排斥勢(shì)場(chǎng)影響逐漸變小,此時(shí)波浪滑翔器受間距勢(shì)能場(chǎng)的影響,相鄰設(shè)備間的間距逐漸恢復(fù)到期望范圍內(nèi)。
圖6 波浪滑翔器編隊(duì)運(yùn)動(dòng)軌跡Fig.6 Motion trajectories of the wave glider formation
圖9為波浪滑翔器編隊(duì)中各設(shè)備的速度圖,可以看出各設(shè)備的速度保持穩(wěn)定,所以編隊(duì)保持較為穩(wěn)定。
圖7 波浪滑翔器和障礙物之間的距離Fig.7 The distances of the wave glider and the obstacle
圖8 相鄰波浪滑翔器之間的距離Fig.8 The distances among adjacent wave gliders
圖9 波浪滑翔器速度Fig.9 The velocities of wave gliders
波浪滑翔器編隊(duì)的控制主要是用來調(diào)節(jié)相鄰波浪滑翔器之間的距離。圖10用間距勢(shì)能場(chǎng)實(shí)現(xiàn)波浪滑翔器的編隊(duì)控制,圖11的編隊(duì)避障算法不包括間距勢(shì)能場(chǎng)的控制,所以兩個(gè)波浪滑翔器的運(yùn)動(dòng)軌跡有重合的部分,表明兩設(shè)備發(fā)生了碰撞。圖12給出了無間距勢(shì)能場(chǎng)的作用下相鄰波浪滑翔器之間的距離,可以看出隨著編隊(duì)朝向目標(biāo)點(diǎn)運(yùn)動(dòng),受障礙物排斥勢(shì)場(chǎng)的影響,設(shè)備間距越來越小,在距離障礙物最近的時(shí)候WG1和WG2發(fā)生了碰撞,圖中的紅色實(shí)線表明兩個(gè)波浪滑翔器WG1和WG2之間的距離為零。
圖10 有間距勢(shì)能場(chǎng)的編隊(duì)軌跡Fig.10 Formation trajectories with spaced potential field
圖11 無間距勢(shì)能場(chǎng)的編隊(duì)軌跡Fig.11 Formation trajectories without spaced potential field
圖12 無間勢(shì)能場(chǎng)下相鄰波浪滑翔器間距Fig.12 The distances among adjacent wave gliders without spaced potential field
針對(duì)欠驅(qū)動(dòng)波浪滑翔器的編隊(duì)避障問題,提出了一種基于領(lǐng)航跟隨者法和人工勢(shì)場(chǎng)法的多波浪滑翔器編隊(duì)避障方法。通過MATLAB仿真實(shí)驗(yàn),得出如下結(jié)論。
(1)采用領(lǐng)航跟隨法與人工勢(shì)場(chǎng)法結(jié)合的思想,實(shí)現(xiàn)了波浪滑翔器編隊(duì)。通過間距勢(shì)能場(chǎng)產(chǎn)生的力使各個(gè)波浪滑翔器連接成一個(gè)構(gòu)型,同時(shí)控制設(shè)備的間距,避免編隊(duì)內(nèi)部發(fā)生碰撞的同時(shí)實(shí)現(xiàn)了編隊(duì)隊(duì)形的保持。
(2)采用人工勢(shì)場(chǎng)法實(shí)現(xiàn)波浪滑翔器編隊(duì)的避障,針對(duì)人工勢(shì)場(chǎng)法的局部最小域問題,通過引入沿障礙物切線方向的引力,幫助波浪滑翔器打破平衡狀態(tài),從而逃脫局部最小域。通過仿真驗(yàn)證了所提波浪滑翔器編隊(duì)避障方法的有效性。