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

说明书

技术领域

本发明涉及深水机器人技术领域，尤其涉及一种用于深水水机器人组合导航的装置和方法。

背景技术

深水机器人航行深度可达数千米。在水下航行过程中，导航误差会随着路程和时间的增加而逐渐增大。当深水机器人航行一段距离后，需要对其位置进行修正，以提高导航精度。

当前对深水机器人进行位置修正的方法主要是：通过声学定位系统输出的定位信息对深水机器人位置进行直接修正。但这种方法都存在一定的缺陷：

1）无法消除声学传播时延引起的定位误差；

2）深水机器人航行轨迹不平滑，不利于导航控制。

发明内容

为了克服现有方法的不足，本发明要解决的技术问题是提供一种深水机器人组合导航装置与方法，使得航行轨迹平滑且定位误差小。

本发明为实现上述目的所采用的技术方案是：一种深水机器人组合导航装置和方法，包括

测距器，安装于深水机器人壳体外部，连接导航计算机，用于通过声信号测量深水机器人到各个信标的距离；

信标，至少三个，布设于海底；

传感器，包括航向传感器、航速传感器和深度传感器，均安装于深水机器人中并连接导航计算机，分别用于测量深水机器人的的航向、航速和所处深度；

导航计算机，用于采集测距器和传感器的信号并实时计算深水机器人当前时刻的位置。

所述测距器按固定周期进行测距，周期在3秒～60秒之间。

所述导航计算机按固定周期采集数据，周期在0.2秒～2秒之间。

一种深水机器人组合导航方法，包括以下步骤：

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

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

判断初始位置是否有效；

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

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

所述计算深水机器人的初始位置包括以下步骤：

当测距周期内的测距数据数量大于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号信标的单程传播距离；

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

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

所述判断初始位置是否有效具体为：

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

如果e<=Bias

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

否则N=0，返回1）。

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

所述判断距离数据是否有效的方法为：判断公式

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为观测噪声。

所述根据距离数据对深水机器人的位置进行在线修正包括以下步骤：