[发明专利]一种深水机器人组合导航装置和方法

权利要求书

1.一种深水机器人组合导航方法，其特征在于，包括以下步骤：

深水机器人下水后，导航计算机对测距器、航向传感器、航速传感器和深度传感器进行数据采集；

当导航计算机采集到测距器的距离数据时，计算深水机器人的初始位置；

判断初始位置是否有效；

如果初始位置有效，则判断距离数据是否有效，如果距离数据无效，则舍弃该距离数据，返回计算深水机器人的初始位置步骤；

如果距离数据有效，则根据距离数据对深水机器人的位置进行在线修正。

2.根据权利要求1所述的一种深水机器人组合导航方法，其特征在于，所述计算深水机器人的初始位置包括以下步骤：

1)当测距周期内的测距数据数量大于3时，将初始值N置0，对深水机器人水平面坐标进行解算，解算公式如下：

Ax＝v(1)

式中，

(x_i,y_i,z_i)(i＝1,2,3)为第i号信标的三维坐标，(x,y,z)为深水机器人水平面坐标和深度，为第i号信标到坐标系原点的水平间距，为第i号信标到深水机器人的水平距离，其中R_i表示第i号信标的单程传播距离；

2)根据解算的深水机器人水平面坐标推算航位：

式中，(x₀,y₀)为初始点，Δt为采集周期，(x_t,y_t)为推算航位，v_e＝vsinθ，v_n＝vcosθ，θ为当前航向，v为当前航速。

3.根据权利要求2所述的一种深水机器人组合导航方法，其特征在于，所述判 断初始位置是否有效具体为：

在下一个测距周期到来时，重新解算公式(1)中的深水机器人水平面坐标，将其与所述推算航位求差后求取均方根误差，即：

如果e＜＝Bias

则N＝N+1，返回2)，直到N＞＝Window时，判定当前测距周期内解算的初始位置 有效，

否则N＝0，返回1)；

上式中，Bias范围为1米～100米，Window范围为2～30。

4.根据权利要求1所述的一种深水机器人组合导航方法，其特征在于，所述判断距离数据是否有效的方法为：判断公式

S(k)＝H_kP_k|k-1H'_k|k-1+R_k

是否成立，如果成立，则使用当前测量值z_i；否则，舍弃；

式中，γ范围为6～500，H_k为观测矩阵，P_k|k-1为预测协方差矩阵，H'_k|k-1为预测观测矩阵，R_k为观测噪声，为测量值的估计矩阵。

5.根据权利要求1所述的一种深水机器人组合导航方法，其特征在于，所述根据距离数据对深水机器人的位置进行在线修正包括以下步骤：

设任意信标的坐标为(x_i,y_i)(i＝1,2,...,)，则离散系统状态方程为

其中，

s_k＝[x_k y_k]^T，x_k，y_k分别表示当前时刻k的东向坐标、北向坐标；

z_k＝c·τ_i，表示双程声传播距离，其中c表示声速，τ_i表示第i个信标的双程声传播时间，v_e为深水机器人东向速度，x_i为第i个信标的东向坐标，v_n为深水机器人北向速 度，y_i为第i个信标的北向坐标，z_i为第i个信标的深度,z_k为深水机器人当前时刻k的深度；

w_k是一个二维向量，为过程驱动噪声，w_k(1)和w_k(2)为正值且不大于10；

ν_k是一个一维向量，为观测噪声，ν_k为正值且不大于50；

1)初始化

2)预测

P_k|k-1＝AP_k|k-1A'+BQ_kB'

3)修正

K_k＝P_k|k-1H'_k(H_kP_k|k-1H'_k|k-1+R_k)^-1

P_k|k＝(I-K_kH_k)P_k|k-1

其中

Q_k＝diag([w_k(1))² (w_k(2))²])，