欠驅動自主水下航行器移動式回收控制及視景仿真

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

(1.哈爾濱工程大學 水下機器人技術重點實驗室, 黑龍江 哈爾濱 150001; 2.河南省水下智能裝備重點實驗室, 河南 鄭州 450015; 3.中國船舶航海保障技術實驗室, 天津 300131)

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

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

本文介紹了欠驅動AUV移動式回收任務的流程,設計了一種新的前向速度導引律,并采用基于模型的軌跡跟蹤方法來控制欠驅動AUV自主完成移動式回收任務。另外,為了直觀展現移動式回收裝置與欠驅動AUV的移動式回收過程,本文搭建了半物理仿真系統并利用視景仿真軟件Vega實時顯示移動式回收場景。

1 任務流程和規劃系統

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

圖1 移動式回收任務流程

2 軌跡跟蹤控制

2.1 建立軌跡跟蹤誤差方程

六自由度AUV建模可參考文獻[11],由于通過推進器、水平舵和垂直舵控制的欠驅動AUV不具備控制橫滾角的能力,所以軌跡跟蹤控制方法中將橫滾角相關項視作模型不確定影響,利用五自由度模型推導控制輸入并對六自由度AUV進行控制,其五自由度運動學模型為:

(1)

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

動力學模型:

(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的質量;Ix、Iy和Iz是慣性矩;Xqq、Xrr、Xvr等均為水動力參數;推進力τ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)

對上述軌跡跟蹤誤差進行微分可得:

(13)

(14)

(15)

2.2 運動學控制器設計

新導引律設計為:

(Δcos(θ-α))

(16)

θd=λ+α

(17)

ψd=γd-φ-χ

(18)

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

證明定義:

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)

因為不等式 |sinx/x|≤1和|(cosx-1)/x|<0.73對于任意x都成立, 所以Γx、Γy和Γz的上界為1.73。

(28)

(29)

式中k1是正常數。

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

(30)

選擇李雅普諾夫函數:

V=(ex2+ey2+ez2)/2

(31)

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

(32)

定義:

(33)

(34)

(35)

則式 (32) 變為:

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

(36)

因此,在有限的時間內跟蹤誤差向量E的范數有界:

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

(37)

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

證畢。

2.3 動力學控制器設計

動力學控制器設計分為2部分,即速度控制器和姿態控制器。

1)速度控制器設計。

為保證速度跟蹤誤差ue穩定, 選擇李雅普諾夫函數:

(38)

結合式 (19) 對其微分:

(39)

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

(40)

式中k3為正常數。

則式 (39) 變為:

(41)

2)姿態控制器設計。

為保證速度跟蹤誤差ψe和θe穩定,選擇李雅普諾夫函數:

(42)

結合式 (20)～(21) 對其微分:

(43)

(44)

(45)

式中k4和k5為正常數。

因為虛擬變量rd和qd不是真實控制輸入,定義誤差變量為:

re=r-rd

(46)

qe=q-qd

(47)

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

(48)

為使誤差變量re和qe穩定, 選擇如下李雅普諾夫函數:

V3=V2+(re2+qe2)/2

(49)

結合式 (46)～(47) 對其微分可得:

(50)

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

(51)

(52)

式中k6和k7為正常數。

則式 (50)變為:

(53)

根據李雅普諾夫穩定性理論可知系統全局一致穩定。

3 半物理仿真系統

如圖2所示,半物理仿真系統由規劃控制計算機、傳輸計算機、仿真計算機和以太網組成[12]。

圖2 半物理仿真系統

圖3 規劃控制界面

規劃控制計算機承擔最大的計算任務,負責計算移動式回收裝置的位姿信息,通過TCP網絡與傳輸計算機通信,接收來自仿真計算機的AUV位姿信息,并據此完成規劃控制計算,最后將控制指令通過TCP網絡通信和傳輸計算機傳遞給仿真計算機。

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

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

3.1 規劃控制部分

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

3.2 實物傳輸部分

PC104工作板搭載了嵌入式實時操作系統Vxworks5.5,共包含3個任務,即主循環任務、與規劃控制計算機的通信任務和與仿真計算機的通信任務。主循環任務使用了看門狗定時器和信號量實現固定時間間隔進行計算;與規劃控制計算機的通信任務中傳輸計算機作為服務器,規劃控制計算機作為客戶端;與仿真計算機的通信任務中傳輸計算機作為客戶端,仿真計算機作為服務器。

3.3 運動仿真部分

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

圖4 運動仿真流程

3.4 視景仿真部分

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

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

圖5 移動式回收任務仿真場景

最后,除了借助Lynx圖形界面進行基本設置以外,還需要通過操縱性仿真軟件與Vega模塊聯合驅動整個視景場景的運行。基于Vega的視景仿真模塊每個時間步都會接收來自操縱性仿真軟件的AUV和回收裝置的位姿信息,通過vgPosVec函數和vgPos函數對AUV和回收裝置的player進行重新賦值,并且更新圖5中的文字信息,文字信息的內容是回收裝置的位置、艏向和AUV的位姿信息。

4 仿真研究

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

(54)

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

(55)

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

圖6 軌跡跟蹤控制方法對比

圖7 相對位置誤差

圖8 期望前向速度和實際前向速度

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

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

圖9 第1組仿真規劃的軌跡與實際軌跡

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

圖10 第2組仿真規劃的軌跡與實際軌跡

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

由上述3組仿真實驗可以看出欠驅動AUV由不同位置出發均能順利完成移動式回收任務。本仿真系統能夠驗證AUV移動式對接規劃控制方法,便于直觀展示移動式回收過程,并能夠與工控機通信驗證嵌入式程序是否合理,可有效地節約外場試驗時間。

5 結論

1)設計了一種新的前向速度導引律,并采用基于模型的軌跡跟蹤方法來控制欠驅動AUV,并完成了AUV由不同位置出發的移動式回收仿真實驗。

2)搭建了半物理仿真系統及視景仿真系統實時顯示欠驅動AUV水下自主對接回收的過程,驗證了所用方法的可行性和實時性。

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