0 引　言

随着传感器技术和机器学习算法的飞速发展，无人艇被广泛应用于多个海洋关键领域，如资源勘探、自主搜索与救援、环境信息监测等。无人艇以其强大的自主作业能力和高效的任务执行效率，成为了现代海洋探索和资源开发的重要工具^[1]。其中，路径跟踪方法作为其关键技术之一，扮演着至关重要的角色^[2]。无论是在开阔的海域，还是在复杂的近岸水域，无人艇都能依据路径跟踪算法提供的精确指导，保持稳定的航向和速度，确保各项任务能够准确无误地完成^[3]。

无人艇路径跟踪算法是一种通过计算当前位置与目标路径之间的偏差，动态调整路径跟踪策略，以提高路径跟踪的准确性和稳定性。无人艇路径跟踪问题的关键是如何对目标轨迹采用优化算法，制定出更有效的路径规划策略以供控制器使用^[4]。为了解决这些不确定性问题，许多研究提出了不同的方法。滑模控制方案的研究中，控制策略主要是将固定时间与滑膜理论相结合。这种基于输出反馈的方法通常在直线路径跟踪问题上取得较好效果，而在较复杂路径下跟踪效果并不理想^[5]。Zhao等^[6]在连续折线跟踪场景下，提出一种不确定环境下无人艇的折线跟踪算法，但其收敛速度均有较大的提升空间。

随着信息技术的快速发展，深度学习方法正在逐步增强无人艇路径追踪研究对模型不确定性的适应。近年来，许多强化学习算法正在逐渐取代传统的方法，例如深度确定性策略梯度算法等策略优化学习算法也被应用到无人艇路径跟踪研究中^[7]，Zhong等^[8]也证明通过构造新的状态空间和奖励函数，能够提高无人艇跟踪的有效性和稳定性。尽管强化学习方法在无人艇路径跟踪问题上具有较好的性能，可以通过学习来解决无人艇模型的不确定性。但在训练过程中，特别是在训练初期，仍然存在较大误差的问题。同时，无人艇运动学模型的训练成本高、建模不准确等问题仍然阻碍着强化学习方法的发展。

在无人艇路径跟踪中，贝叶斯平滑器（Bayesian Smoothing，BS）主要用于去除传感器噪声，还原真实轨迹数据。例如，通过卡尔曼滤波和平滑器对传感器探测到的数据进行预处理，可以过滤掉无关数据，得到更加准确的位置信息。Mousazadeh等^[9]开发了一种基于扩展卡尔曼滤波、搜索球和势流场概念的融合算法。但该研究一般不考虑模型的附加约束问题，采集输入是完整状态，不考虑测量误差。卡尔曼滤波的优势在于能够通过递归更新，结合噪声和不确定性信息，对动态系统的状态进行实时、精准估计。何静^[10]利用卡尔曼滤波技术，结合适当的运动模型和状态方程，对船舶轨迹进行动态建模，通过估计与实际观测的偏差有效实现异常轨迹的检测。虽然贝叶斯平滑器能够处理去除传感器数据中的噪声和异常值，但会引入一定的相位失真，影响数据的准确性。

因此，本文结合贝叶斯平滑器与优化方法的优势，提出了一种基于贝叶斯平滑-交替方向乘子法（Bayesian Smoothing-Alternating Direction Method of Multipliers, BS-ADMM）的无人艇路径跟踪方法。该方法基于无人艇的三自由度运动学和动力学模型，构建了无人艇路径跟踪的状态空间模型，定义相应的等式和不等式约束函数，以适应多样化的无人艇任务需求。通过将无人艇路径跟踪问题建模为一类非线性约束下的优化问题，利用提出的BS-ADMM方法进行求解，最终实现高效、精确的无人艇路径跟踪。

1 无人艇状态空间方程建模 1.1 无人艇三自由度运动方程

无人艇运动的数学方程建模过程可以从运动学和动力学两方面来考虑。为了描述无人艇运动情况，可选取2种右手直角坐标系：惯性坐标系（或地球坐标系）和运动坐标系（或随体坐标系）。地球坐标系的原点位于地心，x坐标轴选为向东为正，$y$坐标轴向北为正；运动坐标系的原点选为船舶重心处，$x$坐标轴选为船纵轴，以指向船首为正；$y$轴与纵剖面垂直，以指向右舷为正，2个坐标系下船舶平面运动示意图如图1所示。

当无人艇受到外力扰动时，可能伴随着纵摇、横摇和升沉方向上的运动。大多数无人艇在力的作用下，这些方向上的运动较小，因此为了简便运算，通常忽略不计。在无人艇运动建模中，只考虑如表1所示的纵荡、横荡和艏摇3个自由度，其中在各个自由度上的位置矢量和速度矢量命名规则遵循美国造船工程师学会。

在建立运动方程中，通常假设无人艇质量分布均匀且关于$XOZ$平面对称，由此可得在$XOY$和$YOZ$平面的惯性矩${I_{xy}}$和${I_{yz}}$均为0，船舶重心和浮心均垂直位于$z$轴上。在上述条件下，能够得到无人艇的三自由度运动学和动力学模型^[11]：

$ \left\{ {\begin{aligned} &\boldsymbol{\dot{\eta}=J}\left(\boldsymbol{\eta}\right)\boldsymbol{\nu}，\\ &\boldsymbol{M\dot{\nu}=-C(\nu)\nu-(D+}\boldsymbol{D}_{\boldsymbol{n}}\boldsymbol{(\nu))\nu+\tau+}\boldsymbol{\tau}_{\boldsymbol{E}}。\end{aligned}} \right. $

(1)

式中：${\boldsymbol{\eta }} = {\left[ {x,y,\phi } \right]^{\text{T}}}$为3个自由度上的位置向量；${\boldsymbol{\nu }} = {\left[ {u,v,r} \right]^{\text{T}}}$为3个自由度上的速度向量；${\boldsymbol{J}}\left( {\boldsymbol{\eta }} \right)$为2个坐标系之间转换的旋转矩阵；${\boldsymbol{M}}$为附加质量矩阵；${\boldsymbol{C}}\left( {\boldsymbol{\nu }} \right)$为流体力学科氏力矩与向心力矩阵；${\boldsymbol{D}}$为线性阻尼矩阵；${{\boldsymbol{D}}_{\boldsymbol{n}}}\left( {\boldsymbol{\nu }} \right)$为非线性阻尼矩阵。环境扰动常常代表着风、浪、流对于船舶运动的干扰，在动力学数学模型中建模为$ \boldsymbol{\tau}_{\boldsymbol{E}}=\left[\tau_{uE},\tau_{vE},\tau_{rE}\right]\mathit{^{\text{T}}} $，分别是作用在三自由度上的扰动力和力矩。模型中推进力和力矩$ \boldsymbol{\tau}=\left[\tau_u,0,\tau_r\right]\mathit{^{\text{T}}} $，在推进系统中，没有考虑横荡自由度上独立的推进器，这意味着所提出的模型是欠驱动的，符合无人艇的特性。

具体来说，各矩阵定义如下^[12]：

$ \left\{ {\begin{aligned} { \begin{gathered} {\boldsymbol{J(\eta )}} = \left( {\begin{array}{*{20}{c}} {\cos \phi }&{ - \sin \phi }&0 ，\\ {\sin \phi }&{\cos \phi }&0 ，\\ 0&0&1 \end{array}} \right), \\ {\boldsymbol{M}} = \left( {\begin{array}{*{20}{c}} {m - X\dot u}&0&0 \\ 0&{m - Y\dot v}&{m{x_g} - Y\dot v} \\ 0&{m{x_g} - Y\dot r}&{{I_z} - N\dot r} \end{array}} \right) ，\\ {\boldsymbol{C(\nu )}} = \left( {\begin{array}{*{20}{c}} 0 & 0 & { - m({x_g}r + v) + Y\dot vv + Y\dot rr} \\ 0 & 0 & {mu - X\dot uu} \\ {m({x_g}r + v) - Y\dot vv - Y\dot rr} & { - mu + X\dot uu} & 0 \end{array}} \right)，\\ {\boldsymbol{D}} = - \left( {\begin{array}{*{20}{c}} {{X_u}}&0&0 \\ 0&{{Y_v}}&{{Y_r}} \\ 0&{{N_V}}&{{N_r}} \end{array}} \right), \\ {{\boldsymbol{D}}_{\boldsymbol{n}}}{\boldsymbol{(\nu )}} = - \left( {\begin{array}{*{20}{c}} {{X_{\left| u \right|u}}\left| u \right|}&0&0 \\ 0&{{Y_{\left| v \right|v}}\left| v \right| + {Y_{\left| v \right|r}}\left| r \right|}&{{Y_{\left| v \right|r}}\left| v \right|} \\ 0&{{N_{\left| v \right|v}}\left| v \right| + {N_{\left| v \right|r}}\left| r \right|}&{{N_{\left| v \right|r}}\left| v \right| + {N_{\left| r \right|r}}\left| r \right|} 。\end{array}} \right)。\\ \end{gathered} } \end{aligned}} \right.$

(2)

式中：$m$为无人艇质量；${x_g}$为重心在随体坐标系中的横坐标，本文将坐标原点选在重心处，即为0。F_n、${F_{\dot n}}$、${F_{\left| n \right|n}} $、${F_{\dot nn}} $为对应自由度上的水动力导数，分为一阶导数和二阶导数，速度导数和加速度导数，需要通过水动力学试验确定。

在一些情况下，为了简化运算，常常考虑到船舶左/右的对称性，以及水面平面运动不需要考虑顶/底部的不对称性，而艏/艉不对称通常是惯性矩阵和阻尼矩阵的非对角线项为零，但与主对角线元素相比，这些项非常小，可以忽略不计。因此，忽略附加质量矩阵${\boldsymbol{M}}$和线性阻尼矩阵${\boldsymbol{D}}$的非对角项和非线性阻尼矩阵${{\boldsymbol{D}}_{\boldsymbol{n}}}\left( {{\nu }} \right)$的全部元素，同时不考虑外部环境扰动影响，可将式(2)简化如下：

$ \begin{gathered} {\boldsymbol J}(\eta ) = \left( {\begin{array}{*{20}{c}} {\cos \phi } & { - \sin \phi } & {0} \\ {\sin \phi } & {\cos \phi } & {0} \\ 0 & {0} & {1} \end{array}} \right),{\boldsymbol M} = \left( {\begin{array}{*{20}{c}} {{m_{11}}} & {0} & {0} \\ 0 & {{m_{22}}} & {0} \\ 0 & {0} & {{m_{33}}} \end{array}} \right)，\\ {\boldsymbol C}(\nu ) = \left( {\begin{array}{*{20}{c}} {0\;\;\;\;\;\;} & {0} & { - {m_{22}}v} \\ 0 & {0} & {{m_{11}}u} \\ {{m_{22}}v} & { - {m_{11}}u} & {\;\;\;\;0} \end{array}} \right){\text{,}}{\boldsymbol D} = \left( {\begin{array}{*{20}{c}} {{d_{11}}} & {\;0} & {0} \\ 0 & {{d_{22}}} & {0} \\ 0 & {0} & {{d_{33}}} \end{array}} \right) 。\\ \end{gathered} $

(3)

式中：${m_{11}} = m - {X_{\dot u}}$，${m_{22}} = m - {Y_{\dot v}}$，$ {m_{33}} = {I_z} - {N_{\dot r}} $；${d_{11}} = - {X_u}$，${d_{22}} = - {Y_v}$，${d_{33}} = - {N_r}$；${I_z}$为无人艇在运动坐标系下$z$轴上的惯性分量。

将该模型展开为方程形式，得到如下的无人艇三自由度运动方程：

$ \left\{ {\begin{aligned} & \dot{x}=u\cos\phi-v\sin\phi，\\ & \dot{y}=u\sin\phi+v\cos\phi，\\ & \dot{\phi}=r，\\ & \dot{u}=\displaystyle\frac{m_{22}}{m_{11}}vr-\displaystyle\frac{d_{11}}{m_{11}}u+\displaystyle\frac{1}{m_{11}}\tau_{u，} \\ & \dot{v}=-\displaystyle\frac{m_{11}}{m_{22}}ur-\displaystyle\frac{d_{22}}{m_{22}}v，\\ & \dot{r}=\displaystyle\frac{m_{11}-m_{22}}{m_{33}}uv-\displaystyle\frac{d_{33}}{m_{33}}r+\displaystyle\frac{1}{m_{33}}\tau_{r。} \end{aligned}} \right. $

(4)

1.2 状态空间方程建模

为了提高路径跟踪算法的计算速度，将无人艇运动视为一个部分观测马尔可夫过程：

$ {{\boldsymbol{x}}_t} \sim p\left( {{{\boldsymbol{x}}_t}\left| {{{\boldsymbol{x}}_{t - 1}}} \right.} \right),{\text{ }}{{\boldsymbol{y}}_t} \sim p\left( {{{\boldsymbol{y}}_t}\left| {{{\boldsymbol{x}}_t}} \right.} \right){\text{ }} 。$

(5)

式中：${{\boldsymbol{x}}_t} \in {\mathbb{R}^{{N_x}}}$为系统的${N_x}$状态向量；${{\boldsymbol{y}}_t} \in {\mathbb{R}^{{N_y}}}$为系统的${N_y}$维测量信息：时间变量$t = 1,2, \ldots ,T$。$p\left( {{{\boldsymbol{x}}_t}\left| {{{\boldsymbol{x}}_{t - 1}}} \right.} \right)$为马尔可夫过程的转移概率密度；$p\left( {{{\boldsymbol{y}}_t}\left| {{{\boldsymbol{x}}_t}} \right.} \right)$为测量的条件概率密度。另一方面，定义无人艇的状态向量${{\boldsymbol{x}}_t} = {\left[ {x,y,\phi ,u,v,r} \right]^{\text{T}}}$，观测向量${{\boldsymbol{y}}_t}$，该定义充分考虑了测量传感器因环境扰动和噪声影响带来的非完备特性。

假设系统误差${{\boldsymbol{q}}_t}$和${{\boldsymbol{r}}_t}$是相互独立的零均值高斯随机变量，其正定协方差矩阵分别为${{\boldsymbol{Q}}_t}$和${{\boldsymbol{R}}_t}$，则无人艇状态空间模型如下：

$ \boldsymbol{x}_t=\boldsymbol{a}_t\left(\boldsymbol{x}_{t-1}\right)+\boldsymbol{q}_t,\text{ }\boldsymbol{y}_t=\boldsymbol{h}_t\left(\boldsymbol{x}_t\right)+\boldsymbol{r}_t。$

(6)

式中：${{\boldsymbol{h}}_t}:{\mathbb{R}^{{N_x}}} \to {\mathbb{R}^{{N_y}}}$是系统的测量方程；${{\boldsymbol{a}}_t}:{\mathbb{R}^{{N_x}}} \to {\mathbb{R}^{{N_x}}}$为系统的转移函数。通常初始状态${{\boldsymbol{x}}_1}$被认为是符合高斯分布的，均值为$ {{m}_1 } $，协方差矩阵为${{\boldsymbol{P}}_{\text{1}}}$。将外部环境作为约束条件，得到如下不等式方程：

$ {{e} _t}\left( {{{\boldsymbol{x}}_t}} \right) = 0,{\text{ }}{{c} _t}\left( {{{\boldsymbol{x}}_t}} \right) \leqslant 0 。$

(7)

式中：${{e} _t}:{\mathbb{R}^{{N_x}}} \to {\mathbb{R}^{{N_e}}}$和${{c} _t}:{\mathbb{R}^{{N_x}}} \to {\mathbb{R}^{{N_c}}}$为无人艇状态的约束函数。根据式(4)，可以计算得到连续条件下无人艇非线性状态空间模型为：

$ \boldsymbol{a}_t\left(\boldsymbol{x}_{t-1}\right) = \left[ \begin{array}{*{20}{c}}\cos\left(\phi\right)u-\sin\left(\phi\right)v \\ \sin\left(\phi\right)u+\cos\left(\phi\right)v \\ r \\ \displaystyle\frac{m_{22}}{m_{11}}vr - \displaystyle\frac{d_{11}}{m_{11}}u + \frac{\tau_u}{m_{11}} \\ -\displaystyle\frac{m_{22}}{m_{11}}uv-\displaystyle\frac{d_{22}}{m_{22}}v \\ -\displaystyle\frac{m_{22}}{m_{33}}vu + \displaystyle\frac{m_{11}}{m_{33}}uv-\displaystyle\frac{d_{33}}{m_{33}}r+\displaystyle\frac{\tau_r}{m_{33}}\end{array} \right],\boldsymbol{h}_t\left(\boldsymbol{x}_t\right) = \left[ \begin{array}{*{20}{c}}\boldsymbol{x} \\ \boldsymbol{y}\end{array} \right]。$

(8)

此时，状态空间方程为连续非线性，应用优化求解方法时要将状态空间方程转为离散情况。常见的离散化方法有欧拉法和零阶保持法^[13]。为了简化计算和形式简单，选择欧拉法对状态空间方程离散化，利用泰勒展开的一阶近似，将状态导数表示为：

$ \boldsymbol{\dot{x}}=\frac{\boldsymbol{x}_{t+1}-\boldsymbol{x}_t}{T_z}。$

(9)

式中：${T_z}$为采样时间。离散条件下的无人艇状态空间方程为：

$ \left\{ {\begin{aligned} \boldsymbol{x}_t=\boldsymbol{A}_t\boldsymbol{x}_{t-1}+\boldsymbol{q}_t，\\ \boldsymbol{y}_t=\boldsymbol{H}_tx_{t-1}+\boldsymbol{r}_t。\\ \end{aligned}} \right.$

(10)

在离散的状态空间方程中，$ {{\boldsymbol{A}}_t} = I + {T_z}{{\boldsymbol{a}}_t}\left( {{{\boldsymbol{x}}_{t - 1}}} \right) $，$ {{\boldsymbol{H}}_t} = {{\boldsymbol{h}}_t}\left( {{{\boldsymbol{x}}_t}} \right) $。因此，通过将上述的转移和观测向量$ {{\boldsymbol{a}}_t}\left( {{{\boldsymbol{x}}_{t - 1}}} \right) $和$ {{\boldsymbol{h}}_t}\left( {{{\boldsymbol{x}}_t}} \right) $写成矩阵形式，可得到离散形式下的转移矩阵和观测矩阵为：

$ \begin{gathered}\boldsymbol{A}_t=\left[ \begin{array}{*{20}{c}}T_z & 0 & 0 & \cos\phi_{t-1} & -\sin\phi_{t-1} & 0 \\ 0 & T_z & 0 & \sin\phi_{t-1} & \cos\phi_{t-1} & 0 \\ 0 & 0 & T_z & 0 & 0 & 1 \\ 0 & 0 & 0 & T_z-\displaystyle\frac{d_{11}}{m_{11}} & \displaystyle\frac{m_{22}}{m_{11}}r_{t-1} & 0 \\ 0 & 0 & 0 & -\displaystyle\frac{m_{11}}{m_{22}}r_{t-1} & T_z-\displaystyle\frac{d_{22}}{m_{22}} & 0 \\ 0 & 0 & 0 & \displaystyle\frac{m_{11}}{m_{33}}v_{t-1} & -\displaystyle\frac{m_{22}}{m_{33}}u_{t-1} & T_z+\displaystyle\frac{d_{33}}{m_{33}}\end{array} \right] \\ \boldsymbol{H}_t=\left[\begin{array}{*{20}{c}}1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0\end{array}\right]。\\ \end{gathered}，$

(11)

至此，结合公式(6)、式(7)和式(10)，将无人艇的路径追踪视作从部分观测序列$ {{\boldsymbol{y}}_{1:T}} = \left\{ {{{\boldsymbol{y}}_t}} \right\}_{t = 1}^{\text{T}} $估计原始无人艇状态信息${{\boldsymbol{x}}_{1:T}} = \left\{ {{{\boldsymbol{x}}_t}} \right\}_{t = 1}^{\text{T}}$的过程。根据最大后验概率估计理论，本文将无人艇路径跟踪问题建模为一个非线性约束下的优化问题。为了有效求解该问题，本文提出了一种基于贝叶斯平滑的交替方向乘子法（BS-ADMM），用于解决无人艇路径跟踪中的复杂优化挑战。

2 贝叶斯平滑-交替方向乘子法（BS-ADMM） 2.1 交替方向乘子法（ADMM）算法框架

在无人艇路径跟踪问题中，引入等式和不等式约束具有重要的现实意义。这些约束不仅确保了无人艇在遵循特定动态模型和边界条件下的运动，还避免了复杂约束带来的求解困难问题。通常情况下，等式约束用于模拟无人艇在运动过程中必须遵循的物理条件限制，或确保无人艇按照预定路径行驶；而不等式约束则用于处理实际操作中的各种限制条件，如避免碰撞、避开禁航区，或考虑环境因素（如水流、风力等）对航行的影响。通过引入这些约束，路径跟踪能够更加贴近实际情况，从而显著提高无人艇航行的安全性、稳定性和效率。

ADMM是一种广泛应用于求解分布式、大规模优化问题的算法^[14]，其核心思想是利用增广拉格朗日法，将多优化变量问题转化为单优化变量问题。在无人艇路径跟踪问题中，由于该问题是一个高斯驱动的非线性系统，其成本函数可以表示为：

$ \begin{split}\theta\left(\boldsymbol{x}_{1:T}\right)= & \frac{1}{2}\sum\limits_{t=1}^T \left\| \boldsymbol{y}_t-\boldsymbol{h}_t\left(\boldsymbol{x}_t\right) \right\| _{\boldsymbol{R}_t^{-1}}^2+ \\ & \frac{1}{2}\sum\limits_{t=2}^T \left\| \boldsymbol{x}_t-\boldsymbol{a}_t\left(\boldsymbol{x}_{t-1}\right) \right\| _{\boldsymbol{Q}_t^{-1}}^2+\frac{1}{2} \left\| \boldsymbol{x}_1-\boldsymbol{m}_1 \right\| _{\boldsymbol{P}_1^{-1。}}^2\end{split} $

(12)

式中：$\left\| {{\boldsymbol x}} \right\|_{{\boldsymbol R}}^2 = {{{\boldsymbol x}}^{{{{\rm T}}}}}{\boldsymbol{Rx}}$。引入等式和不等式约束函数方程(7)，则该问题可以写成如下非凸优化问题：

$ \mathop {\min }\limits_{{{\boldsymbol{x}}_{1:T}}} \theta \left( {{{\boldsymbol{x}}_{1:T}}} \right),{\text{ s}}{\text{.t}}{\text{.}}{e} \left( {{{\boldsymbol{x}}_{1:T}}} \right) = 0,{c} \left( {{{\boldsymbol{x}}_{1:T}}} \right) \leqslant 0 。$

(13)

在ADMM的计算过程中，将大的全局问题分解为多个较小、较容易求解的局部子问题，并通过计算子问题的解而得到原始问题的最优解，从而减少计算量，提高计算效率。对式(13)，引入一系列辅助变量${n_t}$和对应的障碍函数$ {I} \left( {{n_t}} \right) $，可得：

$ \begin{aligned} &{\mathop {\min }\limits_{{{\boldsymbol{x}}_{1:T}}} \theta \left( {{{\boldsymbol{x}}_{1:T}}} \right)} \\ &{{\text{s}}{\text{.t}}{\text{. }}{{e} _t}\left( {{{\boldsymbol{x}}_t}} \right) = 0,{{c} _t}\left( {{{\boldsymbol{x}}_t}} \right) + {n_t} = 0,{\text{ }}t = 1,2, \ldots T} \\ &{{I} \left( {{n_t}} \right) = \left\{ {\begin{array}{l} {0,{\text{ }}{n_t} \geqslant 0}，\\ {\infty ,{\text{otherwise}}} 。\end{array}} \right.} \end{aligned} ，$

(14)

引入拉格朗日乘子${\eta _{1:T}}$和${\zeta _{1:T}}$和惩罚参数${\rho _1},{\rho _2} > 0$，为公式(14)定义增广拉格朗日函数。然后，定义初始的状态变量和参数变量$ {x^{\left( 0 \right)}},{n^{\left( 0 \right)}},{\eta ^{\left( 0 \right)}},{\zeta ^{\left( 0 \right)}} $，ADMM通过迭代步骤求解优化子问题：

$ \left\{ {\begin{aligned} &{\boldsymbol{x}}_{1:T}^{\left( {k + 1} \right)} = \mathop {\arg \min }\limits_{{{\boldsymbol{x}}_{1:T}}} \theta \left( {{{\boldsymbol{x}}_{1:T}}} \right) + \frac{{{\rho _1}}}{2}\left\| {{c} \left( {{{\boldsymbol{x}}_{1:T}}} \right) + n_{1:T}^{(k)} + } \right.{{\left. {{{\eta _{1:T}^{(k)}} / {{\rho _1}}}} \right\|}^2} +\\ &\ \ \ \ \ \ \ \ \ \ \ \ \ \frac{{{\rho _2}}}{2}\left\| {e} \left( {{{\boldsymbol{x}}_{1:T}}} \right) + {{{\zeta _{1:T}^{(k)}} /{{\rho _2}}}} \right\|^2 ，\\ &{n_{1:T}^{(k + 1)} = \max \left( {0, - {c} \left( {{\boldsymbol{x}}_{1:T}^{(k + 1)}} \right) - {{\eta _{1:T}^{(k)}} \mathord{\left/ {\vphantom {{\eta _{1:T}^{(k)}} {{\rho _1}}}} \right. } {{\rho _1}}}} \right)}，\\ &{\eta _{1:T}^{(k + 1)} = n_{1:T}^{(k + 1)} + {\rho _1}\left( {{c} \left( {{\boldsymbol{x}}_{1:T}^{(k + 1)}} \right) + n_{1:T}^{(k + 1)}} \right)} ，\\ &{\zeta _{1:T}^{(k + 1)} = \zeta _{1:T}^{(k)} + {\rho _2}{e} \left( {{\boldsymbol{x}}_{1:T}^{(k + 1)}} \right)} 。\end{aligned}} \right.$

(15)

式中：max为取最大值函数，min为取最小值函数。在子问题迭代求解过程中，求解步骤采用交替$ {x}_{1:T},{n}_{1:T}, {\eta }_{1:T}和{\zeta }_{1:T} $最小化的方式求解。在${{\boldsymbol{x}}_{1:T}}$子问题，当时间状态T较大时，直接通过梯度下降法求解其最优解会带来较高的计算代价。为此，本文提出了一种增广贝叶斯平滑器，将${{\boldsymbol{x}}_{1:T}}$子问题按照从1～$T$的时间序列分解，从而得到其对应的函数集。这种方法不仅降低了计算复杂度，还提高了求解效率，特别适用于大规模时间状态下的优化问题。

2.2 ${{\boldsymbol{x}}_{1:T}}$子问题的求解方法

当无人艇状态空间是线性仿射的，对于式(15)中${{\boldsymbol{x}}_{1:T}}$子问题，能够定义为：

$ \left\{ {\begin{aligned} & \boldsymbol{a}_t\left(\boldsymbol{x}_{t-1}\right)=\boldsymbol{A}_t\boldsymbol{x}_{t-1}+\boldsymbol{b}_t,\text{ }\boldsymbol{h}_t\left(\boldsymbol{x}_t\right)=\boldsymbol{H}_t\boldsymbol{x}_t+\boldsymbol{g}_t，\\ & \boldsymbol{c}_t\left(\boldsymbol{x}_t\right)=\boldsymbol{C}_t\boldsymbol{x}_t+\boldsymbol{d}_t,\text{ }\boldsymbol{e}_t\left(\boldsymbol{x}_t\right)=\boldsymbol{E}_t\boldsymbol{x}_t+\boldsymbol{f}_t。\end{aligned}} \right. $

(16)

式中：${{\boldsymbol{E}}_t}$和${{\boldsymbol{C}}_t}$为约束函数矩阵；${{\boldsymbol{b}}_t},{{\boldsymbol{g}}_t},{{\boldsymbol{f}}_t},{{\boldsymbol{d}}_t}$为应系统方程内的偏置向量。因此，在公式(15)中，${{\boldsymbol{x}}_{1:T}}$子问题变成如下形式：

$ \begin{split} {\boldsymbol{x}}_{1:T}^* = & \mathop {\arg \min }\limits_{{{\boldsymbol{x}}_{1:T}}} \frac{1}{2}\sum\limits_{t = 1}^T {\left\| {{{\boldsymbol{y}}_t} - {{\boldsymbol{H}}_t}{{\boldsymbol{x}}_t} - {{\boldsymbol{g}}_t}} \right\|_{{\boldsymbol{R}}_t^{ - 1}}^2} + \\ &\frac{1}{2}\sum\limits_{t = 2}^T {\left\| {{{\boldsymbol{x}}_t} - {{\boldsymbol{A}}_t}{{\boldsymbol{x}}_{t - 1}} - {{\boldsymbol{b}}_t}} \right\|_{{\boldsymbol{Q}}_t^{ - 1}}^2} + \frac{{{\rho _2}}}{2}\sum\limits_{t = 1}^T {{{\left\| {{{\boldsymbol{E}}_t}{{\boldsymbol{x}}_t}{\text{ + }}{{\boldsymbol{f}}_t} + \frac{{{\zeta _t}}}{{{\rho _2}}}} \right\|}^2}} +\\ &\frac{{{\rho _1}}}{2}\sum\limits_{t = 1}^T {{{\left\| {{{\boldsymbol{C}}_t}{{\boldsymbol{x}}_t}{\text{ + }}{{\boldsymbol{d}}_t} + {{\boldsymbol{n}}_t} + \frac{{{\eta _t}}}{{{\rho _1}}}} \right\|}^2}} + \frac{1}{2}\left\| {{{\boldsymbol{x}}_1} - {{\boldsymbol{m}}_1}} \right\|_{{\boldsymbol{P}}_1^{ - 1}}^2 。\\ \end{split} $

(17)

为了解决式(17)，定义2个人工测量噪声$ {{\boldsymbol{\sigma}} _t} $和$ \boldsymbol{\delta}_t $，对应的协方差矩阵为$ {\displaystyle\sum _t} $和${\Delta _t}$，以及2个伪测量${{z} _t},{{w} _t}$。通过引入额外的测量信息来对子问题中的状态空间建模，可得到：

$ \begin{split}&\sum_t^{ }=\boldsymbol{I}\mathord{\left/\vphantom{\boldsymbol{I}\rho_1}\right.}\boldsymbol{\rho}_1,\text{ }z_t=-\boldsymbol{b}_t-\boldsymbol{\eta}_t\mathord{\left/\vphantom{\eta_t\rho_1}\right.}\boldsymbol{\rho}_1 ，\\ &\Delta_t=\boldsymbol{I}\mathord{\left/\vphantom{\boldsymbol{I}\rho_2}\right.}\boldsymbol{\rho}_2,\text{ }w_t=\boldsymbol{\zeta}_t\mathord{\left/\vphantom{\zeta_t\rho_2}\right.}\boldsymbol{\rho}_2。\end{split} $

(18)

式中：${\boldsymbol{I}}$为单位矩阵。为了解决式(17)，将${{\boldsymbol{x}}_{1:T}}$子问题按照从1$T$的时间序列分解，定义如下状态空间方程：

$ \left\{ {\begin{aligned} & \boldsymbol{x}_t=\boldsymbol{A}_t\boldsymbol{x}_{t-1}+\boldsymbol{b}_t+\boldsymbol{q}_t\text{ }，\\ & \text{ }\boldsymbol{y}_t=\boldsymbol{H}_t\boldsymbol{x}_t+\boldsymbol{g}_t+\boldsymbol{r}_t，\\ & z_t=\boldsymbol{C}_t\boldsymbol{x}_t+\boldsymbol{d}_t+\boldsymbol{\sigma}_t ，\\ & w_t=\boldsymbol{E}_t\boldsymbol{x}_t+\boldsymbol{f}_t+\boldsymbol{\delta}_t 。\end{aligned}} \right.$

(19)

式中：$ {{\boldsymbol{x}}_t} $为状态向量；$ {{\boldsymbol{y}}_t} $、$ {{z} _t} $和$ {{w} _t} $为观测向量；$t = 1,2, \ldots ,T$。接下来，可以利用下述2.3章节的增广贝叶斯平滑器求解。

2.3 增广贝叶斯平滑器

贝叶斯平滑算法的核心在于结合观测数据$ {{\boldsymbol{y}}_{1:T}} $来改进状态估计，计算某个时刻$k$的最优状态${{\boldsymbol{x}}_k}$。算法包含了滤波和平滑两大步骤，旨在实现逐步预测各时刻状态和结合观测数据改进每个时刻状态估计^[15]。对于状态空间方程(19)，预测步骤如下：

$ \left\{ {\begin{aligned} & m_t^ - = {{\boldsymbol{A}}_t}{m_{t - 1}} + {b_t} ，\\ & {\boldsymbol{P}}_t^ - = {{\boldsymbol{A}}_t}{{\boldsymbol{P}}_{t - 1}}{\boldsymbol{A}}_t^{\rm{T}} + {{\boldsymbol{Q}}_t} 。\end{aligned}} \right.$

(20)

式中：$m_t^ - $和${\boldsymbol{P}}_t^ - $分别为$t$时刻的预测均值和协方差。对于每一个观测向量分别更新，${{\boldsymbol{y}}_t}$的更新步骤如下：

$\left\{ {\begin{aligned} &{{\boldsymbol{S}}_t^y = {{\boldsymbol{H}}_t}{\boldsymbol{P}}_t^ - {\boldsymbol{H}}_t^{\text{T}} + {{\boldsymbol{R}}_t}}，\\ &{{\boldsymbol{K}}_t^y = {\boldsymbol{P}}_t^ - {\boldsymbol{H}}_t^{\text{T}}{{\left[ {{\boldsymbol{S}}_t^y} \right]}^{ - 1}}} ，\\ &{m_t^y = m_t^ - + {\boldsymbol{K}}_t^y\left[ {{{\boldsymbol{y}}_t} - {{\boldsymbol{H}}_t}m_t^ - - {g_t}} \right]} ，\\ &{{\boldsymbol{P}}_{_t}^y = {\boldsymbol{P}}_t^ - - {\boldsymbol{K}}_t^y{\boldsymbol{S}}_t^y{{\left[ {{\boldsymbol{K}}_t^y} \right]}^{\text{T}}}}。\end{aligned}} \right. $

(21)

${{\boldsymbol{z}}_t}$的更新步骤如下：

$ \left\{ {\begin{aligned} &{{\boldsymbol{S}}_t^z = {{\boldsymbol{C}}_t}{\boldsymbol{P}}_t^y{\boldsymbol{C}}_t^{\text{T}} + {\sum _t}} ，\\ &{{\boldsymbol{K}}_t^z = {\boldsymbol{P}}_t^y{\boldsymbol{C}}_t^{\text{T}}{{\left[ {{\boldsymbol{S}}_t^z} \right]}^{ - 1}}} ，\\ &{m_t^z = m_t^y + {\boldsymbol{K}}_t^z\left[ {{{\boldsymbol{z}}_t} - {{\boldsymbol{C}}_t}m_t^y - {d_t}} \right]} ，\\ &{{\boldsymbol{P}}_{_t}^z = {\boldsymbol{P}}_t^y - {\boldsymbol{K}}_t^z{\boldsymbol{S}}_t^z{{\left[ {{\boldsymbol{K}}_t^z} \right]}^{\text{T}}}}。\end{aligned}} \right. $

(22)

${{\boldsymbol{w}}_t}$的更新步骤如下：

$ \left\{ {\begin{aligned} &{{\boldsymbol{S}}_t^w = {{\boldsymbol{E}}_t}{\boldsymbol{P}}_t^z{\boldsymbol{E}}_t^{\text{T}} + {\Delta _t}} ，\\ &{{\boldsymbol{K}}_t^w = {\boldsymbol{P}}_t^z{\boldsymbol{E}}_t^{\text{T}}{{\left[ {{\boldsymbol{S}}_t^w} \right]}^{ - 1}}} ，\\ & {{m_t} = m_t^z + {\boldsymbol{K}}_t^w\left[ {{{\boldsymbol{w}}_t} - {{\boldsymbol{E}}_t}m_t^z - {f_t}} \right]} ，\\ &{{{\boldsymbol{P}}_t} = {\boldsymbol{P}}_t^z - {\boldsymbol{K}}_t^w{\boldsymbol{S}}_t^w{{\left[ {{\boldsymbol{K}}_t^w} \right]}^{\text{T}}}} 。\end{aligned}} \right. $

(23)

式中：${\boldsymbol{S}}_t^y,{\boldsymbol{S}}_t^z,{\boldsymbol{S}}_t^w$为新息协方差；${\boldsymbol{K}}_t^y,{\boldsymbol{K}}_t^z,{\boldsymbol{K}}_t^w$为增益矩阵；$m_t^y,m_t^z$和${m_t}$为观测向量在$t$时刻的均值，对每一个新的观测向量，采用前一向量的均值按式更新；${\boldsymbol{P}}_t^y,{\boldsymbol{P}}_t^z$和${{\boldsymbol{P}}_t}$是对应的协方差。最后，平滑步骤为：

$\left\{ {\begin{aligned} &{{{\boldsymbol{G}}_t} = {{\boldsymbol{P}}_t}{\boldsymbol{A}}_{t + 1}^{\text{T}}{{\left[ {{\boldsymbol{P}}_{t + 1}^ - } \right]}^{ - 1}}} ，\\ &{m_t^s = {{\boldsymbol{P}}_t} + {{\boldsymbol{G}}_t}\left[ {m_{t + 1}^s - m_{t + 1}^ - } \right]} ，\\ &{{\boldsymbol{P}}_t^s = {{\boldsymbol{P}}_t} + {{\boldsymbol{G}}_t}\left[ {{\boldsymbol{P}}_{t + 1}^s - {\boldsymbol{P}}_{t + 1}^ - } \right]{\boldsymbol{G}}_t^{\text{T}}} 。\end{aligned}} \right.$

(24)

式中：$T$时刻$m_T^s = {m_T},{\boldsymbol{P}}_T^s = {{\boldsymbol{P}}_T}$。因此，ADMM分离出的状态向量子问题更新结果如下：

$ \boldsymbol{x}_{1:T}^{\left(k+1\right)}=m_{1:T}^s。$。

(25)

对于ADMM分离出的其他子问题按照式(15)迭代求解，经过ADMM分离的子问题结构按照例如最小二乘法等常规解法通常面临着计算复杂度较高，迭代时间较长等问题。贝叶斯平滑能够将约束函数等条件转换为新的观测向量，按照平滑器步骤计算，能够有效提高计算效率，对我们最关心的状态${{\boldsymbol{x}}_{1:T}}$求得最优解。

2.4 小　结

在无人艇路径跟踪问题中，BS-ADMM为解决复杂约束优化问题提供了一种高效的计算方法。通过引入等式和不等式约束，ADMM将全局优化问题分解为多个易于求解的局部子问题，显著降低了计算复杂度。为进一步提升求解效率，本文提出了BS方法，通过将状态空间分解为时间序列并引入伪测量信息，优化了子问题的求解过程。BS-ADMM不仅能够有效处理无人艇路径跟踪中的动态约束和状态估计问题，还在保证计算精度的同时大幅提升了算法的实时性和鲁棒性，为复杂水域环境下的无人艇路径跟踪提供了可靠的技术支持。为验证该方法的稳定性，本文还提出了理论1，对其稳定性进行了深入分析。本文算法总结如表2所示。

理论[BS-ADMM稳定性]：当$ {Q_t} $和$ {P_1} $是正定的，目标函数$\theta \left( {{{\boldsymbol{x}}_{1:T}}} \right)$是凸函数。由算法BS-ADMM产生的迭代序列$ \{ {x^{\left( k \right)}},{n^{\left( k \right)}},{\eta ^{\left( k \right)}},{\zeta ^{\left( k \right)}}\} $存在唯一的稳定点$ ({x^{\left( * \right)}},{n^{\left( * \right)}},{\eta ^{\left( * \right)}},{\zeta ^{\left( * \right)}}) $。

证明：根据无人艇状态空间可知，$ {Q_t} $和$ {P_1} $为正定矩阵，因此，目标函数$\theta \left( {{{\boldsymbol{x}}_{1:T}}} \right)$为凸函数。由算法BS-ADMM产生的迭代序列$ \{ {x^{\left( k \right)}},{n^{\left( k \right)}},{\eta ^{\left( k \right)}},{\zeta ^{\left( k \right)}}\} $是在$ k $处单调不可增且有界的。根据凸函数性质^[16,17]，则可推出序列必定收敛到唯一的稳定点$ ({x^{\left( * \right)}},{n^{\left( * \right)}},{\eta ^{\left( * \right)}},{\zeta ^{\left( * \right)}}) $。

3 仿真试验结果

本试验中，无人艇模型的水动力导数参考Fossen工具包中现有的模型^[18]，在状态空间模型中，m₁₁=25.8，m₂₂=33.8，m₃₃=0.7225，d₂₂=0.8612，d₃₃=1.9，采样时间${T_z} = 0.1$。仿真试验旨在评估所提出的贝叶斯平滑-交替方向乘子法路径跟踪性能。通过仿真将该方法与无约束方法、鲁棒卡尔曼-布什滤波（Constrained Kalman-Bucy Smoother, CKBS）方法^[19]进行了对比分析。仿真平台基于Matlab实现，采用了一个包含三自由度无人艇运动的位置矢量和速度矢量的六维轨迹模型。给定基准轨迹如下：

$ {x_t} = {\left( {\begin{array}{*{20}{c}} t&{1.3 - \sin t}&1&1&{ - \cos t}&0 \end{array}} \right)^{\text{T}}} 。$

(26)

转移函数的协方差矩阵设置如下：

$ {{\boldsymbol{Q}}_t} = \left[ {\begin{array}{*{20}{c}} {\Delta {t^3}} & 0 & 0 & {{{\Delta {t^2}} \mathord{\left/ {\vphantom {{\Delta {t^2}} 2}} \right. } 2}} & 0 & 0 \\ 0 & {{{\Delta {t^3}} \mathord{\left/ {\vphantom {{\Delta {t^3}} 3}} \right. } 3}} & 0 & 0 & {{{\Delta {t^2}} \mathord{\left/ {\vphantom {{\Delta {t^2}} 2}} \right. } 2}} & 0 \\ 0 & 0 & {{{\Delta {t^3}} \mathord{\left/ {\vphantom {{\Delta {t^3}} 3}} \right. } 3}} & 0 & 0 & {{{\Delta {t^2}} \mathord{\left/ {\vphantom {{\Delta {t^2}} 2}} \right. } 2}} \\ {{{\Delta {t^2}} \mathord{\left/ {\vphantom {{\Delta {t^2}} 2}} \right. } 2}} & 0 & 0 & {\Delta t} & 0 & 0 \\ {{{\Delta {t^2}} \mathord{\left/ {\vphantom {{\Delta {t^2}} 2}} \right. } 2}} & 0 & 0 & {\Delta t} & 0 & 0 \\ 0 & 0 & {{{\Delta {t^2}} \mathord{\left/ {\vphantom {{\Delta {t^2}} 2}} \right. } 2}} & 0 & 0 & {\Delta t} \end{array}} \right] 。$

(27)

式中：$T = 100$，$\Delta t = {{2{\text{π}} } \mathord{\left/ {\vphantom {{2{\text{π}} } T}} \right. } T}$。观测函数的协方差矩阵设置如下：

$ {{\boldsymbol{R}}_t} = \left[ {\begin{array}{*{20}{c}} {{\tau ^2}}&0 \\ 0&{{\tau ^2}} \end{array}} \right] 。$

(28)

式中：$\tau = 0.25$，约束条件是$c{}_t({x_t}) = 1.25 - \sin {x_{1,t}} - {x_{2,t}} \leqslant 0$。惩罚参数$\rho $设置为1，最大迭代次数设置为100。

路径跟踪的效果如图2所示，图中展示了二维平面内的轨迹跟踪效果，以及纵荡和横荡方向上的跟踪效果对比。从图中可以看出，基于约束的BS-ADMM相比无约束条件下的方法，能够更有效地跟踪实际轨迹。并且BS-ADMM方法在跟踪精度上显著优于传统的约束卡尔曼-布什滤波（CKBS）方法，展现了其在复杂路径跟踪问题中的优越性能。

邻近正则化子问题方法（Proximal Regularized Subproblem，PRS）和分裂Bregman方法（Splitting Bregman Method，SBM）是2种变量分离方法，其中PRS通过引入邻近正则化项改进收敛性能，适用于非线性优化问题；而SBM利用Bregman距离分解变量，提高求解稀疏和分段线性问题的效率，两者都被用于路径跟踪问题研究中^[19]。本文将提出的BS-ADMM与其他变量分割方法PRS、SBM结合方法的代价函数值与迭代速度的关系如图3所示。可以发现，基于BS-ADMM的方法迭代次数少，迭代速度快，相比其他的滤波方法和变量分离法，迭代速度平均约提高了65%。能够有效解约束状态估计问题，大大降低了估计的时间成本。

每个迭代步骤的估计误差如图4所示，相比其他的组合方法（BS-PRS和BS-SBM），基于BS-ADMM的路径跟踪方法能够更快的跟踪到真实路径，且保持稳定估计的误差更小，对比不同的方法误差平均约下降了15%。

4 结　语

本文提出了一种基于贝叶斯平滑-交替方向乘子法（BS-ADMM）的无人艇路径跟踪方法，核心在于基于ADMM框架对约束估计问题进行分解，得到若干子问题后利用增广贝叶斯平滑器迭代求解。该方法将船舶运动的三自由度运动学和动力学模型相结合，建立了无人艇平面运动的状态空间方程。引入非线性约束来模拟环境限制，有效地模拟了海洋环境和测量噪声的影响。为了解决环境干扰和目标遮挡对传感器数据造成的数据误差，采用了非完备测量来有效地解决这些问题。

通过仿真试验验证了本文方法相较于传统的优化算法，具有更高的路径跟踪精度和更快的收敛效率。通过试验分析，相较于传统的滤波方法和其他变量分离法，迭代速度提高约65%，跟踪误差减小约15%。