【论文翻译】A measurement fusion algorithm of sensors angle association for multi-target tracking-第一部分

---

A measurement fusion algorithm of active and passive sensors based on angle association for multi-target tracking——基于角度关联的主动与被动传感器测量融合算法用于多目标跟踪

---

摘要

不同类型传感器之间的多目标跟踪在充分利用各种类型的测量数据方面面临巨大挑战。为此，本文提出了一种基于角度关联（AA）的单个主动传感器与多个被动传感器（SAMPS）测量融合算法，称为SAMPS-AA算法，用于多目标跟踪。首先，为了缩小关联范围，提出了一种有效的筛选算法，用于提取两种传感器的公共角度测量数据。然后，通过基于角度测量的统计方法，开发了一种错误关联组的排除策略。随后，通过基于最小二乘法（LS）的角度关联，获得融合测量数据的坐标。最后，利用主动传感器的测量特性，提出了另一种错误测量点的排除策略。实验结果表明，所提出的SAMPS-AA算法可以充分结合这两类传感器的优势，有效排除尽可能多的错误关联组，显著降低计算复杂度，并显著提高跟踪精度。

---

1. 引言

多目标跟踪是在目标检测之后出现的一种技术，指的是通过传感器收集目标的位置信息、轨迹以及运动特征，其跟踪需求是准确估计目标的数量和运动状态【1–3】。当前，多目标跟踪技术广泛应用于军事和民用领域【4,5】。用于多目标跟踪的传感器有多种类型，按是否能自主发送和接收信号，可分为主动传感器和被动传感器【6】。主动传感器包括主动雷达、超声波传感器【7】等，而被动传感器则包括被动雷达、红外传感器【8】、声呐【9】等。此外，根据传感器获取的测量类型，可将其分为仅测距传感器【10】、仅测角传感器【11】、测距测角传感器等。由于不同传感器获得的测量维度存在差异，因此在不同类型传感器之间存在数据融合的问题。目前，多传感器【12,13】多目标跟踪算法主要考虑相同类型传感器之间的测量关联。

上世纪的多目标跟踪技术主要考虑基于数据关联的算法，如全局最近邻（GNN）【14】、联合概率数据关联（JPDA）【15】、多假设跟踪（MHT）【16】等。这些算法通常只考虑单传感器多目标跟踪，因为数据关联的计算复杂性较高。本世纪初，随着随机有限集（RFS）【17】的提出，基于RFS的多目标跟踪算法被提出，如概率假设密度（PHD）【18】、计数PHD（CPHD）【19】、多目标多伯努利（MeMBer）【20】、计数平衡MeMBer（CBMeMBer）【21】滤波器等。上述基于RFS的算法主要的优势在于避免了数据关联，从而减少了计算复杂性。然而，这些基于RFS的算法只能估计目标的运动状态，而不能估计轨迹。为此，Vo等人提出了基于标记RFS【22】和数据关联的算法，如广义标记多伯努利（GLMB）【23】滤波器、标记多伯努利（LMB）【24】滤波器等。最近，提出了泊松多伯努利混合（PMBM）滤波器【25】来处理具有更高跟踪精度的多目标跟踪问题。然而，由于全球关联历史假设的存在，PMBM滤波器面临着巨大的计算负担。为了进一步提高RFS滤波算法的跟踪性能，我们的早期工作中提出了几种创新算法【26-29】。

对于主动传感器，随着基于RFS的算法的发展，已经提出了几种对应的多传感器多目标跟踪算法。一般来说，为了将单传感器多目标跟踪算法扩展到多传感器多目标跟踪中，需要将多个传感器获得的数据关联起来，然后将多传感器问题转化为单传感器问题，以便于处理。迭代修正PHD（ICPHD）【30】滤波器是一种应用于多传感器的实现，它通过多个单传感器PHD滤波器实现多传感器多目标跟踪，从而避免了数据关联，但其跟踪精度会受到传感器顺序的影响。最近，提出了多传感器联合GLMB（JGLMB）【31】滤波器，以解决基于吉布斯采样的GLMB的多维分配问题，但由于数据关联，其计算复杂性较高。

对于被动传感器，由于它们只能获取目标的角度测量数据，对于非机动目标，通常考虑多被动传感器跟踪，即合理布置每个传感器在空间中的位置，以便从不同方向获得更可靠的目标跟踪【32】。对于多被动传感器获取的角度测量数据，通常采用联合到达角（AOA）定位【33】来定位目标。然而，随着传感器数量和杂波的增加，错误关联结果的数量也急剧增加，导致跟踪精度下降。

上述多传感器多目标跟踪算法主要考虑相同类型传感器或具有相同类型测量的传感器之间的测量关联，而较少考虑不同类型传感器或具有测量差异的传感器之间的关联问题。为此，本研究提出了一种基于角度关联（AA）的有效实现方法，该方法可应用于单个主动传感器与多个被动传感器（SAMPS），称为SAMPS-AA算法。特别强调的是，所提出的SAMPS-AA算法不仅可以实现多源传感器的有效融合，还可以提高跟踪精度并降低计算复杂性。本文的主要贡献总结如下：(1) 通过提取两种传感器共有的角度测量数据，导出了有效的筛选算法，从而缩小了测量关联范围；(2) 提出了一种基于角度测量的关联结果排除策略，通过建立统计模型来实现；(3) 为了获得充足的测量点，角度关联通过最小二乘法计算，从而得到融合测量的坐标；(4) 为了消除错误的测量点，利用主动传感器的独特位置测量，开发了另一种排除策略。因此，通过使用所提出的SAMPS-AA算法，多目标跟踪的精度显著提高，而计算复杂性得到有效降低。

本文的其余部分安排如下。首先，第二节介绍了两种多目标运动模型，以及本文涉及的两种传感器的测量差异和特性。然后，第三节介绍了基于角度关联的初步筛选算法的有效实现。接下来，第四节推导了多源数据融合和排除策略的实现。随后，第六节展示了三种场景下的数值实验。最后，第七节给出了一些结论。

---

2. 背景

首先，第二节1小节介绍了两种经典的多目标运动模型：恒定速度（CV）模型和恒定转弯（CT）模型。此外，第二节2小节介绍了多目标跟踪中常用的两种雷达传感器类型，即主动雷达和被动雷达。这些雷达传感器和运动模型是本文算法建模的基础。

2.1 目标运动模型

在多目标跟踪领域，状态方程和观测方程分别建模为：

$${\mathbf{x}}_k=f({\mathbf{x}}_{k-1})+{\mathbf{w}}_{k-1}\text{\hspace{1em}(1)}$$
$${\mathbf{z}}_k=h({\mathbf{x}}_k)+{\mathbf{v}}_k\text{\hspace{1em}(2)}$$

其中，${\mathbf{x}}_k$ 和 ${\mathbf{z}}_k$ 分别表示时刻 $k$ 的状态向量和测量向量；$f(\cdot )$ 表示状态转移函数，而 $h(\cdot )$ 表示测量函数；${\mathbf{w}}_{k-1}$ 和 ${\mathbf{v}}_k$ 是相互独立的过程噪声和观测噪声，并服从标准高斯分布。此外，常用的目标运动模型有两种：恒定速度（CV）模型和恒定转弯（CT）模型。

(1) 恒定速度模型

在CV模型中，状态向量 ${\mathbf{x}}_k=[x_k,v_{x,k},y_k,v_{y,k},z_k,v_{z,k}{]}^T$，其中 $x_k$、$y_k$ 和 $z_k$ 表示目标的位置，而 $v_{x,k}$、$v_{y,k}$ 和 $v_{z,k}$ 分别表示目标在 $x$、$y$ 和 $z$ 方向的速度。随着时间的变化，CV模型中的目标轨迹始终是直线，且每个目标的速度在每一时刻都是恒定的。换句话说，CV模型是一个线性模型。因此，状态转移矩阵可以表示为：

$$\mathbf{F}=\left[\begin{array}{cccccc}1& T& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& T& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& T\\ 0& 0& 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(3)}$$

其中，$T$ 表示采样周期。

同样地，测量矩阵表示为：

$$\mathbf{H}=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right]\text{\hspace{1em}(4)}$$

(2) 恒定转弯模型

相比于恒定速度（CV）模型，恒定转弯（CT）模型的状态向量变为 ${\mathbf{x}}_k=[x_k,v_{x,k},y_k,v_{y,k},z_k,v_{z,k},{\omega }_k{]}^T$，其中新增的变量 ${\omega }_k$ 表示时刻 $k$ 的角速度。由于 ${\omega }_k$ 的存在，CT 模型中的目标轨迹不再是直线，而是曲线。因此，CT 模型属于非线性模型，之前的矩阵 $\mathbf{F}$（如公式（3）中的矩阵）已不再适用。

为了解决这个问题，我们参考了文献【34】和【35】的思路，这些文献详细推导了二维（2D）连续时间的状态转移矩阵，如公式（5）所示。这样，非线性计算可以简化为线性计算，即：

$$\mathbf{F}=\left[\begin{array}{ccccc}1& \frac{\sin (\omega T)}{\omega }& 0& -\frac{1-\cos (\omega T)}{\omega }& 0\\ 0& \cos (\omega T)& 0& -\sin (\omega T)& 0\\ 0& \frac{1-\cos (\omega T)}{\omega }& 1& \frac{\sin (\omega T)}{\omega }& 0\\ 0& \sin (\omega T)& 0& \cos (\omega T)& 0\\ 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0\end{array}\right]\text{\hspace{1em}(5)}$$

请注意，此处的运动场景假设目标保持在一个固定高度的二维平面内运动，换句话说，目标不会在垂直方向上移动，而仅在水平方向上运动。因此，在垂直方向上，目标的运动可以视为恒定速度模型（CV 模型），那么公式（5）可以重新写为：

$$\mathbf{F}=\left[\begin{array}{ccccccc}1& \frac{\sin (\omega T)}{\omega }& 0& -\frac{1-\cos (\omega T)}{\omega }& 0& 0& 0\\ 0& \cos (\omega T)& 0& -\sin (\omega T)& 0& 0& 0\\ 0& \frac{1-\cos (\omega T)}{\omega }& 1& \frac{\sin (\omega T)}{\omega }& 0& 0& 0\\ 0& \sin (\omega T)& 0& \cos (\omega T)& 0& 0& 0\\ 0& 0& 0& 0& 1& T& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(6)}$$

此处的测量函数 $h$ 由不同类型的雷达传感器的测量形式决定。但可以肯定的是，$h$ 涉及非线性计算。对于常见的传感器，通常获得的测量值包括距离 $l_{distance}$（或位置坐标）、方位角 $\alpha$ 和俯仰角 $\beta$。然后，公式（2）中的 $h({\mathbf{x}}_k)$ 可以表示为：

$$h({\mathbf{x}}_k)=\left[\begin{array}{c}\alpha \\ \beta \\ l_{distance}\end{array}\right]=\left[\begin{array}{c}\arctan \left(\frac{\Delta y}{\Delta x}\right)\\ \arctan \left(\frac{\Delta z}{\sqrt{(\Delta x{)}^2+(\Delta y{)}^2}}\right)\\ \sqrt{(\Delta x{)}^2+(\Delta y{)}^2+(\Delta z{)}^2}\end{array}\right]\text{\hspace{1em}(7)}$$

常见的跟踪算法通常涉及卡尔曼滤波的思想，因此需要测量矩阵 $h$。我们必须将非线性的 $h({\mathbf{x}}_k)$ 转化为线性方程。根据泰勒公式，我们分别对 ${\mathbf{x}}_k$ 的每个分量进行一阶偏导数，并将结果放入一个矩阵 $\mathbf{H}$ 中，即雅可比矩阵：

$$\mathbf{H}=\left[\begin{array}{ccccccc}-\frac{\Delta y}{a}& 0& \frac{\Delta x}{a}& 0& 0& 0& 0\\ 0& -\frac{\Delta x\Delta z}{b\sqrt{a}}& 0& -\frac{\Delta y\Delta z}{b\sqrt{a}}& 0& \frac{\sqrt{a}}{b}& 0\\ 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0\end{array}\right]\text{\hspace{1em}(8)}$$

其中，$a=(\Delta x{)}^2+(\Delta y{)}^2$，$b=(\Delta x{)}^2+(\Delta y{)}^2+(\Delta z{)}^2$。然后，我们可以将 $\mathbf{H}$ 替代 $h$ 应用于后续计算，避免处理非线性矩阵。

2.2. 雷达传感器

如第 2.1 节所述，${\mathbf{z}}_k$ 的组成取决于雷达传感器的类型。因此，本节将讨论两种类型的雷达传感器。

主动雷达可以发射和接收如电磁波之类的信号【36】。因此，它可以获取完整的测量数据，包括方位角 $\alpha$、俯仰角 $\beta$ 和位置坐标 $x,y,z$，即 ${\mathbf{z}}_k=[{\alpha }_k,{\beta }_k,x_k,y_k,z_k{]}^T$。完整的测量数据可以帮助主动雷达获取精确的目标位置。然而，由于测量向量是高维的，主动雷达之间的数据融合计算复杂度也相应增加。

至于被动雷达，它只能接收从目标反射回来的信号【37】。通常，它只能获取包括 $\alpha$ 和 $\beta$ 的测量数据。因此，被动雷达通常无法像主动雷达那样获得精确的位置。不过，从另一方面来看，被动雷达之间的数据融合计算复杂度相比主动雷达要低得多。

---

3. 角度测量的初步筛选

在本节中，我们将结合被动雷达和主动雷达的优势，从而降低计算复杂度。由于这两种类型的传感器都可以测量方位角和俯仰角，因此选择这两个角度的测量值来计算和缩小测量关联与融合的范围。

整个过程可以分为两个步骤。第 3.1 节的步骤 1 通过传感器之间的位置关系缩小范围，而第 3.2 节的步骤 2 则进一步利用传感器与各方位角测量值之间的几何关系来缩小范围，步骤 2 的情况比步骤 1 更为复杂。为简化描述，以下提到的所有测量值均指方位角测量值。

3.1 传感器之间的测量范围

在不失一般性的情况下，以下描述设置了三个传感器，包括两个被动传感器和一个主动传感器。无需验证传感器是主动的还是被动的，因为我们这里只使用方位角测量值。

3.1.1 传感器的张角

为了方便描述，我们将传感器及其检测范围简化为几何图形，如图 1 所示，$O_i$ 和 $O_j$ 分别表示传感器 $S_i$ 和 $S_j$ 的位置；两个圆分别代表两个传感器的检测范围。显然，两个传感器的检测范围有重叠部分。

定义 1：从 $O_i$ 向 $S_i$ 和 $S_j$ 的检测范围重叠区域画两条射线，这两条射线形成若干张角。在这些角度中，以连接线为角平分线的最大角度即为从 $S_i$ 到 $S_j$ 的张角，即 ${\theta }_{ij}$。

考虑到传感器之间的位置关系，以及每次角度关联仅发生在两个传感器之间，因此可以在三种情况下讨论 ${\theta }_{ij}$ 的计算。

情况 1：
$$\sqrt{R_i^2+R_j^2}<d_{ij}<R_i+R_j$$
即两个传感器的检测范围重叠，端点 $M$ 和 $N$ 是圆 $O_i$ 和 $O_j$ 的交点，如图 2(a) 所示。此时，我们有：
$${\theta }_{ij}=2\cdot \arccos \left(\frac{R_i^2+d_{ij}^2-R_j^2}{2\cdot R_i\cdot d_{ij}}\right)\text{\hspace{1em}(9)}$$
其中，$R_i$ 和 $R_j$ 分别是圆 $O_i$ 和 $O_j$ 的半径，$d_{ij}$ 是 $S_i$ 和 $S_j$ 之间的距离，即 $O_i{O}_j$ 的长度。

情况 2：
$$R_j<d_{ij}\le \sqrt{R_i^2+R_j^2}$$
即两个传感器的检测范围重叠，端点 $M$ 和 $N$ 是圆 $O_j$ 的切点，如图 2(b) 所示。此时，我们有：
$${\theta }_{ij}=2\cdot \arcsin \left(\frac{R_j}{d_{ij}}\right)\text{\hspace{1em}(10)}$$

情况 3：
$$d_{ij}\le R_j$$
即 $S_i$ 位于 $S_j$ 的检测范围内，如图 2© 所示。由于缺乏距离测量，$S_i$ 的每个测量值所对应的实际目标可能位于 $S_j$ 的检测范围内。为了避免测量值的遗漏，$S_i$ 的所有测量值都应与 $S_j$ 的测量值相关联，即：
$${\theta }_{ij}=360^{\circ }\text{\hspace{1em}(11)}$$

3.1.2. 测量的初步关联范围

在计算出 ${\theta }_{ij}$ 后，可以结合 $O_i{O}_j$ 的方位角 ${\alpha }_{ij}$ 来计算测量值的初步关联范围。同样，这一范围的计算需要在三种情况下进行讨论。

情况 1：$d_{ij}\ge R_i+R_j$，即两个传感器的检测范围完全不重叠，如图 3 所示。显然，两个传感器之间不存在关联范围。

情况 2：$R_j<d_{ij}<R_i+R_j$，即两个传感器的检测范围重叠，但 $S_i$ 位于圆 $O_j$ 的外部。因此，我们有：
$$\{\begin{array}{l}{\beta }_{ij}^{\text{min}}={\alpha }_{ij}-\frac{1}{2}{\theta }_{ij}\\ {\beta }_{ij}^{\text{max}}={\alpha }_{ij}+\frac{1}{2}{\theta }_{ij}\end{array}\text{\hspace{1em}(12)}$$
其中，${\beta }_{ij}^{\text{min}}$ 和 ${\beta }_{ij}^{\text{max}}$ 分别是初步关联范围的下限和上限。

请注意，由公式 (12) 给出的上下限可能会出现大于 $180^{\circ }$ 或小于 $-180^{\circ }$ 的情况，这可能导致某些测量值的遗漏。为了解决这个问题，需要根据以下情况对 ${\beta }_{ij}^{\text{min}}$ 和 ${\beta }_{ij}^{\text{max}}$ 进行修正。

情况 1：$0^{\circ }\le {\alpha }_{ij}\le 180^{\circ }$，如图 4(a) 所示。在这种情况下，上限 ${\beta }_{ij}^{\text{max}}$ 可能大于 $180^{\circ }$。因此，整个范围应分为两部分，一部分是小于 $180^{\circ }$ 的范围，另一部分是大于 $180^{\circ }$ 的范围。修正后的关联范围由这两部分组成。

情况 2：$-180^{\circ }<{\alpha }_{ij}<0^{\circ }$，如图 4(b) 所示。这时，下限 ${\beta }_{ij}^{\text{min}}$ 可能小于 $-180^{\circ }$。类似地，整个范围应分为两部分，一部分是大于 $-180^{\circ }$ 的范围，另一部分是小于 $-180^{\circ }$ 的范围。因此，修正后的关联范围由这两部分组成。

情况 3：$d_{ij}<R_j$，即 $S_i$ 位于 $S_j$ 的检测范围内，如图 5 所示。由于 ${\theta }_{ij}=360^{\circ }$，$S_i$ 的所有测量值都应与 $S_j$ 的测量值关联。

在确定了所有传感器之间的初步关联范围后，$S_j$ 中的每个测量值只需与 $S_i$ 至 $S_j$ 的初步关联范围内的测量值关联即可。

3.2. 传感器和测量值之间的测量范围

第 3.1 节介绍的初步关联范围是针对两个传感器之间的，这只将测量值的范围缩小到了两个传感器的重叠区域。然而，没有必要将 $S_i$ 到 $S_j$ 的初步关联范围内的测量值与 $S_j$ 到 $S_i$ 的初步关联范围内的测量值关联，这将导致较高的计算成本以及大量无效的测量关联组。因此，进一步缩小范围是必要的。

对于这个问题，有两种解决方案值得考虑。一种方法是转换初步关联范围内 $S_i$ 到 $S_j$ 的每个测量值 $z_{m,k}$（时间 $k$ 时刻的第 $m$ 次测量）的坐标，通过测量方程获取它在 $S_j$ 到 $S_i$ 的关联范围内的分布范围，并将这个范围作为 $z_{m,k}$ 的新关联范围。

在时间 $k$，获取 $z_{m,k}$ 的方位角和俯仰角的测量方程可以写为：
$$z_{m,k}=h(x_k)+v_k=\left[\begin{array}{c}\arctan \left(\frac{y_{m,k}-y_i}{x_{m,k}-x_i}\right)\\ \arctan \left(\frac{z_{m,k}-z_i}{\sqrt{(x_{m,k}-x_i{)}^2+(y_{m,k}-y_i{)}^2}}\right)\end{array}\right]+\left[\begin{array}{c}{v}_{\alpha ,k}\\ v_{\beta ,k}\end{array}\right]\text{\hspace{1em}(13)}$$

然后，$S_i$ 和 $S_j$ 的测量方程 (13) 可以写为：
$$z_{i,k}=h_i(x_k)+v_{i,k},z_{j,k}=h_j(x_k)+v_{j,k}\text{\hspace{1em}(14)}$$

显然，测量值坐标的转换涉及非线性矩阵的计算，这可能会导致较高的计算负担。

为了避免上述问题，我们在本文中考虑另一种解决方案。基于几何关系，可以建立 $S_i$ 到 $S_j$ 的初步关联范围内的测量值与 $S_j$ 之间的范围，这个范围将比第 3.1 节中的范围更小。下面将详细推导该解决方案。

3.2.1. 测量值的张角

定义 2：如图 6 所示，$z_{m,k}$ 是 $S_i$ 到 $S_j$ 的初步关联范围内的一个测量值。一条从 $O_i$ 到 $z_{m,k}$ 的射线分别与圆 $O_i$ 和 $O_j$ 在 $E$ 和 $F$ 处相交。当 $O_j$ 连接 $E$ 和 $F$ 时，会形成若干个角度。在这些角度中，包含 $z_{m,k}$ 位置的最小角度就是从 $S_j$ 到 $z_{m,k}$ 的张角，即 ${\varphi }_{jim}$。

为了简化描述，在推导之前需要明确几个与 ${\varphi }_{jim}$ 相关的变量。设 $\angle 1$ 为 $\angle E{O}_i{O}_j$，$\angle 2$ 为 $\angle E{O}_j{O}_i$，$\angle 3$ 为 $\angle O_{i}F{O}_j$，$\angle 4$ 为 $\angle O_i{O}_{j}F$，$l_1$ 为 $O_{j}E$，$l_2$ 为 $O_{i}F$。

在将两个角度都转换为正值后，有：
$$\angle 1=|{\alpha }_{ij}-{\alpha }_{ijm}|\text{\hspace{1em}(15)}$$
一旦计算出 $\angle 1$，我们得到：
$$l_1=\sqrt{R_i^2+d_{ij}^2-2\cdot R_i\cdot d_{ij}\cdot \cos (\angle 1)}\text{\hspace{1em}(16)}$$

考虑到余弦函数的性质，$\angle 2$ 和 $\angle 3$ 的计算可能会有两种情况，即：
$$\angle 2=\{\begin{array}{ll}\arcsin \left(\frac{R_i\cdot \sin (\angle 1)}{l_1}\right),& \text{如果 }\angle 2\text{ 是锐角}\\ 180^{\circ }-\arcsin \left(\frac{R_i\cdot \sin (\angle 1)}{l_1}\right),& \text{如果 }\angle 2\text{ 是钝角}\end{array}\text{\hspace{1em}(17)}$$
$$\angle 3=\{\begin{array}{ll}\arcsin \left(\frac{d_{ij}\cdot \sin (\angle 1)}{R_j}\right),& \text{如果 }\angle 3\text{ 是锐角}\\ 180^{\circ }-\arcsin \left(\frac{d_{ij}\cdot \sin (\angle 1)}{R_j}\right),& \text{如果 }\angle 3\text{ 是钝角}\end{array}\text{\hspace{1em}(18)}$$

结合 (15) 和 (18)，我们得到：
$$\angle 4=180^{\circ }-\angle 1-\angle 3\text{\hspace{1em}(19)}$$
同样，我们可以通过 (15) 和 (19) 计算出 $l_2$，即：
$$l_2=R_j\cdot \frac{\sin (\angle 4)}{\sin (\angle 1)}\text{\hspace{1em}(20)}$$

在计算出上述六个变量后，可以在以下三种情况下讨论 ${\varphi }_{jim}$。

情况 1：$R_j<d_{ij}<R_i+R_j$，与第 3.1 节中计算测量值初步关联范围时的情况 1 相同。如图 7(a) 所示，可以明显地计算出 ${\varphi }_{jim}$，即 $\angle E{O}_{j}F$，由下式计算：
$${\varphi }_{jim}=\angle 2-\angle 4\text{\hspace{1em}(21)}$$

情况 2：$d_{ij}<R_j$ 且 $l_2<O_{i}E$。如图 7(b) 所示，${\varphi }_{jim}$ 为 $\angle 4$。

情况 3：$d_{ij}<R_j$ 且 $l_2\ge O_{i}E$。如图 7© 所示，${\varphi }_{jim}$ 为 $\angle 2$。

3.2.2. 测量的有效关联范围

尽管在第3.2.1节中得到了${\varphi }_{jim}$，但测量的有效关联范围的推导并不像测量的初步关联范围那样简单。对于任何传感器的测量，传感器在检测目标时存在误差，这是由于观测噪声$v_{\alpha }$所致。根据第2.1节，$v_{\alpha }\sim N(0,{\sigma }_{\alpha }^2)$，其中${\sigma }_{\alpha }$是传感器测量误差的标准差。如果存在任何测量遗漏，在推导测量的有效关联范围时应考虑测量误差。同样，这种推导应像上一节那样预先讨论三种情景。

场景1：$R_j<d_{ij}<R_i+R_j$，如图7(a)所示。考虑到${\alpha }_{ij}$的正负，以及$z_{m,k}$和${\alpha }_{ij}$之间的位置关系，会影响${\varphi }_{jim}$，最终影响有效关联范围的上限${\beta }_{jim}^{\text{max}}$和下限${\beta }_{jim}^{\text{min}}$。因此，可以根据${\alpha }_{ij}$的正负讨论以下两种情况：

情况1：$0^{\circ }\le {\alpha }_{ij}\le 180^{\circ }$

• 当${\beta }_{ij}^{\text{min}}\le {\alpha }_{ijm}<{\alpha }_{ij}$时：
  $$\begin{array}{rl}{\beta }_{jim}^{\text{max}}& ={\alpha }_{ji}+\angle 2+3{\sigma }_{\alpha }\\ {\beta }_{jim}^{\text{min}}& ={\beta }_{jim}^{\text{max}}-{\varphi }_{jim}-3{\sigma }_{\alpha }\end{array}\text{\hspace{1em}(22)}$$
• 当${\alpha }_{ij}<{\alpha }_{ijm}\le {\beta }_{ij}^{\text{max}}$时：
  $$\begin{array}{rl}{\beta }_{jim}^{\text{min}}& ={\alpha }_{ji}-\angle 2-3{\sigma }_{\alpha }\\ {\beta }_{jim}^{\text{max}}& ={\beta }_{jim}^{\text{min}}+{\varphi }_{jim}+3{\sigma }_{\alpha }\end{array}\text{\hspace{1em}(23)}$$
  (23)式中给出的下限${\beta }_{jim}^{\text{min}}$需要在测量遗漏情况下，按照第3.1.2节中推导初步关联范围的方法进行修正。

情况2：$-180^{\circ }<{\alpha }_{ij}<0^{\circ }$

• 当${\alpha }_{ij}<{\alpha }_{ijm}\le {\beta }_{ij}^{\text{max}}$时：
  $$\begin{array}{rl}{\beta }_{jim}^{\text{min}}& ={\alpha }_{ji}-\angle 2-3{\sigma }_{\alpha }\\ {\beta }_{jim}^{\text{max}}& ={\beta }_{jim}^{\text{min}}+{\varphi }_{jim}+3{\sigma }_{\alpha }\end{array}\text{\hspace{1em}(24)}$$
• 当${\beta }_{ij}^{\text{min}}\le {\alpha }_{ijm}<{\alpha }_{ij}$时：
  $$\begin{array}{rl}{\beta }_{jim}^{\text{max}}& ={\alpha }_{ji}+\angle 2+3{\sigma }_{\alpha }\\ {\beta }_{jim}^{\text{min}}& ={\beta }_{jim}^{\text{max}}-{\varphi }_{jim}-3{\sigma }_{\alpha }\end{array}\text{\hspace{1em}(25)}$$
  同样，(22)式中给出的上限${\beta }_{jim}^{\text{max}}$也需要进行修正。

场景2：$d_{ij}<R_j$且$l_2<O_{i}E$，如图7(b)所示。与场景1类似，${\alpha }_{ij}$的正负以及$z_{m,k}$和${\alpha }_{ij}$之间的位置关系仍然存在影响。然而，由于${\theta }_{ij}=360^{\circ }$，$z_{m,k}$和${\alpha }_{ij}$之间的位置关系不能简单通过它们的大小来判断。在本研究中，考虑将圆$O_i$沿$O_i{O}_j$分为两个半圆A和B。

A) $0^{\circ }\le {\alpha }_{ij}\le 180^{\circ }$。当$z_{m,k}$位于半圆A，如图8(a)所示：
$$\begin{array}{rl}{\beta }_{jim}^{\text{max}}& ={\alpha }_{ji}+{\varphi }_{jim}+3{\sigma }_{\alpha }\\ {\beta }_{jim}^{\text{min}}& ={\alpha }_{ji}-3{\sigma }_{\alpha }\end{array}\text{\hspace{1em}(26)}$$
当$z_{m,k}$位于半圆B，如图8(b)所示：
$$\begin{array}{rl}{\beta }_{jim}^{\text{min}}& ={\alpha }_{ji}-{\varphi }_{jim}-3{\sigma }_{\alpha }\\ {\beta }_{jim}^{\text{max}}& ={\alpha }_{ji}+3{\sigma }_{\alpha }\end{array}\text{\hspace{1em}(27)}$$

B) $-180^{\circ }<{\alpha }_{ij}<0^{\circ }$。当$z_{m,k}$位于半圆A，如图9(a)所示：
$$\begin{array}{rl}{\beta }_{jim}^{\text{min}}& ={\alpha }_{ji}-{\varphi }_{jim}-3{\sigma }_{\alpha }\\ {\beta }_{jim}^{\text{max}}& ={\alpha }_{ji}+3{\sigma }_{\alpha }\end{array}\text{\hspace{1em}(28)}$$
当$z_{m,k}$位于半圆B，如图9(b)所示：
$$\begin{array}{rl}{\beta }_{jim}^{\text{max}}& ={\alpha }_{ji}+{\varphi }_{jim}+3{\sigma }_{\alpha }\\ {\beta }_{jim}^{\text{min}}& ={\alpha }_{ji}-3{\sigma }_{\alpha }\end{array}\text{\hspace{1em}(29)}$$
同样，(27)式和(29)式中给出的${\beta }_{jim}^{\text{min}}$和${\beta }_{jim}^{\text{max}}$需要进行修正。

场景3：$d_{ij}<R_j$且$l_2\ge O_{i}E$，如图7©所示。在这种情况下，${\beta }_{jim}^{\text{min}}$和${\beta }_{jim}^{\text{max}}$的推导与场景2中的相同，因此这里不再重复。

确定了传感器和测量值之间的所有有效关联范围后，只需将处于$S_i$到$S_j$初步关联范围内的每个测量值$z_{m,k}$与$S_j$到$z_{m,k}$的有效关联范围内的测量值进行关联。

3.3 结果比较

在本节中，我们构建了一个相对复杂的场景，场景高度为500米，包含三个传感器，其中两个是被动传感器，即S1和S2，另一个是主动传感器，即S3。该场景中同时存在六个目标和均匀分布的杂波。每个传感器都位于另两个传感器的探测范围内，这意味着需要关联的测量数量非常庞大。S1、S2和S3的坐标分别为 (0, -5km, 0)、(5km, 0, 0) 和 (0, 5km, 0)。
请注意，这里的比较仅旨在展示初步筛选算法对角度测量的效果，因此图10中的数据是随机选择的某一时刻的跟踪数据。
从图10可以看出，初步筛选算法对角度测量的效果显著，具有减少数据融合计算成本的优势。

---

4. 多源数据的融合与校正

显而易见，需关联的测量数量大幅减少，所有这些都是无效的测量关联组。然而，当两个传感器的重叠区域过大，导致大量测量数据时，初步筛选后剩余的测量中仍然保留了许多无效测量或错误的测量关联组。为了解决上述问题，本节基于第3节介绍的理论推导，提出了SAMPS-AA算法，主要包括：(1) 在第4.1节中基于角度测量的统计方法，用于排除剩余的无效测量组；(2) 在第4.2节中推导了基于最小二乘法(LS) [38] 的角度关联；(3) 在第4.3节中提出了三种排除角度关联错误结果的策略。

4.1 基于统计的筛选

在不失一般性的情况下，该场景中包含三个传感器，其中S1和S2是被动传感器，而S3是主动传感器。经过初步筛选算法处理后，从这些传感器之一（如S1）中选取所有测量作为分组依据。然后，将Sj (j = 2, 3) 在有效范围内的每个测量与S1的测量按顺序分组。一个测量关联组的形式可以表示为{(${\alpha }_{1r}$, ${\beta }_{1r}$); (α2s, β2s); (α3t, β3t)}，其中1r, 2s和3t分别指代S1, S2和S3的第r, s, t对(α, β)。

在三维$x-y-z$直角坐标系中，每个传感器的(α, β)对可以构建一条三维直线，即位置线；而在二维$x-y$直角坐标平面中，每个传感器的α可以构建一条二维直线，即方位线。考虑到包含z轴的二维平面不是唯一的，这会导致不同传感器的位置线在不同平面上变成不相交的直线，因此我们选择基于方位角测量在$x-y$平面上建立统计数据。
在一个测量关联组{(α1r, β1r); (α2s, β2s); (α3t, β3t)}中，定义来自α1r的方位线与来自α2s的方位线的交点为M，如图11所示。根据α1r、α2s以及S1和S2的坐标，M的坐标，即$(x_M,y_M)$，由以下公式给出：

$$\begin{array}{rl}{x}_M& =\frac{x_1\tan {\alpha }_{1r}-x_2\tan {\alpha }_{2s}+y_2-y_1}{\tan {\alpha }_{1r}-\tan {\alpha }_{2s}}\\ y_M& =\frac{y_2\tan {\alpha }_{1r}-y_1\tan {\alpha }_{2s}+(x_1-x_2)\tan {\alpha }_{1r}\tan {\alpha }_{2s}}{\tan {\alpha }_{1r}-\tan {\alpha }_{2s}}\end{array}\text{\hspace{1em}(30)}$$

定义从S3到M的方位角为αM，则有

$$\tan {\alpha }_M=\frac{y_M-y_3}{x_M-x_3}\text{\hspace{1em}(31)}$$

在没有测量噪声的理想情况下，显然有${\alpha }_M={\alpha }_{3t}$。然而，在实际情况下，由于存在测量噪声，${\alpha }_M$与${\alpha }_{3t}$的残差成为统计量，即$\Delta \alpha ={\alpha }_M-{\alpha }_{3t}$。为了获得$\Delta \alpha$的分布，进行了50次蒙特卡罗试验，获得了$8.47\times 10^6$个样本数据。概率密度函数（PDF）的曲线如图12所示。由图12可见，$\Delta \alpha$大致服从标准高斯分布，即$\Delta \alpha \sim N(0,{\sigma }_{\Delta \alpha }^2)$，其中${\sigma }_{\Delta \alpha }^2$是$\Delta \alpha$的方差，并近似为

$${\sigma }_{\Delta \alpha }^2\approx {\left|\frac{\partial \Delta \alpha }{\partial {\alpha }_{1r}}{\sigma }_{{\alpha }_1}\right|}^2+{\left|\frac{\partial \Delta \alpha }{\partial {\alpha }_{2s}}{\sigma }_{{\alpha }_2}\right|}^2+{\left|\frac{\partial \Delta \alpha }{\partial {\alpha }_{3t}}{\sigma }_{{\alpha }_3}\right|}^2\text{\hspace{1em}(32)}$$

其中${\sigma }_{{\alpha }_1},{\sigma }_{{\alpha }_2}$和${\sigma }_{{\alpha }_3}$分别是每个传感器测量误差的标准差。
根据标准高斯分布的性质，可以将置信区间设置为$(-3{\sigma }_{\Delta \alpha },3{\sigma }_{\Delta \alpha })$，其对应的概率为99.73%。如果$-3{\sigma }_{\Delta \alpha }<\Delta \alpha <3{\sigma }_{\Delta \alpha }$，则认为当前测量关联组中的三个(α, β)对属于同一目标；否则，应排除当前测量关联组。

为了更准确地说明我们的算法，设置了一个数值例子。给定一个测量关联组{(α1r, β1r); (α2s, β2s); (α3t, β3t)}，其中${\alpha }_{1r}=98.06^{\circ }$，${\alpha }_{2s}=161.71^{\circ }$，${\alpha }_{3t}=-107.86^{\circ }$。根据公式(30)和(31)，我们得出${\alpha }_M=-108.12^{\circ }$。由于存在测量噪声，${\alpha }_M\ne {\alpha }_{3t}$。然后，我们可以得到$\Delta \alpha ={\alpha }_M-{\alpha }_{3t}=-0.26^{\circ }$，以及通过公式(32)得到其方差${\sigma }_{\Delta \alpha }^2\approx 0.03$。相应地，${\sigma }_{\Delta \alpha }\approx 0.16$。根据标准高斯分布的性质，由于$-3{\sigma }_{\Delta \alpha }=-0.48^{\circ }<\Delta \alpha =-0.26^{\circ }<3{\sigma }_{\Delta \alpha }=0.48^{\circ }$，因此可以将给定测量关联组中的(α, β)对保留进行进一步计算。

4.2 角度关联

基于统计的筛选算法不仅将不同传感器的测量分组，还筛选掉了大量错误的测量关联组，这使得可以在较小的关联组规模下进行角度关联。同时，在三维直角坐标系中，跟踪步骤需要以(x, y, z)的形式进行测量。因此，本节将介绍角度关联。

如第4.1节所述，每个传感器的(α, β)对构建一条位置线。在图13中，$L_i$ (i = 1, 2, ⋯, N，其中N为传感器的总数，在此为了介绍方便设N = 3) 分别表示与测量关联组{(α1r, β1r); (α2s, β2s); (α3t, β3t)}对应的三条位置线。在没有测量噪声的理想情况下，来自同一测量关联组的三条位置线应交于一点，即对应目标的位置。然而，由于存在测量噪声，这些直线不会在一点相交，而是在空间中形成一个三角形。更糟糕的是，这些直线可能根本不会与其他两条直线相交。

为了找到目标的相对准确位置，我们考虑取与所有直线距离最短的点作为目标的测量点，即图13中的T($x_T,y_T,z_T$)。
位置线的方向余弦由下式给出：

$$l_i=\cos {\beta }_i\cos {\alpha }_i,m_i=\cos {\beta }_i\sin {\alpha }_i,n_i=\sin {\beta }_i\text{\hspace{1em}(33)}$$

然后，位置线可以写成：

$$\frac{x_T-x_i}{l_i}=\frac{y_T-y_i}{m_i}=\frac{z_T-z_i}{n_i}\text{\hspace{1em}(34)}$$

T点到Li的距离为：

$$d_i=\frac{\left|\begin{array}{ccc}i& j& k\\ x_T-x_i& y_T-y_i& z_T-z_i\\ l_i& m_i& n_i\end{array}\right|}{\sqrt{l_i^2+m_i^2+n_i^2}}\text{\hspace{1em}(35)}$$

随后，T点到三条位置线的平方距离和为：

$$d^2=\sum _{i=1}^3{d}_i^2=\sum _{i=1}^3\left(u_i^2+v_i^2+w_i^2\right)\text{\hspace{1em}(36)}$$

其中：

$$\begin{array}{rl}{u}_i& =(y_T-y_i)n_i-(z_T-z_i)m_i\\ v_i& =(z_T-z_i)l_i-(x_T-x_i)n_i\\ w_i& =(x_T-x_i)m_i-(y_T-y_i)l_i\end{array}\text{\hspace{1em}(37)}$$

根据最小二乘法的思想，让$\frac{\partial d^2}{\partial x_T}=0$，$\frac{\partial d^2}{\partial y_T}=0$，以及$\frac{\partial d^2}{\partial z_T}=0$，得到以下方程组：

$$\left[\begin{array}{ccc}L& T& S\\ T& M& R\\ S& R& N\end{array}\right]\left[\begin{array}{c}{x}_T\\ y_T\\ z_T\end{array}\right]=\left[\begin{array}{c}E\\ F\\ G\end{array}\right]\text{\hspace{1em}(38)}$$

其中，

$$\begin{array}{rl}L& =\sum _{i=1}^3\left(m_i^2+n_i^2\right),M=\sum _{i=1}^3\left(n_i^2+l_i^2\right),N=\sum _{i=1}^3\left(l_i^2+m_i^2\right),\\ R& =-\sum _{i=1}^3(m_i{n}_i),S=-\sum _{i=1}^3(n_i{l}_i),T=-\sum _{i=1}^3(l_i{m}_i)\end{array}\text{\hspace{1em}(39-40)}$$

$$\begin{array}{rl}E& =\sum _{i=1}^3\left[(m_i^2+n_i^2)x_i-l_i{m}_i{y}_i-n_i{l}_i{z}_i\right],\\ F& =\sum _{i=1}^3\left[-l_i{m}_i{x}_i+(n_i^2+l_i^2)y_i-m_i{n}_i{z}_i\right],\\ G& =\sum _{i=1}^3\left[-n_i{l}_i{x}_i-m_i{n}_i{y}_i+(l_i^2+m_i^2)z_i\right]\end{array}\text{\hspace{1em}(41)}$$

令

$$Q=\left|\begin{array}{ccc}L& T& S\\ T& M& R\\ S& R& N\end{array}\right|\text{\hspace{1em}(42)}$$

最后，(38)方程的解，即我们需要的测量点的位置，可以通过以下公式计算：

$$\begin{array}{rl}{\hat{x}}_T& =\frac{EMN+FRS+GRT-GMS-FTN-E{R}^2}{Q},\\ {\hat{y}}_T& =\frac{ERS+FLN+GST-GLR-ETN-F{S}^2}{Q},\\ {\hat{z}}_T& =\frac{ERT+FST+GLM-FLR-EMS-G{T}^2}{Q}\end{array}\text{\hspace{1em}(43)}$$

通过获得所有的测量点，我们可以使用合适的滤波器开始跟踪。然而，这些测量点可能来自错误的关联组，这最终会影响跟踪结果。因此，在接下来的第4.3节中需要制定一些策略来消除这些不良影响。

4.3. 排除测量值

在存在测量噪声的情况下，可能会存在其他测量（目标或杂波）产生的定位线，接近正确关联组的定位线，这些定位线可能在第3节和第4.1节中通过了筛选。这些定位线可能与一些正确关联组的定位线组成一个新的关联组。如图14所示，{（α11, β11）；（α21, β21）；（α31, β31）} 是一个正确的组，而 {（α12, β12）；（α22, β22）；（α32, β32）} 是另一个正确的组。由于这些定位线之间的距离较短，会产生几个新的错误关联组。此外，在第4.2节中从这些错误组中获得的测量点也位于正确点附近，这会影响跟踪结果。因此，需要一系列的排除策略。

4.3.1. 通过距离排除

根据第4.2节，若没有测量噪声，从测量点 T 到三个对应定位线的平方距离之和应满足
$$d^2=\sum _{i=1}^3{d}_i^2=0\text{\hspace{1em}(44)}$$
然而，实际上，结果（44）不再为零，即
$$d^2=\sum _{i=1}^3{d}_i^2\ne 0\text{\hspace{1em}(45)}$$
显然，结果（45）对应于正确点的值远小于对应于任何其他错误点的值。因此，我们可以通过 $d^2$ 判断测量点是否正确。

设 $(x_A,y_A,z_A)$ 为当前时刻目标 A 的位置。由于最小二乘法的准确性，由它得到的测量点的误差通常在一个小范围内（$-{\sigma }_l$, ${\sigma }_l$），其中${\sigma }_l$（$l=x,y,z$）是一个活动传感器的测量误差 $v_l$（$v_l\sim N(0,{\sigma }_l^2)$）的标准差，那么最大误差下的测量点 P 的位置为
$$\{\begin{array}{l}{x}_P=x_A\pm {\sigma }_x\\ y_P=y_A\pm {\sigma }_y\\ z_P=z_A\pm {\sigma }_z\end{array}\text{\hspace{1em}(46)}$$
如图15所示，d 是 P 到对应定位线 L 的距离。显然，在直角三角形 $\Delta PAB$ 中，$PA\ge d$，其中 B 是从 P 到 L 的垂足。两条虚线指的是具有测量误差的定位线。在误差为 $3{\sigma }_l$ 的情况下，$PA\ge d$ 是有效的。

$PA$ 的长度为
$$PA=\sqrt{{\sigma }_x^2+{\sigma }_y^2+{\sigma }_z^2}\text{\hspace{1em}(47)}$$
因此，$d^2$ 的近似门限 $\gamma$ 为
$$\gamma =3({\sigma }_x^2+{\sigma }_y^2+{\sigma }_z^2)\text{\hspace{1em}(48)}$$
如果 $d^2\le \gamma$，则相应的测量点可以被视为属于目标或杂波；否则，即为错误的测量点。

4.3.2. 通过主动传感器的测量排除

对于主动传感器，它们可以在每个跟踪时间点获取位置测量。因此，这些位置测量可以用来排除错误的测量点。一旦测量点与主动传感器的对应位置测量之间的距离足够长，该测量点被认为是错误的。

从第4.3.1节的（46）式中，我们了解到，通过最小二乘法（LS）得到的测量点满足
$$\{\begin{array}{l}{\hat{x}}_T=x_A+a{\sigma }_x\\ {\hat{y}}_T=y_A+b{\sigma }_y\\ {\hat{z}}_T=z_A+c{\sigma }_z\end{array}\text{\hspace{1em}(49)}$$
其中 $a,b,c\in (-1,1)$。

通常，主动传感器的位置信息可以写成
$$\{\begin{array}{l}{x}_{3t}=x_A+e{\sigma }_x\\ y_{3t}=y_A+f{\sigma }_y\\ z_{3t}=z_A+g{\sigma }_z\end{array}\text{\hspace{1em}(50)}$$
因此，判断位置测量是否属于目标的方法是设定一个置信区间为（$-3{\sigma }_l$, $3{\sigma }_l$），使得 $e,f,g\in (-3,3)$。结合（49）式和（50）式，我们得到
$$\{\begin{array}{l}|{\hat{x}}_T-x_{3t}|=|a-e|{\sigma }_x\\ |{\hat{y}}_T-y_{3t}|=|b-f|{\sigma }_y\\ |{\hat{z}}_T-z_{3t}|=|c-g|{\sigma }_z\end{array}\text{\hspace{1em}(51)}$$
然后，最大的门限分别为
$${\gamma }_x=4{\sigma }_x,{\gamma }_y=4{\sigma }_y,{\gamma }_z=4{\sigma }_z\text{\hspace{1em}(52)}$$
一旦测量点满足 $|{\hat{x}}_T-x_{3t}|\le 4{\sigma }_x$、$|{\hat{y}}_T-y_{3t}|\le 4{\sigma }_y$ 和 $|{\hat{z}}_T-z_{3t}|\le 4{\sigma }_z$，该点被认为是一个正确的测量点。同时，门限应根据杂波数量进行调整。当杂波数量较多时，门限应较小。

最后，通过上述排除策略，可以排除超过90%的错误测量点。然而，可能仍存在少量错误测量点成功通过最后两步排除，因为这些点距离正确点太近而无法被排除。一旦这些点被保留并放入跟踪滤波器中，跟踪结果必然会受到影响。因此，我们考虑通过对接近其他点的点进行坐标平均来合并这些点，即
$$x=\frac{x_r+x_s}{2},y=\frac{y_r+y_s}{2},z=\frac{z_r+z_s}{2}\text{\hspace{1em}(53)}$$
其中 $(x_r,y_r,z_r)$ 和 $(x_s,y_s,z_s)$ 表示上述测量点的坐标，而 $(x,y,z)$ 表示跟踪前的最终测量值。