0.   引　言

自主水下航行器（AUV）在资源勘探和海洋学研究等诸多领域中发挥着至关重要的作用^[1]。中小型AUV通常在推进和负载能力方面存在物理限制，与之相比，大型AUV具有航程长、航速高、载荷大等明显优势，在执行复杂困难任务时表现更佳^[2]。但是AUV尺寸和速度的增加也将给复杂水下三维场景避障带来严峻挑战。

近年来学者们针对中小型AUV低速三维避障开展了一些研究。Yuan等^[3]提出一种基于制导律的方案，并利用声呐数据将障碍物建模为有向边界框。Yang等^[4]开发一种节省时间的路径规划方法，在避障的同时考虑海流对AUV运动的影响。区别于上述方法，Lin等^[5]消除障碍物显式建模的需求，并提出一种端到端架构用于提高AUV实时避障能力。此外，为将避障与实际应用需求结合，Huang等^[6]应用高斯过程运动规划实现以任务为导向的水下自主航行。虽然现有的部分方法能够不同程度地实现AUV三维避障，但普遍无法兼顾有限视域下感知动静态障碍物的准确性与实时性，且较少考虑系统内外复杂多元约束下AUV避障的敏捷性。

从公开发表文献来看，目前针对有限视域与复杂约束下的大型、高速、欠驱动AUV三维动静态障碍规避问题，还没有开展相关的研究工作。鉴于此，本文将提出一种大型、高速、欠驱动AUV的水下三维动静态障碍规避方法。首先，在静态感知层面，采用数据融合方法将原始声呐数据转化为2.5D双分辨率海底地形图，实现感知精度和计算效率的平衡。在动态感知层面，整合多变量卡尔曼滤波器、特征提取及匹配算法构建动态感知框架，实现对未知移动障碍物的最小包围椭球体建模与运动预测。其次，在全局规划方面，提出风险感知路径搜索方法，根据截断欧几里得有符号距离场和AUV运动动力学模型设计多元代价函数，实现低风险初始可行路径生成；在局部规划方面，采用时空联合轨迹优化方法，解析并推导AUV三维避障复杂多元约束及其梯度传播链，实现三维敏捷避障轨迹高效生成。以往研究仅根据航行器中心与障碍物之间的相对距离来判定AUV避障安全性，本文将通过标准化、细致化的动静态障碍物碰撞建模，实现考虑AUV位置和姿态角在内的全身避障，从而增强其高速自主航行的灵活性与安全性。其中包括由AUV凸包顶点集合与可通行区域半空间的有符号距离判断AUV与静态障碍物的碰撞程度；根据点集到空间运动椭球体的归一化距离判断AUV与移动障碍物的碰撞程度。然后，在跟踪控制层面，结合航向指数修正方法设计反馈控制器，实现考虑欠驱动以及非最小相位特性下AUV位置轨迹的精确跟踪。最后，将通过高保真仿真实验验证所提方法的有效性，并证明本文所提方法能兼具准确性、鲁棒性与实时性，可有效提升大型高速AUV自主作业能力。

1.   预备知识与问题描述

1.1   欠驱动AUV运动模型

为确定AUV的运动学和动力学方程，首先定义坐标系。如图1所示，惯性坐标系定义为${\mathcal{F}^{\text{E}}}$，原点${O_{\text{E}}}$固定在地球上；机体坐标系定义为${\mathcal{F}^{\text{B}}}$，原点${O_{\text{B}}}$固定在AUV上，并随AUV运动。欠驱动AUV完整的运动学与动力学模型如式（1）和式（2）所示。

图  1  AUV坐标系示意图

Figure  1.  AUV coordinate system diagram

下载: 全尺寸图片 幻灯片

以上式中：${\boldsymbol{\eta }} = {[ {{\boldsymbol{\varepsilon }},{\boldsymbol{\kappa }}} ]^{\text{T}}}$表示位姿向量；${\boldsymbol{\varepsilon }} = {[ {{\varepsilon _x},{\varepsilon _y},{\varepsilon _{\textit{z}}}} ]^{\text{T}}}$和${\boldsymbol{\kappa }} = {[ {\phi ,\theta ,\psi } ]^{\text{T}}}$分别代表${\mathcal{F}^{\text{E}}}$中的空间位置坐标和姿态角；${\boldsymbol{v}} = {[ {u,v,w,p,q,r} ]^{\text{T}}}$是${\mathcal{F}^{\text{B}}}$中AUV线速度和角速度组成的向量；${\boldsymbol{J}}({\boldsymbol{\eta }}) \in {\mathbb{R}^{6 \times 6}}$表示从${\mathcal{F}^{\text{B}}}$到${\mathcal{F}^{\text{E}}}$的坐标变换矩阵；M，${\boldsymbol{C}}({\boldsymbol{v}})$和${\boldsymbol{D}}({\boldsymbol{v}})$是${\mathbb{R}^{6 \times 6}}$的矩阵，分别对应惯性、科氏与向心效应以及流体阻尼；${\boldsymbol{g}}({\boldsymbol{\eta }}) \in {\mathbb{R}^6}$表示由重力和浮力叠加而产生的恢复力向量；${\boldsymbol{\tau }} \in {\mathbb{R}^6}$代表系统的控制输入向量。

1.2   轨迹规划的平坦模型

对于高速航行状态下大型AUV的实时三维避障，计算效率不可忽视。注意到AUV完整的运动与动力学特性包含强耦合性和高非线性等特点，显式地考虑这些因素来进行避障规划对于确保计算实时性是一项巨大的挑战。为此，引入微分平坦性，根据非线性系统的平坦输出变量及其导数来解析表示系统状态和控制输入变量。基于该特性，通过精心设计系统的平坦输出轨迹，其动力学中的微分约束能够得到隐式消除。根据文献[7]，AUV可以被证明具有微分平坦性，因为其系统中所有的状态和控制输入变量，即$\left\{ {{\boldsymbol{\eta }},{\boldsymbol{v}},{\boldsymbol{\tau }}} \right\}$，均可以通过位姿向量${\boldsymbol{\eta }}$及其导数组成的表达式来解析表示：

式中：${\boldsymbol{G}}({\boldsymbol{\eta }}) = {{\boldsymbol{J}}^{ - {\text{T}}}}{\boldsymbol{g}}({\boldsymbol{\eta }})$；${\boldsymbol{D}}({\boldsymbol{v}},{\boldsymbol{\eta }}) = {{\boldsymbol{J}}^{ - {\text{T}}}}{\boldsymbol{D}}({\boldsymbol{v}}){{\boldsymbol{J}}^{ - 1}}$；${\boldsymbol{C}}({\boldsymbol{v}}, {\boldsymbol{\eta }}) = {{\boldsymbol{J}}^{ - {\text{T}}}}({\boldsymbol{C}}({\boldsymbol{v}}) - {\boldsymbol{M}}{{\boldsymbol{J}}^{ - 1}}\dot {\boldsymbol{J}}){{\boldsymbol{J}}^{ - 1}}$；${\boldsymbol{M}}({\boldsymbol{\eta }}) = {{\boldsymbol{J}}^{ - {\text{T}}}}{\boldsymbol{M}}{{\boldsymbol{J}}^{ - 1}}$；以及${\boldsymbol{\tau }}({\boldsymbol{\eta }}) = {{\boldsymbol{J}}^{ - {\text{T}}}}{\boldsymbol{\tau }}$。为简化起见，上述${\boldsymbol{J}} = {\boldsymbol{J}}({\boldsymbol{\kappa }})$。

更进一步地，如果直接规划包含全部位置和姿态角的平坦输出轨迹，即${\boldsymbol{z}}(t) = {\boldsymbol{\eta }}(t)$，计算成本依然高昂。因此，本文为高速欠驱动AUV建立了基于平坦性的简化模型，从而降低问题复杂度。根据文献[8]，AUV横摇运动通常被认为是微小且自趋稳定的，于是在后续轨迹规划过程中暂不考虑横摇角$\phi$的影响。根据式（3），v可以被展开为如下形式：

考虑本文研究的欠驱动AUV在横荡和垂荡方向上缺少直接的控制输入，于是假设其具备严格的非完整性约束，这也意味着AUV航向与其移动方向一致。由该特性可以方便地从空间位置轨迹${\boldsymbol{\varepsilon }}(t)$中推导出俯仰角$\theta$和偏航角$\psi$：

随后，根据式（1）～式（7），控制输入向量${\boldsymbol{\tau }}$也可以被重写为仅由位置向量${\boldsymbol{\varepsilon }}$及其导数组成的表达式。综上所述，选择${\boldsymbol{\varepsilon }}$作为平坦输出项z，将原系统转换为三阶积分链形式。进而，根据三阶积分链性质，只需使得位置轨迹的四阶导数项${{\boldsymbol{\varepsilon }}^{(4)}}(t)$存在且连续，就能够确保系统状态和控制输入变量具备平滑性。

1.3   有限视域的声呐模型

在障碍感知部分，使用一种空间视域有限的多波束声呐来获取水下未知环境中的障碍物信息。如图2所示，声呐的最大探测距离为${D_{\text{r}}}$。其开角在水平方向跨度为${\gamma _{\text{h}}}$，垂直方向跨度为${\gamma _{\text{v}}}$，每个声呐波束在水平和垂直方向上的角度间隔分别为$\Delta {\gamma _{\text{v}}}$和$\Delta {\gamma _{\text{h}}}$。三维多波束声呐提供的障碍物初始点云位于坐标系${\mathcal{F}^{\text{S}}}$（原点$O\mathrm{_S}$固定在声呐的测量中心，且${X_{\text{S}}}{Y_{\text{S}}}{Z_{\text{S}}}$轴与${\mathcal{F}^{\text{B}}}$的轴线对齐）。为便于后续地形构建，给出点云坐标转换公式如下：

图  2  三维多波束声呐示意图

Figure  2.  The illustration of the 3D multibeam sonar

下载: 全尺寸图片 幻灯片

式中：${{\boldsymbol{p}}^{\text{S}}}$，${{\boldsymbol{p}}^{\text{B}}}$和${{\boldsymbol{p}}^{\text{E}}}$是${\mathbb{R}^3}$中的坐标向量，分别代表声呐探测到的障碍物点在${\mathcal{F}^{\text{S}}}$，${\mathcal{F}^{\text{B}}}$和${\mathcal{F}^{\text{E}}}$坐标系中的位置；${{\boldsymbol{J}}_{\text{u}}}({\boldsymbol{\kappa }}) \in {\mathbb{R}^{3 \times 3}}$是雅可比矩阵${\boldsymbol{J}}({\boldsymbol{\kappa }})$的左上角子矩阵，而${\boldsymbol{p}}_{{O_{\text{S}}}}^{\text{B}}$和${\boldsymbol{p}}_{{O_{\text{B}}}}^{\text{E}}$表示原点偏移量，这些偏移量由声呐的安装位置和AUV当前位置确定。

2.   高速避障方法

2.1   动静态障碍物感知

首先，需要对三维多波束声呐提供的原始点云数据进行预处理，以提高精度和后续感知规划的实时性。应用直通滤波器去除超过可信阈值的点。接着，对点云实施体素化，以平均点云密度和减少需要处理的数据量。异常值滤波器被用于移除离群的噪声点。其次，如式（8）所描述，经滤波处理后的点云由${\mathcal{F}^{\text{S}}}$转换到${\mathcal{F}^{\text{E}}}$坐标系。最后，引入基于密度的聚类算法（DBSACN），将过滤后的点云划分若干个不等规模的集合，再根据点云集合的特征进行筛选和匹配，从而区分出不同的障碍物。

鉴于大型AUV通常在半开放空间中航行，而非像洞穴或隧道那样的封闭区域，可以参考文献[9]将海底地形视为主要的静态障碍物。为实时高效地从过滤后的点云中提取障碍物的几何特征，本文采用2.5D栅格地图实现灵活的障碍物表示和高效的数据处理过程。如图3所示，该地图以AUV为中心，以滑动窗口的形式更新。与该地图固联的坐标系定义为${\mathcal{F}^{\text{M}}}\left( {{O_{\text{M}}} - {X_{\text{M}}}{Y_{\text{M}}}{Z_{\text{M}}}} \right)$，其中${O_{\text{M}}}$是AUV位置坐标在海平面上的投影，${X_{\text{M}}}{Y_{\text{M}}}{Z_{\text{M}}}$轴分别与${\mathcal{F}^{\text{E}}}$的轴线对齐。考虑到${Z_{\text{M}}}$轴垂直指向海底，因此地图的每个单元格记录在该位置检测到的障碍物的最小水深，以确保AUV水下自主航行安全。

图  3  海底地形图坐标系

Figure  3.  The coordinate system of the seafloor terrain map

下载: 全尺寸图片 幻灯片

由于从单一测量结果中获取准确水深信息具有挑战性，为减少噪声影响，在每个单元格的深度确定过程中应用数据融合，即单变量卡尔曼滤波器，这类似于文献[10]在构建地面高程图时所做的高度修正过程。此外，引入马氏距离用于处理同一单元格内出现多个测量结果的情况。数据融合的更新规则与马氏距离的计算公式如下：

以上式中：${{\textit{z}}_k}$为某单元格的第k次深度测量结果；而${\hat d_{k - 1}}$为该单元上一次的深度估计结果；对应的方差分别是$\sigma _{{{\textit{z}}_k}}^2$和$\sigma _{{{\hat d}_{k - 1}}}^2$，前者可以从声呐的测量误差模型中获取，而后者则根据式（10）进行更新。阈值m用于评估新的深度测量结果，如果该结果对应的马氏距离低于m，则深度更新遵循数据融合的标准规则式（9）。如果该距离超过m，新的深度测量结果${{\textit{z}}_k}$将被标记为异常值。该异常深度小于${\hat d_{k - 1}}$时，将被用于重置深度估计结果，以确保AUV尽可能安全航行。否则，不对异常测量结果进行处理并保留之前的深度估计结果。深度估计的方差也采用与上述过程类似的保留和重置策略完成在线更新。

双分辨率海底地形图构建步骤如图4所示。首先，结合三维声呐的当前测量结果与历史估计信息，由数据融合得到最新的深度估计结果。然后，经由异常值与安全性评估，可以持续更新深度信息。上述深度估计过程可扩展至整个地图上的所有单元格。最后，根据AUV的几何参数可将单元格设置为两种不同的尺寸，进而并行构建出双分辨率2.5D海底地形图。该地图包括低分辨率和高分辨率地图，并通过滤波链进行优化，以填补信息缺失单元并减少噪声。

图  4  双分辨率海底地形构建过程

Figure  4.  The process of dual-resolution seafloor mapping

下载: 全尺寸图片 幻灯片

高分辨率海底地形图以及由低分辨率海底地形图生成的截断欧几里得有符号距离场（TESDF）地图如图5所示。其中，高分辨率地图与海底轮廓匹配度较高，可用于准确描述水下静态障碍物；而TESDF地图则用于快速量化AUV航行风险。本文中，AUV规划过程分为全局和局部两个层级，上述不同分辨率的地图服务于规划过程中的不同层级。在全局规划中，该地图低分辨率的存储结构和TESDF预设的有限截断边界可以提高感知和规划过程的计算效率。在局部规划中，高分辨率地图被用来精确构建围绕AUV的可通行无障碍区域，以确保AUV全身避障的可行性与安全性。

图  5  高分辨率海底地形图与TESDF地图

Figure  5.  The high-resolution seafloor map and the TESDF map

下载: 全尺寸图片 幻灯片

此外，如图6所示，考虑到AUV航行过程中可能遭遇位于海底地形上方的移动障碍物，本文设计了包含滤波器、特征提取与匹配算法在内的动态感知框架，实现针对未知移动障碍物的几何建模与运动预测。先构建最小包围椭球体来显式描述障碍物外形，继而引入多变量卡尔曼滤波器对障碍物状态进行实时估计，最后基于历史数据的一致性来实现障碍物分配、跟踪以及运动轨迹预测。

图  6  移动障碍物感知过程示意图

Figure  6.  The illustration of perception process for dynamic obstacles

下载: 全尺寸图片 幻灯片

由参考文献[11]的最小包围椭球体算法，可以将信息冗杂的点云集合$\mathcal{C} = \left\{ {{{\textit{C}}^1}, \cdot \cdot \cdot ,{{\textit{C}}^i}, \cdot \cdot \cdot ,{{\textit{C}}^n}} \right\}$，简化为一系列具备明确几何意义的三维空间椭球体集合$\mathcal{E}=\left\{{\textit{E}}^{1},\cdot \cdot \cdot ,{\textit{E}}^{i},\cdot \cdot \cdot ,{\textit{E}}^{n}\right\}$。每个椭球体都由其中心位置${{\boldsymbol{e}}_{\text{p}}} = \left[ {{e_x},{e_y},{e_{\textit{z}}}} \right]$，三轴半径${{\boldsymbol{e}}_{\text{r}}} = \left[ {{e_a},{e_b},{e_c}} \right]$以及姿态四元数${\boldsymbol{q}} = \left[ {{q_w},{q_x},{q_y},{q_{\textit{z}}}} \right]$来进行唯一参数化。为减少点云噪声的影响，本文使用多变量卡尔曼滤波器来估计障碍物状态，而最小包围椭球体则用来代表滤波器中的测量过程。以${E^i}$为例，将需要估计的状态划分为几何特征与运动特征，其中几何特征主要考虑椭球体的三轴半径${\boldsymbol{e}}_{\text{r}}^i$以及姿态四元数${{\boldsymbol{q}}^i}$，运动特征则包括椭球体的中心位置坐标${\boldsymbol{e}}_{\text{p}}^i$，惯性坐标上的线速度${{\boldsymbol{v}}^i} = \left[ {v_x^i,v_y^i,v_{\textit{z}}^i} \right]$以及线加速度${{\boldsymbol{a}}^i} = \left[ {a_x^i,a_y^i,a_{\textit{z}}^i} \right]$。为提升状态估计的计算效率，本文将滤波变量的状态空间根据其特征拆分为两个独立且互不干扰的子空间，即${\boldsymbol{D}}_{\text{G}}^i = {\left[ {{\boldsymbol{e}}_{\text{r}}^i,{{\boldsymbol{q}}^i}} \right]^{\text T} }$和${\boldsymbol{D}}_{\text{M}}^i = {\left[ {{\boldsymbol{e}}_{\text{p}}^i,{{\boldsymbol{v}}^i},{{\boldsymbol{a}}^i}} \right]^{\text T}}$，对应的状态转移过程矩阵和测量矩阵分别表示如下：

上述式中：${F_{\text{G}}} \in {\mathbb{R}^{9 \times 9}}$；$\Delta t$代表滤波器的时间步长。之后，结合状态转移过程方差和测量方差，可以设计两个独立的多变量卡尔曼滤波器。最后，经由卡尔曼滤波器预测和更新的标准流程^[12]，可以得到移动障碍物未来时域上的状态变量。

此外，实现障碍物轨迹预测还需要当前状态与历史数据的准确匹配。根据当前椭球体集合${\mathcal{E}_{{\text{cur}}}}$与历史椭球体集合${\mathcal{E}_{{\text{last}}}}$，计算不同时刻不同椭球体中心位置间距。之后，采用匈牙利算法实现不同时刻障碍物之间的数据关联与匹配。最终，整合上述建模、滤波、跟踪流程，动态感知模块可以实时输出障碍物当前和未来的形状、位姿以及运动状态。

2.2   多元约束轨迹规划

针对全局规划，本文改进混合A星算法，设计满足AUV运动动力学可行性并考虑碰撞风险的全局路径搜索方法。受文献[13]启发，算法中的状态向量、控制输入向量以及状态转移方程根据二阶线性时不变系统定义。混合A星的运动原语由一组有序有界输入向量${\boldsymbol{U}} = \left[ {{{\boldsymbol{u}}_1}, \cdots ,{{\boldsymbol{u}}_m}} \right]$和有序有界的时间步长向量$\Delta {\boldsymbol{T}} = \left[ {\Delta {t_1}, \cdots ,\Delta {t_n}} \right]$生成。不同运动原语的比较筛选则由对应代价函数的值来决定。

为确保全局路径的平滑性和敏捷性，控制输入及其持续时间被加入到成本函数中。此外，由于欠驱动AUV执行器存在物理限制，角速度和角加速度也被纳入考虑。根据式（5），可求得运动原语的平均角速度${q_{{\text{avg}}}}$，${r_{{\text{avg}}}}$及角加速度${\dot q_{{\text{avg}}}}$，${\dot r_{{\text{avg}}}}$如下：

以上式中：${\theta _{\text{s}}}$和${\theta _{\text{e}}}$分别为运动原语起始和结束状态的俯仰角；权重${\bar \omega _i}$由梯形求和法则给出；${\dot \psi _i}$和${\theta _i}$分别为在持续时间$\Delta t$第${i \mathord{\left/ {\vphantom {i n}} \right. } n}$时刻对应的偏航角变化率和俯仰角。同理，起始和结束状态的角加速度也可以根据给定运动原语的状态和控制输入获得。综上，等式右侧的变量均可通过式（5）、式（6）和式（7）唯一确定。

为提高安全性，本文根据TESDF在给定空间位置的函数值来量化AUV与静态障碍物的碰撞风险，具体计算公式为

式中：$\mathcal{R}$表示风险函数；$D(\varepsilon )$为空间位置$\varepsilon$处对应的TESDF函数值；${D_{\text{b}}}$为风险评估距离，同时也是TESDF的截断边界；${D_{\text{c}}}$为临界距离，低于该阈值，可以认为发生碰撞；$\alpha$和k用于调整函数形式。

在运动原语的可行性检查中，将不等式约束定义为：不违背AUV俯仰角、速度和角速度限制，以及和障碍物之间的距离不低于${D_{\text{c}}}$。经由上述步骤，可以得到由AUV当前位置到终点位置的安全可行全局路径。之后，根据该全局路径，可以开展AUV高速避障的局部轨迹规划。

在局部规划过程中，首先根据全局规划获得的离散有序航路点序列q以及航路点之间的时间间隔序列T，采用多项式轨迹参数化方法，通过线性时间复杂度映射得到对应分段多项式轨迹的系数矩阵${\boldsymbol{c}} = \mathcal{M}({\boldsymbol{q}},{\boldsymbol{T}})$，具体过程可以参考文献[14]给出的解释和最优性证明公式。然后，由系数矩阵c和自然基${\boldsymbol{\beta }}$构建全局轨迹，再基于预设的局部规划边界确定局部目标的位置，进而以该位置为界可以获得用于快速热启动局部轨迹优化问题的初始可行轨迹。最后，AUV三维高速避障的多约束局部轨迹优化问题被定义为如下形式：

式中：目标函数中的$\rho$用于时间正则化以确保轨迹的快速性；$\varepsilon _{}^{[k]}(t)$表示从$\varepsilon (t)$依次求导至第k阶导数项所组成的集合。该优化问题包含的约束从上至下依次是：初始/终末状态约束、中间航路点约束、状态连续性约束、时间非负约束以及一般不等式约束。上述问题中的所有等式约束在多项式轨迹参数映射方法$\mathcal{M}$中得到自然满足，时间非负约束可以使用微分同胚映射消除。剩下一般形式的不等式约束，将其设计为惩罚函数，并随后纳入优化问题的目标函数中，进而可以将多约束优化问题转化为无约束的多目标优化问题

式中：${\mathcal{J}_{\text{o}}}$代表式（19）中的原始目标函数；${\mathcal{J}_ \star }$为基于多元约束惩罚项构建的目标函数，下角标$\star$代表多元约束；${\lambda _ \star }$为各约束项的权重。

基于目标函数梯度的方法可以被用来高效求解无约束优化问题，但前提是该问题的梯度存在且能够被解析表示。由于完整多项式轨迹所对应的目标函数值是m段轨迹所对应的函数值总和。因此，为简化公式，后续推导以其中第i段轨迹为例。下面将构造优化问题式（20）中各目标函数解析式，并推导其各自对应的梯度传播链。

目标函数${\mathcal{J}_{\text{o}}}$在优化问题中决定了轨迹的敏捷性与快速性，其对应的梯度表达式如下：

不等式约束构成的目标函数${\mathcal{J}_ \star }$可以使用分段轨迹上一系列离散约束点对应的惩罚函数值加权和来量化。分段轨迹上约束点的采样策略被设计为固定时间间隔采样，最终给出${\mathcal{J}_ \star }$的一般形式及梯度求解过程如下：

式中：${\bar \omega _j}$是基于梯形法则求和权重；${{\boldsymbol{\beta }}^{(k)}}(j\Delta T)$和${\boldsymbol{\varepsilon }}_{i,j}^{(k)}$分别是$t = j\Delta T$时刻，${\boldsymbol{\beta }}(t)$和${{\boldsymbol{\varepsilon }}_i}(t)$的k阶导数。

基于上述推导，可以对AUV三维高速动静态避障涉及到的各项不等式约束及其对应梯度进行逐一分析。由于各离散约束点所对应的函数值之间相互独立，而目标函数值是各约束点函数值的加权和。为进一步简化公式，后续推导以分段轨迹上第j个约束采样点为例。

考虑AUV在平移方向上的运动与动力学约束。相应的不等式和梯度表达式如下：

式中，${V_{{\text{max}}}}$和${A_{{\text{max}}}}$分别表示根据AUV实际性能和任务需求预设的最大速度和最大加速度。

对于旋转方向，将最大俯仰角设为${\theta _{{\text{max}}}}$，并分别限制俯仰和偏航的最大角速度为${q_{{\text{max}}}}$和${r_{{\text{max}}}}$。根据AUV动力学方程，控制力矩${\tau _M}$和${\tau _N}$与前进速度的平方项成正比，因此角加速度限制被定义为与$\left\| {\dot \varepsilon } \right\|_2^2$相关的动态边界${\dot q_b}$和${\dot r_b}$。由此可以给出AUV旋转方向上的不等式约束及梯度如下：

式中：${\mu _{\dot q}} \in (0,1)$为权重系数；${\dot r_{\text{b}}}$以式（30）类似形式构建，为简化公式故省略。同理，式（29）中其余约束项的梯度也按照式（31）和式（32）来推导。之后，根据式（5）～式（7）可以计算${\theta _{i,j}}$，${q_{i,j}}$和${r_{i,j}}$，将其关于时间求导可得${\dot q_{i,j}}$和${\dot r_{i,j}}$。最终，上述式（30）～式（32）的右边各项均可以由约束采样点及其导数所确定，故式（29）中各项不等式约束及其梯度可被解析表示。

考虑避障安全性约束，对于大型AUV轨迹优化问题，过于保守地描述航行器外形将严重限制可行解的数量和质量。因此，本文选择将其建模为由一组空间顶点${\boldsymbol{P}} = \left\{ {{{\boldsymbol{p}}_1}, \cdot \cdot \cdot ,{{\boldsymbol{p}}_k}, \cdot \cdot \cdot ,{{\boldsymbol{p}}_e}} \right\}$所确定的三维凸包，如图7所示。其中，以顶点${{\boldsymbol{p}}_k}$为例，可以给出其随AUV运动而空间位置变化的表达式

图  7  AUV凸包示意图

Figure  7.  The convex hull of the AUV

下载: 全尺寸图片 幻灯片

式中：${{\boldsymbol{q}}_k}$为顶点${{\boldsymbol{p}}_k}$在${\mathcal{F}^{\text{B}}}$上的位置坐标；${{\boldsymbol{J}}_{\text{u}}}(\dot {\boldsymbol{\varepsilon }}(t))$与式（8）中的坐标转换矩阵类似，表达式为

根据用于热启动的初始可行轨迹和高分辨率海底地形图，可以构建用于AUV局部轨迹规划的安全可航行区域$\mathcal{S}$，构建的具体过程可以参考文献[15]。该区域由一系列连续的凸多面体所组成：

上述式中：m是凸多面体的数量；$\mathcal{P}_i^\mathcal{H}$表示第i段多项式轨迹对应的凸多面体，由s个半空间所包围，其中第l个半空间定义为${\mathcal{H}_{i,l}}$；${{\boldsymbol{a}}_{i,l}} \in {\mathbb{R}^3}$和${b_{i,l}} \in \mathbb{R}$分别表示半空间的法向量和偏移量。

为确保大型AUV全身避障安全性，在其航行过程中需要保证$P(t) \subseteq \mathcal{S}$。更一般地说，第i段轨迹对应凸包的顶点集合${P_i}(t)$应包含在$\mathcal{P}_i^\mathcal{H}$内。将${{\boldsymbol{\varepsilon }}_{i,j}}$对应的凸包顶点集合与半空间${\mathcal{H}_{i,l}}$边界之间的垂直有向距离定义为${D_{i,l}}$，其表达式如下：

当存在凸包顶点位于半空间外部时，${D_{i,l}}$为正。为了区分，此处将正值记为${D_{i,{l^ + }}}$。因此，大型AUV全身避障约束${\mathcal{G}_S}$可以解析表示为

式中：$s_i^ +$是${D_{i,{l^ + }}}$的数量；相应的半空间记作${\mathcal{H}_{i,{l^ + }}}$。对应的约束梯度计算过程如下：

考虑AUV动态避障时的全身安全性，为简化公式，此处仅以单个移动障碍物为例，对于存在多个移动障碍物的情况，需要按照给定的规则重复计算。将上一节中动态感知过程得到的最小包围椭球体记为$\mathcal{B}(t)$，对应表达式为

式中：${\boldsymbol{R}} = {\text{Diag}}\{ {e_a},{e_b},{e_c}\}$是椭球三轴半径矩阵；${\boldsymbol{\gamma }} \in {\mathbb{R}^3}$代表椭球参数化向量；${{\boldsymbol{J}}_{\text{u}}}({\boldsymbol{q}})$是用于坐标变换的雅可比矩阵，与式（8）形式基本一致。

判断AUV是否与移动障碍物发生碰撞需要利用三维凸包和椭球体的位姿信息。考虑到包围AUV的凸包顶点与椭球体之间最小距离的计算涉及到非线性优化问题，不存在简单的解析解，这对后续梯度求解造成挑战。为解决该问题，本文采用点到椭球体的归一化距离来度量碰撞程度。约束点${{\boldsymbol{\varepsilon }}_{i,j}}$对应的三维凸包顶点集合和椭球体$\mathcal{B}(t)$之间的归一化距离被定义为${Z_i}$，计算过程如下：

当存在凸包顶点位于椭球体内部时，${Z_i} < 1$。为了区分，将该类值记为${Z_{{i^ - }}}$，与之相关的顶点坐标写作${{\boldsymbol{q}}_{{z^ - }}}$。因此，大型AUV动态避障的全身安全性约束及其梯度可以被解析推导为：

综上所述，本节完成了涵盖AUV轨迹敏捷性、快速性、运动学与动力学可行性以及动静态避障全身安全性在内的复杂多元目标与约束的解析构建过程，并进行了其各自对应梯度传播链的详细解释与推导。至此，式（20）满足梯度求解的基本条件。之后，可以借助现有的梯度下降或拟牛顿法等梯度类优化求解算法获取该无约束优化问题的最优解。

2.3   航向修正轨迹跟踪

由于AUV欠驱动以及非最小相位等特性影响，直接在笛卡尔直角坐标系下跟踪局部轨迹${{\boldsymbol{\varepsilon }}_{\text{d}}}(t)$较为困难。受文献[8]启发，本文将原始问题转化为在球坐标系下跟踪期望速度和角度，坐标变换如图8所示，新参考信号与原始信号关系如下：

图  8  坐标变换示意图

Figure  8.  The illustration of coordinate transformation

下载: 全尺寸图片 幻灯片

式中，$u_{{\text{ld}}}^{{\text{emo}}}(t)$，$\theta _{{\text{ld}}}^{{\text{emo}}}(t)$，$\psi _{{\text{ld}}}^{{\text{emo}}}(t)$分别表示球坐标系下的期望速度和姿态角，上角标emo表示航向的指数修正过程（EMO）。该修正是为了保证当系统跟踪上${\boldsymbol{S}}_{{\text{ld}}}^{{\text{emo}}}$时，位置误差${{\boldsymbol{e}}_{\boldsymbol{\varepsilon }}}(t) = {\left\| {{{\boldsymbol{\varepsilon }}_{\text{d}}}(t) - {\boldsymbol{\varepsilon }}(t)} \right\|_2}$也能够以指数形式收敛至零。

3.   高保真实验结果

3.1   实验环境与参数设置

为验证提出的三维避障方法的有效性，在基于ROS和UWSim开发的高保真仿真平台上进行测试。实验所用的大型AUV长13.96 m，直径2 m，并考虑六自由度的完整运动学与动力学模型和执行器限制。此外，安装在AUV头部的虚拟三维多波束声呐参数配置与Coda Octopus Products有限公司的实际设备基本一致，其中单波范围为0.5～150 m，开角范围为102º×51º，波束间距为0.80º×0.40º。实验所用的水下未知非结构化避障环境如图9所示。此外，高保真实验的水下环境中还包含了两艘具备独立运行轨迹的水下航行器作为移动障碍物，其速度分别被设置为2.5和2 m/s。为验证算法的有效性和鲁棒性，设计两组速度约束不同的高速避障实验，其中AUV的最大速度分别被设置为6.0和7.0 m/s。两组避障实验中，AUV最大加速度被设置为1.0 m/s，俯仰角被限制在0.4 rad以内。AUV俯仰和偏航角速度分别被限制在0.08和0.12 rad/s，最大角加速度则被设置为0.1和0.15 rad/s²。

图  9  高保真实验水下环境

Figure  9.  Underwater environment for simulation

下载: 全尺寸图片 幻灯片

3.2   长航程海床高速跨越

如图10和图11所示，在两组速度约束不同的高速避障实验中，AUV都成功躲避了未知的水下动静态障碍物并抵达最终目标，全航程分别维持6.0和7.0 m/s的不同预设速度。现有的大多数二维或伪三维水下避障方法将AUV限制在水平面或垂直面，而本文所提出的三维高速避障方案使AUV能够有效穿越非结构化的三维海底地形，同时根据声呐传感器信息来实时估计并预测移动障碍物运动状态，实现敏捷的碰撞风险规避。

图  10  长距离海床高速跨越实验（1）结果

Figure  10.  Results of long-range seafloor terrain high-speed crossing experiment (1)

下载: 全尺寸图片 幻灯片

图  11  长距离海床高速跨越实验（2）结果

Figure  11.  Results of long-range seafloor terrain high-speed crossing experiment (2)

下载: 全尺寸图片 幻灯片

此外，AUV在避障过程中严格遵守多元约束。其中，由图10和图11可见，AUV动静态避障安全性约束得到显式满足。对于运动学与动力学可行性约束，如图12和图13所示，AUV的俯仰角始终维持在预定义的界限0.4 rad内。如图14和图15所示，AUV的速度范数分别维持在预设速度6.0和7.0 m/s上，且其前进速度与合速度之间的一致性能够支持基于严格非完整性约束所假设的简化模型的合理性。最后，如表1和表2所总结，长航程海床高速跨越过程中AUV运动学与动力学状态量均可满足此前设置的各项可行性约束。

图  12  高保真实验（1）AUV俯仰角

Figure  12.  Pitch angles of the AUV in experiment (1)

下载: 全尺寸图片 幻灯片

图  13  高保真实验（2）AUV俯仰角

Figure  13.  Pitch angles of the AUV in experiment (2)

下载: 全尺寸图片 幻灯片

图  14  高保真实验（1）AUV速度剖面

Figure  14.  Speed profiles of the AUV in experiment (1)

下载: 全尺寸图片 幻灯片

图  15  高保真实验（2）AUV速度剖面

Figure  15.  Speed profiles of the AUV in experiment (2)

下载: 全尺寸图片 幻灯片

表  1  实验（1）运动学与动力学状态量

Table  1.  Kinematic and dynamic variables in experiment (1)

参数	属性	数值		参数	属性	数值
速度 /（m·s−1）	平均值	5.66		加速度 /（m·s−2）	平均值	0.11
	标准差	1.05			标准差	0.20
	最大值	6.06			最大值	0.88
俯仰角 速度/（rad·s−1）	平均值	0.02		俯仰角加速度 /（rad·s−2）	平均值	0.01
	标准差	0.02			标准差	0.02
	最大值	0.07			最大值	0.09
偏航角 速度/（rad·s−1）	平均值	0.04		偏航角加速度 /（rad·s−2）	平均值	0.02
	标准差	0.03			标准差	0.03
	最大值	0.12			最大值	0.13

下载: 导出CSV 

| 显示表格

表  2  实验（2）运动学与动力学状态量

Table  2.  Kinematic and dynamic variables in experiment (2)

参数	属性	数值		参数	属性	数值
速度 /（m·s−1）	平均值	6.47		加速度 /（m·s−2）	平均值	0.16
	标准差	1.39			标准差	0.24
	最大值	7.08			最大值	0.92
俯仰角 速度/（rad·s−1）	平均值	0.02		俯仰角加速度 /（rad·s−2）	平均值	0.01
	标准差	0.02			标准差	0.02
	最大值	0.08			最大值	0.11
偏航角 速度/（rad·s−1）	平均值	0.04		偏航角加速度 /（rad·s−2）	平均值	0.03
	标准差	0.03			标准差	0.03
	最大值	0.12			最大值	0.15

下载: 导出CSV 

| 显示表格

两组速度约束不同的对比实验进一步验证了本文所提出的AUV三维高速避障方法的有效性与鲁棒性。在实际应用中，可以灵活调整约束，以遵守设备的性能限制和满足作业的任务需求。

4.   结　语

本文提出了一种针对大型、高速、欠驱动AUV的实时三维动静态障碍物规避方法，集成了以机器人为中心的双分辨率海底地形图构建、包含多变量滤波以及椭球特征提取与匹配的移动障碍物跟踪、全局风险感知路径搜索、局部时空联合轨迹优化以及考虑航向指数修正的反馈控制器。长航程海床高速跨越的高保真实验验证了方法有效性，实现了大型、高速、欠驱动AUV在未知非结构化环境中考虑有限视域与复杂多元约束下的安全、敏捷、高效三维避障。结果表明，实验过程中AUV的运动与动力学可行性和动静态避障全身安全性均得到满足。本研究为大型AUV深海长距离高速航行的三维避障提供技术支撑，有助于提升其自主作业能力。本方法的集成化设计与对复杂环境的适应能力为大型AUV在深海探测、海洋资源勘探以及国家安全等领域的广泛应用提供了可能。但目前本方法还没有考虑高速海流和声学可见度严重受限的情况，因此，在极端海洋环境条件下的应用还有待进一步探索。