[发明专利]一种基于IMU和TOF的水下机器人单信标导航方法

权利要求书

1.一种基于IMU和TOF的水下机器人单信标导航方法，其特征在于：包括如下步骤：

步骤1：建立单信标测距导航系统、惯性导航系统和数据处理中心，在数据处理中心建立单信标测距导航数据采集模块、惯性导航数据采集模块、耦合距离滤波器、最大工作范围滤波器、状态接收阈值滤波器、动态模型模块、累加线性化模块、水下机器人位置模块、初始位置模块和误差计算模块；

单信标测距导航数据采集模块用于接收和存储单信标测距导航系统传输过来的位置数据声学包；

惯性导航数据采集模块用于接收并存储惯性导航系统传送过来的机器人姿态数据；

步骤2：惯性导航数据采集模块在接收到机器人姿态数据后，再将机器人姿态数据发送到耦合距离滤波器中进行滤波；

步骤3：动态模型模块根据耦合距离滤波器滤波后的数据进行水下机器人的动态位置计算，获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新，水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；

步骤4：初始位置模块将第一次从动态模型模块中获取到的水下机器人的当前位置作为水下机器人的初始位置，并存储该初始位置；

步骤5：单信标测距导航数据采集模块将位置数据声学包中的数据放入最大工作范围滤波器进行滤波，判断位置数据声学包中的数据是否超出了最大工作范围：是，则执行步骤6；否，则执行步骤9；

步骤6：累加线性化模块将位置数据声学包中的位置数据通过一阶泰勒级数展开线性化，并对观测时间点的状态值处进行评估从而获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新；水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；

步骤7：状态接收滤波器对步骤6中获取到的水下机器人的当前位置进行滤波，判断是否超出了状态接收阈值：是，则执行步骤8；否，则执行步骤9；

步骤8：误差计算模块进行TOF测距累加误差计算，获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新；水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；

步骤9：使用预定状态作为水下机器人的当前位置，并将当前位置发送给水下机器人位置模块进行位置更新。

2.如权利要求1所述的一种基于IMU和TOF的水下机器人单信标导航方法，其特征在于：所述单信标测距导航系统、所述惯性导航系统和所述数据处理中心通过无线网络或光线相互通信。

3.如权利要求1所述的一种基于IMU和TOF的水下机器人单信标导航方法，其特征在于：在执行步骤5时，最大工作范围滤波器用于防止在故障距离上处理测量更新方程，其判断如下：如果所述位置数据声学包中的数据范围大于设置的最大范围值，则丢弃该范围，并且不处理测量更新方程，通过多种方法确定最大工作范围值，包括基于环境的地理限制或者声学范围限制；

所述位置数据声学包中的数据为单信标测距范围数据。

4.如权利要求1所述的一种基于IMU和TOF的水下机器人单信标导航方法，其特征在于：在执行步骤7时，状态接收滤波器用于防止信标状态估计不准确而导致接收水下机器人的更新状态估计，在单信标测距导航系统传输过来的位置数据声学包中，从信标的最后一次单信标测距测量开始，跟踪先前状态估计的位置和时间，在确定更新状态之后，从上次距离测量时的水下机器人状态位置到当前水下机器人更新状态位置的距离，然后将此距离除以自上次单信标测距范围更新以来的时间，通过计算速度，与预定速度值进行比较：如果计算速度小于该预定速度值，则处理更新状态并确定加速度偏差测量；如果计算的速度大于预定的速度值，则忽略更新的状态估计，并向前传输水下机器人的预测状态。

5.如权利要求4所述的一种基于IMU和TOF的水下机器人单信标导航方法，其特征在于：在计算速度时，采用以下公式进行：

Δt＝k-t；

其中，x和y是水下机器人在世界坐标系中的估计位置坐标，v_p为预定速度值，k和t分别是当前时间和最后一次范围更新时间。

6.如权利要求1所述的一种基于IMU和TOF的水下机器人单信标导航方法，其特征在于：在所述数据处理中心建立预测模块，预测模块用于建立预测模型，根据水下机器人初始位置信息，结合惯性导航数据采集模块采集到的机器人姿态数据获得环境状态信息和机器人位姿信息，预测出当前机器人的位置信息。