基于牛頓力學積分的水下滑翔機群協同控制研究

王貴凱，馬純永*,2，高占文，張勝勝，陳 戈,2

(1.中國海洋大學 信息科學與工程學院，山東 青島 266100；2.青島海洋科學與技術試點國家實驗室 區域海洋動力學與數值模擬功能實驗室，山東 青島 266237)

0 引言

Glider在海域中的觀測任務通常是多臺機器組成固定陣型進行集體觀測，PENG et al[8]在對南海北部海洋氣候的研究中明確提出對海洋的研究在未來將很大程度上受益于動態持續的glider觀測網絡。然而海洋流場會破壞glider運動的陣型，一個強的、可變的流場運動速度往往比glider的速度還要快[2]。由于海洋流場的影響，glider的陣型通常會偏離目標區域，達不到觀測的目的。而如何令glider在不穩定的海洋流場作用下穩定地保持陣型是海洋科學中一個令人興奮的挑戰。

對glider陣列保持的研究一直是海洋科學的熱點，李沛倫 等[9]針對水下滑翔機的運動特點，改進了傳統的人工勢場，并且引入速度勢場函數，結合glider本身的運動特性對glider進行路徑規劃，使glider在單個運動周期內的運動能夠有效躲避水中的障礙物；FIORELLI et al[10]運用勢函數調節glider之間的距離，并引入虛擬領導者引領glider陣型的整體運動；SEPULCHRE et al[11]和HERNANDEZ et al[12]提出一種閉環反饋控制率約束滑翔機的運動，后又將該算法推廣到三維空間； LAGOR et al[13]提出一種邊界巡回算法，通過對基于拉格朗日傳感器位置線性觀測參數的非高斯狀態估計來對滑翔機進行路徑規劃。前人對水下滑翔機陣列保持的研究多為建立glider間的數學模型，利用數學函數調節glider間的距離或是根據已知數據利用某種算法來規劃最佳路徑。然而在實際海試過程中海洋流場處于未知的狀態，并且海洋流場對glider運動的影響缺乏定量描述，具體的調節方法也與實際glider海上實驗過程不相符。為盡可能真實地還原glider在海洋中的運動狀態，本研究分析了glider在海洋中的實際受力情況，以基礎力學的公式推演glider的運動路線。

隨著現代計算機技術和圖像信息處理技術的發展，基于運動檢測的基礎力學實驗成為現代實驗中最具有吸引力的手段[14]。它具有其他實驗所沒有的簡潔性和有效性，同時可以實現實驗結果的可視化顯示。本研究將高分辨率Regional Ocean Modeling System(ROMS)海洋模式數據作為實驗數據，通過牛頓力學積分算法模擬glider在水下的運動軌跡，根據運動軌跡與理想軌跡的偏移來調整glider運動的相關參數，實現了基于牛頓力學積分的水下滑翔機群的協同控制，并將實驗結果進行可視化動態顯示，最后對實驗結果進行李雅普諾夫穩定性評估以及與相關算法的誤差對比，驗證了這種算法的可靠性。

1 數據來源與研究方法

1.1 數據來源

本研究所采用的數據源是ROMS海洋模式數據，ROMS模式為三維自由表面非線性原始方程近海區域模式，其包含高精度差分算法且與多種模式進行耦合，同時也包含多種垂向混合方案，近年來被廣泛應用于近海海洋環流的模擬。ROMS海洋模式數據在垂向上采用S坐標(Song and Haidvogel)，水平方向采用C網格和標準笛卡爾坐標系，數據存放格式為NetCDF格式。

本研究采用的ROMS模式數據采樣時間為2016年6月，采樣區域為南海150°E—151°E，30°N—31°N，水平方向上將其劃分為150×150個小格，空間分辨率為641.5 m。由于其特有的高分辨率優勢，ROMS海洋模式數據較傳統模式的數據更加接近海洋狀況，能夠滿足本研究積分算法對精度的要求。

1.2 研究方法

本研究的組網仿真實驗應用牛頓力學積分原理模擬glider在海洋中的運動，仿真實驗中 glider的模擬運動軌跡與設定軌跡的偏移可大致看作運動周期內海洋洋流對glider運動的影響[15]。以此來估計未來的運動周期內海洋洋流的影響，并適當調整glider的初始運動參數，使glider的運動軌跡能夠更加切合預定的軌跡。在實驗的最后用李雅普諾夫穩定性評估仿真實驗結果以驗證仿真實驗結果的有效性。

1.2.1 3種坐標系下坐標的轉換

組網仿真實驗所采用數據的坐標為C網格坐標，使用網格號來表示采樣區域內某一點具體的海洋流場。仿真實驗的結果呈現在大地坐標系上，利用大地經度和大地緯度來表示glider運動的出、入水點的位置。而對glider進行的牛頓力學積分的具體運算則需要在高斯平面直角坐標系下進行。所以本次仿真實驗的基礎則是實現3種坐標系坐標的轉換，坐標轉換基本流程如下。

首先，將C水平網格坐標以大地坐標為媒介，轉化為高斯平面直角坐標，轉化過程為

(1)

(2)

(3)

(4)

其次，在高斯平面直角坐標系上進行水下滑翔機牛頓力學積分和協同控制相關算法的計算工作。最后，將計算結果轉換回大地坐標，并且將轉換后的結果成圖展示，轉化過程為

(5)

(6)

在上述轉換過程中，M為C水平網格坐標的橫坐標，N為C水平網格坐標的縱坐標；L和B分別表示大地經度和緯度；R為WGS 84橢球的長半軸長度，其中WGS 84橢球為坐標原點與地球質心重合，空間直角坐標的Z軸指向北極點，X軸指向本初子午線與赤道交點，Y軸與X、Z軸構成右手坐標系的參考橢球體；X、Y指高斯平面直角坐標系相應的橫、縱坐標。經過上述坐標轉換，可以將不同坐標背景下的數據源統一到同一坐標系下進行計算。

1.2.2 牛頓力學積分算法

1.2.2.1 牛頓力學積分算法原理

牛頓力學積分原理如圖1所示，將實驗區域劃分為與海洋流場數據分辨率一致的格網，格網中的每一個小格都有其特定的海洋流場數據，海洋流場數據的不同造成glider在每個小格中的受力狀態不同，從而導致glider在每個小格中運動的加速度不同。圖中右側曲線表示在該海洋流場作用下的glider速度-時間曲線和位移-時間曲線。通過牛頓運動學公式在每個小格中的應用，計算出在每個小格中微小的位移和速度變化，將這些微小量在glider的一個運動周期內進行累加，實現類似數學積分的算法。在具體積分運算中以每個積分區間的位移變化決定下次積分運算所應用的流場數據，積分區間速度的變化作為下次積分運算的初始速度參數，同時位移變化的累加將會模擬最終的glider運動結果。在一個運動周期內具體的計算過程為

圖1 牛頓力學積分算法原理示意圖Fig.1 Sketch map of Newton mechanicsintegral algorithm principle

Vi+1=Vi+ai+1Δt,i=0,1,2,…

(7)

(8)

(9)

式中：i表示一個運動周期的積分次數；Δt是進行積分運算的最小時間單元；V代表glider運動的速度；a是glider運動的加速度，由海洋流場數據經過處理得到；Δx是glider在一個積分區間的位移變化量；X是在一個運動周期內總的位移，由多個Δx累加得到。

1.2.2.2 牛頓力學積分算法的實際應用

本文中牛頓力學積分的實現建立在海洋流場數據已知的前提下，然而實際海洋實驗過程中海洋流場是未知的因素，在具體實驗中需要利用glider上安裝的流場傳感器實時測得的流場數據進行計算。對于尚未完成觀測任務的滑翔機，其水下軌跡計算過程中所需的流場數據根據已測得的流場，使用客觀分析方法即可預測得到。

1.2.3 水下滑翔機群的協同控制

1.2.3.1 動態修正算法

目前在glider的工作中，為使glider較好地保持路徑，所采用的方法為動態修正算法。該算法根據glider在一個運動周期結束后實際路徑與預期路徑的偏移對glider下一運動周期入水前的參數進行計算，其原理是將glider在上個運動周期受到的洋流影響看作是下個運動周期受到的影響。本研究中動態修正算法的實現是通過積分運算模擬glider在海洋中的運動軌跡，以不同軌跡的差別為依據調整下個周期glider運動設置的初始參數，依次往復循環計算，直至glider完成最終的觀測任務，具體計算過程如下

ex=X-X0

(10)

(11)

V′=V-ΔV

(12)

式中：X為glider運動周期結束后的出水位置，X0為glider的目標出水位置，ex表示在一個運動周期內glider實際運動軌跡與預期軌跡的誤差，ΔV是根據此誤差作出的速度改正，V′為改正后的下一運動周期的起始速度參數，T為glider一個完整運動周期所持續的時間。

1.2.3.2 出、入水異步性

在本實驗中部署的glider具有不同的出、入水間隔，該初始參數的設定造成glider在運動過程中出、入水的異步性。異步性更加符合glider在實際應用中的運動情況，故本實驗在單臺glider的動態修正算法的基礎上提出一種改進的基于水下滑翔機群運動的協同控制算法。利用glider出水的異步性，將協同控制算法應用在glider之間，使glider的運動受到多重約束，即每臺glider在整體陣型中的運動要受其他glider的約束和控制。這樣在實際應用中可以將glider群受流場作用產生的誤差降到最低，使整體陣型保持得最好，從而達到在區域海洋中以不同分辨率進行采樣的目的。

1.2.3.3 協同控制算法

協同控制算法是在上文動態修正算法基礎上的全面優化，基本思想是根據glider群出水的異步性，將修正算法應用到glider運動周期之中。每臺glider會根據其他glider的出水狀態在運動周期內修正自身的航向，在一次完整的運動周期內多次修正自身的軌跡，使其運動軌跡更加貼合預設軌跡，既保留了原始改正算法的優點，又將洋流的影響降到最低。具體原理圖如圖2所示。

圖2 協同控制算法原理圖Fig.2 Schematics of cooperative control algorithm(紅色虛線是glider未經改正算法的運動軌跡，藍色線條是glider在一個運動周期內經協同算法改正的軌跡，黃色箭頭是協同算法約束下修正的航向。)(The red dashed line is the trajectory of the glider without correctionof the algorithm, the blue line is the trajectory of the glidercorrected by the cooperative algorithm within a motion cycle,and the yellow arrows are the direction corrected underthe constraints of the cooperative algorithm.)

協同控制算法的基本流程為

(1)根據glider設定的相互間的距離和預定的運動陣型，計算完成觀測任務所需運動周期個數，當glider完成最后一個周期的觀測任務，該glider的總觀測任務終止。

(2)將4臺glider安放在矩形陣型的4個角點上，分別按照預先設定的航向角同時入水觀測。

(3)由于glider群出水的異步性，glider群不會同時出水，但只要1臺glider出水，所有的glider會依據當前位置和目標位置的誤差計算洋流造成的航向偏移。

(4)每臺glider根據計算出的航向偏移修正預設的航向角，用新計算產生的航向角替代原先預設的航向角，進行新一運動周期的觀測工作，直至總觀測任務結束。

協同控制算法與動態修正算法最大的不同是根據glider群出水的異步性，將動態修正算法運用到glider在海洋的運動過程中。換言之在glider的運動周期內將運用多次動態修正算法，將一個完整的運動周期劃分為多個小運動周期，根據每個小周期的誤差來估計下一個小周期洋流的影響并做出航向角改正，直至整個運動周期結束。這樣可以將一個運動周期的洋流誤差降到最低，而原始的動態修正算法只能在本次運動周期結束后根據本次運動周期的誤差進行下一個運動周期誤差的改正，這樣會不可避免地造成誤差分布的不均衡性，從而不能很好地保持整體陣型。

2 多機仿真實驗設置

2.1 仿真陣型

在glider部署陣型中，首先需要將陣型設計成長而封閉的曲線，以便于對海洋數據梯度變化的橫斷面進行重復測量。在所有封閉的陣型中，本次仿真實驗選擇的是矩形陣型，選擇矩形陣型的原因一是便于對海洋區域進行整體的研究，一塊固定海洋區域的準確測量對區域海洋科學的研究具有重要意義；二是因為矩形是最簡單的陣型，本著研究問題先易后難的原則，只有當水下滑翔機群協同控制算法在簡單的陣型中達到最理想的實驗效果后才有可能向復雜的陣型拓展。本實驗在南海150° E—151° E，30° N—31° N的一片矩形海域布置4臺glider，glider的初始位置位于矩形陣型的4個角點，按逆時針方向沿設定軌跡運動(圖3)。

圖3 仿真陣型Fig.3 The formation of simulation(黑色實線表示glider運動的理想陣型；黃色虛線表示glider之間的協同作用，即單臺glider的運動要受到其他glider的約束；背景場為具體的海洋流場。)(The solid black line shows the ideal formation of glider movement.The dotted yellow line shows the synergy between gliders, that is,the movement of a single glider is restricted by other gliders.The background represents a specific ocean current field.)

2.2 動態修正算法仿真實驗結果

為定量描述動態修正算法在路徑保持方面的效用，本研究設置了2次微型仿真實驗，利用單臺glider在海域中做簡單的直線運動，給定一個較大的出入水間距(10 km)，并附加一個強度較高的海洋流場。其中一次實驗不應用動態修正算法，按照預定的參數輸入，在整個運動過程中驗證流場對glider運動的影響；另一次實驗運用glider動態修正算法，檢驗動態修正算法是否可以有效地減少流場影響，使glider按照預定軌跡運行。

根據圖4可以明顯地看出動態修正算法可以抵消部分洋流影響，應用動態修正算法前(圖4a)，由于洋流的影響glider距目標出水點距離較遠，偏離預定軌跡約12 km；應用動態修正算法后(圖4b)，glider軌跡偏離誤差減小到約6.6 km，誤差降低約45%。由此可知該改正算法具有一定的有效性，但并不能完全抵消海洋流場的作用。原因是該改正算法只是對海洋流場的簡單估計，換言之是將glider在上個運動周期受到的流場影響看作是在下個運動周期中即將受到的影響。而實際海洋流場是一個復雜多變的過程，沒有流場數據完全相同的兩個位置，該算法只有當glider相鄰兩個運動周期海洋流場相同或相近時才能有效降低誤差，若這兩個運動周期海洋流場的方向相反，誤差則不會降低反而會擴大。

圖4 未經動態修正算法約束(a)和經過動態修正算法約束(b)的glider的運動軌跡對比Fig.4 Comparison of glider trajectories without dynamic correction algorithm constraints(a) and after dynamiccorrection algorithm constraints(b)(圖中紅色線表示glider的運動軌跡，黃色點表示glider的出水點，黑色點表示glider的目標出水點，背景場為具體的海洋流場。)(The red line in the figure shows the trajectory of the glider, the yellow dots represent the water outlet points of the glider, and theblack dot represents the target water outlet point of the glider. The background represents a specific ocean current field.)

2.3 多機協同仿真實驗結果

基于牛頓力學積分的水下滑翔機群協同控制算法仿真實驗結果如圖5所示，圖像中的黑色矩形是本次glider群運動的設定軌跡，圖像中不同顏色不同形狀的色點表示不同glider的出水點。當glider的位置出現與之相對應的色點表示glider在此刻已經出水，當前運動周期結束；當glider的位置沒有出現色點表示glider目前正在水下執行觀測任務，當前運動周期未結束。實驗結果表明glider群的運動具有出水的異步性，單個glider的運動要受到其他glider運動的約束和影響，故陣列的保持是多臺glider協同作用的結果。本次仿真實驗中，在設定軌跡上glider群的陣型保持得較好，不存在偏離預定軌跡程度較大的點，但要準確描述glider群協同控制算法的有效性，還需要對其實驗結果進行誤差評定以及與其他算法的實驗結果進行對比。

圖5 協同控制算法約束下glider的運動Fig.5 The motion of the glider constrained by thecooperative control algorithm(黑色實線是glider運動的理想陣型，黃色虛線是glider間的協同作用，背景為海洋流場。不同顏色的色點代表不同glider的出水點。)(The solid black line is the ideal formation for gliders movement,the dotted yellow lines are the synergy between gliders, and thebackground shows the ocean current field.The color dots ofdifferent colors represent the water outlet points ofdifferent gliders.)

3 仿真實驗結果分析與評價

通過運用協同控制算法，本次仿真實驗實現了4臺glider在一塊矩形區域相互間以相對固定的距離沿設定軌跡進行運動，為驗證協同控制算法的優越性，對glider運動的軌跡進行誤差評價與分析，比較經過協同算法改正和未經協同算法改正的glider運動軌跡誤差的分布情況，并對整個glider運動的陣型進行李雅普諾夫穩定性評估，驗證4臺glider協同運動陣型的穩定性。

3.1 仿真結果對比與評價

為準確評價協同控制的仿真實驗結果，擬采用2種結果對比與評價方式。第一種評價采取單獨對比的方式，選取第2臺glider(隨機選取)作為評價對象，提取第2臺glider經過協同控制算法改正的軌跡和經過動態修正算法改正的軌跡，對兩種軌跡進行誤差對比，從而驗證在glider整體協同控制算法約束下運行的單臺glider對海洋流場作用的抵消效果；第二種評價采用總體對比，對4臺glider運行的整體誤差進行評估，檢驗經過協同控制算法改正的glider群的陣列保持狀況。

3.1.1 單臺glider軌跡誤差對比

單臺glider的軌跡誤差對比結果如圖6所示，圖中紅色的曲線代表動態修正算法下的單臺glider誤差曲線，藍色曲線代表協同控制算法約束下的glider誤差曲線。對比結果顯示，經過協同控制算法約束的glider軌跡要優于經過動態修正算法改正的軌跡，動態修正算法的glider軌跡的平均誤差為97.8 m，標準差約為1.43 km；而經過協同控制算法約束的glider軌跡的平均誤差為61.3 m，標準差約為0.53 km。由此可知協同控制算法能夠有效地約束glider的軌跡，降低glider運動的誤差。但協同控制算法也可能將個別出水點的誤差放大，這是因為在具體的glider運動過程中海流狀況是一個不可預知的因素，該算法也可能會出現矯枉過正的情況，但整體的軌跡誤差優于動態修正算法。

圖6 不同算法約束下的glider運動軌跡誤差Fig.6 Trajectory errors of glider under differentalgorithm constraints

3.1.2 glider陣型總體誤差對比

對glider整體運動陣型的評估選用glider實際運動軌跡與設定軌跡的角度誤差。角度誤差越小，證明glider運動的總體陣型保持得越好，與設定的軌跡貼合度越高。glider運動陣型角度誤差統計結果如表1所示，其中動態修正算法約束下角度誤差大部分分布在5°～10°，還有相當一部分的誤差超過10°，總體陣型保持情況不是很理想；而經協同算法約束下的大部分誤差都分布在0°～3°之間，數值較大的誤差占比大幅度下降。

表1 角度誤差的統計結果Tab.1 Statistical results of angle errors

與動態修正算法相比，協同控制算法約束下的glider角度誤差較小，能更有效地保持glider的總體運動陣型，但仍有部分誤差分布在3°～10°之間。出現這種情況的原因是glider在矩形陣型下運動至少需要改變3次航向，在矩形拐角處航向的變動加上海流的影響，不可避免地會將角度誤差放大，但是誤差的放大只是在小部分區域，航向改正后協同控制算法還是能夠將誤差控制在合理范圍內。

3.1.3 相關算法的誤差對比

就誤差的修正情況來看，前人用勢函數和虛擬領導者引導glider編隊執行追渦觀測任務，在角度誤差的改正和約束方面，glider編隊的平均誤差由18.2°減小到8.1°，但是標準偏差卻由7.8°增加到8.1°。本文提出的協同算法引導4臺glider沿固定陣型進行運動，角度誤差由改正前的7.2°減小到4.6°，標準偏差由改正前的7.1°減小到5.6°。就距離誤差而言，前者平均誤差最大為出入水間距的22%，最小為出入水間距的7%；而本文提出的協同控制算法將glider運行的平均誤差降低約37%(表2)。綜合以上情況而言，本文提出的水下滑翔機群協同控制算法在誤差矯正方面要優于勢函數調節，但兩種算法研究的海洋問題背景不同，為進一步比較兩種算法的優越性，需要將本研究的協同算法應用到與之對應的觀測任務中，這將在以后的研究中得到證實?？晌阌怪靡傻氖遣徽撃姆N算法都證明雖然觀測任務不同，海洋流場強度不同，但glider之間的協同控制可以有效降低glider在海洋中的運動誤差，證明在未來的海洋科學研究中glider的協同控制算法必定會有很大的應用空間。

表2 不同算法的誤差改正對比Tab.2 Comparison of error correction of different algorithms

3.2 李雅普諾夫穩定性評估

李雅普諾夫穩定性理論對于研究控制系統的穩定性具有十分重要的意義，控制系統可以分為線性系統和非線性系統，李雅普諾夫理論可以應用到兩種系統的評估。對于線性系統通常采用求取系統狀態方程的解來判斷系統是否具有穩定性，而對于非線性系統求取其狀態方程的解非常困難，此時李雅普諾夫穩定性理論就顯示出很大的優越性。其判斷控制系統穩定性的基本思想是首先確定系統的平衡點，而對于打破平衡狀態的初始時間t0，存在一個誤差閾值ε，當任意給定的時間t大于t0時，若在t時刻系統的歐幾里得范數小于ε，則說明這個系統是穩定的，數學表達式為

(13)

(14)

就本文進行的仿真實驗而言，glider運動系統的平衡位置是預先設定好的軌跡，glider運動的誤差Er(i) 為

Er(i)=X(i)-X0(i)

(15)

其中：i表示glider的出水次數，X(i)表示第i次出水glider的實際位置，X0(i)表示第i次出水glider的目標位置，這樣glider運動系統的范數可以表示為

(16)

(17)

當glider出水次數N趨于無窮大時，每一次glider出水的實際位置與目標位置的誤差相當于均值為0的高斯白噪聲，N個高斯白噪聲的范數和在N趨于無窮時趨于0。

而在仿真實驗中無窮大是不可能實現的，所以只能通過不斷增加N的次數來驗證這一規律。通過設定不同的出入水間距來實現在整個觀測周期內出水次數的不同，不同出水次數的系統范數計算結果如表3 所示。

表3 不同出水次數下的系統范數和Tab.3 Sum of system norms at different times of water outlet

通過計算結果表明，隨著出水次數的增加，系統的經度范數LON(N)和緯度范數LAT(N)沒有明顯增加的趨勢，甚至還有略微的減小，說明系統具有穩定性，進而證明glider協同控制算法能夠有效地抵消海洋流場的影響，使整個glider系統保持一定的穩定性。

4 結論與展望

本文基于高分辨率ROMS海洋模式數據，利用牛頓力學積分模擬glider在海洋中的運動，根據glider實際運動軌跡與設定軌跡的偏差來對運動的起始參數進行改正，在動態修正算法的基礎上繼續深入，提出了針對glider群整體運動的協同控制算法。根據在陣型中glider群出水的異步性，將動態修正算法應用到glider的運動周期內，每臺glider的運動要受到其它多臺glider的約束，一個完整的運動周期要運用多次動態修正算法。將協同控制算法應用到實際運動軌跡中去，以有效的方式對復雜的海洋環境作出模擬，以整體約束的思想對glider的軌跡進行調節，使glider的運動更加貼近理想情況。研究結果表明協同控制算法能夠有效地保持glider運動的整體陣型，抵消部分洋流的影響。在協同控制算法約束下的glider可以較好地保持預定軌跡，glider間能夠以相對穩定的距離關系進行組網觀測。

本文的創新點主要集中在兩個方面：

(1)由海洋實際流場推演glider在海洋中的運動狀態，以力學積分的方式模擬glider的運動，glider運動陣型的協同控制是通過glider改變下個運動周期的航向角實現，使得glider在仿真實驗中的虛擬運動更加符合glider在實際海洋實驗中的操作流程。

(2)在實際應用中以真實海洋流場造成的glider運動誤差為依據調節glider的運動，比起運用數學函數調節glider的運動陣型具有更高的真實準確性。

但是采用這種方法來約束glider的軌跡仍然具有一些不足之處：目前水下滑翔機群協同控制算法仍然局限于可控的流場強度，如果流場的強度過高、流速過快，則無法實現對glider陣型的有效保持。