CN116893687A - 一种复杂环境下改进lazy theta*的无人机路径规划方法 - Google Patents

Abstract

本发明公开了一种复杂环境下改进lazy theta*的无人机路径规划方法，包括下述步骤：建立地图：将空间分解为方形网格的栅格单元,获得环境的静态全局地图，设置起点x_start和终点x_goal；寻找初始路径集合：使用改进的lazy theta*算法寻找一条安全的初始路径集合Path；调整初始路径集合：利用抛物线的梯度信息调整初始路径中转折点之间的节点分布，使节点分布满足中间少，两端多的对称分布情况，并令节点按照路径代价g(n)升序排序得到处理后的路径集合path；优化路径集合；本发明旨在提供一种复杂环境下改进lazy theta*的无人机路径规划方法，融合改进的lazy theta*算法和改进的人工势场法的路径规划算法，能够实现无人机对目标点的全局最优路径的搜索以及局部的动态避障。

Description

一种复杂环境下改进lazy theta*的无人机路径规划方法

技术领域

本发明涉及无人机技术领域，尤其涉及一种复杂环境下改进lazy theta*的无人机路径规划方法。

背景技术

无人机机动性强，灵活度高，体积小等特点使其能够代替人类完成如洞穴勘探、灾害救援、电力巡检，警用侦察等一些危险的飞行任务，这对于挽救人类财产有着重大意义。因此为了使无人机能够更好地完成在陌生环境的巡视，勘探的任务，我们需要规划出合理安全的飞行路径，使无人机能够应对各种复杂环境，并自主地完成对目标点的飞行。

无人机路径规划的算法可以分为全局路径规划算法和局部路径规划算法。传统的A星算法是一种很常用的路径查找和图形遍历算法，它根据代价函数的大小选择相邻节点进行搜寻，直至搜寻到终点，该算法有着较好的性能和准确度。但是它的缺点也很明显：(1)A星算法的检索方向只有8个方向，移动的角度只能是45°的倍数，搜索路径受到角度的限制，导致得到的路径并不是实际最优路径。(2)A星算法得到的节点大部分都是靠近障碍物的，对于无人机的实际飞行来说这种路径点并不安全。(3)不具备动态避障的能力。

发明内容

本发明的目的在于提出一种复杂环境下改进lazy theta*的无人机路径规划方法，融合改进的lazy theta*算法和改进的人工势场法的路径规划算法，能够实现无人机对目标点的全局最优路径的搜索以及局部的动态避障。

为达此目的，本发明采用以下技术方案：一种复杂环境下改进lazy theta*的无人机路径规划方法，包括下述步骤：

建立地图：将空间分解为方形网格的栅格单元,获得环境的静态全局地图，设置起点x_start和终点x_goal；

寻找初始路径集合：使用改进的lazy theta*算法寻找一条安全的初始路径集合Path；

调整初始路径集合：利用抛物线的梯度信息调整初始路径中转折点之间的节点分布，使节点分布满足中间少，两端多的对称分布情况，并令节点按照路径代价g(n)升序排序得到处理后的路径集合path；

优化路径集合：使用B样条曲线对处理后的路径集合path进行平滑处理；

动态避障完成任务：使用改进的人工势场法实现动态避障功能，并根据不同的障碍物情况应用动态避障的方案完成任务。

优选的，在所述建立地图的步骤中，具体包括将空间分解为方形网格的栅格单元，每个方形网格的中心作为确定网格属性的节点的位置，即每个网格有两种状态：可通行和不可通行，根据栅格是否可以通行来表示环境中的障碍物,如果栅格不可通行，则此区域存在障碍物，用黑色栅格表示，数值上用1表示；反之，白色栅格表示自由通行区域，没有障碍物存在，数值上用0表示，获得全局地图，设置起点x_start和终点x_goal。

优选的，在所述寻找初始路径集合的步骤中，改进的Lazy theta*算法具体的路径规划具体为：

步骤1：初始化起点x_start和终点x_goal，并设置存放待检测节点的集合open_collection和用于已检测过节点的集合close_collection；

步骤2：初始化起点x_start的成本值f(start)，并将起点x_start加入到集合close_collection；

步骤3：判断open_collection是否为空集，如果是空集则搜索路径失败，没有找到路径，否则执行步骤4；

步骤4：从集合open_collection取出成本值f(x)最小的节点x，然后执行步骤5；

步骤5：检测节点x与其父节点x_parent的连线上是否存在LOS，若存在，则执行步骤7，若不存在，则保留x_parent作为节点x的父节点，然后则执行步骤6；

步骤6：节点x与节点parent(x)不存在LOS，则从close_collection中寻找满足LOS的相邻节点x_near，然后将节点x的父节点设置为相邻节点x_near，然后更新成本值f(x)，跳转步骤7；

步骤7：判断当前节点x是否就是终点x_goal,如果是则说明找到了最终路径，跳转到步骤9，否则将当前节点x加入到close_collection，然后执行步骤8；

步骤8：检索当前节点x的相邻节点x_near，检索步骤具体如下：

子步骤1：判断x_near是否已经存在于close_collection中或者是障碍物点，若是，则检索下一个的相邻节点，若不是则执行子步骤2；

子步骤2：判断x_near是否存在open_collection，若已经存在于open_collection，则重新计算成本值f(x_near)作为新的成本值，如果新的成本值小于原本的成本值，则需要更新该节点的成本值和父节点，否则若新的成本值大于原有成本值，则保留原有的成本值和父节点的设置；若x_near还没存在于open_collection，则将节点x_near加入到open_collection，然后计算该节点的成本值f(x_near)；执行子步骤3；

子步骤3：若检索完所有相邻节点，则跳转到步骤3，否则检索下一个相邻节点；

步骤9：路径搜索完成，从终点回溯到起点，可以得到由起点x_start，终点x_goal和转折点组成的路径点集合Path＝{P₁，P₂，...，P_m}，其中P₁是起点x_start，P_m是终点x_goal。

优选的，在所述寻找初始路径集合的步骤中，具体包括获取每个节点到最近障碍物的距离成本，使选择的节点远离障碍物，得到更安全的路径，障碍物距离成本函数c_o(x)为：

c_o(x)＝C_max·e^-α*D(x)；

其中D(x)是从当前节点s到最近障碍物的欧式距离，以米为单位；d是指数衰减的比例因子，用于调节衰减比；C_max是节点靠近障碍物的最大成本；e是自然对数的底数；

以当前节点s到最近障碍物的距离和设定最小和最大安全距离的关系作为判定条件，引入了启发函数h(x)动态权值c_h，其公式为：

其中h(x)是当前节点x到终点的欧式距离；c_h为h(x)的权值；c_h_min是c_h设定的最小值；c_h_max是c_h设定的最大值；D是起点到终点的欧式距离；d_safe_min是设定的最小安全距离；d_safe_max是设定的最大安全距离。

优选的，在所述寻找初始路径集合的步骤中，还包括LOS可视性判断步骤：

两节点之间是否可视：判断两点间的连线与障碍物栅格是否保持最小安全距离d_safe，设e₁的坐标为(x_e₁，y_e₁)，e₂的坐标为(x_e₂，y_e₂)；

连接e₁和e₂得到直线e₁e₂，直线e₁e₂的斜率为：

直线e₁e₂的方程为：

y＝tanθ*x-tanθ*x_e₁+y_e₁；

遍历区域内的每一个栅格点，如果该栅格不是障碍物则忽略，若该栅格点是障碍物，则通过点到直线的公式计算该栅格点的坐标obs_i(x_obs_i，y_obs_i)到直线的距离：

其中A＝-tanθ，B＝1，C＝tanθ*x_e₁-y_e₁，d_safe_min为设定的最小安全距离；

若d＞d_safe_min，则说明直线与该栅格障碍物保持安全距离，两点间存在可视的直线，否则，则说明直线经过障碍物或者存在碰撞风险，也即e₁，e₂不存在可视性。

优选的，在所述寻找初始路径集合的步骤中，Lazy theta*算法中节点的代价函数由三部分组成，包括：

f(x)＝c_gg(x)+c_hh(x)+c_w*c_o(x)；

h(x)＝c(x_start，x_goal)；

g(x)＝g(x′)+c(x′，x)；

c_g＝1-c_h；

其中，f(x)为当前节点x的总代价成本；g(x)为起点到当前节点的实际成本函数，即起点节点x_start到当前节点x的父节点x’的实际路径成本再加上父节点x’到节点x的欧氏距离；c_g为g(x)的权重值；h(x)为启发函数，即当前节点x到终点节点x_goal的预计路径成本，等于当前节点x到终点节点x_goal的欧式距离；c_h是h(x)的权重值；c_o(x)为障碍物的距离成本函数；c_w为距离成本的权重因子；c(x，x′)用于表示节点x到节点x’的路径成本，为两点的欧氏距离。

优选的，在所述调整初始路径集合的步骤中，包括对初始路径集合Path进行预处理，其步骤包括：

确定节点件的欧式距离：设初始路径集合Path中一共有m个节点，初始路径集合Path＝{P₁，P₂，...，P_m}，设节点i的坐标为(P_x_i，P_y_i)，根据欧式距离公式获得任意两个连续的节点之间的距离：

根据各段线段之间的欧式距离确定增加的节点个数N_i，确定两连续转折点的坐标连线的倾斜角θ_i，其公式为：

若则采取纵向取点，即根据横坐标的差值去确定拐点间可保留节点个数N’：

若则采取横向取点，则根据纵坐标的差值去确定拐点间可保留节点个数N’：

其中，d_w为栅格地图的分辨率；[x]为取整函数，表示不超过x的最大整数；N’为拐点间可保留节点个数，N’是一个正整数；γ为节点个数权重因子，γ∈[0.5，1]；

确定各段线段的中心点坐标：

设拐点P_i和拐点P_i+1中心点为flex_mid_i，其坐标为(P_mid_x_i，P_mid_y_i)，则：

中心点的路径代价g(flex_mid_i)为：

g(flex_mid_i)＝g(P_i)+c(P_i，flex_mid_i)；

利用抛物线的梯度信息对拐点间的中间路径点进行重分布：

以中间路径点个数作为横坐标，拐点的中心点到右端拐点的欧式距离/>作为纵坐标，令/>获得抛物线曲线：

s²＝2pn；

其中，将n＝1，2，…，N_i，代入抛物线函数获得到右端各个节点相对中心点距离的偏差值s_i，n：

根据相对距离的偏差值s_i，n和两连续拐点之间连线的倾斜角θ_i可以得到右端新增节点的坐标及其路径代价值，设右端新增节点right_i，n的坐标为(P_right_x_i，n，P_right_y_i，n)，则横纵坐标的值为：

P_right_x_i，n＝P_mid_x_i+s_i，n*cosθ_i；

P_right_y_i，n＝P_mid_y_i+s_i，n*sinθ_i；

起点到该点的路径代价g(right_i)为：

g(right_i)＝g(flex_mid_i)+s_i，n；

设左端新增节点left_i，n的坐标为(P_left_x_i，n，P_let_y_i，n)，则横纵坐标的值为：

P_left_x_i，n＝P_mid_x_i-s_i，n十cosθ_i；

P_left_y_i，n＝P_mid_y_i-s_i，n*sinθ_i；

起点到该点的路径代价g(left_i，n)为：

g(left_i，n)＝g(flex_mid_i)-s_i，n；

获得处理后的路径集合path：将所计算得到的关键点根据路径代价值g(n)从小到大进行排序得到处理后的关键路径点集合path＝{p₁，p₂，…，p_k}，k为处理后的路径集合path的路径点个数。

优选的，在所述动态避障完成任务的步骤中，具体包括：

设无人机在二维中操作并且无人机的位置表示为s＝(x，y)^T，重力场函数的公式为：

其中k_a表示重力增益系数，p(s，s_goal)表示无人机当前位置s和目标点s_goal之间的距离，无人机在重力场的负梯度方向上接收重力，即重力F_att(s)的公式为：

F_att(s)＝-k_ap(s，s_goal)；

障碍物对无人机产生的排斥场函数为：

其中k_r是排斥力增益系数，p(s，s_obs)是无人机和障碍物之间的最短距离，s_max是障碍物的最大作用范围；

根据排斥场函数，无人机上的排斥力公式为：

其中，F_req1(s)为障碍物指向无人机的模拟力向量分量，F_req2(s)为无人机指向目标点的模拟力向量分量：

Fre1(s)是从障碍物到无人机的分量方向，Fre2(s)则是从无人机到目标点的分量方向；

合力大小为F(n)为：

F(s)＝F_att(s)+F_req(s)；

对引力函数进行了优化：

力Fre1(s)的方向被定义为与力Fre2(s)的方向偏差α角度为：

优选的，在所述动态避障完成任务的步骤中，针对不同的障碍物的处理步骤为：

动态障碍物：无人机通过将集合Path中离当前位置最近的关键点作为局部目标点回到全局规划路线，从而跟随全局规划路径前进；

新增加的静态障碍物：

步骤A：将新增的静态障碍物信息增加到全局地图，将预处理后的集合path中离当前位置最近的关键点与新增障碍物信息判断，关键点是否与障碍物重合，若重合，则将目标点切换成下一个，若没有重合，则确定该点作为局部目标点；

步骤B：如果当前点受到引力和斥力的合力为0，则判断该点是否为局部目标点，是则说明到达局部目标点，否则说明陷入了局部最小值；当出现局部最小值时，则改变Fre1(s)的受力方向，打破原有的受力平衡，使无人机脱离局部最小值，然后重新到达局部目标点时，再将目标点切换成下一个，直至达到全局目标点；

步骤C：当无人机长时间无法到达局部目标点，则被判定原有全局路径严重受阻，需要重新全局规划，以当前点为起点，全局目标点为终点重新使用改进的lazy tehta*规划一条安全的路径；若能重新规划则执行建立地图，若不能重新规划出一条路线，即判定为新增障碍物阻断了起点和终点的路径，无法搜索出路径，则执行调整初始路径集合；

步骤D：由于无法得到一条由当前点到终点的路径，则判定为搜索失败，无人机进入返航模式，以当前点为起点，以出发点为终点搜索一条安全路径，然后跟踪轨迹回到出发点并安全降落。

本发明的一个技术方案的有益效果：针对lazy theta*算法得到的节点贴近障碍物，并且LOS可视性判定也存在让两可视节点之间的连线贴近障碍物的问题，因此本专利设计了改进的lazy theta*算法，根据障碍物的距离代价设计新的代价函数，并设计了一种对安全距离限定的LOS可视化判定，另外还为g(s)和启发函数h(s)加入了动态变化的权重，使无人机能够选择更安全合理的节点。通过本专利设计改进的lazy theta*算法，得到的节点会远离障碍物，并且两节点之间的连线也会与障碍物保持一定安全距离，这样就可以找到一条安全，距离短的最优路径。

附图说明

图1是本发明一个实施例的流程示意图；

图2是本发明一个实施例改进的lazy theta*路径规划算法的流程示意图的上部分；

图3是本发明一个实施例改进的lazy theta*路径规划算法的流程示意图的下部分；

图4是本发明一个实施例使用改进的lazy theta*路径规划算法的示意图；

图5是本发明一个实施例预处理过程的横向采点并重分布节点的示意图；

图6是本发明一个实施例预处理过程的纵向采点并重分布节点的示意图；

图7是本发明一个实施例改进的人工势场法的局部最小值问题示意图；

图8是本发明一个实施例人工势场法的动态避障方案流程图的上部分；

图9是本发明一个实施例人工势场法的动态避障方案流程图的下部分。

具体实施方式

下面结合附图并通过具体实施方式来进一步说明本发明的技术方案。

下面详细描述本发明的实施例。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

参阅图1至图9所示，一种复杂环境下改进lazy theta*的无人机路径规划方法，其特征在于，包括下述步骤：

建立地图：将空间分解为方形网格的栅格单元,获得环境的静态全局地图，设置起点x_start和终点x_goal；

寻找初始路径集合：使用改进的lazy theta*算法寻找一条安全的初始路径集合Path；

调整初始路径集合：利用抛物线的梯度信息调整初始路径中转折点之间的节点分布，使节点分布满足中间少，两端多的对称分布情况，并令节点按照路径代价g(n)升序排序得到处理后的路径集合path；

优化路径集合：使用B样条曲线对处理后的路径集合path进行平滑处理；

动态避障完成任务：使用改进的人工势场法实现动态避障功能，并根据不同的障碍物情况应用动态避障的方案完成任务。

针对lazy theta*算法得到的节点贴近障碍物，并且LOS可视性判定也存在让两可视节点之间的连线贴近障碍物的问题，因此本专利设计了改进的lazy theta*算法，根据障碍物的距离代价设计新的代价函数，并设计了一种对安全距离限定的LOS可视化判定，另外还为g(s)和启发函数h(s)加入了动态变化的权重，使无人机能够选择更安全合理的节点。通过本专利设计改进的lazy theta*算法，得到的节点会远离障碍物，并且两节点之间的连线也会与障碍物保持一定安全距离，这样就可以找到一条安全，距离短的最优路径。

由于lazy theta*算法得到的节点基本都是转折点，也称之为拐点，而拐点之间并没有路径点的信息，这样不仅不利于保留路径的特征，并且由于拐点之间的梯度信息没有变化，而拐点处会有明显的梯度变化，仅仅保留拐点的路径点会影响后端轨迹优化拐点处的的拟合效果。因此针对这两个问题，本专利设计了初始路径集合的预处理方法，利用抛物线的梯度信息实现拐点之间的节点满足中间节点少，两端节点多的对称分布。通过在拐点之间插入适当数量的节点，不仅可以保留更多的路径信息，便于后续局部路径规划的动态避障，还可以提升后端轨迹优化拐点处的曲线拟合效果，使无人机的飞行更平滑。

针对人工势场法容易陷入局部最小值的问题，本专利设计了改进的人工势场法，不仅在斥力函数中加入了目标点的影响，并且也对排斥函数的方向做出了修改，将经过预处理得到的路径点集合作为局部目标点，即以当前无人机的路径点集合中最近的关键点作为局部目标点。将局部关键点作为目标点相当于减少无人机当前位置与全局目标点之间的距离和障碍物数量，并且由于在斥力函数中加入了局部目标点的影响，这使无人机更容易到达局部目标点。而当无人机陷入最小值时，我们则对排斥函数的Fre1(s)的方向做出修改，从而打破原有的受力平衡，使无人机脱离局部最小值，然后重新到达局部目标点，进而重新跟随全局规划路径。

根据无人机在实际的动态避障的过程中可能遇到的突发情况，增加对障碍物的判定，并设计无人机针对不同障碍物的处理方案，能够使无人机的飞行更好，更安全地完成任务。

本专利设计了融合改进的lazy theta*算法和改进的人工势场法的路径规划算法，能够实现无人机对目标点的全局最优路径的搜索以及局部的动态避障。

具体地，在所述建立地图的步骤中，具体包括将空间分解为方形网格的栅格单元，每个方形网格的中心作为确定网格属性的节点的位置，即每个网格有两种状态：可通行和不可通行，根据栅格是否可以通行来表示环境中的障碍物,如果栅格不可通行，则此区域存在障碍物，用黑色栅格表示，数值上用1表示；反之，白色栅格表示自由通行区域，没有障碍物存在，数值上用0表示，获得全局地图，设置起点x_start和终点x_goal。

优选的，在所述寻找初始路径集合的步骤中，改进的Lazy theta*算法具体的路径规划具体为：

步骤1：初始化起点x_start和终点x_goal，并设置存放待检测节点的集合open_collection和用于已检测过节点的集合close_collection；

步骤2：初始化起点x_start的成本值f(start)，并将起点x_start加入到集合close_collection；

步骤3：判断open_collection是否为空集，如果是空集则搜索路径失败，没有找到路径，否则执行步骤4；

步骤4：从集合open_collection取出成本值f(x)最小的节点x，然后执行步骤5；

步骤5：检测节点x与其父节点x_parent的连线上是否存在LOS，若存在，则执行步骤7，若不存在，则保留x_parent作为节点x的父节点，然后则执行步骤6；

步骤6：节点x与节点parent(x)不存在LOS，则从close_collection中寻找满足LOS的相邻节点x_near，然后将节点x的父节点设置为相邻节点x_near，然后更新成本值f(x)，跳转步骤7；

步骤7：判断当前节点x是否就是终点x_goal,如果是则说明找到了最终路径，跳转到步骤9，否则将当前节点x加入到close_collection，然后执行步骤8；

步骤8：检索当前节点x的相邻节点x_near，检索步骤具体如下：

子步骤1：判断x_near是否已经存在于close_collection中或者是障碍物点，若是，则检索下一个的相邻节点，若不是则执行子步骤2；

子步骤2：判断x_near是否存在open_collection，若已经存在于open_collection，则重新计算成本值f(x_near)作为新的成本值，如果新的成本值小于原本的成本值，则需要更新该节点的成本值和父节点，否则若新的成本值大于原有成本值，则保留原有的成本值和父节点的设置；若x_near还没存在于open_collection，则将节点x_near加入到open_collection，然后计算该节点的成本值f(x_near)；执行子步骤3；

子步骤3：若检索完所有相邻节点，则跳转到步骤3，否则检索下一个相邻节点；

步骤9：路径搜索完成，从终点回溯到起点，可以得到由起点x_start，终点x_goal和转折点组成的路径点集合Path＝{P₁，P₂，...，P_m}，其中P₁是起点x_start，P_m是终点x_goal。

具体地，在所述寻找初始路径集合的步骤中，具体包括获取每个节点到最近障碍物的距离成本，使选择的节点远离障碍物，得到更安全的路径，障碍物距离成本函数c_o(x)为：

c_o(x)＝C_max·e^-α*D(x)；

其中D(x)是从当前节点s到最近障碍物的欧式距离，以米为单位；d是指数衰减的比例因子，用于调节衰减比；C_max是节点靠近障碍物的最大成本；e是自然对数的底数，其值约为2.71828；

在实际飞行过程中，在远离障碍物的情况下，我们的启发函数在选择节点时可以相对激进一点，而在障碍物附近时，我们选择的节点则可以相对保守一点，因此恒定的g(s)和h(s)并不符合实际飞行的情况。针对lazy theta*算法中的代价函数g(s)和h(s)的权值恒定的问题，以当前节点s到最近障碍物的距离和设定最小和最大安全距离的关系作为判定条件，引入了启发函数h(x)动态权值c_h，其公式为：

其中h(x)是当前节点x到终点的欧式距离；c_h为h(x)的权值；c_h_min是c_h设定的最小值；c_h_max是c_h设定的最大值；D是起点到终点的欧式距离；d_safe_min是设定的最小安全距离；d_safe_max是设定的最大安全距离。

针对lazy theta*算法中的LOS视线检测存在两点连线贴近障碍物的问题，这样引入距离成本项以获得更安全路径的作用可能会丧失。因此在LOS的判定上做出修改，引入最小安全距离d_safe，当两点构成的矩形空间中任意一个障碍物栅格坐标点到两点连线之间的距离均大于d_safe时，才说明两点可视，否则若存在一个障碍物栅格坐标点到两点连线之间的距离小于d_safe，则说明两点不可视。

优选的，在所述寻找初始路径集合的步骤中，还包括LOS可视性判断步骤：

两节点之间是否可视：判断两点间的连线与障碍物栅格是否保持最小安全距离d_safe，设e₁的坐标为(x_e₁，y_e₁)，e₂的坐标为(x_e₂，y_e₂)；

连接e₁和e₂得到直线e₁e₂，直线e1e₂的斜率为：

直线e₁e₂的方程为：

y＝tanθ*x-tanθ*x_e₁+y_e₁；

遍历区域内的每一个栅格点，如果该栅格不是障碍物则忽略，若该栅格点是障碍物，则通过点到直线的公式计算该栅格点的坐标obs_i(x_obs_i，y_obs_i)到直线的距离：

其中A＝-tanθ，B＝1，C＝tanθ*x_e₁-y_e₁，d_safe_min为设定的最小安全距离；

若d＞d_safe_min，则说明直线与该栅格障碍物保持安全距离，两点间存在可视的直线，否则，则说明直线经过障碍物或者存在碰撞风险，也即e₁，e₂不存在可视性。

针对lazy theta*算法中的LOS视线检测存在两点连线贴近障碍物的问题，这样引入距离成本项以获得更安全路径的作用可能会丧失。因此在LOS的判定上做出修改，引入最小安全距离d_safe，当两点构成的矩形空间中任意一个障碍物栅格坐标点到两点连线之间的距离均大于d_safe时，才说明两点可视，否则若存在一个障碍物栅格坐标点到两点连线之间的距离小于d_safe，则说明两点不可视。

优选的，在所述寻找初始路径集合的步骤中，Lazy theta*算法中节点的代价函数由三部分组成，包括：

f(x)＝c_gg(x)+c_hh(x)+c_w*c_o(x)；

h(x)＝c(x_start，x_goal)；

g(x)＝g(x′)+c(x′，x)；

c_g＝1-c_h；

其中，f(x)为当前节点x的总代价成本；g(x)为起点到当前节点的实际成本函数，即起点节点x_start到当前节点x的父节点x’的实际路径成本再加上父节点x’到节点x的欧氏距离；c_g为g(x)的权重值；h(x)为启发函数，即当前节点x到终点节点x_goal的预计路径成本，等于当前节点x到终点节点x_goal的欧式距离；c_h是h(x)的权重值；c_o(x)为障碍物的距离成本函数；c_w为距离成本的权重因子；c(x，x′)用于表示节点x到节点x，的路径成本，为两点的欧氏距离。

具体地，在所述调整初始路径集合的步骤中，包括对初始路径集合Path进行预处理，其步骤包括：

确定节点件的欧式距离：设初始路径集合Path中一共有m个节点，初始路径集合Path＝{P₁，P₂，...，P_m}，设节点i的坐标为(P_x_i，P_y_i)，根据欧式距离公式获得任意两个连续的节点之间的距离：

根据各段线段之间的欧式距离确定增加的节点个数N_i，确定两连续转折点的坐标连线的倾斜角θ_i，其公式为：

若则采取纵向取点，即根据横坐标的差值去确定拐点间可保留节点个数N’：

若则采取横向取点，则根据纵坐标的差值去确定拐点间可保留节点个数N’：

其中，d_w为栅格地图的分辨率；[x]为取整函数，表示不超过x的最大整数；N’为拐点间可保留节点个数，N’是一个正整数；γ为节点个数权重因子，γ∈[0.5，1]；γ用于调整拐点之间节点的个数，避免插入过多的节点导致增加了后端轨迹优化的计算压力。

为使两转折点之间的节点分布满足中间少，两端多的特点，因此N的个数应该确保为单数，一个作为中心点，其余节点则关于中心点对称。因此若N_i′为偶数，则N_i＝N_i′+1，若N_i′为奇数，则N_i＝N_i′。

确定各段线段的中心点坐标：

设拐点P_i和拐点P_i+1中心点为flex_mid_i，其坐标为(P_mid_x_i，P_mid_y_i)，则：

中心点的路径代价g(flex_mid_i)为：

g(flex_mid_i)＝g(P_i)+c(P_i，flex_mid_i)；

利用抛物线的梯度信息对拐点间的中间路径点进行重分布：

以中间路径点个数作为横坐标，拐点的中心点到右端拐点的欧式距离/>作为纵坐标，令/>获得抛物线曲线：

s²＝2pn；

其中，将n＝1，2，…，N_i，代入抛物线函数获得到右端各个节点相对中心点距离的偏差值s_i，n：

根据相对距离的偏差值s_i，n和两连续拐点之间连线的倾斜角θ_i可以得到右端新增节点的坐标及其路径代价值，设右端新增节点right_i，n的坐标为(P_right_x_i，n，P_right_y_i，n)，则横纵坐标的值为：

P_right_x_i，n＝P_mid_x_i+s_i，n*cosθ_i；

P_right_y_i，n＝P_mid_y_i+s_i，n*sinθ_i；

起点到该点的路径代价g(right_i)为：

g(right_i)＝g(flex_mid_i)+s_i，n；

而由于左端节点与右端节点关于中心点的对称，因此也可以得到左端新增节点的坐标。设左端新增节点left_i，n的坐标为(P_left_x_i，n，P_leff_y_i，n)，则横纵坐标的值为：

P_left_x_i，n＝P_mid_x_i-s_i，n*cosθ_i；

P_left_y_i，n＝P_mid_y_i-s_i，n*sinθ_i；

起点到该点的路径代价g(left_i，n)为：

g(left_i，n)＝g(flex_mid_i)-si_，n；

获得处理后的路径集合path：将所计算得到的关键点根据路径代价值g(n)从小到大进行排序得到处理后的关键路径点集合path＝{p₁，p₂，...，p_k}，k为处理后的路径集合path的路径点个数。

由于lazy theta*算法的特点，找到的路径点为起点，终点和转折点，所得到的路径特征比较少，因此可以在拐点之间插入一定节点保留路径特征，这将可以用作后续局部规划时的局部目标点。

另外无人机转向时，转弯半径过小会降低飞行安全系数，这是由于无人机存在飞行惯性，当在很小的角度下进行转向时容易发生碰撞。因此为了保证无人机的飞行安全，需要在转折点附近插入适当节点保留转折点的梯度信息，避免转弯半径过小而与障碍物发生碰撞。

同时由于两拐点的中间没有过多梯度信息变化，而两端的拐点存在梯度变化，因此插入的节点应当满足中间少，两端多的对称分布情况。通过在拐点之间插入适当的过渡节点，在不明显影响后端轨迹优化的计算效率前提下，提升曲线的拟合效果，能够使无人机的飞行更加连续和平滑。

基于上述原因，本专利为了使拐点之间的节点实现中间节点少，两端节点多的对称分布，通过拐点连线的倾斜角θ_i和权重因子γ去确定拐点间可保留节点个数N′，然后判断N′是奇数还是偶数，若时奇数则保留，若是偶数则加1变成奇数得到N，最后以可使用的节点个数(N-1)/2为横坐标，拐点间的欧式距离的一半作为纵坐标构造抛物线函数，利用抛物线的梯度信息可以确定新增的右端节点与中心点的相对偏移位置，由于左端节点和右端节点对称，因此可以得到拐点之间的全部节点。通过在拐点之间插入适当数量的节点，不仅可以保留更多的路径信息，便于后续局部路径规划的动态避障，还可以提升后端轨迹优化拐点处的曲线拟合效果，使无人机的飞行更平滑。

在优化路径集合的步骤中，B样条曲线法是Bezier样条曲线的一个特例，通过逼近多边形而实现对航迹的平滑处理。该方法是对整个航迹综合考虑，通过采用三次B样条曲线对路径进行平滑处理，平滑后的轨迹整体上过渡自然，相邻航段间曲率变化均匀，且平滑后控制点不会发生变动，能较好的满足无人机在飞行过程中速率与加速率连续变化的要求。因此采用三次B样条曲线平滑预处理后的路径点集合path。

针对改进的lazy theta*可以得到全局最优结果，但不能实现动态避障的问题，通过引入改进的人工势场方法来解决无人机的动态避障问题。但人工势场法虽然具有良好的动态避障性能，但缺乏全局最优，本发明通过采取两种方法融合的策略来应对复杂的动态环境下的路径规划。

针对人工势场的目标不可达的问题，本发明在斥力场函数加入了目标点pⁿ(s，s_goal)的影响因素，通过在排斥函数的公式中包括从无人机到目标点的距离作为影响因素，机器人和目标点之间的线性距离随着机器人接近障碍物而变短，并且尽管排斥场仍然增加，但增加的速度比以前慢得多，在一定程度上解决了过度排斥的问题，使机器人目标可达。

人工势场法主要将无人机的工作空间抽象为人工虚拟力场，并使用梯度下降方法定义势场。无人机的目标点产生的重力F_att(s)是重力势场的负梯度方向，障碍物产生的排斥力F_req(s)是排斥场的梯度方向。

优选的，在所述动态避障完成任务的步骤中，具体包括：

设无人机在二维中操作并且无人机的位置表示为s＝(x，y)^T，重力场函数的公式为：

其中k_a表示重力增益系数，p(s，s_goal)表示无人机当前位置s和目标点s_goal之间的距离，无人机在重力场的负梯度方向上接收重力，即重力F_att(s)的公式为：

F_att(s)＝-k_ap(s，s_goal)；

障碍物对无人机产生的排斥场函数为：

其中k_r是排斥力增益系数，p(s，s_obs)是无人机和障碍物之间的最短距离，s_max是障碍物的最大作用范围；

根据排斥场函数，无人机上的排斥力公式为：

其中，F_req1(s)为障碍物指向无人机的模拟力向量分量，F_req2(s)为无人机指向目标点的模拟力向量分量：

Fre1(s)是从障碍物到无人机的分量方向，Fre2＝(s)则是从无人机到目标点的分量方向；

无人机的合力是斥力和引力的矢量叠加。合力的方向就是无人机的运动方向。在合力的作用下，无人机可以绕过障碍物，到达终点。

合力大小为F(n)为：

F(s)＝F_att(s)+F_req(s)；

路径规划为了避免人工势场法进入局部最优解，存在目标不可达的情况。对引力函数进行了优化：

现在的方程是path中离当前无人机位置最近的关键点，将集合中最近的关键点作为局部目标点，从而间接减少当前位置与目标点之间的障碍物数量，可以增大目标点的引力和减少障碍物的斥力。当到达当前目标点时，再将目标点切换成下一个，使机器人在局部路径规划中跟随全局规划的路径集合点path前进。

当无人机、目标点和障碍物在同一条直线上时，不可避免地会产生局部极小点。由于针对目标不可达的问题，我们已经对排斥函数做出修改修改，但排斥函数的方向也仍需要修改。产生局部极小点的原因是排斥力和引力之间的角度相差180°，合力为零。由于Fre1(s)是从障碍物到无人机的分量方向，Fre2(s)则是从无人机到目标点的分量方向。因此我们选择保持力Fre2(s)方向不变，并改变力Fre1(s)的方向。

力Fre1(s)的方向被定义为与力Fre2(s)的方向偏差α角度为：

其中φ为障碍物的影响范围相切得到的偏移角，δ为安全裕度偏移角；力Fre1(s)与力Fre2(s)的方向的角度小于90°，排斥力和合力之间的角度小于90°。由于原有Fre1(s)的受力方向发生变化，原有受力平衡被打破，从而解决局部极小点问题。

当全局规划的路线受到阻碍时，为了使无人机能够完成从起点到目标点的任务，我们使用改进的人工势场法进行避障。在无人机的动态避障过程中会遇到两种障碍物，一种是动态障碍物，一种是静态障碍物，而通过搭载在无人机上的激光雷达，我们可以得到障碍物的信息。

本专利对障碍物进行了定义，动态障碍物定义为运动轨迹经过原有全局规划路线的障碍物，这部分障碍信息不被保留在全局地图，而静态障碍物定义为阻挡全局规划路线的新增静态障碍物，该部分新增静态障碍物信息将被保留在全局地图，以更新全局地图并用于下次的路径规划。

优选的，其特征在于，在所述动态避障完成任务的步骤中，针对不同的障碍物的处理步骤为：

动态障碍物：动态障碍物产生的斥力是变化的，在障碍物的移动过程中，很少会出现局部最小值的情况，即便出现了也会随着动态障碍物的移动而打破受力平衡，因此无人机通过将集合Path中离当前位置最近的关键点作为局部目标点回到全局规划路线，从而跟随全局规划路径前进；

新增加的静态障碍物：除了动态障碍物之外，还可能出现新增静态障碍物的情况，而新增静态障碍物的出现可能导致原有路径发生变化，并且容易出现局部最小值的情况，具体的应对方案对下：

步骤A：将新增的静态障碍物信息增加到全局地图，将预处理后的集合path中离当前位置最近的关键点与新增障碍物信息判断，关键点是否与障碍物重合，若重合，则将目标点切换成下一个，若没有重合，则确定该点作为局部目标点；

步骤B：如果当前点受到引力和斥力的合力为0，则判断该点是否为局部目标点，是则说明到达局部目标点，否则说明陷入了局部最小值；当出现局部最小值时，则改变Fre1(s)的受力方向，打破原有的受力平衡，使无人机脱离局部最小值，然后重新到达局部目标点时，再将目标点切换成下一个，直至达到全局目标点；

步骤C：当无人机长时间无法到达局部目标点，则被判定原有全局路径严重受阻，需要重新全局规划，以当前点为起点，全局目标点为终点重新使用改进的lazy tehta*规划一条安全的路径；若能重新规划则执行建立地图，若不能重新规划出一条路线，即判定为新增障碍物阻断了起点和终点的路径，无法搜索出路径，则执行调整初始路径集合；

步骤D：由于无法得到一条由当前点到终点的路径，则判定为搜索失败，无人机进入返航模式，以当前点为起点，以出发点为终点搜索一条安全路径，然后跟踪轨迹回到出发点并安全降落。

在本说明书的描述中，参考术语“实施例”、“示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

以上结合具体实施例描述了本发明的技术原理。这些描述只是为了解释本发明的原理，而不能以任何方式解释为对本发明保护范围的限制。基于此处的解释，本领域的技术人员不需要付出创造性的劳动即可联想到本发明的其它具体实施方式，这些方式都将落入本发明的保护范围之内。

Claims (9)

1.一种复杂环境下改进lazy theta*的无人机路径规划方法，其特征在于，包括下述步骤：

建立地图：将空间分解为方形网格的栅格单元,获得环境的静态全局地图，设置起点x_start和终点x_goal；

寻找初始路径集合：使用改进的lazy theta*算法寻找一条安全的初始路径集合Path；

调整初始路径集合：利用抛物线的梯度信息调整初始路径中转折点之间的节点分布，使节点分布满足中间少，两端多的对称分布情况，并令节点按照路径代价g(n)升序排序得到处理后的路径集合path；

优化路径集合：使用B样条曲线对处理后的路径集合path进行平滑处理；

动态避障完成任务：使用改进的人工势场法实现动态避障功能，并根据不同的障碍物情况应用动态避障的方案完成任务。

2.根据权利要求1所述的一种复杂环境下改进lazy theta*的无人机路径规划方法，其特征在于，在所述建立地图的步骤中，具体包括将空间分解为方形网格的栅格单元，每个方形网格的中心作为确定网格属性的节点的位置，即每个网格有两种状态：可通行和不可通行，根据栅格是否可以通行来表示环境中的障碍物,如果栅格不可通行，则此区域存在障碍物，用黑色栅格表示，数值上用1表示；反之，白色栅格表示自由通行区域，没有障碍物存在，数值上用0表示，获得全局地图，设置起点x_start和终点x_goal。

3.根据权利要求1所述的一种复杂环境下改进lazy theta*的无人机路径规划方法，其特征在于，在所述寻找初始路径集合的步骤中，改进的Lazy theta*算法具体的路径规划具体为：

步骤1：初始化起点x_start和终点x_goal，并设置存放待检测节点的集合open_collection和用于已检测过节点的集合close_collection；

步骤2：初始化起点x_start的成本值f(start)，并将起点x_start加入到集合close_collection；

步骤3：判断open_collection是否为空集，如果是空集则搜索路径失败，没有找到路径，否则执行步骤4；

步骤4：从集合open_collection取出成本值f(x)最小的节点x，然后执行步骤5；

步骤5：检测节点x与其父节点x_parent的连线上是否存在LOS，若存在，则执行步骤7，若不存在，则保留x_parent作为节点x的父节点，然后则执行步骤6；

步骤6：节点x与节点parent(x)不存在LOS，则从close_collection中寻找满足LOS的相邻节点x_near，然后将节点x的父节点设置为相邻节点x_near，然后更新成本值f(x)，跳转步骤7；

步骤7：判断当前节点x是否就是终点x_goal,如果是则说明找到了最终路径，跳转到步骤9，否则将当前节点x加入到close_collection，然后执行步骤8；

步骤8：检索当前节点x的相邻节点x_near，检索步骤具体如下：

子步骤1：判断x_near是否已经存在于close_collection中或者是障碍物点，若是，则检索下一个的相邻节点，若不是则执行子步骤2；

子步骤2：判断x_near是否存在open_collection，若已经存在于open_collection，则重新计算成本值f(x_near)作为新的成本值，如果新的成本值小于原本的成本值，则需要更新该节点的成本值和父节点，否则若新的成本值大于原有成本值，则保留原有的成本值和父节点的设置；若x_near还没存在于open_collection，则将节点x_near加入到open_collection，然后计算该节点的成本值f(x_near)；执行子步骤3；

子步骤3：若检索完所有相邻节点，则跳转到步骤3，否则检索下一个相邻节点；

步骤9：路径搜索完成，从终点回溯到起点，可以得到由起点x_start，终点x_goal和转折点组成的路径点集合Path＝{P₁，P₂，...，P_m}，其中P₁是起点x_start，P_m是终点x_goal。

4.根据权利要求3所述的一种复杂环境下改进lazy theta*的无人机路径规划方法，其特征在于，在所述寻找初始路径集合的步骤中，具体包括获取每个节点到最近障碍物的距离成本，使选择的节点远离障碍物，得到更安全的路径，障碍物距离成本函数c_o(x)为：

c_o(x)＝C_max·e^-α*D(x)；

其中D(x)是从当前节点s到最近障碍物的欧式距离，以米为单位；α是指数衰减的比例因子，用于调节衰减比；C_max是节点靠近障碍物的最大成本；e是自然对数的底数；

以当前节点s到最近障碍物的距离和设定最小和最大安全距离的关系作为判定条件，引入了启发函数h(x)动态权值c_h，其公式为：

其中h(x)是当前节点x到终点的欧式距离；c_h为h(x)的权值；c_h-min是c_h设定的最小值；c_h_max是c_h设定的最大值；D是起点到终点的欧式距离；d_safe_min是设定的最小安全距离；d_safe_max是设定的最大安全距离。

5.根据权利要求3所述的一种复杂环境下改进lazy theta*的无人机路径规划方法，其特征在于，在所述寻找初始路径集合的步骤中，还包括LOS可视性判断步骤：

两节点之间是否可视：判断两点间的连线与障碍物栅格是否保持最小安全距离d_safe，设e₁的坐标为(x_e₁，y_e₁)，e₂的坐标为(x_e₂，y_e₂)；

连接e₁和e₂得到直线e₁ e₂，直线e₁ e₂的斜率为：

直线e₁ e₂的方程为：

y＝tanθ*x-tanθ*x_e₁+y_e₁；

遍历区域内的每一个栅格点，如果该栅格不是障碍物则忽略，若该栅格点是障碍物，则通过点到直线的公式计算该栅格点的坐标obs_i(x_obs_i，y_obs_i)到直线的距离：

其中A＝-tanθ，B＝1，C＝tanθ*x_e₁-y_e₁，d_safe_min为设定的最小安全距离；

若d＞d_safe_min，则说明直线与该栅格障碍物保持安全距离，两点间存在可视的直线，否则，则说明直线经过障碍物或者存在碰撞风险，也即e₁，e₂不存在可视性。

6.根据权利要求3所述的一种复杂环境下改进lazy theta*的无人机路径规划方法，其特征在于，在所述寻找初始路径集合的步骤中，Lazy theta*算法中节点的代价函数由三部分组成，包括：

f(x)＝c_gg(x)+c_hh(x)+c_w*c_o(x)；

h(x)＝c(x_start，x_goal)；

g(x)＝g(x′)+c(x′,x)；

c_g＝1-c_h；

其中，f(x)为当前节点x的总代价成本；g(x)为起点到当前节点的实际成本函数，即起点节点x_start到当前节点x的父节点x’的实际路径成本再加上父节点x’到节点x的欧氏距离；c_g为g(x)的权重值；h(x)为启发函数，即当前节点x到终点节点x_goal的预计路径成本，等于当前节点x到终点节点x_goal的欧式距离；c_h是h(x)的权重值；c_o(x)为障碍物的距离成本函数；c_w为距离成本的权重因子；c(x，x′)用于表示节点x到节点x’的路径成本，为两点的欧氏距离。

7.根据权利要求1所述的一种复杂环境下改进lazy theta*的无人机路径规划方法，其特征在于，在所述调整初始路径集合的步骤中，包括对初始路径集合Path进行预处理，其步骤包括：

确定节点件的欧式距离：设初始路径集合Path中一共有m个节点，初始路径集合Path＝{P₁，P₂，...，P_m}，设节点i的坐标为(P_x_i，P_y_i)，根据欧式距离公式获得任意两个连续的节点之间的距离：

根据各段线段之间的欧式距离确定增加的节点个数N_i，确定两连续转折点的坐标连线的倾斜角θ_i，其公式为：

若则采取纵向取点，即根据横坐标的差值去确定拐点间可保留节点个数N’：

若则采取横向取点，则根据纵坐标的差值去确定拐点间可保留节点个数N’：

其中，d_w为栅格地图的分辨率；[x]为取整函数，表示不超过x的最大整数；N’为拐点间可保留节点个数，N’是一个正整数；γ为节点个数权重因子，γ∈[0.5，1]；

确定各段线段的中心点坐标：

设拐点P_i和拐点P_i+1中心点为flex_mid_i，其坐标为(P_mid_x_i，P_mid_y_i)，则：

中心点的路径代价g(flex_mid_i)为：

g(flex_mid_i)＝g(P_i)+c(P_i，flex_mid_i)；

利用抛物线的梯度信息对拐点间的中间路径点进行重分布：

以中间路径点个数作为横坐标，拐点的中心点到右端拐点的欧式距离/>作为纵坐标，令/>获得抛物线曲线：

s²＝2pn；

其中，将n＝1，2，…，N_i，代入抛物线函数获得到右端各个节点相对中心点距离的偏差值s_i，n：

根据相对距离的偏差值s_i，n和两连续拐点之间连线的倾斜角θ_i可以得到右端新增节点的坐标及其路径代价值，设右端新增节点right_i，n的坐标为(P_right_x_i，n，P_right_y_i，n)，则横纵坐标的值为：

P_right_x_i，n＝P_mid_x_i+s_i，n*cosθ_i；

P_right_y_i，n＝P_mid_y_i+s_i，n*sinθ_i；

起点到该点的路径代价g(right_i)为：

g(right_i)＝g(flex_mid_i)+s_i，n；

设左端新增节点left_i，n的坐标为(P_left_x_i,n，P_left_y_i,n)，则横纵坐标的值为：

P_left_x_i，n＝P_mid_x_i-s_i，n*cosθ_i；

P_left_y_i,n＝P_mid_y_i-s_i，n*sinθ_i；

起点到该点的路径代价g(left_i，n)为：

g(left_i，n)＝g(flex_mid_i)-s_i，n；

获得处理后的路径集合path：将所计算得到的关键点根据路径代价值g(n)从小到大进行排序得到处理后的关键路径点集合path＝{p₁，p₂，...，p_k}，k为处理后的路径集合path的路径点个数。

8.根据权利要求1所述的一种复杂环境下改进lazy theta*的无人机路径规划方法，其特征在于，在所述动态避障完成任务的步骤中，具体包括：

设无人机在二维中操作并且无人机的位置表示为s＝(x，y)^T，重力场函数的公式为：

其中k_a表示重力增益系数，p(s，s_goal)表示无人机当前位置s和目标点s_goal之间的距离，无人机在重力场的负梯度方向上接收重力，即重力F_att(s)的公式为：

F_att(s)＝-k_ap(s，s_goal)；

障碍物对无人机产生的排斥场函数为：

其中k_r是排斥力增益系数，p(s，s_obs)是无人机和障碍物之间的最短距离，s_max是障碍物的最大作用范围；

根据排斥场函数，无人机上的排斥力公式为：

其中，F_req1(s)为障碍物指向无人机的模拟力向量分量，F_req2(s)为无人机指向目标点的模拟力向量分量：

Fre1(s)是从障碍物到无人机的分量方向，Fre2(s)则是从无人机到目标点的分量方向；

合力大小为F(n)为：

F(s)＝F_att(s)+F_req(s)；

对引力函数进行了优化：

力Fre1(s)的方向被定义为与力Fre2(s)的方向偏差α角度为：

9.根据权利要求1所述的一种复杂环境下改进lazy theta*的无人机路径规划方法，其特征在于，在所述动态避障完成任务的步骤中，针对不同的障碍物的处理步骤为：

动态障碍物：无人机通过将集合Path中离当前位置最近的关键点作为局部目标点回到全局规划路线，从而跟随全局规划路径前进；

新增加的静态障碍物：

步骤A：将新增的静态障碍物信息增加到全局地图，将预处理后的集合path中离当前位置最近的关键点与新增障碍物信息判断，关键点是否与障碍物重合，若重合，则将目标点切换成下一个，若没有重合，则确定该点作为局部目标点；

步骤B：如果当前点受到引力和斥力的合力为0，则判断该点是否为局部目标点，是则说明到达局部目标点，否则说明陷入了局部最小值；当出现局部最小值时，则改变Fre1(s)的受力方向，打破原有的受力平衡，使无人机脱离局部最小值，然后重新到达局部目标点时，再将目标点切换成下一个，直至达到全局目标点；

步骤C：当无人机长时间无法到达局部目标点，则被判定原有全局路径严重受阻，需要重新全局规划，以当前点为起点，全局目标点为终点重新使用改进的lazy tehta*规划一条安全的路径；若能重新规划则执行建立地图，若不能重新规划出一条路线，即判定为新增障碍物阻断了起点和终点的路径，无法搜索出路径，则执行调整初始路径集合；

步骤D：由于无法得到一条由当前点到终点的路径，则判定为搜索失败，无人机进入返航模式，以当前点为起点，以出发点为终点搜索一条安全路径，然后跟踪轨迹回到出发点并安全降落。