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

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

An optimization method of track planning for four rotor unmanned aerial vehicles based on ant colony algorithm

【技术实现步骤摘要】
一种基于蚁群算法的四旋翼无人机的航迹规划优化方法
本专利技术属于信息
，涉及一种四旋翼无人机的蚁群算法航迹规划方法，具体涉及一种基于蚁群算法的四旋翼无人机的航迹规划优化方法。
技术介绍
近几年，无人机的发展十分迅速，各种型号无人机相继出现，但是四旋翼无人机的航迹规划存在以下诸多方面的不足：大部分现有的路径规划算法只能规划二维平面路径，而一般的三维规划算法，大多数运算算法复杂，需要很大的存储空间，同时难以进行全局路径规划。在应用基本三维蚁群算法进行航迹搜索时，两平面直接相连容易使航迹直接穿过障碍物，并且搜索出的航迹节点较多，适应度值过大。针对上述缺陷，目前已出现了一系列的解决办法，其中利用到的理论，具体可以总结如下：(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－本文档来自技高网...

【技术保护点】
一种基于蚁群算法的四旋翼无人机的航迹规划优化方法，其特征在于，包括以下步骤：步骤1：利用三维栅格图法划分规划空间；步骤2：绘制基于三维栅格图的三维环境地图；步骤3：进行四旋翼无人机路径规划。

【技术特征摘要】
1.一种基于蚁群算法的四旋翼无人机的航迹规划优化方法，其特征在于，包括以下步骤：步骤1：利用三维栅格图法划分规划空间；步骤2：绘制基于三维栅格图的三维环境地图；步骤3：进行四旋翼无人机路径规划。2.根据权利要求1所述的基于蚁群算法的四旋翼无人机的航迹规划优化方法，其特征在于：步骤1中，以纬度值作为横坐标，经度值作为纵坐标，以地面障碍物的高度作为竖坐标，垂直于海平面，划分三维栅格单元；单元格内若有障碍物，则判定该单元格是不能通过的单元格。3.根据权利要求1所述的基于蚁群算法的四旋翼无人机的航迹规划优化方法，其特征在于，步骤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表示向正无穷方向取整；所述主方向的选取，是选择纬度方向和经度方向中栅格变化数最多的方向作为四旋翼无人机航迹规划主方向；所述种群数的确定，根据实际情况进行人为确定；所述迭代次数的选择，根据实际情况进行人为确定；所述航迹搜寻范围选取，假设选定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.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上的节点进行局部信息素更新；步骤3.4：重复执行步骤3.2和步骤3.3，直到到达平面Πn－1，然后使用变主方向搜索策略和简化航迹策略；步骤3.5：按照适应度值函数计算每条航迹的适应度值，比较找出最小适应度值，而最小适应度对应的航迹即为当前最优航迹；适应度值函数为：W＝k1c1L+k2c2/dmin+(1-k1-k2)c3H；其中，W为适应度值，k1、k2为权值系数，c1、c2、c3为比例系数，L为路程，dmin为路径上的每一点离最危险点的距离，H为每一点离海平面的高度；步骤3.6：进行全局信息素更新；从现有的搜索到的路径中选择出最短的航迹，更新适应度值最小的航迹所经过的所有栅格...

【专利技术属性】
技术研发人员：王粟，江鑫，朱飞，李庚，邱春辉，
申请(专利权)人：湖北工业大学，
类型：发明
国别省市：湖北,42