無人艇運動控制仿真試驗訓練系統設計研究

劉 峰,許二旭

(1. 中國人民解放軍92941部隊43分隊,遼寧 葫蘆島 125000;2. 中國人民解放軍91976部隊,廣東 廣州 510080)

無人裝備模型構建和運動控制仿真技術是近年來國內外軍工靶場的研究重點,也是無人裝備數字化鑒定及模擬訓練的核心關鍵,其鑒定考核具有標準依據少、方法設計難等特點,傳統試訓方法不能直接觀察到控制系統各參數變化對運動穩定性的影響,試訓人員對控制參數的調試理解不深,使得訓練效果不佳,因此,通過模型構建及仿真技術來驗證無人裝備運動控制參數和性能,對其融入作戰體系及能力生成具有重要意義[1]。本文基于CoppliaSim仿真軟件設計無人艇運動控制仿真試驗訓練系統,基于構建的無人艇動力學模型和運動學模型,設計構建穩健比例積分(PID)控制、滑膜控制和模糊PID控制三種控制器,仿真系統與實裝艇通過通信系統連接,試訓人員能夠通過仿真系統調節參數,結合實船演示和仿真可視化的方法驗證不同控制器下的軌跡變化特性,為無人艇試驗訓練提供手段方法和基礎支撐[2]。

設計過程包括CoppeliaSim中的仿真運動過程和軟件界面設計整合過程。CoppeliaSim中的仿真過程包括路徑規劃、控制器設計、運動學仿真三大部分,仿真全過程的邏輯框圖如圖1所示。在實現運動仿真后,軟件界面對系統功能進行整合,以運動模式、行程點和控制參數為輸入量,經運動學仿真后,將運動狀態輸出顯示并進行運動渲染,其具體邏輯框圖如圖2所示。

圖1 仿真過程邏輯圖Fig.1 Logic diagram of simulation process

圖2 軟件邏輯框架Fig.2 Software logic framework

1 無人艇實體模型及環境搭建

為仿真試驗提供可視化效果,需要構建相應的無人艇實體模型和水面環境模型[3]。

1.1 無人艇實體模型

考慮仿真平臺運行時的性能和無人艇的運動特性,本系統采用CoppliaSim仿真軟件中的簡單立方體代替無人艇實體模型進行仿真。螺旋槳也用兩個簡單的立方體代替,并用力傳感器將替代螺旋槳的立方體與無人艇體進行連接,虛擬無人艇組成部分從屬關系圖如圖3所示。

圖3 虛擬無人艇組成部分從屬關系圖Fig.3 Dependency diagram of virtual unmanned surface vehicle components

為了畫面效果,依據試驗用實物艇建立無人艇的實體建模,將模型放置在顯示的圖層,而將仿真的實體放在隱藏的圖層,如圖4所示。

圖4 導入完成的全部模型顯示Fig.4 Shows of imported model

1.2 環境模型搭建

CoppliaSim軟件提供的模型不具備如水浮力、水阻力等物理特征,而本系統關注的是無人艇的水面運動狀態,即我們是基于三自由度無人艇模型進行后續設計工作,因此,對模型進行簡化,將無人艇實體放在一個支撐大平面上進行仿真。水阻力以及風浪等干擾可以通過在調用軟件中添加力和力矩的函數進行仿真。

2 動力學模型構建

2.1 無人艇坐標系

無人艇在水面上航行時,可將運動行為看成在六個自由度上的運動。運動行為分為線性運動和旋轉運動。線性運動為沿軸x0、y0、z0方向的運動,旋轉運動則是沿這三軸的往復運動。為了更加詳細地描述無人艇的運動狀態和位置,我們建立了描述無人艇位置的慣性坐標系和描述其運動姿態的機體坐標系,如圖5所示。

圖5 坐標系構建Fig.5 Construction of coordinate system

慣性坐標系也稱為大地坐標系,其原點選為地球表面某個運動參考點,通常情況下為無人艇某個時刻的重心。o0x0軸的正方向指向正北方,o0y0軸的正方向指向正東方,o0z0軸的正方向垂直向下指向地心。機體坐標系也稱為運動坐標系,用于描述無人艇的運動信息[4]。o為無人艇的重心,該坐標系會跟隨艇一起進行移動。規定:ox軸正方向指向艇首,oy軸正方向指向艇體的右舷,oz軸正方向垂直向下。

無人艇狀態變量定義為η1=[x,y,z],表示無人艇在慣性坐標系的位置,η2=[φ,θ,ψ]表示無人艇在慣性坐標系中的姿態;v1=[u,v,w]為無人艇在運動坐標系中的線速度,分別對應了縱蕩、橫蕩和垂蕩速度,v2=[p,q,r]則是代表在同一坐標系中的角速度,分別對應了橫搖、縱搖和艏搖角速度;τ1=[X,Y,Z]代表了無人艇在運動坐標系下所受到的各方向外力,τ2=[K,M,N]則代表在同一坐標系下的各方向力矩。綜上,定義η=[η1,η2]為無人艇的位置向量,v=[v1,v2]為速度向量,τ=[τ1,τ2]為其所受外力/力矩[5]。

2.2 無人艇動力學模型

無人艇線速度在兩坐標系之間轉換關系為

(1)

其中,J1(η2)為轉換矩陣,表示為

(2)

無人艇角速度在兩坐標系之間轉換關系為

(3)

其中,J2(η2)為轉換矩陣,表示為

(4)

結合式(1)和(3)可得無人艇水面運動模型:

(5)

式(5)可簡化為

(6)

將無人艇視為剛體,由牛頓-歐拉方程推導出的剛體力和力矩平衡方程為

(7)

其中,rg=[xg,yg,zg]為重心坐標,I0為剛體轉動慣量。

將式(7)改寫為

(8)

式中,v=[u,v,w,p,q,r],τRB=[X,Y,Z,K,M,N]。

MRB為剛體慣性矩陣:

(9)

CRB(v)是剛體科里奧利向心力矩陣,令CRB(v)=[C1,C2],則:

(10)

向量τRB由流體力和力矩向量τH、外部環境干擾力和力矩向量τE、推進力和力矩向量τ組成。下面重點介紹τH的組成。

由流體力學可知,作用于剛體上的流體動力和力矩是線性疊加的[6]。流體動力和力矩由附加質量、水動力阻尼和重力與浮力引起的回復力三部分組成,因此,τH可以寫成

(11)

MA是附加質量矩陣。

(12)

CA(v)是流體力學科里奧利向心力矩陣。

(13)

其中,

(14)

D(v)是阻尼矩陣,可寫為線性和非線性阻尼矩陣的和。

D(v)=D+Dn(v)

(15)

其中,線性阻尼矩陣D為

(16)

g(η)是回復力和力矩矩陣向量。

g(η)=

(17)

其中,W為重力,B為浮力。

W=mg,B=ρgρ′

(18)

式中,m為無人艇質量,g為重力加速度,ρ為水密度,ρ′為排水密度。

2.3 三自由度無人艇運動模型

在無人艇的實際航行過程中,主要關注運動坐標系縱向運動、橫向運動以及轉艏運動(水平面上的運動),因此,可以將六自由度無人艇運動數學模型簡化為三自由度的平面運動數學模型,使運動控制問題得到簡化,水平坐標系如圖6所示。

圖6 水平坐標系Fig.6 Horizontal coordinate system

假設無人艇的幾何中心與重心重合,同時忽略風、浪和流等外界干擾因素,則可以得到無人艇水面三自由度運動數學模型:

(19)

其中,位置矢量η=[x,y,ψ],速度矢量v=[u,v,r],而仿真對象是雙槳無人艇,則其所受推力和力矩矢量τ=[T1+T2,0,(T1-T2)B/2]。T1和T2分別是無人艇左右槳提供的推力,B是槳間的水平距離。

對于螺旋槳提供的推力,用以下數學模型進行描述:

(20)

式中,ρ表示水密度;(JP)為槳推力系數,其中,JP為進速系數;tP為伴流系數;DP表示螺旋槳直徑;n是螺旋槳轉速。

轉換矩陣J(η)可表示為

(21)

慣性矩陣M=MRB+MA,科里奧利力矩陣C(v)=CRB(v)+CA(v),阻尼矩陣D(v)=D+Dn(v)。其中:

(22)

(23)

(24)

CA(v)=

(25)

(26)

(27)

(28)

結合式(24)和(25)可得C(v)的具體表達形式:

(29)

阻尼矩陣包含線性阻尼矩陣D以及非線性阻尼矩陣Dn(v),忽略高于二階的流體動力阻尼項,則非線性阻尼矩陣中各項系數均為零,因此:

(30)

綜上,將式(19)展開為

(31)

無人艇基本參數及無人艇水動力模型參數如表1、表2所示。

表1 無人艇基本參數Tab.1 Basic parameters of unmanned surface vehicle

表2 無人艇水動力模型參數Tab.2 Hydrodynamic model parameters of unmanned surface vehicle

3 控制器構建

無人水面艇在海上航行,由于受到風、浪、流等環境的影響,給定的航向會不可避免地產生偏移。精準的航向控制是解決航跡跟蹤、自主航行、自動避碰等問題的前提。相對民船而言,無人艇的重量小,速度快,動力有限,風、浪、流等海洋環境的干擾顯得非常突出,其控制要求更高。因此,必須研究出更加先進的無人艇運動控制技術來代替傳統的控制方法,以滿足控制要求[7]。無人艇的運動控制器由航向控制器、航跡控制器和速度控制器三個控制器組成,為了讓試訓人員更直觀地體驗控制器的設計,我們分別采用了PID控制器、滑?？刂破骱湍：齈ID控制器來進行無人艇的仿真航向控制,并預留可供修改的控制器參數來體驗控制器的特性,控制邏輯如圖7所示。

圖7 無人艇控制邏輯Fig.7 Control logic of unmanned boat

我們使用的無人艇是一種雙推進器無人艇。對于該艇,航跡控制器的輸入為無人艇與目標航跡的距離偏差(e),輸出為預期航向角(ψd);航向控制器的輸入為航向誤差(Δψ),輸出為推進器轉速差值;速度控制器輸入為速度誤差(ΔV),輸出為推進器轉速均值。航向控制器起到了調整航向的作用,在目標航向已知的情況下向目標航向靠攏;航跡控制器在航向控制器表現較好時才能發揮更好的作用,其會根據航行實際位置與規劃航跡綜合分析,對無人艇航行航向進行直接決策,扮演指令下達者的角色;速度控制器用來控制無人水面艇航速,在不同水面情況下會有不同航速要求,速度控制器負責達到預期的航速目標。

設計三種不同的控制器,分別是PID控制器、滑?？刂破骱汪敯艨刂破?模糊PID控制器)。三種控制器各有優劣。其中,PID控制器和模糊PID控制器在存在未知干擾的情況下會有更好的控制效果,具有更高的魯棒性能。而滑?？刂破鲗δＰ途珳识纫蟾?但在模型確定的情況下控制精度更好。下面是我們用三個控制器進行航向角控制,結果如圖8～圖10所示。

圖8 PID航向角控制結果Fig.8 PID heading angle control results

圖9 滑模航向角控制結果Fig.9 Sliding mode heading angle control results

圖10 模糊PID航向角控制結果Fig.10 Fuzzy PID heading angle control results

由仿真結果可知,三個控制器都可以達到較好的結果,三個控制器對航向角階躍信號的響應時間和控制精度分別為:

PID控制器響應時間:21.65 s,收斂后的波動范圍是(179.041 3,180.424 5);

滑膜控制器響應時間:26.35 s,收斂后的波動范圍是(169.521 2,170.752 5);

模糊PID控制器響應時間是21.55 s,收斂后的波動范圍是(175.114 2,175.658 7)。

三個控制器中,PID控制器最為簡單,具有一定的魯棒性;模糊PID魯棒性最高,有自適應能力;滑膜控制器對模型的精度要求最高,參數調整較為困難,若相關參數調整不合理,容易出現抖震現象。

4 Navmesh尋路算法

Navmesh (Navigation Mesh)尋路算法是目前比較主流的一種尋路算法,是一種基于凸多邊形網格的尋路方式[8]。Navmesh是對基于導航網格尋路體系的統稱,實現方式復雜多樣。但無論具體使用怎樣的方法,Navmesh尋路算法的全部流程至少包含兩個部分:導航網格構建和尋路算法。本平臺使用的Navmesh尋路算法原理如圖11所示。

圖11 Navmesh尋路算法實現原理Fig.11 Navmesh pathfinding algorithm implementation principle

4.1 導航網格構建

導航網格構建(Navigation Mesh Construction)是指根據得到的地圖信息中障礙物和陸地的位置,在地圖上靜態地構建可供尋路算法使用的導航網格的過程。導航網格構建的基本流程如下:

體素化場景:將提供的源地圖數據通過體素化方式建立實體Span,與此同時,對體素化模型進行簡單過濾,以去除無人艇可能無法到達的位置;

創建Regions:將第一步生成的體素模型描述的可行走區域劃分為重疊的2D區域,檢測可跨越的Span,并為其生成簡單的輪廓數據;

創建Polygon:根據輪廓數據對區域進行化簡,獲得導航多邊形,再對得到的導航多邊形進行處理,得到凸多邊形。

4.2 尋路算法

尋路算法(Path Finding Algorithm)是指根據前面構建的導航網格體,迅速生成一條可供無人艇在自動駕駛模式下無碰撞行駛的合理路徑的過程。尋路算法的實現主要分為以下三個階段。

4.2.1 確定優化目標

為構建的凸多邊形網格體添加一個“成本”值,這樣在搜索路徑時,尋路算法會嘗試找到總成本最低的路徑。

4.2.2 確定凸多邊形集合

以經過導航網格的總“成本”值最小為目標,使用其他尋路算法可以獲得無人艇達到終點所需要經過的導航網格集合。這里所使用的尋路算法通常為A*算法[9]。A*算法是一種應用十分廣泛的尋路算法,不一定能找到最短的路徑,但可以在較短的時間內搜索到一條相對較短的路徑,在Dijkstra算法基礎上引入啟發因子,尋找那些“看起來離終點更近”的節點。A*算法的運行結果如圖12所示。

圖12 A*算法的運行結果*注:綠色網格為起點,紅色網格為終點,灰色網格為障礙物,黃色折線為搜索到的最短路徑。Fig.12 Operation results of the A* algorithm

4.3 算法仿真應用

通過使用Navmesh尋路算法,系統可以生成無人艇到達終點的合理路徑。本文設計的路徑規劃方法在仿真平臺中實現了以下目標:

根據較復雜的場景中的障礙物信息,構建導航網格體,并在短時間內迅速生成一條成本相對較低的路徑;

將得到的路徑轉化為保障無人艇不接觸障礙的路徑點,同時有利于提高控制器的控制效果;

根據路徑的整體情況以及船的轉彎、制動、加速性能得到速度矩陣,并根據路徑點的分布、位置、距離等參數進行速度矩陣的再次修正,最終傳輸給控制器進行每一段速度的控制,如圖13所示。

圖13 路徑的初步規劃Fig.13 Preliminary planning of the path

4.4 路徑生成優化

4.4.1 不同速度下的尋路優化

在實際運行過程中,隨著速度的增大,轉彎半徑也會增大,進而影響無人艇的路徑規劃[10]。為了能夠在仿真平臺中體現出這一特點,我們為障礙物的碰撞模型增添了一段安全距離,以確保有一定寬度的無人艇可以安全通過。設置安全距離后的效果如圖14所示。圖中,綠色方格是構建的導航網格體,而不規則的灰色環圍成的范圍則是設置的安全距離。由于轉彎半徑與速度有關,隨著速度的增大,其安全距離也應隨之增大,在尋路算法運行的時候,會把安全距離以內的區域認定為被阻擋的區域,進而保證船在高速狀態下也可選擇合適且安全的路徑。

圖14 障礙物實際碰撞范圍影響導航網格體Fig.14 Actual collision range of obstacles affects the navigation grid body

圖15、16是在搭建合適的場景后,無人艇行駛速度慢與快對路徑生成帶來的影響。在慢速情況下,無人艇的轉彎半徑較小,因此,尋路算法可以在障礙物比較密集的地方生成路徑;而在快速情況下,無人艇的轉彎半徑增大,為了行駛的安全以及保持較快的行駛速度,此時路徑軌跡的生成則會選擇在寬闊的海面上。

圖15 較低速度下的路徑規劃Fig.15 Path planning at lower speeds

4.4.2 減少船的轉彎

在實際運行過程中,頻繁的轉彎會影響無人艇的運行速度,同時可能會影響控制器的效果。從圖16可以看出,使用A*算法得到的路徑效果并不是真正意義上的最短路徑,這是由A*算法所使用的啟發式函數決定的,而之后使用的路徑平滑算法也無法減少轉彎的次數,如圖17所示。因此,需要對生成的路徑點進行額外的處理。下面將對處理的方式進行介紹:

圖16 較高速度下的路徑規劃Fig.16 Path planning at higher speeds

圖17 仿真平臺中減少轉彎次數的算法示意圖Fig.17 Schematic diagram of the algorithm for reducing the number of turns in the simulation platform

Step1:對于尋路算法得到的點集,如果點集中的元素個數不少于3個,則轉入Step2處理;否則不需要處理;

Step2:取出點集中的第一個點與第三個點,記為C1和C3,由C1向C3發射一條射線,如果射線沒有命中障礙物,說明無人艇可以通過直線直接到達C3,可以把二者間的點刪除,再取出點集中的第三個點進行檢測;如果命中障礙物,說明C1和C3間的點需要保留,此時取出C2和C4進行檢測;

Step3:重復上述過程,直到取出點集中的倒數第二個點為止。

在減少無人艇轉彎次數時,如果刪除過多點,可能會導致無人艇在某次轉彎過程中轉彎幅度過大,會對行駛造成不好的影響。因此,我們基于一定規則,對本該刪除的點進行必要的取舍,以保證在不降低無人艇轉彎平穩性的同時,減少無人艇轉彎次數。最后達到的路徑規劃效果如圖18所示。相對圖16,無人艇自動生成的路徑中轉彎次數出現了較明顯減少。

圖18 使用優化方法減少路徑中的轉彎次數Fig.18 Using optimization methods to reduce the number of turns in the path

5 結束語

本文設計的仿真試驗訓練系統,彌補了靶場無人艇仿真試驗鑒定領域不足,基本解決了無人艇模擬訓練手段缺乏的難題?；谠撓到y開展試驗訓練工作,可加深對無人裝備運動控制理論知識的理解掌握,有利于試訓人員對各種控制算法及數字化試驗鑒定方法的理解。同時,基于該系統,能夠對無人裝備運動控制試驗訓練方法進行創新設計,有助于提升海軍靶場試驗訓練綜合保障能力。