一种基于APF-IRRT融合算法的救灾无人机路径规划方法

1.一种基于APF-IRRT融合算法的救灾无人机路径规划方法，其特征在于，包括以下步骤：步骤1、参数初始化，包括以下步骤：S1、定义任务空间与状态空间；S2、定义无人机动力学模型；S3、规划参数初始化，包括：定义起点 Q start 与目标点 Q goal ，确定算法参数，定义搜索树；步骤2、节点扩展，包括以下步骤：S4、吸引力与斥力计算；S5、自适应搜索范围，包括：根据合力方向限制采样角度范围并在该角度范围内搜索随机采样点 P rand ，碰撞检测与节点选择；S6、动态步长调整；S7、新节点生成；步骤3、路径优化与平滑，包括以下步骤：S8、计算转向角并设定约束条件；S9、路径切割与优化，包括：路径切割与局部路径优化，所述路径切割具体为：对生成的全局路径进行分段切割，所述局部路径优化具体为：对每一段路径，检查起点和终点之间的直线是否穿越障碍物，若无碰撞，删除中间节点，直接连接两节点，减少转弯次数，并确保转向角满足约束条件；S10、路径平滑：使用曲线拟合或多项式插值，对路径进行平滑处理，减小路径的曲率变化；步骤4、算法终止条件，包括：到达目标点和达到最大迭代次数，所述到达目标点的具体条件为：当新节点 Q new 与目标点 Q goal 之间的距离满足; ,其中， 表示预设的收敛阈值，即新节点到目标点的距离容差；结束搜索，输出最终路径；所述达到最大迭代次数具体为：若达到最大采样次数COUNT_MAX，结束搜索，返回未找到可行路径的信息；步骤5、输出结果：输出优化和平滑后的无人机飞行路径。 2.根据权利要求1所述的一种基于APF-IRRT融合算法的救灾无人机路径规划方法，其特征在于，所述步骤S1具体为：定义所述任务空间 Z ： ，其中， Z free 表示飞行的安全区域，即无障碍区域， Z obs 表示有障碍物的危险区域；则所述状态空间的表达式为： ，即安全区域与障碍物区域互不重叠。 3.根据权利要求1所述的一种基于APF-IRRT融合算法的救灾无人机路径规划方法，其特征在于，所述步骤S2中的无人机动力学模型的表达式为： ，其中， 表示无人机状态 x ( t )的变化率， x ( t )表示无人机状态，包括：位置信息和速度信息， h ( t )表示控制输入， 表示系统动力学函数， x (0) 表示系统在起始时间 t =0的状态， x 0 表示系统的初始状态。 4.根据权利要求1所述的一种基于APF-IRRT融合算法的救灾无人机路径规划方法，其特征在于，所述步骤S3中的算法参数包括：吸引力系数、斥力系数、最大步长、安全距离、最大转向角、采样角度初始值、采样角度增量和最大采样次数，所述搜索树的表达式为： ， ，其中， T 表示搜索树， Q start 表示路径规划的起点坐标。 5.根据权利要求1所述的一种基于APF-IRRT融合算法的救灾无人机路径规划方法，其特征在于，所述步骤S4中吸引力与斥力计算的表达式为： ， ， ，其中， Q goal 表示路径规划的目标点坐标， Q near 表示当前搜索树中最近的节点坐标，ζ表示吸引力系数，F att 表示吸引力向量， Q obs 表示障碍物的坐标， η 表示斥力系数， 表示当前节点到障碍物的距离， 表示安全距离，F rep 表示斥力向量，F total 表示合力向量。 6.根据权利要求1所述的一种基于APF-IRRT融合算法的救灾无人机路径规划方法，其特征在于，所述步骤S5中根据合力方向限制采样角度范围的表达式为： ， ，其中， θ init 表示采样角度初始值，即合力方向的角度， F total,x 表示合力在 x 方向的分量， F total,y 表示合力在y方向的分量，Δ θ 表示采样角度增量；所述步骤S5中碰撞检测与节点选择具体为：对当前搜索树中最近的节点 Q near 和随机采样点 P rand 之间的路径进行碰撞检测，若路径未穿越障碍物，则保留随机采样点 P rand ，作为候选节点；若路径穿越障碍物，则增大角度范围： ，并继续在新的角度范围内采样；若在所有角度内均未找到合适节点，则丢弃随机采样点 P rand ，并重新随机采样新的点。 7.根据权利要求1所述的一种基于APF-IRRT融合算法的救灾无人机路径规划方法，其特征在于，所述步骤S6中动态步长调整的表达式为： ，其中， S v 表示动态步长， k 表示步长调整比例常数， d 表示当前节点到障碍物的距离， 表示安全距离，step max 表示最大步长，即节点扩展时允许的最大移动距离。 8.根据权利要求1所述的一种基于APF-IRRT融合算法的救灾无人机路径规划方法，其特征在于，所述步骤S7包括以下步骤：计算新节点 Q new 和进行碰撞检测，所述新节点 Q new 的计算公式为： ， ， ，其中， Q new 表示新节点坐标， Q near 表示当前搜索树中最近的节点坐标， S v 表示动态步长，F total 表示合力向量，step max 表示最大步长， F x 表示合力在 x 方向的分量， F y 表示合力在 y 方向的分量；所述碰撞检测具体为：对 Q near 和 Q new 之间的路径进行碰撞检测，若无碰撞，则将 Q new 添加到搜索树中；若有碰撞，返回步骤S5,调整角度范围，重新采样。 9.根据权利要求1所述的一种基于APF-IRRT融合算法的救灾无人机路径规划方法，其特征在于，所述步骤S8中转向角的计算公式为： ，其中，γ表示转向角， Q new 表示新节点坐标， Q near 表示当前搜索树中最近的节点坐标， Q prev 表示 Q near 的父节点坐标， 表示向量点积；所述约束条件为：若∣γ∣>γ max ，则需要对路径进行优化，γ max 表示最大允许转向角度。 10.根据权利要求1所述的一种基于APF-IRRT融合算法的救灾无人机路径规划方法，其特征在于，所述步骤S10中曲线拟合平滑为样条曲线平滑，包括以下步骤：（1）、输入离散路径点：设原始路径由一系列节点组成： ，其中， Q 1 表示起点， Q n 表示终点， Q i 表示路径规划中的第 i 个节点， x i 表示节点 Q i 在 x 轴上的坐标， y i 表示节点 Q i 在 y 轴上的坐标， n 表示路径中的点的总数；（2）、构建三次样条函数：在相邻两个路径点之间 Q i 和 Q i+ 1 ，构建三次多项式函数 S i ( t )： ，其中， S i ( t )表示第 i 段样条函数， t 表示标准化参数， 表示多项式系数，通过边界条件求解；（3）、设置边界条件，包括：位置连续、一阶导数连续、二阶导数连续和起点与终点条件，所述位置连续为：在每个节点处，曲线必须连续，即： ，其中， S i (1)表示第 i 段样条函数在 t =1处的值， S i+ 1 (0) 表示第 i+ 1段样条函数在 t =0处的值；所述一阶导数连续为：在相邻段的交界点速度必须连续，即： ，其中， 表示第 i 段样条函数对 t =1的一阶导数， 表示第 i+ 1段样条函数对 t =0的一阶导数；所述二阶导数连续为：在相邻段的交界点，加速度必须连续，即： ，其中， 表示第 i 段样条函数对 t =1的二阶导数， 表示第 i+ 1段样条函数对 t =0的二阶导数；所述起点与终点条件的表达式为： ，其中， v start 表示起点的速度边界条件， v end 表示终点的速度边界条件；（4）、求解多项式系数：根据边界条件，构建一个线性方程组，解出 ，使用高斯消元法求解方程组，所述方程组的表达式为： ， ， ， ， ， ，其中， i 表示路径中的索引编号， n 表示路径中的节点总数， y i 表示路径中第 i 个节点的纵坐标， y i+ 1 表示路径中第 i+ 1个节点的纵坐标；（5）、生成平滑路径：在每个段上，用样条函数 S i ( t ) 计算连续点，形成平滑路径： ，其中， P s mooth 表示平滑后的路径点集合， t 表示标准化参数；（6）、输出平滑路径：将平滑后的路径点输出，路径更加光滑，适合无人机飞行。