一种基于蚁群算法的四旋翼无人机的航迹规划优化方法与流程

本发明属于信息技术领域，涉及一种四旋翼无人机的蚁群算法航迹规划方法，具体涉及一种基于蚁群算法的四旋翼无人机的航迹规划优化方法。

背景技术：

近几年，无人机的发展十分迅速，各种型号无人机相继出现，但是四旋翼无人机的航迹规划存在以下诸多方面的不足：

大部分现有的路径规划算法只能规划二维平面路径，而一般的三维规划算法，大多数运算算法复杂，需要很大的存储空间，同时难以进行全局路径规划。

在应用基本三维蚁群算法进行航迹搜索时，两平面直接相连容易使航迹直接穿过障碍物，并且搜索出的航迹节点较多，适应度值过大。

针对上述缺陷，目前已出现了一系列的解决办法，其中利用到的理论，具体可以总结如下：

(1)基于栅格图法的环境模型构建；

基于栅格图法的环境模型构建栅格图法，是把自由空间c划分成一系列规则的单元格，根据单元格中是否有障碍物和障碍物的覆盖面积来判断。机器人路径规划中栅格图法得到广泛应用。根据自由空间的维数，栅格图法可以分为二维栅格图法和三维栅格图法。请见图1，二维栅格图是一个平面图形，三维栅格图是一个三维模魔方形。二维栅格图中，设x轴的最大值为xmax，最小值为xmin，x轴划分单位大小为xgrid，y轴的最大值为ymax，最小值为ymin，y轴划分单位大小为ygrid，则二维栅格图的总网格数目n为：

请见图2，对于三维栅格图，设x轴的最大值xmax，最小值xmin，x轴划分单位大小为xgrid，y轴的最大值ymax，最小值ymin，y轴划分单位大小为ygrid，z轴的最大值zmax，最小值zmin，z轴划分单位大小为zgrid，则三维栅格图的总网格数目n为：

(2)三维蚁群算法；

蚁群算法是一种概率搜索算法，它利用生物信息激素作为蚂蚁选择后续行为的依据。每只蚂蚁会对一定范围内其它蚂蚁散布的生物信息激素做出反应，依据生物信息激素的强度在每一个道口对多条路径选择做出概率上的判断并执行该选择，由此察觉到并影响它们以后的行为。受到蚂蚁这种觅食方式的启发，m.dorigo、v.maniez-zo、a.colorini等人在1991年首先提出了蚁群算法，并将其应用于tsp问题，取得了一定成效。这里主要讨论蚁群算法在无人机航路规划中的应用问题中由于具有动态性、分布性和协同性等一些特性，蚁群算法在航路规划甚至是任务规划中将有很好的发展前景。

技术实现要素：

为了解决上述技术问题，本发明提供了一种基于蚁群算法的四旋翼无人机的航迹规划与优化方法。

本发明提供的一种基于蚁群算法的四旋翼无人机的航迹规划优化方法，其特征在于，包括以下步骤：

步骤1：利用三维栅格图法划分规划空间；

步骤2：绘制基于三维栅格图的三维环境地图；

步骤3：进行四旋翼无人机路径规划。

本发明针对解决与优化四旋翼无人机的航迹规划问题；在建立模型方面，考虑到无人机的自身特点和外界复杂的环境，本发明在建立模型的时候可以建立了一个高程度的三维环境模型来减少碰到障碍物的概率。在规划算法方面，本发明采用的是三维蚁群算法，通过选取主方向的来完成航迹的搜索，同时在算法的改进上采用了发明的变主方向搜索策略和简化航迹策略，成功的解决了传统基本三维蚁群算法进行航迹搜索时，两平面直接相连容易使航迹直接穿过障碍物，并且搜索出的航迹节点较多，适应度值过大的问题，很好地避开障碍物，减小了路径长度，提高了搜索效率。

附图说明

图1为本发明背景技术中二维栅格图；

图2为本发明背景技术中三维栅格图；

图3为本发明实施例中选定x方向为主方向的航迹规划原理图。

具体实施方式

为了便于本领域普通技术人员理解和实施本发明，下面结合附图及实施例对本发明作进一步的详细描述，应当理解，此处所描述的实施示例仅用于说明和解释本发明，并不用于限定本发明。

三维蚁群算法解决的问题主要是三维航迹规划问题，该模型和基本的蚁群算法模型有很多不同之处。在原蚁群算法及改进算法的基础之上对其进行了变形，在变形时需要考虑到航迹规划的环境模型。在此，本发明将使用三维栅格图作为环境模型。因为直接在三维栅格图中对所有的栅格进行搜索找到最优路径需要的计算量和计算时间很大，所以这里采用的方法并不是对所有的栅格进行搜索，而是对部分栅格进行搜索以找到最优航迹。

本发明提供的一种基于蚁群算法的四旋翼无人机的航迹规划优化方法，包括以下步骤：

步骤1：利用三维栅格图法划分规划空间；

发明先利用三维栅格图法来划分规划空间。在使用三维栅格图作为环境模型时就必须考虑到三维栅格的分辨率问题，分辨率既不能太大也不能太小。考虑三维栅格分辨率时，首先要考虑的问题就是四旋翼无人机的体积。要保证四旋翼无人机在飞行过程中能够不碰到障碍物，栅格的长、宽、高要大于四旋翼的长、宽、高，另外还要考虑到误差。另外一方面，设定三维栅格分辨率时还要考虑四旋翼的定位方式。gps定位是目前最常用的定位方式之一，在四旋翼无人机上安装一个gps导航模块就可以通过接收gps信号，进而判断四旋翼无人机的当前位置，所以本发明选择使用gps定位。gps定位时，纬度上每变化0.001'约为1.837m，经度上每变化0.001'约为1.281m，gps定位时会存在1～2m的误差。综上考虑上述两个方面的因素，可根据实际情况以纬度值作为横坐标，经度值作为纵坐标，以地面障碍物的高度作为竖坐标，垂直于海平面。这样划分三维栅格单元，就可以兼顾四旋翼的尺寸，又考虑到了gps定位的误差，从而可以减小碰到障碍物的概率。

本在对栅格单元划分时有两种方案：一种是只要单元格内有障碍物就判定该单元格是不能通过的；另一种方案是根据单元格内障碍物覆盖面积来判断单元格是否可通过，即单元格内障碍物占的面积如果大于一定比例就认为该单元格是不可通过，否则就认为是可以通过的。第二种方案比第一种方案找到最优解的可能性高，但无人机碰到障碍物的可能性也更高，因此选择第一种方案的更多一些。

步骤2：绘制基于三维栅格图的三维环境地图；

步骤3：进行四旋翼无人机路径规划；具体实现包括以下子步骤：

步骤3.1：参数初始化设置，包括起始点的确定、主方向的选取、种群数的确定、迭代次数的选择、航迹搜寻范围选取、信息素初始化设置；

起始点的确定，假设四旋翼无人机s起始点所在的栅格的坐标值(xstart，ystart，zstart)，终止点所在栅格的坐标值(xend，yend，zend)，三维栅格地图原点坐标值为(xgridstart，ygridstart，zgridstart)，则四旋翼无人机s的放置位置(slat，slon，sh)和其所在栅格的栅格坐标位置(xstart，ystart，zstart)的关系为：

其中：xgrid表示x轴划分的单位大小，ygrid表示y轴划分的单位大小，zgrid表示z轴划分的单位大小，slat表示当前无人机放置的x轴的坐标，slon表示当前无人机放置的y轴的坐标，sh表示当前无人机放置的z轴的坐标，ceil表示向正无穷方向取整。

主方向的选取，是选择纬度方向和经度方向中栅格变化数最多的方向作为四旋翼无人机航迹规划主方向；

本实施例中主方向的选取，是比较起始点和终止点横纵坐标的变化值大小，即比较(xstart－xend)/xgrid和(ystart－yend)/ygrid的大小，如果(xstart－xend)/xgrid大于(ystart－yend)/ygrid，则选择x方向作为主方向；否则选择y方向为主方向；

所述种群数的确定，根据实际情况进行人为确定，数量很重要，因为数量过大时，会导致搜索过的路径上信息素变化趋于平均，这样就不好找出好的路径了；数量过小时，易使未被搜索到的路径信息素减小到0，这样可能会出现早熟，没找到全局最优解。在本实施例中可以暂定种群数为25。

所述迭代次数的选择，根据实际情况进行人为确定，数量很重要，因为迭代次数值过小，可能导致算法还没收敛就已结束；过大则会导致资源浪费，在本实施例中可以暂定迭代数为50。

航迹搜寻范围选取，假设选定x方向为主方向，沿x轴方向从xstart到xend划分成n＝|xstart－xend|+1个平面，编号为π1，π2，π3，...，πn，则四旋翼无人机航迹就分成(n-1)段；假设四旋翼无人机运行至第i个平面πi上的一点(xi，yi，zi)处，那么下一个运行的栅格就在πi+1上；下一个栅格坐标选择的具体过程为：对于主方向x上直接以平面πi+1的横坐标作为下一个节点的横坐标，即新的x坐标值为xi+1；对于y方向和z方向坐标值的选择不是直接选择的，而是在平面πi+1选择没有障碍物的栅格放入数组allowed中，否则被舍弃，；然后从中选择一个栅格点作为下一个运行栅格；然而因为直接在三维栅格图中对所有的栅格进行搜索找到最优路径需要的计算量和计算时很大，所以本文采用的方法并不是对所有的栅格进行搜索，而是对部分栅格进行搜索以找到最优航迹。对于非主方向y，从平面πi到平面πi+1，对于y方向上以yi为中心从yi－bcmax到yi+bcmax范围内的点都是可选择作为yi+1点；同样，对于z方向上来说以zi为中心从zi－hcmax到zi+hcmax范围内的点都是可选择作为zi+1点；其中，bcmax表示在y轴上搜索的半径范围，hcmax表示在z轴上搜索的半径范围。

请见图3，对于x方向，平面上任意一个栅格(x，y，z)作为下一个运行栅格的概率计算为：

式中：τ(x，y，z)是平面πi+1上坐标为(x，y，z)的栅格的信息素值；h(x，y，z)是平面πi+1上坐标为(x，y，z)的栅格的启发函数，其计算公式为：

h(x，y，z)＝d(x，y，z)^ω1×s(x，y，z)^ω2×q(x，y，z)^ω3；

式中：d(x，y，z)是当前点和(x，y，z)的路径长度，这可以促使蚂蚁尽可能选则距离当前点最近的点，计算公式为：

s(x，y，z)表示安全性因素，促使蚂蚁选择安全点；当前栅格(xi，yi，zi)和(x，y，z)是不可连通的，或者栅格(x，y，z)内有障碍物，则称该栅格是不可达的；s(x，y，z)的计算公式为：

q(x，y，z)为栅格(x，y，z)到终止栅格(xend，yend，zend)的距离，q(x，y，z)可以促使蚂蚁选择离目标栅格更近的栅格，其计算公式为：

ω1、ω2、ω3是系数，其大小代表了上述各因素的重要性程度，系数值越大代表该项越重要，否则说明该项越不重要；

信息素初始化设置，把三维栅格图的三维环境地图中的所有栅格信息素值设置为固定值；

步骤3.2：航迹搜索；

在航迹搜索过程中，假设popnum只蚂蚁中的第k只蚂蚁已运行至平面πi上的点(xi，yi，zi)处，搜索在平面πi+1上以(xi+1，yi，zi)为中心count＝(2×bcmax+1)×(2×hcmax+1)个点；将count个栅格中所有的可行栅格放入数组allowed中，可行栅格是没有障碍物的栅格；如果数组allowed为空，那么将当前点(xi，yi，zi)的zi坐标值加1，当前点坐标变为(xi，yi，zi+1)，重新搜索平面上的可行栅格，直到数组allowed不为空；从数组allowed中按照轮盘赌法选出一个可行的栅格作为平面πi+1上的航迹节点；

步骤3.3：对平面πi上的节点进行局部信息素更新；

蚁群算法中蚂蚁是根据信息素浓度来进行搜索路径的，在每走完一段或全部路径时，就要对信息素值进行更新，所以信息素初始值设定和更新对于蚁群算法是否能够成功搜索具有重要影响。这里把信息素值存储在三维空间一系列离散的点中，然后对这些离散点的信息素值进行更新，所以对于每一个栅格来说都有一个信息素值，这个信息素值就代表了该栅格对蚂蚁的吸引程度，每个栅格的信息素值在蚂蚁经过之后都要进行更新。信息素的更新分为局部更新和全局更新两部分。局部更新是指只要有蚂蚁经过某栅格，该栅格的信息素值就会被更新，更新后栅格的信息素值会被减少，这样这个栅格在以后的搜索中被选中的概率就被降低，而相应地增加其他未被搜索的栅格被搜索的概率，这样就可以达到全局搜索的目的。

局部搜索的信息素更新公式为：

式中，ζ表示信息素衰减系数，τx，y，z表示栅格(x，y，z)的信息素值。

全局信息素更新是指蚂蚁完成一条航迹搜索时，计算该路径的适应度值，从现的搜索到的路径中选择出最短的航迹，更新适应度值最小的航迹所经过的所有栅格的信息素值，信息素更新公式为

τx,y,z＝(1-ρ)τx,y,z+ρδτx,y,z

式中：length(m)表示蚂蚁m经过的路径长度，ρ表示信息素挥发系数，k为系数。

步骤3.4：重复执行步骤3.2和步骤3.3，直到到达平面πn－1，然后使用变主方向搜索策略和简化航迹策略；

在应用基本三维蚁群算法进行航迹搜索时，两平面直接相连容易使航迹直接穿过障碍物，并且搜索出的航迹节点较多，适应度值过大。所以针对这两个问题本发明对算法进行了改进，提出了变主方向搜索策略和简化航迹策略。

1.变主方向搜索策略：

将πn－1平面上节点和πn平面上的节点即终点直接相连容易穿过障碍物。因此首先需要判断两点间的连通性。想要知道空间中两点是否连通，首先要判断空间直线经过哪些栅格，然后判断这些栅格所在的经纬度位置的障碍物高度和直线高度关系，如果障碍物高度高于直线高度，那么这条直线经过障碍物，也就是说空间中两点是不可连通的，否则就是可连通的。而需要判断三维直线投影经过哪些栅格时，只要找出直线与栅格的横纵坐标的交点就可以。

变主方向搜索策略的基本思想是:如果πn－1平面上的节点和πn平面上终点是不连通的，那么就以πn－1平面上的节点(xn－1，yn－1，zn－1)为起始点，以πn平面上终点(xend，yend，zend)为终止点，再次搜索航迹。记第一次搜索的主方向上平面划分为π1，1，π1，2，...，π1，n1，搜索节点依次为(x1，1，y1，1，z1，1)，(x1，2，y1，2，z1，2)，…，(x1，n1－1，y1，n1－1，z1，n1－1)；同理，第m次搜索的主方向上的平面划分记为πm，1，πm，2，...，πm，nm，搜索的节点依次记为(xm，1，ym，1，zm，1)，(xm，2，ym，2，zm，2)，...，(xm，nm－1，ym，nm－1，zm，nm－1)。最终的三维航迹为：

(x1，1，y1，1，z1，1)，...，(x1，n1－1，y1，n1－1，z1，n1－1)，(x2，2，y2,2，z2，2)，...，(x2，n2－1，y2，n2－1，z2，n2－1)，...，(xm，2，ym，2，zm，2)，...，(xm，nm－1，ym，nm－1，zm，nm－1)，(xend，yend，zend)

2.简化航迹策略：

航迹节点过多意味着四旋翼无人机在飞行的过程中需要转折次数过多，采用一定的简化策略来减少航迹节点既能减小适应度值还能减小航迹主要思路是:假设变主方向搜索策略所搜索出的一条可行航迹为route＝(r1，r2，...，rn)。首先把节点r1放入新航迹数组newroute中，判断r1与rn的连通性，如果是连通的，把r1放入数组newroute中；否则判断r1与rn－1的连通性，直到找到一个点ri与r1是连通的。对ri进行相同的操作，直到终止点。

步骤3.5：按照适应度值函数计算每条航迹的适应度值，比较找出最小适应度值，而最小适应度对应的航迹即为当前最优航迹；

适应度值函数为：w＝k1c1l+k2c2/dmin+(1-k1-k2)c3h；

其中，w为适应度值。k1.k2为权值系数,可根据不同的任务要求进行调整。c1.c2.c3为比例系数。l为路程。dmin为路径上的每一点离最危险点的距离。h为每一点离海平面的高度。

步骤3.6：进行全局信息素更新，即从现有的搜索到的路径中选择出最短的航迹，更新适应度值最小的航迹所经过的所有栅格的信息素值。

τx,y,z＝(1-ρ)τx,y,z+ρδτx,y,z

式中:length(m)表示蚂蚁m经过的路径长度；ρ表示信息素挥发系数；k为系数。

步骤3.7：将上述步骤3.2-步骤3.6过程迭代n次，找到迭代n次的最优航迹。

步骤4：利用已经测试到的环境模型的经度、纬度和高度数据绘制三维环境地图，然后在三维环境地图中绘制最优航迹。

应当理解的是，本说明书未详细阐述的部分均属于现有技术。

应当理解的是，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。