1.一种基于两步卡尔曼滤波的非视距水下航行器定位方法，其特征在于：包括如下步骤：步骤1：在检测海域内，随机部署N个水声传感器节点，其位置记录为Pn(xn，yn，zn)；其中P1(x1，y1，z1)为主传感器节点，其余传感器节点为副传感器节点；在待定位水下航行器进入检测海域后，水下航行器以ΔT为时间间隔循环发送初始化信息给所有传感器节点来启动定位过程，该信息中包含初始化信息的发送时刻Ts,s；

步骤2：在时刻ts,n，水下传感器节点接收到来自水下航行器发送的信息，传感器节点通过收集的时间戳信息得到信号从水下航行器到传感器节点的时延τs,n；

步骤3：副传感器接收到待定位水下航行器的初始化消息后，向主传感器节点发送定位信息包，该定位信息包中含有该传感器节点的坐标Pn和该传感器节点接收到定位启动信息的时延τs,n，当主传感器节点收到全部副传感器节点的信息时，主传感器节点计算水下航行器发送初始化信号到达主副传感器节点的时间差Δtn,1；

步骤4：采用卡尔曼滤波算法对主传感器节点获取的时间差信息进行滤波，并将卡尔曼滤波迭代过程中的新息值与阈值进行比较，从而实时判别当前测量是否受到非视距误差干扰；

步骤5：若判别当前测量值中含有NLOS误差，则对NLOS误差值进行粗略估计，并对NLOS误差估计值进行单独卡尔曼滤波，滤波结果作为NLOS误差修正值，利用NLOS误差修正值，去除测量值中的非视距误差，进而得到状态估计；若判别当前测量中不含有非视距误差，则直接利用卡尔曼滤波得到状态估计；

步骤6：根据步骤5中得到的时间差信息的滤波值，利用两步加权最小二乘法计算水下航行器位置；

步骤7：重复上述步骤4‑6，得到水下航行器位置的序贯估计结果。

2.根据权利要求1所述的一种基于两步卡尔曼滤波的非视距水下航行器定位方法，其特征在于：所述水声传感器节点数量N≥4。

3.根据权利要求1所述的一种基于两步卡尔曼滤波的非视距水下航行器定位方法，其特征在于：步骤2所述时延的计算方法为 其中τs,1为信号从水下航行器到主传感器节点的时延，τs,n为信号从水下航行器到第n个副传感器节点的时延。

4.根据权利要求1所述的一种基于两步卡尔曼滤波的非视距水下航行器定位方法，其特征在于：步骤3所述时间差的计算方法为Δtn,1＝(ts,n‑Ts,s)‑(ts,1‑Ts,s)＝τs,n‑τs,1。

5.根据权利要求1所述的一种基于两步卡尔曼滤波的非视距水下航行器定位方法，其特征在于：步骤4所述卡尔曼滤波算法以测量时间差作为滤波对象，卡尔曼滤波标准方程如下：X(k|k‑1)＝FX(k‑1),

T

P(k|k‑1)＝FP(k‑1)F+Q,

上述两式为卡尔曼滤波的预测步方程，其中X(k‑1)是k‑1时刻的目标状态，X(k|k‑1)是对k时刻的状态预测，F反映了两个相邻时刻目标状态的动态演化，P(k‑1)是k‑1时刻的预测误差协方差矩阵，P(k|k‑1)是对k时刻的误差协方差预测，Q为过程噪声的协方差矩阵；

T T ‑1

K＝P(k|k‑1)H(HP(k|k‑1)H+R) ,

上式为卡尔曼增益矩阵，其中H为状态测量矩阵；R为测量误差的协方差矩阵，其值一般为预先设定好的常数；

X(k)＝X(k|k‑1)+K(Z(k)‑HX(k|k‑1)),P(k)＝(I‑KH)P(k|k‑1),

上述两式为卡尔曼滤波的更新步方程，X(k)是k时刻的目标状态，其中Z(k)‑HX(k|k‑1)为测量新息，表示为：e(k)＝Z(k)‑HX(k|k‑1),

在卡尔曼滤波中，测量新息定义为当前观测值和状态预测值之差，当传感器节点处于LOS通信环境中时，e(k)为零均值高斯变量；当测量值中混入NLOS误差时，测量值Z(k)大小将远远偏离真实值，此时的测量新息e(k)的均值不再为零；利用e(k)可以判断测量值是否受到NLOS误差干扰，设定为以下判别式：当ek小于阈值ξ时，k时刻测量值不受到NLOS误差干扰；当ek大于等于阈值ξ时，判断此时测量值受到NLOS误差干扰，进行第二步卡尔曼滤波；其中经验阈值ξ表达式如下：上式中λ为允许测距的最大误差，其值应该根据环境信息与定位精度要求提前设定；

为系统误差阈值。

6.根据权利要求1所述的一种基于两步卡尔曼滤波的非视距水下航行器定位方法，其特征在于：所述步骤5消除NLOS误差的具体方法是：当测量值Z(k)中含有NLOS误差时，测量值大小将远远偏离真实值，而相较于测量值Z(k)而言，预测值X(k|k‑1)的值更接近于真实值，那么，令k时刻的状态估计为：基于k时刻的状态估计，k时刻的NLOS误差估计为：

对NLOS误差估计进行进一步卡尔曼滤波过程如下：

Pn(k)＝(I‑KnHn)Pn(k|k‑1)，

其中 为k‑1时刻的NLOS误差估计值， 为k时刻的NLOS误差修正值，基于此，得到k时刻的重构测量值 为：在NLOS误差干扰下，将重构测量值 带入更新步，代替测量值进行卡尔曼滤波，可以有效削除NLOS误差影响。

7.根据权利要求1所述的一种基于两步卡尔曼滤波的非视距水下航行器定位方法，其特征在于：步骤6中使用Chan算法求解水下航行器的位置，算法表达式如下：ΔDn,1＝||Ps‑Pn||‑||Ps‑P1||，

其中ΔDn,1表示水下航行器到第n个副传感器节点的距离与到主传感器节点的实际距离之差，Ps表示待定位水下航行器的位置；考虑噪声后，上述表达式的加权最小二乘解为其中，2

ψ为权重的倒数，在这里定义为ψ＝c BQB，其中B矩阵是由水下航行器到水下传感器节点的实际距离构成的对角阵；

考虑到xs,ys,zs与Ds,1之间的相关性，为了消除 中变量相关性带来的误差，将 的前三个元素分别减去x1，y1和z1并将其所有元素分别求平方，进行第二步加权最小二乘计算，解出目标的最终位置坐标矢量为：