CN113311869B - 一种多约束条件下无人机路径规划方法 - Google Patents

Abstract

本发明公开一种多约束条件下无人机路径规划方法，属于路径规划技术领域，包括：根据无人机校正点位置信息，建立多约束路径规划数学模型，将其转化为双目标规划模型，确定约束函数与约束条件；运用贪心算法对无人机最优化校正点进行求解，得到最优校正点和最短飞行距离。与现有技术相比，本发明建立最小校正次数和最短距离的双目标规划模型，优化校正点校正类型，同时利用贪心算法求解，该算法寻找局部最近三点作为无人机下一遍历节点，并在规定步长范围内，提高该算法的求解能力；本发明在二维平面上三点求解弧度曲线长度，使飞行曲线平滑过渡自然，但未过度增大路径距离，通过本发明可以获得较优的飞行路径。

Description

一种多约束条件下无人机路径规划方法

技术领域：

本发明属于无人机路径规划技术领域，具体涉及一种多约束条件下无人机路径规划方法。

背景技术

近年来，得益于人工智能的飞速发展及计算机技术的显著提升，无人机控制已成为军事作战任务的核心内容。无人机以其操作灵活性，在电力巡航、区域勘测、无损检测、抢险救灾、物资投放等作业中发挥着越来重要的作用，因此无人机路径规划也成为研究热点。在无人机执行作战任务前，根据不同目标位置有效且快速规划出最佳航迹路线能够缩短任务执行时间、提高能源利用率和扩展活动空间范围。然而，无人机飞行环境复杂多变，存在着诸如建筑物、山体障碍物及突如其来的不可抗因素，极大影响着无人机路径规划的合理性与飞行安全性。

在现实环境应用中，由于地理环境的复杂性和无人机自身结构等诸多因素，使得无人机面对复杂多变的外界环境快速路径规划便成为重中之重。无人机在固定的规划路径中，避免导航定位误差造成检测失败。因此，在无人机飞行过程中需要考虑外界环境，如何快速而最优的规划出水平误差、垂直误差、路径约束和无人机转弯半径条件约束下的空间节点，并使这些空间节点形成一条有效的路径规划尤为重要。

发明内容

技术目的：针对背景技术中存在的问题，本发明提出一种多约束条件下无人机路径规划方法，建立最小校正次数和最短距离的双目标规划模型，利用贪心算法求解，提高该算法的求解能力；在二维平面上三点求解弧度曲线长度，使飞行过渡自然，曲线平滑，但未过度增大路径距离，最终获得较优的飞行路径。

技术方案：本发明公开了一种多约束条件下无人机路径规划方法，包括以下步骤：

步骤1：首先分析无人机路径规划特点，建立三维数学模型，忽略无人机大小，将其视为质点，用(x_i，y_i，z_i)表示第i个校正点的坐标i＝1，2，3，…，n，起点A为(x₀，y₀，z₀)，终点B点(x_n+1，y_n+1，z_n+1)，无人机在校正过程中，从A出发，经过k个校正点到达终点，轨迹可表示为A点与k个校正点到B点依次相连的轨迹，建立所述三维数学模型的目标函数并确定其约束条件：

min k (2)

其中，公式(1)表示飞行轨迹尽可能小，公式(2)表示矫正次数尽可能少，

表示最小轨迹为起点A(x₀，y₀，z₀)到第一个校正点i₁的距离；

表示第1个到第k个校正点之间相邻校正点累积距离之和；

表示第k个校正点到目的点B(x_n+1，y_n+1，z_n+1)的距离；i_f表示为第f个校正点；

表示为第i_f-1个校正点到第i_f个校正点的距离；

其约束条件为：

1)用ξ_i表示C_i校正的类型，无人机飞行过程中的定位误差有垂直误差和水平误差：

2)无人机在出发点处不存在定位误差，即水平误差和垂直误差为0；

3)到达终点B时垂直误差和水平误差均应小于θ个单位：

其中，

和

分别表示经过校正点

处调整后的水平误差和垂直误差；无人机每飞行1m，垂直误差和水平误差增加δ个专用单位；

4)飞行器在转弯时受到结构和控制系统的限制，无法完成即时转弯，利用转弯模型算法计算无人机的转弯半径，其约束半径r为：

(a-x₁)²+(b-y₁)²+(c-z₁)²＝r²

其中，(a，b，c)为圆心坐标O₁坐标，(x₁，y₁，z₁)为第一个校正点的坐标；

5)无人机在部分校正点进行误差校正时存在无法达到理想校正点，即将某个误差精确校正为0，用η_i表示校正点C_i是否为理想校正点：

根据实时位置，获取校正点坐标和校正误差，实时判断是否在设置范围内，并判断误差校正的类型；

步骤2：确定双目标规划模型，运用贪心算法进行全局搜索最优解对无人机最优化校正点进行求解，得到最优校正点和最短飞行距离。

进一步地，无人机在出发点A原始误差为0，到达第一个校正点i₁前无需校正；同时由最后一个校正点i_k到达终点B时定位误差小于θ也无需矫正。

进一步地，所述最小轨迹为A(x₀，y₀，z₀)到第一个校正点i₁的距离：

第一个校正点到第k个校正点的距离之和为：

其中，i_f表示为第f个校正点；

表示为第i_f-1个校正点到第i_f个校正点的距离；

第k个校正点到目的点B(x_n+1，y_n+1，z_n+1)的距离：

进一步地，在出发点A，无人机的水平误差和垂直误差均为0，无人机从第i_f-1个校正点到i_f校正点的距离为

产生的积累误差为

同时在到达校正点i_f时含有i_f-1未校正的原始误差

无人机在校正点i_f校正前的垂直误差

为积累误差

与原始误差ri_f-1之和，水平误差

与垂直误差

′可表示为：

情况一，若校正点

是垂直误差校正点，需满足：

经过垂直误差校正点

校正后的误差为：

其中，α₁为垂直误差设定值，α₂为水平误差设定值，且都为常数；

情况二，若校正点

是水平误差校正点，需满足：

其中，β₁为垂直误差设定值，β₂为水平误差设定值，且都为常数；

经过水平误差校正点

校正后的误差为：

进一步地，所述步骤3中求解具体过程为：

第一步：赋初值；设置起点A、终点B，无人机垂直和水平误差均为0，s₀＝0，r₀＝0；

第二步：计算任意的一个校正点飞到离自身最近的一个校正点飞行积累下来的误差，判断累积下来的误差是否在允许范围内，若水平误差在允许范围内，累计误差为垂直误差则进行垂直校正，若在垂直误差允许范围内，累计误差为水平误差则进行水平校正，否则，该点为不可校正点；

第三步：从当前点出发，找到所有垂直校正点集合为Conset，判断Conset是否为空集，如果Conset是空集，则显示“路径寻找失败”，并且退出循环；如果Conset不是空集，则在集合Conset寻找距离终点B最近的一个校正点，记为C_k；

第四步：设置起点为C_k，记录调整前后的误差，若C_k不是理想校正点，用均匀分布随机数ζ来仿真纠正是否成功校正后水平剩余误差均为0，记录校正前后的误差，并设置校正后的误差值；

第五步：判断当前点能否到达终点B，如果能到达B则输出最终路线，并退出循环；如果不能到达B，转到下一步；

第六步：从当前点出发，找到所有水平校正点集合为Conset′，判断Conset′是否为空集，如果Conset′是空集，则显示“路径寻找失败”，并且退出循环；如果Conset′不是空集，则在集合Conset′寻找距离终点B最近的一个校正点，记为C_k′；

第七步：设置起点为C_k′，记录调整前后的误差，若C_k′不是理想校正点，用均匀分布随机数ζ来仿真纠正是否成功，校正后水平剩余误差均为0，记录校正前后的误差，并设置校正后的误差值；

第八步：判断当前点能否到达终点B，如果能到达B则输出最终路线，并退出循环；如果不能到达B，转到第二步。

进一步地，所述第四步与第七步中用服从(0，1)均匀分布的随机数ζ纠正具体为：若ζ∈[0，0.2]则视为校正失败，校正后的剩余误差为设为min(error，5)，其中error为校正前误差；反之，ζ∈(0.2，1]视为校正成功。

有益效果：

1、本发明针对无人机在不同实际飞行环境中有效路径规划方法，建立最优路径和最小校正次数为目标函数，定位误差约束，转弯控制受限约束和无法达到理想校正点约束条件下的多种约束构建数学模型，利用贪心算法对无人机路径进行快速规划，垂直校正点和水平校正点可校正的误差允许范围不相同，且达到终点时也在允许范围内，路径规划更加符合实际飞行过程。

2、寻找局部最近三点作为求解圆弧长度，将三维空间转化为二维平面上的圆弧曲线长度求解，使飞行过渡平滑更加自然，但并未过多增大飞行路径，可获得最优路径。

附图说明

图1为本发明实施例无人机航迹(以C标记的为校正点)；

图2为本发明实施例求取无人机转弯轨迹的切点与转弯半径示意图；

图3为本发明实施例无人机选择的航迹路径示意图；

图4为本发明实施例无人机转弯放大后的轨迹示意图；

图5为本发明模型求解算法运行1000次的路径图。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

本发明公开了一种多约束条件下无人机路径规划方法，具体包括以下步骤：

步骤1：先分析无人机路径规划特点，建立多约束条件的数学模型；

建立三维数学模型，用(x_i，y_i，z_i)表示第i个校正点C_i的坐标i＝1，2，3，…，n。A点坐标为(x₀，y₀，z₀)，B点坐标(x_n+1，y_n+1，z_n+1)，用ξ_i表示C_i校正的类型：

无人机在校正过程中，从A点出发，经过k个校正点到达终点B，轨迹可表示为A点与k个校正点到B点依次相连的轨迹，表示为

其中，

表示校正点C_i中的第k个子集元素。

最小轨迹为A(x₀，y₀，z₀)到第一个校正点i₁的距离：

第一个校正点

到第k个校正点

的距离之和为：

其中，i_f表示为第f个校正点；

表示为第i_f-1个采样校正点到第i_f个采样校正点的距离。

第k个校正点到目的点B(x_n+1，y_n+1，z_n+1)的距离：

无人机路径规划过程中要考虑两个条件：

1)第一个是：飞行轨迹尽可能小，即：

2)第二个条件是：矫正次数尽可能少，即

min k。

无人机每飞行1m，垂直误差和水平误差增加δ个专用单位。s_i和r_i分别表示经过校正点C_i处调整后的水平误差和垂直误差。水平误差s_i′和垂直误差r_i′表示在经过校正点C_i处调整前的误差。在出发A点，无人机的水平误差和垂直误差均为0，所以有s₀＝0和r₀＝0。

无人机从第i_f-1个校正点到i_f校正点的距离为

产生的积累误差为

同时在到达校正点i_f时含有i_f-1未校正的原始误差

无人机在校正点i_f校正前的垂直误差

为积累误差

与原始误差

之和。水平误差

与垂直误差

可表示为：

情况一：若校正点

是垂直误差校正点，需满足：

其中，α₁为垂直误差设定值，α₂为水平误差设定值，且都为常数；

经过垂直误差校正点

校正后的误差为：

情况二：若校正点

是水平误差校正点

其中，β₁为垂直误差设定值，β₂为水平误差设定值，且都为常数；

经过水平误差校正点

校正后的误差为：

到达终点时垂直误差和水平误差均应小于θ个单位即可，所以有：

其中，

和

分别表示经过校正点

处调整后的水平误差和垂直误差。

其次无人机在转弯时受到结构和控制系统的限制，无法完成即时转弯(飞行器前进方向无法突然改变)，且转弯时不能直角转弯，故需增加约束条件，转弯半径不能小于200米，故对飞行轨迹进行优化。

利用转弯模型算法计算无人机的转弯半径，其约束半径r为：

(a-x₁)²+(b-y₁)²+(c-z₁)²＝r²

其中，(a，b，c)为圆心坐标O₁坐标，(x₁，y₁，z₁)为第一个校正点的坐标。

下面需要对空间上k个校正点轨迹进行优化，将三维空间飞行转变为二维平面求解。参见附图1：

1)从出发点A出发时无误差，无人机可直接飞到校正点C₁，线段AC₁为第一段飞行轨迹。

2)不在同一直线上的三点A、C₁和C₂，确定一个平面AC₁C₂；在平面AC₁C₂上，以直线AC₁为切线，C₁为切点做半径为200米的圆O₁(圆心)，过C₂点的做圆0₁的切线，切点为F₁(在圆0₁上的属于非校正点切点)，圆弧C₁F₁即为旋转时轨迹(第二段轨迹)。

3)线段C₂F₁为第三段轨迹。

4)接着以C₂F₁为切线做半径为200米的圆O₂，再过C₃点的做圆O₂的切线，切点为F₂(在圆O₂上的属于非校正点切点)，圆弧C₃F₂即为旋转时轨迹(第四段轨迹)。剩下校正点以此类推，直至B点。

参见附图2，具体过程为：

1.确定平面：

参见附图2，设A、C₁、C₂三点坐标为(x₀，y₀，z₀)、(x₁，y₁，z₁)、(x₂，y₂，z₂)，根据三点坐标AC₁C₂的方程为

圆心坐标O₁坐标为(a，b，c)，由3个条件：

a)点在平面AC₁C₂

b)线段C₁O₁长度等于200米

(a-x₁)²+(b-y₁)²+(c-z₁)²＝200²

c)向量

与向量

垂直

(a-x₁)(x₁-x₀)+(b-y₁)(y₁-y₀)+(c-z₁)(z₁-z₀)＝0

可知，存在两个符合条件的圆心O₁和0₁′，需要选择一个合适初值点作为参考点。有效的圆心点所在位置可由

设R₁坐标为(X_R1，Y_R1，Z_R1)，则：

2.求切点F₁坐标

设F₁坐标为(d，e，f)，由下面的3个条件：

a)F₁点在平面AC₁C₂

b)线段O₁F₁长度等于200米

(a-d)²+(b-e)²+(c-f)²＝200²

c)向量

与向量

垂直

(a-x₂)(d-a)+(b-y₂)(e-b)+(c-z₂)(f-c)＝0

可知，存在两个符合条件的切点F₁与F₁′，但只有一个为有效切点。有效的切点所在位置可由向量

设R₂坐标为(X_R2，Y_R2，Z_R2)，则：

参见附图4，看出无人机路径在直线飞行与转弯飞行时过渡自然，曲线平滑，效果更好。

无人机在部分校正点进行误差校正时存在无法达到理想校正点(即将某个误差精确校正为0)。假设无人机在部分校正点能够成功将某个误差校正为0的概率为80％，如果校正失败，则校正后的剩余误差为min(error，5)个单位。用η_i表示校正点C_i的是否为理想校正点，即：

综上所述，多约束条件转化为双目标规划模型如下：

α₁＝25，α₂＝15，β₁＝20，β₂＝25，θ＝30，δ＝0.001，当无人机的垂直误差不大于α₁个单位，水平误差不大于α₂个单位时才能进行垂直误差校正。当无人机的垂直误差不大于β₁个单位，水平误差不大于β₂个单位时才能进行水平误差校正。

下面以垂直校正再水平校正误差进行对上述的双目标规划模型进行求解，其算法思想如下：

第一步：赋初值；设置起点A，设置终点B，无人机垂直和水平误差均为0，s₀＝0，r₀＝0；

第二步：计算校正点之间距离，判断飞到自身最近的一个校正点累积下来的误差是否在允许范围内，如果水平误差在允许范围内，累计误差为垂直误差则进行垂直校正，如果在垂直误差允许范围内，累计误差为水平误差则进行水平校正，否则，该点为不可校正点；

第三步：从当前点出发，找到所有垂直校正点集合为Conset，判断Conset是否为空集，如果Conset是空集，则显示“路径寻找失败”，并且退出循环；如果Conset不是空集，则在集合Conset寻找距离终点B最近的一个校正点，记为C_k；

第四步：设置起点为C_k，记录调整前后的误差，若C_k不是理想校正点，用均匀分布随机数ζ来仿真纠正是否成功，若ζ∈[0，0.2]则视为校正失败，校正后的剩余误差为设为min(error，5)，其中error为校正前误差；反之，ζ∈(0.2，1]视为校正成功，校正后水平剩余误差均为0，记录校正前后的误差，并设置校正后的误差值；

第五步：判断当前点能否到达终点B，如果能到达B则输出最终路线，并退出循环；如果不能到达B，转到下一步；

第六步：从当前点出发，找到所有水平校正点集合为Conset′，判断Conset′是否为空集，如果Conset′是空集，则显示“路径寻找失败”，并且退出循环；如果Conset′不是空集，则在集合Conset′寻找距离终点B最近的一个校正点，记为C_k′；

第七步：设置起点为C_k′，记录调整前后的误差，若C_k′不是理想校正点，用均匀分布随机数ζ来仿真纠正是否成功，若ζ∈[0，0.2]则视为校正失败，校正后的剩余误差为设为min(error，5)，其中error为校正前误差；反之，ζ∈(0.2，1]视为校正成功，校正后水平剩余误差均为0，记录校正前后的误差，并设置校正后的误差值；

第八步：判断当前点能否到达终点B，如果能到达B则输出最终路线，并退出循环；如果不能到达B，转到第二步。

第九步：无人机校正点集合为

设置飞行航迹上的切点集合

集合中

第十步：设置三点为

用A1、A2和A3确定圆心O，再确定切点

求出圆弧长度

第十一步：判断A3是否等于

若相等则A3＝B，退出循环；否则转到第八步，直至结束。

程序循环运行1000次，参见附图5所示的路径图，统计成功路径规划的次数，从而得到该算法的成功率96.70％。

经过校正前后的误差结果对比表

基于优化转弯时的算法，仍使得校正点的个数为8，与前面的校正点个数相等，且飞行轨迹为110185.13米，只比未优化的飞行轨迹多107.89米，比对结果优良，因此该算法的性能较好。

上述实施方式只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所做的等效变换或修饰，都应涵盖在本发明的保护范围之内。

Claims (5)

1.一种多约束条件下无人机路径规划方法，其特征在于，包括以下步骤：

步骤1：首先分析无人机路径规划特点，建立三维数学模型，忽略无人机大小，将其视为质点，用(x_i，y_i，z_i)表示第i个校正点的坐标i＝1，2，3，…，n，起点A为(x₀，y₀，z₀)，终点B为(x_n+1，y_n+1，z_n+1)，无人机在校正过程中，从A出发，经过k个校正点到达终点，轨迹可表示为A点与k个校正点到B点依次相连的轨迹，建立所述三维数学模型的目标函数并确定其约束条件：

mink (2)

其中，公式(1)表示飞行轨迹尽可能小，公式(2)表示矫正次数尽可能少，

表示最小轨迹为起点A(x₀，y₀，z₀)到第一个校正点i₁的距离；

表示第1个到第k个校正点之间相邻校正点累积距离之和；

表示第k个校正点到目的点B(x_n+1，y_n+1，z_n+1)的距离；i_f表示为第f个校正点；

表示为第i_f-1个校正点到第i_f个校正点的距离；

所述最小轨迹为A(x₀，y₀，z₀)到第一个校正点i₁的距离：

第一个校正点到第k个校正点的距离之和为：

其中，i_f表示为第f个校正点；

表示为第i_f-1个校正点到第i_f个校正点的距离；

第k个校正点到目的点B(x_n+1，y_n+1，z_n+1)的距离：

在出发点A，无人机的水平误差和垂直误差均为0，无人机从第i_f-1个校正点到i_f校正点的距离为

产生的积累误差为

同时在到达校正点i_f时含有i_f-1未校正的原始误差

无人机在校正点i_f校正前的垂直误差

为积累误差

与原始误差

之和，水平误差

与垂直误差

可表示为：

其约束条件为：

1)用ξ_i表示C_i校正的类型，无人机飞行过程中的定位误差有垂直误差和水平误差：

2)无人机在出发点处不存在定位误差，即水平误差和垂直误差为0；

3)到达终点B时垂直误差和水平误差均应小于θ个单位：

其中，

和

分别表示经过校正点

处调整后的水平误差和垂直误差；无人机每飞行1m，垂直误差和水平误差增加δ个专用单位；

4)飞行器在转弯时受到结构和控制系统的限制，无法完成即时转弯，利用转弯模型算法计算无人机的转弯半径，其约束半径r为：

(a-x₁)²+(b-y₁)²+(c-z₁)²＝r²

其中，(a，b，c)为圆心坐标O₁坐标，(x₁，y₁，z₁)为第一个校正点的坐标；

转弯半径不能小于200米 ，对飞行轨迹进行优化，具体优化步骤为：

(1)从出发点A出发时无误差，无人机可直接飞到校正点C₁，线段AC₁为第一段飞行轨迹；

(2)不在同一直线上的三点A、C₁和C₂，确定一个平面AC₁C₂；在平面AC₁C₂上，以直线AC₁为切线，C₁为切点做半径为200米的圆O₁，O₁为圆心，过C₂点的做圆O₁的切线，切点为F₁，在圆O₁上的属于非校正点切点，圆弧C₁F₁即为旋转时轨迹，即为第二段轨迹；

(3)线段C₂F₁为第三段轨迹；

(4)接着以C₂F₁为切线做半径为200米的圆O₂，再过C₃点的做圆O₂的切线，切点为F₂，在圆O₂上的属于非校正点切点，圆弧C₃F₂即为旋转时轨迹，即第四段轨迹，剩下校正点以此类推，直至B点；

设A、C₁、C₂三点坐标为(x₀，y₀，z₀)、(x₁，y₁，z₁)、(x₂，y₂，z₂)，根据三点坐标AC₁C₂的方程为

圆心坐标O₁坐标为(a，b，c)，由3个条件：

a)点在平面AC₁C₂

b)线段C₁O₁长度等于200米

(a-x₁)²+(b-y₁)²+(c-z₁)²＝200²

c)向量

与向量

垂直

(a-x₁)(x₁-x₀)+(b-y₁)(y₁-y₀)+(c-z₁)(z₁-z₀)＝0

可知，存在两个符合条件的圆心O₁和O₁′，需要选择一个合适初值点作为参考点，有效的圆心点所在位置可由

设R₁坐标为(X_R1，Y_R1，Z_R1)，则：

求切点F₁坐标：

设F₁坐标为(d，e，f)，由下面的3个条件：

a)F₁点在平面AC₁C₂

b)线段O₁F₁长度等于200米

(a-d)²+(b-e)²+(c-f)²＝200²

c)向量

与向量

垂直

(a-x₂)(d-a)+(b-y₂)(e-b)+(c-z₂)(f-c)＝0

可知，存在两个符合条件的切点F₁与F₁′，但只有一个为有效切点，有效的切点所在位置可由向量

设R₂坐标为(X_R2，Y_R2，Z_R2)，则：

5)无人机在部分校正点进行误差校正时存在无法达到理想校正点，即将某个误差精确校正为0，用η_i表示校正点C_i是否为理想校正点：

根据实时位置，获取校正点坐标和校正误差，实时判断是否在设置范围内，并判断误差校正的类型；

步骤2：确定双目标规划模型，运用贪心算法进行全局搜索最优解对无人机最优化校正点进行求解，得到最优校正点和最短飞行距离

多约束条件转化为双目标规划模型如下：

2.根据权利要求1所述的多约束条件下无人机路径规划方法，其特征在于，无人机在出发点A原始误差为0，到达第一个校正点i₁前无需校正；同时由最后一个校正点i_k到达终点B时定位误差小于θ也无需矫正。

3.根据权利要求1所述的多约束条件下无人机路径规划方法，其特征在于，

情况一，若校正点

是垂直误差校正点，需满足：

经过垂直误差校正点

校正后的误差为：

其中，α₁为垂直误差设定值，α₂为水平误差设定值，且都为常数；

情况二，若校正点

是水平误差校正点，需满足：

其中，β₁为垂直误差设定值，β₂为水平误差设定值，且都为常数；

经过水平误差校正点

校正后的误差为：

4.根据权利要求1所述的多约束条件下无人机路径规划方法，其特征在于，所述步骤2中求解具体过程为：

第一步：赋初值；设置起点A、终点B，无人机垂直和水平误差均为0，s₀＝0，r₀＝0；

第二步：计算任意的一个校正点飞到离自身最近的一个校正点飞行积累下来的误差，判断累积下来的误差是否在允许范围内，若水平误差在允许范围内，累计误差为垂直误差则进行垂直校正，若在垂直误差允许范围内，累计误差为水平误差则进行水平校正，否则，该点为不可校正点；

第三步：从当前点出发，找到所有垂直校正点集合为Conset，判断Conset是否为空集，如果Conset是空集，则显示“路径寻找失败”，并且退出循环；如果Conset不是空集，则在集合Conset寻找距离终点B最近的一个校正点，记为C_k；

第四步：设置起点为C_k，记录调整前后的误差，若C_k不是理想校正点，用均匀分布随机数ζ来仿真纠正是否成功校正后水平剩余误差均为0，记录校正前后的误差，并设置校正后的误差值；

第五步：判断当前点能否到达终点B，如果能到达B则输出最终路线，并退出循环；如果不能到达B，转到下一步；

第六步：从当前点出发，找到所有水平校正点集合为Conset′，判断Conset′是否为空集，如果Conset′是空集，则显示“路径寻找失败”，并且退出循环；如果Conset′不是空集，则在集合Conset′寻找距离终点B最近的一个校正点，记为C_k′；

第七步：设置起点为C_k′，记录调整前后的误差，若C_k′不是理想校正点，用均匀分布随机数ζ来仿真纠正是否成功，校正后水平剩余误差均为0，记录校正前后的误差，并设置校正后的误差值；

第八步：判断当前点能否到达终点B，如果能到达B则输出最终路线，并退出循环；如果不能到达B，转到第二步。

5.根据权利要求4所述的多约束条件下无人机路径规划方法，其特征在于，所述第四步与第七步中用服从(0，1)均匀分布的随机数ζ纠正具体为：若ζ∈[0，0.2]则视为校正失败，校正后的剩余误差为设为min(error，5)，其中error为校正前误差；反之，ζ∈(0.2，1]视为校正成功。

Images