0 引　言

港口船舶的跟踪与状态估计在船舶安全、交通管理与智能航行等方面发挥着重要作用，特别是在复杂的港口环境中，精确的船舶位置和运动轨迹估计是实现高效导航的关键。

传统的卡尔曼滤波方法在非线性系统中跟踪性能有限，Xu等^[1]和Arulampalam等^[2]分别基于粒子滤波（Particle Filter, PF）提出了约束优化方法和船舶跟踪的PF约束算法，有效解决非线性限制问题。薛锋等^[3]将地域和机动性设置为约束条件，对粒子的分布和权重进行调整。但实际船舶运动受航道限制、碰撞避让、港口区域等动态约束影响，此类约束难以有效融合至PF框架。

为进一步提高重采样效率，Gong等^[4]通过引入辅助粒子滤波（Auxiliary Particle Filter, APF）提出了自适应APF，提高了估计精度。在约束处理方面，Hong等^[5]提出正则化软测量约束更新方法，Zhang等^[6]提出约束多模型APF，但均未充分考虑船舶运动的动态约束，且现有方法的固定权重策略难以适配动态约束环境，为此，Yu等^[7]提出需针对不同运动状态动态调整约束优先级。随着机器学习的发展，Mücke^[8]将深度学习应用于粒子滤波中，提高了采样效率。Tang等^[9]利用长短期记忆网络优化船舶轨迹预测，改善了预测精度。Ma等^[10]将随机森林应用于船舶运动预测。但此类方法缺乏有效的约束机制，实时性相对欠佳。

针对上述问题，本文提出一种动态多约束条件的辅助粒子滤波（Dynamic Multi-Constraint Auxiliary Particle Filter，DMCAPF）。

1 考虑动态约束的船舶运动状态 1.1 含约束的非线性随机动态系统

假定非线性随机动态系统的运动状态方程为^[11]：

$ {\boldsymbol{x}}_{k}={\boldsymbol{f}}_{k-1}\left({\boldsymbol{x}}_{k-1},{\boldsymbol{u}}_{k-1}\right)+{\boldsymbol{\xi }}_{k-1}。$

(1)

对应的量测方程为：

$ {\boldsymbol{z}}_{k}={\boldsymbol{h}}_{k}\left({\boldsymbol{x}}_{k}\right)+{\boldsymbol{\eta }}_{k}。$

(2)

式中：$ {\boldsymbol{x}}_{k}={\left[{x}_{1,k},\ldots,{x}_{n,k}\right]}^{\text{T}}\in {\mathrm{\Re }}^{n} $为状态向量；$ {\boldsymbol{u}}_{k-1}= {\left[{u}_{1,k-1},\ldots,{u}_{m,k-1}\right]}^{\text{T}}\in {\mathrm{\Re }}^{m} $为控制输入变量；$ {\boldsymbol{z}}_{k}= {\left[{z}_{1,k},\ldots,{z}_{l,k}\right]}^{\text{T}}\in {\mathrm{\Re }}^{l} $为观测变量；k为时间步长；$ {\boldsymbol{f}}_{k-1}\colon {\mathrm{\Re }}^{n}\times {\mathrm{\Re }}^{m}\rightarrow {\mathrm{\Re }}^{n} $为$ {\boldsymbol{x}}_{k} $和$ \boldsymbol{u}{}_{k} $非线性函数；$ {\boldsymbol{h}}_{k}\colon {\mathrm{\Re }}^{n}\rightarrow {\mathrm{\Re }}^{l} $为$ {\boldsymbol{x}}_{k} $的非线性函数；过程噪声$ {\boldsymbol{\xi }}_{k}\in {\mathrm{\Re }}^{n} $和测量噪声$ {\boldsymbol{\eta }}_{k}\in {\mathrm{\Re }}^{l} $为均值0、统计独立的随机变量，$ {p}_{\xi }\left({\boldsymbol{\xi }}_{k}\right) $和$ {p}_{\eta }\left({\boldsymbol{\eta }}_{k}\right) $分别为概率密度函数，且二阶偏导连续。

实际上，系统状态还需满足下列广义约束条件集合：

$ C=\left\{{c}_{m}\colon \chi \rightarrow \left\{0,1\right\},m=1,2,\cdots ,M\right\}。$

(3)

式中：每个约束$ {c}_{m} $定义为状态空间的可行域。

将上述约束条件纳入状态向量$ {\boldsymbol{x}}_{k} $中：

$ {\boldsymbol{x}}_{k}\in \mathcal{F}\left({\boldsymbol{x}}_{k}\right)=\overset{M}{\underset{m=1}{\cap }}\left\{x|{c}_{m}\left(x\right)=1\right\}。$

(4)

式中：$ {c}_{m}\left(x\right) $为第$ m $个约束函数，$ \left\{x|{c}_{m}\left(x\right)=1\right\} $表示满足约束的所有状态集合；$ \overset{M}{\underset{m=1}{\cap }} $为约束集合的交集；$ \mathcal{F}\left({\boldsymbol{x}}_{k}\right) $为最终的可行域。

于是，含约束非线性随机动态系统可表示为：

$ \begin{cases} {\boldsymbol{x}}_{k}={\boldsymbol{f}}_{k-1}\left({\boldsymbol{x}}_{k-1},{\boldsymbol{u}}_{k-1}\right)+{\boldsymbol{\xi }}_{k-1}，\\ {\boldsymbol{z}}_{k}={\boldsymbol{h}}_{k}\left({\boldsymbol{x}}_{k}\right)+{\boldsymbol{\eta }}_{k}，\\ {\boldsymbol{x}}_{k}\in \mathcal{F}\left({\boldsymbol{x}}_{k}\right)=\overset{M}{\underset{m=1}{\cap }}\left\{x\left| {c}_{m}\left(x\right)=1\right.\right\}。\end{cases} $

(5)

1.2 约束条件纳入贝叶斯框架

现有贝叶斯滤波中常利用传感器观测$ {\boldsymbol{z}}_{k} $更新状态估计。由于系统的物理约束包含状态的重要信息，本文将约束视为伪测量值，一并纳入到贝叶斯框架。

对于第$ m $个约束$ {c}_{m} $，定义约束满足：

$ C_{m}^{k}=\left\{{c}_{m}\left({\boldsymbol{x}}_{k}\right)=1\right\}。$

(6)

即在$ k $时刻，状态$ {\boldsymbol{x}}_{k} $满足约束$ {c}_{m} $。为保证数值稳定性和连续性，采用软约束：

$ p\left({c}_{m}=1\left| {\boldsymbol{x}}_{k}\right.\right)=p_{c}^{m}\left({\boldsymbol{x}}_{k}\right)=\exp \left(-{\lambda }_{m}{\phi }_{m}\left({d}_{m}\left({\boldsymbol{x}}_{k}\right)\right)\right)。$

(7)

式中：$ {d}_{m}\left({\boldsymbol{x}}_{k}\right) $为状态$ {\boldsymbol{x}}_{k} $对约束$ {c}_{m} $的违反度量；$ {\lambda }_{m} \gt 0 $为第$ m $个约束的敏感度参数；$ {\phi }_{m} $为单调递增的惩罚函数，满足$ {\phi }_{m}(0)=0 $。

考虑到不同约束的重要性不同，引入约束权重机制$ {\omega }_{m} $，将所有约束信息通过加权几何平均得到伪测量值：

$ {{\boldsymbol{\tilde{z}}}}_{k}=\psi \left({\boldsymbol{x}}_{k},C\right)=\prod\limits_{m=1}^{M}{\left[p_{c}^{m}\left({\boldsymbol{x}}_{k}\right)\right]}^{{{\omega }_{m}}}。$

(8)

式中：$ {{\boldsymbol{\tilde{z}}}}_{k} $为所有约束信息的伪测量值；$ {\omega }_{m}\in \left[0,1\right] $为第m个约束信息权重。

再将伪测量视为额外观测信息，扩展后验分布为：

$ p\left({\boldsymbol{x}}_{k}\left| {\boldsymbol{z}}_{1\colon k},\mathcal{C}\right.\right)=p\left({\boldsymbol{x}}_{k}\left| {\boldsymbol{z}}_{1\colon k},{\mathbf{\tilde{\boldsymbol{z}}}}_{k}\right.\right) 。$

(9)

根据贝叶斯定理，有：

$ \begin{split}&p\left({\boldsymbol{x}}_{k}\left| {\boldsymbol{z}}_{1\colon k},{{{\boldsymbol{z}}}}_{k}\right.\right)\propto p\left({\boldsymbol{z}}_{k}\left| {\boldsymbol{x}}_{k}\right.\right)\\&\qquad p\left({{{\boldsymbol{z}}}}_{k}\left| {\boldsymbol{x}}_{k}\right.\right)p\left({\boldsymbol{x}}_{k}\left| {\boldsymbol{z}}_{1\colon k-1},{f{{\boldsymbol{z}}}}_{1\colon k-1}\right.\right)。\end{split} $

(10)

$ p\left({\boldsymbol{x}}_{k}\left| {\boldsymbol{z}}_{1\colon k},{\mathbf{\tilde{\boldsymbol{z}}}}_{k}\right.\right)\propto {\mathbf{\tilde{\boldsymbol{z}}}}_{k}p\left({\boldsymbol{z}}_{k}\left| {\boldsymbol{x}}_{k}\right.\right)p\left({\boldsymbol{x}}_{k}\left| {\boldsymbol{z}}_{1\colon k-1},{\mathbf{\tilde{\boldsymbol{z}}}}_{1\colon k-1}\right.\right)。$

(11)

可以看出，式(11)为含约束信息的贝叶斯框架。

1.3 港口船舶运动状态的系统模型

假定船舶在港口区域运动的状态向量为：

$ {\boldsymbol{x}}_{k}={\left[{x}_{k},{y}_{k},{v}_{k},{\theta }_{k}\right]}^{\text{T}}。$

(12)

式中：$ \left({x}_{k},{y}_{k}\right) $为k时刻船舶位置；$ {v}_{k} $为k时刻船舶速度；$ {\theta }_{k} $为k时刻船舶航向角。采用恒速转向模型描述船舶运动：

${\begin{split} &\boldsymbol{x}_k = \boldsymbol{f}_{k-1}\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k-1}\right) + \boldsymbol{\xi}_{k-1} =\\ & \left[\begin{aligned} &x_{k-1} + \left(2v_{k-1}/\omega\right)\sin\left(\omega T/2\right)\cos\left(\theta_{k-1} + \omega T/2\right) \\ &y_{k-1} + \left(2v_{k-1}/\omega\right)\sin\left(\omega T/2\right)\sin\left(\theta_{k-1} + \omega T/2\right) \\ &v_{k-1} + a_{k-1}T \\ &\theta_{k-1} + \omega T \end{aligned}\right] + \boldsymbol{\xi}_{k-1}。\end{split}}$

(13)

式中：ω为转向率；T为采样间隔；$ {a}_{k-1} $为加速度。

船舶在港口区域航行时，易受最大航速、最大加速度限制、船舶间距离限制^[12]。本文将港口区域的地理边界c₁、船舶最大速度c₂与加速度c₃、船舶之间最小安全距离c₄设置约束条件，并转化为概率形式，如表1所示。其中，(x, y)为船舶的位置，G为港口区域地理边界，$ {d}_{ij} $为船舶$ i $与船舶j之间的欧氏距离。边界约束防止船舶搁浅或进入禁航区；速度约束确保港口交通安全；加速度约束反映船舶机动能力；安全距离约束避免船舶发生碰撞。惩罚函数的选择基于约束的严重程度。二次惩罚d²用于边界约束，线性惩罚d用于速度约束，当船舶距离接近安全距离时，指数惩罚$ ({e}^{d}-1) $则随碰撞风险上升。

利用式(7)将各约束转化为概率形式，则边界约束概率为：

$ p_{c}^{1}\left({\boldsymbol{x}}_{k}\right)=\exp \left(-{\lambda }_{1}{\left[\mathrm{dist}\left(\left(x,y\right),G\right)\right]}^{2}\right)。$

(14)

式中：$ \mathrm{dist}\left(\left(x,y\right),G\right) $为计算船舶位置到边界的最短距离。

速度约束概率为：

$ p_{c}^{2}\left({\boldsymbol{x}}_{k}\right)=\exp \left(-{\lambda }_{2}\max \left(0,{v}_{k}-{v}_{\max }\right)\right)。$

(15)

加速度约束概率为：

$ p_{c}^{3}\left({\boldsymbol{x}}_{k}\right)=\exp \left(-{\lambda }_{3}\max \left(0,\left|\left|{\mathrm{a}}_{k}\right|\right|-{a}_{\max }\right)\right)。$

(16)

式中：加速度由相邻时刻速度差分估计定义$ {a}_{k}= \left({v}_{k}-{v}_{k-1}\right)/T $。

安全距离约束概率为：

$ p_{c}^{4}\left({\boldsymbol{x}}_{k}\right)=\exp \left(-{\lambda }_{4}\left({e}^{\max \left(0,{d}_{\min }-{d}_{\mathrm{i}j}\right)}-1\right)\right)。$

(17)

根据约束条件优先级，设定约束权重向量：

$ {\mathbf{\omega }}_{m}={\left[{\omega }_{1},{\omega }_{2},{\omega }_{3},{\omega }_{4}\right]}^{\text{T}} $

(18)

式中：ω₁为边界约束权重；ω₂为速度约束权重；ω₃为加速度约束权重；ω₄为安全距离约束。

将上述约束信息代入式(8)，得到伪测量值：

$ {\mathbf{\tilde{\boldsymbol{z}}}}_{k}=\prod\limits_{m=1}^{4}{\left[p_{c}^{m}\left({\boldsymbol{x}}_{k}\right)\right]}^{{{\omega }_{m}}}。$

(19)

2 含动态多约束的辅助粒子滤波

本节将约束信息融入APF，提出一种DMCAPF，将设定约束条件视为伪量测值，达到与观测信息融合。

2.1 APF过程

假定非线性随机动态系统在时刻$ k $的后验分布可由贝叶斯公式表示为：

$ {p\left({\boldsymbol{x}}_{k}\left| {\boldsymbol{z}}_{1\colon k}\right.\right)=p\left({\boldsymbol{z}}_{k}\left| {\boldsymbol{x}}_{k}\right.\right)p\left({\boldsymbol{x}}_{k}\left| {\boldsymbol{z}}_{1\colon k-1}\right.\right)/p\left({\boldsymbol{z}}_{k}\left| {\boldsymbol{z}}_{1\colon k-1}\right.\right)。} $

(20)

重要性权重递归更新为：

$ {\omega _{k}^{i}=p\left({\boldsymbol{z}}_{k}\left| \boldsymbol{x}_{k}^{i}\right.\right)p\left(\boldsymbol{x}_{k}^{i}\left| \boldsymbol{x}_{k-1}^{i}\right.\right)\omega _{k-1}^{i}/\text{π} \left(\boldsymbol{x}_{k}^{i}\left| \boldsymbol{x}_{0\colon k-1}^{i},{\boldsymbol{z}}_{1\colon k}\right.\right)。} $

(21)

式中：$ \text{π} \left({\boldsymbol{x}}^{i}{}_{k}\left| {\boldsymbol{x}}^{i}{}_{0\colon k},{\boldsymbol{z}}_{1\colon k}\right.\right)=p\left({\boldsymbol{x}}^{i}{}_{k}\left| {\boldsymbol{x}}^{i}{}_{k-1}\right.\right) $为重要性分布函数。于是，式(21)可简化为$ \omega _{k}^{i}=\omega _{k-1}^{i}p\left({\boldsymbol{z}}_{k}\left| \boldsymbol{x}_{k}^{i}\right.\right) $，但未考虑输出测量，会出现粒子退化。APF在此基础上，通过两阶段采样策略提升采样效率。

在预测阶段，计算粒子选择概率$ \mu _{k}^{(i)}\propto p\left({\boldsymbol{z}}_{k}\left| {\hat{\boldsymbol{x}}}_{k}^{i}\right.\right) $，$ {\hat{\boldsymbol{x}}}_{k}^{i} $为条件期望，即：

$ {\hat{\boldsymbol{x}}}_{k}^{i}=E\left[{\boldsymbol{x}}_{k}\left| {\boldsymbol{x}}_{k-1}\right.\right]=\int{\boldsymbol{x}}_{k}p\left({\boldsymbol{x}}_{k}\left| \boldsymbol{x}_{k-1}^{i}\right.\right)\text{d}{\boldsymbol{x}}_{k}。$

(22)

在采样阶段，由选择概率进行重采样：

$ \omega _{k}^{i}=p\left({\boldsymbol{z}}_{k}\left| \boldsymbol{x}_{k}^{i}\right.\right)/p\left({\boldsymbol{z}}_{k}\left| \mathbf{\hat{\boldsymbol{x}}}_{k}^{i}\right.\right)。$

(23)

可以看出，APF的优势是重采样机制仅让高概率粒子进入下一步，但因忽略物理约束会产生不可靠粒子，且部分边缘粒子会因越界被淘汰。

2.2 动态多约束条件纳入APF

粒子违反约束则伪测量值降低、重采样保留概率低；反之，满足约束的粒子权重高。假设真实观测$ {\boldsymbol{z}}_{k} $与伪测量$ {{\tilde{\boldsymbol{z}}}}_{k} $在给定状态$ {\boldsymbol{x}}_{k} $时条件独立：

$ p\left({\boldsymbol{z}}_{k},{\boldsymbol{\tilde{\boldsymbol{z}}}}_{k}\left| {\boldsymbol{x}}_{k}\right.\right)=p\left({\boldsymbol{z}}_{k}\left| {\boldsymbol{x}}_{k}\right.\right)p\left({\mathbf{\tilde{\boldsymbol{z}}}}_{k}\left| {\boldsymbol{x}}_{k}\right.\right)。$

(24)

引入$ {{\tilde{\boldsymbol{z}}}}_{k} $后，权重更新需同时考虑真实观测与约束条件。定义扩展重要性分布函数为$ {\text{π}} \left({\boldsymbol{x}}_{k}\left| {\boldsymbol{x}}_{k-1},{\boldsymbol{z}}_{k},{\mathbf{\tilde{\boldsymbol{z}}}}_{k}\right.\right) $，式(21)修正为：

$ \omega _{k}^{i}\propto p\left(\boldsymbol{x}_{k}^{i}\left| {\boldsymbol{z}}_{1\colon k},{{\tilde{\boldsymbol{z}}}}_{1\colon k}\right.\right)/{\text{π}} \left(\boldsymbol{x}_{k}^{i}\left| \boldsymbol{x}_{k-1}^{i},{\boldsymbol{z}}_{k},{{\tilde{\boldsymbol{z}}}}_{k}\right.\right)。$

(25)

将后验分布展开为递推形式：

$ \begin{split} &p\left({\boldsymbol{x}}_{k}\left| {\boldsymbol{z}}_{1\colon k},{\mathbf{\tilde{\boldsymbol{z}}}}_{1\colon k}\right.\right)=\frac{p\left({\boldsymbol{z}}_{k},{\mathbf{\tilde{\boldsymbol{z}}}}_{k}\left| {\boldsymbol{x}}_{k}\right.\right)p\left({\boldsymbol{x}}_{k}\left| {\boldsymbol{z}}_{1\colon k-1},{\mathbf{\tilde{\boldsymbol{z}}}}_{1\colon k-1}\right.\right)}{p\left({\boldsymbol{z}}_{k},{\mathbf{\tilde{\boldsymbol{z}}}}_{k}\left| {\boldsymbol{z}}_{1\colon k-1},{\mathbf{\tilde{\boldsymbol{z}}}}_{1\colon k-1}\right.\right)} =\\ &\qquad\frac{p\left({\boldsymbol{z}}_{k}\left| {\boldsymbol{x}}_{k}\right.\right)p\left({\mathbf{\tilde{\boldsymbol{z}}}}_{k}\left| {\boldsymbol{x}}_{k}\right.\right)p\left({\boldsymbol{x}}_{k}\left| {\boldsymbol{z}}_{1\colon k-1},{\mathbf{\tilde{\boldsymbol{z}}}}_{1\colon k-1}\right.\right)}{p\left({\boldsymbol{z}}_{k},{\mathbf{\tilde{\boldsymbol{z}}}}_{k}\left| {\boldsymbol{z}}_{1\colon k}{}_{-1},{\mathbf{\tilde{\boldsymbol{z}}}}_{1\colon k-1}\right.\right)} ，\\[-1pt]\end{split} $

(26)

$ \begin{split}&p\left({\boldsymbol{x}}_{k}\left| {\boldsymbol{z}}_{1\colon k-1},{\mathbf{\tilde{\boldsymbol{z}}}}_{1\colon k-1}\right.\right)=\int p\left({\boldsymbol{x}}_{k}\left| {\boldsymbol{x}}_{k-1}\right.\right)\\&\qquad p\left({\boldsymbol{x}}_{k-1}\left| {\boldsymbol{z}}_{1\colon k-1},{\mathbf{\tilde{\boldsymbol{z}}}}_{1\colon k-1}\right.\right)\text{d}{\boldsymbol{x}}_{k-1}。\end{split}$

(27)

用粒子集近似预测分布表示为：

$ p\left({\boldsymbol{x}}_{k}\text{}\left| {\boldsymbol{z}}_{1\colon k-1}\text{},{\mathbf{\tilde{\boldsymbol{z}}}}_{1\colon k-1\text{}}\right.\right)\approx \sum\limits_{j=1}^{{N}_{s}}\omega _{k-1}^{j}\text{}p\left({\boldsymbol{x}}_{k}\text{}\left| {\boldsymbol{x}}^{j}{}_{k-1}\right.\text{}\right)。$

(28)

式中：$ {N}_{s} $为粒子总数。

综合式(25)~式(28)，带约束的权重更新公式：

$ \begin{split} &\omega _{k}^{i}=\frac{p\left({\boldsymbol{z}}_{k}\left| \boldsymbol{x}_{k}^{i}\right.\right)p\left({\mathbf{\tilde{\boldsymbol{z}}}}_{k}\left| \boldsymbol{x}_{k}^{i}\right.\right)p\left(\boldsymbol{x}_{k}^{i}\left| \boldsymbol{x}_{k-1}^{i}\right.\right)}{{\text{π}} \left(\boldsymbol{x}_{k}^{i}\left| \boldsymbol{x}_{k-1}^{i},{\boldsymbol{z}}_{k},{{\tilde{\boldsymbol{z}}}}_{k}\right.\right)}\omega _{k-1}^{i}=\\&\qquad\frac{p\left({\boldsymbol{z}}_{k}\left| \boldsymbol{x}_{k}^{i}\right.\right){\prod\limits_{m=1}^{M}\left[p_{c}^{m}\left(\boldsymbol{x}_{k}^{i}\right)\right]}^{{{\omega }_{m}}}p\left(\boldsymbol{x}_{k}^{i}\left| \boldsymbol{x}_{k-1}^{i}\right.\right)}{{\text{π}} \left(\boldsymbol{x}_{k}^{i}\left| \boldsymbol{x}_{k-1}^{i},{\boldsymbol{z}}_{k},{{\tilde{\boldsymbol{z}}}}_{k}\right.\right)}\omega _{k-1}^{i}。\end{split} $

(29)

可以看出，当粒子满足全部约束时，$ p_{c}^{m}\left(x_{k}^{i}\right)\approx 1 $，权重主要由观测似然决定；当粒子违反约束时$ p_{c}^{m}\left(\boldsymbol{x}_{k}^{i}\right) \lt 1 $，则该项对权重产生惩罚，降低该粒子被保留概率。

随后，将约束信息融入APF两阶段采样，即约束粒子的选择、权重修正。

第1阶段：修正选择概率，确保符合观测且满足约束的粒子具有较高概率：

$ \mu _{k}^{(i)}\propto \omega _{k-1}^{i}p\left({\boldsymbol{z}}_{k}\left| {\hat{\boldsymbol{x}}}_{k}^{i}\right.\right)\prod\limits_{m=1}^{M}{\left[p_{c}^{m}\left(\mathbf{\hat{\boldsymbol{x}}}_{k}^{i}\right)\right]}^{{{\omega }_{m}}}。$

(30)

第2阶段：对选中的粒子$ \boldsymbol{x}_{k}^{i} $，在重要性分布函数采样后修正权重为：

$ \omega _{k}^{i}=\frac{p\left({\boldsymbol{z}}_{k}\left| \boldsymbol{x}_{k}^{i}\right.\right)\prod\limits_{m=1}^{M}{\left[p_{c}^{m}\left(\boldsymbol{x}_{k}^{i}\right)\right]}^{{{\omega }_{m}}}}{p\left({\boldsymbol{z}}_{k}\left| {\hat{\boldsymbol{x}}}_{k}^{i}\right.\right)\prod\limits_{m=1}^{M}{\left[p_{c}^{m}\left({\hat{\boldsymbol{x}}}_{k}^{i}\right)\right]}^{{{\omega }_{m}}}} 。$

(31)

式中，分子项为粒子实际位置的似然和约束概率，分母项则为预测中心值。

采用状态转移先验作为扩展重要性分布，综合式(29)～式(31)，权重更新可简化为：

$ \omega _{k}^{i}=p\left({\boldsymbol{z}}_{k}\left| \boldsymbol{x}_{k}^{i}\right.\right)\prod\limits_{m=1}^{M}{\left[p_{c}^{m}\left(\boldsymbol{x}_{k}^{i}\right)\right]}^{{{\omega }_{m}}}\omega _{k-1}^{i}。$

(32)

最终，将上述4类约束信息代入式(32)，可得DMCAPF的粒子权重更新：

$ \omega _{k}^{i}=p\left({\boldsymbol{z}}_{k}\left| \boldsymbol{x}_{k}^{i}\right.\right)\prod\limits_{m=1}^{4}{\left[p_{c}^{m}\left({\boldsymbol{x}}_{k}\right)\right]}^{{{\omega }_{m}}}\omega _{k-1}^{i} 。$

(33)

2.3 约束权重的双层自适应机制

针对APF融入动态约束条件时存在约束权重分配问题，固定权重策略无法适应不同场景需求：船舶接近边界时需强化边界约束，而在开阔水域则应侧重安全距离约束。这种差异源于各约束的敏感特性不同。为此，本文提出双层自适应约束权重框架：基础层利用敏感特性函数反映约束固有特性；修正层通过随机森林模型融合环境动态信息，实现约束权重动态调整。

首先构建第一层基础权重层，引入敏感特性函数$ {\psi }_{i}\left(b\right) $，定义归一化的违反度量为：

$ {b}_{i,k}={d}_{i}\left({\boldsymbol{x}}_{k}\right)/{d}_{i,\max }\in \left[0,1\right] 。$

(34)

式中：$ {d}_{i,\max } $为第$ i $个约束的最大可接受违反值。

根据约束的物理特性，定义3类敏感函数：

1）近距离敏感型。用于边界约束和安全距离约束：

$ {\psi }_{i}\left(b\right)=\exp \left({\mu }_{i}b\right)。$

(35)

式中：$ {\mu }_{i} \gt 0 $为衰减速率控制参数。

2）高敏感型。用于速度约束和加速度约束：

$ {\psi }_{i}\left(b\right)=\exp \left({\mu }_{i}\left(b-1\right)\right)。$

(36)

3）均匀敏感型。适用于不确定优先级的约束：

$ {\psi }_{i}\left(b\right)=1 。$

(37)

基于上述函数，定义第i个约束在k时刻的自适应权重：

$ {\omega }_{i,k}={\psi }_{i}\left({b}_{i,k}\right)/\sum\limits_{j=1}^{m}{\psi }_{j}\left({b}_{j,k}\right)。$

(38)

式中：m为约束信息总数，满足$ \sum\limits_{i=1}^{m}{\omega }_{i,k}=1 $。针对港口船舶系统，边界约束和安全距离约束参数设定$ {\mu }_{1}={\mu }_{4}=3 $，速度约束和加速度约束采参数设定$ {\mu }_{2}={\mu }_{3}=2 $。得到第一层基础权重向量为：

$ {\mathbf{\omega }}_{m,k}={\left[{\omega }_{1,k},{\omega }_{2,k},{\omega }_{3,k},{\omega }_{4,k}\right]}^{\text{T}}。$

(39)

式(39)中反映了各约束的内在敏感特性，但参数$ {\mu }_{i} $为固定值，且权重仅依赖当前违反度，未考虑船舶所处的动态环境状态，存在自适应能力不足的局限性。

为解决第一层基础权重的局限性，引入随机森林模型^[13]构建第二层动态修正机制，将环境状态信息融入权重调整过程。

首先，构建特征向量E_k，作为随机森林的输入：

$ {{\boldsymbol{E}}_{k}={\left[{b}_{1,k},{b}_{2,k},{b}_{3,k},{b}_{4,k},{\tilde{b}}_{1,k},{\tilde{b}}_{2,k},{\tilde{b}}_{3,k},{\tilde{b}}_{4,k},{\rho }_{k},\bigtriangleup {v}_{k},\bigtriangleup {\theta }_{k},{\sigma }_{v,k}\right]}^{\text{T}}。}$

(40)

式中：$ {\tilde{b}}_{i,k}=\left({b}_{i,k}-{b}_{i,k-1}\right)/\Delta t $为违反度变化率；$ {\rho }_{k} $为船舶局部密度；$ \Delta {v}_{k} $和$ \Delta {\theta }_{k} $分别为船舶速度和航向角变化幅度；$ {\sigma }_{v,k} $为速度波动标准差。为每个约束构建随机森林模型$ {\mathcal{F}}_{i} $，预测修正因子为：

$ {\gamma }_{i,k}={\mathcal{F}}_{i}\left({{E}}_{k}\right)=\frac{1}{{N}_{\text{tree}}}\sum\limits_{j=1}^{{N}_{\text{tree}}}{T}_{i,j}\left({{E}}_{k}\right)。$

(41)

式中：$ {\gamma }_{i,k}\in \left[0.5,2.0\right] $保证权重的稳定性；$ {N}_{\text{tree}} $为决策树总数；$ {T}_{i,j} $为第j棵决策树。

综上，双层权重自适应框架表示为：

第1层由式(39)计算基础权重$ \omega _{i,k}^{\text{base}} $，反映约束固有特性；第2层利用随机森林模型输出修正因子，并得到最终约束权重：

$ \omega _{i,k}^{\text{final}}=\omega _{i,k}^{\text{base}}{\gamma }_{i,k}/\sum\limits_{j=1}^{4}\omega _{j,k}^{\text{base}}{\gamma }_{i,k}。$

(42)

将式(42)代入式(8)、式(20)、式(31)和式(33)，得出DMCAPF的粒子权重更新：

$ \omega _{k}^{i}=p\left({z}_{k}\left| x_{k}^{i}\right.\right)\prod\limits_{m=1}^{4}{\left[p_{c}^{m}\left(x_{k}^{\mathrm{i}}\right)\right]}^{{\omega _{i,k}^{\text{final},\left(m\right)}}}。$

(43)

最后，生成新的粒子集$ \left\{x_{k}^{i},i=1,2,3,\cdots ,N\right\} $。

3 数值实验与结果分析

数值实验仿真平台为Matlab 2020a，硬件配置为Intel Core i7-12700H（2.7 GHz），32 GB内存，操作系统为Windows 10，仿真时间为时间步长为t=1 s，仿真时间为100 s。为验证DMCAPF在港口区域对船舶跟踪的性能，分析在处理船舶速度、加速度和安全距离等动态约束时的性能。本文利用均方根误差（Root Mean Square Error，RMSE）量化船舶跟踪精度与误差。

$ {{RMS E}}_{k}=\sqrt{\sum\limits_{\tau =1}^{{M}_{c}}\left({\left(x_{k}^{\tau }-{\overline{x}}_{k|k}\right)}^{2}+{\left(y_{k}^{\tau }-{\overline{y}}_{k|k}\right)}^{2}\right)/{M}_{c}}。$

(44)

式中：$ \left({\overline{x}}_{k|k},{\overline{y}}_{k|k}\right) $为重采样后的目标位置；$ {M}_{c} $为蒙特卡洛实验次数；$ \left(x_{k}^{\tau },y_{k}^{\tau }\right) $为k时刻的船舶位置。

假定港口的地理边界长为5 000 m、宽为2 000 m的矩形区域，船舶的运动模型为：

${ {\boldsymbol{x}}_{k+1}=\left[\begin{matrix} 1 & 0 & \Delta t\cos {\theta }_{k} & 0\\ 0 & 1 & \Delta t\sin {\theta }_{k} & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{matrix} \right]{\boldsymbol{x}}_{k}+\left[\begin{matrix} 0 & 0\\ 0 & 0\\ \Delta t & 0\\ 0 & \Delta t \end{matrix} \right]\left[\begin{array}{c} {a}_{k}\\ {\varpi }_{k} \end{array}\right]+{\boldsymbol{\xi }}_{k}。} $

(45)

式中：$ {\boldsymbol{x}}_{k}={\left[{x}_{k},{y}_{k},{v}_{k},{\theta }_{k}\right]}^{\text{T}} $为状态向量；$ {a}_{k} $为船舶加速度；$ {\varpi }_{k} $为船舶角速度；$ {\boldsymbol{\xi }}_{k} $为4维的0均值白噪声，其协方差矩阵$ {\boldsymbol{Q}}_{k} $为单位阵。船舶由距离和方位传感器进行跟踪，量测模型满足：

$ {\boldsymbol{z}}_{k}=\left[\begin{array}{c} \sqrt{x_{k}^{2}+y_{k}^{2}}\\ \arctan \left({y}_{k}/{x}_{k}\right) \end{array}\right]+{\boldsymbol{\eta }}_{k}。$

(46)

式中：$ \sqrt{x_{k}^{2}+y_{k}^{2}} $为量测距离；$ \arctan \left({y}_{k}/{x}_{k}\right) $为方位角；$ {\boldsymbol{\eta }}_{k} $为二维零均值白噪声；$ {\boldsymbol{R}}_{k}=\text{diag}\left(8,0.002\right) $为协方差矩阵。

假定港口区域2艘中型船舶初始速度均为2 kn，船舶1的初始位置为(100 m,1000 m)，船舶2的初始位置为(4900 m,1000 m)，v_max=6 kn，a_max=0.05 m/s²，最小安全距离为80 m。船舶x方向为匀速运动，y方向为曲线运动。船舶1驶入港口区域，船舶2驶出港口区域，2艘船舶为相向运动。约束参数初始化：λ₁=0.5，λ₂=0.2，λ₃=0.1，λ₄=0.6。初始化约束权重：$ {\mathbf{\omega }}_{m}={\left[0.3,0.2,0.2,0.3\right]}^{\text{T}} $。随机森林模型相关参数：$ {\gamma }_{i,k}=1.0 $，$ {T}_{i,j} $=50，最小叶节点为5，初始粒子数量为100个，进行200次蒙特卡洛实验。

图1为双层约束权重自适应机制的有效性。图1(a)显示船舶在不同运动阶段约束优先级存在差异，边界约束和安全距离约束占主导地位。由图1(b)可知边界约束、速度约束和安全距离约束的标准差均超过12%，呈现较强的时变特性。相比之下，加速度约束的标准差与均值较稳定，表明该机制能实时响应船舶的运动变化。

图2为船舶航迹图。结果表明，DMCAPF的预测航迹存在噪声引起的轻微波动，但整体与船舶真实航迹基本符合。

图3为两艘船舶的航迹图，两船航迹在x方向存在交点，但两船在y方向保持了有效距离，验证了DMCAPF中安全距离约束的有效性。

为评估算法在不同船舶数量下的性能，图4为自适应权重机制在不同船舶数量下的箱线图，随着船舶数量从2艘增至50艘，各约束权重分布呈现收敛趋势，箱体高度逐渐减小，异常值数量也减少，表明权重稳定性提高。

图5为DMCAPF在不同船舶数量下的性能。可以看出，当船舶数量为10艘时，RMSE达到最小；当船舶数量为50艘时，RMSE提升到16.1 m，计算时间由0.15 s增至0.42 s。

3.1 对比实验

本节将所提出的DMCAPF与APF^[14]、不敏感粒子滤波（Unscented Particle Filter，UPF）^[15]、约束粒子滤波（Constrained Particle Filter，CPF）^[16]和基于Gibbs的约束粒子滤波CPF（Gibbs）^[17]进行对比，从RMSE、粒子有效性（Particle Effective Sample Size，PESS）、约束满足率（Constraint Satisfaction Rate，CSR）和计算时间效率（Computer Time，CT）四方面验证算法的有效性。

$ {PES S=}1/{N}_{s}\sum\limits_{i=1}^{N}\omega _{i}^{2}。$

(47)

式中：$ {\omega }_{i} $为第i个粒子的权重；N_s为粒子总数。

$ {CS R=}{N}_{\text{sat}}\times 100{{\text{%}} }/N。$

(48)

式中：$ {N}_{\text{sat}} $为满足约束的粒子数。

图6为RMSE对比结果。可知，DMCAPF的平均RMSE为11 m，其他滤波算法的RMSE均在35～50 m，受限于自身算法复杂度，CPF（Gibbs）的误差最大。

图7为粒子有效样本对比结果。可知，DMCAPF的均值稳定在0.8以上，表明粒子的多样性与有效性较高，UPF、CPF、CPF（Gibbs）的PESS值逐渐降低。

图8为CSR与效率之间对比结果。可知，DMCAPF可在保存精度与约束的前提下，计算时间位于的合理区间。UPF、CPF、APF逐渐递减；CPF（Gibbs）因模拟极端情况达到100%。

表2为不同算法的性能对比结果。DMCAPF在各场景下表现出最优性能。当船舶数量为5艘时，RMSE为10.56 m约束满足率达92.77%，相比现有APF分别提升41.7%和96.2%。随着船舶数量增至50艘，DMCAPF仍保持良好性能，计算时间在0.5 s以内，满足港口船舶跟踪需求。

3.2 消融实验

本节对所提出的DMCAPF进行消融实验，融合了随机森林特征权重（Random Forest Characteristics Weight，RFCW）、双层约束机制（Double Constrain，DC）和自适应权重（Adaptive Weight，AP）组件。通过移除单组件对比实验，从RMSE损失率、PFP损失率、CSR损失率和时间节省率4个维度量化各组件的性能影响。此外，通过调整船舶数量、粒子数、蒙特卡洛实验次数，分析不同条件对DMCAPF性能影响。

表3为各组件移除后的性能退化情况，结果表明，去除RFCW模块后，RMSE损失率达38.23%，PESS、CSR和时间节省率分别下降9.33%、14.95%和20%，说明随机森林权重优化对算法各项指标的提升最为关键，尤其是跟踪精度。去除DC组件后，使RMSE损失32.35%，验证了动态多约束条件融入的必要性。去除AP组件后，各项指标退化最小，但RMSE仍损失15.69%，表明3个模块协同作用才能发挥最优性能。

表4为N=100和Mc=200条件下不同船舶数量下的算法性能。可知，当船舶数量从5艘增至50艘时，DMCAPF的RMSE由10.56 m逐渐降至16.19 m，CSR由92.77%降至85.06%，计算时间由0.153 s增至0.443 s。表明多目标场景下，船舶间安全距离约束的复杂性显著增加，但DMCAPF仍保持较高的有效粒子占比和约束满足能力。

表5为不同参数配置下的算法性能。可知，当N=100和M_C=400时，性能达到最优，验证了双层自适应权重框架的有效性。粒子数量过少（N=50）导致粒子贫化，粒子数量过多（N=400）则增加了计算负担且精度有所下降。

综上，DMCAPF的组件均为各项指标优化的关键。其中，RFCW是提升定位精度的核心驱动力，对复杂环境下的特征自适应能力贡献最大；AP对约束满足率影响加大，是保障船舶航行安全的关键。DC在平衡精度与约束机制合理性中具有重要作用。在参数优化实验中，通过不同的参数配置，DMCAPF能满足港口船舶跟踪要求。

4 结　语

本文提出一种用于港口区域船舶跟踪的DMCAPF，将边界、速度、加速度及安全距离约束作为伪测量值纳入贝叶斯框架，同时通过双层自适应约束权重框架和两阶段采样对APF进行优化，可依据环境动态调整约束优先级。在复杂港口环境仿真实验中，将DMCAPF与PF、APF及其他约束滤波对比，结果显示其优势显著：不仅能有效避免粒子违反物理约束、提升采样效率，还在跟踪精度、粒子有效性、约束满足率及计算效率上表现更优，提高了船舶密集区域与边界约束区域的跟踪精度。DMCAPF有效解决APF 处理约束条件的痛点，为港口船舶跟踪提供新思路，具有重要理论意义与实际应用价值。