基于牛顿力学积分的水下滑翔机群协同控制研究

王贵凯，马纯永*,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)。综合以上情况而言，本文提出的水下滑翔机群协同控制算法在误差矫正方面要优于势函数调节，但两种算法研究的海洋问题背景不同，为进一步比较两种算法的优越性，需要将本研究的协同算法应用到与之对应的观测任务中，这将在以后的研究中得到证实。可毋庸置疑的是不论哪种算法都证明虽然观测任务不同，海洋流场强度不同，但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阵型的有效保持。