多无人船编队控制及其位置估计算法研究

邓志良，王振凎，沈薇，刘云平

（南京信息工程大学 自动化学院，南京 210044）

目前，多无人船编队控制被广泛应用于水质监测、气象监测、水产养殖以及军事等领域[1-3]。与单无人船水面作业相比，多无人船编队通过在航行过程中形成动态网络，多艘无人船之间进行信息交互，可有效提高作业效率和系统的容错性，增强系统防御突击能力，保证任务的完成度。

已知的编队控制方法有领航-跟随法[4]、基于行为法[5]、虚拟结构法[6]、人工势场法[7]和基于图论法[8]等。领航-跟随法因其结构简单，理论成熟且易于实现，并有着良好的拓展性，被广泛应用。Wei[9]提出虚拟领航者的概念并将此应用到领航跟随编队中，利用虚拟领航者消除真实领航者出现故障的隐患，提高了系统的稳定性；Wu 等[10]基于领航法，提出通过调整偏差角减少避障时间，同时缩短编队需要调整的距离；Wang 等[11]将领航-跟随应用于欠驱动水下航行器编队控制中，提出一种结合反步法、自适应神经网络和动态水面控制技术的滑模编队控制策略。本文主要针对编队算法的实际应用，选择领航跟随法。

编队导航是编队控制实际应用的核心技术之一，主要由GPS 获取的经纬度信息确定位置信息。GPS 所测得的数据精度相对较低，且在有遮挡的环境下通常会出现数据漂移的现象，无人船中使用的GPS 测量误差大约在2～ 5 m，定位精度低会导致编队队形的不稳定。Kwang-Kyo 等[12]研究了基于方向对准和位置估计的单双积分器在二维平面上的编队控制，并提出一种基于方向对准和位置估计的编队控制策略；Tsuyoshi 等[13]融合距离传感器和无线传感器的信息，并结合了扩展卡尔曼滤波器实现机器人编队控制中的位置估计；王念曾等[14]融合GNSS、UWB 和INS 设计了基于无人机之间距离信息的相对导航方案，并建立系统状态方程及观测方程，引入无迹卡尔曼滤波器对相对状态进行估计；Yang 等[15]开发了一种位置估计器，并基于估计的相对位置提出针对跟随者的控制律，通过仿真验证了系统的稳定性及有效性。然而，以上研究并未考虑定位精度对无人船编队队形控制的影响，且具体的精度要求尚未有明确规定。

本文针对多无人船编队队形控制问题，利用领航-跟随法建立多无人船编队控制模型，并设计多无人船编队控制器，保证多无人船编队结构的稳定性。同时，针对实际应用研究中定位精度低引起的编队队形不稳定的问题，提出了基于扩展卡尔曼滤波技术的位置估计算法，通过融合GPS 和IMU 数据，进一步提高定位精度获得更为精准的位置信息，并通过仿真验证方法的有效性及稳定性。

1 问题描述

1.1 无人船系统模型

无人船的运动是在三维空间的复合运动，由沿3 个坐标轴的直线运动和绕着3 个坐标轴的旋转运动组成，即沿x轴 的纵荡和绕x轴 旋转的横摇；沿y轴的横荡和绕y轴旋转的纵摇；沿z轴的垂荡和绕z轴旋转的艏摇。本文所研究的无人船主要在河道水面航行，可忽略垂荡、纵摇及横摇的影响，因此无人船模型被简化为三自由度的非线性模型，其运动模型见图1。

图1 单船运动模型

根据文献[16]建立无人船数学模型：

本文中的整个编队行动基于领航者的运动轨迹，通过指定其中一艘无人船为领航者，形成领航跟随结构，如图2 所示。领航船与跟随船的位置分别为(xl,yl)和(xf,yf)，L为两船之间的相对距离，Lx、Ly分别为两船之间的横纵距离，φl、φf分别为领航船和跟随船的航向。通过控制跟随船到领航船之间的期望距离Ld和实际距离L，实现船之间的相对距离保持不变，从而实现多无人船的编队。

图2 无人船领航-跟随编队模型

由图2 可得领航船与跟随船之间相对距离为：

对两边求导得到两艘无人船的相对运动方程：

式中：Ld、θd分别为领航船与跟随船之间的期望距离和期望相对角度。x、y方向上的距离误差分别为：

两边求导得：

基于领航-跟随法的无人船编队控制模型为：

式中：τu f、τr f分别为跟随船的前进推力和艏摇力矩。

1.2 编队控制目标

2）通过改变控制输入τu f和 τr f，使uf→ufd，rf→，即：

1.3 卡尔曼滤波器

卡尔曼滤波是控制领域中一个重要的研究方向，它可以从一组测量值中得到所需的信息，滤过数值中包含噪声和偏差等问题。线性方程可以由最小二乘递推得出，被用于线性系统预测。

设一个线性方程表示的离散控制系统为

系统测量值为

式中：X(k)为无人船k时刻的状态；Z(k)为观测值；A为状态转移矩阵；H为观测矩阵；W(k-1)为过程噪声；V(k)为测量噪声。

由上一时刻无人船的状态预测当前k时刻无人船的状态，即过程方程为

由状态转移矩阵A和 过程噪声矩阵Q，更新预测状态X(k|k-1)的协方差矩阵P，即

由观测矩阵H、测量噪声矩阵R和P(k|k-1)计算增益Kg，即

根据增益Kg、预测状态X(k|k-1)和观测矩阵H，计算k时刻的最优状态估计值X(k|k)，即

由增益Kg、观测矩阵H和P(k|k-1),计算出k时刻的最优值P(k|k)，即

2 控制器设计

2.1 编队控制器设计

由式（8）和式（9）的编队目标，根据反步法设计相应的控制器，首先引入两个坐标转换z1和z2，即：

两边求导可得：

则式（7）转化为：

假设uf、vf为(z1,z2)系统的输入量，和分别为uf和vf的虚拟量，定义 Lyapunov 函数V1=z12/2+z22/2，两边求导可得

假设选择

将式（21）代入式（20）可得

由此可知，(z1,z2)渐近稳定。

引入速度误差变量ue=uf-ufα，两边求导可得

假设选择τu f

代入式（24）可得

所以，(ue,eφ)是渐近稳定的。

定义速度误差

两边求导可得

选择 τrf为

式中k4为正常数。将式（30）代入式（29），得-k4ue2≤0。

通过给出式（25）和式（30）的控制输入 τuf和 τr f，可使无人船之间保持期望的距离和相对角度，从而可按照期望队形航行，实现编队。

2.2 无人船位置估计设计

由1.1 节的数学模型可知，根据多无人船系统中的每艘无人船的位置信息可实现多无人船编队。本文采用扩展卡尔曼滤波算法，将GPS 所测得的数据与IMU 所测得的数据进行融合，获得较为精准的位置信息，提高了定位的精度，保证了编队队形的稳定性。

选择的状态量为

式中：px、py为无人船的二维坐标；vx、vy为航行线速度；ax、ay为航行加速度。各参数都在北东地坐标系下。

建立过程方程，将所使用的方法进行线性化，使其适用于无人船系统。其次，再根据式（12）和式（16）对其进行预测和更新。当前时刻k的位置信息，泰勒展开式为

速度信息的泰勒展开式为

假设短时间内，忽略加速度，则

结合式（13）～ 式（15），针对式（10）的离散系统设计的状态转移矩阵为

选择的测量值向量为

式中：pmx、pmy为二维测量值，由GPS 所测得的差值计算得来；vmx、vmy为由GPS 测得的速度；amx、amy是由IMU 的加速度计获得的。

式（11）中的观测矩阵H为

结合式（10）、式（11）、式（31）、式（35）～ 式（37）可得出位置估计的系统方程，即：

由式（12）和式（16）得无人船位置的估计值，即

3 仿真结果

对设计的控制器进行仿真实验，实验中的多无人船系统由3 艘无人船组成，各项参数[17]如下：

在两种不同初值的情况下，采用不同的队形约束和相同的速度约束来验证编队算法的可行性。领航无人船的航行轨迹记为xL，两艘跟随无人船的航行轨迹分别记为xF1和xF2。使跟随无人船在船体坐标系下与领航无人船的相对位置(lx,ly)=即相对期望距离保持为2 m，领航无人船相对其跟随无人船的方位分别为 π/6和-π/6。设置领航无人船从（1,1）起航，偏航角为 π/6，以速度（2,0）匀速直线航行，在航行过程中，航向保持不变，两艘无人船的起始位置分别为（1,-1）和（-1,3），初始偏航角分别为π/12和0，速度为（1,0）。

3 艘无人船仿真的偏航角变化如图3 所示，两艘跟随无人船的偏航角收敛至 π/6，与领航无人船偏航角一致。领航无人船相对两艘跟随无人船的偏航角收敛至 π/6和-π/6，达到设定的偏航角。

期望相对位置的仿真结果如图4 所示，跟随无人船在靠近期望距离过程中，通过调整航速和航向，逐渐达到期望距离，虽有一定波动，但在可接受范围内，达到编队要求。

图4 期望相对位置仿真

结合图3 和图4 的仿真结果，可见本文设计的编队方法可有效控制两艘无人船形成稳定的l-φ结构的编队队形。如图5 仿真结果所示，领航无人船以2 m/s 的速度匀速航行，跟随无人船在保持队形的同时，速度逐渐收敛至2 m/s，并保持该速度继续航行，保证了系统编队速度的一致性。

图5 跟随无人船速度仿真

多无人船编队系统仿真航行轨迹如图6 所示。开始三艘无人船并未形成期望距离2 m 的编队队形结构，之后通过编队控制器的不断调整，开始保持队形，逐渐满足编队队形要求。

图6 多无人船编队队形仿真

从无人船的偏航角变化、期望距离、速度变化以及3 艘无人船航行轨迹这几个方面进行分析，从仿真结果来看，误差均在接受范围内，可见该编队方法是可行的。

依旧使用3 艘无人船进行编队，一艘为领航者，其余两艘为跟随者，从不同位置按照约束进行编队。最初设定的期望距离为2 m，角度为 π/4，航行一段时间后，改变领航无人船的航向，将编队队形中设定的期望距离改为5 m，角度改为 π/6，航行一段时间之后再恢复成初始设定，仿真结果如图7 所示。当队形发生变化之后，跟随无人船迅速调整自身姿态，形成新的队形，满足队形控制的要求。

图7 变换队形仿真

为验证位置估计算法的可行性，对GPS 和IMU的数据融合进行仿真实验。为贴近实际无人船的航行，仿真数据中加入了加速度计和陀螺仪的漂移等误差信息，并分析无人船的位置信息。设定重力加速度为9.8 m/s,初始经纬度信息为（N 32.02,E 118.14）。引入误差信息如表1 所示。

表1 误差参数

图8 为单独使用GPS 测得的位置误差，GPS 的测量误差不随时间积累，但误差范围偏大，最大达到了3 m。在多无人船编队实际应用中，难以满足实际需求。

图8 GPS 位置误差

图9 为将GPS 和IMU 数据融合之后再经过扩展卡尔曼滤波的位置误差。短时间内误差较小，随着时间的增加，误差存在一定波动。扩展卡尔曼滤波对误差进行了补偿，所以误差范围较小，纬度误差在0.6 m左右；经度误差在0.4 m 左右。与GPS 单独工作相比，定位精度明显提高，证明了滤波算法的有效性。为验证多无人船编队的队形稳定性，对编队过程中的数据进行处理，生成如图10 所示的航行轨迹。从图中可以看出，编队轨迹较为平滑，虽有一定的波动，但在可接受范围之内。结果表明，该方法可有效完成编队控制，保证队形结构的稳定性。

图9 位置估计后的位置误差

图10 引入EKF 后的编队航行轨迹

对引入扩展卡尔曼滤波后的多无人船编队航行数据进行分析。跟随无人船速度仿真结果和偏航角偏差仿真结果见图11。跟随无人船速度逐渐趋近于领航无人船的速度，仅伴随少许波动，跟随无人船偏航角偏差逐渐趋近于0，仅在0 附近上下波动。编队过程中跟随无人船纵向和横向的位置数据分析见图12。跟随无人船的横向、纵向距离逐渐趋近于期望距离，存在一定波动，但波动都相对稳定，误差大约为0.6 m，一定程度的提高了定位精度。

图11 跟随无人船航速及偏航角偏差

图12 位置数据分析

多无人船系统变换队形阶段位置数据如图13所示。图13a）中，期望距离从5 m 变为2 m 时，跟随无人船可较快地进行调整以完成队形的重构及保持。图13b）中，可在变换队形后迅速重构队形。

图13 变换队形阶段位置曲线

4 结论

结合扩展卡尔曼滤波算法和领航-跟随法，提出了一种可有效解决编队队形不稳定问题的编队控制策略。通过融合GPS 和IMU 两部分数据，获得了更为精准的位置信息。对多无人船编队及队形变换进行了MATLAB 仿真实验，验证了编队控制算法和位置估计算法的有效性和稳定性。