謝天奇, 李曄, 姜言清,2, 龐碩, 張文君, 徐雪峰

(1.哈爾濱工程大學(xué) 水下機(jī)器人技術(shù)重點(diǎn)實(shí)驗(yàn)室, 黑龍江 哈爾濱 150001; 2.河南省水下智能裝備重點(diǎn)實(shí)驗(yàn)室, 河南 鄭州 450015; 3.中國(guó)船舶航海保障技術(shù)實(shí)驗(yàn)室, 天津 300131)

自主水下航行器(autonomous underwater vehicle,AUV)目前被廣泛應(yīng)用于水下勘測(cè)、礦產(chǎn)勘探和海洋測(cè)繪等水下作業(yè)任務(wù)[1]。因此,關(guān)于水下航行器的研究也吸引了眾多研究人員的關(guān)注,水下自主回收技術(shù)就是其中一個(gè)研究熱點(diǎn)[2]。由于AUV的航行時(shí)間一直受到其電池容量的限制,借助自主回收技術(shù)AUV可以實(shí)現(xiàn)隨時(shí)自動(dòng)充電,這是一種提高AUV任務(wù)執(zhí)行時(shí)間的替代方案;且在水下自主回收技術(shù)的幫助下,水下航行器在回收成功后可以采取有線方式高速傳輸數(shù)據(jù)。另外,與固定式回收技術(shù)相比,移動(dòng)式回收具有更大的靈活性。例如,水面無(wú)人艇可以作為AUV的運(yùn)輸和支持平臺(tái),與AUV一同協(xié)作完成更復(fù)雜更耗時(shí)的任務(wù)。

由于AUV水下自主回收技術(shù)具有重大的研究意義和實(shí)用價(jià)值,所以水下自主回收技術(shù)的理論研究和試驗(yàn)正大力開展[3-7]。文獻(xiàn)[3]介紹了一種基于雙目攝像機(jī)的可靠視覺定位方法,該方法根據(jù)雙目攝像機(jī)中標(biāo)記點(diǎn)的圖像數(shù)量在單目和雙目組合定位方法之間切換,并進(jìn)行了水下回收試驗(yàn),試驗(yàn)證明了AUV可以由不同的起點(diǎn)出發(fā)進(jìn)入固定漏斗形回收站。文獻(xiàn)[4]中提出了一種自動(dòng)布放和回收系統(tǒng),可滿足2種布放回收方案,即移動(dòng)式與固定式USV布放和回收AUV,海上固定式回收回收試驗(yàn)已證明此系統(tǒng)可用于自主布放和回收AUV。文獻(xiàn)[5]提出了一種基于視覺的移動(dòng)式回收方法,用于在淺水中由一個(gè)AUV回收另一個(gè)AUV,外場(chǎng)試驗(yàn)驗(yàn)證了提出的視覺導(dǎo)航方法的可靠性。文獻(xiàn)[6]和文獻(xiàn)[7]中通過仿真驗(yàn)證了AUV對(duì)接回收過程中軌跡規(guī)劃方法的合理性,但移動(dòng)式對(duì)接軌跡規(guī)劃方法仍然難以解決軌跡規(guī)劃最優(yōu)性和實(shí)時(shí)計(jì)算之間的矛盾。文獻(xiàn)[8-10]中通過仿真研究了在海流影響的情況下的AUV軌跡跟蹤控制問題,但由于文獻(xiàn)[10]中期望前向速度公式設(shè)計(jì)錯(cuò)誤導(dǎo)致當(dāng)AUV位于目標(biāo)點(diǎn)前方時(shí)控制器發(fā)散。本文針對(duì)文獻(xiàn)[10]的不足提出了新的期望速度公式保證AUV順利完成對(duì)接任務(wù)。另外,擬通過利用半物理仿真驗(yàn)證AUV移動(dòng)式對(duì)接軌跡規(guī)劃方法的實(shí)時(shí)計(jì)算能力并通過視景仿真直觀地展示移動(dòng)式回收過程、檢驗(yàn)控制算法是否合理。

本文介紹了欠驅(qū)動(dòng)AUV移動(dòng)式回收任務(wù)的流程,設(shè)計(jì)了一種新的前向速度導(dǎo)引律,并采用基于模型的軌跡跟蹤方法來控制欠驅(qū)動(dòng)AUV自主完成移動(dòng)式回收任務(wù)。另外,為了直觀展現(xiàn)移動(dòng)式回收裝置與欠驅(qū)動(dòng)AUV的移動(dòng)式回收過程,本文搭建了半物理仿真系統(tǒng)并利用視景仿真軟件Vega實(shí)時(shí)顯示移動(dòng)式回收?qǐng)鼍啊?/p>

1 任務(wù)流程和規(guī)劃系統(tǒng)

結(jié)合圖1說明欠驅(qū)動(dòng)AUV完成移動(dòng)式回收任務(wù)的流程。移動(dòng)式回收任務(wù)較為復(fù)雜,本文令回收裝置隨載體作相對(duì)簡(jiǎn)單的勻速直線運(yùn)動(dòng),速度為Ud,AUV上攜帶超短基線聲學(xué)定位系統(tǒng)和攝像頭,通過聲學(xué)定位和光視覺定位可得到AUV和回收裝置之間的相對(duì)位置關(guān)系,若此時(shí)相對(duì)位置關(guān)系滿足回收成功條件,AUV被固定在回收裝置中隨載體一起運(yùn)動(dòng),那么此時(shí)AUV可斷電停止;若未完成回收任務(wù),則可結(jié)合船位推算單元計(jì)算回收裝置的運(yùn)動(dòng)方向;然后通過規(guī)劃制導(dǎo)系統(tǒng)[11]得到下一時(shí)刻的期望位置;最后通過控制器和操縱執(zhí)行機(jī)構(gòu)完成對(duì)AUV的自動(dòng)控制。

圖1 移動(dòng)式回收任務(wù)流程

2 軌跡跟蹤控制

2.1 建立軌跡跟蹤誤差方程

六自由度AUV建?？蓞⒖嘉墨I(xiàn)[11],由于通過推進(jìn)器、水平舵和垂直舵控制的欠驅(qū)動(dòng)AUV不具備控制橫滾角的能力,所以軌跡跟蹤控制方法中將橫滾角相關(guān)項(xiàng)視作模型不確定影響,利用五自由度模型推導(dǎo)控制輸入并對(duì)六自由度AUV進(jìn)行控制,其五自由度運(yùn)動(dòng)學(xué)模型為:

(1)

式中:x、y和z代表大地坐標(biāo)系下的慣性坐標(biāo);θ和ψ是俯仰角和艏向角;u、v、w代表艇體坐標(biāo)系下前向、側(cè)向和垂向的速度分量;q和r代表俯仰角速度和轉(zhuǎn)艏角速度。

動(dòng)力學(xué)模型:

(2)

|(v2+w2)1/2||r|+Yrur+Y0u2+Yvuv+

(3)

|(v2+w2)1/2||q|+Z0u2+Zwuw+

Zw|w|w|(v2+w2)1/2|+Zww|w(v2+w2)1/2|+

(4)

|(v2+w2)1/2|q+Mwuw+Mw|w|w|(v2+w2)1/2|+

M|w|u|w|+Mww|w(v2+w2)1/2|+M0u2+Mvvv2-

(5)

N|v|r|(v2+w2)1/2|r+N0u2+Nvuv+

Nv|v|v|(v2+w2)1/2|+Nvwvw+Nu2δru2δr)×

(6)

其中:

(7)

(8)

(9)

式中:m是AUV的質(zhì)量;Ix、Iy和Iz是慣性矩;Xqq、Xrr、Xvr等均為水動(dòng)力參數(shù);推進(jìn)力τu,水平舵角δs和垂直舵角δr是可提供的控制輸入。

軌跡跟蹤誤差定義為[10]：

ex=(x-xd)cosγd+(y-yd)sinγd

(10)

ey=-(x-xd)sinγd+(y-yd)cosγd

(11)

ez=z-zd

(12)

對(duì)上述軌跡跟蹤誤差進(jìn)行微分可得:

(13)

(14)

(15)

2.2 運(yùn)動(dòng)學(xué)控制器設(shè)計(jì)

新導(dǎo)引律設(shè)計(jì)為:

(Δcos(θ-α))

(16)

θd=λ+α

(17)

ψd=γd-φ-χ

(18)

定理1如果能保證u、ψ、θ收斂到他們的期望值附近,那么使用上述導(dǎo)引律 (16)～(18), 可保證位置跟蹤誤差能夠收斂到0附近的一個(gè)閉集中。

證明定義:

ue=u-ud

(19)

ψe=ψ-ψd

(20)

θe=θ-θd

(21)

將式 (16)～(21) 代入式 (13)～ (15) 中可得:

(22)

(23)

(24)

式中:

Γx=cosχ(cosψe-1)/ψe+sinχsinψe/ψe

(25)

Γy=cosχsinψe/ψe-sinχ(cosψe-1)/ψe

(26)

Γz=cosλsinθe/θe+sinλ(cosθe-1)/θe

(27)

因?yàn)椴坏仁?|sinx/x|≤1和|(cosx-1)/x|<0.73對(duì)于任意x都成立, 所以Γx、Γy和Γz的上界為1.73。

(28)

(29)

式中k1是正常數(shù)。

將式 (28)～(29) 代入式 (22), 可得

(30)

選擇李雅普諾夫函數(shù):

V=(ex2+ey2+ez2)/2

(31)

結(jié)合式(23) ～(24)和(30)可求其時(shí)間微分為:

(32)

定義:

(33)

(34)

(35)

則式 (32) 變?yōu)?

‖E‖‖N‖=-‖E‖(‖E‖‖M‖-‖N‖)

(36)

因此,在有限的時(shí)間內(nèi)跟蹤誤差向量E的范數(shù)有界:

‖E‖≤‖N‖/‖M‖≤λmax(N)/λmin(M)

(37)

式中λmin為M的最小特征值;λmax為N的最大特征值。另外, 當(dāng)u、ψ、θ收斂到他們的期望值附近時(shí),參數(shù)可以被選擇可保證位置跟蹤誤差能夠收斂到0附近的一個(gè)閉集中。

證畢。

2.3 動(dòng)力學(xué)控制器設(shè)計(jì)

動(dòng)力學(xué)控制器設(shè)計(jì)分為2部分,即速度控制器和姿態(tài)控制器。

1)速度控制器設(shè)計(jì)。

為保證速度跟蹤誤差ue穩(wěn)定, 選擇李雅普諾夫函數(shù):

(38)

結(jié)合式 (19) 對(duì)其微分:

(39)

如果選擇控制輸入τu為:

(40)

式中k3為正常數(shù)。

則式 (39) 變?yōu)?

(41)

2)姿態(tài)控制器設(shè)計(jì)。

為保證速度跟蹤誤差ψe和θe穩(wěn)定,選擇李雅普諾夫函數(shù):

(42)

結(jié)合式 (20)～(21) 對(duì)其微分:

(43)

(44)

(45)

式中k4和k5為正常數(shù)。

因?yàn)樘摂M變量rd和qd不是真實(shí)控制輸入,定義誤差變量為:

re=r-rd

(46)

qe=q-qd

(47)

將式 (44)～(47) 代入式 (37) 可得:

(48)

為使誤差變量re和qe穩(wěn)定, 選擇如下李雅普諾夫函數(shù):

V3=V2+(re2+qe2)/2

(49)

結(jié)合式 (46)～(47) 對(duì)其微分可得:

(50)

如果選擇控制輸入δr和δs為:

(51)

(52)

式中k6和k7為正常數(shù)。

則式 (50)變?yōu)?

(53)

根據(jù)李雅普諾夫穩(wěn)定性理論可知系統(tǒng)全局一致穩(wěn)定。

3 半物理仿真系統(tǒng)

如圖2所示,半物理仿真系統(tǒng)由規(guī)劃控制計(jì)算機(jī)、傳輸計(jì)算機(jī)、仿真計(jì)算機(jī)和以太網(wǎng)組成[12]。

圖2 半物理仿真系統(tǒng)

圖3 規(guī)劃控制界面

規(guī)劃控制計(jì)算機(jī)承擔(dān)最大的計(jì)算任務(wù),負(fù)責(zé)計(jì)算移動(dòng)式回收裝置的位姿信息,通過TCP網(wǎng)絡(luò)與傳輸計(jì)算機(jī)通信,接收來自仿真計(jì)算機(jī)的AUV位姿信息,并據(jù)此完成規(guī)劃控制計(jì)算,最后將控制指令通過TCP網(wǎng)絡(luò)通信和傳輸計(jì)算機(jī)傳遞給仿真計(jì)算機(jī)。

仿真計(jì)算機(jī)由欠驅(qū)動(dòng)AUV的操縱性仿真模塊和基于Vega的視景仿真模塊組成,可以模擬欠驅(qū)動(dòng)AUV六自由度運(yùn)動(dòng)并將移動(dòng)式回收裝置和AUV的位姿實(shí)時(shí)展示在建立的海洋環(huán)境中。仿真計(jì)算機(jī)中的軟件程序均是在MFC框架下編寫,可通過TCP網(wǎng)絡(luò)與傳輸計(jì)算機(jī)通信,接收傳輸計(jì)算機(jī)發(fā)送過來的回收裝置位姿、推進(jìn)器推力和舵角指令,然后基于欠驅(qū)動(dòng)AUV六自由度運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)方程內(nèi)部解算出AUV當(dāng)前的位置和姿態(tài),反饋給規(guī)劃控制計(jì)算機(jī),使得規(guī)劃控制計(jì)算機(jī)根據(jù)獲得的狀態(tài)信息完成下一步的解算。同時(shí)還需要將當(dāng)前時(shí)刻的AUV位姿信息和回收裝置位姿信息通過Vega視景模塊不斷刷新顯示。

傳輸計(jì)算機(jī)是半物理仿真系統(tǒng)中的實(shí)物部分,為集成了奔騰Ⅲ處理器的PC104工作板,其上安裝了實(shí)時(shí)嵌入式操作系統(tǒng)Vxworks5.5,可通過裝有Tornado2.2軟件的宿主機(jī)進(jìn)行交叉編譯,并將生成的可執(zhí)行文件下載到傳輸計(jì)算機(jī)處理器中。傳輸計(jì)算機(jī)主要用于水下航行器的信息中轉(zhuǎn),實(shí)際應(yīng)用中此處理器可采集各傳感器數(shù)據(jù)供規(guī)劃控制計(jì)算機(jī)使用和向下發(fā)送控制指令,且存有最基本的控制程序,當(dāng)規(guī)劃控制計(jì)算機(jī)失靈時(shí)傳輸計(jì)算機(jī)能保證AUV浮出水面等待救援。

3.1 規(guī)劃控制部分

本部分的用戶界面由Matlab軟件設(shè)計(jì),如圖所示,可設(shè)置的參數(shù)為AUV的初始位置、姿態(tài)、速度和移動(dòng)式回收任務(wù)執(zhí)行的總時(shí)間,任務(wù)結(jié)束時(shí)可選擇輸出基于回轉(zhuǎn)圓的軌跡規(guī)劃結(jié)果和AUV與移動(dòng)式回收裝置的實(shí)際軌跡曲線。當(dāng)鼠標(biāo)點(diǎn)擊“開始”按鈕,程序開始執(zhí)行,先創(chuàng)建全局變量、讀入用戶界面的位姿、速度與時(shí)間設(shè)置;再利用了定時(shí)器timer函數(shù)以固定時(shí)間間隔進(jìn)行計(jì)算;使用tcpip函數(shù)與傳輸計(jì)算機(jī)建立通信并使用fscanf函數(shù)和fwrite函數(shù)進(jìn)行數(shù)據(jù)讀寫;另外采用自編寫的規(guī)劃函數(shù)planning.m和控制函數(shù)controller.m函數(shù)生成規(guī)劃指令和控制指令。

3.2 實(shí)物傳輸部分

PC104工作板搭載了嵌入式實(shí)時(shí)操作系統(tǒng)Vxworks5.5,共包含3個(gè)任務(wù),即主循環(huán)任務(wù)、與規(guī)劃控制計(jì)算機(jī)的通信任務(wù)和與仿真計(jì)算機(jī)的通信任務(wù)。主循環(huán)任務(wù)使用了看門狗定時(shí)器和信號(hào)量實(shí)現(xiàn)固定時(shí)間間隔進(jìn)行計(jì)算;與規(guī)劃控制計(jì)算機(jī)的通信任務(wù)中傳輸計(jì)算機(jī)作為服務(wù)器,規(guī)劃控制計(jì)算機(jī)作為客戶端;與仿真計(jì)算機(jī)的通信任務(wù)中傳輸計(jì)算機(jī)作為客戶端,仿真計(jì)算機(jī)作為服務(wù)器。

3.3 運(yùn)動(dòng)仿真部分

本文采用實(shí)驗(yàn)室在MFC框架下編寫的欠驅(qū)動(dòng)水下航行器操縱性仿真軟件[13],軟件輸入為推進(jìn)器推力、垂直舵角和水平舵角,模塊輸出為水下航行器的位姿及速度,六自由度運(yùn)動(dòng)仿真流程如圖4所示。

圖4 運(yùn)動(dòng)仿真流程

3.4 視景仿真部分

為了直觀展現(xiàn)移動(dòng)式回收裝置與欠驅(qū)動(dòng)AUV的水下回收過程,首先進(jìn)行仿真環(huán)境建模,包括海底地形、自主水下航行器、母艇與回收裝置,另外在回收裝置上加了5個(gè)圓球用來模擬光源[14-15]。需要特別注意的是為了突出展現(xiàn)回收裝置以及AUV終端對(duì)準(zhǔn)的情況,在顯示時(shí)利用Vega下的圖形化工具Lynx對(duì)母艇進(jìn)行了縮小操作。在海底地形、水下航行器等視景仿真模型建立完成后,通過Lynx圖形界面將模型導(dǎo)入并進(jìn)行三維場(chǎng)景渲染,借助Lynx圖形界面對(duì)三維場(chǎng)景進(jìn)行觀察和修改,并為水下航行器和母艇添加player。

圖5為欠驅(qū)動(dòng)AUV移動(dòng)式回收的視景仿真場(chǎng)景,借此可以直觀且清晰地觀察整個(gè)移動(dòng)式回收過程。為了更好展示整個(gè)回收過程,還設(shè)置多個(gè)觀察者視角,包括俯視、側(cè)視、鳥瞰以及2個(gè)模擬攝像機(jī)的觀察者,然后設(shè)置了相應(yīng)的通道,這樣就可以在同一個(gè)畫面中展現(xiàn)出不同的觀察者視角。圖中間部分為鳥瞰視角,一直處于AUV的后上方,且隨AUV移動(dòng)。側(cè)視視角和俯視視角處于圖像左側(cè),與母艇固定且隨母艇移動(dòng)。當(dāng)AUV距離回收裝置較遠(yuǎn)時(shí),主要通過鳥瞰視角觀察AUV運(yùn)動(dòng)情況,當(dāng)AUV距離回收裝置較近時(shí)則可以搭配側(cè)視和俯視視角觀察AUV與移動(dòng)式回收裝置的相對(duì)位置關(guān)系。右上角的2個(gè)畫面用來模擬AUV前端攜帶的攝像機(jī)的拍攝圖像,拍攝到的光點(diǎn)可用于光視覺定位。

圖5 移動(dòng)式回收任務(wù)仿真場(chǎng)景

最后,除了借助Lynx圖形界面進(jìn)行基本設(shè)置以外,還需要通過操縱性仿真軟件與Vega模塊聯(lián)合驅(qū)動(dòng)整個(gè)視景場(chǎng)景的運(yùn)行?；赩ega的視景仿真模塊每個(gè)時(shí)間步都會(huì)接收來自操縱性仿真軟件的AUV和回收裝置的位姿信息,通過vgPosVec函數(shù)和vgPos函數(shù)對(duì)AUV和回收裝置的player進(jìn)行重新賦值,并且更新圖5中的文字信息,文字信息的內(nèi)容是回收裝置的位置、艏向和AUV的位姿信息。

4 仿真研究

首先對(duì)采用不同期望前向速度導(dǎo)引律的軌跡跟蹤控制方法進(jìn)行仿真與比較,文獻(xiàn)[10]中采用的期望前向速度導(dǎo)引律(圖中標(biāo)注為控制器1)為:

(54)

軌跡跟蹤控制方法對(duì)比仿真中采用的期望軌跡方程為:

(55)

仿真結(jié)果如圖6所示,2個(gè)控制器的艏向控制都是正確的,但從圖7可以看出,由控制器1控制的AUV的相對(duì)位置跟蹤誤差卻越來越大,即當(dāng)AUV的初始位置位于期望位置前方時(shí),由圖8可知方程(54)的結(jié)果明顯不合理。

圖6 軌跡跟蹤控制方法對(duì)比

圖7 相對(duì)位置誤差

圖8 期望前向速度和實(shí)際前向速度

然后進(jìn)行移動(dòng)式回收任務(wù)的半物理仿真實(shí)驗(yàn),回收裝置隨載體作相對(duì)簡(jiǎn)單的勻速直線運(yùn)動(dòng),假設(shè)它的起點(diǎn)為(0,0,-50),運(yùn)動(dòng)方向?yàn)閄軸正向,速度為Ud=1.0 m/s,AUV最大規(guī)劃速度為ur=1.5 m/s,回收成功條件為縱向垂向距離差小于1 m,艏向角和俯仰角度差小于30°。規(guī)劃導(dǎo)引系統(tǒng)可參考文獻(xiàn)[11], 基于回轉(zhuǎn)圓的軌跡規(guī)劃方法以仿真實(shí)驗(yàn)或外場(chǎng)試驗(yàn)測(cè)得的AUV最小回轉(zhuǎn)半徑、最小回轉(zhuǎn)周期和最大航行速度作為AUV的運(yùn)動(dòng)約束,利用幾何知識(shí)將欠驅(qū)動(dòng)AUV移動(dòng)式對(duì)接任務(wù)的歸航軌跡規(guī)劃為直線與回轉(zhuǎn)圓曲線的組合,采用多個(gè)時(shí)間步逼近的形式實(shí)時(shí)預(yù)測(cè)欠驅(qū)動(dòng)AUV移動(dòng)式對(duì)接任務(wù)的最終對(duì)接位置和姿態(tài)并生成AUV的期望跟蹤軌跡。仿真實(shí)驗(yàn)表明相比于基于最優(yōu)控制理論的軌跡規(guī)劃方法,該方法計(jì)算簡(jiǎn)單、能夠滿足實(shí)時(shí)性要求且規(guī)劃結(jié)果更加穩(wěn)定;相比于固定式對(duì)接任務(wù)中的點(diǎn)對(duì)點(diǎn)制導(dǎo)方法,該方法能夠引導(dǎo)欠驅(qū)動(dòng)AUV更快地進(jìn)入作直線運(yùn)動(dòng)的對(duì)接裝置中。規(guī)劃導(dǎo)引系統(tǒng)在深度方向采用視線法導(dǎo)引律,在水平面上采用純跟蹤導(dǎo)引律、基于回轉(zhuǎn)圓的軌跡規(guī)劃方法和視線法導(dǎo)引律相結(jié)合的方法,由于純跟蹤導(dǎo)引律和視線法導(dǎo)引律的計(jì)算結(jié)果隨AUV和回收裝置位置的變化而變化,所以仿真圖中只繪制了由基于回轉(zhuǎn)圓的軌跡規(guī)劃方法規(guī)劃出的軌跡。其參數(shù)選為d1=100 m,d2=80 m,d3=8 m,R=13.3 m,T=55.8 s,控制器中的參數(shù)選為k1=1,k2=0.06,k3=2,k4=2,k5=3,k6=5,k7=4,Δ=8,下面通過3組仿真對(duì)AUV移動(dòng)式回收過程進(jìn)行說明。

第1組仿真:AUV的起點(diǎn)為(-50,100,-30),艏向角為0。如圖9所示,AUV接近移動(dòng)式回收裝置的軌跡分為2部分,第1部分是AUV沿基于回轉(zhuǎn)圓的軌跡規(guī)劃方法規(guī)劃出的一段直線移動(dòng);第2部分為當(dāng)AUV進(jìn)入回收裝置正后方矩形區(qū)域時(shí)由視線法導(dǎo)引從回收裝置后方追趕并完成回收任務(wù)。

圖9 第1組仿真規(guī)劃的軌跡與實(shí)際軌跡

第2組仿真:AUV的起點(diǎn)為(200,200,-30),艏向角為0。如圖10所示,AUV接近移動(dòng)式回收裝置的軌跡分為3部分,第1部分是AUV由純跟蹤制導(dǎo)方法導(dǎo)引先做回轉(zhuǎn)運(yùn)動(dòng)使其前進(jìn)方向朝向移動(dòng)式回收裝置,再沿他們間的連線靠近移動(dòng)式回收裝置;第2部分是AUV沿基于回轉(zhuǎn)圓的軌跡規(guī)劃方法規(guī)劃出的2段圓弧及一段直線移動(dòng);第3部分為當(dāng)AUV進(jìn)入回收裝置正后方矩形區(qū)域時(shí)由視線法導(dǎo)引從回收裝置后方追趕并完成回收任務(wù)。

圖10 第2組仿真規(guī)劃的軌跡與實(shí)際軌跡

第3組仿真:AUV的起點(diǎn)為(200,0,-30),艏向角為0。如圖11所示,AUV接近移動(dòng)式回收裝置的軌跡分為3部分,第1部分是AUV由純跟蹤制導(dǎo)方法導(dǎo)引先做回轉(zhuǎn)運(yùn)動(dòng)使其前進(jìn)方向朝向移動(dòng)式回收裝置,再沿他們間的連線靠近移動(dòng)式回收裝置;第2部分是AUV沿基于回轉(zhuǎn)圓的軌跡規(guī)劃方法規(guī)劃出的2段圓弧移動(dòng);第3部分為當(dāng)AUV進(jìn)入回收裝置正后方矩形區(qū)域時(shí)由視線法導(dǎo)引從回收裝置后方追趕并完成回收任務(wù)。

由上述3組仿真實(shí)驗(yàn)可以看出欠驅(qū)動(dòng)AUV由不同位置出發(fā)均能順利完成移動(dòng)式回收任務(wù)。本仿真系統(tǒng)能夠驗(yàn)證AUV移動(dòng)式對(duì)接規(guī)劃控制方法,便于直觀展示移動(dòng)式回收過程,并能夠與工控機(jī)通信驗(yàn)證嵌入式程序是否合理,可有效地節(jié)約外場(chǎng)試驗(yàn)時(shí)間。

5 結(jié)論

1)設(shè)計(jì)了一種新的前向速度導(dǎo)引律,并采用基于模型的軌跡跟蹤方法來控制欠驅(qū)動(dòng)AUV,并完成了AUV由不同位置出發(fā)的移動(dòng)式回收仿真實(shí)驗(yàn)。

2)搭建了半物理仿真系統(tǒng)及視景仿真系統(tǒng)實(shí)時(shí)顯示欠驅(qū)動(dòng)AUV水下自主對(duì)接回收的過程,驗(yàn)證了所用方法的可行性和實(shí)時(shí)性。

本文只涉及到了移動(dòng)式回收裝置隨載體做勻速直線運(yùn)動(dòng)的情況,接下來會(huì)繼續(xù)研究移動(dòng)式回收裝置隨載體做勻速圓周運(yùn)動(dòng)或跟蹤矩形軌跡的情況。