CN110926477B - 一种无人机航路规划及避障方法 - Google Patents

Abstract

本发明公开了一种无人机航路规划及避障方法，包括：S1，建立二维空间模型，根据飞行起点和飞行终点设置空间模型数据；S2，利用空间模型数据获得全局离线航路，设飞行航路为全局离线航路；S3，无人机沿飞行航路飞行，并通过传感器实时监测飞行航路上是否出现动态障碍物；S4，出现动态障碍物时，进入下一步骤；未出现动态障碍物且未达到飞行终点，进入步骤S3，若达到飞行终点，则进入步骤S8；S5，采用带约束的伴随矩阵方法对鸽群算法进行初始化，再用鸽群算法进行局部航路规划；S6，使用B样条曲线对局部航路进行平滑化处理；S7，飞行航路为全局离线航路与局部航路结合，进入步骤S3；S8，完成飞行任务，流程结束。

Description

一种无人机航路规划及避障方法

技术领域

本发明属于无人机的技术领域，具体涉及一种无人机航路规划及避障方法。

背景技术

飞速发展的无人机技术，使得无人机开始应用在航空拍摄、物流等领域。但是随着无人机数量的增多，无人机在飞行时如果面对障碍物威胁，依靠无人机飞手来操作避障将消耗过多的人力资源，由无人机自主飞行来代替无人机飞手的人工操作，就需要对无人机的飞行航路进行动态避障规划，因此，在遭遇突发障碍物时，如何规划出一条安全、快捷、经济的无人机航路是现有技术中亟待解决的重要问题。

目前，常用的无人机航路规划方法主要包括迪杰斯特拉(Dijkstra)方法、伴随矩阵(A*)方法，维诺(Voronoi)图法等确定型搜索方法，且群智能仿生算法逐渐应用到无人机航路规划中，并且由于其不确定性，使得其有更多的机会求解全局最优解，例如：1991年被提出来的蚁群优化算法，用于求解TSP问题；2005年被提出的人工蜂群算法，用于求解函数优化问题；2007年提出的狼群算法等。鸽群算法与这些智能仿生算法相比，其在群体编队、路径规划等行动中的辨别方式与无人机的航路规划过程类似，且均可以看作已知起始点、目标点的多约束求解问题。例如：2017年提出的采用威胁启发鸽群优化的武装直升机航路规划；2018年提出的基于自适应权重鸽群算法的无人机航路规划。

上述运用鸽群算法进行无人机飞行航路规划过程中，存在算法威胁代价消耗过大，算法收敛速度较慢，飞行航路长度较长以及未对飞行航路进行平滑化等问题，导致无人机飞行航路不适用于复杂的飞行环境。综上所述，选择合适的算法进行针对性改进，使其应用在无人机航路规划时能提高无人机飞行任务的安全性和快速性变得十分重要。

发明内容

为解决上述问题中的至少之一，本发明提出一种无人机航路规划及避障方法。

本发明的目的通过以下技术方案实现：

本发明提供了一种无人机航路规划及避障方法，包括：

S1，利用栅格法建立二维空间模型，根据无人机的飞行起点和飞行终点设置带起始节点、航路节点和目标节点的空间模型数据；

S2，利用空间模型数据获得全局离线航路，设置无人机的飞行航路为全局离线航路；

S3，无人机沿飞行航路飞行，并通过无人机上的传感器实时监测飞行航路上是否出现动态障碍物；

S4，监测飞行航路上出现动态障碍物时，进入下一步骤；监测飞行航路上未出现动态障碍物时，判断无人机是否达到飞行终点，若未达到飞行终点，进入步骤S3，若达到飞行终点，则进入步骤S8；

S5，采用带约束的伴随矩阵方法对鸽群算法进行初始化，再用鸽群算法进行局部航路规划；

S6，使用B样条曲线对规划后的局部航路进行平滑化处理，实现对无人机飞行航路进行动态避障规划；

S7，设置无人机的飞行航路为全局离线航路与局部航路结合，进入步骤S3；

S8，完成飞行任务，流程结束。

作为进一步的改进，所述步骤S2中所述利用空间模型数据获得全局离线航路具体为，采用双向迪杰斯特拉算法进行全局离线航路规划，将飞行航路上各节点之间的权重视为航路的代价消耗，从起始节点往目标节点和从目标节点往起始节点同时进行单向航路节点搜索，当两个方向搜索在同一个航路节点时，得出规划后的全局离线航路。

作为进一步的改进，所述步骤S4中所述监测飞行航路上出现动态障碍物后，对动态障碍物进行膨胀化操作，使动态障碍物的边界外扩膨胀化为一个包含该动态障碍物的内圆，进一步根据无人机最小安全半径在内圆外膨胀化一个更大的外圆，将无人机勘测到动态障碍物时的当前位置设为局部起始航点，全局离线航路上未受外圆影响的下一航路节点为局部目标航点。

作为进一步的改进，所述步骤S5中所述采用带约束的伴随矩阵方法对鸽群算法进行初始化之前，在伴随矩阵方法中增加从局部起始航点开始朝着局部目标航点方向搜索航路节点的约束条件，找出代价消耗较低的航路，将代价消耗较低的航路上两个航路节点之间违反无人机飞行机动性能的航路节点去除，留下符合飞行机动性能的航路节点。

作为进一步的改进，所述步骤S5中所述采用带约束的伴随矩阵方法对鸽群算法进行初始化具体为，首先，随机取代价消耗较低的航路节点上下一定数量的初始数据对鸽群算法进行初始化，在二维空间平面中进行坐标轴方向转换，将局部起始航点与局部目标航点的坐标轴方向转换成新的横坐标，并根据新的横坐标建立新的坐标系。

作为进一步的改进，所述步骤S5中所述伴随矩阵方法初始化鸽群算法过程中，对鸽群算法采用一种自适应的编码方式，以代价消耗较低航路上的一个坐标值作为鸽群算法粒子的新编码值，粒子在旋转后坐标系中的横坐标获得公式如下所示：

其中，X是坐标轴名称，i是粒子个数，D是空间解的维数，X_start是局部起始航点在坐标转换后的横坐标，X_goal是局部目标航点在坐标转换后的横坐标。

作为进一步的改进，所述步骤S5中，所述鸽群算法使用改进的地磁算子迭代，进行局部目标航点的搜索，每一次迭代都会得到鸽群新的位置和速度，在地磁算子的迭代次数达到规定的次数时，启用改进的地标算子迭代来更新鸽群种群数量和种群粒子的位置，获得全体粒子中最优的粒子，进而得出局部重规划后的局部航路。

作为进一步的改进，所述改进的地磁算子更新算法如下所示：

v_i(t)＝v_i(t-1)e^-Rt+rand(x′_g-x_i(t-1))

x_i(t)＝x_i(t-1)+v_i(t)

其中，t是鸽群迭代的次数，i是鸽子的数量，v_i(t)为第i只鸽子在第t次迭代后的速度，x_i(t)为第i只鸽子在第t次迭代后的位置，e是自然对数函数的底数，R是地磁因子，x′_g为此次迭代中改进的全局最优位置，rand是一个产生(0，1)之间随机数的函数。

作为进一步的改进，所述步骤S5中改进的地标算子的更新算法如下所示：

N_p(t)＝logsig((Nc_max/2-t)/k)×N_pmax

x_i(t)＝x_i(t-1)+rand·(x_c(t)-x_i(t-1))

其中，N_p(t)表示当前迭代次数的鸽群数量，N_pmax表示种群规模的数量最大值，Nc_max为地标算子阶段的最大迭代次数，t为当前迭代次数，k为logsig函数的斜率。i表示鸽群中鸽子的只数，x_c(t)代表第t代鸽群的中心，x_i(t)为第i只鸽子在第t次迭代中的位置，rand是一个产生(0，1)之间随机数的函数，f(x_i(t))是第i只鸽子的适应度函数。

作为进一步的改进，在所述S6步骤中所述使用B样条曲线对规划后的局部航路进行平滑化处理具体为，通过基于三次B样条曲线来进行局部航路的平滑化，三次B样条曲线平滑化后存在航迹有部分进入到障碍物内部时，选取B样条曲线中离相交碰撞部分最近的四个航迹点，对B样条曲线三条边的中点取新的航迹点进行平滑化，仍有部分航迹进入障碍物时，则继续重复上述操作，直至消除航迹与障碍物的碰撞为止。

本发明提供的一种无人机航路规划及避障方法，包括：S1，利用栅格法建立二维空间模型，根据无人机的飞行起点和飞行终点设置带起始节点、航路节点和目标节点的空间模型数据；S2，利用空间模型数据获得全局离线航路，设置无人机的飞行航路为全局离线航路；S3，无人机沿飞行航路飞行，并通过无人机上的传感器实时监测飞行航路上是否出现动态障碍物；S4，监测飞行航路上出现动态障碍物时，进入下一步骤；监测飞行航路上未出现动态障碍物时，判断无人机是否达到飞行终点，若未达到飞行终点，进入步骤S3，若达到飞行终点，则进入步骤S8；S5，采用带约束的伴随矩阵方法对鸽群算法进行初始化，再用鸽群算法进行局部航路规划；S6，使用B样条曲线对规划后的局部航路进行平滑化处理，实现对无人机飞行航路进行动态避障规划；S7，设置无人机的飞行航路为全局离线航路与局部航路结合，进入步骤S3；S8，完成飞行任务，流程结束。使用本发明在出现动态障碍物时，规划后的无人机飞行航路具有更短的航路长度和更小的威胁代价,并且飞行任务消耗的时间更短，既可以节省无人机的飞行成本，还能针对动态障碍物作出准确的避障及局部航路的重规划，使动态障碍物对无人机飞行航路的影响更小，提高无人机飞行任务的安全性和快速性。

附图说明

利用附图对本发明作进一步说明，但附图中的实施例不构成对本发明的任何限制，对于本领域的普通技术人员，在不付出创造性劳动的前提下，还可以根据以下附图获得其它的附图。

图1是本发明的整体流程图。

图2是本发明局部航路规划的流程图。

图3是本发明膨胀化示意图。

图4是本发明威胁代价示意图。

图5是本发明带约束的伴随矩阵方法示意图。

图6是本发明坐标转换示意图。

图7a是本发明平滑化后部分航路进入障碍物示意图。

图7b是本发明局部航路规划后消除飞行航路与障碍物碰撞的示意图。

图8是本发明迪杰斯特拉算法规划的全局离线航路。

图9a是本发明检测到一个突发障碍物时局部航路规划后的示意图。

图9b是本发明检测到一个突发障碍物时局部航路规划并平滑化处理后的示意图。

图10a是本发明检测到二个突发障碍物时局部航路规划后的示意图。

图10b是本发明检测到二个突发障碍物时局部航路规划并平滑化处理后的示意图。

图11a是传统鸽群算法检测到一个突发障碍物时重规划航路且平滑化后的示意图。

图11b是传统鸽群算法检测到二个突发障碍物时重规划航路且平滑化后的示意图。

图12是本发明与传统鸽群算法收敛曲线对比图。

具体实施方式

为了使本领域的技术人员更好地理解本发明的技术方案，下面结合附图和具体实施例对本发明作进一步详细的描述，需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

结合图1和图2所示，本发明实施例提供一种无人机航路规划及避障方法，包括：

S1，利用栅格法建立二维空间模型，其中，建筑物、树木、无法穿越的静态物体等设置为静态障碍物。根据无人机的飞行起点和飞行终点设置带起始节点、航路节点和目标节点以及静态障碍物的数量、位置及大小等的空间模型数据。

S2，利用空间模型数据获得全局离线航路，，设置无人机的飞行航路为全局离线航路，采用双向迪杰斯特拉算法将飞行航路上各节点之间的权重视为航路的代价消耗，从起始节点往目标节点和从目标节点往起始节点同时进行单向航路节点搜索，从起始节点开始执行的搜索定义为前向搜索，从目标节点开始执行的搜索称为后向搜索，当两个方向搜索在同一个航路节点时，得出规划后的全局离线航路。具体过程如下：

(1)创建两组OPEN，CLOSE表，OPEN表存放未被检查的航路节点，CLOSE表存放检查过的航路节点。

(2)访问空间模型数据的栅格表中距离起始节点最近且未被检查的航路节点(可能不止一个)，将这些航路节点放入第一组的OPEN表中；访问空间模型数据的栅格表中距离目标节点最近且未被检查的航路节点(可能不止一个)，将这些航路节点放入第二组的OPEN表中。

(3)取出两组OPEN表中的数据，检查第一组的OPEN表中距离起始节点最近的节点A，并找出节点A的所有子节点，然后把节点A放入第一组CLOSE表；同样的，检查第二组的OPEN表中距离起始节点最近的节点B，并找出节点B的所有子节点，然后把节点B放入第二组CLOSE表。

(4)遍历步骤(3)两个并列操作中找到的所有子节点，计算节点A的子节点距起始节点的距离值并将节点A的子节点放到第一组OPEN表，同时计算节点B的子节点距目标节点的距离值并将节点B的子节点放到第二组OPEN表。

(5)重复步骤(3)，步骤(4)，当两个方向的搜索在同一个航路节点相遇时，得出规划后的全局离线航路。

S3，无人机沿飞行航路飞行，并通过无人机上的激光雷达传感器实时监测飞行航路上是否出现动态障碍物。

S4，监测飞行航路上出现动态障碍物时，如图3所示，对动态障碍物31进行膨胀化操作，使动态障碍物31的边界外扩膨胀化为一个包含该动态障碍物31的内圆32，进一步根据无人机最小安全半径34在内圆32外膨胀化一个更大的外圆33，将无人机勘测到动态障碍物时的当前位置设为局部起始航点，全局离线航路上未受外圆33影响的下一航路节点为局部目标航点，然后进入下一步骤；监测飞行航路上未出现动态障碍物31时，判断无人机是否达到飞行终点，若未达到飞行终点，进入步骤S3，若达到飞行终点，则进入步骤S8。

S5，采用带约束的伴随矩阵方法对鸽群算法进行初始化，再用鸽群算法进行局部航路规划。传统伴随矩阵方法计算过程中需要遍历包含上、下、左、右、左上、右上、左下、右下8个方向，在伴随矩阵方法中增加约束条件，使搜索节点从局部起始航点开始朝着局部目标航点方向搜索航路节点，这样不仅减少了航路节点搜索量，同样方便高效的找出最优方向路线。全局离线航路的起始节点和目标节点连线的大方向为右上方向，在伴随矩阵方法中增加约束条件后，如图5所示，搜索方向限定在上、下、右、右上、右下五个方向。另外，在利用伴随矩阵方法搜索代价消耗较低的航路时可能会出现两个航路节点之间的转弯角度过大，违反了无人机自身的飞行机动性能，将飞行航路上违反无人机飞行机动性能的航路节点去除，留下符合飞行机动性能的航路节点，以加快伴随矩阵方法对代价消耗较低航路的搜索速度。

具体的，伴随矩阵方法中获得代价消耗较低航路的评价函数如下所示：

f(n)＝g(n，s)+h(n，e)

其中：f(n)表示从局部起始航点开始经过航路节点n到局部目标航点的最优代价，n为搜索过程中遇到的航路节点，g(n，s)表示局部起始航点s到航路节点n的航路实际代价，h(n，e)表示从航路节点n到局部目标航点e的航路估计代价，使用局部起始航点s到航路节点n和航路节点n到局部目标航点e的欧氏距离，公式如下所示：

其中，(x_s，y_s)为局部起始航点s的坐标，(x_n，y_n)为航路节点n的坐标，(x_e，y_e)为局部目标航点e的坐标。

在利用伴随矩阵方法初始化鸽群算法之前，对鸽群算法空间解的维数进行初始化，其中，解的维数与问题的规模有关，表示问题在规模维度的空间中进行寻优。然后，将伴随矩阵方法得出的代价消耗较低的航路视为主航路，随机取连接主航路的航路节点上下一定数量的初始数据，对鸽群算法进行初始化，使鸽群算法获得更多伴随矩阵方法的初始信息。在规划空间的二维平面中进行坐标轴方向转换，如图6所示，局部起始航点O和局部目标航点A在原XY坐标系中的横坐标为B纵坐标为C，现将局部起始航点与局部目标航点的连线OA所在的直线转换成为新坐标系中的OX′轴，建立新的直角坐标系OX′Y′，方便后续鸽群算法对局部航路的规划，在转换后的OX′轴上对OA做n等分，在n-1个等分线上利用伴随矩阵方法对纵坐标Y′进行初始化，使初始数据更容易引入鸽群算法中。在伴随矩阵方法初始化鸽群算法过程中，对鸽群算法采用一种自适应的编码方式，以主航路上位于局部起始航点与局部目标航点之间的n-1个等分点的坐标值作为鸽群算法粒子的新编码值，粒子在旋转后坐标系中的横坐标获得公式如下所示：

其中，X是坐标轴名称，i是粒子个数，D是空间解的维数，X_start是局部起始航点在坐标转换后的横坐标，X_goal是局部目标航点在坐标转换后的横坐标。

鸽群算法使用地磁算子迭代进行局部目标航点的搜索，每一次迭代都会得到鸽群新的位置和速度，在地磁算子的迭代次数达到规定的次数时，启用地标算子迭代来更新鸽群种群数量和种群粒子的位置，获得全体粒子中最优的粒子，进而得出局部重规划后的局部航路。

改进的地磁算子更新算法如下所示：

v_i(t)＝v_i(t-1)e^-Rt+rand(x′_g-x_i(t-1))

x_i(t)＝x_i(t-1)+v_i(t)

其中，t是当前鸽群迭代的次数，i是鸽子的数量，v_i(t)为第i只鸽子在第t次迭代后的速度，x_i(t)为第i只鸽子在第t次迭代后的位置，e是自然对数函数的底数，R是地磁因子，x′_g为此次迭代中改进的全局最优位置，rand是一个产生(0，1)之间随机数的函数。

在地磁算子的速度迭代公式中，e^-Rt是一个递减的指数函数，在迭代后期将趋近于零，因此，地磁算子十分依赖全局最优位置x′_g，本实施例对全局最优位置进行改进，对其引入高斯扰动，避免鸽群粒子的最优位置陷入局部最优，提高整个种群的多样性，具体公式如下所示：

x_G(t)＝x_g(t)+x_g(t)·η·G

其中，x_g(t)为全局最优位置，x_G(t)为全局最优位置受到高斯扰动后的位置，η是一个(0，1)之间的控制参数，G是服从均值为0方差为1的高斯分布，其概率密度函数如下式所示：

其中，x是随机变量，G是关于随机变量x的函数，e是自然对数函数的底数。

进一步的，鸽群优化算法在搜索过程中，应以一定的概率接受较差解，可以有效地避免在迭代搜索过程中陷入局部最优，本实施例提出使用模拟退火算法规则来解决这一问题，设扰动前的全局最优位置x_g的适应度值为f(x_g)，受到高斯扰动后的全局最优位置x_G的适应度值为f(x_G)。如果f(x_G)优于f(x_g)或者下面的不等式成立，则将受到扰动后的位置x_G作为改进后的全局最优位置x′_g；否则，仍使用扰动前的全局最优位置x_g作为改进后的全局最优位置x′_g，具体公式如下所示：

T(t)＝μ×T(t-1)

其中，exp()表示以自然常数e为底的指数函数，t为迭代次数，K为玻尔兹曼常数，rand是一个产生(0，1)之间随机数的函数，μ是衰减因子，本发明中设置为0.99，T(t)为当前迭代的退火温度，随着迭代的进行该值逐渐下降。

改进的地标算子更新算法如下所示：

N_p(t)＝logsig((Nc_max/2-t)/k)×N_pmax

x_i(t)＝x_i(t-1)+rand·(x_c(t)-x_i(t-1))

其中，N_p(t)表示当前迭代次数的鸽群数量，N_pmax表示种群规模的数量最大值，Nc_max为地标算子阶段的最大迭代次数，t为当前迭代次数，k为logsig函数的斜率本实施例中设置为5。在原始地标算子中，迭代后期种群数量过少，影响算法寻优。地标算子是对目标的精细搜索，在迭代初期种群规模可以稍大一些，但随着迭代次数逐渐增大时，种群规模应逐渐减小。而logsig函数具有从1到0非线性减少的特性，所以本实施例引入logsig函数作为鸽群数量的步长。i表示鸽群中鸽子的只数，x_c(t)代表第t代鸽群的中心，x_i(t)为第i只鸽子在第t次迭代中的位置，rand是一个产生(0，1)之间随机数的函数，f(x_i(t))是第i只鸽子的适应度函数。

航路代价计算式如下所示：

f＝kf₁+(1-k)f₂

其中，f表示总的航路代价，f₁表示威胁源代价，f₂表示航程代价，权重k为一个(0，1)之间的随机数。

如图4所示，本实施例将每条航路分为十段，取航路上第一、三、五、七、九这五个航路节点，将五个航路节点与威胁源41之间的威胁源代价相加来表示整条航路的威胁源代价f₁，公式如下所示：

其中，i为航路上的航段个数，k为威胁源个数，L_i为第i段航路的长度，N_t为威胁源数量，t_k为表示第k个威胁源的威胁因子，d_0.1，k、d_0.3，k、d_0.5，k、d_0.7，k、d_0.9，k分别表示航路上第一、三、五、七、九个航路节点距第k个威胁源的距离。

航程代价f₂的计算公式如下所示：

其中，n为航路上的航段，i为航路上的航段个数，l_i为第i段航路的长度。

S6，使用B样条曲线对规划后的局部航路进行平滑化处理，实现对无人机飞行航路进行动态避障规划。B样条曲线是贝塞尔样条曲线的一种特例曲线，通过逼近多边形而获得曲线，B样条曲线中一段样条曲线获得的公式如下所示：

其中，i是控制点个数，P_i为第i个控制点，n为B样条曲线阶次，B_i，n(t)是n阶(n-1)次B样条曲线基函数，t∈[0，1]。P(t)表示每个控制点与其对应的基函数曲线相乘再相加的结果，每个控制点发生变化，都会影响最终生成的整条曲线。整条B样条曲线由几段样条曲线组成，相邻曲线有共同的控制点整条B样条曲线由几段样条曲线连接而成，相邻样条曲线有共同的控制点，B样条曲线次数越多，控制点影响的曲线段数就越多，不利于曲线的局部控制，因此，通过基于三次B样条曲线来进行局部航路的平滑化，取n＝3，则三次B样条曲线的基函数表示公式如下所示：

B样条曲线的矩阵表示公式如下所示：

其中，t∈[0，1]，P_i(i＝0，1，2，3)表示控制三次B样条曲线的4个控制点。

如图7a所示，如果三次B样条曲线平滑化后还存在航迹有部分进入到动态障碍物31膨化后的外圆33内部时，选取B样条曲线中离相交碰撞部分最近的四个航迹点，如图7b所示，对B样条曲线三条边的中点取新的航迹点进行重规划，若仍有部分航路进入障碍物，则继续重复上述操作，直至消除航路与障碍物的碰撞为止。

S7，设置无人机的飞行航路为为全局离线航路与局部航路结合，进入步骤S3。

S8，完成飞行任务，流程结束。

通过仿真实验对本发明的优越性进行验证，仿真实验在一台64位windows 10操作系统的PC机中使用Matlab R2017b软件下编程实现，硬件设置为：英特尔i5-8300H@2.30GHz处理器，8G内存。栅格法建立的二维空间模型采用的地理范围为33*33km，起始节点坐标设置为(3，3)，目标节点坐标设置为(33，33)，静态障碍物的数量设置为7个，其大小与位置随机设置，如图8所示，建筑物，树木等无法穿越的静态物体设置为静态障碍物，用黑色栅格表示。无人机可自由移动的栅格用白色栅格表示。

在综合考虑复杂度和精度的情况下，鸽群的种群数量设置为150，地磁算子迭代次数设置为150，地标算子迭代次数设置为50，空间解的维度D设置为20，地磁因子R设置为0.2，航路代价中的权重k设置为0.9，初始退火温度设置为100。根据给定的地理范围设定一个突发动态障碍，如图9a和图9b所示，突发动态障碍物用圆形的威胁源表示，其威胁源中心坐标设置为(20，13)，威胁半径设置为2，威胁因子设置为3，然后对突发障碍物部分进行局部航路规划并平滑化处理。如图10a和图10b所示，再次检测到动态障碍物时，其威胁源中心坐标设置为(26，21)，威胁半径设置为2，威胁因子设置为2，同样进行局部航路规划和平滑化处理，并且后续过程未检测到障碍物，无人机到达目标节点时，输出飞行航路。

使用传统鸽群算法，出现一个突发动态障碍时，进行局部航路规划及未进行平滑化处理后的效果如图11a所示，出现二个突发动态障碍时，进行局部航路规划及未进行平滑化处理后的效果如图11b所示。

在相同环境下，使用本发明与传统鸽群算法进行200次不同数量障碍物情况的仿真实验对比，有效的分析出算法的性能如图12所示，其中，传统鸽群算法10次仿真实验性能指标结果如表1所示：

表1

使用本发明的10次仿真实验性能指标结果如表2所示：

表2

根据表1和表2的数据可以看出，在相同的环境下，从航路长度角度分析，使用发明在一个突发障碍物和两个突发障碍物时，多次实验均值分别为49.040km和48.005km，比传统鸽群算法规划的飞行航路出的航路长度51.050km和50.026km分别缩短了3.94％和4.04％；从威胁代价角度分析，使用发明在一个突发障碍物和两个突发障碍物时，多次实验均值分别为4.615和4.636，比传统鸽群算法的威胁代价消耗5.301和5.641分别减少了12.94％和17.82％；从算法执行时间角度分析，使用本发明在一个突发障碍物和两个突发障碍物时，多次实验均值分别为1.982s和3.969s，比传统鸽群算法执行所耗时间3.072s和5.139s分别降低了35.48％和22.77％。仿真实验结果表明，使用本发明在出现动态障碍物时规划后的无人机飞行航路具有更短的航路长度和更小的威胁代价,并且飞行任务消耗的时间更短，所以采用本发明在进行无人机飞行航路的规划时既可以节省无人机的飞行成本，还能针对动态障碍物作出准确的避障及局部航路的重规划，使动态障碍物对无人机飞行航路上的影响更小，提高无人机飞行任务的安全性和快速性。

上面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，不能理解为对本发明保护范围的限制。

总之，本发明虽然列举了上述优选实施方式，但是应该说明，虽然本领域的技术人员可以进行各种变化和改型，除非这样的变化和改型偏离了本发明范围，否则都应该包括在本发明的保护范围内。

Claims (4)

1.一种无人机航路规划及避障方法，其特征在于，包括：

S1，利用栅格法建立二维空间模型，根据无人机的飞行起点和飞行终点设置带起始节点、航路节点和目标节点的空间模型数据；

S2，利用空间模型数据获得全局离线航路，设置无人机的飞行航路为全局离线航路；

S3，无人机沿飞行航路飞行，并通过无人机上的传感器实时监测飞行航路上是否出现动态障碍物；

S4，监测飞行航路上出现动态障碍物时，进入下一步骤；监测飞行航路上未出现动态障碍物时，判断无人机是否达到飞行终点，若未达到飞行终点，进入步骤S3，若达到飞行终点，则进入步骤S8；

S5，采用带约束的伴随矩阵方法对鸽群算法进行初始化，再用鸽群算法进行局部航路规划；

具体为：所述采用带约束的伴随矩阵方法对鸽群算法进行初始化之前，在伴随矩阵方法中增加从局部起始航点开始朝着局部目标航点方向搜索航路节点的约束条件，找出代价消耗较低的航路，将代价消耗较低的航路上两个航路节点之间违反无人机飞行机动性能的航路节点去除，留下符合飞行机动性能的航路节点；

所述采用带约束的伴随矩阵方法对鸽群算法进行初始化具体为，首先，随机取代价消耗较低的航路节点上下一定数量的初始数据对鸽群算法进行初始化，在二维空间平面中进行坐标轴方向转换，将局部起始航点与局部目标航点的坐标轴方向转换成新的横坐标，并根据新的横坐标建立新的坐标系；

所述伴随矩阵方法初始化鸽群算法过程中，对鸽群算法采用一种自适应的编码方式，以代价消耗较低航路上的一个坐标值作为鸽群算法粒子的新编码值，粒子在旋转后坐标系中的横坐标获得公式如下所示：

其中，X是坐标轴名称，i是粒子个数，D是空间解的维数，X_start是局部起始航点在坐标转换后的横坐标，X_goal是局部目标航点在坐标转换后的横坐标；

所述鸽群算法使用改进的地磁算子迭代，进行局部目标航点的搜索，每一次迭代都会得到鸽群新的位置和速度，在地磁算子的迭代次数达到规定的次数时，启用改进的地标算子迭代来更新鸽群种群数量和种群粒子的位置，获得全体粒子中最优的粒子，进而得出局部重规划后的局部航路；

改进的地磁算子更新算法如下所示：

v_i(t)＝v_i(t-1)e^-Rt+rand(x′_g-x_i(t-1))

x_i(t)＝x_i(t-1)+v_i(t)

其中，t是鸽群迭代的次数，i是鸽子的数量，v_i(t)为第i只鸽子在第t次迭代后的速度，x_i(t)为第i只鸽子在第t次迭代后的位置，e是自然对数函数的底数，R是地磁因子，x′_g为此次迭代中改进的全局最优位置，rand是一个产生(0，1)之间随机数的函数；

改进的地标算子的更新算法如下所示：

N_p(t)＝logsig((Nc_max/2-t)/k)×N_pmax

x_i(t)＝x_i(t-1)+rand·(x_c(t)-x_i(t-1))

其中，N_p(t)表示当前迭代次数的鸽群数量，N_pmax表示种群规模的数量最大值，Nc_max为地标算子阶段的最大迭代次数，t为当前迭代次数，k为log sig函数的斜率，i表示鸽群中鸽子的只数，x_c(t)代表第t代鸽群的中心，x_i(t)为第i只鸽子在第t次迭代中的位置，rand是一个产生(0，1)之间随机数的函数，f(x_i(t))是第i只鸽子的适应度函数；

S6，使用B样条曲线对规划后的局部航路进行平滑化处理，实现对无人机飞行航路进行动态避障规划；

S7，设置无人机的飞行航路为全局离线航路与局部航路结合，进入步骤S3；

S8，完成飞行任务，流程结束。

2.如权利要求1所述的一种无人机航路规划及避障方法，其特征在于：所述步骤S2中所述利用空间模型数据获得全局离线航路具体为，采用双向迪杰斯特拉算法进行全局离线航路规划，将飞行航路上各节点之间的权重视为航路的代价消耗，从起始节点往目标节点和从目标节点往起始节点同时进行单向航路节点搜索，当两个方向搜索在同一个航路节点时，得出规划后的全局离线航路。

3.如权利要求2所述的一种无人机航路规划及避障方法，其特征在于：所述步骤S4中所述监测飞行航路上出现动态障碍物后，对动态障碍物进行膨胀化操作，使动态障碍物的边界外扩膨胀化为一个包含该动态障碍物的内圆，进一步根据无人机最小安全半径在内圆外膨胀化一个更大的外圆，将无人机勘测到动态障碍物时的当前位置设为局部起始航点，全局离线航路上未受外圆影响的下一航路节点为局部目标航点。

4.如权利要求3所述的一种无人机航路规划及避障方法，其特征在于：在所述S6步骤中所述使用B样条曲线对规划后的局部航路进行平滑化处理具体为，通过基于三次B样条曲线来进行局部航路的平滑化，三次B样条曲线平滑化后存在航迹有部分进入到障碍物内部时，选取B样条曲线中离相交碰撞部分最近的四个航迹点，对B样条曲线三条边的中点取新的航迹点进行平滑化，仍有部分航迹进入障碍物时，则继续重复上述操作，直至消除航迹与障碍物的碰撞为止。

Images