中国舰船研究

Input：

$ {{\boldsymbol{q}}_{{\text{start}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $

Result：

$ \{ {{\boldsymbol{q}}_1},{{\boldsymbol{q}}_2}, \cdots ,{{\boldsymbol{q}}_K}\} $

$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $

←

$ {\text{KDTre}}{{\text{e}}_{{\text{2D}}}} $

;

reach_goal←false

while (

$ i \lt {i_{\max }} $

and

$ t \lt {t_{\max }} $

) do

if CurPhase == Phase1 then

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

←Sample2DState();

else

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

←SampleFullState();

$ {{\boldsymbol{q}}_{{\text{near}}}} $

←FindNearest(KDTree,

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

);

$ {{\boldsymbol{q}}_{{\text{new}}}} $

←Step(

$ {{\boldsymbol{q}}_{{\text{near}}}},{{\boldsymbol{q}}_{{\text{rand}}}} $

);

if IsStateValid(

$ {{\boldsymbol{q}}_{{\text{new}}}} $

) then

$ {{\boldsymbol{q}}_{{\text{cur}}}} $

←ChooseParentAndRewire(

$ {{\boldsymbol{q}}_{{\text{new}}}} $

);

$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $

.Insert(

$ {{\boldsymbol{q}}_{{\text{cur}}}} $

);

if IsNearGoal(

$ {{\boldsymbol{q}}_{{\text{cur}}}} $

)&&IsLineFree(

$ {{\boldsymbol{q}}_{{\text{cur}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $

) then

$ {\text{C}} $

←

$ {{\boldsymbol{q}}_{{\text{cur}}}}.{\text{Cost}} $

+Cost(

$ {{\boldsymbol{q}}_{{\text{cur}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $

);

if reach_goal == false then

reach_goal←true;

changePhaseTo2();

$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $

←

$ {\text{KDTre}}{{\text{e}}_{5{\text{D}}}} $

;

else

CheckandUpdateCost(

$ {\text{C}} $

);

retrun tracebackFormGoal(

$ {{\boldsymbol{q}}_{{\text{goal}}}} $

)

Input：

$ {{\boldsymbol{q}}_{{\text{start}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $

Result：

$ \{ {{\boldsymbol{q}}_1},{{\boldsymbol{q}}_2}, \cdots ,{{\boldsymbol{q}}_K}\} $

$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $

←

$ {\text{KDTre}}{{\text{e}}_{{\text{2D}}}} $

;

reach_goal←false

while (

$ i \lt {i_{\max }} $

and

$ t \lt {t_{\max }} $

) do

if CurPhase == Phase1 then

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

←Sample2DState();

else

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

←SampleFullState();

$ {{\boldsymbol{q}}_{{\text{near}}}} $

←FindNearest(KDTree,

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

);

$ {{\boldsymbol{q}}_{{\text{new}}}} $

←Step(

$ {{\boldsymbol{q}}_{{\text{near}}}},{{\boldsymbol{q}}_{{\text{rand}}}} $

);

if IsStateValid(

$ {{\boldsymbol{q}}_{{\text{new}}}} $

) then

$ {{\boldsymbol{q}}_{{\text{cur}}}} $

←ChooseParentAndRewire(

$ {{\boldsymbol{q}}_{{\text{new}}}} $

);

$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $

.Insert(

$ {{\boldsymbol{q}}_{{\text{cur}}}} $

);

if IsNearGoal(

$ {{\boldsymbol{q}}_{{\text{cur}}}} $

)&&IsLineFree(

$ {{\boldsymbol{q}}_{{\text{cur}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $

) then

$ {\text{C}} $

←

$ {{\boldsymbol{q}}_{{\text{cur}}}}.{\text{Cost}} $

+Cost(

$ {{\boldsymbol{q}}_{{\text{cur}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $

);

if reach_goal == false then

reach_goal←true;

changePhaseTo2();

$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $

←

$ {\text{KDTre}}{{\text{e}}_{5{\text{D}}}} $

;

else

CheckandUpdateCost(

$ {\text{C}} $

);

retrun tracebackFormGoal(

$ {{\boldsymbol{q}}_{{\text{goal}}}} $

)

UUV	参数	属性	数值
UUV05	速度/(m∙s⁻¹)	平均值	0.188 9
标准差	0.030 3
最大值	0.267 8
加速度/(m∙s⁻²)	平均值	0.092 0
标准差	0.149 7
最大值	0.923 8
UUV06	速度/(m∙s⁻¹)	平均值	0.190 3
标准差	0.033 0
最大值	0.269 5
加速度/(m∙s⁻²)	平均值	0.092 0
标准差	0.145 9
最大值	0.895 5
UUV07	速度/(m∙s⁻¹)	平均值	0.189 9
标准差	0.026 2
最大值	0.279 7
加速度/(m∙s⁻¹)	平均值	0.089 0
标准差	0.139 2
最大值	0.073 9

UUV

参数

属性

数值

UUV05

速度/(m∙s⁻¹)

平均值

0.188 9

标准差

0.030 3

最大值

0.267 8

加速度/(m∙s⁻²)

平均值

0.092 0

标准差

0.149 7

最大值

0.923 8

UUV06

速度/(m∙s⁻¹)

平均值

0.190 3

标准差

0.033 0

最大值

0.269 5

加速度/(m∙s⁻²)

平均值

0.092 0

标准差

0.145 9

最大值

0.895 5

UUV07

速度/(m∙s⁻¹)

平均值

0.189 9

标准差

0.026 2

最大值

0.279 7

加速度/(m∙s⁻¹)

平均值

0.089 0

标准差

0.139 2

最大值

0.073 9

UUV	参数	属性	数值
UUV05	速度/(m∙s⁻¹)	平均值	0.188 9
标准差	0.030 3
最大值	0.267 8
加速度/(m∙s⁻²)	平均值	0.092 0
标准差	0.149 7
最大值	0.923 8
UUV06	速度/(m∙s⁻¹)	平均值	0.190 3
标准差	0.033 0
最大值	0.269 5
加速度/(m∙s⁻²)	平均值	0.092 0
标准差	0.145 9
最大值	0.895 5
UUV07	速度/(m∙s⁻¹)	平均值	0.189 9
标准差	0.026 2
最大值	0.279 7
加速度/(m∙s⁻¹)	平均值	0.089 0
标准差	0.139 2
最大值	0.073 9

UUV

参数

属性

数值

UUV05

速度/(m∙s⁻¹)

平均值

0.188 9

标准差

0.030 3

最大值

0.267 8

加速度/(m∙s⁻²)

平均值

0.092 0

标准差

0.149 7

最大值

0.923 8

UUV06

速度/(m∙s⁻¹)

平均值

0.190 3

标准差

0.033 0

最大值

0.269 5

加速度/(m∙s⁻²)

平均值

0.092 0

标准差

0.145 9

最大值

0.895 5

UUV07

速度/(m∙s⁻¹)

平均值

0.189 9

标准差

0.026 2

最大值

0.279 7

加速度/(m∙s⁻¹)

平均值

0.089 0

标准差

0.139 2

最大值

0.073 9

基于仿射变换的多UUV自适应队形变换路径规划方法

PDF下载

郭凯 , 姚明启 , 乔磊 ^*

中国舰船研究 | 水下无人系统总体设计技术 2026,21(2): 101-111

收起

中国舰船研究 | 水下无人系统总体设计技术 2026, 21(2): 101-111

基于仿射变换的多UUV自适应队形变换路径规划方法

全屏

郭凯, 姚明启, 乔磊^*

作者信息

上海交通大学船舶海洋与建筑工程学院，上海 200240

郭凯，男，2001年生，硕士生。研究方向：海洋机器人协同编队规划。E-mail：guokai2023@sjtu.edu.cn

乔磊，男，1989年生，博士，副教授，博士生导师。研究方向：海洋智能机器人和无人系统。E-mail：qiaolei@sjtu.edu.cn

通讯作者:

^* 乔磊

A path planning method for adaptive formation reshaping of multi-UUVs based on affine transformation

Kai GUO, Mingqi YAO, Lei QIAO^*

Affiliations

School of Ocean and Civil Engineering, Shanghai Jiao Tong University, Shanghai 200240, China

出版时间: 2026-04-30 doi: 10.19693/j.issn.1673-3185.04560

文章导航

摘要

收起

目的

针对多无人潜航器（UUV）编队在复杂水下环境中面临的编队保持与灵活避障难以同时实现的难题，提出一种能够自适应队形变换的全局路径规划方法。

方法

基于仿射变换框架，将多UUV系统的协同路径规划问题映射至二维平面仿射参数空间进行求解。首先，采用一种改进的快速搜索随机树^*（RRT*）算法进行前端路径搜索，该算法通过整合快速探索与迭代优化阶段、加权KD（k-dimensional）树、混合采样机制以及自适应采样参数调整，实现初始仿射状态序列的高效生成；然后，利用基于B样条曲线的后端优化器，采用梯度下降法最小化一个综合考虑轨迹平滑性、UUV运动学可行性、环境碰撞安全性以及自适应编队缩放代价的目标函数，优化得到连续光滑且满足多重约束的仿射参数轨迹。

结果

湖上试验结果表明，所提规划方法能够生成安全、可行的编队路径，能成功引导多UUV编队穿越模拟的狭窄障碍区域，且UUV的实际速度和加速度均满足设定的可行性约束。

结论

所提的基于仿射变换的全局规划方法能够通过自适应队形变换为航行于复杂障碍场景下的多UUV编队提供安全可行的编队路径，实现多UUV编队的安全航行，同时，也能显著提升海洋无人潜航器的自主性与环境适应性，对推动海洋无人装备技术的发展和实际应用具有积极意义。

关键词

无人潜航器 / 编队路径规划 / 仿射变换 / 快速搜索随机树* / 轨迹优化

Abstract

收起

Objective

To address the challenge of simultaneously maintaining formation integrity and enabling flexible obstacle avoidance for multi-unmanned underwater vehicle (multi-UUV) formations in complex underwater environments, this paper proposes a global path planning method that supports adaptive formation reshaping.

Method

The proposed method is built upon an affine transformation framework that maps the cooperative path planning problem of the multi-UUV system into a two-dimensional affine parameter space. First, a front-end path search is conducted using an improved rapidly-exploring random tree* (RRT*) algorithm. By integrating fast exploration and iterative optimization phases, a weighted k-dimensional (KD) tree, a hybrid sampling mechanism, and adaptive tuning of sampling parameters, this algorithm efficiently generates an initial sequence of affine states. Subsequently, a B-spline-based back-end optimizer employs a gradient descent method to minimize a comprehensive objective function that accounts for trajectory smoothness, UUV kinematic feasibility, environmental collision safety, and the cost associated with adaptive formation scaling. The optimization process yields a continuous and smooth trajectory of affine parameters that satisfies multiple constraints.

Results

Lake experiments demonstrate that the proposed planning method can generate safe and feasible formation paths. It successfully guided the multi-UUV formation through a simulated narrow obstacle region, while the actual velocities and accelerations of the UUVs remained within the predefined feasibility constraints.

Conclusion

The proposed global planning method, based on affine transformation, effectively generates safe and feasible paths for multi-UUV formations navigating complex obstacle environments by enabling adaptive formation reshaping. This method significantly enhances the autonomy and environmental adaptability of marine unmanned vehicles, and holds great value for advancing the development and practical application of marine unmanned systems technology.

Key words

unmanned underwater vehicle / formation path planning / affine transformation / rapidly-exploring random tree* / trajectory optimization

引用本文

郭凯, 姚明启, 乔磊. 基于仿射变换的多UUV自适应队形变换路径规划方法. 中国舰船研究, 2026 , 21 (2) : 101 -111 . DOI: 10.19693/j.issn.1673-3185.04560

Kai GUO, Mingqi YAO, Lei QIAO. A path planning method for adaptive formation reshaping of multi-UUVs based on affine transformation[J]. Chinese Journal of Ship Research, 2026 , 21 (2) : 101 -111 . DOI: 10.19693/j.issn.1673-3185.04560

正文

收起

0　引　言

收起

随着海洋探索与开发任务的日益复杂，单个无人潜航器（unmanned underwater vehicle，UUV）在感知范围、续航能力、任务适应性以及应急处理方面开始显现出其局限性^[1-2]。多UUV通过分布式协作与协同控制，能够显著提升任务的执行效率，扩大作业范围，增强系统鲁棒性与容错能力，在海洋环境监测、资源勘探、水下搜救和安全巡逻等领域具有重要的应用价值^[3-4]。然而，复杂多变的水下环境，特别是障碍物密集的区域，给多UUV编队协同作业提出了严峻的挑战。因此，如何在保持编队稳定性的同时实现灵活、自主、高效的队形变换以规避障碍物，是提升多UUV编队系统环境适应性和任务安全性的关键。

理想的多智能体编队系统需要能在规避障碍的同时，还能维持必要的编队结构或是根据环境约束灵活调整队形^[5]。针对多智能体编队避障问题，一种主流方法是基于个体轨迹优化的分布式策略，即通过为编队内每个个体独立规划避障轨迹，并在优化目标中加入队形约束项来协调避障与编队保持^[6-7]，试图在个体避障与整体队形保持间取得平衡。但在密集障碍或通信受限的环境下，易引发编队变形，如个体避障绕行导致的编队解列。对于水下通信带宽有限且存在延迟、遮挡的UUV系统，这种依赖个体频繁信息交互和独立决策的分布式方法稳定性较差。另一类方法侧重于将编队作为一个整体进行避障规划，即将编队视为一个虚拟的刚性结构或凸包区域^[8-9]，通过规划该整体区域的路径或优化其中心位置来避开障碍物。这种方法简化了问题并能严格维持预设队形，但主要缺陷在于缺乏局部调整能力，即使编队中仅有少数个体面临碰撞风险，也需要整个编队进行统一的避让机动，这不仅会导致路径过度保守，也使其难以适应需要精细局部调整的复杂障碍环境。还有一些学者针对通过调整编队队形本身实现避障，在触发特定环境条件时切换至适配队形以通过障碍进行了探索^[10]。然而，该方法受限于预设队形库的完备性以及切换算法的实时性，因队形类别不足难以应对复杂障碍布局，而库容过大又会导致计算效率下降，难以满足动态环境响应需求。

为了实现更平滑、灵活、连续的队形调整以适应避障需求，将仿射变换（affine transformation，AT）理论引入了编队机动控制领域^[11-14]。仿射变换是一种几何变换，能够保持几何形状的平行性，由该变换定义的编队构型可以统一进行缩放、旋转、平移和剪切等几何变换操作，从而改变编队的整体形态和朝向，这为编队适应性避障提供了新的可能性。Pang等^[11]和Zhao^[12]利用仿射编队结构实现了UUV编队的整体缩放与旋转，并结合人工势场法引导编队绕过了障碍物。但变换参数依赖于预设规则或领导者路径，缺乏对环境信息的主动响应。因此，本文拟针对现有方法在自主适应环境进行队形变换避障方面的问题，提出一种基于仿射变换的多UUV全局规划方法。首先，将编队轨迹规划问题映射到仿射参数空间，通过改进的快速搜索随机树*（rapidly-exploring random tree*，RRT*）算法为多UUV编队生成考虑队形变换的全局初始路径点；然后，在仿射参数空间进行参数轨迹优化，以生成既满足避障、避碰约束，又保证队形变换平滑连续的编队路径。

1　问题描述

收起

1.1　UUV运动学及动力学模型

本文将研究UUV在恒定深度下的水平面运动，并只考虑纵荡、横荡和艏摇三个自由度。以第j个UUV为例，其坐标系定义和UUV的状态如图1所示。图中：

$ {O_{\rm{E}}} $

表示惯性坐标系，所有UUV共用一个惯性坐标系；

$ O_{\rm{B}}^j $

表示第j个UUV的机体坐标系；

$ {x_j},{y_j},{\psi _j} $

分别为UUV在固定坐标系下的纵、横向位置和艏向角；

$ {u_j},{v_j} $

分别为UUV在载体坐标系下的纵荡和横荡速度；

$ {{\boldsymbol{V}}_j}{\text{ = }}{\left[ {{u_j},{v_j}} \right]^{\text{T}}} $

，为二维速度矢量。根据文献[15]中建立的UUV数学模型，本文的三自由度运动学方程定义如下：

$ {\dot {\boldsymbol{\eta}} _j} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{R}}_j}\left( {{\psi _j}} \right)}&{{{\boldsymbol{0}}_{2 \times 1}}} \\ {{{\boldsymbol{0}}_{1 \times 2}}}&1 \end{array}} \right]{{\boldsymbol{v}}_j} $

(1)

式中：

$ {{\boldsymbol{\eta}} _j} = {[{x_j},{y_j},{\psi _j}]^{\text{T}}} $

，

$ {{\boldsymbol{\nu}} _j} = {\left[ {{u_j},{v_j},{r_j}} \right]^{\text{T}}} $

，其中

$ {r_j} $

为艏向角速度；

$ {{\boldsymbol{R}}_j}\left( {{\psi _j}} \right) $

为坐标旋转矩阵。

第j个UUV的三自由度动力学方程定义如下：

$ \left\{ \begin{gathered} {{\boldsymbol{M}}_j}{{\dot v}_j} + {{\boldsymbol{C}}_j}({{\boldsymbol{v}}_j}){{\boldsymbol{v}}_j} + {{\boldsymbol{D}}_j}({{\boldsymbol{v}}_j}){{\boldsymbol{v}}_j} = {{\boldsymbol{\tau}} _j} + {\boldsymbol{\tau}} _{\text{d}}^j \\ {{\boldsymbol{\tau}} _j} = {{\boldsymbol{T}}_j}{{\boldsymbol{u}}_j} \\ \end{gathered} \right. $

(2)

式中：

$ {{\boldsymbol{M}}_j} \in {\mathbb{R}^{3 \times 3}} $

，为惯性矩阵，包括刚体惯性和水动力附加质量；

$ {{\boldsymbol{C}}_j}\left( {{v_j}} \right) $

，

$ {{\boldsymbol{D}}_j}\left( {{v_j}} \right) $

分别为科里奥利和向心力矩阵以及水动力阻尼矩阵；

$ {{\boldsymbol{\tau }}_j} = {[{X_j},{Y_j},{N_j}]^{\text{T}}} $

，为由

$ P_{1} $

～

$ P_{4} $

推进器推力在纵向、横向和垂向产生的合力向量；

$ {\boldsymbol{\tau}} _{\text{d}}^j $

为环境扰动；

$ {\boldsymbol{T}}_{j} \in \mathbb{R}^{3 \times 4} $

，为推进器的推力分配矩阵；

$ {{\boldsymbol{u}}_j} = {\left[ {{P_1},{P_2},{P_3},{P_4}} \right]^{\text{T}}} $

，为4个推进器上的推力向量。

1.2　仿射编队与规划目标

为了对多UUV目标编队进行统一的几何变换，将采用基于齐次坐标的二维仿射变换^[11]描述目标编队构型的动态变化。定义一组包含

${N_{\text{U}}}$

（

${N_{\text{U}}} \geqslant 3$

）个UUV的编队系统，仿射编队结构允许目标编队在标称编队构型的基础上进行平移、旋转、缩放和剪切等操作，也即任意时刻

$ t $

的目标编队构型

$ {{\boldsymbol{\tilde P}}^ * }(t) $

是标称编队构型

$ {{\boldsymbol{\tilde P}}^0} $

的一种仿射变换，计算方式如下：

$ {{\boldsymbol{\tilde P}}^ * }\left( t \right) = {\boldsymbol{A}}(t){{\boldsymbol{\tilde P}}^0} $

(3)

$ {{\boldsymbol{\tilde p}}^*}_j(t) = \left[ {\begin{array}{*{20}{c}} {x_j^ * (t)} \\ {y_j^ * (t)} \\ 1 \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{A_1}}&{{A_3}}&{{A_5}} \\ {{A_2}}&{{A_4}}&{{A_6}} \\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {x_j^0} \\ {y_j^0} \\ 1 \end{array}} \right] = {\boldsymbol{A}}(t){{\boldsymbol{\tilde p}}^0}_j $

(4)

式中：

$ {{\boldsymbol{\tilde P}}^0} = [ {{\boldsymbol{\tilde p}}_1^0(t),{\boldsymbol{\tilde p}}_2^0(t), \cdots ,{\boldsymbol{\tilde p}}_{{N_{\text{U}}}}^0(t)} ] \in {\mathbb{R}^{3 \times {N_{\text{U}}}}} $

；

$ {{\boldsymbol{\tilde P}}^ * }(t) = [ {\boldsymbol{\tilde p}}_1^ * (t), {\boldsymbol{\tilde p}}_2^ * (t), \cdots ,{\boldsymbol{\tilde p}}_{{N_{\text{U}}}}^ * (t) ] \in {\mathbb{R}^{3 \times {N_{\text{U}}}}} $

，其中

$ {{\boldsymbol{\tilde p}}^m}_j(t) = {[x_j^m(t),y_j^m(t),1]^{\text{T}}}, m = \{ 0,*\} ,j \in \left[ {1,{N_{\text{U}}}} \right] $

，为第j个UUV在t时刻的二维齐次坐标；

${A_1}\sim {A_6}$

为

$ {\boldsymbol{A}}\left( t \right) $

中的时变参数；

$ {\boldsymbol{A}}\left( t \right) \in {\mathbb{R}^{3 \times 3}} $

，为二维齐次仿射变换矩阵，其定义如下^[11]：

$ {\boldsymbol{A}}(t) = \lambda (t){{\boldsymbol{A}}_{\text{T}}}(t){{\boldsymbol{A}}_{\text{R}}}(t){{\boldsymbol{A}}_{{\text{Sh}}}}(t){{\boldsymbol{A}}_{\text{S}}}(t) $

(5)

式中：

$ \lambda $

为整体缩放因子；

$ {{\boldsymbol{A}}_{\text{T}}}(t) $

由平移参数

$ {t_x}(t) $

和

$ {t_y}(t) $

定义，表示编队整体在二维平面的位移；

$ {{\boldsymbol{A}}_{\text{R}}}(t) = {{\boldsymbol{A}}_{\text{R}}}(\theta (t)) $

，由角度

$ \theta (t) $

定义，表示编队相对参考编队方向的旋转角度；

$ {{\boldsymbol{A}}_{{\text{Sh}}}}(t) $

由x与y方向的倾斜参数

$ s{h_{xy}}(t) $

和

$ s{h_{y{\text{x}}}}(t) $

定义，为编队产生的倾斜和变形；

$ {{\boldsymbol{A}}_{\text{S}}}(t) $

由沿x与y轴的缩放参数

$ {s_x}(t) $

和

$ {s_y}(t) $

定义，使编队分别沿x轴和y轴进行独立的缩放。

为简化规划问题的维度和复杂度，将仿射变换简化为旋转、缩放和平移。定义规划的仿射状态为

$ {\boldsymbol{q}}(t) = {\left[ {{t_x}(t),{t_y}(t),\theta (t),{s_x}(t),{s_y}(t)} \right]^{\text{T}}} \in {\mathbb{R}^5} $

(6)

规划算法的目标是找到一条从初始规划状态

$ {\boldsymbol{q}}(0) $

到目标规划状态

$ {\boldsymbol{q}}({\text{T}}) $

的连续、可行轨迹

$ {\mathcal{T}_{\boldsymbol{A}}} = \{ {\boldsymbol{q}}(t)|t \in [0,T]\} $

，其他仿射参数（整体缩放因子和剪切量）由任务定义并在规划阶段保持固定。因此，式（3）和式（4）可以进一步表述成

$ {\boldsymbol{q}}(t) $

和

$ {{\boldsymbol{\tilde P}}^0} $

的仿射映射关系函数

$ {f_{{\text{af}}}}{\text{(}} \cdot ) $

：

$ {{\boldsymbol{\tilde P}}^ * }(t) = {\boldsymbol{A}}({\boldsymbol{q}}(t)){{\boldsymbol{\tilde P}}^0} = {f_{{\text{af}}}}{\text{(}}{\boldsymbol{q}}(t), {{\boldsymbol{\tilde P}}^0}) $

(7)

$ {{\boldsymbol{\tilde p}}^*}_j(t) = {\boldsymbol{A}}({\boldsymbol{q}}(t)){{\boldsymbol{\tilde p}}^0}_j = {f_{{\text{af}}}}{\text{(}}{\boldsymbol{q}}(t), {{\boldsymbol{\tilde p}}^0}_j) $

(8)

为确保生成的轨迹在复杂水域环境中安全、可行且高效，这条仿射参数轨迹必须满足以下多重约束：

1） UUV运动学约束，即每个UUV的速度、加速度必须满足其动力学特性限制（

$ {\left\| {{{\boldsymbol{V}}_j}} \right\|_2} \leqslant {V_{\max }}, {\left\| {{{\boldsymbol{A}}_j}} \right\|_2} \leqslant {A_{\max }} $

）；

2）编队中任意UUV与环境障碍物的距离

$ \{ {{\boldsymbol{q}}_1},{{\boldsymbol{q}}_2}, \cdots ,{{\boldsymbol{q}}_K}\} $

必须大于安全阈值

$ {d_{{\text{safe}}}} $

，且编队内部个体间需保持最小安全间距，以避免碰撞；

3）仿射参数轨迹必须保证速度、加速度的连续性，避免产生不可执行的突变。

所考虑的避障环境需符合实际岛屿的复杂非结构化环境，因不同区域的可通行空间差异显著，因此，编队需根据当前位置环境特征动态调整编队形态，以达到与环境特征的全局最优匹配。

综上，多UUV编队规划任务由原本复杂的

${N_{\text{U}}}$

个UUV规划路径的多体问题转换为求解仿射状态的演化轨迹问题。

2　编队全局规划方法设计

收起

针对问题描述中的规划目标，本文提出一种包含前端路径搜索和后端轨迹优化的全局规划框架，如图2所示。其中，前端规划器采用一种改进的RRT*算法，该算法采用分阶段搜索方式，在五维仿射参数空间（平移、旋转、独立轴缩放）快速找到一条连接起始与目标编队状态、并初步规避障碍物的离散仿射状态序列。针对前端输出的离散状态点，后端采用基于B样条曲线的轨迹优化器，通过最小化一个综合考虑轨迹平滑性、运动学可行性、碰撞安全性以及编队形态自适应性的目标函数，生成一条连续、光滑且满足多重约束的最优仿射参数轨迹，最终指导多UUV编队完成复杂的避障任务。因本文侧重于解决静态场景下的全局编队轨迹规划问题，故采用占据栅格地图来表示无障碍区域。

2.1　AT−RRT*算法

本节将在RRT*算法的基础上进行改进，设计一种基于仿射变换的RRT*算法。该算法的核心思想是在仿射参数空间中进行搜索，直接生成一系列描述编队平移、旋转和缩放变化的仿射状态点

$ \{ {{\boldsymbol{q}}_1},{{\boldsymbol{q}}_2}, \cdots ,{{\boldsymbol{q}}_K}\} $

，从而构成初始的编队路径。

2.1.1　扩展采样空间

传统的RRT*在

$ {\mathbb{R}^2} $

或者

$ SE(2) $

空间中通常为单机器人搜索路径点，对于具有仿射编队结构的多UUV编队，本文将采样空间扩展至包含式（6）的二维仿射参数空间：

$ \mathcal{Q} = {\mathbb{R}^2} \times SO(2) \times \mathcal{S} \in {\mathbb{R}^5} $

(9)

式中：

$ {\mathbb{R}^2} $

部分对应仿射参数空间的平移参数

$ {\boldsymbol{t}} = ({t_x},{t_y}) $

，其范围受限于环境空间边界

$ {\boldsymbol{t}} \in [{t_{x,{\text{min}}}},{t_{x,{\text{max}}}}] \times [{t_{y,\min }},{t_{y,\max }}] $

；

$ SO(2) $

对应旋转参数

$ \theta $

，通常参数化为

$ \theta \in [ - {\text{π }},{\text{π }}] $

；

$ \mathcal{S} $

部分对应编队横向和纵向缩放因子

$ {\boldsymbol{s}} = ({s_x},{s_y}) $

，其范围由允许的最小和最大缩放值界定，

$ {\boldsymbol{s}} = [{s_{x,\min }},{s_{x,{\text{max}}}}] \times [{s_{y,{\text{min}}}},{s_{y,{\text{max}}}}] $

。

算法采用KD树数据结构在搜索过程（剪枝、重布线）中高效地查找邻近状态节点。然而，KD树是直接在原始的五维状态空间

$ \mathcal{Q} $

中使用标准欧氏距离进行邻近搜索，并未考虑各维度的尺度差异。在本文中，旋转和缩放量的搜索范围远小于位移量，导致KD树在选择邻近状态节点时主要按照位移维度进行划分，在旋转和缩放维度几乎考虑了搜索范围内的所有值，因此在这几个维度上退化成了普通的数据结构，导致搜索效率降低。针对这个问题，本算法对KD树操作进行了加权表示，即定义一组维度权重

$ w = [{w_x},{w_y}, {w_\theta }, {w_{{s_x}}},{w_{{s_y}}}] $

，对于原始状态

$ {\boldsymbol{q}} = {[{t_x},{t_y},\theta ,{s_x},{s_y}]^{\text{T}}} $

，加权后的搜索状态表示为

$ {{\boldsymbol{q}}_w} = [{w_x}{t_x},{w_y}{t_y},{w_\theta }\theta ,{w_{{s_x}}}{s_x}, {w_{{s_y}}}{s_y}]^{\text{T}} $

。通过这种加权表示，可以更加均衡地考虑不同维度在邻近搜索中的贡献，避免因尺度差异导致搜索偏向某些维度，从而提高KD树在多尺度状态空间中的划分效率和搜索精度。

2.1.2　两阶段搜索策略

为了在保证搜索效率的同时获得高质量的编队路径，AT−RRT*算法采用了分阶段搜索策略，并分别设计不同的多目标的代价函数来引导搜索过程。

第1阶段：初始路径搜索。此阶段的首要任务是尽快在复杂的五维状态空间

$ \mathcal{Q} $

中找到一条连接起始仿射状态

$ {{\boldsymbol{q}}_{{\text{start}}}} $

到目标仿射状态

$ {{\boldsymbol{q}}_{{\text{goal}}}} $

且全程无碰撞的路径。

为了加速这个过程，在该阶段，节点状态扩展类似于传统的RRT*算法，只在位置分量

$ {\boldsymbol{t}} = ({t_x},{t_y}) $

维度进行探索。对于其他维度，旋转分量

$ \theta $

设置为前进方向，

$ {\theta _{{\text{new}}}} = {\text{dir}}({{\boldsymbol{t}}_{{\text{cur}}}},{{\boldsymbol{t}}_{{\text{new}}}}) $

，缩放分量固定设置为最小安全缩放比例量进行碰撞检测以提高搜索效率，

$ {{\boldsymbol{s}}_{{\text{new}}}} = ({s_{x,{\text{min}}}},{s_{y,\min }}) $

。该阶段的KD树操作与传统算法相同，只在位置分量维度进行查询、插入和删除。一旦找到一条连接起点和终点状态的节点通路

$ \{ {{\boldsymbol{q}}_1},{{\boldsymbol{q}}_2}, \cdots ,{{\boldsymbol{q}}_M}\} $

，算法就会切换至第2阶段，并将这条通路上的所有节点进行加权操作插入到全状态的KD树中，以供第2阶段查询，具体的算法流程见算法1。

第2阶段：路径迭代优化。在该阶段，会在完整的五维状态空间

$ \mathcal{Q} $

进行节点搜索和KD树操作，其主要目的是在已找到的第1阶段路径的基础上进行迭代优化，得到高质量的初始全局路径。

RRT*算法通过比较不同连接路径的累积代价来选择父节点和执行重布线，从而驱动路径向低成本优化。在该算法中，2个阶段的代价函数相同，通过综合考虑多个评价因素，定义如下：

$ \mathcal{C} = {w_{\rm{p}}}{c_{\rm{p}}} + {w_{\Delta \theta }}{c_{\Delta \theta }} + {w_{{\theta _{\rm{f}}}}}{c_{{\theta _{\rm{f}}}}} + {w_{\Delta {\rm{s}}}}{c_{\Delta {\rm{s}}}} + {w_{{s_{{\text{std}}}}}}{c_{{s_{{\text{std}}}}}} $

(10)

式中：

$ {w_{\rm{p}}},{w_{\Delta \theta }},{w_{{\theta _{\rm{f}}}}},{w_{\Delta {\rm{s}}}},{w_{{s_{{\text{std}}}}}} $

为各项对应的权重；

$ {c_{\rm{p}}} $

为位置移动代价，该项在整条通路的累计代价近似为路径长度；

$ {c_{\Delta \theta }} $

为旋转分量变化成本，用于惩罚剧烈的旋转；

$ {c_{{\theta _{\rm{f}}}}} $

为前向偏差成本，以鼓励编队旋转角度与移动方向一致；

$ {c_{\Delta {\rm{s}}}} $

为缩放变化成本，用于惩罚剧烈的缩放变化；

$ {c_{{s_{{\text{std}}}}}} $

为与标准编队缩放偏差的成本，该项鼓励目标编队缩放量接近标称编队构型的缩放量，也即标准缩放量

$ {{\boldsymbol{s}}_{{\text{std}}}} = (1,1) $

。

在优化阶段，算法利用该代价函数，通过不断添加新节点并寻找更优的连接（重布线），迭代降低从

$ {{\boldsymbol{q}}_{{\text{start}}}} $

到

$ {{\boldsymbol{q}}_{{\text{goal}}}} $

的累积路径成本，直至满足最大迭代次数、时间和成本收敛等终止条件。

2.1.3　混合采样策略

为了加速算法收敛，并根据不同阶段的搜索目标，在有效引导搜索树向目标区域扩展的同时保持对整个状态空间的探索能力，AT−RRT*采用4种采样模式的混合采样策略，如图3所示。

由图3（a）可知，随机均匀采样允许在状态空间

$ \mathcal{Q} $

内均匀随机地生成采样点；目标偏置采样直接选择目标状态节点

$ {{\boldsymbol{q}}_{{\text{goal}}}} $

作为采样点；目标区域偏置采样在目标状态

$ {{\boldsymbol{q}}_{{\text{goal}}}} $

周围的一个邻域进行高斯分布采样；路径区域偏置采样只会在第2阶段中启用，是该阶段的主要采样方式，会在已有路径点中随机选择一个状态点

$ {{\boldsymbol{q}}_k} $

，并在周围的一个邻域使用高斯采样得到

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

，在通过碰撞检测后直接作为

$ {{\boldsymbol{q}}_{{\text{new}}}} $

。在这种情况下，如果

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

得到的成本更少，例如图3（d）中

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

和

$ {{\boldsymbol{q}}_k} $

，

$ {{\boldsymbol{q}}_{k + 2}} $

构成的路径长度比之前的路径更短，则后续重布线操作将使用

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

代替

$ {{\boldsymbol{q}}_{k + 1}} $

。

总的来说，采样方式（a）保证算法有对整个状态空间的探索能力，方式（b）和方式（c）引导搜索树向目标区域扩展，方式（d）则加速了第2阶段的路径成本收敛。在实际算法中，4种采样方式会按照预设的概率进行选择，其中第1阶段侧重于探索，方式（a）的概率更大，阶段2侧重于迭代已有路径成本，方式（d）的概率更大。综上所述，采用4种采样模式的混合采样策略能够有效平衡全局探索和局部优化，从而在不同搜索阶段实现高效且有针对性的路径搜索，最终提升算法的整体性能与收敛速度。

2.1.4　自适应采样和扩展参数

图3中，前3种采样方式在得到

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

后，

$ {{\boldsymbol{q}}_{{\text{near}}}} $

会朝着

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

的方向生成

$ {{\boldsymbol{q}}_{{\text{new}}}} $

，扩展的步长为

$ {{\boldsymbol{\gamma }}_{{\text{step}}}} $

。为了鼓励算法在空旷的区域进行激进的步进以加速扩展速度，需在障碍物复杂、狭窄的区域进行保守的步进以提高采样点碰撞检测的成功率，加快算法整体的运行时间。其中，步进长度会根据障碍物密度

$ {d_{\boldsymbol{q}}} $

进行调整，并且因为不同维度的尺度不同，步长也需各自定义，

$ {{\boldsymbol{\gamma }}_{{\text{step}}}}{\text{ = }}\left[ {{\gamma _{\text{t}}},{\gamma _\theta },{\gamma _{\text{s}}}} \right] $

和

$ {d_{\boldsymbol{q}}} $

的定义如下所示。

$ {d_{\boldsymbol{q}}} = d({\boldsymbol{q}},R) = \left\{ \begin{aligned} &0，&&{E({\boldsymbol{q}}) \geqslant R{\text{ }}} \\ & {1 - E({\boldsymbol{q}})/R}，&&{0 \leqslant E({\boldsymbol{q}}) \leqslant R} \end{aligned} \right. $

(11)

式中：

$ E({\boldsymbol{q}}) $

为采样点q位置分量

$ ({t_x},{t_y}) $

的欧几里得距离场（Euclidean signed distance field，ESDF）值，表示该点距离最近障碍物的距离；R为设定的查询范围。

$ {d_{\boldsymbol{q}}} \in [0,1] $

，其中0表示查询范围内无障碍，1表示

$ ({t_x},{t_y}) $

点有障碍物。

$ {\gamma _i}\left( {{d_{\boldsymbol{q}}}} \right) = \gamma _{i,\max }^{{\text{step}}} \times (\beta _i^{{\text{step}}} + (1 - \beta _i^{{\text{step}}}) \times (1 - {d_{\boldsymbol{q}}})) $

(12)

式中：

$ i \in \{ t,\theta ,s\} $

，分别为位移、旋转和缩放的3种分量；

$ \gamma _{i,\max }^{{\text{step}}} $

为分量i的最大步长距离；

$ \beta _i^{{\text{step}}} $

为分量的调制因子；

$ {d_{\boldsymbol{q}}} = d({{\boldsymbol{q}}_k},\gamma _{i,\max }^{{\text{step}}}) $

。

$ {\gamma _i}\left( {{d_{\boldsymbol{q}}}} \right) $

会随着障碍物密度的增大而减小。

同样，对于路径区域采样方式，其效果很大程度上依赖于如何在选定的路径点

$ {{\boldsymbol{q}}_k} $

周围生成新的采样点。因此，算法对采样半径

$ {{\boldsymbol{\gamma }}_{{\text{sample}}}} $

也进行了自适应调整，在环境拥挤的区域，采样范围较小，以进行更精细、更安全的局部探索；反之，采样范围会相对增大，以鼓励更大胆的探索，增加采样的成功率。在算法中，采样半径是通过调节高斯采样的标准差来实现的，即

$ {{\boldsymbol{\gamma }}_{{\text{sample}}}} = \left[ {{\sigma _t},{\sigma _\theta },{\sigma _s}} \right] $

，定义如下：

$ {\sigma _i}\left( {{d_{\boldsymbol{q}}}} \right) = {\sigma _{i,\max }} \times (\beta _i^{{\text{sample}}} + (1 - \beta _i^{{\text{sample}}}) \times (1 - {d_q})) $

(13)

式中：

$ {d_{\boldsymbol{q}}} = d({{\boldsymbol{q}}_k},{\sigma _{{\text{t}},\max }}) $

；

$ {\sigma _{i,\max }} $

为分量i的高斯采样最大标准差；参数

$ \beta _i^{{\text{sample}}} $

的定义与式（12）同，但具体的取值不同。

2.2　仿射参数优化器

本节将在AT−RRT*得到离散仿射状态序列的基础上进行优化，旨在寻找一个时序变化的仿射参数序列，以使整个编队在满足运动学约束、避免碰撞的同时，尽可能保持标称编队构型并使轨迹平滑。

2.2.1　基于B样条的轨迹表示

为了将离散的仿射状态序列转化为适合优化的初始连续轨迹

$ \mathcal{T}_{\boldsymbol{A}}^{{\text{init}}} $

，本节使用均匀B样条曲线

$ \varPhi $

进行轨迹参数化表示，该曲线是一个分段多项式，由次数

$ {{\boldsymbol{p}}_{\text{b}}} $

、结点间距

$ \Delta t $

以及

$ {N_{\text{c}}} $

个控制点

$ \{ {{\boldsymbol{c}}_k}\} _{k = 0}^{{N_{\text{c}}} - 1} $

唯一确定。本文选择

$ {{\boldsymbol{p}}_{\text{b}}} = 3 $

，即三次均匀B样条，且

$ {{\boldsymbol{c}}_k} \in {\mathbb{R}^5} $

也在仿射参数空间，因此整个轨迹的时间跨度为

$ ({N_{\text{c}}} - 3)\Delta t $

。

因由RRT类算法生成的路径点分布可能不均匀，故首先对离散的仿射状态序列进行重采样，得到

$ {N_{\text{c}}} - 1 $

个五维仿射状态点序列，并作为插值点结合边界条件（起始和终止时刻的速度为0）求解确定一组B 样条曲线的控制点序列

$ {\boldsymbol{C}} = \{ {{\boldsymbol{c}}_0},{{\boldsymbol{c}}_1}, \cdots ,{{\boldsymbol{c}}_{{N_{\text{c}}} - 1}}\} $

。对于均匀三次B样条，第

$ k $

个时间步长的轨迹约束点

$ {{\boldsymbol{q}}_k} $

可以由其相邻的3个控制点通过下式计算得到^[16]：

$ {{\boldsymbol{q}}_k} = {\boldsymbol{q}}({t_k}) = \frac{1}{6}({{\boldsymbol{c}}_k} + 4{{\boldsymbol{c}}_{k + 1}} + {{\boldsymbol{c}}_{k + 2}}) $

(14)

式中，

$ k = 1,2,...,{N_{\rm{c}}} - 2 $

。在轨迹约束点

$ {{\boldsymbol{q}}_k} $

处，第j个UUV的目标位置

$ {\boldsymbol{p}}_{k,j}^ * {\text{ = }}\left[ {x_{k,j}^ * ,y_{k,j}^ * } \right] $

由式（8）中的仿射映射关系

$ {f_{{\text{af}}}}{\text{(}} \cdot {\text{)}} $

计算得到：

$ {\boldsymbol{\tilde p}}_{k,j}^ * = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{p}}_{k,j}^ * } \\ 1 \end{array}} \right] = {f_{{\text{af}}}}{\text{(}}{{\boldsymbol{q}}_{k,j}},{\boldsymbol{\tilde p}}_j^0) $

(15)

2.2.2　优化问题和目标函数

优化变量为

$ {N_{\text{c}}} - 2{{\boldsymbol{p}}_{\text{b}}} + 1 $

个控制点的子集

$ \{ {{\boldsymbol{c}}_{{{\boldsymbol{p}}_{\text{b}}}}},{{\boldsymbol{c}}_{{{\boldsymbol{p}}_{\text{b}}} + 1}}, \cdots ,{{\boldsymbol{c}}_{{N_{\text{c}}} - {{\boldsymbol{p}}_{\text{b}}}}}\} $

，由于边界约束点

$ {{\boldsymbol{q}}_0} $

和

$ {{\boldsymbol{q}}_N} $

由前

$ {{\boldsymbol{p}}_{\text{b}}} $

个和最后

$ {{\boldsymbol{p}}_{\text{b}}} $

个控制点确定^[17]，因此无需修改。优化问题表述如下：

$ \min \mathcal{J} = {\lambda _{\text{s}}}{\mathcal{J}_{\text{s}}} + {\lambda _{\text{f}}}{\mathcal{J}_{\text{f}}} + {\lambda _{\text{c}}}{\mathcal{J}_{\text{c}}} + {\lambda _{{\text{sc}}}}{\mathcal{J}_{{\text{sc}}}} $

(16)

式中：

$ {\mathcal{J}_{\text{s}}} $

表示平滑性代价；

$ {\mathcal{J}_{\text{f}}} $

表示可行性代价；

$ {\mathcal{J}_{\text{c}}} $

表示碰撞代价；

$ {\mathcal{J}_{{\text{sc}}}} $

表示自适应编队缩放代价；

$ {\lambda _{\text{s}}},{\lambda _{\text{f}}},{\lambda _{\text{c}}},{\lambda _{{\text{sc}}}} $

分别为对应的权重。

为了简化导数的计算，UUV的速度

$ {{\boldsymbol{V}}_{{\boldsymbol{kj}}}} $

、加速度

$ {{\boldsymbol{A}}_{kj}} $

、加加速度

$ {{\boldsymbol{J}}_{kj}} $

分别使用第1，2，3阶有限差分进行近似：

$ {{{\boldsymbol{V}}_{kj}}} \approx \frac{{{\boldsymbol{p}}_{k + 1,j}^ * - {\boldsymbol{p}}_{k,j}^ * }}{{\Delta t}} $

(17)

$ {{{\boldsymbol{A}}_{kj}}} \approx \frac{{{{\boldsymbol{V}}_{k + 1,j}} - {{\boldsymbol{V}}_{kj}}}}{{\Delta t}} \approx \frac{{{\boldsymbol{p}}_{k + 2,j}^ * - 2{\boldsymbol{p}}_{k + 1,j}^ * + {\boldsymbol{p}}_{kj}^ * }}{{\Delta {t^2}}} $

(18)

$ {{{\boldsymbol{J}}_{kj}}} \approx \frac{{{{\boldsymbol{A}}_{k + 1,j}} - {{\boldsymbol{A}}_{k,j}}}}{{\Delta t}} \approx \frac{{{\boldsymbol{p}}_{k + 3,j}^ * - 3{\boldsymbol{p}}_{k + 2,j}^ * + 3{\boldsymbol{p}}_{k + 1,j}^ * - {\boldsymbol{p}}_{kj}^ * }}{{{{(\Delta t)}^3}}}$

(19)

平滑性代价

$ {\mathcal{J}_{\text{s}}} $

惩罚编队中每个UUV在其轨迹上的加速度和加加速度的大小，使得轨迹变化更加连续和平滑，定义如下：

$ {\mathcal{J}_{\text{s}}} = \mathop \sum \limits_{k = 1}^{{N_{\text{c}}} - 4} \mathop \sum \limits_{j = 1}^{{N_{\text{U}}}} \parallel {{\boldsymbol{A}}_{kj}}\parallel _2^2 + \mathop \sum \limits_{k = 1}^{{N_{\text{c}}} - 5} \mathop \sum \limits_{j = 1}^{{N_{\text{U}}}} \parallel {{\boldsymbol{J}}_{kj}}\parallel _2^2 $

(20)

可行性代价

$ {\mathcal{J}_{\text{f}}} $

确保生成的轨迹对于每个UUV都是可执行的，本文对每个约束点

$ {{\boldsymbol{q}}_k} $

上的每个UUV的速度

$ {{\boldsymbol{V}}_{kj}} $

和加速度

$ {{\boldsymbol{A}}_{kj}} $

进行约束，惩罚超过预设的

$ {V_{\max }} $

和

$ {A_{\max }} $

的部分：

$ \begin{split} &{\mathcal{J}_{\text{f}}} = \mathop \sum \limits_{k = 1}^{{N_{\text{c}}} - 3} \mathop \sum \limits_{j = 1}^{{N_{\text{U}}}} {F_{\text{f}}}({{\boldsymbol{V}}_{kj}}) + \mathop \sum \limits_{k = 1}^{{N_{\text{c}}} - 4} \mathop \sum \limits_{j = 1}^{{N_{\text{U}}}} {F_{\text{f}}}({{\boldsymbol{A}}_{kj}}) \\&\;\;\; {F_{\text{f}}}({{\boldsymbol{V}}_{kj}}) = {\text{max}}{(0,\parallel {{\boldsymbol{V}}_{kj}}\parallel _2^2 - V_{{\text{max}}}^{\text{2}})^2} \\&\;\;\; {F_{\text{f}}}({{\boldsymbol{A}}_{kj}}) = {\text{max}}{(0,\parallel {{\boldsymbol{A}}_{kj}}\parallel _2^2 - A_{{\text{max}}}^{\text{2}})^2} \end{split}$

(21)

碰撞代价

$ {\mathcal{J}_{\text{c}}} $

为了避免编队中的每个UUV与环境中的障碍物发生碰撞，会在每个约束点

$ {{\boldsymbol{q}}_k} $

上惩罚每个与障碍物的距离小于安全阈值

$ {d_{{\text{safe}}}} $

的UUV：

$ \begin{split} &\qquad {\mathcal{J}_{\text{c}}} = \mathop \sum \limits_{k = 1}^{{N_{\text{c}}} - 2} \mathop \sum \limits_{j = 1}^{{N_{\text{U}}}} {F_{\text{c}}}({\boldsymbol{p}}_{kj}^ * ) \\& {F_{\text{c}}}({\boldsymbol{p}}_{kj}^ * ) = \max {(0,{d_{{\text{safe}}}} - E({\boldsymbol{p}}_{kj}^ * ))^2} \end{split} $

(22)

式中，

$ E({\boldsymbol{p}}_{kj}^ * ) $

为

$ \left( {x_{kj}^ * ,y_{kj}^ * } \right) $

点处的ESDF值。

设计自适应编队缩放代价

$ {\mathcal{J}_{{\text{sc}}}} $

的目标是平衡编队构型保持与环境适应性需求，即在较为开阔、无障碍的空间，希望编队尽可能保持其预设的标称构型的缩放量（

$ {s_{x,{\text{std}}}} = {s_{y,{\text{std}}}} = 1.0 $

）；而在障碍物密集或需要穿越狭窄通道时，编队构型可以缩小以适应环境。此外，为了防止个体间的碰撞，

$ {\mathcal{J}_{{\text{sc}}}} $

会惩罚小于缩放量的最小安全阈值

$ ({s_{x,\min }},{s_{y,\min }}) $

，即对于约束点

$ {{\boldsymbol{q}}_k} $

，其x方向缩放量的分段惩罚项如式（23）所示，y方向的同理。

$ {\mathcal{C}_x}({s_{x,k}},{\alpha _k}) = \left\{ \begin{aligned} &{\alpha _{\min }}{({s_{x,\min}} - {s_{x,k}})^2},&& {s_{x,k}} \lt {s_{x,\min}} \\& {\alpha _k}{(1.0 - {s_{x,k}})^2} ,&& {s_{x,k}} \geqslant {s_{x,\min}} \end{aligned}\right. $

(23)

式中：

$ {\alpha _{\min }} $

为缩放量低于最小安全阈值的固定惩罚系数，数值较大；

$ {\alpha _k} = \alpha \left( {{d_{{{\boldsymbol{q}}_k}}}} \right) $

，为与当前轨迹约束点的障碍物环境有关的惩罚系数，定义如下：

$ \alpha \left( {{d_{{{\boldsymbol{q}}_k}}}} \right) = {\alpha _{\max }} \times ({\beta _{\rm{s}}} + (1 - {\beta _{\rm{s}}}) \times (1 - {d_{{{\boldsymbol{q}}_k}}})) $

(24)

式中：

$ {d_{{{\boldsymbol{q}}_k}}} = d\left( {{{\boldsymbol{q}}_k}{\text{,}}{{\boldsymbol{R}}_{s{\text{,max}}}}} \right) $

，为

$ {{\boldsymbol{q}}_k} $

约束点的位置点

$ ({t_{x,k}}, {t_{y,k}}) $

的障碍物密度，其中

$ {{\boldsymbol{R}}_{s{\text{,max}}}} $

为x和y轴缩放为最大缩放时

$ \left( {{s_{x,\max }},{s_{y,\max }}} \right) $

的外接圆半径，如图4所示；

$ {\beta _{\rm{s}}} $

为调制因子，

$ {\alpha _{\min }} = {\alpha _{\max }} \times {\beta _{\rm{s}}} $

。

自适应编队缩放代价

$ {\mathcal{J}_{\rm{s}}} $

由所有轨迹点在x，y方向惩罚的加权求和得到：

$ {\mathcal{J}_{{\text{sc}}}} = \mathop \sum \limits_{k = 1}^{{N_{\rm{c}}} - 2} \left( {{w_x}{\mathcal{C}_x}({s_{x,k}},{\alpha _k}) + {w_y}{\mathcal{C}_y}({s_{y,k}},{\alpha _k})} \right) $

(25)

式中，

$ {w_x} $

和

$ {w_y} $

为对应的权重系数。

2.2.3　梯度计算与优化求解

为了能够高效地求解上述无约束优化问题，本文使用L-BFGS算法，该算法是基于梯度的数值优化方法，通过存储有限的历史梯度信息来近似海森矩阵的逆，从而有效确定搜索方向。因此，要使用该算法就需计算目标函数

$ \mathcal{J} $

相对于优化变量

$ {{\boldsymbol{c}}_m} $

的梯度

$ {{\partial \mathcal{J}} \mathord{\left/ {\vphantom {{\partial \mathcal{J}} {\partial {{\boldsymbol{c}}_m}}}} \right. } {\partial {{\boldsymbol{c}}_m}}} $

。梯度可以通过轨迹约束点

$ {{\boldsymbol{q}}_k} $

反向传播至控制点，可见，总代价

$ \mathcal{J} $

对单个控制点

$ {{\boldsymbol{c}}_m} $

的梯度，为通过所有受

$ {{\boldsymbol{c}}_m} $

影响的轨迹点

$ {{\boldsymbol{q}}_k} $

对

$ {{\boldsymbol{c}}_m} $

求导的贡献之和：

$ \frac{{\partial \mathcal{J}}}{{\partial {{\boldsymbol{c}}_m}}} = \mathop \sum \limits_k {\left( {\frac{{\partial \mathcal{J}}}{{\partial {{\boldsymbol{q}}_k}}}} \right)^{\text{T}}}\frac{{\partial {{\boldsymbol{q}}_k}}}{{\partial {{\boldsymbol{c}}_m}}} $

(26)

由式（14）可知，上式只有当

$k = m, m - 1, m - 2$

时

$ {{\boldsymbol{q}}_k} $

才会对

$ {{\boldsymbol{c}}_m} $

的梯度有贡献，因此，只有当上述3项为非零项时，将

$k \in \{ m, m - 1, m - 2\} $

代入式（14）推导出的偏导数才为非零项。得到代价关于轨迹约束点的梯度关系如下：

$ \frac{{\partial \mathcal{J}}}{{\partial {{\boldsymbol{c}}_m}}} = {\left( {\frac{1}{6}\frac{{\partial \mathcal{J}}}{{\partial {{\boldsymbol{q}}_{m - 2}}}} + \frac{4}{6}\frac{{\partial \mathcal{J}}}{{\partial {{\boldsymbol{q}}_{m - 1}}}} + \frac{1}{6}\frac{{\partial \mathcal{J}}}{{\partial {{\boldsymbol{q}}_m}}}} \right)^{\rm{T}}} $

(27)

此外，由于平滑、可行和避碰代价中涉及第j个UUV在时间步k的期望位置点

$ {\boldsymbol{p}}_{k,j}^ * $

，因此，还需要计算

$ {\boldsymbol{p}}_{k,j}^ * $

关于

$ {{\boldsymbol{q}}_m} $

的梯度

$ {{\partial {\boldsymbol{p}}_{k,j}^ * } / {\partial {{\boldsymbol{q}}_m}}} $

，根据链式求导法则。当

$m = k$

时，该项不为0，结合式（3）、式（6）和式（7），偏导数的雅可比矩阵表示如下：

$ \dfrac{{\partial {\boldsymbol{p}}_{k,j}^ * }}{{\partial {{\boldsymbol{q}}_m}}} = \left[ {\begin{array}{*{20}{c}} {\dfrac{{\partial x_{_{k,j}}^ * }}{{\partial {t_m}}}}&{\dfrac{{\partial x_{_{k,j}}^ * }}{{\partial {t_m}}}}&{\dfrac{{\partial x_{_{k,j}}^ * }}{{\partial {\theta _m}}}}&{\dfrac{{\partial x_{_{k,j}}^ * }}{{\partial {s_{x,m}}}}}&{\dfrac{{\partial x_{_{k,j}}^ * }}{{\partial {s_{y,m}}}}} \\ {\dfrac{{\partial y_{_{k,j}}^ * }}{{\partial {t_m}}}}&{\dfrac{{\partial y_{_{k,j}}^ * }}{{\partial {t_m}}}}&{\dfrac{{\partial y_{_{k,j}}^ * }}{{\partial {\theta _m}}}}&{\dfrac{{\partial y_{_{k,j}}^ * }}{{\partial {s_{x,m}}}}}&{\dfrac{{\partial y_{_{k,j}}^ * }}{{\partial {s_{y,m}}}}} \end{array}} \right] $

(28)

下面，开始推导前3项代价关于

$ {\boldsymbol{p}}_{k,j}^ * $

的梯度计算方式。平滑性代价为加速度项

$ \mathcal{J}_{\boldsymbol{A}}^{\text{s}} $

与加加速度项

$ \mathcal{J}_{\boldsymbol{J}}^{\text{s}} $

的代价之和，

$ {\mathcal{J}_{\text{s}}} = \mathop \sum \limits_k \mathop \sum \limits_j \mathcal{J}_{{\boldsymbol{A}},kj}^{\text{s}} + \mathop \sum \limits_k \mathop \sum \limits_j \mathcal{J}_{J,kj}^{\text{s}} $

。以加速度项为例，其对轨迹约束点

$ {{\boldsymbol{q}}_m} $

的梯度根据加速度的定义式（18）和式（20），可以使用链式法则计算：

$ \begin{split} &\qquad {\frac{{\partial \mathcal{J}_{{\boldsymbol{A}},kj}^{\text{f}}}}{{\partial {{\boldsymbol{q}}_m}}}} = \frac{{\partial \mathcal{J}_{{\boldsymbol{A}},kj}^{\text{f}}}}{{\partial {{\boldsymbol{A}}_{kj}}}} \cdot \frac{{\partial {{\boldsymbol{A}}_{kj}}}}{{\partial {{\boldsymbol{q}}_m}}} =\\& \frac{{2{\boldsymbol{A}}_{kj}^{\rm{T}}}}{{\Delta {t^2}}}\left( {\frac{{\partial {\boldsymbol{p}}_{k + 2,j}^ * }}{{\partial {{\boldsymbol{q}}_m}}} - 2\frac{{\partial {\boldsymbol{p}}_{k{\text{ + 1}},j}^ * }}{{\partial {{\boldsymbol{q}}_m}}} + \frac{{\partial {\boldsymbol{p}}_{k,j}^ * }}{{\partial {{\boldsymbol{q}}_m}}}} \right)\end{split} $

(29)

式中，当且仅当

$m \in \{ k, k + 1, k + 2\} $

时梯度非零。加加速度项的梯度

$ {{\partial \mathcal{J}_{{\boldsymbol{J}},kj}^{\text{s}}} / {\partial {{\boldsymbol{q}}_m}}} $

与

$ {{\partial \mathcal{J}_{{\boldsymbol{A}},kj}^{\text{s}}} / {\partial {{\boldsymbol{q}}_m}}} $

的形式相同。

可行性代价的梯度与平滑性代价的形式相似，以加速度项的梯度

$ {{\partial \mathcal{J}_{{\boldsymbol{v}},kj}^{\text{f}}} / {\partial {{\boldsymbol{q}}_m}}} $

为例，

$ {\frac{{\partial \mathcal{J}_{{\boldsymbol{V}},kj}^{\text{f}}}}{{\partial {{\boldsymbol{q}}_m}}}} = \frac{{\partial \mathcal{J}_{{\boldsymbol{V}},kj}^{\text{f}}}}{{\partial {{\boldsymbol{V}}_{kj}}}} \cdot \frac{{\partial {{\boldsymbol{V}}_{kj}}}}{{\partial {{\boldsymbol{q}}_m}}} = \frac{{\partial {{F}_{\text{f}}}({{\boldsymbol{V}}_{kj}})}}{{\partial {{\boldsymbol{V}}_{kj}}}} \cdot \left( {\frac{{\partial {\boldsymbol{p}}_{k+1,j}^ * }}{{\partial {{\boldsymbol{q}}_m}}} - \frac{{\partial {\boldsymbol{p}}_{k,j}^ * }}{{\partial {{\boldsymbol{q}}_m}}}} \right) $

(30)

式中，当且仅当

$ m \in \{ k,k + 1\} $

时梯度非零。加速度项

$ {{\partial \mathcal{J}_{{\boldsymbol{V}},kj}^{\text{f}}} / {\partial {{\boldsymbol{q}}_m}}} $

与

$ {{\partial \mathcal{J}_{{\boldsymbol{A}},kj}^{\text{s}}} / {\partial {{\boldsymbol{q}}_m}}} $

的形式相同。

碰撞代价的梯度

$ {{\partial \mathcal{J}_{kj}^{\text{c}}} / {\partial {{\boldsymbol{q}}_m}}} $

与

$ {\boldsymbol{p}}_{kj}^ * $

处的ESDF距离值有关：

$ \frac{{\partial \mathcal{J}_{kj}^{\text{c}}}}{{\partial {{\boldsymbol{q}}_m}}} = \frac{{\partial {F_{\text{c}}}({\boldsymbol{p}}_{kj}^ * )}}{{\partial E}} \cdot \tilde N E{({\boldsymbol{p}}_{kj}^ * )^{\rm{T}}} \cdot \frac{{\partial {\boldsymbol{p}}_{kj}^ * }}{{\partial {{\boldsymbol{q}}_m}}} $

(31)

式中：当

$ m = k $

且

$ d_{\text {safe }}-E\left({\boldsymbol{p}}_{kj}^{*}\right)>0 $

时梯度非零；

$ \tilde N E({\boldsymbol{p}}_{kj}^ * ) $

为ESDF距离场在

$ {\boldsymbol{p}}_{kj}^ * $

点的梯度信息，通过双线性插值计算得到。

自适应编队缩放梯度只与状态约束点中对应方向的缩放量有关，等于x和y方向的加权梯度之和：

$ {{\partial \mathcal{J}_k^{{\text{sc}}}}/ {\partial {{\boldsymbol{q}}_m}}} = {w_x}{{\partial {\mathcal{C}_{x,k}}} / {\partial {{\boldsymbol{q}}_m}}} + {w_y}{{\partial {\mathcal{C}_{y,k}}} / {\partial {{\boldsymbol{q}}_m}}} $

。以

$ x $

方向为例，其对第m个状态约束点

$ {{\boldsymbol{q}}_m} $

的梯度如下：

$ \frac{{\partial {\mathcal{C}_x}({s_{x,k}},{\alpha _k})}}{{\partial {{\boldsymbol{q}}_m}}} = \frac{{\partial {\mathcal{C}_x}}}{{\partial {s_{x,k}}}}\frac{{\partial {s_{x,k}}}}{{\partial {{\boldsymbol{q}}_m}}} + \frac{{\partial {\mathcal{C}_x}}}{{\partial {\alpha _{k,j}}}}\frac{{\partial {\alpha _{k,j}}}}{{\partial {{\boldsymbol{q}}_m}}} $

(32)

式中，当且仅当

$ m = k $

时梯度不为0，各项的具体公式如下：

$ \frac{{\partial {\mathcal{C}_x}}}{{\partial {s_{x,k}}}} = \left\{ \begin{aligned} &- 2{\alpha _{\min }}({s_{x,\min}} - {s_{x,k}}),&& {s_{x,k}} \lt {s_{x,\min}} \\& - 2{\alpha _k}(1.0 - {s_{x,k}}) ,&& {s_{x,k}} \geqslant {s_{x,\min}} \end{aligned} \right. $

(33)

$ \frac{{\partial {s_{x,k}}}}{{\partial {{\boldsymbol{q}}_m}}} = \left( 0\;\;0\;\;0\;\;1\;\;0 \right) $

(34)

$ \frac{{\partial {\mathcal{C}_x}}}{{\partial {\alpha _k}}} = \left\{ \begin{aligned} &0 ,&& {s_{x,k}} \lt {s_{x,\min}} \\& {(1.0 - {s_{x,k}})^2},&& {s_{x,k}} \geqslant {s_{x,\min}} \end{aligned} \right. $

(35)

$ \frac{{\partial {\alpha _k}}}{{\partial {{\boldsymbol{q}}_m}}} = - {\alpha _{\max }}(1 - {\beta _{{\text{sc}}}})\nabla E(({t_{m,x}},{t_{m,y}})) $

(36)

式（32）中，障碍物密度相对于约束点的梯度与

$ E({{\boldsymbol{q}}_k}) = E(({t_x},{t_y})) $

有关，

$ {{\partial {\mathcal{C}_y}} / {\partial {{\boldsymbol{q}}_m}}} $

与式（32）～式（36）的推导过程相同，不同的是

$ {{\partial {s_{y,k}}} /{\partial {{\boldsymbol{q}}_m}}} = \left( 0\;\;0\;\;0\;\;0\;\;1 \right) $

。

3　湖上试验

收起

3.1　试验设置

为了验证所提方法的有效性，在上海交通大学致远湖进行了湖上试验，试验设备如图5所示。由于试验条件有限，采用3台BlueROV2作为编队中的部分UUV，剩余的UUV则作为虚拟UUV在地面站电脑上进行仿真实验。实际UUV的位姿由多普勒速度仪（DVL）、惯性测量单元（IMU）和全球导航卫星系统（GNSS）获得，其中GNSS用于获取地面站和3个真实UUV初始位置的经纬度，地面站为NED坐标系原点。任务开始后，通过DVL进行航位推演，虚拟UUV的位姿由仿真平台确定。标称编队如图6所示，其中绿色编号为实际UUV，黄色编号为虚拟UUV。实际和仿真UUV的轨迹跟踪控制器全部使用串级PID控制器。算法中，可行性约束中的最大速度和最大加速度项分别设置为

$ {V_{\max }} = 0.3{{\text{ m}} /{\text{s}}},\;{A_{\max }} = 1.0{{{\text{ m}}/ {\text{s}}}^2} $

。

图7所示为栅格化的试验地图和ESDF距离场的可视化。图8给出了地图坐标系和NED坐标系、障碍物区域、起始和目标状态的信息。其中，MAP坐标系下

$ {{\boldsymbol{q}}_{{\text{start}}}} $

和

$ {{\boldsymbol{q}}_{{\text{goal}}}} $

的具体值如下：

$ \begin{gathered} {{\boldsymbol{q}}_{{\text{start}}}} = {\left[ {0.5}\;\;\;{ - 11.23}\;\;\;{1.57}\;\;\;{1.0}\;\;\;{1.0} \right]^{\text{T}}} \\ {{\boldsymbol{q}}_{{\text{goal}}}} = {\left[ {3.5}\;\;\;{9.25}\;\;\;{1.57}\;\;\;{1.0}\;\;\;{1.0} \right]^{\text{T}}} \\ \end{gathered} $

3.2　试验结果

试验过程如图9所示，图中展示了湖面和软件平台在t = 0，33，72，115 s共4个时刻时UUV的运动位置。从图中可以看出，编队UUV通过缩放、旋转以及调整整体的位置，通过了狭窄的水道，并在较为宽阔的区域保持了标称编队的缩放尺度。

图10和图11给出了实际UUV的轨迹跟踪误差。由图可见，其整体跟随误差较小，位置误差小于0.25 m，艏向角误差小于10°，这说明算法规划的轨迹具有较高的可执行性、良好的平滑性和连续性。此外，图12和表1还给出了实际UUV在试验过程中的速度

${\left\| {\boldsymbol{V}} \right\|_2}$

和加速度

${\left\| {\boldsymbol{A}} \right\|_2}$

曲线与状态量（平均值、标准差、绝对值最大值），从中可以看出，速度和加速度均满足任务设置的可行性约束。湖上试验的视频见文献[18]。

4　结　语

收起

本文针对多UUV编队系统在复杂水下环境中执行协同任务时面临的编队保持与灵活避障难以同时实现的问题，提出了一种基于仿射变换的编队全局路径规划方法。该方法的核心思想是将编队整体的几何形态控制与路径规划问题映射到仿射参数空间中进行求解，从而实现从高维多体协同到低维参数优化的有效转化。算法的前端是两阶段的路径搜索，同时兼顾高效率的初始路径搜索和高质量的路径迭代优化；后端利用B样条优化平滑仿射参数轨迹，并进行约束惩罚，兼顾编队保持与避障需求。通过虚实结合的UUV混合编队湖上试验，对所提方法的有效性和实用性进行了验证。试验结果表明，该方法能够引导多UUV编队在模拟的复杂障碍环境中实现平滑、连续的队形变换从而安全通过狭窄通道，同时在开阔水域恢复并保持期望队形。

本研究可为提升UUV编队系统在复杂环境下的自主导航能力、环境适应性以及任务成功率等提供可行的解决方案，为水下搜救、资源勘探、环境监测等实际应用场景中多UUV系统的部署和高效作业提供可能。但是，目前本文方法还未完全考虑实际多UUV应用场景中面临的问题，未来可以进一步研究并提高算法在海流干扰和声学通信延迟方面的影响。

参考文献

收起

文献

收起

参考文献引证文献

排序方式：

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.

MILLÁN P, ORIHUELA L, JURADO I, et al. Formation control of autonomous underwater vehicles subject to communication delays[J]. IEEE Transactions on Control Systems Technology, 2014, 22(2): 770–777.

SAHU B K, SUBUDHI B. Flocking control of multiple AUVs based on fuzzy potential functions[J]. IEEE Transactions on Fuzzy Systems, 2018, 26(5): 2539–2551.

FANG Z, JIANG D, HUANG J, et al. Autonomous underwater vehicle formation control and obstacle avoidance using multi-agent generative adversarial imitation learning[J]. Ocean Engineering, 2022, 262: 112182.

YANG Y, XIAO Y, LI T S. A survey of autonomous underwater vehicle formation: performance, formation control, and communication capability[J]. IEEE Communications Surveys & Tutorials, 2021, 23(2): 815–841.

VAN PARYS R, PIPELEERS G. Distributed MPC for multi-vehicle systems moving in formation[J]. Robotics and Autonomous Systems, 2017, 97: 144–152.

QUAN L, YIN L J, ZHANG T R, et al. Robust and efficient trajectory planning for formation flight in dense environments[J]. IEEE Transactions on Robotics, 2023, 39(6): 4785–4804.

TANG Q R, ZHANG Y, YU F C, et al. An obstacle avoidance approach based on system outlined rectangle for cooperative transportation of multiple mobile manipulators[C]//2018 IEEE International Conference on Intelligence and Safety for Robotics (ISR). Shenyang: IEEE, 2018: 533−538.

ALONSO-MORA J, BAKER S, RUS D. Multi-robot formation control and object transport in dynamic environments via constrained optimization[J]. The International Journal of Robotics Research, 2017, 36(9): 1000–1021.

施宇. 复杂环境AUV集群自组织编队协同及避障控制[D]. 长春: 吉林大学, 2022.

SHI Y. Research on self-organized formation cooperation and obstacle avoidance control of multiple AUVs in complex environment[D]. Changchun: Jilin University, 2022 (in Chinese).

PANG W, ZHU D Q, SUN C Y. Multi-AUV formation reconfiguration obstacle avoidance algorithm based on affine transformation and improved artificial potential field under ocean currents disturbance[J]. IEEE Transactions on Automation Science and Engineering, 2024, 21(2): 1469–1487.

ZHAO S Y. Affine formation maneuver control of multiagent systems[J]. IEEE Transactions on Automatic Control, 2018, 63(12): 4140–4155.

YAO M Q, YU L, GUO K, et al. Distributed two-layered leader–follower affine formation control for multiple AUVs in 3-D space[J]. IEEE Transactions on Systems, Man, and Cybernetics: Systems, 2025, 55(1): 85–95.

姚明启, 于林, 郭凯, 等. 多自主水下航行器分布式三维仿射编队机动控制[J]. 中国舰船研究, 2025, 20(1): 360–368.

YAO M Q, YU L, GUO K, et al. Distributed three-dimensional affine formation maneuver control for multiple autonomous underwater vehicles[J]. Chinese Journal of Ship Research, 2025, 20(1): 360–368 (in Chinese).

陈盼召, 郭凯, 葛彤, 等. 外部扰动下水下无人航行器鲁棒快速轨迹跟踪控制[J]. 中国舰船研究, 2025, 20(6): 294–303.

CHEN P Z, GUO K, GE T, et al. Robust and fast trajectory tracking control of UUVs with external disturbances[J]. Chinese Journal of Ship Research, 2025, 20(6): 294–303 (in Chinese).

LIN Y, LIU Y, ZHANG P P, et al. Safety-first tracker: a trajectory planning framework for omnidirectional robot tracking[C]//2024 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Abu Dhabi: IEEE, 2024: 5416−5423.

ZHOU B Y, GAO F, WANG L Q, et al. Robust and efficient quadrotor trajectory generation for fast autonomous flight[J]. IEEE Robotics and Automation Letters, 2019, 4(4): 3529–3536.

SJTU-MIRUS. This video is supporting material for my recently submitted paper[EB/OL]. (2025-06-15)[2025-06-16]. https://sjtu-mirus.github.io/MIRUS.github.io/research/kai.

2026年第21卷第2期

PDF下载

引用本文

BibTeX

文章信息

doi: 10.19693/j.issn.1673-3185.04560

接收时间：2025-06-16
首发时间：2026-05-20
出版时间：2026-04-30

补充材料

相关文章

文章信息

作者

出版历史

收稿日期：2025-06-16
修回日期：2025-10-13

基金

作者信息

上海交通大学船舶海洋与建筑工程学院，上海 200240

通讯作者:

^* 乔磊

参考文献

分享链接

https://castjournals.cast.org.cn/joweb/zgjcyj/CN/10.19693/j.issn.1673-3185.04560

分享至

全文二维码

扫描看全文

引用本文

BibTeX

本文的引用情况

2种不同金属材料的力学参数

科 Family	属数 Number of genus	种数 Number of species	占总种数比例 Percentage of total species (%)	属 Genus	种数 Number of species	占总种数比例 Percentage of total species (%)
鹅膏菌科Amanitaceae	2	11	5.26	鹅膏菌属 Amanita	10	4.78
小菇科 Mycenaceae	2	12	5.74	丝盖伞属 Inocybe	5	2.39
多孔菌科 Polyporaceae	8	14	6.70	蜡蘑属 Laccaria	5	2.39
红菇科 Russulaceae	3	23	11.00	小皮伞属 Marasmius	6	2.87
				小菇属 Mycena	11	5.26
				光柄菇属 Pluteus	5	2.39
				红菇属 Russula	17	8.13
				栓菌属 Trametes	5	2.39

关闭全屏

BibTeX
EndNote
RefWorks
TxT

Input：

$ {{\boldsymbol{q}}_{{\text{start}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $

Result：

$ \{ {{\boldsymbol{q}}_1},{{\boldsymbol{q}}_2}, \cdots ,{{\boldsymbol{q}}_K}\} $

$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $

←

$ {\text{KDTre}}{{\text{e}}_{{\text{2D}}}} $

;

reach_goal←false

while (

$ i \lt {i_{\max }} $

and

$ t \lt {t_{\max }} $

) do

if CurPhase == Phase1 then

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

←Sample2DState();

else

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

←SampleFullState();

$ {{\boldsymbol{q}}_{{\text{near}}}} $

←FindNearest(KDTree,

$ {{\boldsymbol{q}}_{{\text{rand}}}} $

);

$ {{\boldsymbol{q}}_{{\text{new}}}} $

←Step(

$ {{\boldsymbol{q}}_{{\text{near}}}},{{\boldsymbol{q}}_{{\text{rand}}}} $

);

if IsStateValid(

$ {{\boldsymbol{q}}_{{\text{new}}}} $

) then

$ {{\boldsymbol{q}}_{{\text{cur}}}} $

←ChooseParentAndRewire(

$ {{\boldsymbol{q}}_{{\text{new}}}} $

);

$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $

.Insert(

$ {{\boldsymbol{q}}_{{\text{cur}}}} $

);

if IsNearGoal(

$ {{\boldsymbol{q}}_{{\text{cur}}}} $

)&&IsLineFree(

$ {{\boldsymbol{q}}_{{\text{cur}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $

) then

$ {\text{C}} $

←

$ {{\boldsymbol{q}}_{{\text{cur}}}}.{\text{Cost}} $

+Cost(

$ {{\boldsymbol{q}}_{{\text{cur}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $

);

if reach_goal == false then

reach_goal←true;

changePhaseTo2();

$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $

←

$ {\text{KDTre}}{{\text{e}}_{5{\text{D}}}} $

;

else

CheckandUpdateCost(

$ {\text{C}} $

);

retrun tracebackFormGoal(

$ {{\boldsymbol{q}}_{{\text{goal}}}} $

)

UUV	参数	属性	数值
UUV05	速度/(m∙s⁻¹)	平均值	0.188 9
标准差	0.030 3
最大值	0.267 8
加速度/(m∙s⁻²)	平均值	0.092 0
标准差	0.149 7
最大值	0.923 8
UUV06	速度/(m∙s⁻¹)	平均值	0.190 3
标准差	0.033 0
最大值	0.269 5
加速度/(m∙s⁻²)	平均值	0.092 0
标准差	0.145 9
最大值	0.895 5
UUV07	速度/(m∙s⁻¹)	平均值	0.189 9
标准差	0.026 2
最大值	0.279 7
加速度/(m∙s⁻¹)	平均值	0.089 0
标准差	0.139 2
最大值	0.073 9

UUV

参数

属性

数值

UUV05

速度/(m∙s⁻¹)

平均值

0.188 9

标准差

0.030 3

最大值

0.267 8

加速度/(m∙s⁻²)

平均值

0.092 0

标准差

0.149 7

最大值

0.923 8

UUV06

速度/(m∙s⁻¹)

平均值

0.190 3

标准差

0.033 0

最大值

0.269 5

加速度/(m∙s⁻²)

平均值

0.092 0

标准差

0.145 9

最大值

0.895 5

UUV07

速度/(m∙s⁻¹)

平均值

0.189 9

标准差

0.026 2

最大值

0.279 7

加速度/(m∙s⁻¹)

平均值

0.089 0

标准差

0.139 2

最大值

0.073 9

Input： $ {{\boldsymbol{q}}_{{\text{start}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $
Result： $ \{ {{\boldsymbol{q}}_1},{{\boldsymbol{q}}_2}, \cdots ,{{\boldsymbol{q}}_K}\} $
$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $←$ {\text{KDTre}}{{\text{e}}_{{\text{2D}}}} $;
reach_goal←false
while ($ i \lt {i_{\max }} $and$ t \lt {t_{\max }} $) do
	if CurPhase == Phase1 then
		$ {{\boldsymbol{q}}_{{\text{rand}}}} $←Sample2DState();
	else
		$ {{\boldsymbol{q}}_{{\text{rand}}}} $←SampleFullState();
	$ {{\boldsymbol{q}}_{{\text{near}}}} $←FindNearest(KDTree,$ {{\boldsymbol{q}}_{{\text{rand}}}} $);
	$ {{\boldsymbol{q}}_{{\text{new}}}} $←Step($ {{\boldsymbol{q}}_{{\text{near}}}},{{\boldsymbol{q}}_{{\text{rand}}}} $);
	if IsStateValid($ {{\boldsymbol{q}}_{{\text{new}}}} $) then
		$ {{\boldsymbol{q}}_{{\text{cur}}}} $←ChooseParentAndRewire($ {{\boldsymbol{q}}_{{\text{new}}}} $);
		$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $.Insert($ {{\boldsymbol{q}}_{{\text{cur}}}} $);
		if IsNearGoal($ {{\boldsymbol{q}}_{{\text{cur}}}} $)&&IsLineFree($ {{\boldsymbol{q}}_{{\text{cur}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $) then
				$ {\text{C}} $←$ {{\boldsymbol{q}}_{{\text{cur}}}}.{\text{Cost}} $+Cost($ {{\boldsymbol{q}}_{{\text{cur}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $);
			if reach_goal == false then
				reach_goal←true;
				changePhaseTo2();
				$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $←$ {\text{KDTre}}{{\text{e}}_{5{\text{D}}}} $;
			else
				CheckandUpdateCost($ {\text{C}} $);
retrun tracebackFormGoal($ {{\boldsymbol{q}}_{{\text{goal}}}} $)

Input： $ {{\boldsymbol{q}}_{{\text{start}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $
Result： $ \{ {{\boldsymbol{q}}_1},{{\boldsymbol{q}}_2}, \cdots ,{{\boldsymbol{q}}_K}\} $
$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $←$ {\text{KDTre}}{{\text{e}}_{{\text{2D}}}} $;
reach_goal←false
while ($ i \lt {i_{\max }} $and$ t \lt {t_{\max }} $) do
	if CurPhase == Phase1 then
		$ {{\boldsymbol{q}}_{{\text{rand}}}} $←Sample2DState();
	else
		$ {{\boldsymbol{q}}_{{\text{rand}}}} $←SampleFullState();
	$ {{\boldsymbol{q}}_{{\text{near}}}} $←FindNearest(KDTree,$ {{\boldsymbol{q}}_{{\text{rand}}}} $);
	$ {{\boldsymbol{q}}_{{\text{new}}}} $←Step($ {{\boldsymbol{q}}_{{\text{near}}}},{{\boldsymbol{q}}_{{\text{rand}}}} $);
	if IsStateValid($ {{\boldsymbol{q}}_{{\text{new}}}} $) then
		$ {{\boldsymbol{q}}_{{\text{cur}}}} $←ChooseParentAndRewire($ {{\boldsymbol{q}}_{{\text{new}}}} $);
		$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $.Insert($ {{\boldsymbol{q}}_{{\text{cur}}}} $);
		if IsNearGoal($ {{\boldsymbol{q}}_{{\text{cur}}}} $)&&IsLineFree($ {{\boldsymbol{q}}_{{\text{cur}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $) then
				$ {\text{C}} $←$ {{\boldsymbol{q}}_{{\text{cur}}}}.{\text{Cost}} $+Cost($ {{\boldsymbol{q}}_{{\text{cur}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $);
			if reach_goal == false then
				reach_goal←true;
				changePhaseTo2();
				$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $←$ {\text{KDTre}}{{\text{e}}_{5{\text{D}}}} $;
			else
				CheckandUpdateCost($ {\text{C}} $);
retrun tracebackFormGoal($ {{\boldsymbol{q}}_{{\text{goal}}}} $)

Input： $ {{\boldsymbol{q}}_{{\text{start}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $
Result： $ \{ {{\boldsymbol{q}}_1},{{\boldsymbol{q}}_2}, \cdots ,{{\boldsymbol{q}}_K}\} $
$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $←$ {\text{KDTre}}{{\text{e}}_{{\text{2D}}}} $;
reach_goal←false
while ($ i \lt {i_{\max }} $and$ t \lt {t_{\max }} $) do
	if CurPhase == Phase1 then
		$ {{\boldsymbol{q}}_{{\text{rand}}}} $←Sample2DState();
	else
		$ {{\boldsymbol{q}}_{{\text{rand}}}} $←SampleFullState();
	$ {{\boldsymbol{q}}_{{\text{near}}}} $←FindNearest(KDTree,$ {{\boldsymbol{q}}_{{\text{rand}}}} $);
	$ {{\boldsymbol{q}}_{{\text{new}}}} $←Step($ {{\boldsymbol{q}}_{{\text{near}}}},{{\boldsymbol{q}}_{{\text{rand}}}} $);
	if IsStateValid($ {{\boldsymbol{q}}_{{\text{new}}}} $) then
		$ {{\boldsymbol{q}}_{{\text{cur}}}} $←ChooseParentAndRewire($ {{\boldsymbol{q}}_{{\text{new}}}} $);
		$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $.Insert($ {{\boldsymbol{q}}_{{\text{cur}}}} $);
		if IsNearGoal($ {{\boldsymbol{q}}_{{\text{cur}}}} $)&&IsLineFree($ {{\boldsymbol{q}}_{{\text{cur}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $) then
				$ {\text{C}} $←$ {{\boldsymbol{q}}_{{\text{cur}}}}.{\text{Cost}} $+Cost($ {{\boldsymbol{q}}_{{\text{cur}}}},{{\boldsymbol{q}}_{{\text{goal}}}} $);
			if reach_goal == false then
				reach_goal←true;
				changePhaseTo2();
				$ {\text{KDTre}}{{\text{e}}_{{\text{cur}}}} $←$ {\text{KDTre}}{{\text{e}}_{5{\text{D}}}} $;
			else
				CheckandUpdateCost($ {\text{C}} $);
retrun tracebackFormGoal($ {{\boldsymbol{q}}_{{\text{goal}}}} $)