基于Serret-Frenet坐标系的多AUV编队路径跟踪控制

赵宁宁，徐德民，高　剑，张秦南

（1. 中国船舶重工集团公司 第705研究所，陕西 西安，710075； 2. 西北工业大学 航海学院，陕西 西安，710072）

基于Serret-Frenet坐标系的多AUV编队路径跟踪控制

赵宁宁1，徐德民2，高剑2，张秦南1

（1. 中国船舶重工集团公司 第705研究所，陕西 西安，710075； 2. 西北工业大学 航海学院，陕西 西安，710072）

针对多自主水下航行器（AUV）的编队路径跟踪控制问题，提出了基于Serret-Frenet坐标系的虚拟结构编队控制方法，分为单个AUV队形路径跟踪控制，以及多个AUV间路径跟踪参考点的一致性协调控制两部分。首先为每个AUV定义一个沿期望路径运动的路径跟踪参考点，并以其为原点建立Serret-Frenet坐标系描述编队跟踪误差及动态模型。通过对AUV的航向角和速度的控制，使其跟踪并收敛于该Serret-Frenet坐标系下的期望队形位置。然后采用基于比例-积分控制的一致性算法，调节各个参考点的路径参数变化率使其达到一致，并以给定的期望速度沿路径运动。通过数学仿真实现了3个AUV以一定队形跟踪期望路径，验证了该算法的有效性。

自主水下航行器； 编队控制； 路径跟踪； Serret-Frenet坐标系

0　引言

多自主水下航行器（autonomous underwater vehicles，AUV）通过合作和协调动作执行任务，能够极大地提高工作效率以及整个系统的可靠性［1］。作为多AUV协作中的典型性和通用性问题，编队控制正在成为多AUV研究领域的热点。

由于AUV所依赖的水声通信特性及自身动力学系统的复杂性，目前编队控制研究将信息一致性方法［2-3］和路径跟踪控制方法［4-5］相结合，实现系统的协调控制目标，称为协同路径跟踪控制。该方法在机器人、无人机等领域已有一定研究成果［6-7］，在多AUV协作系统也开展了一些研究［8-10］。文献［8］通过构造级联系统和一致性算法实现直线路径的协同跟踪； 文献［9］采用一致性算法与虚拟结构法相结合研究了运动学层面的AUV小尺度编队控制问题； 文献［10］基于无源性理论与一致性跟踪理论，研究了通信受限下的多AUV编队路径跟踪问题。因此可知，队形的定义基本为将一条期望路径通过一定关系映射成多条并行路径。

文中，采用不同的队形定义方式，考虑多个AUV以期望队形跟踪空间一条期望路径。在该期望路径上定义编队参考点，将其映射为该路径上的与AUV相对应的路径跟踪参考点，并建立Serret-Frenet坐标系设计路径跟踪控制器［11］，使跟踪误差趋于由给定队形定义的位置坐标； 之后对所有路径参数进行一致性协调，使得路径参数渐近一致，且其描述的路径跟踪参考点速度收敛至编队参考点速度，实现编队控制任务。

1　问题描述

1.1AUV模型

以欠驱动AUV为研究对象，其水平面运动模型［12］为

式中: u, v和r分别为前向速度、侧向速度和航向角速度; x和y为AUV坐标; Ψ为航向角; X为前向推力; N为偏航力矩; m11, m22和m33为惯性质量; d11, d22和d33为阻尼系数。

1.2基于Serret-Frenet坐标系的路径跟踪误差

给定1条光滑曲线Ω，由一般的曲线参数ξ唯一确定，O点为AUV载体坐标系的原点，P点为路径Ω上的路径跟踪参考点，在P点上建立由切向量和法向量构成的Serret-Frenet坐标系xsfysf，记为｛SF｝，如图1所示。

图1　Serret-Frenet坐标系及路径跟踪误差示意图Fig. 1　Schematic of Serret-Frenet coordinate system and path following errors

参考点以速度Up沿参考路径Ω运动，xsf轴与地面坐标系｛N｝的x轴夹角为Ψp。O点在｛SF｝坐标系中的坐标为（τe，ne），该坐标定义了AUV的路径跟踪误差。AUV前向速度与x轴夹角为Ψ，为航向角跟踪误差。

1.3控制目标

考虑n个AUV组成1个整体系统，假设曲线Ω上P0点为该系统的编队参考点，则会有P0=定义路径跟踪参考点Pi为AUVi对编队参考点的估计，则有，对应的速度其中ξi可以看作是对ξ的估计，如图2所示。在｛SF｝坐标系下，AUVi相对于曲线Ω的跟踪路径误差为εi=li=

编队路径跟踪的控制目标分为以下步骤。

1）路径跟踪控制

为每个AUV设计路径跟踪控制器，使其最终收敛到Serret-Frenet坐标系中由队形定义的位置坐标，即路径跟踪误差εi最终收敛至期望坐标, 则

图2　编队路径跟踪设定示意图Fig. 2　Setting schematic of formation path following

2）路径参数一致性控制

定义vL为路径参数ξi变化率的期望值。当且仅当ξi=ξj，=vL时，整个系统沿期望路径形成且保持队形匀速运动，即AUV对应的路径跟踪参考点均渐近收敛到编队参考点

2　协同路径跟踪控制器设计

2.1路径跟踪控制

分析图1得到AUVi跟踪期望路径的跟踪误差模型为［11］

式中，rpi为参考点Pi的角速率，且

定义Lyapunov函数

对其求时间导数，并代入式（6）的前2个方程，整理可得的侧滑角。

从式（8）可以看出，需要通过控制输入Upi的作用使误差τei-lτi稳定，而nei-lni的稳定则需要调节AUV的航向误差Ψvfi来实现。与一般路径跟踪控制相比，常值队形参数lni和lτi的存在使编队路径跟踪控制更加复杂。其中，切向偏差lτi使得当-rpilτi+vtisinΨvfi随着nei-lni收敛到零时，有vtisinΨvfi=rpilτi≠0。因此cosΨvfi≠1，在设计参考点速度Upi时，不能消除cosΨvfi项的影响，同时rpilni中包含参考点的速度ξ.，难以分析和设计参考点速度和AUV的速度，实现队形路径跟踪。因此，这里针对以下2种特殊情况加以研究。

1）一字型平行编队运动

在该队形下lτi=0，即

选择AUV的航向角满足

同时，选择AUV的航行速度为

其中，χ为有界单调递增函数，χ（0）=0，χ（τ）∈［-a，a］，且对于任意τ＜r，χ（τ）＞kχ（r ）τ，满足

将控制输入代入V.e方程，并考虑到

因此，当vti＞0时负定，编队路径跟踪误差

全局一致渐近稳定。

为了保证航行速度vti＞0，要求参考点的移动速度和角速度满足其中rp，max为参考点沿路径运动时的最大角速度。

2）直线参考路径

直线是最为常见的一种路径形式，此时rpi=0，得

选择AUV的航向角满足

同时，选择AUV的航行速度为

将控制输入代入Ve.方程，并考虑到

当航行速度vti＞0时, 有V.e负定, 因此编队路径跟踪误差τei-lτi, nei-lni全局一致渐近稳定。此时, 为了保证航行速度vti＞0, 要求参考点的移动速度满足Upi＞kτγ。

2.2路径参数一致性控制

假定通信拓扑结构图为无向连通图，将编队参考点作为一个节点0放入通信拓扑结构图中，令所有AUV中只有节点1知道编队参考点的期望参数变化率，节点2和节点3分别通过与节点1和节点2的信息交互来间接获取编队参考点的路径参数信息，如图3所示。

图3　增加了编队参考点的通信拓扑图Fig. 3　Communication topology with an additional formation reference point

由于节点2和3不能直接获得编队参考点的路径参数变化率，需要通过参数自适应算法来估计。于是，设计比例-积分一致性算法［13］

其中： 一致性参数a＞0，c＞0均为常数； Ji为与AUVi有信息交互的邻居AUV集合。显然，当 t→∞时，实现了控制目标（4）和（5）。

3　仿真结果与分析

考虑3个AUV以三角形队形跟踪直线路径，以弧长为路径参数，直线路径表示为：队形定义［lτi，lni］分别为［0，0］，［-20，20］，［-20，-20］。路径参数变化率的期望值vL=6。一致性算法中参数a=0.2，c=0.01。控制参数kτ=0.5，kΨ=5。AUV初始状态分别为［0，10，π2］T，［10，10，0］T，［10，0，-π2］T，初始速度均为5 m/s。仿真中AUV数学模型采用美国伍兹霍尔海洋研究所的REMUS AUV数学模型［14］。

仿真结果由图4给出。其中，图4（a）为轨迹曲线图，各AUV快速形成1个三角形队形，并保持相对固定位置运动。图4（b）～图4（d）分别表示3个AUV的速度曲线、跟踪误差曲线和路径参数曲线。图中，ξ为路径参数，ξ_dot为路径参数变化率。

图4　多AUV编队路径跟踪控制仿真图Fig. 4　Simulation results of the formation path following control for multiple AUVs

可以看出，AUV在运动初期不断调整自身速度，之后快速收敛到期望速度，路径跟踪误差收敛到由队形定义的期望值，路径参数和变化率快速一致收敛。

4　结束语

针对多AUV协作中的编队路径跟踪控制问题进行了研究。首先，考虑队形的约束，为整个AUV系统设定1个编队参考点，假定仅1个AUV知道该编队参考点的速度（路径参数变化率），设计路径跟踪算法，令AUV到自身路径跟踪参考点的跟踪误差值渐近收敛至期望值，同时设计一致性算法，令每个AUV对应的路径跟踪参考点对编队参考点的路径参数的估计渐近达到一致。仿真结果表明，文中的算法实现了3个AUV以一定队形跟踪期望路径，其描述的路径跟踪参考点速度收敛至编队参考点速度，很好地实现了AUV编队控制任务。

［1］Healey A J. Application of Formation Control for Multi-Vehicle Robotic Mine Sweeping［C］//Proceedings of the IEEE Conference on Decision and Control. New York：IEEE，2001： 1497-1502.

［2］Benediktsson J A，Swain P H. Consensus Theoretic Classification Methods［J］. IEEE Transactions System，Man，Cybernetics，1992，22（4）： 688-704.

［3］Olfati-Saber R，Murray R M. Consensus Problems in Networks of Agents with Switching Topology and Time-Delays［J］. IEEE Transactions Automatic Control，2004，49（9）： l520-l533.

［4］Do K D，Pan J. State-and-Output Feedback Robust Path-Following Controllers for Underactuated Ships Using Serret-Frenet Frame［J］. Ocean Engineering，2004，31（5/6）： 587-613.

［5］Lapierre L，Soetanto D. Nonlinear Path-Following Control of an AUV［J］. Ocean Engineering，2007，34（11/12）：1734-1744.

［6］Ren W，Sorensen N. Distributed Coordination Architecture for Multi-robot Formation Control［J］. Robotics and Autonomous Systems，2008，56（4）： 324-333.

［7］毕鹏，罗建军，张博. 一种基于一致性理论的航天编队飞行器协同控制方法［J］. 宇航学报，2010，31（1）： 70-74.

Bi Peng，Luo Jian-jun，Zhang Bo. Cooperate Control Algorithm for Spacecraft Formation Flying Based on Consensus Theory［J］. Journal of Astronautics，2010，31（1）：70-74.

［8］Børhaug E，Pavlov A，Pettersen K. Straight Line Path Following for Formations of Underactuated Marine Surface Vessels［J］. IEEE Transactions on Control Systems Technology，2011，19（3）： 493- 506.

［9］袁健，唐功友. 采用一致性算法与虚拟结构的多自主水下航行器编队控制［J］. 智能系统学报，2011，6（3）：248-253.

Yuan Jian，Tang Gong-you. Formation Control of Autonomous Underwater Vehicles with Consensus Algorithms and Virtual Structure［J］. CAAI Transactions on Intelligent Systems，2011，6（3）： 248 -253.

［10］王银涛，严卫生. 多自主水下航行器系统一致性编队跟踪控制［J］. 控制理论与应用，2013，30（3）： 379-384.

Wang Yin-tao，Yan Wei-sheng. Consensus Formation Tracking Control of Multiple Autonomous Underwater Vehicle Systems［J］. Control Theory & Applications，2013，30（3）： 379-384.

［11］高剑，刘富樯，赵江，等. 欠驱动自主水面船的非线性路径跟踪控制［J］. 机器人，2012，34（3）： 329-336.

Gao Jian，Liu Fu-qiang，Zhao Jiang，et al. Non-linear Path Following Control of Underactuated Autonomous Surface Vehicles［J］. Robot，2012，34（3）： 329-336.

［12］Fossen Thor I. Marine Control Systems： Guidance，Navigation and Control of Ships，Rigs and Underwater Vehicles［M］. Trondheim，Norway： Marine Cybernetics，2002.

［13］Kaminer I，Yakimenko O，Pascoal A，et al. Path Generation，Path Following and Coordinated Control for Time- Critical Missions of Multiple UAVs［C］//Proceedings of the American Control Conference，Minneapolis，Minnesota，USA，2006：4906-4913.

［14］Prestero T. Verification of a Six-degree of Freedom Simulation Model for The REMUS Autonomous Underwater Vehicle［D］. MS Thesis，MIT/WHOI Joint Program in Oceanographic Engineering，2001.

（责任编辑： 杨力军）

Formation Path Following Control of Multiple AUVs Based on Serret-Frenet Coordinate System

ZHAO Ning-ning1，XU De-min2，GAO Jian2，ZHANG Qin-nan1
（1. The 705 Research Institute，China Shipbuilding Industry Corporation，Xi′an 710075，China； 2. School of Marine Science and Technology，Northwestern Polytechnical University，Xi′an 710072，China）

A virtual structure formation control method based on a Serret-Frenet coordinate system is proposed for the formation path following control of multiple autonomous underwater vehicles（AUVs）. The formation control system is composed of two parts，namely，the path following control for each AUV and the consistency coordination control for the path following reference points of multiple AUVs. Firstly，a path following reference point is defined for each AUV which moves along the desired path，and a Serret-Frenet coordinate system，which takes the reference point as its origin，is built to describe the formation path following errors and the dynamic model. The course angle and velocity are properly controlled to make the AUV converge to its predefined desired position in the Serret-Frenet coordinate system. Secondly，a proportional-integral consistency algorithm for path parameters is designed for all AUVs to make the virtual reference points synchronously move at a given change rate along the desired path with the desired velocity. At last，three AUVs in following a desired path with a certain formation are simulated，and the results validate the effectiveness of the proposed control method.

autonomous underwater vehicle（AUV）； formation control； path following； Serret-Frenet coordinate system

TJ630.33； TP13

A

1673-1948（2015）01-0035-05

2014-12-18；

2015-01-06.

赵宁宁（1980-），女，博士，研究方向为自主式水下航行器协同控制与仿真.