一种适用于水下机器人的快速融合避障方法技术

本发明专利技术提供一种适用于水下机器人的快速融合避障方法，通过测距声呐的测距标准差与方差确定AUV的定位误差区域，根据定位误差区域获得声呐安装方向上的AUV的静态安全距离。根据不同方向上的实时速度分量与静态安全距离得到相应方向上的安全警戒距离阈值。将测距声呐测得的障碍物距离数据与该方向上的安全警戒距离进行判断，当障碍物距离大于安全警戒距离时，该方向上的声呐将以基础工作间隔进行轮询工作。当障碍物距离小于或等于安全警戒距离时，该方向上的声呐将按照动态声呐工作间隔方程的输出进行轮询工作。本发明专利技术提出的可使AUV在狭窄航道内快速准确的获得障碍物距离信息，对多传感器数据进行融合，实现快速融合避障，为AUV的安全作业提供保障。

A fast fusion obstacle avoidance method for underwater robot

The invention provides a fast fusion obstacle avoidance method suitable for underwater vehicle. It determines the location error area of AUV through the ranging standard deviation and variance of ranging sonar. According to the location error area, we get the static safety distance of AUV in the direction of sonar installation. According to the real-time velocity component in different directions and the static security distance, the security warning distance threshold in the corresponding direction is obtained. The distance between obstacle data measured by ranging sonar and the safe distance in the direction is judged. When the distance between obstacles is greater than the security warning distance, the sonar in this direction will be polling based on the basic working interval. When the distance of the obstacle is less than or equal to the security alert distance, the sonar in this direction will be polled according to the output of the dynamic sonar working interval equation. The invention can enable AUV to get the distance information of obstacles quickly and accurately in narrow channels, and integrate multi-sensor data, so as to achieve fast convergence and obstacle avoidance, and provide guarantee for AUV's safe operation.

【技术实现步骤摘要】
一种适用于水下机器人的快速融合避障方法
本专利技术涉及智能水下机器人自主避障技术，尤其涉及一种适用于水下机器人的快速融合避障方法，属于水下机器人

技术介绍
智能水下机器人(AUV)是我们关心海洋、认识海洋、经略海洋的重要工具，而自主避障能力是智能水下机器人安全、顺利进行作业任务的必要保障。声呐作为基本的避障传感器已广泛应用在水下机器人中，高效的获取稳定而准确的距离数据是测距声呐的重要性能指标。本专利技术所建立的AUV声呐布置模型为艏部、艉部、左舷、右舷四个方向各至少布置一个声呐，测距声呐的传统工作模式为各声呐以固定的工作间隔同时进行测距。试验表明，在测距声呐数据的稳定性与准确性将在航道宽度低于某一阈值后随着航道宽度的减小而迅速降低，甚至无法得到正确的距离数据。采用轮询工作模式的测距声呐虽然可以避免声呐之间的干扰，但获得距离信息的效率将大大降低，降低了AUV的避障成功率，因此需要采用动态加权轮询的工作模式来提高测距声呐的避障效率。结合目前声呐工作模式与试验数据并针对上述现有状况的不足，本专利技术提出一种适用于水下机器人的快速融合避障方法。其特点是测距声呐采取动态加权轮询的工作模式，能够自主识别环境复杂情况，动态建立环境模型并切换声呐的工作间隔。通过轮询模式可以避免声呐间的干扰，提高声呐的数据准确性；针对多声呐根据各声呐方向上的速度分量与障碍物距离建立相应的动态加权工作间隔方程，通过动态切换声呐的工作间隔可以保障声呐按照当前方向上的环境变量自适应的增加或缩短工作间隔，在速度较快或障碍物较近时，快速获得当前方向的障碍物距离信息；通过基于速度插值的融合算法，将不同工作间隔测距声呐的数据进行时间配准，解决了多测距声呐的异步问题，并使系统按照短工作间隔声呐的间隔时间进行障碍物距离输出。
技术实现思路
本专利技术的目的是为了提供一种适用于水下机器人的快速融合避障方法，本专利技术中测距声呐采取动态加权轮询工作模式，使AUV能够在狭窄航道内获得稳定准确的距离数据，同时保障AUV能够高效的进行避障作业。本专利技术的目的是这样实现的：在AUV的艏部、艉部、左舷、右舷分别设置有测距声纳，步骤如下：步骤一：根据所设置的测距声呐的测距标准差和方差以及AUV的实时艏向角确定定位误差区域；步骤二：根据定位误差区域确定AUV在不同方向上的静态安全距离；步骤三：根据得到的静态安全距离，结合静态安全距离修正系数、速度、速度-距离修正系数建立安全警戒距离阈值方程；步骤四：根据障碍物信息，速度信息及安全警戒距离方程建立动态声呐工作间隔方程；步骤五：根据输出的声呐工作间隔进行多声呐的动态加权轮询工作，通过基于速度插值的融合算法实现系统的短工作间隔数据输出。本专利技术还包括这样一些结构特征：1.步骤一中的定位误差区域为椭圆，满足：式中：a为AUV划定的定位误差区域椭圆的半长轴，b为划定的定位误差椭圆的半短轴；φ为AUV的艏向角，σe为AUV左右舷上的测距声呐的测距标准差，σn为AUV艏艉部的测距声呐的测距标准差；和为测距声呐的测距方差；σen和σne为测距声呐的测距协方差；通过姿态传感器实时获得；σ0为扩展因子。2.步骤三中的安全警戒距离阈值方程为：hi＝d·ai+vi·bi式中：hi为AUV在i方向上安全警戒距离阈值，d为静态安全距离，ai为i方向上的静态距离修正系数，vi为i方向上的速度，bi为i方向上的速度-距离修正系数，i方向包括艏部、艉部、左舷、右舷的方向。3.步骤四中的动态声呐工作间隔方程为：式中：Δti为i方向上的声呐工作间隔，Δt0为声呐基础工作间隔，h为安全距离触发因子，vi为i方向上的速度，mi为i方向上的速度-工作间隔修正系数，si为i方向上声呐探测到的障碍物距离，ni为i方向上距离-工作间隔修正系数；g(x)为外部接口函数。4.步骤五具体为：动态加权轮询工作有三种：(1)当所有方向上的障碍物距离均大于安全警戒距离时，AUV上的声呐将以基础工作间隔Δta进行轮询工作，此时的轮询模式为艏部、左舷声呐发射声波，间隔时间Δta后，AUV的艉部、右舷声呐发射声波；(2)当部分或全部方向上的声呐采取动态工作间隔使各个方向上的声呐工作间隔相同时，工作间隔均为Δtb，AUV上的声呐将以轮询模式进行工作，此时的轮询模式为艏部、左舷声呐发射声波，间隔时间Δtb后，艉部、右舷声呐发射声波，且有Δtb<Δta；(3)当部分或全部方向上的声呐采取动态工作间隔使各个方向上的声呐工作间隔不全部相同时，AUV上的声呐将按照各自的工作间隔进行动态加权工作；为避免相对方向上声呐的干扰问题，采用如下的算法：给定相对方向上的两个声呐分别为1号、2号，实时工作间隔分别为：Δt1、Δt2，实时工作间隔中较小值为Δtmin＝min(Δt1，Δt2)，声呐工作间隔下限为tmin，上限为tmax，两个声呐工作总时间分别为t1、t2，t为系统工作总时间；设定避障系统的工作间隔为t0，t0为所有可能出现的工作间隔的最大公约数，即：Δt1＝p·t0(p＝1，2，3...)Δt2＝q·t0(q＝1，2，3...)，利用看门狗计时器，通过递归调用实现延时为t0的循环；当t1+Δt1＝t2+Δt2时，令Δtmin＝1.5Δtmin，避免相对方向上声呐间的相互干扰；基于速度插值的融合算法实现系统的短工作间隔数据输出是指：给定短工作间隔信号工作总时间为th，长工作间隔信号工作总时间为tl，速度在其方向上的分量为vl，则其输出的基于速度插值的障碍物距离满足：sl＝s`l-(th-tl)·vl式中：sl为长工作间隔传感器插值得到的障碍物距离值，sl`为长工作间隔传感器上一次得到的障碍物距离值。与现有技术相比，本专利技术的有益效果是：1)现有测距声呐的工作模式(多声呐以固定工作间隔同时采样)在航道变窄的情况下将无法获取准确的距离信息，经试验证明当航道宽度为3m，声呐工作间隔为1s时，测距声呐相对误差超过100％，无法获得可供参考的距离数据；采用轮询模式工作的测距声呐，在相同工况下，相对误差保持在10％以内，能够获得可供参考的距离数据；而采用动态加权轮询工作模式的声呐在某一方向声呐工作间隔缩短至0.5s时，仍可获得相同精度的数据，且其他方向上的声呐不受到干扰。试验表明采用动态加权轮询工作模式的测距声呐可以快速而准确的获得距离数据。2)声呐工作间隔将基于速度和障碍物距离的进行动态调整。在安全警戒距离之外，声呐将自动增加工作间隔，减小耗能，延长AUV的作业时间。当速度较快或距离障碍物较近时将自适应的缩短相应方向上声呐的工作间隔，从而实现AUV的快速自主避障。3)动态加权轮询模式输出的不同传感器的不同工作间隔将导致异步问题，从工程的角度来看，现有的最小二乘法、内推外插法、最大熵法等时间配准方法都有一定的局限性和片面性，其采用长工作间隔传感器的间隔时间为标准，降低了量测数据的使用率，降低了系统的精度，使本专利技术提出的快速融合避障方法失去了意义，因此不适用于本专利技术。本专利技术提出的基于速度插值的融合算法，以最短工作间隔声呐的间隔时间为标准进行数据输出，既避免了由于多声呐工作间隔不一致而导致的异步问题又可以进行高效的数据输出，实现了数据的快速融合。4)声呐动态工作间隔方程预留外部接口，可以根据需要接入电池电压信本文档来自技高网...

【技术保护点】
一种适用于水下机器人的快速融合避障方法，其特征在于：在AUV的艏部、艉部、左舷、右舷分别设置有测距声纳，步骤如下：步骤一：根据所设置的测距声呐的测距标准差和方差以及AUV的实时艏向角确定定位误差区域；步骤二：根据定位误差区域确定AUV在不同方向上的静态安全距离；步骤三：根据得到的静态安全距离，结合静态安全距离修正系数、速度、速度‑距离修正系数建立安全警戒距离阈值方程；步骤四：根据障碍物信息，速度信息及安全警戒距离方程建立动态声呐工作间隔方程；步骤五：根据输出的声呐工作间隔进行多声呐的动态加权轮询工作，通过基于速度插值的融合算法实现系统的短工作间隔数据输出。

【技术特征摘要】
1.一种适用于水下机器人的快速融合避障方法，其特征在于：在AUV的艏部、艉部、左舷、右舷分别设置有测距声纳，步骤如下：步骤一：根据所设置的测距声呐的测距标准差和方差以及AUV的实时艏向角确定定位误差区域；步骤二：根据定位误差区域确定AUV在不同方向上的静态安全距离；步骤三：根据得到的静态安全距离，结合静态安全距离修正系数、速度、速度-距离修正系数建立安全警戒距离阈值方程；步骤四：根据障碍物信息，速度信息及安全警戒距离方程建立动态声呐工作间隔方程；步骤五：根据输出的声呐工作间隔进行多声呐的动态加权轮询工作，通过基于速度插值的融合算法实现系统的短工作间隔数据输出。2.根据权利要求1所述的一种适用于水下机器人的快速融合避障方法，其特征在于：步骤一中的定位误差区域为椭圆，满足：式中：a为AUV划定的定位误差区域椭圆的半长轴，b为划定的定位误差椭圆的半短轴；φ为AUV的艏向角，σe为AUV左右舷上的测距声呐的测距标准差，σn为AUV艏艉部的测距声呐的测距标准差；和为测距声呐的测距方差；σen和σne为测距声呐的测距协方差；通过姿态传感器实时获得；σ0为扩展因子。3.根据权利要求1或2所述的一种适用于水下机器人的快速融合避障方法，其特征在于：步骤三中的安全警戒距离阈值方程为：hi＝d·ai+vi·bi式中：hi为AUV在i方向上安全警戒距离阈值，d为静态安全距离，ai为i方向上的静态距离修正系数，vi为i方向上的速度，bi为i方向上的速度-距离修正系数，i方向包括艏部、艉部、左舷、右舷的方向。4.根据权利要求1或2所述的一种适用于水下机器人的快速融合避障方法，其特征在于：步骤四中的动态声呐工作间隔方程为：式中：Δti为i方向上的声呐工作间隔，Δt0为声呐基础工作间隔，h为安全距离触发因子，vi为i方向上的速度，mi为i方向上的速度-工作间隔修正系数，si为i方向上声呐探测到的障碍物距离，ni为i方向上距离-工作间隔修正系数；g(x)为外部接口函数。5.根据权利要求3所述的一种适用于水下机器人的快速融合避障方法，其特征在于：步骤四中的动态声呐工作间隔方程为：式中：Δti为i方向上的声呐工作间隔，Δt0为声呐基础工作间隔，h为安全距离触发因子，vi为i方向上的速度，mi为i方向上的速度-工作间隔修正系数，si为i方向上声呐探测到的障碍物距离，ni为i方向上距离-工作间隔修正系数；g(x)为外部接口函数。6.根据权利要求1或2所述的一种适用于水下机器人的快速融合避障方法，其特征在于：步骤五具体为：动态加权轮询工作有三种：(1)当所有方向上的障碍物距离均大于安全警戒距离时，AUV上的声呐将以基础工作间隔Δta进行轮询工作，此时的轮询模式为艏部、左舷声呐发射声波，间隔时间Δta后，AUV的艉部、右舷声呐发射声波；(2)当部分或全部方向上的声呐采取动态工作间隔使各个方向上的声呐工作间隔相同时，工作间隔均为Δtb，AUV上的声呐将以轮询模式进行工作，此时的轮询模式为艏部、左舷声呐发射声波，间隔时间Δtb后，艉部、右舷声呐发射声波，且有Δtb<Δta；(3)当部分或全部方向上的声呐采取动态工作间隔使各个方向上的声呐工作间隔不全部相同时，AUV上的声呐将按照各自的工作间隔进行动态加权工作；为避免相对方向上声呐的干扰问题，采用如下的算法：给定相对方向上的两个声呐分别为1号、2号，实时工作间隔分别为：Δt1、Δt2，实时工作间隔中较小值为Δtmin＝min(Δt1，Δt2)，声呐工作间隔下限为tmin，上限为tmax，两个声呐工作总时间分别为t1、t2，t为系统工作总时间；设定避障系统的工作间隔为t0，t0为所有可能出现的工作间隔的最大公约数，即：Δt1＝p·t0(p＝1，2，3...)Δt2＝q·t0(q＝1，2，3...)，利用看门狗计时器，通过递归调用实现延时为t0的循环；当t1+Δt1＝t2+Δt2时，令Δtmin＝1.5Δtmin，避免相对方向上声呐间的相互干扰；基于速度插值的融合算法实现系统的短工作间隔数据输出是指：给定短工作间隔信号工作总时间为th，长工作间隔信号工作总时间为tl，速度在其方向上的分量为υl，则其输出的基于速度插值的障碍物距离满足：sl＝sl`-(th-tl)·υl式中：sl为长工作间隔传感器插值得到的障碍物距离值，sl`为长工作间隔传感器上一次得到的障碍物距离值。7.根据权利要求1或2所述的一种适用于水下机器人的快速融合避障方法，其特征在于：步骤五具体为：动态加权轮询工作有三种：(1)当所有方向上的障碍物距离均大于安全警戒距离时，AUV上的声呐将以基础工作间隔Δta进行轮询工作，此时的轮询模式为艏部、左舷声呐发射声波，间隔时间Δta后，AUV的艉部、右舷声呐发射声波；(2)当部分或全部方向上的声呐采取动态工作间隔使各个方向上的声呐工作间隔相同时，工作间隔均为Δtb，AUV上的声呐将以轮询模式进行工作，此时的轮询模式为艏部、左舷声呐发射声波，间隔时间Δtb后，艉部、右舷声呐发射声波，且有Δtb<Δta；(3)当部分或全部方向上的声呐采取动态工作间隔...

【专利技术属性】
技术研发人员：孙玉山，王相斌，张国成，万磊，徐昊，冉祥瑞，王立锋，焦文龙，程俊涵，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江,23