[发明专利]一种基于超短基线的导航定位方法

摘要

本发明公开一种基于超短基线(USBL)的导航定位方法，该发明将水下无人无缆水下机器人(水下机器人)的超短基线定位信息通过声通讯机发给水下机器人，以实现水下机器人的组合导航功能。该发明分成2个阶段，第一阶段是利用USBL对水下机器人进行跟踪定位，并将定位信息通过声通讯机发给水下机器人，第二阶段是水下机器人接收到定位信息，再经过运动补偿对导航算法进行状态更新。本发明操作简单，能准确有效地利用USBL定位信息实现水下机器人组合导航，进而提高水下机器人在水下的导航定位精度。

主权项

1.一种基于超短基线的导航定位方法，其特征在于，包括以下步骤：利用超短基线对水下机器人进行定位；通过声通讯机将母船位置信息和超短基线定位到的水下机器人位置信息发给水下机器人；水下机器人接收声通讯机发出的定位信息；结合运动补偿算法计算水下机器人收到声通讯机信息时自身的位置信息；将经过运动补偿后的位置信息作为观测量对水下机器人的实际位置进行更新；所述运动补偿包括以下步骤：第1步：获得母船和水下机器人位置信息；第2步：获得水下机器人收到声通讯机发出信息时刻的深度数据；第3步：解算水下机器人当前时刻的位置：其中，d₁是k时刻母船与水下机器人之间的水平距离；d₂是k‑1时刻母船与水下机器人间的水平距离，该变量为已知量；d₃是Δt时刻里水下机器人所走过的水平距离；Δt是从声通讯机发送数据到水下机器人接收数据所经历的时间；h_k是k时刻水下机器人所在的深度；X_k‑1是k‑1时刻超短基线获得的水下机器人位置信息；f(·)是水声信号在水下的传播距离和传播时间的关系函数；是水下机器人在接收声通讯机数据过程中的速度矢量，为已知量；k‑1时刻为超短基线得到水下机器人位置信息的时刻；k时刻为水下机器人收到其位置信息的时刻；解上面方程组得到Δt，则k时刻水下机器人所在的位置Xk为：