第 40卷 第 6期
2019年 6月
仪 器 仪 表 学 报
Chinese Journal of Scientific Instrument
Vol. 40 No. 6
Jun． 2019
DOI:10. 19650 / j．cnki．cjsi．J1904889
收稿日期:2019-03-25 Ｒeceived Date:2019-03-25
* 基金项目:国家自然科学青年基金(51609046)、水下机器人技术重点实验室研究基金(No．614221502061701)资助项目
基于领航跟随法的多 AUV编队控制算法研究*
李 娟1，2，袁锐锟2，张宏瀚2
(1. 哈尔滨工程大学 水下机器人技术重点实验室 哈尔滨 150001;2. 哈尔滨工程大学 自动化学院 哈尔滨 150001)
摘 要:针对已知路径下基于领航者的多自主水下机器人(AUV)编队队形控制问题，提出了一种 AUV路径控制和编队协调控
制相结合的新型编队控制器。其中，AUV的路径跟踪控制采用反步滑模控制器，将 AUV 位置、姿态和时变速度跟踪转化虚拟
速度控制，使 AUV能达到期望的位置、速度等，避免了反步控制中的奇异值问题，并能够很好实现不确定的模型的控制，同时又
提高了跟随者协同定位精度;在路径跟踪控制基础上，编队协调控制器将领航者与跟随者的位置误差控制转化为跟随者的速度
误差控制，使跟随者能快速达到期望位置，从而使所有 AUV实现期望的队形并保持。仿真实验对该控制策略进行了可行性验
证，结果表明，该算法提高了编队的响应速度、控制精度和稳定性;再应用 3台 AUV进行了湖上试验验证，证明了该控制策略的
有效性，能有效应用到实际中。
关键词:自主水下机器人;路径跟踪控制;编队协调控制;滑模控制
中图分类号:TP24 TH701 文献标识码:A 国家标准学科分类代码:510. 80
Ｒesearch on multiple AUVs formation control algorithm based
on leader-follower method
Li Juan1，2，Yuan Ｒuikun2，Zhang Honghan2
(1．Science and Technology on Underwater Vehicle Technology，Harbin Engineering University，Harbin 150001，China;
2．College of Automation，Harbin Engineering University，Harbin 150001，China)
Abstract:Aiming at the formation control problem of AUVs based on leader in a known path，a new formation controller combining AUV
path control and formation coordination control is proposed． Firstly，in the controller the backstepping adaptive dynamical sliding mode
control method is adopted to achieve the path following control of the AUV，which transforms the position，attitude and time-varying
velocity tracking to virtual velocity control，makes the AUVs to reach the expected position and velocity，effectively avoids the singular
value problem of backstepping control，nicely realizes the control of the uncertain model，and at the same time improves the accuracy of
follower collaborative positioning． Secondly，based on the path tracking control，the formation coordination controller converts the position
error control of the leader and follower into speed error control of the follower，enables the followers to quickly reach the desired positions
and makes all the AUVs achieve and maintain the expected formation． Simulation experiment was carried out to verify feasibility of the
proposed control strategy，the result shows that the proposed algorithm improves the response speed，control accuracy and stability of the
formation． The test experiment on the lake with three AUVs was conducted，and the result proves the effectiveness of the proposed control
strategy，which can be effectively applied in actual situation．
Keywords:autonomous underwater vehicle(AUV);path following control;formation coordination control;sliding mode control
0 引 言
自主水下机器人(autonomous underwater vehicles，
AUV)是水下观测，探寻和开发的水下实验工具，更可应
用到军事上，用于侦察，巡逻等方面。而随着水下科学实
验与海洋开发项目的增加，AUV 也应用到各个领域，但
单个 AUV由于可携带负载、航程、航速等方面的限制，往
238 仪 器 仪 表 学 报 第 4 0 卷
往难以胜任十分复杂的任务，于是多 AUV 系统成为研究
热点。多 AUV系统并不是单 AUV 的简单叠加，而是多
AUV之间的协调合作，具体涉及体系结构、通信等多方
面的内容，通过建立合理的多 AUV之间的编队协同探测
机制，改善单个 AUV 的功能缺陷，使其能够更高效且高
质量地完成水下作业。
近几年来，许多学者已经对 AUV的单体路径跟踪控
制问题进行了研究，在视线导引法的条件下，对运动误差
方程提出了 PID控制，滑动模块控制，自适应控制，神经
网络控制和反步法控制［1-7］等方法。文献［1-2］对 AUV
过去，现在，未来的发展进行了详细的描述。文献［3］中
利用 PID控制器简单高效，结构简单、固有鲁棒性，调整
参数简单等特点，一般用于控制 AUV 的航向角，可以让
AUV实现沿着任何三维空间轨迹运动的控制方法。文
献［4］利用滑模控制具有快速响应、系统无需人为辨识，
参数变化快、物理实现简单等优点，从而可以根据环境当
前的状态变化而使控制器自身结构发生相适应变化;文
献［5］自适应控制能不断学习，使系统结构参数能够调
整得到实际值。文献［6］神经网络控制是为了解决系统
在不确定或未知环境中的控制问题，能够以高精度逼近
非线性函数，对复杂的未知问题具有一定自适应和自学
习能力，并可以解决大规模实时计算问题，信息综合能力
十分强等。文献［7］反步法是利用李亚普诺夫定律设计
控制器，并通过李雅普洛夫提供证明，反步法通过增加非
线性阻尼项，可以将非线性方程转换成更简单的形式来
设计控制器，给控制器的设计带来灵活性。这些控制器
均在实际应用中具有优点和缺点。
而在编队控制方面，众多学者提出了多种编队方法，
得到广泛研究以及应用的有 3 种，分别为跟随领航者法
(leader-following)，基于行为法(behavior-based)，虚拟结
构法(virtual structure)［8-11］。文献［8］对近些年学者们对
编队问题进行了研究;文献［9］中领航者-跟随者方法指
定某一 AUV为领航者，其余为跟随者，将编队问题转换
为 AUV间角度和距离控制。文献［10］基于行为将 AUV
控制任务分解为避碰，运动和队形转换的基本行为，从而
简化 AUV 的编队控制。文献［11］运用虚拟结构法把
AUV编队整体看作一个刚体，AUV只需要跟踪刚体上对
应的固定点。
本文针对 AUV轨迹跟踪控制问题，将姿态误差转化
为虚拟速度控制，结合反步技术和自适应滑模控制方法，
设计 AUV轨迹跟踪控制器，而针对 AUV 编队问题上，在
领航跟随法的基础上，遵循相同算法设计领航者和跟随
者的路径，AUV 轨迹跟踪控制器使所有 AUV 都能实现
路径跟踪，然后通过领航者和跟随者“广义沿路路径距离
差”来控制跟随者速度，从而实现编队队形控制。最后在
在仿真实验和湖上试验均得到了有效验证。
1 AUV水平面模型的建立
结合受控对象 AUV的特点，建立 AUV 的数学模型。
AUV是一个空间的六自由度运动载体，为方便描述其运
动，其数学模型通常建立在北东坐标系和运动坐标系下。
在北东坐标系下定义其位置矢量，运动坐标系下定义其
广义速度矢量。建立在 2坐标系下的运动学和动力学水
平面数学模型为:
x = cos(ψ)u － sin(ψ)v，
y = sin(ψ)u + cos(ψ)v，
ψ = r，
u =
m22
m11
vr －
d11
m11
u +
τu + τw1
m11
，
v = －
m11
m22
ur －
d22
m22
v +
τw2
m22
，
r =
m11 － m22
m33
uv －
d33
m33
r +
τr + τw3
m33





(1)
式中:x，y为定义在固定坐标系下 AUV 的纵向位置矢
量，横向位置矢量;ψ为 AUV的航向角;u，v，r为 AUV 的
纵向速度;横向速度和艏向角速度，τu 和 τr 表示 AUV 的
纵向推力和横向力矩;参数 mii 和 dii 是 AUV的水动力参
数;τw1，τw2，τw3 为外界干扰力或干扰力矩。
2 AUV反步滑模控制器设计
控制器设计的目的是根据 AUV的数学模型，设计反
步滑模控制率驱动 AUV，使其在期望速度的情况下，达
到期望位置或在期望路径上运动。
1)定义误差变量
首先给出误差变量 xe、ye、ψe，期望艏向角:
ψd = arc tan
yd
xd( ) (2)
则误差变量可以写为:
xe
ye
ψe


=
cos(ψ) sin(ψ) 0
－ sin(ψ) cos(ψ) 0
0 0 1


x － xd
y － yd
ψ － ψd


(3)
式中:xe 为纵向位置误差;ye 为横向位置误差;ψe 艏向角
误差。
结合 AUV式(1)可得:
xe = u － vpcos(ψe)+ rye，
ye = v + vpsin(ψe)－ rxe，{ (4)
其中，vp = x
2
d + y
2槡 d。
考虑到式(4)，选取李雅普诺夫函数为:
第 6期 李 娟 等:基于领航跟随法的多 AUV编队控制算法研究 239
V1 =
1
2
(x2e + y
2
e) (5)
对式(5)求导并结合(4)可得:
V
·
1 = xe(u － vpcos(ψe)+ rye)+
ye(v + vpsin(ψe)－ rxe)=
xe(u － vpcos(ψe))+ ye(v + vpsin(ψe)) (6)
引入虚拟变量 αv，并令:
αv = vpsin(ψe) (7)
从式(7)可以看出变量 ψe 的控制转换是为了虚拟变
量 αv的控制。为了保证 V
·
为负，u和 αv认为控制变量，它
们的期望变量 ud 和 αvd 为:
ud = vpcos(ψe)－ k1x3 /E (8)
αvd = － v － k2ye /E (9)
其中，E = 1 + x2e + y
2
槡 e，k1，k2 为常数。
ue = u － ud，αve = αv － αvd (10)
那么:
xe = ue － k1xe /E
ye = αve － k2ye /E － rxe{ (11)
由此可得:
V
·
1 =－ (k1x
2
e + k2y
2
e)/E + uexe + αveye (12)
2)稳定虚拟误差变量 ue
ue = u － ud =
m22vr － d11u + τw1 － m11 ud + τu
m11
=
F1 + τu
m11
(13)
其中，F1 = m22vr － d11u + τw1 － m11 ud
选取李雅普诺夫函数:
V2 = V1 +
1
2
m11u
2
e +
1
2
(F1 － F
^
1)
2 (14)
其中，F
^
是 F1 的估计值:
S1 = c1ue + ue +
xe
m11
－
(F1 － F
^
1)
m11
=
c1ue +
(F
·
1 + τu)+ xe
m11
(15)
其中 c1 是正常量，则 ue 的导数为:
ue = S1 － c1ue －
xe
m11
+
(F1 － F
^
1)
m11
(16)
则 V2 的导数为:
V
·
2 =－ (k1x
2
e + k2y
2
e)/E + αveye + m11ueS1 －
c1m11u
2
e + (ue － F
·^
1)(F1 － F
^
1) (17)
选 T1 = τu，那么 S
·
1 则为:
S
·
1 =
c1(F1 + τu)+ T1 + F
·^
1 + xe
m11
(18)
建立李雅普诺夫函数:
V3 = V2 +
1
2
m11S
2
1 (19)
式(19)微分为:
V
·
3 =－ (k1x
2
e + k2y
2
e)/E + αveye + m11ueS1 －
c1m11u
2
e + (ue － F
·^
1)(F1 － F
^
1)+
S1［c1(F1 + τu)+ T1 + F
·^
1 + xe］ (20)
选择滑模控制率 T1 为:
T1 =－ c1(F
^
1 + τu)－ F
·^
1 － xe － m11ue －
ks1sgn(S1)－ ws1S1 (21)
其中，ks1 和 ws1 是正常量，由此可得:
V
·
3 =－ (k1x
2
e + k2y
2
e)/E － c1m11u
2
e － ks1 S1 －
ws1S
2
1 + αveye + (ue + c1S1 － F
·^
1)(F1 － F
^
1) (22)
设计 F1 的自适应控制律为:
F
·^
1 = ue + c1S1 (23)
则 V3 的导数为:
V
·
3 =－ (k1x
2
e + k2y
2
e)/E － c1m11u
2
e － ks1 S1 －
ws1S
2
1 + αveye (24)
3)稳定虚拟速度误差变量 αve
α
·
ve = α
·
v － α
·
vd =
vpsin(ψe)+ vpcos(ψe)(r － ψ
·
d)+ v +
k2(E
－1 － y2e E
－3)ye － k2xeyeE
－3 xe =
vpsin(ψe)+ vpcos(ψe)(r － ψ
·
d)+
F2
m22
+ Q1 (25)
其中，F2 =－ m11ur － d22v + τw2，Q1 = k2(E
－1 －
y2e E
－3)ye － k2xeyeE
－3 xe
为了使 V
·
3 为负，考虑变量 r，设计它的期望变量 rd
为:
rd = ψd +
－ vpsin(ψe)－ F
^
/m22 － Q1 － k3αve － ye /m22
vpcos(ψe)
(26)
其中，F
^
2 是 F2 估计量，k3 是正的常量。
所以引入误差变量:
re = r － rd (27)
则 αve 的时间导数为:
α
·
ve = revpcos(ψe)+ (F2 － F
^
2)/m22 － k3αve － ye /m22
(28)
建立李雅普诺夫函数:
V4 = V3 +
1
2
m22α
2
ve +
1
2
(F2 － F
^
2)
2 (29)
它的时间导数为:
V
·
4 =－ (k1x
2
e + k2y
2
e)/E － c1m11u
2
e － k3m22α
2
ve －
240 仪 器 仪 表 学 报 第 4 0 卷
ks1 | S1 | － ks1 | S1 | － ws1S
2
1 + (αve － F
·^
2)(F2 － F
^
)+
m22αve re vpcos(ψe) (30)
设计 F2 的自适应率为:
F
·^
2 = αve (31)
则 V4 的导数为:
V
·
4 =－ (k1x
2
e + k2y
2
e)/E － c1m11u
2
e － ks1 S1 －
ws1S
2
1 － k3m22α
2
ve + m22αve re vpcos(ψe) (32)
4)考虑 re 为辅助控制，因此接下来稳定误差变量 re
re = r － rd =
(m11 － m22)uv － d33r + τw3 － m33 rd + τr
m33
=
(F3 + τr)
m33
(33)
其中，F3 =(m11 － m22)uv － d33r + τw3 － m33 rd
建立如下李雅普诺夫函数:
V5 = V4 +
1
2
m33r
2
e +
1
2
(F3 － F
^
3)
2 (34)
其中，F
^
3 是 F3 的估计值。
选择动态滑模函数
S2 = c2re + re +
m22αve vpcos(ψe)
m33
－
(F3 － F
^
3)
m33
=
c2re +
(F^ 3 + τr)+ m22αve vpcos(ψe)
m33
=
c2re +
(F^ 3 + τr)
m33
+ Q2 (35)
其中，Q2 =(m22αve vpcos(ψe)/m33)，c2 是正常量。
则:
re = S2 － c2re － Q2 +
(F3 － F
^
3)
m33
(36)
求 V5 对时间的导数 V
·
5，并且把式(36)代入:
V
·
5 =－ (k1x
2
e + k2y
2
e)/E － c1m11u
2
e － ks1 S1 －
ws1S
2
1 － k3m22α
2
ve + m33reS2 － c2m33r
2
e +
(re － F
·^
3)(F3 － F
^
3) (37)
求式(35)对时间的导数，并且令 T2 = τr，则 S
·
2 为:
S
·
2 =
c2(F3 + τr)+ T2 + F
·^
3
m33
+ Q
·
2 (38)
其中，Q
·
2 = (m22 /m33)(α
·
vevpcos(ψe)+ αve vpcos(ψe)－
αve vpsin(r － ψ
·
d))
选取李雅普诺夫函数:
V6 = V5 +
1
2
m33S
2
2 (39)
则:
V
·
6 =－ (k1x
2
e + k2y
2
e)/E － c1m11u
2
e － ks1 S1 －
ws1S
2
1 － k3S
2
1 － k3m22α
2
ve － c2m33r
2
e + m33reS2 +
(re － F
·^
3)(F3 － F
^
3)+
m33S2
c2(F3 + τr)+ T2 + F
·^
3
m33
+ Q
·
2( ) (40)
选择动态滑模控制率 T2 为:
T2 =－ c2(F
^
3 + τr)－ F
·^
3 － m33re － m33Q
·
2 －
ks2sgn(S2)－ ws2S2 (41)
其中，ks2和 ws2是正常量，把式(41)代入式(40)，则
V
·
6 =－ (k1x
2
e + k2y
2
e)/E － c1m11u
2
e － ks1 S1 －
ws1S
2
1 － k3m22α
2
ve － c2m33r
2
e － ks2 S2 － ws2S
2
2 +
(re － F
·^
3 + c2S2)(F3 － F
^
3) (42)
设计 F3 的自适应率:
F
·^
3 = re + c2S2 (43)
则 V6 的导数为:
V
·
6 =－ (k1x
2
e + k2y
2
e)/E － k3m22α
2
ve － c1m11u
2
e －
c2m33r
2
e － ks1 S1 － ws1S
2
1 － ks2 S2 － ws2S
2
2 ≤ 0 (44)
考虑 AUV水平面模型式(1)，存在滑模自适应律式
(23)、(31)、(43)和控制律式(21)、(41)，能够保证最终
跟踪上期望轨迹。
3 编队协调控制器设计
3. 1 控制器设计原理
首先，定义 2 个名词，路径编号和广义沿路距离。
AUV的路径是由多个点 {P1，P2，…，Pk－1，Pk，…，Pn}连
接组成，其中 P1 为起始点，Pn 为终点，而相连 2点组成的
线段有一个编号，即路径编号，如图 1所示，定义 P1P2 线
段的路径编号为PN = 1，P2P3线段的路径编号为PN = 2，
以此类推，PkPk+1 线段的路径编号为 PN = K。
而广义路径距离 ΔS 指的是领航者和跟随者在同一
路径编号时，领航者与跟随者的距离在路径上的投影距
离即为广义路径距离，如图 2所示，其中跟随者的路径根
据编队队形由领航者路径平移、压缩等方法获得。
将领航者和跟随者分别记为 l、f，文中带有 l、f 下标
的参数都是关于领航者和跟随者的参数，例如 ud，l为领航
者的 ud，T1，f 为跟随者的滑模控制率 T1。
单体控制器将 AUV 的姿态跟踪转化为虚拟速度控
制，结合反步法和滑模控制，本节的协调控制器将编队的
问题同样转化为速度控制，由式(44)可知，存在 k1，k2 在
某一定范围时可保证 V
·
6 为负，保证整个系统稳定，使
AUV能跟踪期望轨迹，又由式(8)、(9)知 k1，k2 与 AUV
的 ud 和 αvd 有关，因 k1，k2 是有一定范围的，即 AUV的期
第 6期 李 娟 等:基于领航跟随法的多 AUV编队控制算法研究 241
图 1 AUV路径
Fig．1 The AUV path
图 2 广义路径距离
Fig．2 The generalized path distance
望速度也同样有范围 Umax ≥ ud ≥ Umin ≥ 0，而当速度 ud
在［Umin，Umax］区间内时，单体控制器能够保证 AUV轨迹
跟踪误差系统半全局一致有界，即对任意一个控制量使
轨迹跟踪误差趋于 0。
对于多 AUV的编队协调控制问题，从多个 AUV 中
选择 1 个作为领航者，其余为跟随者。领航者和跟随者
都能够跟踪其各自的路径。领航者以设定速度跟踪路
径，且设定速度在［Umin，Umax］区间内。通过计算领航者
和跟随者在路径上运动的距离，可知跟随者相对于领航
者的广义沿路距离误差为:
ΔS =
100 if PNl ＜ PNf
Sl － Sf － Se，f if PNl = PNf
－ Se，f if PNl ＞ PNf
{ (45)
式中:Se，f为领航者和跟随者保持期望队形时运动方向所
需的期望距离;PNl，PNf 分别为领航者和跟随者路径编
号 Sl，Sf 为领航者和跟随者在路径编号所在路径上走的
距离。
3. 2 领航者控制器设计
因领航者仅需路径跟踪控制，领航者控制器的建立
如下。
由式(8)和(9)可知，AUV的期望 ud，l 和 αvd，l 为:
ud，l = vp，lcos(ψe，l)－ k1，l xe，l /El
αvd，l = － vl － k2，l ye，l /El{ (46)
由式(21)和(41)，领航者 AUV滑模控制率为
T1，l = － c1，l(F
^
1，l + τu，l)－ F
·^
1，l － xe，l － m11，l ue，l －
ks1，lsgn(S1，l)－ ws1，l S1，l (47)
T2，l = － c2，l(F
^
3，l + τr，l)－ F
·^
3，l － m33，l re，l － m33，lQ
·
2，l －
ks2，lsgn(S2，l)－ ws2，l S2，l (48)
由式(23)，(31)和(41) ，领航者 AUV滑模自适应率
为:
F
·^
1，l = ue，l + c1，l S1，l
F
·^
2，l = αve，l
F
·^
3，l = re，l + c2，l S2，l





(49)
3. 3 跟随者控制器设计
跟随者控制器与领航者类似，但跟随者为了减少与
领航者之间的广义沿路距离误差，设计了新的不同于领
航者的速度要求，使得跟随者能够“追上”领航者。
跟随者的速度 ud，f 为:
ud，f = ud，l +
2 arctan ΔS
π
Δu (50)
其中，Δu ＞ 0并且保证 ud，f 在［Umin，Umax］区间里。
跟随者的期望 αvd，f 为:
αvd，f = － vf － k2，f ve，f /Ef (51)
同理，由式(21)和(41)，跟随者 AUV 滑模控制
率为:
T1，f = － c1，f(F
^
1，f + τu，f)－ F
·^
1，f － xe，f － m11，f ue，f －
ks1，fsgn(S1，f)－ ws1，f S1，f (52)
T2，f = － c2，f(F
^
3，f + τr，f)－ F
·^
3，f － m33，f re，f － m33，fQ
·
2，f －
ks2，fsgn(S2，f)－ ws2，f S2，f (53)
由式(23)、(31)和(41)，跟随者 AUV滑模自适应率
为:
F
·^
1，f = ue，f + c1，f S1，f
F
·^
2，f = αve，f
F
·^
3，f = re，f + c2，f S2，f





(54)
在上述控制器的作用下，领航者和跟随者都能跟踪
各自路径并慢慢形成稳定的编队队形，而且跟随者除了
知道领航者的位置信息和速度信息外，不需要其它信息，
大大减少了编队过程中的通讯需求。
242 仪 器 仪 表 学 报 第 4 0 卷
4 试验与分析
4. 1 仿真验证与分析
为了验证本文的控制方法是否能够实现多 AUV 的
编队队形控制，利用 MATLAB 编写程序，其中 AUV 水平
面模型参数为:
m= 185 kg，Iz = 50 kgm
2，X u = －30 kg，Yv = －80 kg，
Nr = －30 kg，Xu = 70 kg /s，Xu | u | = 100 kg /m，
Yv = 100 kg /s，Yv | v | = 200 kg /m，Nr = 50 kgm
2 /s，
Nr | r | = 100 kgm
2
m11 =m－X u，m22 =m－Yv，m33 = Iz －Zr，d11 = Xu +Xu | u |
| u |，d22 =Yv+Yv | v | | v |，d33 =Nr+Nr | r | | r |
控制器参数为:
k1 = 1. 7，k2 = 1，k3 = 0. 1，c1 = 0. 8，c2 = 1. 3，ks1 =
ks2 = 0. 5，ws1 = ws2 = 1
领航者的期望路径参数分别为 (0，0)(1 000，0)
(1 500，500) (1 000，1 000) (0，1 000) (0，0)的折线段。
领航者期望速度为 3 m /s，跟随者速度期望范围为(0，5)
m/s，期望编队队形为 3个 AUV构成等腰直角三角形，其
中领航者位于顶点，两个跟随者构成底边，底边长为
100 m。
3个 AUV初始位置均在(0，0)点，图 3 所示为 AUV
跟踪期望路径时的航迹、速度、艏向角、横向误差和纵向
误差，图 3(a)航迹图可以看出，跟随者在初始位置与编
队期望位置存在较大误差，但在设计的控制器的作用下，
跟随者能快速到达编队期望位置并且所有 AUV 能在运
动中保持编队队形。图 3(b)、(c)分别为 AUV的速度与
艏向角，图 3(b)可以看出领航者能在短时间内达到期望
速度并保持，跟随者达到期望速度慢于领航者，是因为 2
个跟随者的初始广义沿路距离 0小于期望广义沿路距离
50 m，跟随者需以低于领航者的速度运行;图 3(b)中跟
随者速度发生 4次变化和图 3(c)中 3 个 AUV 艏向角发
生 4次变化分别对应图 3(a)中编队控制中 4 次路径编
号发生变化，其它时刻 AUV的速度与艏向角均能保持其
期望值，证明了 AUV的反步滑模控制能精确跟踪上期望
路径;图 3(d)是 AUV路径跟踪时的误差，同时这也可以
看作编队控制时的横向误差，除了在初始时刻和 AUV 跟
踪路径时路径编号时改变有较大误差外，横向误差都接
近于 0，再次证明了单个 AUV路径跟踪能够精确控制，以
及 3个 AUV在编队控制下横向能保持期望距离。图 3
(e)是 AUV编队控制时跟随者与领航者的纵向误差，是
实际广义路径距离与期望广义路径距离的差值，同样，除
了在初始时刻和 AUV 跟踪路径时路径编号时改变有较
大误差外，纵向误差都接近于 0，说明了 3个 AUV在编队
控制下的纵向能保持期望距离。结合图 3(d)、(e)，3 个
AUV编队控制下横向与纵向均能保持期望距离，说明了
AUV能保持期望队形，证明了编队协调控制器能有效使
多个 AUV形成编队并保持。综上所述，仿真结果证明了
控制器的有效性，3 个 AUV 在控制器的作用下，领航者
能快速并稳定的跟踪期望路径，跟随者也能快速到达期
望编队位置，3个 AUV能够实现并保持编队队形运动的
目的。
第 6期 李 娟 等:基于领航跟随法的多 AUV编队控制算法研究 243
图 3 AUVs编队队形控制仿真实验结果
Fig．3 The AUVs formation control simulation experiment result
图 4 试验用多个 AUV图片
Fig．4 The photo of the AUVs used in the test
而值得说明的是，图 3(b)、(d)、(e)中速度变化以
及误差突然变大的原因是在编队过程中路径编号发生变
化，3个 AUV需要重新跟踪新的路径段，计算误差的过
程中路径段的参数发生改变，导致误差突变。
4. 2 湖上试验验证与分析
如图 4所示，采用 3 个 AUV 进行了湖上试验，验证
了本算法编队队形控制的有效性。单个 AUV 的推进系
统结构布置如图 5所示，需要设计 4 个控制器:分别为 X
位置控制器、Y 位置控制器、航向控制器和深度控制器。
4个控制器产生的推进器推力指令经推力解算与推力分
配模块，为每个推进器分配实际的推进器指令，用于单个
AUV的位置、航向、深度控制。
图 5 AUV执行机构结构布置图
Fig．5 The AUV actuator structure layout
试验区域面积较开阔、视野好，水底较平坦，湖区流速
不大于 1 kn、流向稳定，周围水域和岸边无明显持续的噪
声源，试验用 3条 AUV，其中 1个为领航者，另外 2个为跟
随者。首先通过水面工作站对领航 AUV 设计多边形路
径，期望队形为 3个 AUV构成等腰直角三角形，其中领航
者位于顶点，2个跟随者构成底边，边长 50 m。领航者期望
速度为 0. 6 m/s，期望轨迹参数为 4 个点(74，98)、(80，
107)、(80，－32)、(100，－40)构成的折线。控制器参数为:
k1 = 1. 7，k2 = 1，k3 = 0. 1，c1 = 0. 8，c2 = 1. 3，ks1 =
ks2 = 0. 5，ws1 = ws2 = 1
试验结束后下载 AUV 的数据，获得 AUV 实际运动
信息，包括速度，艏向角，AUV 位置信息等。试验结果如
图 6所示。
244 仪 器 仪 表 学 报 第 4 0 卷
图 6 AUVs编队湖上试验
Fig．6 The AUVs formation control test on the lake
根据图 6的试验结果可知，领航者能够在误差允许
下跟踪上所设计的路径，3 个 AUV 也能成功实现编队并
保持，整个编队运动过程中，误差均满足要求，证实了该
算法的有效性，也说明了该策略能有效应用到实际应
用中。
5 结 论
本文针对 AUV 编队控制，设置领航 AUV 的期望路
径和期望队形，使得每个 AUV 都有自己的路径，通过反
步法和滑模结合控制单个 AUV，使其能够跟踪期望路
径，通过跟随者 AUV的速度协调编队控制器使多个 AUV
形成期望编队队形并保持，把整个编队控制转化为速度
控制和姿态控制，提高了系统鲁棒性和自适应，满足多
AUV对速度，姿态，位置的要求。仿真实验和湖试表明:
本文方法能快速有效地实现编队队形控制，并解决传统
领航跟随法编队在折线切换时出现长时间编队重构并可
能相撞等问题。
参考文献
［1］ 钱东，赵江，杨芸． 军用 UUV 发展方向与趋势
(上)———美军用无人系统发展规划分析解读［J］． 水
下无人系统学报，2017(2):5-34．
QIAN D，ZHAO J，YANG Y，et al． Development trend
of military UUV (Ⅰ) :A review of U． S． military
unmanned system development plan［J］． Journal of
Unmanned Undersea Systems，2017(2) :5-34．
［2］ WYNN Ｒ B，HUVENNE V A I，BAS T P L，et al．
Autonomous underwater vehicles (AUVs) :Their past，
present and future contributions to the advancement of
marine geoscience［J］． Marine Geology，2014，352(2) :
451-468．
［3］ KHODAYAＲI M H， BALOCHIAN S． Modeling and
control of autonomous underwater vehicle (AUV) in
heading and depth attitude via self-adaptive fuzzy PID
controller［J］． Journal of Marine Science and Technology，
2015，20(3) :559-578．
［4］ 马利民． 欠驱动 AUV 全局无抖振滑模轨迹跟踪控
制［J］． 智能系统学报，2016，11(2):200-207．
MA L M． Global chattering－free sliding mode trajectory
tracking control of underactuated autonomous underwater
vehicles［J］． CAAI Transactions on Intelligent Systems，
2016，11(2) :200-207．
［5］ SAＲHADI P， NOEI A Ｒ， KHOSＲAVI A． Model
reference adaptive PID control with anti － windup
compensator for an autonomous underwater vehicle［M］．
North－Holland Publishing Co． 2016，83:87-93．
第 6期 李 娟 等:基于领航跟随法的多 AUV编队控制算法研究 245
［6］ 董早鹏，刘涛，万磊，等． 基于 Takagi-Sugeno模糊神经
网络的欠驱动无人艇直线航迹跟踪控制［J］． 仪器仪
表学报，2015，36(4):863-870．
DONG Z P，LIU T，WAN L，et al． Straight － path
tracking control of underactuated USV based on Takagi-
Sugeno fuzzy neural network［J］． Chinese Journal of
Scientific Instrument，2015，36(4) :863-870．
［7］ LIANG X，YOU Y，SU L，et al． Path following control
for underactuated auv based on feedback gain
backstepping［J］． Tehnicki Vjesnik，2015，22(4) :829-
835．
［8］ DAS B，SUBUDHI B，PATI B B． Cooperative formation
control of autonomous underwater vehicles:An overview
［J］． International Journal of Automation and Computing
2016，13(3) :199-225．
［9］ 易国，毛建旭，王耀南，等． 非完整移动机器人领航-
跟随编队分布式控制［J］． 仪器仪表学报，2017，
38(9):182-188．
YI G，MAO J X，WANG Y N，et al． Distributed control
for leader － follower formation tracking of multiple
nonholonomic vehicles［J］． Chinese Journal of Scientific
Instrument，2017，38(9) :182-188．
［10］ 雷艳敏，冯志彬，宋继红． 基于行为的多机器人编队
控制的仿真研究［J］． 长春大学学报，2008，18(8):
40-44．
LEI Y M，FENG ZH B，SONG J H． The simulation study
on formation control of multi － robot system based on
behaviors［J］． Journal of Changchun University，2008，
18(8) :40-44．
［11］ CUI Ｒ X，YAN W S，XU D M，et al． Formation control
of AUV based on virtual reference point［J］． Fire Control
＆ Command Control，2008(10) :53-57．
［12］ PENG Z，WANG D，WANG H， et al． Coordinated
formation pattern control of multiple marine surface
vehicles with model uncertainty and time－varying ocean
currents［J］． Neural Computing and Applications，2014，
25(7-8) :1771-1783．
［13］ BOＲHAUG E，PAVLOV A，PETTEＲSEN K Y． Integral
LOS control for path following of underactuated marine
surface vessels in the presence of constant ocean
currents［C］． 47th IEEE Conference on Decision and
Control． 2008:4984-4991．
［14］ CAHAＲIJA W，PETTEＲSEN K Y，GＲAVDAHL J T，
et al． Integral LOS guidance for horizontal path following
of underactuated autonomous underwater vehicles in the
presence of vertical ocean currents［C］． American Control
Conference (ACC)． 2012:5427-5434．
［15］ 张伟，郁晨曦，滕延斌，等． 基于模型预测控制的 UUV
路径跟踪控制研究［J］． 仪器仪表学报，2017(11):
2659-2666．
ZHANG W，YU CH X，TENG Y B，et al． Ｒesearch on
UUV path tracking control based on model predictive
control［J］． Chinese Journal of Scientific Instrument，
2017(11) :2659-2666．
［16］ FOSSEN T I． Handbook of marine craft hydrodynamics
and motion control［M］． Norwegian University of Science
and Technology，2011:248-252．
［17］ ZHANG W，TENG Y，WEI S L，et al． The robust H－
infinity control of UUV with Ｒiccati equation solution
interpolation ［J］． Ocean Engineering， 2018， 156:
252-262．
［18］ 徐健，汪慢，乔磊，等． 欠驱动 UUV 三维轨迹跟踪的
反步动态滑模控制［J］． 华中科技大学学报(自然科学
版)，2015(8) :107-113．
XU J， WANG M， QIAO L， et al． Backstepping
dynamical sliding mode controller for three－dimensional
trajectory tracking of underactuated UUV［J］． Journal of
Huazhong University of Science and Technology 2015
(8) :107-113．
［19］ 普湛清，王巍，张扬帆，等． UUV 平台 OFDM 水声通
信时变多普勒跟踪与补偿算法［J］． 仪器仪表学报，
2017(7):1634-1644．
PU ZH Q，WANG W，ZHANG Y F，et al． Time－variant
doppler tracking and compensation in underwater acoustic
OFDM communication for UUV platform［J］． Chinese
Journal of Scientific Instrument，2017(7) :1634-1644．
［20］ 王宏健，李村，么洪飞，等． 基于高斯混合容积卡尔曼
滤波的 UUV自主导航定位算法［J］． 仪器仪表学报，
2015，36(2):254-261．
WANG H J，LI C，YAO H F，et al． Gaussian mixture
cubature Kalman filter based autonomous navigation and
localization algorithm for UUV［J］． Chinese Journal of
Scientific Instrument，2015，36(2) :254-261．
246 仪 器 仪 表 学 报 第 4 0 卷
作者简介
李娟，2000年于哈尔滨工程大学获得学
士学位，2003年于哈尔滨工程大学获得硕士
学位，2008年于哈尔滨工程大学获得博士学
位，现为哈尔滨工程大学副教授，主要研究
方向为船舶智能控制。
E-mail:lijuan041@ hrbeu．edu．cn
Li Juan received her B．Sc． degree in 2000，M．Sc． degree in
2003 and Ph． D． degree in 2008 all from Harbin Engineering
University． Now， she is an associate professor in Harbin
Engineering University． Her main research interest includes ship
intelligent control．
袁锐锟，2017年于哈尔滨工程大学获得
学士学位，现为哈尔滨工程大学硕士研究
生，主要研究方向为自主水下机器人智能控
制和编队控制策略。
E-mail:yuanruikun@ hrbeu．edu．cn
Yuan Ｒuikun received his B．Sc． degree in 2017 from Harbin
Engineering University;now，he is a M．Sc． candidate in Harbin
Engineering University． His main research interest includes
intelligent control and formation control of AUVs．
张宏瀚(通信作者)，2005 年于哈尔滨
工程大学获得学士学位，2008年于哈尔滨工
程大学获得硕士学位，2010年于哈尔滨工程
大学获得博士学位，现为哈尔滨工程大学讲
师，主要研究方向为水下无人航行器智能控
制和水下无人系统优化设计。
E-mail:oceanzhh@ hrbeu．edu．cn
Zhang Honghan (Corresponding author)received his B． Sc．
degree in 2005，M．Sc． degree in 2008 and Ph．D． degree in 2010
all from Harbin Engineering University． Now，he is a lecturer in
Harbin Engineering University． His main research interest
includes intelligent control of UUV and optimal design of
underwater unmanned system．

欢迎咨询51作业君