[发明专利]一种水下目标主动跟踪航迹起始方法

摘要

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

主权项

1.一种水下目标主动跟踪航迹起始方法，其特征在于，包括以下步骤：周期性地通过AUV搭载的导航传感器和声纳传感器采集导航传感器数据和声纳传感器数据，并进行校正；根据声纳传感器数据对海洋环境杂波密度进行评估，获得评估值；根据声纳量测点的位置计算波门门限；按照航迹起始方法M/N规则判断是否有潜在轨迹存在；如果有，那么新的声纳量测数据首先用于更新潜在轨迹，如果潜在轨迹被更新，则删除对应已经参与潜在轨迹更新的量测数据，为每一个剩余的没有参与轨迹更新的量测数据单独起始一条潜在轨迹，并作为该潜在轨迹的第一个量测点；如果没有，那么新的量测数据全部用于起始新的潜在轨迹；所述航迹起始条件为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)的最小二乘法解为：