【技术实现步骤摘要】
一种基于两步卡尔曼滤波的非视距水下航行器定位方法
[0001]本专利技术涉及一种基于两步卡尔曼(kalman)滤波的非视距水下航行器定位方法，属于水下目标声学定位
技术介绍
[0002]随着人类对海洋多种资源需求，海洋科学研究价值认识的日益增长，水下物联网(IoUT)在探索海洋方面的巨大优势越来越受到广泛关注。其中水下航行器作为水下物联网的重要组成部分被广泛应用于科学和工业调查，包括防灾和水下矿山侦察、海洋学数据收集和海上漏油监测等方面。在这些应用中，对水下航行器位置的准确估计是非常重要的环节，因为只有当水下航行器的位置信息已知时，这些应用才能发挥效用。因此，高精度的水下目标定位有着十分重要的研究价值。
[0003]一般来说，对于目标的定位方案大多利用接收信号强度(RSSI)，信号到达时间(TOA)，信号到达时间差(TDOA)等技术进行距离测量，通过计算可以得到准确的位置信息。然而水声信道高动态、强干扰的特点会使RSSI测距精度大大降低。而声波作为目前水下通信信息的主要载体，在水下的传播速度只有1500m/...
【技术保护点】
【技术特征摘要】
1.一种基于两步卡尔曼滤波的非视距水下航行器定位方法，其特征在于：包括如下步骤：步骤1：在检测海域内，随机部署N个水声传感器节点，其位置记录为P
n
(x
n
，y
n
，z
n
)；其中P1(x1，y1，z1)为主传感器节点，其余传感器节点为副传感器节点；在待定位水下航行器进入检测海域后，水下航行器以ΔT为时间间隔循环发送初始化信息给所有传感器节点来启动定位过程，该信息中包含初始化信息的发送时刻T
s,s
；步骤2：在时刻t
s,n
，水下传感器节点接收到来自水下航行器发送的信息，传感器节点通过收集的时间戳信息得到信号从水下航行器到传感器节点的时延τ
s,n
；步骤3：副传感器接收到待定位水下航行器的初始化消息后，向主传感器节点发送定位信息包，该定位信息包中含有该传感器节点的坐标P
n
和该传感器节点接收到定位启动信息的时延τ
s,n
，当主传感器节点收到全部副传感器节点的信息时，主传感器节点计算水下航行器发送初始化信号到达主副传感器节点的时间差Δt
n,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所述时间差的计算方法为Δt
n,1
＝(t
s,n
‑
T
s,s
)
‑
(t
s,1
‑
T
s,s
)＝τ
s,n
‑
τ
s,1
。5.根据权利要求1所述的一种基于两步卡尔曼滤波的非视距水下航行器定位方法，其特征在于：步骤4所述卡尔曼滤波算法以测量时间差作为滤波对象，卡尔曼滤波标准方程如下：X(k|k
‑
1)＝FX(k
‑
1),P(k|k
‑
1)＝FP(k
‑
1)F
T
+Q,上述两式为卡尔曼滤波的预测步方程，其中X(k
‑
1)是k
‑
1时刻的目标状态，X(k|k
‑
1)是对k时刻的状态预测，F反映了两个相邻时刻目标状态的动态演化，P(k
‑
1)是k
‑
1时刻的预测
误差协方差矩阵，P(k|k
‑
1)是对k时刻的误差协方差预测，Q为过程噪声的协方差矩阵；K＝P(k|k
‑
【专利技术属性】
技术研发人员：李鑫滨，张文浩，韩松，赵海红，于海峰，
申请(专利权)人：燕山大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。