CN106228805A - 一种交互式多车辆跟踪方法及装置 - Google Patents

Abstract

本发明提供了一种交互式多车辆跟踪方法及装置，所述方法包括：S1.建立多车辆交互的车辆跟踪模型；S2.利用传感器分别获取每个待跟踪车辆在第k时刻的测量信息；其中，一个传感器对应一个待跟踪车辆；S3.根据步骤S2获取的测量信息，以及步骤S1建立的车辆跟踪模型，利用非扩维方法估计待跟踪车辆的运动状态参数，实现对待跟踪车辆的跟踪。本发明提供的交互式多车辆跟踪方法，能够获得较为准确的车辆跟踪结果。

Description

一种交互式多车辆跟踪方法及装置

技术领域

本发明涉及智能交通领域，具体涉及一种交互式多车辆跟踪方法及装置。

背景技术

多车辆跟踪是智能交通系统的关键组成部分，是分析交通流的重要手段。多车辆跟踪在视频观测、交通检测、人的活动分析、运动视频分析等许多应用中至关重要。对车辆轨迹的分析能预测车辆未来的路径，从而可采取适当的动作及时做出决策，从而规避拥堵、分散车流，缓解交通压力，降低交通事故发生率，继而实现交通系统的智能化和集约化。例如，跟踪密集地区的车流状况，以便于对车辆的流向进行预测，探测到可能发生的异常活动等。

在现有的多车辆跟踪过程中，往往采用经典的多目标跟踪方法，通过建立车辆的运动模型和传感器测量模型，估计各车辆的运动状态参数，其中，建立的车辆运动模型往往是不依赖于外部环境和周围车辆的。然而，在实际交通系统中，车辆之间是有交互行为的。例如，驾驶员总是期望与前导车保持一个安全的车头间距，当前导车突然制动时，驾驶员所驾驶的车辆需要做出相应的减速停车，以避免发生碰撞；同样，如果前导车加速运动时，驾驶员也会根据实际的安全情况做出相应的加速运动，以尽快到达目的地。因此，在实际的多车辆跟踪过程中，车辆运动的加速度与周围车辆的运动状态有很强的相关性，只有充分考虑车辆之间的交互式行为，才能更好地提高车辆的跟踪精度。然而现有技术中的多车辆跟踪方法均没有考虑到车辆运动状态之间的交互作用，而有时交互作用会对整个跟踪系统产生很大的影响，以至于现有技术中的多车辆跟踪方法并不能很好的进行车辆追踪，甚至有可能与真实车辆轨迹产生巨大的偏离。

发明内容

针对现有技术中的缺陷，本发明提供一种交互式多车辆跟踪方法及装置，能够获得较为准确的车辆跟踪结果。

第一方面，本发明提供了一种交互式多车辆跟踪方法，包括以下步骤：

S1：建立多车辆交互的车辆跟踪模型：

$\left\{\begin{array}{c}{x}_{i,k+1}=f\left(x_{i,k}\right)+\underset{j=1,j\≠i}{\overset{N}{\Σ}}{w}_{ij}\mathrm{\Γ}(x_{j,k}-x_{i,k})+B_{i,k}{\stackrel{\‾}{w}}_{i,k}\\ y_{i,k}=h\left(x_{i,k}\right)+v_{i,k}\end{array}\right.$

其中，y_i,k表示第k时刻第i个传感器获得的测量信息；x_i,k+1表示第k+1时刻第i个待跟踪车辆的运动状态参数；f(x)函数与h(x)函数是状态方程与观测方程中的非线性函数；w_ij表示第i个待跟踪车辆与第j个待跟踪车辆之间的交互强度；Γ表示第i个待跟踪车辆与第j个待跟踪车辆之间状态的耦合矩阵；与v_i,k表示第k时刻的系统噪声与观测噪声，B_i,k表示第k时刻系统的适维矩阵；

S2、利用传感器分别获取每个待跟踪车辆在第k时刻的测量信息，其中，一个传感器对应一个待跟踪车辆；

S3、根据步骤S2获取的测量信息，以及步骤S1建立的车辆跟踪模型，利用非扩维方法估计待跟踪车辆的运动状态参数，实现对待跟踪车辆的跟踪。

进一步地，所述S3根据步骤S2获取的测量信息，以及步骤S1建立的车辆跟踪模型，利用非扩维方法估计待跟踪车辆的运动状态参数，实现对待跟踪车辆的跟踪，包括：

S31：针对每一个状态，获取状态的预测方程和更新方程，并对每个状态进行实时更新：

${\hat{x}}_{i,k+1|k}=f\left({\hat{x}}_{i,k|k}\right)+\underset{j=1,j\≠i}{\overset{N}{\Σ}}{w}_{ij}\mathrm{\Γ}({\hat{x}}_{j,k|k}-{\hat{x}}_{i,k|k})$

${\hat{x}}_{i,k+1|k+1}={\hat{x}}_{i,k+1|k}+K_{i,k+1}(y_{i,k+1}-h({\hat{x}}_{i,k+1|k}\left)\right)$

其中，K_i,k+1表示滤波增益矩阵；表示第k时刻对第i个待跟踪车辆估计的预测值；表示第k+1时刻对第i个待跟踪车辆估计的更新值；表示第k时刻对第i个待跟踪车辆估计的更新值；

S32：利用非扩维方法获取每一个状态的预测估计误差协方差矩阵与更新估计误差的协方差矩阵的上界：

$\begin{array}{c}{\mathrm{\Δ}}_{i,k+1|k}=(1+{\mathrm{\α}}_1)(A_{i,k}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}\mathrm{\Γ}){({\mathrm{\Δ}}_{i,k|k}^{-1}-(1+r\left){\mathrm{\β}}_{1}I\right)}^{-1}{(A_{i,k}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}\mathrm{\Γ})}^T\\ +{\mathrm{\β}}_1^{-1}{L}_{i,k}^1{L_{i,k}^1}^T+(1+{\mathrm{\α}}_1^{-1})\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}{\stackrel{\‾}{w}}_{il}{\mathrm{\Γ\Δ}}_{j,k|k}{\mathrm{\Γ}}^T+B_{i,k}{Q}_{i,k}{B}_{i,k}^T\end{array}$

$\begin{array}{c}{\mathrm{\Δ}}_{i,k+1|k+1}=(I-K_{i,k+1}{C}_{i,k+1}){({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{(I-K_{i,k+1}{C}_{i,k+1})}^T\\ +K_{i,k+1}({\mathrm{\β}}_2^{-1}{L}_{i,k}^2{\left(L_{i,k}^2\right)}^T+R_{i,k+1}){K^T}_{i,k+1}\end{array}$

其中，R_i,k+1与Q_i,k代表着噪声的协方差矩阵，Δ_i,k+1|k表示第k时刻对第i个待跟踪车辆的预测估计误差的协方差矩阵；Δ_i,k+1|k+1表示第k+1时刻对第i个待跟踪车辆的更新估计误差的协方差矩阵；C_i,k+1表示第k+1时刻对第i个待跟踪车辆的非线性测量函数的Jacobian矩阵；表示第k时刻对第i个待跟踪车辆的非线性测量函数的线性化误差矩阵；β₂表示一个调节因子；I表示单位矩阵；

S33：获取每一个状态的滤波增益矩阵，以使每一个状态的预测估计误差协方差阵达到最小，并对每一个状态进行实时的修正：

$K_{i,k+1}={({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{C}_{i,k+1}^T{\[C_{i,k+1}{({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{C}_{i,k\mathrm{+1}}^T+({\mathrm{\β}}_2^{-1}{L}_{i,k}^2{\left(L_{i,k}^2\right)}^T+R_{i,k+1})\]}^{-1}$

S34：给定每个状态的初始估计值后，通过上面步骤S31-S33，对每个状态进行实时的递推更新，实现对存在交互性的多个待跟踪车辆的跟踪。

第二方面，本发明还提供了一种交互式多车辆跟踪装置，包括：

建立单元，用于建立多车辆交互的车辆跟踪模型：

$\left\{\begin{array}{c}{x}_{i,k+1}=f\left(x_{i,k}\right)+\underset{j=1,j\≠i}{\overset{N}{\Σ}}{w}_{ij}\mathrm{\Γ}(x_{j,k}-x_{i,k})+B_{i,k}{\stackrel{\‾}{w}}_{i,k}\\ y_{i,k}=h\left(x_{i,k}\right)+v_{i,k}\end{array}\right.$

其中，y_i,k表示第k时刻第i个传感器获得的测量信息；x_i,k+1表示第k+1时刻第i个待跟踪车辆的运动状态参数；f(x)函数与h(x)函数是状态方程与观测方程中的非线性函数；w_ij表示第i个待跟踪车辆与第j个待跟踪车辆之间的交互强度；Γ表示第i个待跟踪车辆与第j个待跟踪车辆之间状态的耦合矩阵；与v_i,k表示第k时刻的系统噪声与观测噪声，B_i,k表示第k时刻系统的适维矩阵；

获取单元，用于利用传感器分别获取每个待跟踪车辆在第k时刻的测量信息，其中，一个传感器对应一个待跟踪车辆；

跟踪单元，用于根据所述获取单元获取的测量信息，以及所述建立单元建立的车辆跟踪模型，利用非扩维方法估计待跟踪车辆的运动状态参数，实现对待跟踪车辆的跟踪。

进一步地，所述跟踪单元，具体用于：

针对每一个状态，获取状态的预测方程和更新方程，并对每个状态进行实时更新：

${\hat{x}}_{i,k+1|k}=f\left({\hat{x}}_{i,k|k}\right)+\underset{j=1,j\≠i}{\overset{N}{\Σ}}{w}_{ij}\mathrm{\Γ}({\hat{x}}_{j,k|k}-{\hat{x}}_{i,k|k})$

${\hat{x}}_{i,k+1|k+1}={\hat{x}}_{i,k+1|k}+K_{i,k+1}(y_{i,k+1}-h({\hat{x}}_{i,k+1|k}\left)\right)$

其中，K_i,k+1表示滤波增益矩阵；表示第k时刻对第i个待跟踪车辆估计的预测值；表示第k+1时刻对第i个待跟踪车辆估计的更新值；表示第k时刻对第i个待跟踪车辆估计的更新值；

通过求解获取每一个状态的预测估计误差协方差矩阵与更新估计误差的协方差矩阵的上界：

$\begin{array}{c}{\mathrm{\Δ}}_{i,k+1|k}=(1+{\mathrm{\α}}_1)(A_{i,k}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}\mathrm{\Γ}){({\mathrm{\Δ}}_{i,k|k}^{-1}-(1+r\left){\mathrm{\β}}_{1}I\right)}^{-1}{(A_{i,k}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}\mathrm{\Γ})}^T\\ +{\mathrm{\β}}_1^{-1}{L}_{i,k}^1{L_{i,k}^1}^T+(1+{\mathrm{\α}}_1^{-1})\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}{\stackrel{\‾}{w}}_{il}{\mathrm{\Γ\Δ}}_{j,k|k}{\mathrm{\Γ}}^T+B_{i,k}{Q}_{i,k}{B}_{i,k}^T\end{array}$

$\begin{array}{c}{\mathrm{\Δ}}_{i,k+1|k+1}=(I-K_{i,k+1}{C}_{i,k+1}){({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{(I-K_{i,k+1}{C}_{i,k+1})}^T\\ +K_{i,k+1}({\mathrm{\β}}_2^{-1}{L}_{i,k}^2{\left(L_{i,k}^2\right)}^T+R_{i,k+1}){K^T}_{i,k+1}\end{array}$

其中，R_i,k+1与Q_i,k代表着噪声的协方差矩阵，Δ_i,k+1|k表示第k时刻对第i个待跟踪车辆的预测估计误差的协方差矩阵；Δ_i,k+1|k+1表示第k+1时刻对第i个待跟踪车辆的更新估计误差的协方差矩阵；C_i,k+1表示第k+1时刻对第i个待跟踪车辆的非线性测量函数的Jacobian矩阵；表示第k时刻对第i个待跟踪车辆的非线性测量函数的线性化误差矩阵；β₂表示一个调节因子；I表示单位矩阵；

获取每一个状态的滤波增益矩阵，以使每一个状态的预测估计误差协方差阵达到最小，并对每一个状态进行实时的修正：

$K_{i,k+1}={({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{C}_{i,k+1}^T{\[C_{i,k+1}{({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{C}_{i,k\mathrm{+1}}^T+({\mathrm{\β}}_2^{-1}{L}_{i,k}^2{\left(L_{i,k}^2\right)}^T+R_{i,k+1})\]}^{-1}$

给定每个状态的初始估计值后，通过上面处理，对每个状态进行实时的递推更新，实现对存在交互性的多个待跟踪车辆的跟踪。

由上述技术方案可知，本发明考虑了现有的车辆追踪方法没有考虑到的交互作用，而交互作用对车辆的跟踪效果具有很大的影响。本发明着重考虑了交互作用的存在对状态产生的影响，解决了模型建模真实性所带来的新难题，满足了多车辆跟踪的要求，且本发明实现起来较为简单。综合起来，本发明填补了在多车辆跟踪这一领域的所存在的缺陷，利用基于状态交互的车辆跟踪模型实现了对多车辆更加准确的跟踪。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例一提供的交互式多车辆跟踪方法的流程图；

图2a-图2b分别为车辆的的跟踪效果图与效果对比图；

图3是本发明实施例二提供的交互式多车辆跟踪装置的结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

对多个车辆的跟踪，不仅要跟踪特定的车辆，对外观类似的车辆进行区分也是很有必要的，这意味着多车辆跟踪算法的成功不仅取决于跟踪器，而且也要对跟踪器进行必要的过滤。进一步地，多车辆跟踪也必须处理车辆隐蔽，即使车辆被其他物体遮蔽或阻断，该车辆也必须被识别跟踪。更进一步的，随着模型的复杂化，状态与状态之间不可避免的存在着交互现象，比如在社交力模型中，不仅存在着以车辆为导向的驱动力，也存在着各种各样的交互情况，比如来自于其他人或物体吸引力，或者是产生厌恶作用的排斥力等，这些交互现象都需要考虑到我们的车辆跟踪中去，所以说交互现象的存在使得不能简单的进行多车辆追踪。为了解决背景技术提到的技术问题，本发明提供了一种交互式多车辆跟踪方法，该方法能够提高车辆跟踪的准确度。

图1示出了本发明实施例一提供的交互式多车辆跟踪方法的流程图，参见图1，所述方法包括如下步骤：

步骤101：建立多车辆交互的车辆跟踪模型：

$\left\{\begin{array}{c}{x}_{i,k+1}=f\left(x_{i,k}\right)+\underset{j=1,j\≠i}{\overset{N}{\Σ}}{w}_{ij}\mathrm{\Γ}(x_{j,k}-x_{i,k})+B_{i,k}{\stackrel{\‾}{w}}_{i,k}\\ y_{i,k}=h\left(x_{i,k}\right)+v_{i,k}\end{array}\right.$

其中，y_i,k表示第k时刻第i个传感器获得的测量信息；x_i,k+1表示第k+1时刻第i个待跟踪车辆的运动状态参数；f(x)函数与h(x)函数是状态方程与观测方程中的非线性函数；w_ij表示第i个待跟踪车辆与第j个待跟踪车辆之间的交互强度；Γ表示第i个待跟踪车辆与第j个待跟踪车辆之间状态的耦合矩阵；与v_i,k表示第k时刻的系统噪声与观测噪声，B_i,k表示第k时刻系统的适维矩阵；Γ可以为单位矩阵，表示目标之间状态的耦合关系。

其中，上述交互强度用于描述车辆之间的跟驰、换道等交互式运动行为。例如可以为(0.1，0.5)。

步骤102：利用传感器分别获取每个待跟踪车辆在第k时刻的测量信息，其中，一个传感器对应一个待跟踪车辆。

在本步骤中，传感器获取的测量信息包括车辆位置、方位角等，其中采用不同的传感器可以获取不同的测量信息。

步骤103：根据步骤102获取的测量信息，以及步骤101建立的车辆跟踪模型，利用非扩维方法估计待跟踪车辆的运动状态参数，实现对待跟踪车辆的跟踪。

由上述技术方案可知，本发明实施例考虑了现有的车辆追踪方法没有考虑到的交互作用，而交互作用对车辆的跟踪效果具有很大的影响。本发明实施例着重考虑了交互作用的存在对状态产生的影响，解决了模型建模真实性所带来的新难题，满足了多车辆跟踪的要求，且本发明实现起来较为简单。综合起来，本发明实施例填补了在多车辆跟踪这一领域的所存在的缺陷，利用基于状态交互的车辆跟踪模型实现了对多车辆更加准确的跟踪。

在本发明其他实施例中，上述步骤103根据步骤102获取的测量信息，以及步骤101建立的车辆跟踪模型，利用非扩维方法估计待跟踪车辆的运动状态参数，实现对待跟踪车辆的跟踪，具体包括：

a：针对每一个状态，获取状态的预测方程和更新方程，并对每个状态进行实时更新：

${\hat{x}}_{i,k+1|k}=f\left({\hat{x}}_{i,k|k}\right)+\underset{j=1,j\≠i}{\overset{N}{\Σ}}{w}_{ij}\mathrm{\Γ}({\hat{x}}_{j,k|k}-{\hat{x}}_{i,k|k})$

${\hat{x}}_{i,k+1|k+1}={\hat{x}}_{i,k+1|k}+K_{i,k+1}(y_{i,k+1}-h({\hat{x}}_{i,k+1|k}\left)\right)$

其中，K_i,k+1表示滤波增益矩阵；表示第k时刻对第i个待跟踪车辆估计的预测值；表示第k+1时刻对第i个待跟踪车辆估计的更新值；表示第k时刻对第i个待跟踪车辆估计的更新值；

b：利用非扩维方法获取每一个状态的预测估计误差协方差矩阵与更新估计误差的协方差矩阵的上界：

$\begin{array}{c}{\mathrm{\Δ}}_{i,k+1|k}=(1+{\mathrm{\α}}_1)(A_{i,k}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}\mathrm{\Γ}){({\mathrm{\Δ}}_{i,k|k}^{-1}-(1+r\left){\mathrm{\β}}_{1}I\right)}^{-1}{(A_{i,k}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}\mathrm{\Γ})}^T\\ +{\mathrm{\β}}_1^{-1}{L}_{i,k}^1{L_{i,k}^1}^T+(1+{\mathrm{\α}}_1^{-1})\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}{\stackrel{\‾}{w}}_{il}{\mathrm{\Γ\Δ}}_{j,k|k}{\mathrm{\Γ}}^T+B_{i,k}{Q}_{i,k}{B}_{i,k}^T\end{array}$

$\begin{array}{c}{\mathrm{\Δ}}_{i,k+1|k+1}=(I-K_{i,k+1}{C}_{i,k+1}){({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{(I-K_{i,k+1}{C}_{i,k+1})}^T\\ +K_{i,k+1}({\mathrm{\β}}_2^{-1}{L}_{i,k}^2{\left(L_{i,k}^2\right)}^T+R_{i,k+1}){K^T}_{i,k+1}\end{array}$

其中，R_i,k+1与Q_i,k代表着噪声的协方差矩阵，Δ_i,k+1|k表示第k时刻对第i个待跟踪车辆的预测估计误差的协方差矩阵；Δ_i,k+1|k+1表示第k+1时刻对第i个待跟踪车辆的更新估计误差的协方差矩阵；C_i,k+1表示第k+1时刻对第i个待跟踪车辆的非线性测量函数的Jacobian矩阵；表示第k时刻对第i个待跟踪车辆的非线性测量函数的线性化误差矩阵；β₂表示一个调节因子；I表示单位矩阵；

c：获取每一个状态的滤波增益矩阵，以使每一个状态的预测估计误差协方差阵达到最小，并对每一个状态进行实时的修正：

$K_{i,k+1}={({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{C}_{i,k+1}^T{\[C_{i,k+1}{({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{C}_{i,k\mathrm{+1}}^T+({\mathrm{\β}}_2^{-1}{L}_{i,k}^2{\left(L_{i,k}^2\right)}^T+R_{i,k+1})\]}^{-1}$

d：给定每个状态的初始估计值后，通过上面步骤a-c，对每个状态进行实时的递推更新，实现对存在交互性的多个待跟踪车辆的跟踪。

从上面可以看出，本发明实施例提供了一种非扩维的方法，即对每个状态进行单独求解，单独进行状态估计。这是因为通用的对车辆进行估计方法大部分是基于扩展kalman滤波，因为自然界中几乎所有的系统都是复杂的非线性的系统，这就不可避免的产生了不确定性因素的存在。由于存在不确定性因素，状态误差协方差真实值的求解几乎是不可能的，这就要求需要寻求误差协方差的一个上界。由于存在着交互作用，使得系统状态之间的关系更加复杂，如果方法不恰当，会导致计算量大大的提高。因此在本步骤中，采用了一种非扩维的方法，对每个状态进行单独求解，单独进行状态估计，具体操作是先通过泰勒级数展开来处理非线性项及高阶项线性化误差的估计，在随机干扰的存在下，用非增广方法设计一个时变状态估计，以保证状态估计误差协方差矩阵存在一个明确的上界，通过求解两个Riccati微分方程，设计滤波器的增益使该上界达到最小，进而实现递归算法。值得注意的是交互作用已经归结到每个状态方程中去了，主要的难点是交互项的处理，在具体处理时，本步骤对交互项进行了放缩处理(步骤b中的两个矩阵Δ_i,k+1|k和Δ_i,k+1|k+1为通过放缩不等式得到的，具体来说就是将交互项的估计误差协方差矩阵用各自状态估计误差的协方差矩阵来代替)，这样虽然使得计算结果有点保守，但是这种方法可以使得每一步的计算矩阵的维数都跟状态的维数相等，即便状态的维数增加也不会增加计算的复杂度，不但降低了整体的计算量，也使得在有故障存在的时候追踪系统也能进行下去。

从上面描述可知，本发明实施例通过递推上面步骤a、b、c得到的等式来实现对状态的追踪，本发明实施例主要的关键点是考虑了其他发明没有考虑的交互作用，因为交互作用是普遍存在的现象，对车辆的跟踪效果也有很大的影响。本发明实施例的另一个关键点是通过非扩维的方法实现了状态估计的递推方程，这种方法使得跟踪算法更加简单，综合起来，本发明实施例填补了在多车辆跟踪这一领域的所存在的缺陷，从而实现对多车辆更加准确的跟踪。

与现有技术相比，本发明实施例提出的技术方案中建立了对存在交互作用的多个车辆的追踪算法，着重考虑了了交互作用的存在对状态产生的影响，解决了模型建模真实性所带来的新难题，基本满足了多车辆跟踪的基本要求，本发明实现简单，达到了应有的要求。

本发明实施例提供的交互多车辆跟踪方法可以应用到很多场景中，例如前面提到的社交力模型，为了对算法的效果进行良好的实验，本发明构造了一个具有四辆车辆的试验场景，系统模型可以用下列状态方程来表示：

$x_{i,k+1}=x_{i,k}+v_{i,k}{\mathrm{cos\θ}}_{i,k}+c\underset{j=1}{\overset{4}{\Σ}}{\mathrm{\γ}}_{ij}(x_{j,k}-x_{i,k})+w_{i,k}^x$

$y_{i,k+1}=y_{i,k}+v_{i,k}{\mathrm{sin\θ}}_{i,k}+c\underset{j=1}{\overset{4}{\Σ}}{\mathrm{\γ}}_{i,k}(y_{j,k}-y_{i,k})+w_{i,k}^y$

${\mathrm{\θ}}_{i,k+1}={\mathrm{\θ}}_{i,k}+{\mathrm{\α}}_{i,k}+c\underset{j=1}{\overset{4}{\Σ}}{\mathrm{\γ}}_{i,k}({\mathrm{\θ}}_{j,k}-{\mathrm{\θ}}_{i,k})+w_{i,k}^{\mathrm{\θ}}$

上述系统模型中，(x_i,k,y_i,k)代表第i辆车的位置，θ_i,k代表第i辆车的方位角，(v_i,k,α_i,k)代表速度矢量，是方差为Q_i,k的零均值白噪声。

通过摄像头来获取视觉测量，其测量方程为：

$p_{i,k}=\frac{{\mathrm{\γ}}_u}{z_f^c}\[-(s_x-x_{i,k}){\mathrm{sin\θ}}_{i,k}+(s_x-y_{i,k}){\mathrm{cos\θ}}_{i,k}-d_2\]+p_0+v_{i,k}^p$

$q_{i,k}=\frac{{\mathrm{\γ}}_v}{z_f^c}\[-(s_y-x_{i,k}){\mathrm{cos\θ}}_{i,k}+(s_y-y_{i,k}){\mathrm{sin\θ}}_{i,k}+d_1\]+q_0+v_{i,k}^q$

(p_i,k,q_i,k)是汽车在图像平面的坐标，(d₁,d₂)是汽车坐标系中的坐标代表从摄像头到车辆上标志点平面的距离，γ_v与γ_u代表像素放大因素，(s_x,s_y)代表了在世界坐标系中的特征坐标。是方差R_i,k为的零均值高斯白噪声。

本实验采用了如下仿真参数：

d₁＝-0.0688,d₂＝0.0536,γ_u＝902.13283,γ_v＝902.50141,u₀＝347.20436

v₀＝284.34705,c＝8×10^-3,λ_ij＝0.5(i＝1,2,3,4),Q_i,k＝diag{0.01,0.01,0.01}

R_i,k＝diag{25²,25²}(i＝1,2,3,4),β₁＝β₂＝0.01

通过实验，可以得到每辆汽车的跟踪轨迹图，如图2a所示。为了说明本发明的跟踪性能，我们将非交互式与交互式耦合的跟踪算法进行对比，并且通过均方根误差来衡量跟踪的效果，对比效果图如图2b所示，可以很明显的看出，考虑了耦合的效果明显比没有考虑耦合的效果好，这也证明了本发明的有效性。

本发明实施例二提供了一种交互式多车辆跟踪装置，参见图3，包括：建立单元31、获取单元32和跟踪单元33；其中，

建立单元31，用于建立多车辆交互的车辆跟踪模型：

$\left\{\begin{array}{c}{x}_{i,k+1}=f\left(x_{i,k}\right)+\underset{j=1,j\≠i}{\overset{N}{\Σ}}{w}_{ij}\mathrm{\Γ}(x_{j,k}-x_{i,k})+B_{i,k}{\stackrel{\‾}{w}}_{i,k}\\ y_{i,k}=h\left(x_{i,k}\right)+v_{i,k}\end{array}\right.$

其中，y_i,k表示第k时刻第i个传感器获得的测量信息；x_i,k+1表示第k+1时刻第i个待跟踪车辆的运动状态参数；f(x)函数与h(x)函数是状态方程与观测方程中的非线性函数；w_ij表示第i个待跟踪车辆与第j个待跟踪车辆之间的交互强度；Γ表示第i个待跟踪车辆与第j个待跟踪车辆之间状态的耦合矩阵；与v_i,k表示第k时刻的系统噪声与观测噪声，B_i,k表示第k时刻系统的适维矩阵；

获取单元32，用于利用传感器分别获取每个待跟踪车辆在第k时刻的测量信息，其中，一个传感器对应一个待跟踪车辆；

跟踪单元33，用于根据所述获取单元32获取的测量信息，以及所述建立单元31建立的车辆跟踪模型，利用非扩维方法估计待跟踪车辆的运动状态参数，实现对待跟踪车辆的跟踪。

优选地，所述跟踪单元33，具体用于：

针对每一个状态，获取状态的预测方程和更新方程，并对每个状态进行实时更新：

${\hat{x}}_{i,k+1|k}=f\left({\hat{x}}_{i,k|k}\right)+\underset{j=1,j\≠i}{\overset{N}{\Σ}}{w}_{ij}\mathrm{\Γ}({\hat{x}}_{j,k|k}-{\hat{x}}_{i,k|k})$

${\hat{x}}_{i,k+1|k+1}={\hat{x}}_{i,k+1|k}+K_{i,k+1}(y_{i,k+1}-h({\hat{x}}_{i,k+1|k}\left)\right)$

其中，K_i,k+1表示滤波增益矩阵；表示第k时刻对第i个待跟踪车辆估计的预测值；表示第k+1时刻对第i个待跟踪车辆估计的更新值；表示第k时刻对第i个待跟踪车辆估计的更新值；

利用非扩维方法获取每一个状态的预测估计误差协方差矩阵与更新估计误差的协方差矩阵的上界：

$\begin{array}{c}{\mathrm{\Δ}}_{i,k+1|k}=(1+{\mathrm{\α}}_1)(A_{i,k}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}\mathrm{\Γ}){({\mathrm{\Δ}}_{i,k|k}^{-1}-(1+r\left){\mathrm{\β}}_{1}I\right)}^{-1}{(A_{i,k}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}\mathrm{\Γ})}^T\\ +{\mathrm{\β}}_1^{-1}{L}_{i,k}^1{L_{i,k}^1}^T+(1+{\mathrm{\α}}_1^{-1})\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}{\stackrel{\‾}{w}}_{il}{\mathrm{\Γ\Δ}}_{j,k|k}{\mathrm{\Γ}}^T+B_{i,k}{Q}_{i,k}{B}_{i,k}^T\end{array}$

$\begin{array}{c}{\mathrm{\Δ}}_{i,k+1|k+1}=(I-K_{i,k+1}{C}_{i,k+1}){({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{(I-K_{i,k+1}{C}_{i,k+1})}^T\\ +K_{i,k+1}({\mathrm{\β}}_2^{-1}{L}_{i,k}^2{\left(L_{i,k}^2\right)}^T+R_{i,k+1}){K^T}_{i,k+1}\end{array}$

其中，R_i,k+1与Q_i,k代表着噪声的协方差矩阵，Δ_i,k+1|k表示第k时刻对第i个待跟踪车辆的预测估计误差的协方差矩阵；Δ_i,k+1|k+1表示第k+1时刻对第i个待跟踪车辆的更新估计误差的协方差矩阵；C_i,k+1表示第k+1时刻对第i个待跟踪车辆的非线性测量函数的Jacobian矩阵；表示第k时刻对第i个待跟踪车辆的非线性测量函数的线性化误差矩阵；β₂表示一个调节因子；I表示单位矩阵；

获取每一个状态的滤波增益矩阵，以使每一个状态的预测估计误差协方差阵达到最小，并对每一个状态进行实时的修正：

$K_{i,k+1}={({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{C}_{i,k+1}^T{\[C_{i,k+1}{({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{C}_{i,k\mathrm{+1}}^T+({\mathrm{\β}}_2^{-1}{L}_{i,k}^2{\left(L_{i,k}^2\right)}^T+R_{i,k+1})\]}^{-1}$

给定每个状态的初始估计值后，通过上面处理，对每个状态进行实时的递推更新，实现对存在交互性的多个待跟踪车辆的跟踪。

本实施例提供的交互式多车辆跟踪装置，可以用于执行上述实施例所述的交互式多车辆跟踪方法，其原理和技术效果类似，此处不再赘述。

在本发明的描述中，需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上实施例仅用于说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (4)

1.一种交互式多车辆跟踪方法，其特征在于，包括以下步骤：

S1：建立多车辆交互的车辆跟踪模型：

$\left\{\begin{array}{c}{x}_{i,k+1}=f\left(x_{i,k}\right)+\underset{j=1,j\≠i}{\overset{N}{\Σ}}{w}_{ij}\mathrm{\Γ}(x_{j,k}-x_{i,k})+B_{i,k}{\stackrel{\‾}{w}}_{i,k}\\ y_{i,k}=h\left(x_{i,k}\right)+v_{i,k}\end{array}\right.$

其中，y_i,k表示第k时刻第i个传感器获得的测量信息；x_i,k+1表示第k+1时刻第i个待跟踪车辆的运动状态参数；f(x)函数与h(x)函数是状态方程与观测方程中的非线性函数；w_ij表示第i个待跟踪车辆与第j个待跟踪车辆之间的交互强度；Γ表示第i个待跟踪车辆与第j个待跟踪车辆之间状态的耦合矩阵；与v_i,k表示第k时刻的系统噪声与观测噪声，B_i,k表示第k时刻系统的适维矩阵；

S2、利用传感器分别获取每个待跟踪车辆在第k时刻的测量信息，其中，一个传感器对应一个待跟踪车辆；

S3、根据步骤S2获取的测量信息，以及步骤S1建立的车辆跟踪模型，利用非扩维方法估计待跟踪车辆的运动状态参数，实现对待跟踪车辆的跟踪。

2.根据权利要求1所述的方法，其特征在于，所述S3根据步骤S2获取的测量信息，以及步骤S1建立的车辆跟踪模型，利用非扩维方法估计待跟踪车辆的运动状态参数，实现对待跟踪车辆的跟踪，包括：

S31：针对每一个状态，获取状态的预测方程和更新方程，并对每个状态进行实时更新：

${\hat{x}}_{i,k+1|k}=f\left({\hat{x}}_{i,k|k}\right)+\underset{j=1,j\≠i}{\overset{N}{\Σ}}{w}_{ij}\mathrm{\Γ}({\hat{x}}_{j,k|k}-{\hat{x}}_{i,k|k})$

${\hat{x}}_{i,k+1|k+1}={\hat{x}}_{i,k+1|k}+K_{i,k+1}(y_{i,k+1}-h({\hat{x}}_{i,k+1|k}\left)\right)$

其中，K_i,k+1表示滤波增益矩阵；表示第k时刻对第i个待跟踪车辆估计的预测值；表示第k+1时刻对第i个待跟踪车辆估计的更新值；表示第k时刻对第i个待跟踪车辆估计的更新值；

S32：利用非扩维方法获取每一个状态的预测估计误差协方差矩阵与更新估计误差的协方差矩阵的上界：

$\begin{array}{c}{\mathrm{\Δ}}_{i,k+1|k}=(1+{\mathrm{\α}}_1)(A_{i,k}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}\mathrm{\Γ}){({\mathrm{\Δ}}_{i,k|k}^{-1}-(1+r\left){\mathrm{\β}}_{1}I\right)}^{-1}{(A_{i,k}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}\mathrm{\Γ})}^T\\ ={\mathrm{\β}}_1^{-1}{L}_{i,k}^1{L_{i,k}^1}^T+(1+{\mathrm{\α}}_1^{-1})\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}{\stackrel{\‾}{w}}_{il}{\mathrm{\Γ\Δ}}_{j,k|k}{\mathrm{\Γ}}^T+B_{i,k}{Q}_{i,k}{B}_{i,k}^T\end{array}$

$\begin{array}{c}{\mathrm{\Δ}}_{i,k+1|k+1}=(I-K_{i,k+1}{C}_{i,k+1}){({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{(I-K_{i,k+1}{C}_{i,k+1})}^T\\ +K_{i,k+1}({\mathrm{\β}}_2^{-1}{L}_{i,k}^2{\left(L_{i,k}^2\right)}^T+R_{i,k+1}){K^T}_{i,k+1}\end{array}$

其中，R_i,k+1与Q_i,k代表着噪声的协方差矩阵，Δ_i,k+1|k表示第k时刻对第i个待跟踪车辆的预测估计误差的协方差矩阵；Δ_i,k+1|k+1表示第k+1时刻对第i个待跟踪车辆的更新估计误差的协方差矩阵；C_i,k+1表示第k+1时刻对第i个待跟踪车辆的非线性测量函数的Jacobian矩阵；表示第k时刻对第i个待跟踪车辆的非线性测量函数的线性化误差矩阵；β₂表示一个调节因子；I表示单位矩阵；

S33：获取每一个状态的滤波增益矩阵，以使每一个状态的预测估计误差协方差阵达到最小，并对每一个状态进行实时的修正：

$K_{i,k+1}={({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{C}_{i,k+1}^T{\[C_{i,k+1}{({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{C}_{i,k\mathrm{+1}}^T+({\mathrm{\β}}_2^{-1}{L}_{i,k}^2{\left(L_{i,k}^2\right)}^T+R_{i,k+1})\]}^{-1}$

S34：给定每个状态的初始估计值后，通过上面步骤S31-S33，对每个状态进行实时的递推更新，实现对存在交互性的多个待跟踪车辆的跟踪。

3.一种交互式多车辆跟踪装置，其特征在于，包括：

建立单元，用于建立多车辆交互的车辆跟踪模型：

$\left\{\begin{array}{c}{x}_{i,k+1}=f\left(x_{i,k}\right)+\underset{j=1,j\≠i}{\overset{N}{\Σ}}{w}_{ij}\mathrm{\Γ}(x_{j,k}-x_{i,k})+B_{i,k}{\stackrel{\‾}{w}}_{i,k}\\ y_{i,k}=h\left(x_{i,k}\right)+v_{i,k}\end{array}\right.$

其中，y_i,k表示第k时刻第i个传感器获得的测量信息；x_i,k+1表示第k+1时刻第i个待跟踪车辆的运动状态参数；f(x)函数与h(x)函数是状态方程与观测方程中的非线性函数；w_ij表示第i个待跟踪车辆与第j个待跟踪车辆之间的交互强度；Γ表示第i个待跟踪车辆与第j个待跟踪车辆之间状态的耦合矩阵；与v_i,k表示第k时刻的系统噪声与观测噪声，B_i,k表示第k时刻系统的适维矩阵；

获取单元，用于利用传感器分别获取每个待跟踪车辆在第k时刻的测量信息，其中，一个传感器对应一个待跟踪车辆；

跟踪单元，用于根据所述获取单元获取的测量信息，以及所述建立单元建立的车辆跟踪模型，利用非扩维方法估计待跟踪车辆的运动状态参数，实现对待跟踪车辆的跟踪。

4.根据权利要求3所述的装置，其特征在于，所述跟踪单元，具体用于：

针对每一个状态，获取状态的预测方程和更新方程，并对每个状态进行实时更新：

${\hat{x}}_{i,k+1|k}=f\left({\hat{x}}_{i,k|k}\right)+\underset{j=1,j\≠i}{\overset{N}{\Σ}}{w}_{ij}\mathrm{\Γ}({\hat{x}}_{j,k|k}-{\hat{x}}_{i,k|k})$

${\hat{x}}_{i,k+1|k+1}={\hat{x}}_{i,k+1|k}+K_{i,k+1}(y_{i,k+1}-h({\hat{x}}_{i,k+1|k}\left)\right)$

其中，K_i,k+1表示滤波增益矩阵；表示第k时刻对第i个待跟踪车辆估计的预测值；表示第k+1时刻对第i个待跟踪车辆估计的更新值；表示第k时刻对第i个待跟踪车辆估计的更新值；

通过求解获取每一个状态的预测估计误差协方差矩阵与更新估计误差的协方差矩阵的上界：

$\begin{array}{c}{\mathrm{\Δ}}_{i,k+1|k}=(1+{\mathrm{\α}}_1)(A_{i,k}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}\mathrm{\Γ}){({\mathrm{\Δ}}_{i,k|k}^{-1}-(1+r\left){\mathrm{\β}}_{1}I\right)}^{-1}{(A_{i,k}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}\mathrm{\Γ})}^T\\ ={\mathrm{\β}}_1^{-1}{L}_{i,k}^1{L_{i,k}^1}^T+(1+{\mathrm{\α}}_1^{-1})\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{w}_{ij}{\stackrel{\‾}{w}}_{il}{\mathrm{\Γ\Δ}}_{j,k|k}{\mathrm{\Γ}}^T+B_{i,k}{Q}_{i,k}{B}_{i,k}^T\end{array}$

$\begin{array}{c}{\mathrm{\Δ}}_{i,k+1|k+1}=(I-K_{i,k+1}{C}_{i,k+1}){({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{(I-K_{i,k+1}{C}_{i,k+1})}^T\\ +K_{i,k+1}({\mathrm{\β}}_2^{-1}{L}_{i,k}^2{\left(L_{i,k}^2\right)}^T+R_{i,k+1}){K^T}_{i,k+1}\end{array}$

其中，R_i,k+1与Q_i,k代表着噪声的协方差矩阵，Δ_i,k+1|k表示第k时刻对第i个待跟踪车辆的预测估计误差的协方差矩阵；Δ_i,k+1|k+1表示第k+1时刻对第i个待跟踪车辆的更新估计误差的协方差矩阵；C_i,k+1表示第k+1时刻对第i个待跟踪车辆的非线性测量函数的Jacobian矩阵；表示第k时刻对第i个待跟踪车辆的非线性测量函数的线性化误差矩阵；β₂表示一个调节因子；I表示单位矩阵；

获取每一个状态的滤波增益矩阵，以使每一个状态的预测估计误差协方差阵达到最小，并对每一个状态进行实时的修正：

$K_{i,k+1}={({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{C}_{i,k+1}^T{\[C_{i,k+1}{({{\mathrm{\Δ}}^{-1}}_{i,k+1|k}-{\mathrm{\β}}_{2}I)}^{-1}{C}_{i,k\mathrm{+1}}^T+({\mathrm{\β}}_2^{-1}{L}_{i,k}^2{\left(L_{i,k}^2\right)}^T+R_{i,k+1})\]}^{-1}$

给定每个状态的初始估计值后，通过上面处理，对每个状态进行实时的递推更新，实现对存在交互性的多个待跟踪车辆的跟踪。