CN106093951A - 基于超声波传感器阵列的物体跟踪方法 - Google Patents

Abstract

本发明提供一种基于超声波传感器阵列的物体跟踪方法，包括：步骤1、建立l阶的系统状态变量拟合目标物的系统状态；步骤2、根据目标物运动特点建立运动状态方程，根据传感器测距方法建立观测方程；步骤3、利用跟踪滤波算法对目标物进行跟踪，对于运动或观测方程存在非线性化部分的系统，可利用能够处理非线性系统的滤波器如扩展卡尔曼滤波器、无迹卡尔曼滤波器、粒子滤波器等进行目标物跟踪。

Description

基于超声波传感器阵列的物体跟踪方法

技术领域

本发明涉及传感器技术领域，特别是指一种基于超声波传感器阵列的物体跟踪方法。

背景技术

传感器技术从出现以来已经成为了一切控制机械的“眼睛”，而一切智能控制都需要依靠传感器技术，是先进驾驶辅助系统(Advanced Automotive Driving System，ADAS)、智能汽车和无人驾驶汽车的基础。目前，环境感知的主要方法是通过顶置旋转激光雷达、激光雷达、毫米波雷达、摄像头、超声波传感器等传感器系统对环境进行感知，尤其是对环境中的静态和动态目标物进行定位和跟踪。

顶置旋转激光雷达、激光雷达、毫米波雷达探测范围大，普遍使用在目前各公司开发的无人驾驶系统中，但缺点是成本非常高，难以实现在普通乘用车中的大规模应用。尤其是旋转激光雷达，谷歌无人驾驶汽车使用的激光雷达由Velodyne生产，早期原型车中使用的版本价格高达7万美元，而目前使用版本的价格也在8000-30000美元。摄像头价格便宜，探测范围较大，主要的技术难点在于后期算法，由于图像处理算法复杂，对运行算法的计算机性能要求非常高。除此之外通过摄像头进行环境感知容易受到光照等环境因素的影响。

超声波传感器成本非常低，具备在乘用车中进行大规模推广的潜力。缺点是探测距离较短，目前使用的长距离超声波传感器能够达到5m的探测距离，已经可以满足汽车周围近距离的环境感知。目前市场中已有的基于超声波传感器的车辆周围环境感知主要应用集中于泊车辅助系统、自动泊车系统和盲区预警系统。已有的算法存在以下两个主要问题：

1)多数系统采用三角定位法进行目标物的定位，传感器误差的存在会对三角定位法的定位造成距离和角度上的偏差，其中间距为50cm的双传感器三角定位法定位中，0.5％的传感器误差可能造成0.65％的距离误差和高达25％的角度误差。

2)多数系统仅根据目标物最近点的距离信号是否到达预警阈值判断是否提供预警。系统仅获得目标物最近点的距离信号，无法获得目标物的速度信息，无法区分静态目标物和动态目标物，进而导致盲区例如预警系统中的误警情况的出现。

发明内容

针对现有技术中存在的基于超声波传感器阵列进行物体跟踪存在的问题，本发明要解决的技术问题是提供一种效果更好的基于超声波传感器阵列的物体跟踪方法。

为了解决上述问题，本发明实施例提出了一种基于n个传感器的超声波传感器阵列的物体跟踪方法，包括：

步骤1、建立l阶的系统状态变量拟合目标物的系统状态；

步骤2、根据目标物运动特点建立运动状态方程，根据传感器测距方程建立观测方程；

X(k)＝f(X(k-1))+W(k-1)

Z(k)＝h(X(k))+V(k)

其中，X(k)为系统状态变量X(k)∈R^l，W(k)为系统过程噪声，过程噪声协方差矩阵为Q。Z(k)为观测量Z(k)∈Rⁿ，V(k)为观测噪声，观测噪声协方差矩阵为R。函数f(·)为系统转移方程，函数h(·)为观测方程。

步骤3、利用跟踪滤波算法对目标物进行跟踪，对于运动或观测方程存在非线性化部分的系统，可利用能够处理非线性系统的滤波器如扩展卡尔曼滤波器、无迹卡尔曼滤波器、粒子滤波器等进行目标物跟踪。其中，所述步骤1具体包括：

步骤11、分析应用场景中目标物在二维平面内的在传感器阵列坐标系中的系统状态特点，进行系统状态归类。可归类为相对静止状态、匀速状态、匀加速或匀减速状态等。

步骤12、根据系统状态类型，建立合适的系统状态变量拟合目标物系统状态。应用场景中，若目标物系统状态可近似认为是静止状态，则可利用四阶(l＝4)的系统状态变量拟合目标物系统状态，系统状态变量如下：

X_k＝[x,y,v_x,v_y]^T

其中，x,y为目标物在x,y坐标系中位置的横纵坐标值，v_x,v_y为目标物在x方向与y方向的速度，由于目标物系统状态可近似认为是静止状态，v_x,v_y可认为是高斯分布的随机误差值。T表示矩阵的转置。

若目标物系统状态可近似认为是匀速运动状态，则可利用六阶(l＝6)的系统状态变量拟合目标物系统状态，系统状态变量如下：

X_k＝[x,y,v_x,v_y,a_x,a_y]^T

其中，x,y为目标物在x,y坐标系中位置的横纵坐标值，v_x,v_y为目标物在x方向与y方向的速度，a_x,a_y为目标物在x方向与y方向的加速度，由于目标物系统状态可近似认为是匀速运动状态，a_x,a_y可认为是高斯分布的随机误差值。

若目标物系统状态可近似认为是匀加速或匀减速运动状态，则可利用八阶(l＝8)的系统状态变量拟合目标物系统状态，系统状态变量如下：

其中，x,y为目标物在x,y坐标系中位置的横纵坐标值，v_x,v_y为目标物在x方向与y方向的速度，a_x,a_y为目标物在x方向与y方向的加速度，为目标物在x方向与y方向的加加速度，由于目标物系统状态可近似认为是匀加速或匀减速运动状态，可认为是高斯分布的随机误差值。

其中，所述步骤2具体包括：

步骤21、物体系统的状态转移过程可以描述为一个离散时间的随机过程。根据目标物运动特点建立运动状态方程，对于二维平面内做线性运动物体，首先获取离散运动转移矩阵。为l阶系统状态变量建立l×l阶运动状态转移矩阵，具体如下：

$A_{4\×4}=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

$A_{6\×6}=\left[\begin{array}{cccccc}1& 0& T& 0& T^2/2& 0\\ 0& 1& 0& T& 0& T^2/2\\ 0& 0& 1& 0& T& 0\\ 0& 0& 0& 1& 0& T\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

$A_{8\×8}=\left[\begin{array}{cccccccc}1& 0& T& 0& T^2/2& 0& T^3/6& 0\\ 0& 1& 0& T& 0& T^2/2& 0& T^3/6\\ 0& 0& 1& 0& T& 0& T^2/2& 0\\ 0& 0& 0& 1& 0& T& 0& T^2/2\\ 0& 0& 0& 0& 1& 0& T& 0\\ 0& 0& 0& 0& 0& 1& 0& T\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$

其中T为目标物跟踪的跟踪周期，即超声波传感器阵列系统的信号采集周期；

步骤22、基于运动状态转移矩阵建立运动状态方程。对于二维平面内做线性运动物体，可建立线性运动状态方程：

X(k)＝AX(k-1)+W(k-1)

其中X(k)为系统状态变量，X(k)∈R^l，W(k)为系统过程噪声，A为状态转移矩阵。

步骤23、根据传感器测距方程建立观测矩阵。根据超声波传感器的测距原理，超声波探测传感器到目标点的距离。对于同步信号系统及异步信号系统中自发自收的传感器，传感器i的测量值为：

$h_i\left(X\right)=\sqrt{{(x_i-x)}^2+{(y_i-y)}^2}$

其中(x_i,y_i),i＝1,…,n为传感器i位置，(x,y)为目标物超声波反射点位置，即为目标物跟踪预测位置X(k)的前两项。

对于异步信号系统中，若存在配对检测，及某一传感器收到的是另一传感器发射的超声波，检测距离为超声波从另一传感器到目标物反射点到该传感器的距离。则传感器i的测量方程为：

$\begin{array}{c}{h}_i\left(X\right)=\mathrm{\γ}\×(\sqrt{{(x_i-x)}^2+{(y_i-y)}^2}\\ +\sqrt{{(x_j-x)}^2+{(y_j-y)}^2})\end{array}$

其中(x_i,y_i),i＝1,…,n为传感器i位置，(x_j,y_j),j＝1,…,n,j≠i为传感器j位置，(x,y)为目标物超声波反射点位置，即为目标物跟踪预测位置X(k)的前两项。γ为参数，由系统设计决定，若记录超声波传播距离长度，则γ＝1，若记录超声波传播距离长度的一半，则γ＝0.5。

基于每个传感器的测量方程，结合传感器的发射时序设计，可建立观测矩阵，观测矩阵为：

h(X)＝[h₁(X),…,h_i(X),…,h_n(X)]^T

如果传感器处于未激活状态或无读数，则h_i(X)为零矩阵。

步骤24、基于观测矩阵建立观测方程。观测方程具体为：

Z(k)＝h(X(k))+V(k)

其中，Z(k)为观测量，Z(k)∈Rⁿ，V(k)为观测噪声，函数h(·)为观测方程。对于每一个传感器，观测方程具体为：

Z_i(k)＝h_i(X(k))+V_i(k)

其中，Z_i(k)为传感器i的观测量，V_i(k)为传感器i的观测噪声，函数h_i(·)为传感器i的观测方程。

其中，所述步骤3具体包括：

步骤31、观测矩阵线性化。二维平面内超声波传感器对目标物距离的测量，即观测矩阵具有非线性成分。部分滤波器如粒子滤波器等具有直接处理非线性系统的能力，可跳过次步骤直接进入步骤32。对于无法直接处理非线性系统的滤波器，如扩展卡尔曼滤波器、无迹卡尔曼滤波器等，需要对观测矩阵进行线性化处理。扩展卡尔曼滤波器，采用的是高阶项采取忽略或逼近措施对非线性环节线性化。利用泰勒二次截断对观测方程进行线性化处理。具体公式如下：

$H_i\left(k\right)=\frac{\∂h_i\left(X\right)}{\∂X}$

其中，H_i(k)为线性化后的观测矩阵。

无迹卡尔曼滤波器采用采用近似方法，对非线性系统进行线性化。常用的采样策略包括对称采样、单形采样、三阶矩偏度采样以及高斯分布4阶矩对称采样等。

步骤32、目标物进入跟踪区域判断与状态初始化。根据传统阵列传感器的读数判断目标物进入跟踪区域，如最靠近阵列端点的两个传感器先后获得有效的距离信息，则可判断目标物进行阵列传感器的跟踪区域。初始状态的计算基于判断目标物进入跟踪区域时传感器的读数计算获得，其他参数的初始值可根据传感器的特性相应设计。

步骤33、对具有有效数据的传感器信息进行多传感器信息融合。其中，扩展卡尔曼滤波器多传感器信息融合的方式由多种选择，包括集中式、顺序式和分布式。三种方法的算法原理如图1所示。滤波器如无迹卡尔曼滤波器、粒子滤波器等可采用集中式数据融合方法进行。有效数据集中式信息融合算法选取具有有效读数的传感器的读数值、相应的观测矩阵与线性化后的观测转移矩阵组成新的测量值矩阵和观测转移矩阵。

$Z_s\left(k\right)={\[Z_{s_1}\left(k\right),\mathrm{...},Z_{s_m}\left(k\right)\]}^T$

$h_s\left(k\right)={\[h_{s_1}\left(k\right),\mathrm{...},h_{s_m}\left(k\right)\]}^T$

$H_s\left(k\right)={\[H_{s_1}\left(k\right),\mathrm{...},H_{s_m}\left(k\right)\]}^T$

其中，s₁,…,s_m为m个有有效读数输出的传感器。

步骤34、目标物系统状态预测与更新。根据滤波器的跟踪原理对利用实时传感器数据对目标物进行系统状态的预测与更新。根据运动转移方程目标物系统状态的先验概率估计，根据传感器的测量值对目标物的系统状态进行更新，获得后验概率估计。应用步骤33中获得的新的测量值矩阵和观测转移矩阵：

a.预测

$\hat{X}(k+1|k)=A\hat{X}\left(k\right|k)$

P(k+1|k)＝AP(k|k)A^T+Q

b.更新

K(k+1)＝P(k+1|k)H_s(k+1)^T[H_s(k+1)P(k+1|k)H_s(k+1)^T+R_s]^T

$\begin{array}{c}\hat{X}(k+1|k+1)\\ =\hat{X}(k+1|k)+K(k+1)\left(Z_s\right(k+1)\\ -h_s\left(\hat{X}\left(k+1|k\right)\right))\end{array}$

P(k+1|k+1)＝[I-K(k+1)H_s(k+1)^T]P(k+1|k)

其中，表示k时刻的先验概率估计，表示k时刻的后验概率估计。R_s为m阶矩阵，对角线值为1²，其余值为0。对于UKF算法，预测过程为对每一个采样点位置的预测，误差协方差矩阵为每个采样点误差协方差的加权。更新过程中，误差协方差均由采样点协方差加权获得。

本发明的上述技术方案的有益效果如下：

1)采用阵列式的设计和布置，扩展单个超声波传感器的探测范围。

2)通过阵列中多传感器信息融合的方式，减小单个传感器误差引起的目标物的定位误差。

3)可以实现对目标物动态的跟踪，跟踪的系统状态信息可包括位置、速度、加速度等。获得目标物的速度信息，不但能够有效的区分静态目标物和动态目标物，减少驾驶辅助系统中误警的产生，也可以提高自车感知周围目标物的能力。

附图说明

图1为三种多传感器信息融合跟踪算法原理；

图1a为集中式数据融合跟踪原理；

图1b为顺序式数据融合跟踪原理；

图1c为分布式数据融合跟踪原理；

图2为一种8传感器等距线性阵列设计，及其车辆上的安装布置形式与坐标系设计；

图3a和图3b为车辆侧向目标物跟踪实例的两种典型跟踪场景；

图4a和图4b为针对图3a、图3b中两种典型场景的有效信号筛选方法。

具体实施方式

为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。

本发明采用跟踪滤波器，如扩展卡尔曼滤波器(Extended Karman Filter，EKF)、无迹卡尔曼滤波器(Unscented Karman Filter，UKF)、粒子滤波器(Particle Filter，PF)等，通过多传感器的信息融合对车辆周围的目标物进行跟踪以获得目标物的位置和速度信号等，实现车辆周围超声波传感器可探测距离内的环境感知，为汽车的先进驾驶辅助系统和自动驾驶的环境感知提供数据基础。

为了实现上述目的，本发明实施例具有以下改进点：

1、重新设计了采用阵列式超声波传感器布置；

2、重新设计了阵列式传感器系统的收发时序，可采用同步发射时序，亦可以采用异步发射时序设计；

3、针对阵列式超声波传感器的布置设计，和收发时序设计，设计相对应的跟踪滤波器跟踪算法。

下面结合附图和某一场景应用实例对本发明进行详细的描述。实例中采用如图2所示的，8个传感器等距直线型阵列设计，布置在车辆侧面，实例中传感器间距s＝50cm，传感器坐标系如图2所示。实例中传感器阵列采用同步时序控制，即每一发射周期，所有传感器同时发射。实例跟踪场景选取车辆侧向目标物跟踪，选取车辆侧向环境中典型的两种目标物进行跟踪，跟踪场景如图3a和图3b所示。在两种跟踪场景中，实例参数选取自车速度5km/h，柱状目标物静止，车辆目标物10km/h，目标物在y轴方向距离汽车2m。

针对上述特定的阵列设计与应用场景实例，可以采用如下方法进行物体跟踪，以扩展卡尔曼滤波器为例，选取集中式数据融合方法：

步骤1、在跟踪场景1中，杆状目标物处于静止状态，考虑汽车在正常行驶过程中的行驶状态，一般分为匀速行驶，加速行驶与减速行驶状态，从较长的行驶时间来看，可认为正常行驶的车辆在进行加速度为恒值，即加加速度为0的运动。在跟踪场景2中，场景主要代表正常行驶过程中，其他车辆相对于自车的超车过程。超车过程中，一般来说相对于匀速行驶的被超车车辆，主动超车车辆采取加速超车或匀速超车的超车策略。因此，主动超车车辆相对于被超车车辆的运动方程可认为是匀速或匀加速运动。加速度的变化率即加加速度可认为是恒值，及加加速度为0的运动。考虑加加速度受到一个系统随机干扰量，故对于跟踪场景1与跟踪场景2，在二维运动平面内，均可采用八阶系统状态变量表示目标物系统状态，系统状态变量具体如下：

步骤2、建立目标物运动转移矩阵，具体如下：

$A=\left[\begin{array}{cccccccc}1& 0& T& 0& T^2/2& 0& T^3/6& 0\\ 0& 1& 0& T& 0& T^2/2& 0& T^3/6\\ 0& 0& 1& 0& T& 0& T^2/2& 0\\ 0& 0& 0& 1& 0& T& 0& T^2/2\\ 0& 0& 0& 0& 1& 0& T& 0\\ 0& 0& 0& 0& 0& 1& 0& T\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$

其中T为目标物跟踪的跟踪周期，即超声波传感器阵列系统的信号采集周期；

基于运动状态转移矩阵建立运动状态方程。对于二维平面内做线性运动物体，可建立线性运动状态方程：

X(k)＝AX(k-1)+W(k-1)

其中X(k)为系统状态变量，W(k)为系统过程噪声，A为状态转移矩阵。

根据传感器测距方程建立观测矩阵。传感器i的测量值为：

$h_i\left(X\right)=\sqrt{{(x_i-x)}^2+{(y_i-y)}^2}$

其中(x_i,y_i),i＝1,…,j为传感器位置，(x,y)为目标物超声波反射点位置，即为目标物跟踪预测位置X_k的前两项。建立每个传感器的观测方为：

Z_i(k)＝h_i(X(k))+V_i(k)

其中，Z_i(k)为传感器i的观测量，V_i(k)为传感器i的观测噪声，函数h_i(·)为传感器i的观测方程。

步骤3、利用泰勒二次截断对观测方程进行线性化处理。具体公式如下：

$H_i\left(k\right)=\frac{\∂h_i\left(X\right)}{\∂X}$

其中，H_i(k)为线性化后的观测矩阵。

基于最靠近阵列端点的两个传感器先后获得有效的距离信息，判断目标物进行阵列传感器的跟踪区域，并开始实施跟踪。初始系统状态中，目标物初始位置利用该时刻两个传感器的读数利用三角定位法计算获得，初始速度传感器间距与这两个传感器第一次获得有效读数的时间差坐商获得，其他状态参数初始值设为0。其他参数的初始值根据传感器的特性相应设计，根据本实例采用的传感器特性，不同传感器之间读数无相关性，R值如下：

$R=\left[\begin{array}{cccccccc}{1}^2& 0& 0& 0& 0& 0& 0& 0\\ 0& 1^2& 0& 0& 0& 0& 0& 0\\ 0& 0& 1^2& 0& 0& 0& 0& 0\\ 0& 0& 0& 1^2& 0& 0& 0& 0\\ 0& 0& 0& 0& 1^2& 0& 0& 0\\ 0& 0& 0& 0& 0& 1^2& 0& 0\\ 0& 0& 0& 0& 0& 0& 1^2& 0\\ 0& 0& 0& 0& 0& 0& 0& 1^2\end{array}\right]$

Q为反应系统受到干扰的大小，考虑到目标物相对于自车做近似与自车平行的运动，故其在x方向的误差大于在y方向的误差。本实例Q值如下：

$Q=\left[\begin{array}{cccccccc}10& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 10& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 100& 0& 0& 0\\ 0& 0& 0& 0& 0& 10& 0& 0\\ 0& 0& 0& 0& 0& 0& 1000& 0\\ 0& 0& 0& 0& 0& 0& 0& 1000\end{array}\right]$

初始状态估计协方差P(0)＝Q。

有效数据的筛选方法为：在对柱状物的跟踪中，由于柱状物尺寸相对于传感器阵列较小，可近似认为所有传感器的测量值形成的反射点为同一点，即所有传感器的信号为有效信号。在对车辆目标物的跟踪中，由于车辆的尺寸相对于传感器阵列量级上具有可比性，随着目标物逐渐进入传感器的探测范围，传感器的测量值形成的反射点不再是同一个点，采取一种简单有效的方法对有效信号进行筛选。即选取每一跟踪时刻车辆目标物估计位置前方的传感器信号为有效信号。有效信号筛选方法如图4a和图4b所示。

采用集中式方法对具有有效数据的传感器信息进行多传感器信息融合。具体的系统状态预测与更新方法如下。线性化后的每个传感器观测方程为：

Z_i(k)＝H_i(k)X(k)+V_i(k)

对于选取具有有效读数的传感器的读数值与相应的线性化后的观测转移矩阵组成新的测量值矩阵和观测转移矩阵。

$Z_s\left(k\right)={\[Z_{s_1}\left(k\right),\mathrm{...},Z_{s_m}\left(k\right)\]}^T$

$H_s\left(k\right)={\[H_{s_1}\left(k\right),\mathrm{...},H_{s_m}\left(k\right)\]}^T$

代入预测和更新方程进行预测与更新。其中表示k时刻的先验概率估计，表示k时刻的后验概率估计。

a.预测

$\hat{X}(k+1|k)=A\hat{X}\left(k\right|k)$

P(k+1|k)＝AP(k|k)A^T+Q

b.更新

K(k+1)＝P(k+1|k)H_s(k+1)^T[H_s(k+1)P(k+1|k)H_s(k+1)^T+R_s]^T

$\hat{X}(k+1|k+1)=\hat{X}(k+1|k)+K(k+1)\left(Z_s\right(k+1)-h_s(\hat{X}\left(k+1|k\right)\left)\right)$

P(k+1|k+1)＝[I-K(k+1)H_s(k+1)^T]P(k+1|k)

其中，R_s为m阶矩阵，对角线值为1²，其余值为0。

经过实验验证，本发明提出的算法针案例中的跟踪场景1和2能够有效实现对目标物的跟踪，跟踪精度优于采用三角定位法。

由上述分析可以看出，本发明实施例具有以下优点：

1、采用阵列式的设计和布置，扩展单个超声波传感器的探测范围。

2、通过阵列中多传感器信息融合的方式，减小单个传感器误差引起的目标物的定位误差。

3、可以实现对目标物动态的跟踪，跟踪的系统状态信息可包括位置、速度、加速度等。获得目标物的速度信息，不但能够有效的区分静态目标物和动态目标物，减少驾驶辅助系统中误警的产生，也可以提高自车感知周围目标物的能力。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

1.一种基于超声波传感器阵列的物体跟踪方法，其特征在于，包括：

步骤1、建立l阶的系统状态变量拟合目标物的系统状态；

步骤2、根据目标物运动特点建立运动状态方程，根据传感器测距方程建立观测方程；

X(k)＝f(X(k-1))+W(k-1)

Z(k)＝h(X(k))+V(k)

其中，X(k)为系统状态变量X(k)∈R^l，W(k)为系统过程噪声，过程噪声协方差矩阵为Q；Z(k)为观测量Z(k)∈Rⁿ，V(k)为观测噪声，观测噪声协方差矩阵为R；函数f(·)为系统转移方程，函数h(·)为观测方程；

步骤3、利用跟踪滤波算法对目标物进行跟踪，对于运动或观测方程存在非线性化部分的系统，可利用能够处理非线性系统的滤波器如扩展卡尔曼滤波器、无迹卡尔曼滤波器、粒子滤波器等进行目标物跟踪。

2.根据权利要求1所述的基于超声波传感器阵列的物体跟踪方法，其特征在于，所述步骤1具体包括：

步骤11、分析应用场景中目标物在二维平面内的在传感器阵列坐标系中的系统状态特点，进行系统状态归类；

步骤12、根据系统状态类型，建立合适的系统状态变量拟合目标物系统状态；

若目标物系统状态是静止状态，则可利用四阶的系统状态变量拟合目标物系统状态，系统状态变量如下：

X_k＝[x,y,v_x,v_y]^T

其中，x,y为目标物在x,y坐标系中位置的横纵坐标值，v_x,v_y为目标物在x方向与y方向的速度，由于目标物系统状态可近似认为是静止状态，v_x,v_y可认为是高斯分布的随机误差值；T表示矩阵的转置；四阶是指l＝4；

若目标物系统状态是匀速运动状态，则可利用六阶的系统状态变量拟合目标物系统状态，系统状态变量如下：

X_k＝[x,y,v_x,v_y,a_x,a_y]^T

其中，x,y为目标物在x,y坐标系中位置的横纵坐标值，v_x,v_y为目标物在x方向与y方向的速度，a_x,a_y为目标物在x方向与y方向的加速度，由于目标物系统状态可近似认为是匀速运动状态，a_x,a_y可认为是高斯分布的随机误差值；六阶是指l＝6；

若目标物系统状态是匀加速或匀减速运动状态，则可利用八阶的系统状态变量拟合目标物系统状态，其中系统状态变量如下：

其中，x,y为目标物在x,y坐标系中位置的横纵坐标值，v_x,v_y为目标物在x方向与y方向的速度，a_x,a_y为目标物在x方向与y方向的加速度，为目标物在x方向与y方向的加加速度，由于目标物系统状态可近似认为是匀加速或匀减速运动状态，为是高斯分布的随机误差值；八阶是指l＝8。

3.根据权利要求1所述的基于超声波传感器阵列的物体跟踪方法，其特征在于，所述步骤2具体包括：

步骤21、将物体系统的状态转移过程描述为一个离散时间的随机过程；根据目标物运动特点建立运动状态方程，对于二维平面内做线性运动物体，首先获取离散运动转移矩阵；为l阶系统状态变量建立l×l阶运动状态转移矩阵，具体如下：

$A_{4\×4}=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

$A_{6\×6}=\left[\begin{array}{cccccc}1& 0& T& 0& T^2/2& 0\\ 0& 1& 0& T& 0& T^2/2\\ 0& 0& 1& 0& T& 0\\ 0& 0& 0& 1& 0& T\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

$A_{8\×8}=\left[\begin{array}{cccccccc}1& 0& T& 0& T^2/2& 0& T^3/6& 0\\ 0& 1& 0& T& 0& T^2/2& 0& T^3/6\\ 0& 0& 1& 0& T& 0& T^2/2& 0\\ 0& 0& 0& 1& 0& T& 0& T^2/2\\ 0& 0& 0& 0& 1& 0& T& 0\\ 0& 0& 0& 0& 0& 1& 0& T\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$

其中T为目标物跟踪的跟踪周期，即超声波传感器阵列系统的信号采集周期；

步骤22、基于运动状态转移矩阵建立运动状态方程对于二维平面内做线性运动物体，可建立线性运动状态方程：

X(k)＝AX(k-1)+W(k-1)

其中X(k)为系统状态变量，X(k)∈R^l，W(k)为系统过程噪声，A为状态转移矩阵；

步骤23、根据传感器测距方程建立观测矩阵；根据超声波传感器的测距原理，超声波探测传感器到目标点的距离；传感器i的测量值为：

$h_i\left(X\right)=\sqrt{{(x_i-x)}^2+{(y_i-y)}^2}$

其中(x_i,y_i),i＝1,…,n为传感器位置，(x,y)为目标物超声波反射点位置，即为目标物跟踪预测位置X(k)的前两项；基于每个传感器的测量值方程，结合传感器的发射时序设计，可建立观测矩阵，例如对于同步发射的传感器时序设计，即每一发射周期所有传感器都是同时发射，观测矩阵为：

h(X)＝[h₁(X),…,h_i(X),…,h_n(X)]^T；

对于异步信号系统中，若存在配对检测，及某一传感器收到的是另一传感器发射的超声波，检测距离为超声波从另一传感器到目标物反射点到该传感器的距离；则传感器i的测量方程为：

$\begin{array}{c}{h}_i\left(X\right)=\mathrm{\γ}\×(\sqrt{{(x_i-x)}^2+{(y_i-y)}^2}\\ +\sqrt{{(x_j-x)}^2+{(y_j-y)}^2})\end{array}$

其中(x_i,y_i),i＝1,…,n为传感器i位置，(x_j,y_j),j＝1,…,n,j≠i为传感器j位置，(x,y)为目标物超声波反射点位置，即为目标物跟踪预测位置X(k)的前两项。γ为参数，由系统设计决定，若记录超声波传播距离长度，则γ＝1，若记录超声波传播距离长度的一半，则γ＝0.5；

基于每个传感器的测量方程，结合传感器的发射时序设计，可建立观测矩阵，观测矩阵为：

h(X)＝[h₁(X),…,h_i(X),…,h_n(X)]^T

如果传感器处于未激活状态或无读数，则h_i(X)为零矩阵；

步骤24、基于观测矩阵建立观测方程；观测方程具体为：

Z(k)＝h(X(k))+V(k)

其中，Z(k)为观测量，Z(k)∈Rⁿ，V(k)为观测噪声，函数h(·)为观测方程；对于每一个传感器，观测方程具体为：

Z_i(k)＝h_i(X(k))+V_i(k)

其中，Z_i(k)为传感器i的观测量，V_i(k)为传感器i的观测噪声，函数h_i(·)为传感器i的观测方程。

4.根据权利要求1所述的基于超声波传感器阵列的物体跟踪方法，其特征在于，所述步骤3具体包括：

步骤31、观测矩阵线性化；二维平面内超声波传感器对目标物距离的测量，即观测矩阵具有非线性成分；部分滤波器如粒子滤波器等具有直接处理非线性系统的能力，可跳过次步骤直接进入步骤32；对于无法直接处理非线性系统的滤波器，如扩展卡尔曼滤波器、无迹卡尔曼滤波器等，需要对观测矩阵进行线性化处理；扩展卡尔曼滤波器，采用的是高阶项采取忽略或逼近措施对非线性环节线性化；利用泰勒二次截断对观测方程进行线性化处理；具体公式如下：

$H_i\left(k\right)=\frac{\∂h_i\left(X\right)}{\∂X}$

其中，H_i(k)为线性化后的观测矩阵；

无迹卡尔曼滤波器采用采用近似方法，对非线性系统进行线性化；常用的采样策略包括对称采样、单形采样、三阶矩偏度采样以及高斯分布4阶矩对称采样等；

步骤32、目标物进入跟踪区域判断与状态初始化；根据传统阵列传感器的读数判断目标物进入跟踪区域，如最靠近阵列端点的两个传感器先后获得有效的距离信息，则可判断目标物进行阵列传感器的跟踪区域；初始状态的计算基于判断目标物进入跟踪区域时传感器的读数计算获得，其他参数的初始值可根据传感器的特性相应设计；

步骤33、对具有有效数据的传感器信息进行多传感器信息融合；其中所述扩展卡尔曼滤波器多传感器信息融合的方式可以为集中式或顺序式或分布式；

其中滤波器为无迹卡尔曼滤波器或粒子滤波器，用集中式数据融合方法进行；有效数据集中式信息融合算法选取具有有效读数的传感器的读数值、相应的观测矩阵与线性化后的观测转移矩阵组成新的测量值矩阵和观测转移矩阵；

$Z_s\left(k\right)={[Z_{s_1}\left(k\right),...,Z_{s_m}\left(k\right)]}^T$

$h_s\left(k\right)={[h_{s_1}\left(k\right),...,h_{s_m}\left(k\right)]}^T$

$H_s\left(k\right)={[H_{s_1}\left(k\right),...,H_{s_m}\left(k\right)]}^T$

其中，s₁,…,s_m为m个有有效读数输出的传感器；

步骤34、根据滤波器的跟踪原理对利用实时传感器数据对目标物进行系统状态的预测与更新，具体包括：根据运动转移方程目标物系统状态的先验概率估计，根据传感器的测量值对目标物的系统状态进行更新，获得后验概率估计。

5.根据权利要求4所述的基于超声波传感器阵列的物体跟踪方法，其特征在于，其中所述步骤34具体包括：应用步骤33中获得的测量值矩阵和观测转移矩阵对目标物进行系统状态的预测与更新，具体包括：

a.预测

$\hat{X}(k+1|k)=A\hat{X}\left(k\right|k)$

P(k+1|k)＝AP(k|k)A^T+Q

b.更新

K(k+1)＝P(k+1|k)H_s(k+1)^T[H_s(k

+1)P(k+1|k)H_s(k+1)^T+R_S]^T

$\begin{array}{c}\hat{X}\left(k+1|k+1\right)\\ =\hat{X}\left(k+1|k\right)+K\left(k+1\right)(Z_s\left(k+1\right)\\ -h_s\left(\hat{X}\left(k+1|k\right)\right))\end{array}$

P(k+1|k+1)＝[I-K(k+1)H_s(k+1)^T]P(k+1|k)

其中，表示k时刻的先验概率估计，表示k时刻的后验概率估计；R_S为m阶矩阵，对角线值为1²，其余值为0；

对于UKF算法，预测过程为对每一个采样点位置的预测，误差协方差矩阵为每个采样点误差协方差的加权；更新过程中，误差协方差均由采样点协方差加权获得。