CN110837255A - 一种适用于高速水面无人艇的自主危险规避方法 - Google Patents

Abstract

本发明提供的是一种适用于高速水面无人艇的自主危险规避方法。步骤1：获取当前无人艇状态、指令信息以及周围障碍物信息；步骤2：通过高可信局部环境建模方法建立无人艇环境模型；步骤3：输出当前环境模型中已稳定障碍物信息；步骤4：通过基于速度障碍物的高可靠航向稳定保持方法得到新的指令信息；步骤5：输出新的指令信息。本发明可以有效提高无人艇探测范围内的障碍物位置、尺寸的可信度；该方法可同时避免不必要的航向调整，有效的保证了高速无人艇自主航行的安全性。大量高速自主危险规避试验(≥40节)证明本发明可以在不同实际海洋环境中保证高速无人艇自主安全航行。

Description

一种适用于高速水面无人艇的自主危险规避方法

技术领域

本发明涉及的是一种水面无人艇控制方法，具体地说是一种适用于高速水面无人艇的自主危险规避方法。

背景技术

水面无人艇(unmanned surface vehicle,USV)是一种具有自主导航、自主避障和自主探测目标区域环境信息等功能的特殊水面无人平台，具有较强的海洋环境适应性、较大的作业/作战半径以及良好的隐身性和抗倾覆能力，可通过大中型舰船或岸基站来布放和回收。水面无人艇具有高航速、大续航力、经济性好、隐身性、浅吃水、小体积、易批量生产、布置方便等突出优势，因此非常适合作为一种通用化、无人化、智能化、信息化和具有较优经济性的海洋武器装备。同时水面无人艇可以在海洋中承担长期、自主、灵活和低廉的作业任务，因此，在民用领域具有非常广阔的应用前景，如海洋测绘与科学调查、环境监测、水文调查和气象预报等。

高速无人艇(≥40节)在执行应急响应、救援等任务时具有明显的优势，同时在执行水文调查、巡逻警戒等任务时具有更高的效率，近年来已成为无人艇发展的主要趋势。智能化和无人化是无人艇的显著特征，提高其智能水平是迫切需要解决的核心问题。无人艇需要在复杂的水面环境中自主航行和作业，因此对高航速下自主危险规避能力及其可靠性提出了更为苛刻的要求。因此，需要提供一种适用于高速水面无人艇的自主危险规避方法，以保证无人艇的自主安全航行。

申请号为201810318563.3，名称为“一种基于速度障碍法和动态窗口法的无人艇动态避障算法”的专利文件中公开了一种基于速度障碍法和动态窗口法的无人艇动态避障算法，在速度障碍法的基础上，考虑船只长宽比大的特点，用椭圆表示无人艇与障碍物，给出一种求解椭圆切线的方法；考虑无人艇的运动学性能，只使用无人艇在给定时间内能到达的速度和方向进行避障计算。该技术方案未与实际传感器结合，仅考虑了障碍物的不稳定性，且未提及高速无人艇避障对航向稳定性要求高的特点。与本发明中充分考虑航海雷达的传感器特性以及避障过程中的航向稳定性在功能和特征上有所区别。

发明内容

本发明的目的在于提供一种能有效提高无人艇探测范围内的障碍物位置、尺寸的可信度，同时能避免不必要的航向调整的适用于高速水面无人艇的自主危险规避方法。

本发明的目的是这样实现的：

步骤一：获取当前无人艇状态、指令信息以及周围障碍物信息；

步骤二：通过高可信局部环境建模方法建立无人艇环境模型；

步骤三：输出当前环境模型中已稳定障碍物信息；

步骤四：通过基于速度障碍物的高可靠航向稳定保持方法得到避碰航向；

步骤五：输出新的指令信息。

当无人艇高速巡航时(≥40节)，需要在极短的时间内对周围环境进行判断并做出合理的规避动作，这对障碍物检测和自主驾控均提出了更高的可靠性和实时性要求。针对无人艇高速自主避障时的障碍物检测结果不稳定的问题和运动控制的高航向稳定性要求，考虑高速无人艇搭载的航海雷达的传感器特性以及无人艇自身的运动学和动力学特性，本发明提供一种高可信局部环境建模方法，以有效提高无人艇探测范围内的障碍物位置、尺寸的可信度；同时在速度障碍物避障方法的基础上提供一种高可靠航向稳定保持方法，该方法可同时避免不必要的航向调整，有效的保证了高速无人艇自主航行的安全性。大量高速自主危险规避试验(≥40节)证明本发明可以在不同实际海洋环境中保证高速无人艇自主安全航行。

本发明针对无人艇高速自主避障时的障碍物检测结果不稳定的问题和运动控制的高航向稳定性要求，提供一种适用于高速水面无人艇的自主危险规避方法。该方法充分考虑高速无人艇搭载的航海雷达的传感器特性以及无人艇自身的运动学和动力学特性，有效提高无人艇探测范围内的障碍物位置、尺寸的可信度，同时避免不必要的航向调整，可以安全稳定的以40节以上航速避开遇到的障碍物。

附图说明

图1为碰撞预测模型；

图2为相对坐标系；

图3高可信局部环境模型建立方法流程图；

图4高可靠航向稳定保持方法流程图；

图5a-图5d 40节高速自主危险规避试验流程，其中：图5a为第70节拍、图5b为第180节拍、图5c为第300节拍、图5d为第550节拍；

图6 40节高速自主危险规避航向变化曲线；

图7 40节高速自主危险规避航速变化曲线；

图8是本发明的流程图。

具体实施方式

下面举例对本发明做更详细的描述。

实施例一：

结合图8，本发明的适用于高速水面无人艇的自主危险规避方法包括如下步骤，

步骤一：获取当前无人艇状态、指令信息以及周围障碍物信息。

步骤二：通过高可信局部环境建模方法提高环境模型中障碍物稳定性。

步骤三：输出当前环境模型中已稳定障碍物信息。

步骤四：通过基于速度障碍物的高可靠航向稳定保持方法提高避碰过程中航向稳定性。

步骤五：输出新的指令信息。

步骤二中所述高可信局部环境建模方法基本原理为充分考虑雷达盲区影响，在雷达盲区外实时更新障碍物位置尺寸信息，在雷达盲区内采用历史累积数据替代障碍物实时检测结果，以提高环境模型中障碍物的可信度。

环境模型范围根据无人艇航速确定，航速小于20节时环境模型范围为0.25海里，航速大于20节小于40节时环境模型范围为0.5海里，航速大于40节时环境模型范围为1海里。

其中雷达盲区经试验测量为200米。

采用位置与尺寸相结合方式实现环境模型中障碍物匹配。

障碍物可信度阈值根据无人艇航速确定，航速小于20节时阈值为4，航速大于20节小于40节时阈值为3，航速大于40节时阈值为2。

步骤四中所述基于速度障碍物的高可靠航向稳定保持方法基本原理为充分考虑高速艇运动学与动力学特性，采用转向标志位方法，每次避碰过程仅对避碰转向方向进行一次判断，以避免避碰过程中不必要的航向调整，保证高速无人艇自主避碰航行稳定性和安全性。

避碰距离根据无人艇航速确定，航速小于20节时避碰距离为0.15海里，航速大于20节小于40节时避碰距离为0.25海里，航速大于40节时避碰距离为0.5海里。

实施例二：

结合图1，无人艇在自主航行过程中，需要对可能发生的碰撞做出判断，并采取相应的规避措施。图中A为水面无人艇当前位置，以航速V_A、艇首向角α前进(在海洋环境中，水面无人艇简化为一个点)。B为半径为R、圆心在B的圆形障碍物，以航速V_B、艇首向角β前进。△V为V_A与V_B的合速度，方向为

与视线AB连线夹角为γ。把△V分解为沿视线AB方向分速度V_S和垂直于AB方向分速度V_θ：

结合图2，以无人艇当前位置为原点，艇首方向为X轴正向，左舷为Y轴正向，建立与障碍物相对运动坐标系。水面无人艇到障碍物直线距离为D，障碍物半径为R，则安全角μ为

通过比较相对速度△V与视线AB夹角γ和安全角μ的相对关系，即可判定是否会发生碰撞。

式中collision_flag＝0时，无人艇保持当前航速和航向不会与障碍物发生碰撞；collision_flag＝1时，无人艇需要改变航向或航速以避免与障碍物发生碰撞。

△γ的调整范围为：

无人艇在危险规避中自身速度大小调整量△VA和自身速度方向即艏向角的调整量△α的计算式为：

结合图3，为了解决障碍物不稳定性问题，提供一种新的局部环境模型构建方法，该方法在离散时域中逐步将障碍物包含到模型中。为了避免虚假障碍物，采用障碍物标记方法，以提高障碍物检测的准确性。不断监测无人艇附近的障碍并与历史记录进行比较。采用特征匹配方法以确保仅将具有高确定性的障碍物包括在环境模型中。该方法具体步骤如下：

步骤一：环境模型初始化，模型(obstacle_model)中的障碍物数量等于0。

步骤二：更新当前时间节拍t，将环境模型的中心设置为无人艇的地理坐标(lon_ship,lat_ship)，并将范围(obstacle_model_range)设置为当前雷达探测范围。obstacle_model_range可以基于下式计算。

步骤三：确定所有障碍物(obstacle_model)是否在当前模型范围内；如果第j个障碍物(obstacle_model_j)在当前模型范围之外，则将其从环境模型中移除，并且模型中的障碍物数量减1。

步骤四：使用USV的当前经纬度(lon_ship,lat_ship)和航向(α)，积分障碍物的相对距离(D)，相对方向角度(θ)，第i个障碍物的地理坐标可由下式计算：

ε＝30.92m为经度相邻两秒之间的距离。

步骤五：确定第i个障碍物(obs_i)是否在更新的距离(d)之外。如果第i个障碍物在更新的距离内，转到步骤七.d是航海雷达的盲区，基于海上试验，d＝200m。

步骤六：确定obs_i是否为新的障碍物。与当前环境模型中所有其他障碍物的距离和半径进行比较。如果obs_i和当前模型中障碍物obstacle_model_j之间的距离小于其半径R_i，并且两个障碍物的半径之差不超过R_i一半，则obs_i是已存在障碍物，更新其位置和大小。为了表示环境模型中障碍物的稳定程度，使用障碍物模型标志位(obstacle_model_flag)，该标志位值将随着成功匹配的次数增加。如果没有匹配成功，则obs_i是一个新的障碍物，并且模型中的障碍物数量(obstacle_model_num)加1。

步骤七：对下一个障碍物重复相同的障碍物匹配过程并返回步骤四，直到完成所有障碍物匹配。

步骤八：将满足阈值(ξ)的环境模型中障碍物的位置和大小信息发送给路径规划系统。

ξ可以根据如下计算：

步骤九：返回步骤二，进入下一个环境模型构建周期。

结合图4，速度障碍物方法以其较好的实时性，广泛应用于无人艇自主危险规避。当无人艇高速巡航时，在保证安全的情况尽量减少航向的改变以及避免航向的反复调整是需要首先考虑的，本发明针对运动控制中的时滞和安全要求问题，提出了一种高可靠航向稳定保持方法。该方法具体步骤如下：

步骤一：初始化，将期望航速和航向赋值为无人艇的当前航速和航向，V_{A_expect}＝V_A，α_expect＝α。

步骤二：对于每个时间节拍t，根据惯导和GPS信息更新当前V_A和α。

步骤三：基于下式计算目标点航向(α_goal)。

其中(lon_ship,lat_ship)和(lon_goal,lat_goal)是无人艇和目标点的纬经度。

步骤四：基于公式(3)所示的碰撞评估策略来确定是否与所有障碍物碰撞。如果安全(collision_flag＝0)，转到步骤五，否则转到步骤六。

步骤五：turn_flag的值重置为0，期望航向转为向目标点航行α_expect＝α_goal，转到步骤十三。turn_flag记录无人艇的历史转向方向，在一次规避动作结束前仅向相同方向增大或减少期望航向，以避免任何大范围航向变化并最大限度地保证无人艇航行稳定。

步骤六：由公式(1)计算第i个障碍物(Obs_i)的相对速度ΔV_i。

步骤七：判断无人艇是否与Obs_i发生碰撞。如果没有发生碰撞转到步骤十一，否则转到步骤八。为了避免由不确定的障碍物引起的任何误报，根据不同航速选择避碰距离(Avoid_dis)，仅考虑在避碰距离内的障碍物，避碰距离可以由下式确定：

步骤八：判断turn_flag的值，如果turn_flag≠0则转到步骤十，否则转到步骤九。

步骤九：turn_flag可由下式定义，右舷转向为1，左舷转向为2。

步骤十：由公式(5)，基于turn_flag计算相对第i个障碍物Obs_i的速度和航向改变量。

步骤十一：对下一个障碍物重复相同的碰撞预测及为危险规避计算，直到所有障碍都被计算过。

步骤十二：选择航向变化量最大的障碍物(Obs_max)作为参考障碍物。计算期望航向与航速(V_{A_expect},α_expect)。

步骤十三：发送指令信息(V_{A_expect},α_expect)运动控制系统并进入下一周期循环，转到步骤二。

结合图5a-图5d、图6、图7，显示40节航速高速无人艇自主危险规避的一次海上试验数据。图5a-图5d为监控系统实时显示的避碰过程，其中箭头表示无人艇位置与航向，实线为无人艇实际航迹，圆形表示障碍物位置与尺寸。图6、图7中的两条线分别表示期望航向与航速和无人艇实际航向与航速。从试验结果可以看出，本方法可以有效的实现高速无人艇的自主危险规避，整个危险规避过程中，期望航向保持相对稳定，正确引导无人艇转向并避开障碍物，无航向反复调整情况出现。保证了高速无人艇自主航行的安全性。

Claims (6)

1.一种适用于高速水面无人艇的自主危险规避方法，其特征是：

步骤1：获取当前无人艇状态、指令信息以及周围障碍物信息；

步骤2：通过高可信局部环境建模方法建立无人艇环境模型；

步骤3：输出当前环境模型中已稳定障碍物信息；

步骤4：通过基于速度障碍物的高可靠航向稳定保持方法得到新的指令信息；

步骤5：输出新的指令信息。

2.根据权利要求1所述的适用于高速水面无人艇的自主危险规避方法，其特征是所述的通过高可信局部环境建模方法建立无人艇环境模型的方法为：

步骤2.1：环境模型初始化，模型中的障碍物数量等于0；

步骤2.2：更新当前时间节拍t，将环境模型的中心设置为无人艇的地理坐标(lon_ship,lat_ship)，并将范围obstacle_model_range设置为当前雷达探测范围；

步骤2.3：确定所有障碍物obstacle_model是否在当前模型范围内；如果第j个障碍物obstacle_model_j在当前模型范围之外，则将其从环境模型中移除，并且模型中的障碍物数量减1；

步骤2.4：使用当前经纬度(lon_ship,lat_ship)和航向α，积分障碍物的相对距离D，相对方向角度θ，第i个障碍物的地理坐标由下式计算：

ε＝30.92m为经度相邻两秒之间的距离；

步骤2.5：确定第i个障碍物obs_i是否在更新的距离d之外，如果第i个障碍物在更新的距离内，转到步骤2.7，d是航海雷达的盲区；

步骤2.6：确定obs_i是否为新的障碍物，与当前环境模型中所有其他障碍物的距离和半径进行比较，如果obs_i和当前模型中障碍物obstacle_model_j之间的距离小于其半径R_i，并且两个障碍物的半径之差不超过R_i一半，则obs_i是已存在障碍物，更新其位置和大小；使用障碍物模型标志位obstacle_model_flag，该标志位值将随着成功匹配的次数增加，如果没有匹配成功，则obs_i是一个新的障碍物，并且模型中的障碍物数量obstacle_model_num加1；

步骤2.7：对下一个障碍物重复相同的障碍物匹配过程并返回步骤2.4，直到完成所有障碍物匹配；

步骤2.8：将满足阈值ξ的环境模型中障碍物的位置和大小信息发送给路径规划系统；

步骤2.9：返回步骤2.9，进入下一个环境模型构建周期。

3.根据权利要求2所述的适用于高速水面无人艇的自主危险规避方法，其特征是所述的基于速度障碍物的高可靠航向稳定保持方法为：

步骤4.1：初始化，将期望航速和航向赋值为无人艇的当前航速和航向，V_{A_expect}＝V_A，α_expect＝α；

步骤4.2：对于每个时间节拍t，根据惯导和GPS信息更新当前航速和航向V_A和α；

步骤4.3：基于下式计算目标点航向α_goal，

其中(lon_ship,lat_ship)和(lon_goal,lat_goal)是无人艇和目标点的纬经度；

步骤4.4：基于公式

所示的碰撞评估策略来确定是否与所有障碍物碰撞，如果安全即collision_flag＝0，转到步骤4.5，否则转到步骤4.6，γ和μ是相对速度△V与视线AB的夹角γ和安全角μ；

步骤4.5：turn_flag的值重置为0，期望航向转为向目标点航行α_expect＝α_goal，转到步骤4.13，turn_flag记录无人艇的历史转向方向，在一次规避动作结束前仅向相同方向增大或减少期望航向；

步骤4.6：由公式

计算第i个障碍物(Obs_i)的相对速度ΔV_i，V_A、V_B为航速，β为艇首向角，△V为V_A与V_B的合速度，为方向，γ为与视线AB连线夹角，V_θ为把△V分解为沿视线AB方向分速度V_S和垂直于AB方向分速度；

步骤4.7：判断无人艇是否与Obs_i发生碰撞，如果没有发生碰撞转到步骤4.11，否则转到步骤4.8；

步骤4.8：判断turn_flag的值，如果turn_flag≠0则转到步骤4.10，否则转到步骤4.9；

步骤4.9：turn_flag由下式定义，右舷转向为1，左舷转向为2，

步骤4.10：由公式

基于turn_flag计算相对第i个障碍物Obs_i的速度和航向改变量；

步骤4.11：对下一个障碍物重复相同的碰撞预测及为危险规避计算，直到所有障碍都被计算过；

步骤4.12：选择航向变化量最大的障碍物Obs_max作为参考障碍物，计算期望航向与航速V_{A_expect},α_expect；

步骤4.13：发送指令信息V_{A_expect},α_expect运动控制系统并进入下一周期循环，转到步骤4.2。

4.根据权利要求3所述的适用于高速水面无人艇的自主危险规避方法，其特征是：环境模型范围obstacle_model_range设置为当前雷达探测范围，obstacle_model_range基于下式计算，

其中雷达盲区为200米。

5.根据权利要求4所述的适用于高速水面无人艇的自主危险规避方法，其特征是：采用位置与尺寸相结合方式实现环境模型中障碍物匹配，障碍物可信度阈值，ξ根据如下计算：

6.根据权利要求3、4或5所述的适用于高速水面无人艇的自主危险规避方法，其特征是，避碰距离由下式确定：

Images