2024年7月11日发(作者：诸葛波鸿)
２０１３年７月
系统仿真技术
Ｓｙｓｔｅｍ Ｓｉｍｕｌａｔｉｏｎ Ｔｅｃｈｎｏｌｏｇｙ
Ｊｕ１．，２０１３
Ｖｏ１．９，Ｎｏ．３
第９卷第３期
中图分类号：ＴＰ ５１０・８０ 文献标识码：Ａ
基于领航位置信息的ＡＵＶ三维编队控制方法
朱大奇，杜青
（上海海事大学水下机器人与智能系统实验室，上海 ２０１３０６）
摘要：研究了自治水下机器人（Ａｕｔｏｎｏｍｏｕｓ Ｕｎｄｅｒｗａｔｅｒ Ｖｅｈｉｃｌｅ，ＡＵＶ）三维环境中编队控制问题，应用领航一跟随式
队形控制方法，仅利用领航者的位置信息及期望编队队形得到虚拟机器人的航行轨迹及速度信息，作为跟随者的航
行参考量，应用反步及滑模控制方法为跟随者设计自适应控制律，使其轨迹收敛于虚拟机器人的轨迹，从而与领航
者保持期望位姿关系。随后，在具体ＡＵＶ动力学模型上，利用ＭＡＴＬＡＢ／ＳＩＭＵＬＩＮＫ平台进行了编队控制的仿真研
究，实现了预期的控制效果，验证了算法的有效性及实用性。
关键词：自治水下机器人；编队控制；领航一跟随式；虚拟机器人；自适应控制律
Ｔｈｒｅｅ－ｄｉｍｅｎｓｉｏｎａＩ Ｆｏｒｍａｔｉｏｎ ＣｏｎｔｒｏＩ Ｍｅｔｈｏｄ ｆｏｒ ＡＵＶ
Ｂａｓｅｄ ｏｎ Ｐｉｌｏｔ Ｌｏｃａｔｉｏｎ ＩｎｆＯｒｍａｔｉｏｎ
ＺＨＵ Ｄａｑｉ，ＤＵ Ｑｉｎｇ
（Ｌａｂｏｒａｔｏｒｙ ｏｆ Ｕｎｄｅｒｗａｔｅｒ Ｖｅｈｉｃｌｅｓ ａｎｄ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍ，Ｓｈａｈｇｈａｉ Ｍａｒｉｔｉｍｅ Ｕｎｉｖｅｒｓｉｔｙ，Ｓｈａｈｇｈａｉ ２０１３０６，Ｃｈｉｎａ）
Ａｂｓｔｒａｃｔ：Ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｐｒｏｂｌｅｍ ｏｆ ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅｓ（ＡＵＶ）ｉｎ ｔｈｒｅｅ—ｄｉｍｅｎｓｉｏｎａｌ
ｕｎｄｅｒｗａｔｅｒ ｅｎｖｉｒｏｎｍｅｎｔ ｉｓ ｄｉｓｃｕｓｓｅｄ ｉｎ ｔｈｉｓ ｐａｐｅｒ．Ｌｅａｄｅｒ—ｆｏｌｌｏｗｅｒ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｍｅｔｈｏｄ ｉｓ ａｐｐｌｉｅｄ．
Ｒｅｆｅｒｅｎｃｅ ｔｒａｊｅｃｔｏｒｙ ａｎｄ ｓｐｅｅｄ ｉｎｆｏｒｍａｔｉｏｎ ｏｆ ｆｏｌｌｏｗｅｒ，ｎａｍｅｌｙ ｖｉｒｔｕａｌ ｖｅｈｉｃｌｅ，ｉｓ ｏｂｔａｉｎｅｄ ｂａｓｅｄ ｏｎ ｔｈｅ
ｌｅａｄｅｒ ｐｏｓｉｔｉｏｎ ａｎｄ ｐｒｅｄｅｔｅｒｍｉｎｅｄ ｆｏｒｍａｔｉｏｎ ｏｎｌｙ．Ｔｈｅｎ ｄｙｎａｍｉｃ ａｄａｐｔｉｖｅ ｖａｒｉａｂｌｅ ｓｔｒｕｃｔｕｒｅ ｃｏｎｔｒｏｌ ｌａｗ ｉｓ
ｄｅｓｉｇｎｅｄ ｆｏｒ ｔｈｅ ｆｏｌｌｏｗｅｒ ｕｓｉｎｇ ｂａｃｋｓｔｅｐｐｉｎｇ ｓｙｎｔｈｅｓｉｓ ａｎｄ ｓｌｉｄｉｎｇ ｍｏｄｅ ｃｏｎｔｒｏｌ ｍｅｔｈｏｄ．Ｔｈｅ ｔｒａｊｅｃｔｏｒｙ ｏｆ
ｔｈｅ ｆｏｌｌｏｗｅｒ ｃａｎ ｃｏｎｖｅｒｇｅ ｔｏ ｔｈｅ ｖｉｒｔｕａｌ ｖｅｈｉｃｌｅ ｔｒａｊｅｃｔｏｒｙ ｗｉｔｈ ｔｈｅ ｃｏｎｔｒｏｌ ｌａｗ．Ｔｈｕｓ ｉｔ ｍａｉｎｔａｉｎｓ ｅｘｐｅｃｔｅｄ
ｒｅｌａｔｉｏｎｓｈｉｐ ｗｉｔｈ ｔｈｅ ｌｅａｄｅｒ．Ｔｈｅｎ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｓｉｍｕｌａｔｉｏｎ ｉｓ ｉｍｐｌｅｍｅｎｔｅｄ ｉｎ ｓｐｅｃｉｆｉｃ ＡＵＶ ｄｙｎａｍｉｃｓ
ｍｏｄｅｌ ｗｉｔｈ ＭＡＴＬＡＢ／ＳＩＭＵＬＩＮＫ ｐｌａｔｆｏｒｍ．Ｆｉｎａｌｌｙ，ｔｈｅ ｅｘｐｅｃｔｅｄ ｃｏｎｔｒｏｌ ｅｆｆｅｃｔ ｉｓ ｒｅａｌｉｚｅｄ ｗｈｉｃｈ
ｖｅｒｉｆｉｅｄ ｔｈｅ ｅｆｆｅｃｔｉｖｅｎｅｓｓ ａｎｄ ｐｒａｃｔｉｃａｂｉｌｉｔｙ ｏｆ ｔｈｅ ｐｒｏｐｏｓｅｄ ａｌｇｏｒｉｔｈｍ．
Ｋｅｙ ｗｏｒｄｓ：ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅ；ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ；ｌｅａｄｅｒ—ｆｏｌｌｏｗｅｒ；ｖｉｒｔｕａｌ ｖｅｈｉｃｌｅ；
ａｄａｐｔｉｖｅ ｃｏｎｔｒｏｌ ｌａｗ
限，往往不能完成一些复杂的任务，并且多ＡＵＶ
１ 引 言
自治水下机器人在海洋开发与探索中发挥
的协作更可增加其安全性以及整个系统的鲁棒
性，多ＡＵＶ的协同控制已经成为ＡＵＶ研究领域
的热点。编队控制是多ＡＵＶ协作的一个典型性
问题，也是研究其他协作问题的基础，在科学考
着十分重要的作用，由于单个ＡＵＶ的工作能力有
基金项目：国家自然科学基金资助项目（５１０７５２５７，５１２７９０９８）；上海市教委科研创新研究重点资助项目（１３ＺＺ１２３）
１９４ 系统仿真技术 第９卷第３期
察、深海探测、沉船打捞、旅游探险等领域有广泛
的应用，对于编队控制问题的研究有重要的现实
意义 一 。
２．２领航一跟随式编队控制
领航一跟随式编队控制过程需要设定领航
ＡＵＶ的行为，并且设计虚拟ＡＵＶ作为期望参考
量，三维空间位置关系如图１所示。
编队控制是指多个机器人在到达目的地的
过程中，保持某种队形，同时又要适应环境约束
的控制技术。目前，编队控制方法大致有基于行
为法、人工势场法、虚拟结构法、领航一跟随法、分
布式控制法等 ］，其中领航一跟随方法又称主从
式编队方法，仅仅给定领航者的行为，为跟随者
设计跟踪控制律就可以控制整个机器人群体的
行为，简化了队形问的合作问题。考虑到水下环
境通讯带宽的限制及水下噪声的干扰等实际情
况，希望能够减少ＡＵＶ间的交互信息量，因此在
领航一跟随式编队控制方法中，仅利用领航者的
位置信息 ，而不利用其速度及角速度信息，得
到跟随者的期望轨迹及速度，据此设计跟踪控制
律。由于滑模控制不需要精确的数学模型及抗
干扰能力强，而广泛应用于ＡＵＶ的控制系统
中 ，对此本文给出了三维环境中ＡＵＶ主从式
编队控制自适应滑模控制算法，并将该控制算法
应用到具体ＡＵＶ动力学模型中，得到推进器推力
值，验证了算法的有效性及实用性。
２ 问题描述
２．１ ＡＵＶ运动学与动力学模型
通常将ＡＵＶ的坐标系统分成惯性坐标系（Ｅ—
ＸＹＺ）和载体坐标系（０一Ｘ。ｙ０Ｚ。）两类 ，三维空
问中完整运动学表示为
Ｍｙ＋ｃ（１，） ＋Ｄ（１，）ｌ，＋ｇ（叼）＝ （１）
叼＝Ｊ（叼）ｌ， （２）
７＝ＢＴ （３）
其中：Ｍ，ｃ，Ｄ，ｇ， ∈Ｒ¨。分别为惯性矩阵、离心
项和科氏矩阵、阻尼矩阵、静力项（重力与浮力合
力）、ＡＵＶ作用力与力矩；ｒ＝［ 口ｗ ｐ ｑ ｒ］ ，为广
义速度向量； ＝［ Ｙ ｚ ］ ，为广义位置向
量；．，（’，）为转换矩阵；Ｂ是适当维度的推力配置
矩阵； 是各个推进器的推力项 。
图１三维空间ＡＵＶ位置关系
Ｆｉｇ．１ ３Ｄ ｓｐａｃｅ ＡＵＶ ｐｏｓｉｔｉｏｎ ｒｅｌａｔｉｏｎｓ
图１中，领航ＡＵＶ、虚拟ＡＵＶ、跟随ＡＵＶ和
领航ＡＵＶ在跟随ＡＵＶ所在平面投影，分别用
， ，
Ａ ，Ａ ，表示，编队任务描述为：Ａ 与 空
间距离ｄ，水平面观望角度 为Ａ Ａ ，与Ａ 线速
度矢量耽 的夹角，垂直方向观望角度孝为Ａ Ａ
与Ａ Ａ ，的夹角。若某一时刻领航ＡＵＶ的广义
位置为叼 ，得出虚拟ＡＵＶ的位置叼 ，对于Ａ ，只
要设计出合理的控制输入ｒ ，得到ＡＵＶ各推进
器推力值，使得ｌｉｍ ｌ叼 一７７ Ｉ＝Ｏ，即可保证Ａ 正
确路径。
在ｎ维ＡＵＶ系统中，将ｎ维ＡＵＶ组分为ｎ一１
个子系统，各子系统包括２个ＡＵＶ（领航ＡＵＶ，跟
随ＡＵＶ），如图２所示。
图２ 三维空间领航一跟随编队控制模式
Ｆｉｇ．２ ３Ｄ ｓｐａｃｅ ｌｅａｄ・ｆｏｌｌｏｗ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｍｏｄｅ
朱大奇，等：基于领航位置信息的ＡＵＶ三维编队控制方法 １９５
３ ＡＵＶ推进器配置及推力分配
本文中仿真模型为上海海事大学水下机器人
与智能系统实验室海筝号ＡＵＶ模型，如图３所示。
该机器人一共包含４个推进器，水平面２个推进器
和垂直面２个推进器，无侧向推进器，具体推进器
布置如图４所示（ＴＨ表示推进器ｔｈｒｕｓｔｅｒ）。
图３ ＡＵＶ结构
Ｆｉｇ．３ ＡＵＶ ｓｔｒｕｃｔｕｒｅ
一
图４推进器布置
Ｆｉｇ．４ Ｔｈｒｕｓｔｅｒ ａｒｒａｎｇｅｍｅｎｔ
针对该ＡＵＶ推进器的布置，将ＡＵＶ完整动
力学方程进行简化解耦工作，考虑ＡＵＶ空间中３
自由度运动：进退（ｓｕｒｇｅ），回转（ｙａｗ）和潜浮
（ｈｅａｖｅ）。因此，惯性坐标及载体坐标系下ＡＵＶ
状态为 ＝［ Ｙ ］ 和ｌ，＝［ ｒ］ 。推算出海
筝号ＡＵＶ空间中各个自由度上产生的合力与合
力矩和推进器的关系¨ ：
［ ］＝［三一 三蚤］ （４）
其中：［ｒ ］ 是作用于ＡＵＶ重心的各个自
由度上的合力与合力矩；Ｋ为常数；Ｒ为两个垂直
推进器之间的距离；［ ］ 是水平
面布置的４个推进器的推力。
归一化推力如式（５）所示：
１ ｌ
一 一
０ ０
２ ２
１ １
０ ０ 一一
２ ２
ｌ １
一 一
一 一一
０ ０
２ ２
１
２
ｆ ｚ
＝
Ｂ・
§Ｊｒ＝ＢＴ （５）
３
ｒＮ
一
ｒ为推力与力矩 的归一化形式，一１≤ｒ ≤
一 一
１，ｉ＝Ｘ，Ｚ，Ｎ， 为推进器推力值 的归一化形
式，一１≤Ｔ ≤１，ｉ＝１，２，３，４。 ｘ＾ｆ，．『 ，ｒⅣＭ表示各
个自由度上最大合力与合力矩。
４虚拟机器人设计
根据领航位置信息及编队任务的要求，得到
期望轨迹：
叼ｄ＝田 ＋．，（叼 ）Ｚ （６）
其中：ｚ＝［ｄ ｓｉｎ ＣＯＳ ，ｄ ｓｉｎ ＣＯＳ ，ｄ ＣＯＳ ，０］ ；
叼 为领航机器人的广义位置；ｄ， 为编队航行过
程中设定的常数。
设虚拟机器人广义位置量为叼 ，定义虚拟机
器人误差量：ｅ ＝叼 一 ∈Ｒ ，定义辅助误差
变量 ¨ ：
一
１９６ 系统仿真技术 第９卷第３期
， ＝ｅ ＋ （７）
其中：ｃｐ：［ 。， ， ，， ｒ，定义其导数：
＝一
５跟随ＡＵＶ编队控制律
（８） （ ）一Ｋｒ
定义虚拟机器人的速度值为：
ｌ， ＝Ｊ （叩 ）（ ，（ ）＋ （ ）） （９）
编队控制的目标是为跟随ＡＵＶ各推进器设
计合适的推力值作为输入量，控制跟随ＡＵＶ的速
度和角速度量，使其能够跟踪虚拟ＡＵＶ的轨迹，
即误差ｅ 收敛于０。
ｅ ／ｆ一７／７ 【ｅ ｅ ｅ：ｅｑｒ ，ｊ］Ｔ
式中：Ｋ＝ｄｉａｇ［ｊｃ１，ｋ２，ｊｃ３， ４］，Ｊｌｃｆ，Ａ ∈Ｒ ； １（ ）＝
［Ａ１ｔａｎｈ（ ｌ／Ａ１），Ａ２ｔａｎｈ（ ２／Ａ ２），Ａ ３ｔａｎｈ（ ３／Ａ ３），
Ａ４ｔａｎｈ（ ４／Ａ４）］ ； ２（ ）＝［ｋｌｔａｎｈ（ １／ｋ１）， ２ｔａｎｈ
（ ２／ｋ２），ｋ３ｔａｎｈ（ ３／ｋ３），ｋ４ｔａｎｈ（ ４／ｋ４）］。， （０）＝
０，ｉ＝１，２，３，４。
［ ｆ— Ｙｆ—Ｙ ｚｆ—ｚ ｆ一 ］ （１０）
系统控制结构如图５所示：
图５控制系统结构框图
Ｆｉｇ．５ Ｃｏｎｔｒｏｌ ｓｙｓｔｅｍ ｓｔｒｕｃｔｕｒｅ ｄｉａｇｒａｍ
ｅ 一后 ｅ；＝一ｋｅ 一ｋｅ；一
：
ｅ 一 ｅ２ ≤０ （１３）
因此系统是一致稳定的 Ｈ 。
５．２ 自适应滑模动力学控制器
将速度误差值ｅ。＝ｌ，。一ｌ，ｆ＝［ｅ ｅ ｅ，］ 作
为滑模控制器的输入，选择滑模面 ：
ｌ， ［Ｉ Ｌ兰ｒ （｝ ＋ ：ｅ： ＋ｅｙｓｉｎ ｆ ＋‘ Ｖｃ。ｓ ｅ 一 Ｖｓｉｎ ｅ ］ｊＩ
＝
＝ ＋２Ａｅ ＋Ａ Ｉｅ
＝ｅ ＋２Ａｅ ＋Ａ ｅ ＝ｅ ＋
（１４）
对等式（１４）求导，得到
２Ａ（ 一 ）＋Ａ ｅ （１５）
系统运行于滑模面时，式（１５）为零
． ．
１
Ｐ
ｃ
＋２以（ ｃ一 （ 一ｃ —
Ｄ１，ｆ—ｇ））＋Ａ ｅ ＝０ （１６）
＝了
１
ｅ ＋了１
２＋
＋
ｅ
２ ＋
ｅ
２ ＋
ｅ
２
＝＋ 。 ２
。
１
１
（１２）（ ２）
等价控制律为
ｅ
＝ ＋
＋ ＋
Ｃ ｆ＋Ｄ ｆ＋ｇ （１７）
朱大奇，等：基于领航位置信息的ＡＵＶ三维编队控制方法 １９７
加入自适应控制项取代切换项，得
＝
郴＋
（１８）
其中： ｒ 是用来估计未知量ｒ 的自适应变化
项。未知的不确定量 满足以下更新率：
。
：Ｆｓ （１９）
最终得到完整的控制律：
＋ ＋（ ＋ ） （２０）
６仿真实验
仿真过程中，设定３个机器人，分别为Ａ。，
Ａ ，Ａ：，其中Ａ。为领航ＡＵＶ，Ａ ，Ａ：分别为左、右
跟随ＡＵＶ，设定Ａ。的航行轨迹为：７／ ＝
［ Ｙ ｌ ］ 。，其中， （ｔ）＝２０×ｓｉｎ（０．５×ｔ）；
Ｙ （ｔ）＝２０×ＣＯＳ（０．５× ）；ｚ （ｔ）＝ｔ； （ｔ）＝０．５
ｘ ｔ。Ａ。Ａ 与Ａ。空间距离为ｄ＝４，观测角度 ＝
一
，’
÷，
叶
Ａ。与Ａ。的观测角度为 ＝÷订，
Ｊ
Ａ。与Ａ 的
４
观测角度为Ｘ ＝÷１Ｔ，形成空间Ｖ形编队队形。
ｊ
选择参数和增益为Ａ，＝２，Ａ ＝１，Ａ ＝１，Ａ
＝
０．５，ｋ ＝２，ｋ２＝１，ｋ３＝１，ｋ ＝０．５，设定ｔ从０时
刻开始运行，Ａ。，Ａ：的起始位置分别为 。（０）＝
［一１０—１５ ０ ０］，７／ （０）＝［一５—２０ ０ ０］，仿真结
果如图６所示。
２０
１５
１０
Ｎ
５
０
２
图６螺旋线编队控制轨迹
Ｆｉｇ．６ Ｓｐｉｒａｌ ｌｉｎｅ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｔｒａｊｅｃｔｏｒｙ
当Ａ。沿着螺旋轨迹航行时，据编队任务要
求，各时刻Ａ ，Ａ 的误差值，如图７和图８所示，
经计算与理论值相符
。
一
２
Ｉ ｉ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ
Ｏ ２ ４ ６ ８ １０ １２ １４ １６ ｌ８ ２０
２
。
一
０ ２ Ｌ——▲——Ｊ———Ｌ——Ｊ———Ｌ—— ——Ｊ———Ｌ——＿Ｌ—＿Ｊ
０ ２ ４ ６ ８ １０ １２ １４ １６ １８ ２０
０．１ ｒ ｔｉｍｅ
．。．
０ ￣Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ［１
０ ２ ４ ６ ８ ｌ０ １２ １４ １６ １８ ２０
墨．０．０５ ｒ ｔｉｍｅ
吾
耄１．。 ．
０ ２ ４
二二＝二＝＝＝＝
６ ８ ｌ０ １２ １４ １６ １８ ２０
图７ Ａ。位置误差量
Ｆｉｇ．７ Ｐｏｓｉｔｉｏｎ ｅｒｒｏｒ ｏｆ Ａ１
—
ｌ ＝＝＝＝二＝二二＝二＝
８ ５ ｒ ｔｉｍｅ
曼．．０ —ｉ Ｉ Ｉ Ｉ—Ｉ ｉ ｉ Ｉ Ｉ
Ｏ．１ ｒ ｔ ｍ。
０
＿０＿
１ Ｉ ＩＩ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ
０ ２ ４ ６ ８ １０ １２ １４ １６ １８ ２０
ｄ
０． ０５ ｔｉｍ ｅ
．。．
图８ Ａ 位置误差量
Ｆｉｇ．８ Ｐｏｓｉｔｉｏｎ ｅｒｒｏｒ ｏｆ Ａ２
根据Ａ。航行轨迹及编队任务要求，得到Ａ
和Ａ：的期望位置信息叼 和叼： ，以Ａ。为例进行
说明，由Ａ 期望位置信息 ，得到虚拟ＡＵＶ速
度值ｌ， ＝［ｕ Ｗ ｒ ］Ｔ，结合式（２）得到虚拟ＡＵＶ
的位置信息Ｊ，７ ，与Ａ 实际航行轨迹比较得到误
差量，将ｌ， 和叼 作为反步控制器的输人，得到Ａ
辅助速度值ｌ， ＝［“ ｒ ］ ，将其ｌ， 作为滑模控
制器输入生成速度控制的力和力矩，得到归一化
推力值Ｔ＝［ ， ］ ，如图９所示。将
推力作用到Ａ ，产生实际速度值如图１０所示，实
现期望的编队控制。
ｌ９８ 系统仿真技术 第９卷第３期
１．０
０．５
０
０ ２ ４ ６ ８ １０ １２ １４ １６ １８ ２Ｏ
１．０
ｔｉｍｅ
０．５
０
０ ２ ４ ６ ８ １０ １２ ｌ４ １６ １８ ２０
１
ｔｉｍｅ
０
—
１
０ ２ ４ ６ ８ １０ ｌ２ １４ １６ １８ ２０
１
ｔｉｍｅ
０
—
１
０ ２ ４ ６ ８ １０ １２ １４ １６ １８ ２０
ｔｉｍｅ
图９ Ａ。推进器推力值
Ｆｉｇ．９ Ｔｈｒｕｓｔｅｒ ｔｈｒｕｓｔ ｖａｌｕｅ
２０
１０
０
２ ４ ６ ８ １０ ｌ２ １４ １６ １８ ２０
２
ｔｉｍｅ
ｌ
Ｏ
２ ４ ６ ８ １０ １２ ｌ４ ｌ６ ｌ８ ２Ｏ
１．Ｏ
ｔｉｍｅ
ｋ Ｏ．５
０ ２ ４ ６ ８ １０ １２ １４ １６ １８ ２０
ｔｉｍｅ
图ｌ０ Ａ。速度值
Ｆｉｇ．１０ Ｖｅｌｏｃｉｔｙ ｏｆ Ａｌ
７ 总结
本文应用主从式编队控制方法，研究了三
维环境中多ＡＵＶ的编队控制问题，仅利用领航
ＡＵＶ的位置信息，结合编队任务要求，设计虚拟
ＡＵＶ，得到跟随ＡＵＶ的参考轨迹及速度值，在运
动学反步控制的基础上，应用变结构滑模控制
方法得到跟随ＡＵＶ的自适应变结构控制律。随
后将该算法应用到具体ＡＵＶ模型上，对领航
ＡＵＶ螺旋线航行轨迹进行仿真。仿真结果表
明，跟随ＡＵＶ能够与领航ＡＵＶ保持期望的位姿
关系，实现预期编队队形，验证了算法的有效性
与实用性。
参考文献：
［１］ Ｓｔｉｌｗｅｌｌ Ｄ Ｊ，Ｂｉｓｈｏｐ Ｂ Ｅ．Ｐｌａｔｏｏｎｓ ｏｆ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅ
［Ｊ］．ＩＥＥＥ Ｃｏｎｔｒｏｌ Ｓｙｓｔｅｍ Ｍａｇａｚｉｎｅ，２０００，２０（８）：４５
—
５２．
［２］Ｃｕｉ Ｒ，Ｓａｍ Ｇｅ Ｓ，Ｖｏｏｎ Ｅｅ Ｈｏｗ Ｂ，ｅｔ ａ１．Ｌｅａｄｅｒ—ｆｏｌｌｏｗｅｒ
ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｏｆ ｕｎｄｅｒａｃｔｕａｔｅｄ ａｕｔｏｎｏｍｏｕｓ
ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅｓ［Ｊ］．Ｏｃｅａｎ Ｅｎｇｉｎｅｅｒｉｎｇ，２０１０，３７
（１７）：１４９１—１５０２．
［３］杨甜甜，刘志远，陈虹，等．移动机器人编队控制的现
状与问题［Ｊ］．智能系统学报，２００７，２（４）：２１—２７．
ＹＡＮＧ Ｔｉａｎｔｉａｎ，ＬＩＵ Ｚｈｉｙｕａｎ，ＣＨＥＮ Ｈｏｎｇ，ｅｔ ａ１．
Ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｏｆ ｍｏｂｉｌｅ ｒｏｂｏｔｓ：ｓｔａｔｅ ａｎｄ ｏｐｅｎ
ｐｒｏｂｌｅｍｓ［Ｊ］．Ｃａａｉ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ，
２００７，２（４）：２１—２７．
［４］ 崔荣鑫，徐德民，严卫生，等．仅利用位置信息的自主
水下航行器主从式编队控制方法［Ｊ］．兵工学报，
２００８，２９（８）：９８０—９８４．
ＣＵＩ Ｒｏｎｇｘｉｎ，ＸＵ Ｄｅｍｉｎ，ＹＡＮ Ｗｅｉｓｈｅｎｇ，ｅｔ ａ１．Ｌｅａｄｅｒ—
ｆｏｌｌｏｗｅｒ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｏｆ ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｒｗａｔｅｒ
ｖｅｈｉｃｌｅｓ ｕｓｉｎｇ ｐｏｓｉｔｉｏｎ ｉｎｆｏｒｍａｔｉｏｎ ｏｎｌｙ［Ｊ］．Ａｃｔａ
Ａｒｍａｍｅｎｔａｒｉｉ，２００８，２９（８）：９８０—９８４．
［５］Ｐａｎ Ｍ Ｌ，Ｓｅｏｋ Ｗ Ｈ，Ｙｏｎｇ Ｋ Ｌ，ｅｔ ａ１．Ｄｉｓｃｒｅｔｅ—ｔｉｍｅ
ｑｕａｓｉ—ｓｌｉｄｉｎｇ ｍｏｄｅ ｃｏｎｔｒｏｌ ｏｆ ａｎ ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｒｗａｔｅｒ
ｖｅｈｉｃｌｅ［Ｊ］．ＩＥＥＥ Ｊｏｕｒｎａｌ ｏｆ Ｏｃｅａｎｉｃ Ｅｎｇｉｎｅｅｒｉｎｇ，１ ９９９，
２４（３）：３８８—３９５．
［６］ Ｓｅｒｄａｒ Ｓ，Ｂｒａｄｌｅｙ Ｊ Ｂ，Ｒｏｎ Ｐ Ｐ．Ａ ｃｈａｔｔｅｒｉｎｇ－ｆｒｅｅ ｓｌｉｄｉｎｇ—
ｍｏｄｅ ｃｏｎｔｒｏｌｌｅｒ ｆｏｒ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅｓ ｗｉｔｈ ｆａｕｌｔ．
ｔｏｌｅｒａｎｔ ｉｎｆｉｎｉｔｙ—ｎｏｒｕｌ ｔｈｒｕｓｔ ａｌｌｏｃａｔｉｏｎ［Ｊ］．Ｏｃｅａｎ
Ｅｎｇｉｎｅｅｒｉｎｇ，２００８，３５（１６）：１６４７—１６５９．
［７］ 朱大奇，李伟崇，颜明重，等．基于Ｄ—ｓ信息融合的
水下机器人地图构建［Ｊ］．系统仿真技术，２０１２，８
（３）：１８１—１８６．
ＺＨＵ Ｄａｑｉ，ＬＩ Ｗｅｉｃｈｏｎｇ，ＹＡＮ Ｍｉｎｇｚｈｏｎｇ，ｅｔ ａ１．Ｍａｐ
ｂｕｉｌｄｉｎｇ ｆｏｒ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅ ｂａｓｅｄ ｏｎ Ｄ—Ｓ ｉｎｆｏｒｍａｔｉｏｎ
ｆｕｓｉｏｎ ａｌｇｏｒｉｔｈｍ［Ｊ］．Ｓｙｓｔｅｍ Ｓｉｍｕｌａｔｉｏｎ Ｔｅｃｈｎｏｌｏｇｙ，
２０１２，８（３）：１８１—１８６．
［８］刘健，于闯，刘爱民．无缆自治水下机器人控制方法研
究［Ｊ］．机器人，２００４，２６（１）：７—１０．
ＬＩＵ Ｊｉａｎ，ＹＵ Ｃｈｕａｎｇ，ＬＩＵ Ａｉｍｉｎ．Ｒｅｓｅａｒｃｈ ｏｎ
ｕｎｔｅｔｈｅｒｅｄ ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅ ｃｏｎｔｒｏｌ
ｍｅｔｈｏｄ［Ｊ］．Ｒｏｂｏｔ，２００４，２６（１）：７—１０．
（下转第２１２页）
2024年7月11日发(作者：诸葛波鸿)
２０１３年７月
系统仿真技术
Ｓｙｓｔｅｍ Ｓｉｍｕｌａｔｉｏｎ Ｔｅｃｈｎｏｌｏｇｙ
Ｊｕ１．，２０１３
Ｖｏ１．９，Ｎｏ．３
第９卷第３期
中图分类号：ＴＰ ５１０・８０ 文献标识码：Ａ
基于领航位置信息的ＡＵＶ三维编队控制方法
朱大奇，杜青
（上海海事大学水下机器人与智能系统实验室，上海 ２０１３０６）
摘要：研究了自治水下机器人（Ａｕｔｏｎｏｍｏｕｓ Ｕｎｄｅｒｗａｔｅｒ Ｖｅｈｉｃｌｅ，ＡＵＶ）三维环境中编队控制问题，应用领航一跟随式
队形控制方法，仅利用领航者的位置信息及期望编队队形得到虚拟机器人的航行轨迹及速度信息，作为跟随者的航
行参考量，应用反步及滑模控制方法为跟随者设计自适应控制律，使其轨迹收敛于虚拟机器人的轨迹，从而与领航
者保持期望位姿关系。随后，在具体ＡＵＶ动力学模型上，利用ＭＡＴＬＡＢ／ＳＩＭＵＬＩＮＫ平台进行了编队控制的仿真研
究，实现了预期的控制效果，验证了算法的有效性及实用性。
关键词：自治水下机器人；编队控制；领航一跟随式；虚拟机器人；自适应控制律
Ｔｈｒｅｅ－ｄｉｍｅｎｓｉｏｎａＩ Ｆｏｒｍａｔｉｏｎ ＣｏｎｔｒｏＩ Ｍｅｔｈｏｄ ｆｏｒ ＡＵＶ
Ｂａｓｅｄ ｏｎ Ｐｉｌｏｔ Ｌｏｃａｔｉｏｎ ＩｎｆＯｒｍａｔｉｏｎ
ＺＨＵ Ｄａｑｉ，ＤＵ Ｑｉｎｇ
（Ｌａｂｏｒａｔｏｒｙ ｏｆ Ｕｎｄｅｒｗａｔｅｒ Ｖｅｈｉｃｌｅｓ ａｎｄ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍ，Ｓｈａｈｇｈａｉ Ｍａｒｉｔｉｍｅ Ｕｎｉｖｅｒｓｉｔｙ，Ｓｈａｈｇｈａｉ ２０１３０６，Ｃｈｉｎａ）
Ａｂｓｔｒａｃｔ：Ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｐｒｏｂｌｅｍ ｏｆ ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅｓ（ＡＵＶ）ｉｎ ｔｈｒｅｅ—ｄｉｍｅｎｓｉｏｎａｌ
ｕｎｄｅｒｗａｔｅｒ ｅｎｖｉｒｏｎｍｅｎｔ ｉｓ ｄｉｓｃｕｓｓｅｄ ｉｎ ｔｈｉｓ ｐａｐｅｒ．Ｌｅａｄｅｒ—ｆｏｌｌｏｗｅｒ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｍｅｔｈｏｄ ｉｓ ａｐｐｌｉｅｄ．
Ｒｅｆｅｒｅｎｃｅ ｔｒａｊｅｃｔｏｒｙ ａｎｄ ｓｐｅｅｄ ｉｎｆｏｒｍａｔｉｏｎ ｏｆ ｆｏｌｌｏｗｅｒ，ｎａｍｅｌｙ ｖｉｒｔｕａｌ ｖｅｈｉｃｌｅ，ｉｓ ｏｂｔａｉｎｅｄ ｂａｓｅｄ ｏｎ ｔｈｅ
ｌｅａｄｅｒ ｐｏｓｉｔｉｏｎ ａｎｄ ｐｒｅｄｅｔｅｒｍｉｎｅｄ ｆｏｒｍａｔｉｏｎ ｏｎｌｙ．Ｔｈｅｎ ｄｙｎａｍｉｃ ａｄａｐｔｉｖｅ ｖａｒｉａｂｌｅ ｓｔｒｕｃｔｕｒｅ ｃｏｎｔｒｏｌ ｌａｗ ｉｓ
ｄｅｓｉｇｎｅｄ ｆｏｒ ｔｈｅ ｆｏｌｌｏｗｅｒ ｕｓｉｎｇ ｂａｃｋｓｔｅｐｐｉｎｇ ｓｙｎｔｈｅｓｉｓ ａｎｄ ｓｌｉｄｉｎｇ ｍｏｄｅ ｃｏｎｔｒｏｌ ｍｅｔｈｏｄ．Ｔｈｅ ｔｒａｊｅｃｔｏｒｙ ｏｆ
ｔｈｅ ｆｏｌｌｏｗｅｒ ｃａｎ ｃｏｎｖｅｒｇｅ ｔｏ ｔｈｅ ｖｉｒｔｕａｌ ｖｅｈｉｃｌｅ ｔｒａｊｅｃｔｏｒｙ ｗｉｔｈ ｔｈｅ ｃｏｎｔｒｏｌ ｌａｗ．Ｔｈｕｓ ｉｔ ｍａｉｎｔａｉｎｓ ｅｘｐｅｃｔｅｄ
ｒｅｌａｔｉｏｎｓｈｉｐ ｗｉｔｈ ｔｈｅ ｌｅａｄｅｒ．Ｔｈｅｎ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｓｉｍｕｌａｔｉｏｎ ｉｓ ｉｍｐｌｅｍｅｎｔｅｄ ｉｎ ｓｐｅｃｉｆｉｃ ＡＵＶ ｄｙｎａｍｉｃｓ
ｍｏｄｅｌ ｗｉｔｈ ＭＡＴＬＡＢ／ＳＩＭＵＬＩＮＫ ｐｌａｔｆｏｒｍ．Ｆｉｎａｌｌｙ，ｔｈｅ ｅｘｐｅｃｔｅｄ ｃｏｎｔｒｏｌ ｅｆｆｅｃｔ ｉｓ ｒｅａｌｉｚｅｄ ｗｈｉｃｈ
ｖｅｒｉｆｉｅｄ ｔｈｅ ｅｆｆｅｃｔｉｖｅｎｅｓｓ ａｎｄ ｐｒａｃｔｉｃａｂｉｌｉｔｙ ｏｆ ｔｈｅ ｐｒｏｐｏｓｅｄ ａｌｇｏｒｉｔｈｍ．
Ｋｅｙ ｗｏｒｄｓ：ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅ；ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ；ｌｅａｄｅｒ—ｆｏｌｌｏｗｅｒ；ｖｉｒｔｕａｌ ｖｅｈｉｃｌｅ；
ａｄａｐｔｉｖｅ ｃｏｎｔｒｏｌ ｌａｗ
限，往往不能完成一些复杂的任务，并且多ＡＵＶ
１ 引 言
自治水下机器人在海洋开发与探索中发挥
的协作更可增加其安全性以及整个系统的鲁棒
性，多ＡＵＶ的协同控制已经成为ＡＵＶ研究领域
的热点。编队控制是多ＡＵＶ协作的一个典型性
问题，也是研究其他协作问题的基础，在科学考
着十分重要的作用，由于单个ＡＵＶ的工作能力有
基金项目：国家自然科学基金资助项目（５１０７５２５７，５１２７９０９８）；上海市教委科研创新研究重点资助项目（１３ＺＺ１２３）
１９４ 系统仿真技术 第９卷第３期
察、深海探测、沉船打捞、旅游探险等领域有广泛
的应用，对于编队控制问题的研究有重要的现实
意义 一 。
２．２领航一跟随式编队控制
领航一跟随式编队控制过程需要设定领航
ＡＵＶ的行为，并且设计虚拟ＡＵＶ作为期望参考
量，三维空间位置关系如图１所示。
编队控制是指多个机器人在到达目的地的
过程中，保持某种队形，同时又要适应环境约束
的控制技术。目前，编队控制方法大致有基于行
为法、人工势场法、虚拟结构法、领航一跟随法、分
布式控制法等 ］，其中领航一跟随方法又称主从
式编队方法，仅仅给定领航者的行为，为跟随者
设计跟踪控制律就可以控制整个机器人群体的
行为，简化了队形问的合作问题。考虑到水下环
境通讯带宽的限制及水下噪声的干扰等实际情
况，希望能够减少ＡＵＶ间的交互信息量，因此在
领航一跟随式编队控制方法中，仅利用领航者的
位置信息 ，而不利用其速度及角速度信息，得
到跟随者的期望轨迹及速度，据此设计跟踪控制
律。由于滑模控制不需要精确的数学模型及抗
干扰能力强，而广泛应用于ＡＵＶ的控制系统
中 ，对此本文给出了三维环境中ＡＵＶ主从式
编队控制自适应滑模控制算法，并将该控制算法
应用到具体ＡＵＶ动力学模型中，得到推进器推力
值，验证了算法的有效性及实用性。
２ 问题描述
２．１ ＡＵＶ运动学与动力学模型
通常将ＡＵＶ的坐标系统分成惯性坐标系（Ｅ—
ＸＹＺ）和载体坐标系（０一Ｘ。ｙ０Ｚ。）两类 ，三维空
问中完整运动学表示为
Ｍｙ＋ｃ（１，） ＋Ｄ（１，）ｌ，＋ｇ（叼）＝ （１）
叼＝Ｊ（叼）ｌ， （２）
７＝ＢＴ （３）
其中：Ｍ，ｃ，Ｄ，ｇ， ∈Ｒ¨。分别为惯性矩阵、离心
项和科氏矩阵、阻尼矩阵、静力项（重力与浮力合
力）、ＡＵＶ作用力与力矩；ｒ＝［ 口ｗ ｐ ｑ ｒ］ ，为广
义速度向量； ＝［ Ｙ ｚ ］ ，为广义位置向
量；．，（’，）为转换矩阵；Ｂ是适当维度的推力配置
矩阵； 是各个推进器的推力项 。
图１三维空间ＡＵＶ位置关系
Ｆｉｇ．１ ３Ｄ ｓｐａｃｅ ＡＵＶ ｐｏｓｉｔｉｏｎ ｒｅｌａｔｉｏｎｓ
图１中，领航ＡＵＶ、虚拟ＡＵＶ、跟随ＡＵＶ和
领航ＡＵＶ在跟随ＡＵＶ所在平面投影，分别用
， ，
Ａ ，Ａ ，表示，编队任务描述为：Ａ 与 空
间距离ｄ，水平面观望角度 为Ａ Ａ ，与Ａ 线速
度矢量耽 的夹角，垂直方向观望角度孝为Ａ Ａ
与Ａ Ａ ，的夹角。若某一时刻领航ＡＵＶ的广义
位置为叼 ，得出虚拟ＡＵＶ的位置叼 ，对于Ａ ，只
要设计出合理的控制输入ｒ ，得到ＡＵＶ各推进
器推力值，使得ｌｉｍ ｌ叼 一７７ Ｉ＝Ｏ，即可保证Ａ 正
确路径。
在ｎ维ＡＵＶ系统中，将ｎ维ＡＵＶ组分为ｎ一１
个子系统，各子系统包括２个ＡＵＶ（领航ＡＵＶ，跟
随ＡＵＶ），如图２所示。
图２ 三维空间领航一跟随编队控制模式
Ｆｉｇ．２ ３Ｄ ｓｐａｃｅ ｌｅａｄ・ｆｏｌｌｏｗ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｍｏｄｅ
朱大奇，等：基于领航位置信息的ＡＵＶ三维编队控制方法 １９５
３ ＡＵＶ推进器配置及推力分配
本文中仿真模型为上海海事大学水下机器人
与智能系统实验室海筝号ＡＵＶ模型，如图３所示。
该机器人一共包含４个推进器，水平面２个推进器
和垂直面２个推进器，无侧向推进器，具体推进器
布置如图４所示（ＴＨ表示推进器ｔｈｒｕｓｔｅｒ）。
图３ ＡＵＶ结构
Ｆｉｇ．３ ＡＵＶ ｓｔｒｕｃｔｕｒｅ
一
图４推进器布置
Ｆｉｇ．４ Ｔｈｒｕｓｔｅｒ ａｒｒａｎｇｅｍｅｎｔ
针对该ＡＵＶ推进器的布置，将ＡＵＶ完整动
力学方程进行简化解耦工作，考虑ＡＵＶ空间中３
自由度运动：进退（ｓｕｒｇｅ），回转（ｙａｗ）和潜浮
（ｈｅａｖｅ）。因此，惯性坐标及载体坐标系下ＡＵＶ
状态为 ＝［ Ｙ ］ 和ｌ，＝［ ｒ］ 。推算出海
筝号ＡＵＶ空间中各个自由度上产生的合力与合
力矩和推进器的关系¨ ：
［ ］＝［三一 三蚤］ （４）
其中：［ｒ ］ 是作用于ＡＵＶ重心的各个自
由度上的合力与合力矩；Ｋ为常数；Ｒ为两个垂直
推进器之间的距离；［ ］ 是水平
面布置的４个推进器的推力。
归一化推力如式（５）所示：
１ ｌ
一 一
０ ０
２ ２
１ １
０ ０ 一一
２ ２
ｌ １
一 一
一 一一
０ ０
２ ２
１
２
ｆ ｚ
＝
Ｂ・
§Ｊｒ＝ＢＴ （５）
３
ｒＮ
一
ｒ为推力与力矩 的归一化形式，一１≤ｒ ≤
一 一
１，ｉ＝Ｘ，Ｚ，Ｎ， 为推进器推力值 的归一化形
式，一１≤Ｔ ≤１，ｉ＝１，２，３，４。 ｘ＾ｆ，．『 ，ｒⅣＭ表示各
个自由度上最大合力与合力矩。
４虚拟机器人设计
根据领航位置信息及编队任务的要求，得到
期望轨迹：
叼ｄ＝田 ＋．，（叼 ）Ｚ （６）
其中：ｚ＝［ｄ ｓｉｎ ＣＯＳ ，ｄ ｓｉｎ ＣＯＳ ，ｄ ＣＯＳ ，０］ ；
叼 为领航机器人的广义位置；ｄ， 为编队航行过
程中设定的常数。
设虚拟机器人广义位置量为叼 ，定义虚拟机
器人误差量：ｅ ＝叼 一 ∈Ｒ ，定义辅助误差
变量 ¨ ：
一
１９６ 系统仿真技术 第９卷第３期
， ＝ｅ ＋ （７）
其中：ｃｐ：［ 。， ， ，， ｒ，定义其导数：
＝一
５跟随ＡＵＶ编队控制律
（８） （ ）一Ｋｒ
定义虚拟机器人的速度值为：
ｌ， ＝Ｊ （叩 ）（ ，（ ）＋ （ ）） （９）
编队控制的目标是为跟随ＡＵＶ各推进器设
计合适的推力值作为输入量，控制跟随ＡＵＶ的速
度和角速度量，使其能够跟踪虚拟ＡＵＶ的轨迹，
即误差ｅ 收敛于０。
ｅ ／ｆ一７／７ 【ｅ ｅ ｅ：ｅｑｒ ，ｊ］Ｔ
式中：Ｋ＝ｄｉａｇ［ｊｃ１，ｋ２，ｊｃ３， ４］，Ｊｌｃｆ，Ａ ∈Ｒ ； １（ ）＝
［Ａ１ｔａｎｈ（ ｌ／Ａ１），Ａ２ｔａｎｈ（ ２／Ａ ２），Ａ ３ｔａｎｈ（ ３／Ａ ３），
Ａ４ｔａｎｈ（ ４／Ａ４）］ ； ２（ ）＝［ｋｌｔａｎｈ（ １／ｋ１）， ２ｔａｎｈ
（ ２／ｋ２），ｋ３ｔａｎｈ（ ３／ｋ３），ｋ４ｔａｎｈ（ ４／ｋ４）］。， （０）＝
０，ｉ＝１，２，３，４。
［ ｆ— Ｙｆ—Ｙ ｚｆ—ｚ ｆ一 ］ （１０）
系统控制结构如图５所示：
图５控制系统结构框图
Ｆｉｇ．５ Ｃｏｎｔｒｏｌ ｓｙｓｔｅｍ ｓｔｒｕｃｔｕｒｅ ｄｉａｇｒａｍ
ｅ 一后 ｅ；＝一ｋｅ 一ｋｅ；一
：
ｅ 一 ｅ２ ≤０ （１３）
因此系统是一致稳定的 Ｈ 。
５．２ 自适应滑模动力学控制器
将速度误差值ｅ。＝ｌ，。一ｌ，ｆ＝［ｅ ｅ ｅ，］ 作
为滑模控制器的输入，选择滑模面 ：
ｌ， ［Ｉ Ｌ兰ｒ （｝ ＋ ：ｅ： ＋ｅｙｓｉｎ ｆ ＋‘ Ｖｃ。ｓ ｅ 一 Ｖｓｉｎ ｅ ］ｊＩ
＝
＝ ＋２Ａｅ ＋Ａ Ｉｅ
＝ｅ ＋２Ａｅ ＋Ａ ｅ ＝ｅ ＋
（１４）
对等式（１４）求导，得到
２Ａ（ 一 ）＋Ａ ｅ （１５）
系统运行于滑模面时，式（１５）为零
． ．
１
Ｐ
ｃ
＋２以（ ｃ一 （ 一ｃ —
Ｄ１，ｆ—ｇ））＋Ａ ｅ ＝０ （１６）
＝了
１
ｅ ＋了１
２＋
＋
ｅ
２ ＋
ｅ
２ ＋
ｅ
２
＝＋ 。 ２
。
１
１
（１２）（ ２）
等价控制律为
ｅ
＝ ＋
＋ ＋
Ｃ ｆ＋Ｄ ｆ＋ｇ （１７）
朱大奇，等：基于领航位置信息的ＡＵＶ三维编队控制方法 １９７
加入自适应控制项取代切换项，得
＝
郴＋
（１８）
其中： ｒ 是用来估计未知量ｒ 的自适应变化
项。未知的不确定量 满足以下更新率：
。
：Ｆｓ （１９）
最终得到完整的控制律：
＋ ＋（ ＋ ） （２０）
６仿真实验
仿真过程中，设定３个机器人，分别为Ａ。，
Ａ ，Ａ：，其中Ａ。为领航ＡＵＶ，Ａ ，Ａ：分别为左、右
跟随ＡＵＶ，设定Ａ。的航行轨迹为：７／ ＝
［ Ｙ ｌ ］ 。，其中， （ｔ）＝２０×ｓｉｎ（０．５×ｔ）；
Ｙ （ｔ）＝２０×ＣＯＳ（０．５× ）；ｚ （ｔ）＝ｔ； （ｔ）＝０．５
ｘ ｔ。Ａ。Ａ 与Ａ。空间距离为ｄ＝４，观测角度 ＝
一
，’
÷，
叶
Ａ。与Ａ。的观测角度为 ＝÷订，
Ｊ
Ａ。与Ａ 的
４
观测角度为Ｘ ＝÷１Ｔ，形成空间Ｖ形编队队形。
ｊ
选择参数和增益为Ａ，＝２，Ａ ＝１，Ａ ＝１，Ａ
＝
０．５，ｋ ＝２，ｋ２＝１，ｋ３＝１，ｋ ＝０．５，设定ｔ从０时
刻开始运行，Ａ。，Ａ：的起始位置分别为 。（０）＝
［一１０—１５ ０ ０］，７／ （０）＝［一５—２０ ０ ０］，仿真结
果如图６所示。
２０
１５
１０
Ｎ
５
０
２
图６螺旋线编队控制轨迹
Ｆｉｇ．６ Ｓｐｉｒａｌ ｌｉｎｅ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｔｒａｊｅｃｔｏｒｙ
当Ａ。沿着螺旋轨迹航行时，据编队任务要
求，各时刻Ａ ，Ａ 的误差值，如图７和图８所示，
经计算与理论值相符
。
一
２
Ｉ ｉ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ
Ｏ ２ ４ ６ ８ １０ １２ １４ １６ ｌ８ ２０
２
。
一
０ ２ Ｌ——▲——Ｊ———Ｌ——Ｊ———Ｌ—— ——Ｊ———Ｌ——＿Ｌ—＿Ｊ
０ ２ ４ ６ ８ １０ １２ １４ １６ １８ ２０
０．１ ｒ ｔｉｍｅ
．。．
０ ￣Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ［１
０ ２ ４ ６ ８ ｌ０ １２ １４ １６ １８ ２０
墨．０．０５ ｒ ｔｉｍｅ
吾
耄１．。 ．
０ ２ ４
二二＝二＝＝＝＝
６ ８ ｌ０ １２ １４ １６ １８ ２０
图７ Ａ。位置误差量
Ｆｉｇ．７ Ｐｏｓｉｔｉｏｎ ｅｒｒｏｒ ｏｆ Ａ１
—
ｌ ＝＝＝＝二＝二二＝二＝
８ ５ ｒ ｔｉｍｅ
曼．．０ —ｉ Ｉ Ｉ Ｉ—Ｉ ｉ ｉ Ｉ Ｉ
Ｏ．１ ｒ ｔ ｍ。
０
＿０＿
１ Ｉ ＩＩ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ Ｉ
０ ２ ４ ６ ８ １０ １２ １４ １６ １８ ２０
ｄ
０． ０５ ｔｉｍ ｅ
．。．
图８ Ａ 位置误差量
Ｆｉｇ．８ Ｐｏｓｉｔｉｏｎ ｅｒｒｏｒ ｏｆ Ａ２
根据Ａ。航行轨迹及编队任务要求，得到Ａ
和Ａ：的期望位置信息叼 和叼： ，以Ａ。为例进行
说明，由Ａ 期望位置信息 ，得到虚拟ＡＵＶ速
度值ｌ， ＝［ｕ Ｗ ｒ ］Ｔ，结合式（２）得到虚拟ＡＵＶ
的位置信息Ｊ，７ ，与Ａ 实际航行轨迹比较得到误
差量，将ｌ， 和叼 作为反步控制器的输人，得到Ａ
辅助速度值ｌ， ＝［“ ｒ ］ ，将其ｌ， 作为滑模控
制器输入生成速度控制的力和力矩，得到归一化
推力值Ｔ＝［ ， ］ ，如图９所示。将
推力作用到Ａ ，产生实际速度值如图１０所示，实
现期望的编队控制。
ｌ９８ 系统仿真技术 第９卷第３期
１．０
０．５
０
０ ２ ４ ６ ８ １０ １２ １４ １６ １８ ２Ｏ
１．０
ｔｉｍｅ
０．５
０
０ ２ ４ ６ ８ １０ １２ ｌ４ １６ １８ ２０
１
ｔｉｍｅ
０
—
１
０ ２ ４ ６ ８ １０ ｌ２ １４ １６ １８ ２０
１
ｔｉｍｅ
０
—
１
０ ２ ４ ６ ８ １０ １２ １４ １６ １８ ２０
ｔｉｍｅ
图９ Ａ。推进器推力值
Ｆｉｇ．９ Ｔｈｒｕｓｔｅｒ ｔｈｒｕｓｔ ｖａｌｕｅ
２０
１０
０
２ ４ ６ ８ １０ ｌ２ １４ １６ １８ ２０
２
ｔｉｍｅ
ｌ
Ｏ
２ ４ ６ ８ １０ １２ ｌ４ ｌ６ ｌ８ ２Ｏ
１．Ｏ
ｔｉｍｅ
ｋ Ｏ．５
０ ２ ４ ６ ８ １０ １２ １４ １６ １８ ２０
ｔｉｍｅ
图ｌ０ Ａ。速度值
Ｆｉｇ．１０ Ｖｅｌｏｃｉｔｙ ｏｆ Ａｌ
７ 总结
本文应用主从式编队控制方法，研究了三
维环境中多ＡＵＶ的编队控制问题，仅利用领航
ＡＵＶ的位置信息，结合编队任务要求，设计虚拟
ＡＵＶ，得到跟随ＡＵＶ的参考轨迹及速度值，在运
动学反步控制的基础上，应用变结构滑模控制
方法得到跟随ＡＵＶ的自适应变结构控制律。随
后将该算法应用到具体ＡＵＶ模型上，对领航
ＡＵＶ螺旋线航行轨迹进行仿真。仿真结果表
明，跟随ＡＵＶ能够与领航ＡＵＶ保持期望的位姿
关系，实现预期编队队形，验证了算法的有效性
与实用性。
参考文献：
［１］ Ｓｔｉｌｗｅｌｌ Ｄ Ｊ，Ｂｉｓｈｏｐ Ｂ Ｅ．Ｐｌａｔｏｏｎｓ ｏｆ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅ
［Ｊ］．ＩＥＥＥ Ｃｏｎｔｒｏｌ Ｓｙｓｔｅｍ Ｍａｇａｚｉｎｅ，２０００，２０（８）：４５
—
５２．
［２］Ｃｕｉ Ｒ，Ｓａｍ Ｇｅ Ｓ，Ｖｏｏｎ Ｅｅ Ｈｏｗ Ｂ，ｅｔ ａ１．Ｌｅａｄｅｒ—ｆｏｌｌｏｗｅｒ
ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｏｆ ｕｎｄｅｒａｃｔｕａｔｅｄ ａｕｔｏｎｏｍｏｕｓ
ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅｓ［Ｊ］．Ｏｃｅａｎ Ｅｎｇｉｎｅｅｒｉｎｇ，２０１０，３７
（１７）：１４９１—１５０２．
［３］杨甜甜，刘志远，陈虹，等．移动机器人编队控制的现
状与问题［Ｊ］．智能系统学报，２００７，２（４）：２１—２７．
ＹＡＮＧ Ｔｉａｎｔｉａｎ，ＬＩＵ Ｚｈｉｙｕａｎ，ＣＨＥＮ Ｈｏｎｇ，ｅｔ ａ１．
Ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｏｆ ｍｏｂｉｌｅ ｒｏｂｏｔｓ：ｓｔａｔｅ ａｎｄ ｏｐｅｎ
ｐｒｏｂｌｅｍｓ［Ｊ］．Ｃａａｉ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ，
２００７，２（４）：２１—２７．
［４］ 崔荣鑫，徐德民，严卫生，等．仅利用位置信息的自主
水下航行器主从式编队控制方法［Ｊ］．兵工学报，
２００８，２９（８）：９８０—９８４．
ＣＵＩ Ｒｏｎｇｘｉｎ，ＸＵ Ｄｅｍｉｎ，ＹＡＮ Ｗｅｉｓｈｅｎｇ，ｅｔ ａ１．Ｌｅａｄｅｒ—
ｆｏｌｌｏｗｅｒ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｏｆ ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｒｗａｔｅｒ
ｖｅｈｉｃｌｅｓ ｕｓｉｎｇ ｐｏｓｉｔｉｏｎ ｉｎｆｏｒｍａｔｉｏｎ ｏｎｌｙ［Ｊ］．Ａｃｔａ
Ａｒｍａｍｅｎｔａｒｉｉ，２００８，２９（８）：９８０—９８４．
［５］Ｐａｎ Ｍ Ｌ，Ｓｅｏｋ Ｗ Ｈ，Ｙｏｎｇ Ｋ Ｌ，ｅｔ ａ１．Ｄｉｓｃｒｅｔｅ—ｔｉｍｅ
ｑｕａｓｉ—ｓｌｉｄｉｎｇ ｍｏｄｅ ｃｏｎｔｒｏｌ ｏｆ ａｎ ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｒｗａｔｅｒ
ｖｅｈｉｃｌｅ［Ｊ］．ＩＥＥＥ Ｊｏｕｒｎａｌ ｏｆ Ｏｃｅａｎｉｃ Ｅｎｇｉｎｅｅｒｉｎｇ，１ ９９９，
２４（３）：３８８—３９５．
［６］ Ｓｅｒｄａｒ Ｓ，Ｂｒａｄｌｅｙ Ｊ Ｂ，Ｒｏｎ Ｐ Ｐ．Ａ ｃｈａｔｔｅｒｉｎｇ－ｆｒｅｅ ｓｌｉｄｉｎｇ—
ｍｏｄｅ ｃｏｎｔｒｏｌｌｅｒ ｆｏｒ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅｓ ｗｉｔｈ ｆａｕｌｔ．
ｔｏｌｅｒａｎｔ ｉｎｆｉｎｉｔｙ—ｎｏｒｕｌ ｔｈｒｕｓｔ ａｌｌｏｃａｔｉｏｎ［Ｊ］．Ｏｃｅａｎ
Ｅｎｇｉｎｅｅｒｉｎｇ，２００８，３５（１６）：１６４７—１６５９．
［７］ 朱大奇，李伟崇，颜明重，等．基于Ｄ—ｓ信息融合的
水下机器人地图构建［Ｊ］．系统仿真技术，２０１２，８
（３）：１８１—１８６．
ＺＨＵ Ｄａｑｉ，ＬＩ Ｗｅｉｃｈｏｎｇ，ＹＡＮ Ｍｉｎｇｚｈｏｎｇ，ｅｔ ａ１．Ｍａｐ
ｂｕｉｌｄｉｎｇ ｆｏｒ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅ ｂａｓｅｄ ｏｎ Ｄ—Ｓ ｉｎｆｏｒｍａｔｉｏｎ
ｆｕｓｉｏｎ ａｌｇｏｒｉｔｈｍ［Ｊ］．Ｓｙｓｔｅｍ Ｓｉｍｕｌａｔｉｏｎ Ｔｅｃｈｎｏｌｏｇｙ，
２０１２，８（３）：１８１—１８６．
［８］刘健，于闯，刘爱民．无缆自治水下机器人控制方法研
究［Ｊ］．机器人，２００４，２６（１）：７—１０．
ＬＩＵ Ｊｉａｎ，ＹＵ Ｃｈｕａｎｇ，ＬＩＵ Ａｉｍｉｎ．Ｒｅｓｅａｒｃｈ ｏｎ
ｕｎｔｅｔｈｅｒｅｄ ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅ ｃｏｎｔｒｏｌ
ｍｅｔｈｏｄ［Ｊ］．Ｒｏｂｏｔ，２００４，２６（１）：７—１０．
（下转第２１２页）