[发明专利]一种适用于高速水面无人艇的自主危险规避方法

权利要求书

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

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

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

步骤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.2，进入下一个环境模型构建周期；

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

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

步骤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：选择航向变化量最大的障碍物Pbs_max作为参考障碍物，计算期望航向与航速V_{A_expect},α_expect；

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

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