[发明专利]一种适用于水下机器人的快速融合避障方法

权利要求书

1.一种适用于水下机器人的快速融合避障方法，其特征在于：在AUV的艏部、艉部、左舷、右舷分别设置有测距声纳，步骤如下：

步骤一：根据所设置的测距声呐的测距标准差和方差以及AUV的实时艏向角确定定位误差区域；

步骤二：根据定位误差区域确定AUV在不同方向上的静态安全距离；

步骤三：根据得到的静态安全距离，结合静态安全距离修正系数、速度、速度-距离修正系数建立安全警戒距离阈值方程；安全警戒距离阈值方程为：

h_i＝d·a_i+v_i·b_i

式中：h_i为AUV在i方向上安全警戒距离阈值，d为静态安全距离，a_i为i方向上的静态距离修正系数，v_i为i方向上的速度，b_i为i方向上的速度-距离修正系数，i方向包括艏部、艉部、左舷、右舷的方向；

步骤四：根据障碍物信息，速度信息及安全警戒距离阈值方程建立动态声呐工作间隔方程；步骤四中的动态声呐工作间隔方程为：

式中：Δt_i为i方向上的声呐工作间隔，Δt_a为声呐基础工作间隔，h为安全距离触发因子，v_i为i方向上的速度，m_i为i方向上的速度-工作间隔修正系数，s_i为i方向上声呐探测到的障碍物距离，n_i为i方向上距离-工作间隔修正系数；g(x)为外部接口函数；

步骤五：根据输出的声呐工作间隔进行多声呐的动态加权轮询工作，通过基于速度插值的融合算法实现系统的短工作间隔数据输出；基于速度插值的融合算法实现系统的短工作间隔数据输出是指：给定短工作间隔信号工作总时间为t_h，长工作间隔信号工作总时间为t_l，速度在其方向上的分量为v_l，则其输出的基于速度插值的障碍物距离满足：

s_l＝s_l`-(t_h-t_l)·v_l

式中：s_l为长工作间隔传感器插值得到的障碍物距离值，s_l`为长工作间隔传感器上一次得到的障碍物距离值。

2.根据权利要求1所述的一种适用于水下机器人的快速融合避障方法，其特征在于：步骤一中的定位误差区域为椭圆，满足：

式中：a为AUV划定的定位误差区域椭圆的半长轴，b为划定的定位误差区域椭圆的半短轴；φ为AUV的艏向角，σ_e为AUV左右舷上的测距声呐的测距标准差，σ_n为AUV艏艉部的测距声呐的测距标准差；和为测距声呐的测距方差；σ_en为测距声呐的测距协方差；通过姿态传感器实时获得艏向角；σ₀为扩展因子。

3.根据权利要求1或2所述的一种适用于水下机器人的快速融合避障方法，其特征在于：步骤五具体为：

动态加权轮询工作有三种：

(1)当所有方向上的障碍物距离均大于安全警戒距离阈值时，AUV上的声呐将以基础工作间隔Δt_a进行轮询工作，此时的轮询模式为艏部、左舷声呐发射声波，间隔时间Δt_a后，AUV的艉部、右舷声呐发射声波；

(2)当部分或全部方向上的声呐采取动态工作间隔使各个方向上的声呐工作间隔相同时，工作间隔均为Δt_b，AUV上的声呐将以轮询模式进行工作，此时的轮询模式为艏部、左舷声呐发射声波，间隔时间Δt_b后，艉部、右舷声呐发射声波，且有Δt_b<Δt_a；

(3)当部分或全部方向上的声呐采取动态工作间隔使各个方向上的声呐工作间隔不全部相同时，AUV上的声呐将按照各自的工作间隔进行动态加权工作；为避免相对方向上声呐的干扰问题，采用如下的算法：

给定相对方向上的两个声呐分别为1号、2号，实时工作间隔分别为：Δt₁、Δt₂，实时工作间隔中较小值为Δt_min＝min(Δt₁，Δt₂)，声呐工作间隔下限为t_min，上限为t_max，两个声呐工作总时间分别为t₁、t₂，t为系统工作总时间；

设定避障系统的工作间隔为t₀，t₀为所有可能出现的工作间隔的最大公约数，即：

Δt₁＝p·t₀，p＝1，2，3...

Δt₂＝q·t₀，q＝1，2，3...，

利用看门狗计时器，通过递归调用实现延时为t₀的循环；当t₁+Δt₁＝t₂+Δt₂时，令Δt_min＝1.5Δt_min，避免相对方向上声呐间的相互干扰。