[发明专利]基于Hybird A*算法的多舰载无人机路径规划方法

权利要求书

1.一种基于Hybird A*算法的多舰载无人机路径规划方法，其特征在于，包括以下步骤：

S1、建立舰载无人机环境模型，具体包括：

S1-1、根据舰面轮廓选取特征坐标点，并基于特征坐标点计算分段函数，得到首尾连接的用于描述舰面轮廓的多边形；

S1-2、计算分段函数中的整数点，并赋值为1，将其余点赋值为0；

S1-3、根据舰载无人机最小包络圆半径确认障碍物扩展宽度；

S1-4、根据分段函数和障碍物扩展宽度，计算距离分段函数障碍物扩展宽度处的平行于分段函数的方程，并将平行线之间的整点赋值为1，得到甲板平面模型；

S1-5、根据机库选取特征坐标点，依次相连得到对应多边形，即得到机库环境模型；

其中甲板平面模型和机库环境模型共同构成舰载无人机环境模型；

S2、基于舰载无人机环境模型，获取舰载无人机的起始点和目标点；

S3、基于障碍物和单架舰载无人机约束条件，将舰载无人机的起始点和目标点输入Hybird A*算法，计算得到单架舰载无人机的目标路径，具体包括：

S3-1、初始化Hybird A*算法的OpenList列表和CloseList列表；

S3-2、将舰载无人机的起始点加入OpenList列表；其中舰载无人机的起始点包括舰载无人机的横坐标、纵坐标和航向角；

S3-3、以起始点为起始搜索扩展点，并选取某个扩展点为当前扩展点，计算舰载无人机的起始点到当前扩展点之间的距离，即得到当前起始点到当前扩展点的实际代价；

S3-4、忽略障碍物，基于单架舰载无人机约束条件，通过Reeds—Shepp曲线算法计算得到当前扩展点到目标点的距离；

S3-5、基于障碍物，通过传统A*算法计算得到扩展点到目标点的距离；

S3-6、比较步骤S3-4和步骤S3-5的结果，取最大值，即得到当前扩展点到目标点的启发代价；

S3-7、将当前起始点到当前扩展点的实际代价值和当前扩展点到目标点的启发代价值相加，得到当前选取的扩展点对应的总代价值；

S3-8、计算所有扩展点对应的总代价值，选取最小值作为当前寻访点；

S3-9、将扩展点存入OpenList列表以及将寻访点存入CloseList列表；

S3-10、判断寻访点和目标点是否在同一个网格中，若是则得到单架舰载无人机的目标路径，并结束；否则进入步骤S3-11；

S3-11、以当前寻访点为起始搜索新的扩展点；

S3-12、判断当前新的扩展点是否在CloseList列表中，若是则忽略新的扩展点，并进行下一个新的扩展点判断；否则进入步骤S3-13；

S3-13、计算当前新的扩展点总代价值，判断当前新的扩展点是否在OpenList列表中，若是则比较当前新的扩展点和对应OpenList列表中相同的扩展点的总代价值，将两者中总代价值更大的扩展点对应的寻访点替换总代价值更小的扩展点对应的寻访点，得到替换后的寻访点，并进入步骤S3-14；否则将当前新的扩展点加入OpenList列表，并返回步骤S3-12进行下一个新的扩展点判断；

S3-14、判断替换后的寻访点和目标点是否在同一个网格中，若是则得到单架舰载无人机的目标路径，并结束；否则进入步骤S3-15；

S3-15、判断是否完成N次扩展，若是则进行Reeds—Shepp曲线直接得到单架舰载无人机的目标路径，并进入步骤S3-16；否则返回步骤S3-11；

S3-16、判断当前单架舰载无人机的目标路径是否可无碰撞通行，若是则完成规划；否则返回步骤S3-11；

S4、基于单架舰载无人机的目标路径、障碍物和多架舰载无人机约束条件，计算得到多架舰载无人机的行驶路径，完成多架舰载无人机的路径规划，具体包括：

S4-1、获取当前舰载无人机的路径距离，并根据路径距离确认当前舰载无人机到达位置点；

S4-2、基于达到位置点，计算得到当前舰载无人机到达各个指定位置的时间节点；其中指定位置的时间节点包括舰载无人机在机库时的起始运动时刻、舰载无人机到达甲板的时刻、舰载无人机达到起飞位时刻、舰载无人机等待导流板升起后的起飞时刻，以及舰载无人机离舰时刻；

S4-3、计算其他舰载无人机到达各个指定位置的时间节点；

S4-4、获取并判断当前舰载无人机起始运动时刻到起飞位准备起飞时刻的时间窗与其他舰载无人机起始运动时刻到起飞位准备起飞时刻的时间窗是否重叠，若是则进入步骤S4-5；否则进入步骤S4-6；

S4-5、循环判断当前舰载无人机与其他舰载无人机轨迹是否碰撞，每检测到一次碰撞将当前舰载无人机各个指定位置的时间节点整体延迟一个时间间隔，直到与其他舰载无人机轨迹均不碰撞时结束循环并进入步骤S4-6；

S4-6、获取并判断当前舰载无人机起飞位准备起飞时刻到离舰时刻的时间窗与其他舰载无人机起飞位准备起飞时刻到离舰时刻的时间窗是否重叠，若是则进入步骤S4-7；否则进入步骤S4-8；

S4-7、循环判断当前舰载无人机与其他舰载无人机轨迹是否碰撞，每检测到一次碰撞将当前舰载无人机的等待导流板升起后的起飞时刻和离舰时刻均延迟一个时间间隔，直到与其他舰载无人机轨迹均不碰撞时结束循环并进入步骤S4-8；

S4-8、判断是否只有当前舰载无人机正在进行加速起飞，若是则正常行驶，并返回步骤S4-1进行下一架舰载无人机的规划，直到完成所有舰载无人机路径规划；否则将当前舰载无人机的等待导流板升起后的起飞时刻和离舰时刻均延迟一个时间间隔，并返回步骤S4-7。

2.根据权利要求1所述的基于Hybird A*算法的多舰载无人机路径规划方法，其特征在于，步骤S4-1中获取舰载无人机的路径距离和到达位置点的具体方法为：

S4-1、根据公式：

得到舰载无人机的路径距离l；其中a舰载无人机的加速度，v_max为舰载无人机的最大速度，t为时间，Δt为时间间隔，T为舰载无人机的起飞任务的总时间；

S4-2、根据公式：

得到舰载无人机t+1时刻的位置点(x_t+1，y_t+1)；其中x_t为舰载无人机t时刻的横坐标，y_t为舰载无人机t时刻的纵坐标，cos为余弦函数，θ_t为舰载无人机航向角。