买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：燕山大学

摘要：本发明公开了一种基于两步卡尔曼滤波的非视距水下航行器定位方法，该方法首先利用第一步卡尔曼滤波对测量值进行状态预测并利用卡尔曼滤波新息去识别当前测量值是否受到非视距（NLOS）误差干扰，如测量值不受NLOS误差干扰则继续第一步卡尔曼滤波得到状态估计；对于受到NLOS误差干扰的测量值，利用第一步卡尔曼滤波的预测结果对NLOS误差进行估计，然后对NLOS误差估计值进行第二步卡尔曼滤波并将滤波结果带入第一步卡尔曼滤波更新步进行状态估计；最后使用CHAN氏算法进行位置求解；本发明不需要NLOS误差的先验统计知识即可对连续NLOS误差进行实时消除，能够明显提高水下环境中水下航行器的定位精度。

主权项：1.一种基于两步卡尔曼滤波的非视距水下航行器定位方法，其特征在于：包括如下步骤：步骤1：在检测海域内，随机部署N个水声传感器节点，其位置记录为Pnxn，yn，zn；其中P1x1，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，得到水下航行器位置的序贯估计结果。

全文数据：

权利要求：

百度查询： 燕山大学 一种基于两步卡尔曼滤波的非视距水下航行器定位方法