CN115182408A - 一种内河水域清理器避障方法及自主移动方法 - Google Patents

Abstract

本发明公开了一种内河水域清理器避障方法及自主移动方法，包括以下步骤：步骤1：获取障碍物的运动状态、障碍物类别和清理器当前的运动状态；运动状态包括：位置信息、移动速度和移动方向；步骤2：基于障碍物类别、障碍物运动状态和清理器当前运动状态，采用避障算法，得到清理器的期望避障路径；步骤3：根据清理器当前的运动状态和清理器的期望避障路径，得到清理器下一时刻的初始运动状态；步骤4：根据清理器下一时刻初始运动状态和下一时刻目标路径节点，清理器得到下一时刻的控制量，进而清理器本体向前移动；步骤5：考虑环境载荷对清理器的作用力和清理器下一时刻的初始运动状态，得到清理器在外加载荷影响下产生的下一时刻的实际运动状态；步骤6：重复执行步骤3至步骤5，直至清理器与障碍物之间的距离大于安全距离，完成避障。

Description

一种内河水域清理器避障方法及自主移动方法

技术领域

本发明属于水域清理技术领域，具体涉及一种内河水域清理器避障方法及自主移动方法。

背景技术

清理器在内河水域进行垃圾清理时，也会受到一些环境情况影响，比如水域中的礁石、紊乱流态区等。现有智能清理装备的控制算法在实际应用时，也会针对运行环境的特点，进行装备控制***参数的人工整定。但对于不同类型的内河水域，清理装备单纯依靠控制算法参数的自整定难以很好地解决此问题。例如，通过摄像头获取水域画面进行图像识别预处理时的滤波算法就需要依照天气与光线条件随时改变；进行自主路径规划时步长的设定也需依据障碍物分布情况、障碍物类型和清理范围进行调整。

发明内容

发明目的：为解决现有清理器避障路径规划不佳等问题，本发明提出了一种内河水域清理器避障方法及自主移动方法，通过利用图像识别技术对障碍物进行分类，针对不同情况采取不同的避障方式，大大提升清理器的工作性能，达到方便、节能的理想效果。

技术方案：一种内河水域清理器避障方法，包括以下步骤：

步骤1：获取障碍物的运动状态、障碍物类别和清理器当前的运动状态；运动状态包括：位置信息、移动速度和移动方向；

步骤2：基于障碍物类别、障碍物运动状态和清理器当前运动状态，采用避障算法，得到清理器的期望避障路径；

步骤3：根据清理器当前的运动状态和清理器的期望避障路径，得到清理器下一时刻的初始运动状态；

步骤4：根据清理器下一时刻初始运动状态和下一时刻目标路径节点，得到清理器下一时刻的控制量，控制清理器向下一时刻目标路径节点移动；

步骤5：考虑环境载荷对清理器的作用力和清理器下一时刻的初始运动状态，得到清理器在外加环境载荷影响下产生的下一时刻的实际运动状态；

步骤6：重复执行步骤3至步骤5，直至清理器与障碍物之间的距离大于安全距离，完成避障；

其中，当障碍物类别为动态非生物障碍物时，所述的避障算法具体为：

记清理器的位置坐标为A(a₁,a₂)，动态非生物障碍物的位置坐标为B(b₁,b₂)，得到线段AB，基于动态非生物障碍物的移动方向和清理器的移动方向，得到线段AB与清理器移动方向的夹角α、线段AB与动态非生物障碍物移动方向的夹角β；

记清理器与动态非生物障碍物的最近相遇点为L_near，记清理器达到L_near的距离为x_A，动态非生物障碍物达到L_near的距离为x_B；

根据式(1)和式(2)，计算得到t_A、t_B：

若t_A＝t_B，则表示清理器与动态非生物障碍物存在碰撞风险，带入速度障碍模型中调整清理器的移动速度和移动方向，使t_A≠t_B；

若t_A≠t_B，则表示清理器与动态非生物障碍物不存在碰撞风险，清理器按照当前运动状态继续移动；

其中，所述的根据速度障碍模型，清理器所需进行的避障调整，具体为：

式中，vΔα为清理器在原有移动速度方向上的改变量，Δv为清理器移动速度大小改变量，v_h为清理器的移动速度与动态非生物障碍物的移动速度之差，Δγ为v_h与线段AB的夹角的变化量，φ为清理器的速度方向与v_h方向之间的夹角，Δγ的调整范围为(μ-γ,μ+γ)，μ为清理器的安全移动方位角；

当障碍物类别为动态非生物障碍物时，得到的清理器的期望避障路径为基于避障算法，形成的轨迹曲线。

进一步的，当障碍物类别为静态障碍物时，所述的避障算法具体为：

控制清理器按当前运动状态移动，直至清理器与静态障碍物之间的距离不大于安全距离，所述安全距离为清理器以当前移动速度在i秒内可达到的距离；

以静态障碍物为圆心且以安全距离为半径画圆，形成避障路径；

将避障路径与清理器当前移动路径的两个交点，记为位置a和位置b，取位置a到位置b的最短绕行曲线作为清理器的期望避障路径。

进一步的，当障碍物类别为动态生物障碍物时，所述的避障算法具体为：

利用声音驱逐动态生物障碍物，直至动态生物障碍物不位于清理器当前移动路径上且动态生物障碍物与清理器之间的距离大于安全距离后，清理器沿当前移动路径继续移动；

当障碍物类别为动态非生物障碍物时，得到的清理器的期望避障路径为清理器当前移动路径和驱逐动态生物障碍物的时长。

进一步的，步骤3具体包括：

记清理器k时刻的运动状态为s_k＝(x_k,y_k,v_k,θ_k)，其中，x_k,y_k,v_k,θ_k分别表示k时刻清理器所处的位置坐标、k时刻清理器的移动速度以及k时刻清理器的方位角；

根据下式，计算得到清理器k+1时刻的运动状态s_k+1＝(x_k+1,y_k+1,v_k+1,θ_k+1)：

式中，ΔD_k+1为清理器移动路径弧段位移增量，Δθ_k+1为清理器方位角移动增量，ΔD_k+1与Δθ_k+1为Δt时间段内清理器以v_k+1速度移动，产生的移动路径弧段位移增量以及方位角位移增量；v_k+1根据具体的避障情况做判断，若为静态障碍物或动态生物障碍物v_k+1＝v_k，若为动态非生物障碍物v_k+1＝v_k+Δv。

进一步的，所述的根据清理器下一时刻初始运动状态和下一时刻目标路径节点，得到清理器下一时刻的控制量，具体包括：

根据下式，将清理器k+1时刻的运动状态s_k+1＝(x_k+1,y_k+1,v_k+1,θ_k+1)转换为清理器下一时刻的控制量e_k+1：

e_k+1＝f_c(s_(k+1),p(x’,y’)) (6)

式中，p(x’,y’)为下一时刻目标路径节点，f_c()为清理器内部关于运动状态到控制量的映射。

进一步的，步骤5，具体包括：

所述环境载荷包括风载荷和流载荷；

所述风载荷对清理器的作用力，表示为：

式中，ρ_a表示空气密度，A_f表示清理器水线以上的正投影面积，A_s表示清理器水线以上侧投影面积，C_wx(α_R)和C_wy(α_R)均为风压力系数，α_R表示清理器有效来流航向角，U_R表示风速；

所述流载荷对清理器的作用力，表示为：

Y_l＝ρLdV_r ²Π/2(2a+b) (8)

式中，ρ为水密度，L为清理器长度，d为内河横向宽度，V_r是水流速度，a，b根据小展弦比机翼理论算得：a＝6.0，b＝3.25；

考虑环境载荷对清理器的作用力和清理器下一时刻的初始运动状态，得到清理器在外加环境载荷影响下产生的下一时刻的实际运动状态s'_k+1＝(x'_k+1,y'_k+1,v_k+1,θ_k+1)，其中：

本发明还公开了一种内河水域清理器自主移动方法，包括以下步骤：

S100：获取清理器当前位置和当前位置下清理器的清理目标点，基于RRT*路径规划算法，得到清理器到达清理目标点的主规划路径；所述主规划路径由多个移动路径节点构成；

S200：获取第j个移动路径节点的位置信息，在清理器前往至第j个移动路径节点的途中，实时获取清理器当前所处水域的水域画面；

S300：对水域画面进行网格化处理，并利用卷积神经网络提取图像特征；

S400：基于提取到的图像特征，判断清理器前往第j个移动路径节点的途中是否存在障碍物；若存在障碍物，则采用避障方法，控制清理器进行避障，避障后继续沿主规划路径前往至第j个移动路径节点；若不存在障碍物，则清理器沿主规划路径前往至第j个移动路径节点；

S500：判断清理器是否达到清理目标点，若达到，则结束移动；若未达到，则j＝j+1，执行S200；

所述避障方法为上述公开的一种内河水域清理器避障方法。

有益效果：本发明与现有技术相比，具有以下优点：

(1)本发明考虑到清理器在内河水域进行垃圾清理时，会受到实际应用环境的影响，另外，清理器应用路径规划算法进行自主移动的最终效能，会受到其自身控制***特征以及实际环境载荷影响，本发明结合环境载荷以及其自身运动状态，建立清理器的自主移动方法，提高清理器路径规划算法的实际应用效能；

(2)本发明根据实际水域状况，分情况进行避障，大大提高避障精度，使清理器更适用于实际应用场景。

附图说明

图1为一种内河水域清理器避障方法的流程示意图；

图2为静态障碍物规避行进图；

图3为动态生物障碍物规避行进图；

图4为速度障碍模型示意图；

图5为一种内河水域清理器自主移动方法的流程示意图。

具体实施方式

下面结合附图和实施例进一步阐述本发明。

实施例1：

如图1所示，一种内河水域清理器避障方法，包括以下步骤：

步骤1：获取障碍物运动状态、障碍物类别和清理器当前运动状态；运动状态信息包括：位置信息、移动速度和移动方向；

步骤2：基于障碍物类别、障碍物运动状态和清理器当前运动状态，采用避障算法，得到清理器的期望避障路径；在本实施例中，障碍物类别包括静态障碍物、动态非生物障碍物和动态生物障碍物；对于不同的障碍物类别，本实施例采用的避障算法也不同。

当障碍物类别为动态非生物障碍物时，避障算法具体为：

记清理器的位置坐标为A(a₁,a₂)，动态非生物障碍物的位置坐标为B(b₁,b₂)，得到线段AB，基于动态非生物障碍物的移动方向和清理器的移动方向，得到线段AB与清理器移动方向的夹角α、线段AB与动态非生物障碍物移动方向的夹角β；

记清理器与动态非生物障碍物的最近相遇点为L_near，记清理器达到L_near的距离为x_A，动态非生物障碍物达到L_near的距离为x_B；

根据式(1)和式(2)，计算得到t_A、t_B：

若t_A＝t_B，则表示清理器与动态非生物障碍物存在碰撞风险，带入速度障碍模型中调整清理器的移动速度和移动方向，使t_A≠t_B，使清理器规避动态非生物障碍物，如图3所示，清理器与动态非生物障碍物的相对位置，以及有关角度有具体展示。

若t_A≠t_B，则表示清理器与动态非生物障碍物不存在碰撞风险，清理器按照当前运动状态继续移动；

如图4所示，速度障碍模型表示为：

式中，vΔα为清理器在原有移动速度方向上的改变量，Δv为清理器移动速度大小改变量，v_h为清理器的移动速度与动态非生物障碍物的移动速度之差，Δγ为v_h与线段AB的夹角的变化量，φ为清理器的速度方向与v_h方向之间的夹角，Δγ的调整范围为(μ-γ,μ+γ)，μ为清理器的安全移动方位角；它是由清理器与动态非生物障碍物之间的直线距离D、障碍圆周半径R决定，具体的，R为不规则障碍物外接圆的半径，具体计算如下：

当障碍物类别为动态非生物障碍物时，得到的清理器的期望避障路径为基于避障算法，清理器形成的轨迹曲线。

按照以下标准选取符合条件的期望避障路径：

标准一：清理器在完成避碰前提下，取最短的路径长度；

标准二：在移动过程中，路径平滑度通过绕行曲线的曲率k来判断，曲率越小，路径平滑度越高，因此取最高的路径平滑度。

当障碍物类别为静态障碍物时，避障算法具体为：

控制清理器按当前运动状态移动，直至清理器与静态障碍物之间的距离不大于安全距离，所述安全距离为清理器以当前移动速度在1秒内可达到的距离；

以静态障碍物为圆心且以安全距离为半径画圆，形成避障路径；

将避障路径与清理器当前移动路径的两个交点，记为位置a和位置b，取位置a到位置b的绕行曲线作为清理器的期望避障路径。

其中，从位置a到位置b的绕行曲线的选取标准为：

标准一：清理器在完成避碰前提下，取最短的路径长度；

标准二：在移动过程中，路径平滑度通过绕行曲线的曲率k来判断，曲率越小，路径平滑度越高，因此取最高的路径平滑度。高的路径平滑度将更加便于清理器进行移动清理，对其自身的水动力性能需求降低，在水域内运行的稳定性更高。

清理器以当前速度沿期望避障路径进行静态障碍物绕行。

如图2所示，图2中的31和32代表路径规划过程中的其中一个移动路径节点和下一个移动路径节点，33为主规划路径，34为静态障碍物，35为期望避障路径。

当障碍物类别为动态生物障碍物时，避障算法具体为：

利用声音驱逐动态生物障碍物，直至动态生物障碍物不位于清理器当前移动路径上且动态生物障碍物与清理器之间的距离大于安全距离后，清理器沿当前移动路径继续移动；

当障碍物类别为动态非生物障碍物时，得到的清理器的期望避障路径为清理器当前移动路径和驱逐动态生物障碍物的时长。

步骤3：根据清理器当前的运动状态和清理器的期望避障路径，得到清理器下一时刻的初始运动状态；具体操作为：

记清理器k时刻的运动状态s_k＝(x_k,y_k,v_k,θ_k)，其中，x_k,y_k,v_k,θ_k分别表示k时刻清理器所处的位置坐标，当时速度以及方位角度。

根据下式，计算得到清理器k+1时刻的运动状态s_k+1＝(x_k+1,y_k+1,v_k+1,θ_k+1)：

式中，ΔD_k+1为清理器移动路径弧段位移增量，Δθ_k+1为清理器方位角移动增量，所述的，ΔD_k+1与Δθ_k+1为Δt时间段内计算机模拟清理器以v_k+1速度移动，产生的移动路径弧段位移增量以及方位角位移增量；v_k+1根据具体的避障情况做判断，若为静态障碍物或动态生物障碍物v_k+1＝v_k，若为动态非生物障碍物v_k+1＝v_k+Δv。

根据下式，将清理器k+1时刻的运动状态s_k+1＝(x_k+1,y_k+1,v_k+1,θ_k+1)转换为清理器下一时刻的控制量e_k+1：

e_k+1＝f_c(s_(k+1),p(x’,y’)) (6)

式中，p(x’,y’)为主路径中当前时刻所处节点的下一节点位置信息，f_c()为清理器内部关于运动状态到控制量的映射，该映射依清理器具体动力***而改变。

步骤4：根据清理器下一时刻初始运动状态和下一时刻目标路径节点，清理器得到下一时刻的控制量，进而清理器本体向前移动；

步骤5：考虑环境载荷对清理器的作用力和清理器下一时刻的初始运动状态，得到清理器在外加环境载荷影响下产生的下一时刻的实际运动状态；环境载荷包括风载荷和流载荷。

风载荷对清理器的作用力，表示为：

式中，ρ_a表示空气密度，A_f表示清理器水线以上的正投影面积，A_s表示清理器水线以上侧投影面积，C_wx(α_R)和C_wy(α_R)均为风压力系数，α_R表示清理器有效来流航向角，U_R表示风速；

流载荷对清理器的作用力，表示为：

Y_l＝ρLdV_r ²Π/2(2a+b) (8)

式中，ρ为水密度，L为清理器长度，d为内河横向宽度，V_r是水流速度，a，b根据小展弦比机翼理论算得：a＝6.0，b＝3.25；

考虑环境载荷对清理器的作用力和清理器下一时刻的控制量，得到清理器k+1时刻的最终运动状态s'_k+1＝(x'_k+1,y'_k+1,v_k+1,θ_k+1)，其中：

步骤6：重复执行步骤3至步骤5，直至清理器与障碍物之间的距离大于安全距离，完成避障。

实施例2：

考虑实际内河水域清理过程中的环境因素，清理器在进行自主移动过程中，会受到清理器自身控制***特征与实际水域环境状况影响，因此本实施例提出了一种内河水域清理器自主移动方法，如图5所示，包括以下步骤：

S100：获取清理器当前位置和当前位置下清理器的清理目标点，基于RRT*算法，得到清理器到达清理目标点的主规划路径；RRT*算法是在RRT算法基础上加入路径渐进最优函数得到一种基于采样的路径规划方法；主规划路径由多个移动路径节点构成；

S200：获取第j个移动路径节点的位置信息，在清理器前往至第j个移动路径节点的途中，实时获取清理器当前所处水域的水域画面；

S300：对水域画面进行网格化处理，并利用卷积神经网络提取图像特征；

S400：基于提取到的图像特征，判断清理器前往第j个移动路径节点的途中是否存在障碍物；若存在障碍物，则采用避障方法，控制清理器进行避障，避障后继续沿主规划路径前往至第j个移动路径节点；若不存在障碍物，则清理器沿主规划路径前往至第j个移动路径节点；

S500：判断清理器是否达到清理目标点，若达到，则结束移动；若未达到，则j＝j+1，执行S200。

本实施例中涉及的避障方法为实施例1公开的一种内河水域清理器避障方法。

Claims (7)

1.一种内河水域清理器避障方法，其特征在于：包括以下步骤：

步骤1：获取障碍物的运动状态、障碍物类别和清理器当前的运动状态；运动状态包括：位置信息、移动速度和移动方向；

步骤2：基于障碍物类别、障碍物运动状态和清理器当前运动状态，采用避障算法，得到清理器的期望避障路径；

步骤3：根据清理器当前的运动状态和清理器的期望避障路径，得到清理器下一时刻的初始运动状态；

步骤4：根据清理器下一时刻初始运动状态和下一时刻目标路径节点，得到清理器下一时刻的控制量，控制清理器向下一时刻目标路径节点移动；

步骤5：考虑环境载荷对清理器的作用力和清理器下一时刻的初始运动状态，得到清理器在外加环境载荷影响下产生的下一时刻的实际运动状态；

步骤6：重复执行步骤3至步骤5，直至清理器与障碍物之间的距离大于安全距离，完成避障；

其中，当障碍物类别为动态非生物障碍物时，所述的避障算法具体为：

记清理器的位置坐标为A(a₁,a₂)，动态非生物障碍物的位置坐标为B(b₁,b₂)，得到线段AB，基于动态非生物障碍物的移动方向和清理器的移动方向，得到线段AB与清理器移动方向的夹角α、线段AB与动态非生物障碍物移动方向的夹角β；

记清理器与动态非生物障碍物的最近相遇点为L_near，记清理器达到L_near的距离为x_A，动态非生物障碍物达到L_near的距离为x_B；

根据式(1)和式(2)，计算得到t_A、t_B：

若t_A＝t_B，则表示清理器与动态非生物障碍物存在碰撞风险，带入速度障碍模型中调整清理器的移动速度和移动方向，使t_A≠t_B；

若t_A≠t_B，则表示清理器与动态非生物障碍物不存在碰撞风险，清理器按照当前运动状态继续移动；

其中，所述的根据速度障碍模型，清理器所需进行的避障调整，具体为：

式中，vΔα为清理器在原有移动速度方向上的改变量，Δv为清理器移动速度大小改变量，v_h为清理器的移动速度与动态非生物障碍物的移动速度之差，Δγ为v_h与线段AB的夹角的变化量，φ为清理器的速度方向与v_h方向之间的夹角，Δγ的调整范围为(μ-γ,μ+γ)，μ为清理器的安全移动方位角；

当障碍物类别为动态非生物障碍物时，得到的清理器的期望避障路径为基于避障算法，形成的轨迹曲线。

2.根据权利要求1所述的一种内河水域清理器避障方法，其特征在于：当障碍物类别为静态障碍物时，所述的避障算法具体为：

控制清理器按当前运动状态移动，直至清理器与静态障碍物之间的距离不大于安全距离，所述安全距离为清理器以当前移动速度在i秒内可达到的距离；

以静态障碍物为圆心且以安全距离为半径画圆，形成避障路径；

将避障路径与清理器当前移动路径的两个交点，记为位置a和位置b，取位置a到位置b的最短绕行曲线作为清理器的期望避障路径。

3.根据权利要求1所述的一种内河水域清理器避障方法，其特征在于：当障碍物类别为动态生物障碍物时，所述的避障算法具体为：

利用声音驱逐动态生物障碍物，直至动态生物障碍物不位于清理器当前移动路径上且动态生物障碍物与清理器之间的距离大于安全距离后，清理器沿当前移动路径继续移动；

当障碍物类别为动态非生物障碍物时，得到的清理器的期望避障路径为清理器当前移动路径和驱逐动态生物障碍物的时长。

4.根据权利要求1所述的一种内河水域清理器避障方法，其特征在于：步骤3具体包括：

记清理器k时刻的运动状态为s_k＝(x_k,y_k,v_k,θ_k)，其中，x_k,y_k,v_k,θ_k分别表示k时刻清理器所处的位置坐标、k时刻清理器的移动速度以及k时刻清理器的方位角；

根据下式，计算得到清理器k+1时刻的运动状态s_k+1＝(x_k+1,y_k+1,v_k+1,θ_k+1)：

式中，ΔD_k+1为清理器移动路径弧段位移增量，Δθ_k+1为清理器方位角移动增量，ΔD_k+1与Δθ_k+1为Δt时间段内清理器以v_k+1速度移动，产生的移动路径弧段位移增量以及方位角位移增量；v_k+1根据具体的避障情况做判断，若为静态障碍物或动态生物障碍物v_k+1＝v_k，若为动态非生物障碍物v_k+1＝v_k+Δv。

5.根据权利要求4所述的一种内河水域清理器避障方法，其特征在于：所述的根据清理器下一时刻初始运动状态和下一时刻目标路径节点，得到清理器下一时刻的控制量，具体包括：

根据下式，将清理器k+1时刻的运动状态s_k+1＝(x_k+1,y_k+1,v_k+1,θ_k+1)转换为清理器下一时刻的控制量e_k+1：

e_k+1＝f_c(s_(k+1),p(x’,y’)) (6)

式中，p(x’,y’)为下一时刻目标路径节点，f_c()为清理器内部关于运动状态到控制量的映射。

6.根据权利要求5所述的一种内河水域清理器避障方法，其特征在于：步骤5，具体包括：

所述环境载荷包括风载荷和流载荷；

所述风载荷对清理器的作用力，表示为：

式中，ρ_a表示空气密度，A_f表示清理器水线以上的正投影面积，A_s表示清理器水线以上侧投影面积，C_wx(α_R)和C_wy(α_R)均为风压力系数，α_R表示清理器有效来流航向角，U_R表示风速；

所述流载荷对清理器的作用力，表示为：

Y_l＝ρLdV_r ²Π/2(2a+b) (8)

式中，ρ为水密度，L为清理器长度，d为内河横向宽度，V_r是水流速度，a，b根据小展弦比机翼理论算得：a＝6.0，b＝3.25；

考虑环境载荷对清理器的作用力和清理器下一时刻的初始运动状态，得到清理器在外加环境载荷影响下产生的下一时刻的实际运动状态s'_k+1＝(x'_k+1,y'_k+1,v_k+1,θ_k+1)，其中：

7.一种内河水域清理器自主移动方法，其特征在于：包括以下步骤：

S100：获取清理器当前位置和当前位置下清理器的清理目标点，基于RRT*路径规划算法，得到清理器到达清理目标点的主规划路径；所述主规划路径由多个移动路径节点构成；

S200：获取第j个移动路径节点的位置信息，在清理器前往至第j个移动路径节点的途中，实时获取清理器当前所处水域的水域画面；

S300：对水域画面进行网格化处理，并利用卷积神经网络提取图像特征；

S400：基于提取到的图像特征，判断清理器前往第j个移动路径节点的途中是否存在障碍物；若存在障碍物，则采用避障方法，控制清理器进行避障，避障后继续沿主规划路径前往至第j个移动路径节点；若不存在障碍物，则清理器沿主规划路径前往至第j个移动路径节点；

S500：判断清理器是否达到清理目标点，若达到，则结束移动；若未达到，则j＝j+1，执行S200；

所述避障方法为权利要求1至6任意一项所述的一种内河水域清理器避障方法。

Images