[发明专利]基于准均匀样条曲线的无人机路径修正方法及系统

摘要

本发明公开一种基于准均匀样条曲线的无人机路径修正方法及系统，其通过建立动态障碍物模型，规划一条连接起点和终点的直线段路径；并以直线段之间的交点为管制点，直线段路径为控制多边形，绘制准均匀三次B样条曲线；找出与障碍物相交的曲线段以及相交曲线段所对应直线段，在对应直线段上添加控制点，调整曲线使其通过该控制点；更新管制点组和曲线，并根据无人机的曲率约束和安全约束微调管制点，最终得到无人机飞行路径。本发明采用增加控制点的办法来修正B样条曲线，使曲线逐渐靠近由控制点构成的多边形，从而使无人机飞行路径能够精确避开障碍物，同时既满足飞行路径全段C2曲率连续性，又使不需要修正的曲线段尽可能多地保持原样。

主权项

1.一种基于准均匀样条曲线的无人机路径修正方法，其特征在于，所述基于准均匀样条曲线的无人机路径修正方法包括如下步骤：/nS1、建立动态障碍物模型，规划一条连接起点和终点的直线段路径；/nS2、以直线段之间的交点为管制点，直线段路径为控制多边形，绘制准均匀三次B样条曲线；/nS3、找出与障碍物相交的曲线段以及相交曲线段所对应直线段，在对应直线段上添加控制点，调整曲线使其通过该控制点；/n所述步骤S3包括以下分步骤：/nS31、找出与障碍物相交的曲线段，以及相交曲线段所对应直线段的四个管制点；/nS32、根据顺序连接四个管制点的形状，确定在对应直线段上添加控制点以及添加控制点的数量；/nS33、根据控制点对曲线进行调整，使曲线通过添加的控制点；/n步骤S32包括下列情况；/n情况一：如果四个管制点的连接形状为四边形，则在对应直线段上添加一个控制点，使曲线通过该控制点；/n情况二：如果四个管制点的连接形状为闪电形，则将位于四个管制点中间的两个管制点中的其中一个管制点向相邻两个管制点的方向进行调整，使闪电形调整为四边形，之后再根据四边形的规则进行调整；/n如果四个管制点的连接形状为四边形，且在对应直线段上添加一个控制点之后仍无法避开障碍，则取消该控制点，并在对应直线段上添加两个控制点，将修正的曲线的中间段变成直线段，使曲线通过该直线段；最佳控制点的求取步骤如下：/n1)在对应直线段上选择多个点，分别以每一个点为添加控制点来修正路径；/n2)判断每一条修正路径是否还与障碍物相交，剔除相交的路径，保留不相交路径所对应的控制点；/n3)根据控制点的曲率和安全距离的权值，计算出每一个控制点修正后曲线的最大曲率和最短安全距离，代入到下列公式中：/n