CN106896363A - 一种水下目标主动跟踪航迹起始方法 - Google Patents

Abstract

本发明提供一种水下目标主动跟踪航迹起始方法，使主动跟踪算法获得更准确的初始估计值，以使算法获得更高的目标跟踪精度，更快地收敛于目标的真实值。该航迹起始方法仅仅依靠水下机器人(AUV)自身携带的导航设备，声纳探测设备与工作站计算机，便可以自主工作。该航迹起始方法分为两个阶段，第一个阶段是确定目标轨迹的有无，第二个阶段是对已确定目标航迹的初始状态和初始协方差矩阵进行估计。本航迹起始方法能够适用于不同的海洋环境，尤其对于高杂波环境，能够准确高效地对目标轨迹进行航迹起始。

Description

一种水下目标主动跟踪航迹起始方法

技术领域

本发明涉及水下机器人技术领域，具体的说是一种水下目标主动跟踪航迹起始方法。

背景技术

随着海洋技术的迅速发展，对水下目标及时准确稳定跟踪成为海洋工程领域迫切的需求。然而水下环境不同于陆地环境，首先，水下存在着大量的杂波，这些杂波可能源于海洋背景环境，也可能源于声纳传感器自身的噪声，也有可能源于目标释放的干扰。其次，声纳传感器相对于其他高精度探测传感器而言，量测精度不是很高。而且，声纳传感器的量测周期一般比较长(主动声纳)，因此，传感器的量测误差因素与目标的运动因素都有重要的影响，因此一些传统的航迹起始方法不再适用，如批处理方法(Hough变换)，启发式的航迹起始方法，基本的逻辑起始方法。因此需要采用新的或者修正的航迹起始方法。无论何种起始方法，都涉及到波门门限的取值，传统的航迹起始方法，因为不需要考虑量测误差的因素，往往波门门限只取决于速度的约束，这在水下环境使用范围有限。最后，在确定一条轨迹存在的同时，还需要对其进行估计初始值，因为现在大多数的滤波器均需要一个良好的初始值，如果初始值不准确的话，会导致随后轨迹收敛较慢，甚至轨迹发散，跟丢目标。因此，航迹起始在水下目标跟踪领域意义重大，同样也是难点之一。

发明内容

在复杂的海洋环境中，为了克服传统的航迹起始方法无法准确快速地建立目标轨迹，并且无法获得目标的初始状态，本发明要解决的技术问题是提供一种水下目标主动跟踪航迹起始方法，能够适应于不同的海洋环境，自适应地调整波门门限，确定目标轨迹，并估计其初始状态。

本发明为实现上述目的所采用的技术方案是：一种水下目标主动跟踪航迹起始方法，包括以下步骤：

周期性地通过AUV搭载的导航传感器和声纳传感器采集导航传感器数据和声纳传感器数据，并进行校正；

根据声纳传感器数据对海洋环境杂波密度进行评估，获得评估值；

根据声纳量测点的位置计算波门门限；

按照航迹起始方法M/N规则判断是否有潜在轨迹存在；如果有，那么新的声纳量测数据首先用于更新潜在轨迹，如果潜在轨迹被更新，则删除对应已经参与潜在轨迹更新的量测数据，为每一个剩余的没有参与轨迹更新的量测数据单独起始一条潜在轨迹，并作为该潜在轨迹的第一个量测点；如果没有，那么新的量测数据全部用于起始新的潜在轨迹；

随着声纳量测数据的不断更新，潜在轨迹也不断更新，如果有潜在轨迹满足航迹起始条件，则就可以认定其是一条确认轨迹；如果有潜在轨迹不满足航迹起始条件，则认为此条潜在轨迹不能升级为一条确认轨迹，将这些不满足要求的潜在轨迹删除；

对所认定的确认轨迹进行初始化。

所述校正，包括空间位置校正和时间校正：

所述位置校正包括：

通过AUV搭载的导航传感器获得AUV的经纬度信息或相对位置信息，将该信息转换为直角坐标系下的位置信息；

通过AUV搭载的声纳传感器获得极坐标下的包含目标量测的量测数据集合，将该量测数据集合转换为直角坐标系下的量测数据集合；

所述时间校正为：将数据采集周期设置为导航传感器的工作周期，在声纳传感器的工作周期到来时，将标志位的值设置为真；在声纳传感器的工作周期未到来时，将标志位的值设置为假。

所述直角坐标系的原点为AUV开始工作的经纬度所在的点。

所述将该量测数据集合转换为直角坐标系下的量测数据集合，具体为：对极坐标系下包含目标的量测数据集合{(ρ_i,θ_i)}进行无偏转换，得到在直角坐标系下的量测数据集合：

其中，x_Ti和y_Ti表示第i个量测在直角坐标系下的位置，θ_m是声纳角度量测误差1标准差，(x_auv,y_auv)为AUV在直角坐标系下的位置信息。

根据声纳传感器数据对海洋环境杂波密度进行评估，获得评估值，包括以下步骤：

在声纳每次量测周期内，记录所有量测的个数，并求出海洋环境的杂波密度；

记录并保存单次杂波密度评估值，统计一段时间内海洋环境杂波密度是否发生变化。

所述航迹起始条件为M/N规则，M表示有效匹配量测的次数，N表示总的量测匹配次数，即如果在不多于N次声纳量测更新中，有不小于M次量测，满足波门规则，则可以认为此是一条轨迹。

所述M/N规则判断是否有潜在轨迹存在，包括以下步骤：

将独立的声纳量测点p_last初步认为是一条潜在轨迹，此声纳量测点作为此条潜在轨迹的第一个点，也是此条潜在轨迹最近更新的量测点，有效量测次数为1，总的量测次数也为1，FlagNo的值设置为0；所述独立的声纳量测点是指没有参与更新确定轨迹或者潜在轨迹，所述FlagNo表示此条潜在轨迹连续没有被声纳更新的次数；

依据最近更新的量测点确定波门门限，此波门门限用于确定下次声纳量测与此条潜在轨迹匹配上的区域；所述波门门限规则为：

γ＝k₁·(FlagNo+1)·V_max·T+k₂·2·ρ (7)

其中，γ是波门门限，V_max是目标的最大速度，T是声纳更新周期，ρ是最近更新的声纳量测的距离值，k₁和k₂是与海洋杂波密度有关的可变系数；

在下一次声纳量测周期更新时，如果有一个声纳量测与此条潜在轨迹匹配上，即满足下列不等式：

|p_last-p_new|＜γ (8)

其中，p_new是下一次声纳量测周期中的一个量测点，则更新此条潜在轨迹，并且FlagNo的值设置为0，有效量测次数加1，总的量测次数也加1，并将此匹配成功的点p_new作为此条轨迹最近更新的点p_last，并依据最近更新的点，更新此条潜在轨迹的波门门限；如果下一次声纳量测周期中，所有的声纳量测点均不满足式(8)，则此条潜在轨迹此时此刻，并没有被更新，则FlagNo加1，有效量测次数维持原先值不变，总的量测个数加1，p_last也保持不变，更新波门门限。

所述对确认轨迹进行初始化，包括：

采用最小二乘法估计所述确认轨迹的初始值；

采用二次函数拟合，确定此条确认轨迹的初始协方差矩阵。

所述采用最小二乘法估计所述确认轨迹的初始值，包括以下步骤：

假设轨迹在时刻i的初始状态为X_i＝[x_i,vx_i,y_i,vy_i]'，其维数为4，状态X_i的观测是Z_i，即

Z_i＝H_iX_i+V_i (9)

Z_i＝[x_i,y_i]是第i时刻二维量测向量，H_i是量测矩阵，V_i是量测误差协方差矩阵，X_i是状态值，x_i是横坐标位置，vx_i是横坐标速度，y_i是纵坐标位置，vy_i是纵坐标速度，则有：

而且状态转移矩阵为F：

第i时刻与第i+1时刻满足如下关系式

X_i+1＝F·X_i (13)

依据式(11-13)将其合并为一个矩阵等式

Z＝HX+V (14)

其中Z和V是列向量，维数为M，H是2×M行4列矩阵，则等式(11)的最小二乘法解为：

所述采用二次函数拟合，确定此条确认轨迹的初始协方差矩阵，具体为：

初始协方差矩阵用对角阵来近似，即PX＝diag(p_x,p_vx,p_y,p_vy)，对角阵中对角线上的元素与其估计初始位置到AUV的距离成二次函数关系；建立初始协方差对角阵元素与初始状态之间的关系，如下关系式：

p_x＝p_y＝0.00014885208·dis²-0.064694135112·dis+1393.938970506778 (17)

p_vx＝p_vy＝0.000000048037392·dis²-0.00001727204999·dis+0.002900138085615 (18)

通过式(16)-(18)便获得了初始协方差矩阵。

本发明具有以下优点及有益效果：

1.方法简单和应用前景广阔。本发明必备的装置仅需要AUV导航传感器(***)、工作站计算机，声纳传感器，不需其它辅助装置，程序即可自动完成目标的航迹起始工作。

2.实用性好，效果好。经过仿真和海试验证，本目标跟踪航迹起始方法，能够准确地确定目标轨迹的有无，较高精度地确定轨迹的初始状态和初始协方差矩阵。

附图说明

图1是本发明航迹起始方法的装置组成以及工作环境示意图；

图2是本发明航迹起始方法的流程图；

图3A是被删除的一条潜在轨迹信息示意图；

图3B是只删除潜在轨迹中前列信息的一条潜在轨迹信息示意图；

图4A是形成确认轨迹所需时间较短的一条潜在轨迹示意图；

图4B是形成确认轨迹所需时间较长的一条潜在轨迹示意图；

其中，圈内有数字的点表示有效的量测，圈内无数字的点表示无效的量测，数字表示该潜在轨迹量测更新时间。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

本发明由AUV和工作站计算机组成，其中AUV需要搭载用于自身定位的导航传感器和用于探测目标位置的声纳传感器，如图1所示。实施本发明方法所需要的硬件包括：声纳传感器(主动声纳)，安装在AUV上，用于测量目标相对于AUV的位置信息(距离和角度)；导航传感器，安装在AUV上，用于确定AUV的位置、航向及姿态。工作站计算机，安装于AUV上，实时地对声纳传感器和导航传感器量测数据进行处理。

新的航迹起始方法如图2所示，AUV在运动过程中，通过导航传感器获得自身位置和通过声纳传感器获得目标与杂波的位置后，将数据上传到工作站计算机，运用新的航迹起始方法进行处理，处理后如果有确认轨迹形成，则将确认轨迹输出。

本发明方法在工作站计算机上实时处理导航数据和声纳量测数据，本发明方法的具体实施步骤如下：

第1步：周期性地获得导航传感器数据(AUV位置信息)和声纳传感器数据(目标与杂波位置信息)。

首先需要进行空间位置校正。因为导航传感器获得的数据是AUV的经纬度信息或者相对位置信息，需要将AUV的位置信息转换到固定直角坐标系下位置信息。同样也需要将声纳量测中的极坐标数据转换到直角坐标系下的数据。

其次，还需要时间校正。因为导航传感器与声纳传感器的工作周期一般不相同(导航传感器的工作周期一般小于声纳传感器工作周期)，则将该方法(对应的程序)被调用周期(被目标跟踪总程序调用)设置为导航传感器的工作周期，在声纳传感器工作周期到来时，标志位(新声纳量测)的值设置为真。

第2步：判断是否有潜在轨迹存在。

如果已经有潜在轨迹，那么新的量测数据应该首先用于更新潜在轨迹，如果潜在轨迹被更新，则对应已经参与潜在轨迹更新的量测点应该及时删除。最后为每一个剩余的没有参与轨迹更新的量测数据单独起始一条潜在轨迹，并作为该潜在轨迹的第一个量测点。

如果没有潜在轨迹，那么新的量测数据全部用于起始新的潜在轨迹。

第3步：进行潜在轨迹删除。

经过步骤2，会形成大量的潜在轨迹，然而，其中很多潜在轨迹并不满足形成确认轨迹的要求，因此可以将这些潜在轨迹删除。轨迹删除的规则是，如果该潜在轨迹总的量测更新次数为N，而有效量测更新次数小于M，则此条潜在轨迹需要被删除。

删除潜在轨迹又包含两种情况，如图3A所示第一种情况，是将此条潜在轨迹信息全部删除。第二种情况如图3B所示，仅删除此条潜在轨迹的前列信息，而保留潜在轨迹最新的信息，即只删除点1，而保留点3,5,6。这样可以提高目标的检测概率。

第4步：进行确认轨迹判别。

进行确认轨迹判别，采用M/N规则，如果此条潜在轨迹经过判别认定为一条确认轨迹，那就需要将此条潜在轨迹信息从潜在轨迹序列中转移到确认轨迹序列中。如图4所示，潜在轨迹升级为确认轨迹的情况之一。

第5步：如果有确认轨迹形成，对确认轨迹进行初始化。

初始化包括确定轨迹的初始状态和初始协方差矩阵。采用最小二乘法估计初始状态，采用二次曲线拟合初始协方差矩阵。

第6步：进入循环，等待总程序下一次调用航迹起始程序。

所述运用声纳传感器获得的量测数据评估周围海洋环境杂波密度，包括以下步骤：

步骤1：在声纳每次量测周期内，记录所有量测的个数，并求出海洋环境的杂波密度。在此由于海洋环境中杂波个数一般多于目标个数，因此在统计海洋环境杂波密度的时候，源于目标的量测忽略不计，并认为所有量测均是源于杂波。

步骤2：记录并保存单次杂波密度评估值，统计一段时间内，海洋环境杂波密度是否发生变化。

循环上述步骤1-2。

所述海洋环境杂波密度评估值分三个等级：杂波极少，杂波适中，杂波密集。并且确定海洋环境是否稳定，如果不稳定，则取海洋环境杂波密度最恶劣的评估值。

所述波门门限值既包括了声纳传感器量测误差因素，也包括了目标的运动因素，还包括了海洋环境杂波密度。

声纳传感器因素是指，声纳传感器的量测误差，比如声纳对目标的量测，角度量测误差服从均值为零，标准差为1度的高斯分布；距离量测误差服从均值为零，标准差为0.015×真实目标距离的高斯分布，即

Δθ～N(0,1) (1)

Δρ～N(0,0.015×ρ) (2)

Δθ表示角度量测误差，θ表示角度量测值，表示目标相对于声纳的真实方位，Δρ表示距离量测误差，ρ表示距离量测值，表示目标相对声纳的真实距离。

目标的运动因素是指水下目标最大运动速度与声纳量测周期的积，即V_max×T，V_max是指目标的最大运动速度，T是声纳量测周期，其反应的是目标在单位量测周期，目标的运动能力。

海洋环境杂波密度反应的是航迹起始计算量。如果杂波密度极小，某种程度上说，波门门限可以设置的比较大，如果杂波密度密集，那么波门门限不能设置的过大，否则会导致潜在轨迹数指数***，计算机负担过重，而且很多潜在轨迹均是虚假潜在轨迹。

确定一条轨迹的存在规则，采用基于逻辑的航迹起始方法M/N。即如果在不多于N次声纳周期量测过程中，有不少于M次量测满足波门门限要求，则认为存在一条轨迹。因为海洋环境比较复杂，相比于传统的基于逻辑的航迹起始方法中M和N值，本航迹起始方法M和N值比较大，M和N值随海洋环境杂波密度进行动态调整(N的取值与杂波密度有关，如果杂波密度较大，则N的取值也较大，反之较小；M通常取值为N/2+1)。在海洋环境杂波密度极少或者适中的情况下，M值一般是5，N值一般是8；如果海洋环境杂波密度密集，则设置M值是6，N值是10。

如果确定一条轨迹已经存在，则需要估计其初始状态和初始协方差矩阵，采用最小二乘法估计目标的初始状态，采用二次项拟合确定初始协方差矩阵。

Claims (10)

1.一种水下目标主动跟踪航迹起始方法，其特征在于，包括以下步骤：

周期性地通过AUV搭载的导航传感器和声纳传感器采集导航传感器数据和声纳传感器数据，并进行校正；

根据声纳传感器数据对海洋环境杂波密度进行评估，获得评估值；

根据声纳量测点的位置计算波门门限；

按照航迹起始方法M/N规则判断是否有潜在轨迹存在；如果有，那么新的声纳量测数据首先用于更新潜在轨迹，如果潜在轨迹被更新，则删除对应已经参与潜在轨迹更新的量测数据，为每一个剩余的没有参与轨迹更新的量测数据单独起始一条潜在轨迹，并作为该潜在轨迹的第一个量测点；如果没有，那么新的量测数据全部用于起始新的潜在轨迹；

随着声纳量测数据的不断更新，潜在轨迹也不断更新，如果有潜在轨迹满足航迹起始条件，则就可以认定其是一条确认轨迹；如果有潜在轨迹不满足航迹起始条件，则认为此条潜在轨迹不能升级为一条确认轨迹，将这些不满足要求的潜在轨迹删除；

对所认定的确认轨迹进行初始化。

2.根据权利要求1所述的一种水下目标主动跟踪航迹起始方法，其特征在于，所述校正，包括空间位置校正和时间校正：

所述位置校正包括：

通过AUV搭载的导航传感器获得AUV的经纬度信息或相对位置信息，将该信息转换为直角坐标系下的位置信息；

通过AUV搭载的声纳传感器获得极坐标下的包含目标量测的量测数据集合，将该量测数据集合转换为直角坐标系下的量测数据集合；

所述时间校正为：将数据采集周期设置为导航传感器的工作周期，在声纳传感器的工作周期到来时，将标志位的值设置为真；在声纳传感器的工作周期未到来时，将标志位的值设置为假。

3.根据权利要求2所述的一种水下目标主动跟踪航迹起始方法，其特征在 于，所述直角坐标系的原点为AUV开始工作的经纬度所在的点。

4.根据权利要求2所述的一种水下目标主动跟踪航迹起始方法，其特征在于，所述将该量测数据集合转换为直角坐标系下的量测数据集合，具体为：对极坐标系下包含目标的量测数据集合{(ρ_i,θ_i)}进行无偏转换，得到在直角坐标系下的量测数据集合：

其中，x_Ti和y_Ti表示第i个量测在直角坐标系下的位置，θ_m是声纳角度量测误差1标准差，(x_auv,y_auv)为AUV在直角坐标系下的位置信息。

5.根据权利要求1所述的一种水下目标主动跟踪航迹起始方法，其特征在于，所述根据声纳传感器数据对海洋环境杂波密度进行评估，获得评估值，包括以下步骤：

在声纳每次量测周期内，记录所有量测的个数，并求出海洋环境的杂波密度；

记录并保存单次杂波密度评估值，统计一段时间内海洋环境杂波密度是否发生变化。

6.根据权利要求1所述的一种水下目标主动跟踪航迹起始方法，其特征在于，所述航迹起始条件为M/N规则，M表示有效匹配量测的次数，N表示总的量测匹配次数，即如果在不多于N次声纳量测更新中，有不小于M次量测，满足波门规则，则可以认为此是一条轨迹。

7.根据权利要求6所述的一种水下目标主动跟踪航迹起始方法，其特征在于，所述M/N规则判断是否有潜在轨迹存在，包括以下步骤：

将独立的声纳量测点p_last初步认为是一条潜在轨迹，此声纳量测点作为此条潜在轨迹的第一个点，也是此条潜在轨迹最近更新的量测点，有效量测次数为1，总的量测次数也为1，FlagNo的值设置为0；所述独立的声纳量测点是指没有参与更新确定轨迹或者潜在轨迹，所述FlagNo表示此条潜在轨迹连续没有被声纳 更新的次数；

依据最近更新的量测点确定波门门限，此波门门限用于确定下次声纳量测与此条潜在轨迹匹配上的区域；所述波门门限规则为：

γ＝k₁·(FlagNo+1)·V_max·T+k₂·2·ρ (7)

其中，γ是波门门限，V_max是目标的最大速度，T是声纳更新周期，ρ是最近更新的声纳量测的距离值，k₁和k₂是与海洋杂波密度有关的可变系数；

在下一次声纳量测周期更新时，如果有一个声纳量测与此条潜在轨迹匹配上，即满足下列不等式：

|p_last-p_new|＜γ (8)

其中，p_new是下一次声纳量测周期中的一个量测点，则更新此条潜在轨迹，并且FlagNo的值设置为0，有效量测次数加1，总的量测次数也加1，并将此匹配成功的点p_new作为此条轨迹最近更新的点p_last，并依据最近更新的点，更新此条潜在轨迹的波门门限；如果下一次声纳量测周期中，所有的声纳量测点均不满足式(8)，则此条潜在轨迹此时此刻，并没有被更新，则FlagNo加1，有效量测次数维持原先值不变，总的量测个数加1，p_last也保持不变，更新波门门限。

8.根据权利要求1所述的一种水下目标主动跟踪航迹起始方法，其特征在于，所述对确认轨迹进行初始化，包括：

采用最小二乘法估计所述确认轨迹的初始值；

采用二次函数拟合，确定此条确认轨迹的初始协方差矩阵。

9.根据权利要求8所述的一种水下目标主动跟踪航迹起始方法，其特征在于，所述采用最小二乘法估计所述确认轨迹的初始值，包括以下步骤：

假设轨迹在时刻i的初始状态为X_i＝[x_i,vx_i,y_i,vy_i]'，其维数为4，状态X_i的观测是Z_i，即

Z_i＝H_iX_i+V_i (9)

Z_i＝[x_i,y_i]是第i时刻二维量测向量，H_i是量测矩阵，V_i是量测误差协方差矩阵，X_i是状态值，x_i是横坐标位置，vx_i是横坐标速度，y_i是纵坐标位置，vy_i是纵坐标速度，则有：

而且状态转移矩阵为F：

第i时刻与第i+1时刻满足如下关系式

X_i+1＝F·X_i (13)

依据式(11-13)将其合并为一个矩阵等式

Z＝HX+V (14)

其中Z和V是列向量，维数为M，H是2×M行4列矩阵，则等式(11)的最小二乘法解为：

。

10.根据权利要求8所述的一种水下目标主动跟踪航迹起始方法，其特征在于，所述采用二次函数拟合，确定此条确认轨迹的初始协方差矩阵，具体为：

初始协方差矩阵用对角阵来近似，即PX＝diag(p_x,p_vx,p_y,p_vy)，对角阵中对角线上的元素与其估计初始位置到AUV的距离成二次函数关系；建立初始协方差对角阵元素与初始状态之间的关系，如下关系式：

p_x＝p_y＝0.00014885208·dis²-0.064694135112·dis+1393.938970506778 (17)

p_vx＝p_vy＝0.000000048037392·dis²-0.00001727204999·dis+0.002900138085615 (18)

通过式(16)-(18)便获得了初始协方差矩阵。