一种障碍环境下多AUV编队队形优化控制方法与流程

本发明涉及一种障碍环境下多auv编队队形优化控制方法，属于水下机器人技术领域。

背景技术：

随着资源的枯竭，对海洋资源的开发利用日益迫切，这就需要具备可以在复杂海洋环境中进行精确安全作业的工具，自主水下航行器(autonomousunderwatervehicle，auv)的诞生给人类探索和利用海洋资源带来了新的工具。因为单个auv的性能指标不能满足要求，就需要多个auv间的协作达成目标。多auv编队在执行任务过程中不可避免的会碰到障碍物，在编队航行遇到障碍物时，通过队形的优化变换，可以大幅降低到达任务点所需的时间，提高任务的完成效率。对于编队队形变换，不少学者进行了相关研究，其中有人基于图论方法协调两种队形间的变换，同时给出了一组机器人为了避障如何自动变换队形形状，但是没有考虑现实环境约束及队形变换时的性能优化问题。

本发明基于视距导航法设计单个auv的路径跟踪控制器，利用图论建立多auv间的通信拓扑关系，设计速度协调控制器，实现编队控制，针对障碍环境，考虑现实的环境约束和队形变换时的性能优化，设计编队队形优化避障策略，通过求解环境适应度函数获得最优编队拓扑结构，最终以最优编队队形通过障碍区域。

技术实现要素：

本发明的目的是为了使多auv编队能够通过队形优化变换通过障碍区域，完成对该区域的侦察任务而提供一种障碍环境下多auv编队队形优化控制方法。

本发明的目的是这样实现的：步骤如下：

步骤一：建立auv的水平面模型；

步骤二：初始化编队中auv的位置、速度信息；

步骤三：基于步骤一所建立的auv水平面模型，基于视距导航法建立路径跟踪误差模型，利用反馈线性化方法设计路径跟踪控制器，使跟踪误差收敛到零，实现单个auv的路径跟踪；

步骤四：在步骤三的基础上，利用图论建立多auv间的通信拓扑关系，依据获取的其它auv的状态信息，设计速度协调控制器，实现多auv间的协调控制；

步骤五：判定航行区域是否存在障碍物，若不存在障碍物，执行步骤八，若存在障碍物，执行步骤六；

步骤六：设计考虑环境约束的编队队形优化避障策略，通过求解环境适应度函数获得最优编队拓扑结构，保证编队以最优队形通过此障碍区域；

步骤七：判定编队中auv是否都通过障碍区域，若已都通过，进行队形恢复，继续执行后续任务，若仍有一部分没有通过障碍区域，返回步骤六，重新求解环境适应度函数，继续执行；

步骤八：保持此队形航行至目标点。

本发明还包括这样一些结构特征：

1.步骤一中建立auv的水平面模型为：

式中：mur＝m-yr，du＝-xuuu²-xvvv²，dv＝-yvuv-yv|v|v|v|，dr＝-nvuv-nv|v|v|v|-nrur；m＝2234.5kg，x{.}、y{.}和n{.}为水动力系数；iz为固定矩；f和t代表auv在纵向上的推力和转艏力矩，u为auv的前向运动速度，v为auv的横向运动速度，r代表auv的转艏速度，x代表auv横向位置，y代表auv纵向位置，ψ代表auv艏向角。

2.步骤二具体为：设置编队auv的数量为n，初始时刻给每个auv赋初始位置、深度、速度的信息。

3.步骤三的具体过程为：

针对所建立的auv的水平面模型，利用视距导航法构建单体的路径跟踪误差方程，

ψd(t)＝-arctan(ye(t)/d)(2)

其中，ψd是视线角，ψd(t)是某一时刻的视线角，ye(t)是某一时刻的纵向位置误差，d为间距；

得到位移误差方程为：

ye＝y-yd,ψe＝ψ-ψd(3)

其中，y和ψ是auv实际的纵向位置和艏向角，yd和ψd是auv期望的纵向位置和视线角；

对auv的直线跟踪问题可以转换为设计控制律，使下式成立：

基于视距导航法设计艏向控制器，使得艏向角ψ渐近跟踪视线角ψd，跟踪误差ye指数收敛到零；

对式(3)求解关于时间的导数，得：

其中，令r为虚拟控制输入，为r选择期望路径：

rd＝-cosψly(usinψ+vcosψ)-kφψecosψ(6)

其中kψ＞0，令r＝rd+re，将其代入式(6)中，得：

由反馈线性化控制器设计原理，反馈控制器为：

其中kr＞0，fr(v)是关于力的一个函数，且满足下式：

动态方程(7)和(9)是级联的线性系统：

4.步骤四的具体过程为：

x的动态方程可以表示为：

已知将其代入(11)可得：

其中：sinψ/ψ，(1-cosψ)/ψ和tan^-1(ψ)/ψ全局有界；

期望速度ud(t)满足下式：

ud(t)∈[umin+l,umax-l](13)

其中，umax＞umin＞0，l＞0；

ucj的协调控制律为：

式中，代表编队中第j个auv和第i个auv间的间距，g(x)是连续可微函数，g′(0)＞0，g(0)＝0。

5.当航行区域存在障碍物时，设计考虑环境约束的编队队形优化避障策略，步骤六的具体过程为：

使用多波束前视声呐对航行区域进行探测，探测区域的障碍物用下式表示：

pj＝(dj,θj),j＝0,...,ns-1(15)

式中：dj为auv和第j个障碍物间的间距，θj为auv和障碍物间连接线和xs轴之间的夹角；

以声呐的坐标原点为中心，在auv艏向方向设置一个半径为rd的扇形区域作为转动窗口，将转动窗口分成ms＝int(120/λ)+1个方向，λ代表步长l是每个栅格对角线的距离，可以用下式来表示所探测到的环境：

rj(j＝1,2,...,ms)表示障碍物和第j个方向上的auv间的距离，可以用下式计算得到各个矢量：

auv和期望点间的距离用ρm表示，当探测到有障碍物存在时会返回auv与障碍物间此刻的距离rj，对于auv周围的环境信息我们可以用向量表示，内的元素可以用下式表示：

建立队形参数矩阵表示各auv间关系和队形的形状参数，参数矩阵为：

esj＝[e1j,e2j,e3j,e4j]^t,j＝1,...,n(20)

其中：e^d代表某个队形形状的参数矩阵，esj代表第j个auvrj的状态信息，esj由四部分构成：e1j是某个跟随者rj的编号，e2j是rj在队形中所跟踪的领航者的编号；e3j表示跟随者rj和领航者之间要间隔固定的距离；e4j是跟随者rj和其它领航者间需间隔的固定角度，队形中r1为领航者，所以es1＝[1,0,0,0,0]^t；

当通过声呐设备测得环境信息的极坐标后，在转动窗口的视域内，当0≤j≤ns时，在j方向上存在障碍物，不满足时，无障碍物；可以分为障碍点集合和非障碍点集合：

其中，no代表auv执行任务区间内障碍物个数；

多auv编队在有障碍环境中执行任务时，若想能够根据现有环境约束进行队形优化变换控制，需要构建基于现有环境约束的环境适应度函数：

fenvfit＝hfdd(ec,et)+hecr(ec,et)+hfcct(ec,et)(24)

其中，hfdd(ec,et)表示编队整体通过含有障碍物区域时，原来初始的编队队形和经过变换后的期望队形两者之间几何关系的变化程度，hecr(ec,et)表示通过含有障碍物区域时，编队中成员在队形变换耗损的能量与编队通过障碍环境耗损的能量之间的比值，hfcct(ec,et)表示编队中成员进行队形变换的时间与编队整体通过障碍区域用的总时间之间的比值，

其中：et是想得到队形的参数阵，ec是想获得队形的参数阵，eδ＝et-ec是队形阵的改变量；

其中：kfs是队形变换时耗损的能量，kp时在完成队形变换后通过障碍环境耗损的能量，n是编队中auv的个数，τl,τr是auv左右主推的输出力矩，ψ是auv的艏向角；

tfs是编队成员进行队形变换所需要的时间，tp是在完成队形变换后编队整体通过障碍区域所需要的时间；

需要考虑现实环境因素和编队初始队形，来决定变换方式，因此将队形变换因子引入ζ到队形变换中：

其中：dmax是指障碍区域中障碍物之间的最大间隔，d是指原始队形中编队横向探测距离；

利用auv自身携带的声呐设备所探测到的障碍物信息，来计算队形变换因子ζ，根据它的值来判定进行哪种变换：

如果ζ＞1，说明障碍物之间的间隔足够大，编队整体可以保持现有队形通过此区域并完成对该区域的探测；

如果ζm＜ζ＜1，就说明编队整体不能保持原始队形通过，却可以通过减小编队队形的横向距离来通过；ζm为变换因子阈值；

如果ζ≤ζm，可知这时候的障碍物间的间距较窄，单个auv就可以完成对该区域的探测，要依据前面提出的环境适应度函数得出队形参数矩阵e'，来确定变换成哪种队形；

环境适应度函数将在中不断重复计算，确定出最终的队形拓扑结构，然后再利用步骤四中设计的编队协调控制器使各auv沿各自期望路径航行，最终可以实现在障碍区域中的队形最优变换。

与现有技术相比，本发明的有益效果是：本发明要解决的技术问题是多auv编队的形成，以及设计一种考虑环境约束的队形优化避障策略，提供一种障碍环境下多auv编队队形优化控制方法。1.针对传统避障方法中，未考虑现实环境约束及只能在固定队形间切换，对其它队形不具通用性的情形，设计了考虑环境约束队形优化避障策略，通过对环境适应度函数求解获得最优编队拓扑结构，保证编队可以在障碍环境中以最优队形通过此区域。2.具有实际应用价值。auv单体往往功能单一，应用会受到限制，多auv编队协同可以更高效的完成作业任务。编队作业可以用于海底石油管道的检修等。

本发明基于视距导航法建立路径跟踪误差模型，采用反馈线性法设计路径跟踪控制器，利用图论建立多auv间通信拓扑关系和协调误差模型，设计速度协调控制器，实现编队控制，针对障碍环境，设计了考虑环境约束队形优化避障策略，通过对环境适应度函数求解获得最优编队拓扑结构，保证编队可以在障碍环境中以最优队形通过此区域。

附图说明

图1是本发明的工作流程图；

图2是障碍环境设置及编队初始队形；

图3是各auv航行轨迹示意图；

图4是领航者速度和角速度示意图；

图5-图8是跟随者速度和角速度示意图；

图9是各auv的位置误差示意图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

结合图1至图9，本发明包括以下步骤：

1、步骤一：建立auv的水平面模型；

其中：mur＝m-yr，du＝-xuuu²-xvvv²，dv＝-yvuv-yv|v|v|v|，dr＝-nvuv-nv|v|v|v|-nrur。iz为固定矩；x{.}、y{.}和n{.}为水动力系数；xuu＝-35.4kg，xvv＝-128.4n·m²,yv＝-346kg，yv|v|＝-667kg·m^-1，yr＝435kg，nr＝-1427kgm,nv|v|＝443kg,nv＝-686kg,iz＝2000n·m²，f和t代表auv在纵向上的推力和转艏力矩，u为auv的前向运动速度，v为auv的横向运动速度，r代表auv的转艏速度，x代表auv横向位置，y代表auv纵向位置，ψ代表auv艏向角。

2、步骤二：设置编队auv的数量为n，初始时刻给每个auv赋初始位置、深度、速度等信息。

3、步骤三：基于步骤一所建立的auv水平面模型，基于视距导航法建立路径跟踪误差模型，利用反馈线性化方法设计路径跟踪控制器，使跟踪误差收敛到零，实现单个auv的路径跟踪。

针对所建立的模型，利用视距导航法构建单体的路径跟踪误差方程，

ψd(t)＝-arctan(ye(t)/d)(2)

其中，ψd是视线角，ψd(t)是某一时刻的视线角，ye(t)是某一时刻的纵向位置误差，d为间距(一般取auv长度的两倍)，d＝9m；

得到位移误差方程为：

ye＝y-yd,ψe＝ψ-ψd(3)

其中，y和ψ是auv实际的纵向位置和艏向角，yd和ψd是auv期望的纵向位置和视线角。

所以，对auv的直线跟踪问题可以转换为设计控制律，使下式成立：

基于视距导航法设计艏向控制器，使得艏向角ψ渐近跟踪视线角ψd，跟踪误差ye指数收敛到零。

对式(3)求解关于时间的导数，得：

其中，令r为虚拟控制输入，为r选择期望路径：

rd＝-cosψly(usinψ+vcosψ)-kφψecosψ(6)

其中kψ＞0，令r＝rd+re，将其代入式(6)中，得

由反馈线性化控制器设计原理，反馈控制器为

其中kr＞0，fr(v)是关于力的一个函数，且满足下式：

动态方程(7)和(9)是级联的线性系统，

4、步骤四：在步骤三的基础上，利用图论建立多auv间的通信拓扑关系，依据获取的其它auv的状态信息，设计速度协调控制器，实现多auv间的协调控制。实施过程如下：

x的动态方程可以表示为：

已知将其代入(11)可得

其中

sinψ/ψ，(1-cosψ)/ψ和tan^-1(ψ)/ψ全局有界。

利用图论建立多auv间的通信拓扑关系，依据获取的其它auv的状态信息，设计控制律ucj(j＝1,...,n)，可以保证编队中auv既满足速度参考指令的约束又实现编队协调控制，auv需要调节自己的纵向速度到期望速度ud(t)，来实现期望编队队形，即在未到达期望速度ud(t)前，速度较慢的auv需要加快速度，速度较快的auv需要降低速度，最后形成期望队形，一旦期望的编队队形形成后，以ud(t)继续航行，可保持期望队形。

期望速度ud(t)满足下式：

ud(t)∈[umin+l,umax-l](13)

其中，其中，umax＝10，umin＝2，l＝1。

ucj的协调控制律为：

式中，代表编队中第j个auv和第i个auv间的间距。g(x)是连续可微函数，g′(0)＞0，g(0)＝0。

5、步骤五：判定航行区域是否存在障碍物，若不存在障碍物，保持此队形航行至目标点；当航行区域存在障碍物时，设计考虑环境约束的编队队形优化避障策略，通过求解环境适应度函数获得最优编队拓扑结构，保证编队以最优队形通过此障碍区域；

使用多波束前视声呐对航行区域进行探测，探测区域的障碍物用下式表示：

pj＝(dj,θj),j＝0,...,ns-1(15)

式中，dj为auv和第j个障碍物间的间距，θj为auv和障碍物间连接线和xs轴之间的夹角。

以声呐的坐标原点为中心，在auv艏向方向设置一个半径为rd的扇形区域作为转动窗口，将转动窗口分成ms＝int(120/λ)+1个方向，λ代表步长l是每个栅格对角线的距离，可以用下式来表示所探测到的环境。

rj(j＝1,2,...,ms)表示障碍物和第j个方向上的auv间的距离，可以用下式计算得到各个矢量：

auv和期望点间的距离用ρm表示，当探测到有障碍物存在时会返回auv与障碍物间此刻的距离rj，对于auv周围的环境信息我们可以用向量表示，内的元素可以用下式表示:

建立队形参数矩阵表示各auv间关系和队形的形状参数，参数矩阵为：

esj＝[e1j,e2j,e3j,e4j]^t,j＝1,...,n(20)

e^d代表某个队形形状的参数矩阵，esj代表第j个auvrj的状态信息，esj由四部分构成：e1j是某个跟随者rj的编号，e2j是rj在队形中所跟踪的领航者的编号；e3j表示跟随者rj和领航者之间要间隔固定的距离；e4j是跟随者rj和其它领航者间需间隔的固定角度，队形中r1为领航者，所以es1＝[1,0,0,0,0]^t。

当通过声呐设备测得环境信息的极坐标后，在转动窗口的视域内，当0≤j≤ns时，在j方向上存在障碍物，不满足时，无障碍物。可以分为障碍点集合和非障碍点集合：

其中，no代表auv执行任务区间内障碍物个数。

多auv编队在有障碍环境中执行任务时，若想能够根据现有环境约束进行队形优化变换控制，需要构建基于现有环境约束的环境适应度函数

fenvfit＝hfdd(ec,et)+hecr(ec,et)+hfcct(ec,et)(24)

其中，hfdd(ec,et)表示编队整体通过含有障碍物区域时，原来初始的编队队形和经过变换后的期望队形两者之间几何关系的变化程度，hecr(ec,et)表示通过含有障碍物区域时，编队中成员在队形变换耗损的能量与编队通过障碍环境耗损的能量之间的比值，hfcct(ec,et)表示编队中成员进行队形变换的时间与编队整体通过障碍区域用的总时间之间的比值。

et是想得到队形的参数阵，ec是想获得队形的参数阵，eδ＝et-ec是队形阵的改变量。

kfs是队形变换时耗损的能量，kp时在完成队形变换后通过障碍环境耗损的能量，n是编队中auv的个数，τl,τr是auv左右主推的输出力矩，ψ是auv的艏向角。

tfs是编队成员进行队形变换所需要的时间，tp是在完成队形变换后编队整体通过障碍区域所需要的时间。

需要考虑现实环境因素和编队初始队形，来决定变换方式，因此将队形变换因子引入到队形变换中：

其中，dmax是指障碍区域中障碍物之间的最大间隔，d是指原始队形中编队横向探测距离。

利用auv自身携带的声呐设备所探测到的障碍物信息，来计算队形变换因子根据它的值来判定进行哪种变换。

如果说明障碍物之间的间隔足够大，编队整体可以保持现有队形通过此区域并完成对该区域的探测。

如果就说明编队整体不能保持原始队形通过，却可以通过减小编队队形的横向距离来通过。为变换因子阈值。

如果可知这时候的障碍物间的间距较窄，单个auv就可以完成对该区域的探测，要依据前面提出的环境适应度函数得出队形参数矩阵e'，来确定变换成哪种队形。

环境适应度函数将在中不断重复计算，确定出最终的队形拓扑结构，然后再利用步骤四中设计的编队协调控制器使各auv沿各自期望路径航行，最终可以实现在障碍区域中的队形最优变换。

6、步骤六：判定编队中auv是否都通过障碍区域，若已都通过，进行队形恢复，继续执行后续任务，若仍有一部分没有通过障碍区域，返回步骤五，重新求解环境适应度函数，继续执行；保持此队形航行至目标点。

下面结合具体数值给出本发明的具体实施例：步骤如下：

步骤一：建立auv的水平面模型：

其中：mur＝m-yr，du＝-xuuu²-xvvv²，dv＝-yvuv-yv|v|v|v|，dr＝-nvuv-nv|v|v|v|-nrur。iz为固定矩；x{.}、y{.}和n{.}为水动力系数；f和t代表auv在纵向上的推力和转艏力矩，u为auv的前向运动速度，v为auv的横向运动速度，r代表auv的转艏速度，x代表auv横向位置，y代表auv纵向位置，ψ代表auv艏向角。

其中：

m＝2234.5kg,xvv＝-128.4kg·m^-1,xuu＝-35.4kg·m^-1,yv|v|＝-667kg·m^-1,yv＝-346kg,yr＝435kg,nr＝-1427kg·m,nv|v|＝443kg,nv＝-686kg,iz＝2000n·m²

步骤二：初始化编队信息

本案例以5个auv为例进行说明，设置编队数量为5，设置每个auv的初始位置和速度等信息。

步骤三：单个auv的路径跟踪控制器设计

假定领航者和跟随者的控制器都是一样的，利用视距导航法构建单体的路径跟踪误差方程，

ψd(t)＝-arctan(ye(t)/d)(2)

其中，ψd是视线角。

得到位移误差方程为：

ye＝y-yd,ψe＝ψ-ψd(3)

其中，y和ψ是auv实际的纵向位置和艏向角，yd和ψd是auv期望的纵向位置和视线角。

所以，对auv的直线跟踪问题可以转换为设计控制律，使下式成立

基于视距导航法对艏向角ψ设计控制器，使得艏向角ψ渐近跟踪视线角ψd，跟踪误差ye指数收敛到零。

对式(3)求解关于时间的导数，得：

其中，令r为虚拟控制输入，为r选择期望路径：

rd＝-cosψly(usinψ+vcosψ)-kφψecosψ(6)

其中kψ＝4，令r＝rd+re，将其代入式(6)中，得

由反馈线性化控制器设计原理，反馈控制器为

其中kr＝40，且满足下式：

动态方程(7)和(9)是级联的线性系统，

步骤四：编队协调控制器设计

x的动态方程可以表示为：

已知将其代入(11)可得

其中

sinψ/ψ，(1-cosψ)/ψ和tan^-1(ψ)/ψ全局有界。

利用图论建立多auv间的通信拓扑关系，依据获取的其它auv的状态信息，设计控制律ucj(j＝1,...,n)，可以保证编队中auv既满足速度参考指令的约束又实现编队协调控制，auv需要调节自己的纵向速度到期望速度ud(t)，来实现期望编队队形，即在未到达期望速度ud(t)前，速度较慢的auv需要加快速度，速度较快的auv需要降低速度，最后形成期望队形，一旦期望的编队队形形成后，以ud(t)继续航行，可保持期望队形。

期望速度ud(t)满足下式：

ud(t)∈[umin+l,umax-l](13)

其中，umax＝10，umin＝2，l＝1。

ucj的协调控制律为：

式中，代表编队中第j个auv和第i个auv间的间距。g(x)是连续可微函数，g′(0)＞0，g(0)＝0，g(x)＝2l/πtan^-1(x)。

步骤五：使用多波束前视声呐对航行区域进行探测，探测区域的障碍物用下式表示：

pj＝(dj,θj),j＝0,...,ns-1(15)

式中，dj为auv和第j个障碍物间的间距，θj为auv和障碍物间连接线和xs轴之间的夹角。

以声呐的坐标原点为中心，在auv艏向方向设置一个半径为rd＝100m的扇形区域作为转动窗口，将转动窗口分成ms＝int(120/λ)+1个方向，λ代表步长λ≤arccos(1-l²/rd²)，l＝20，可以用下式来表示所探测到的环境。

rj(j＝1,2,...,ms)表示障碍物和第j个方向上的auv间的距离，可以用下式计算得到各个矢量：

auv和期望点间的距离用ρm表示，当探测到有障碍物存在时会返回auv与障碍物间此刻的距离rj，对于auv周围的环境信息我们可以用向量表示，内的元素可以用下式表示:

建立队形参数矩阵表示各auv间关系和队形的形状参数，参数矩阵为：

领航auv的参考路径为[xz,yz]＝[(0,0),(0,1300)]，初始位置为p1＝(0,0)，p2＝(-200,0)，p3＝(200,0)，p4＝(-350,0)，p5＝(350,0)，编队的期望航速是ud＝5m/s,初始纵向速度为uj(0)＝1m/s,j＝1,2,3,4,5，初始横向速度为零，初始艏向角ψ＝π/2。

多auv编队在执行任务时的初始队形设定为直线形，仿真障碍环境如图2所示，共包含a、b、c三个区域，每个区域的宽度值各不相同。仿真中用蓝色扇形滚动窗口模拟声呐设备对环境中的障碍物信息进行收集，由收集到的信息和前面提出的策略，进行队形优化完成对含有障碍物区域的探测任务。

从图3中看出，多auv的初始编队队形为直线形，当0≤y≤150时，保持直线队形航行，完成对其经过区域的探测，然后检测到航行路线上有障碍物，并获得可通行的最大距离dmax＝700m，此时编队并没有以传统的序列形式通过障碍区域，而是考虑编队整体的探测距离和环境约束后，对队形进行收缩变换，以三角队形通过障碍区域，当行进至600m附近时，在检测范围内未发现障碍物，所以队形恢复为初始的直线队形，航行至800m附近时，检测到存在障碍物，且最大通行宽度为dmax＝500m，此时想要完成对此区域的探测，直线形队形显然已经不合适，此时会根据前面提出的策略进行队形优化选择，得到编队队形丢失度hfdd＝0.45、能量耗损率iecr＝0.19、变换队形所需时间比ifcct＝50s，可以求得评价函数fenvfit＝14.8，通过和队形数据库中的评价函数相比较，可得双排形最为合适，编队保持此队形通过障碍区域。图4是领航者的速度与角速度曲线，从图中可以看出，可以很快到达期望速度，并保持此速度航行，图5-图8是各个跟随者的速度与角速度曲线，可知，因为存在队形变换，所以速度会出现波动，但最后都收敛到期望速度。图9是各auv的位置误差曲线，从图中可以看出，在航行过程中，存在偏差，最终都收敛到零，可以保持编队稳定航行。

从上述仿真结果中，可以看出，auv编队可以根据环境约束对队形变换模式进行选择，并可以对队形进行优化变换，使得多auv编队以最优编队队形完成对存在障碍物的区域的探测任务。

综上，本发明提出一种障碍环境下多auv编队队形优化控制策略，属于船舶控制技术领域。本发明基于视距导航法建立路径跟踪误差模型，采用反馈线性法设计路径跟踪控制器，利用图论建立多auv间通信拓扑关系和协调误差模型，设计速度协调控制器，实现编队控制，针对障碍环境，设计了考虑环境约束队形优化避障策略，通过对环境适应度函数求解获得最优编队拓扑结构，保证编队可以在障碍环境中以最优队形通过此区域。