[发明专利]一种基于蚁群算法的四旋翼无人机的航迹规划优化方法

权利要求书

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

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

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

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

2.根据权利要求1所述的基于蚁群算法的四旋翼无人机的航迹规划优化方法，其特征在于：步骤1中，以纬度值作为横坐标，经度值作为纵坐标，以地面障碍物的高度作为竖坐标，垂直于海平面，划分三维栅格单元；单元格内若有障碍物，则判定该单元格是不能通过的单元格。

3.根据权利要求1所述的基于蚁群算法的四旋翼无人机的航迹规划优化方法，其特征在于，步骤3的具体实现包括以下子步骤：

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

所述起始点的确定，假设四旋翼无人机S起始点所在的栅格的坐标值(X_start，Y_start，Z_start)，终止点所在栅格的坐标值(X_end，Y_end，Z_end)，三维栅格地图原点坐标值为(X_Gridstart，Y_Gridstart，Z_Gridstart)，则四旋翼无人机S的放置位置(S_lat，S_lon，S_h)和其所在栅格的栅格坐标位置(X_start，Y_start，Z_start)的关系为：

Xstart=ceil(Slat-XGridstartXGrid)×XGrid+XGridstartYstart=ceil(Slon-YGridstartYGrid)×YGrid+YGridstartZstart=ceil(Sh-ZGridstartZGrid)×ZGrid+ZGridstart;]]>

其中：X_Grid表示X轴划分的单位大小，Y_Grid表示Y轴划分单位大小，Z_Grid表示Z轴划分的单位大小，S_lat表示当前无人机放置的X轴的坐标，S_lon表示当前无人机放置的Y轴的坐标，S_h表示当前无人机放置的Z轴的坐标，ceil表示向正无穷方向取整；

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

所述种群数的确定，根据实际情况进行人为确定；

所述迭代次数的选择，根据实际情况进行人为确定；

所述航迹搜寻范围选取，假设选定X方向为主方向，沿X轴方向从X_start到X_end划分成n＝|X_start－X_end|+1个平面，编号为Π₁，Π₂，Π₃，...，Π_n，则四旋翼无人机航迹就分成(n-1)段；假设四旋翼无人机运行至第i个平面Π_i上的一点(X_i，Y_i，Z_i)处，那么下一个运行的栅格就在Π_i+1上；下一个栅格坐标选择的具体过程为：对于主方向X上直接以平面Π_i+1的横坐标作为下一个节点的横坐标，即新的X坐标值为X_i+1；对于Y方向和Z方向坐标值的选择不是直接选择的，而是在平面Π_i+1选择没有障碍物的栅格放入数组Allowed中，否则被舍弃；然后从中选择一个栅格点作为下一个运行栅格；对部分栅格进行搜索以找到最优航迹；对于非主方向Y，从平面Π_i到平面Π_i+1，对于Y方向上以Y_i为中心从Y_i－bc_max到Y_i+bc_max范围内的点都是可选择作为Y_i+1点；同样，对于Z方向上来说以Z_i为中心从Z_i－hc_max到Z_i+hc_max范围内的点都是可选择作为Z_i+1点；其中，bc_max表示在Y轴上搜索的半径范围，hc_max表示在Z轴上搜索的半径范围；

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

步骤3.2：航迹搜索；

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

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

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

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

适应度值函数为：

W＝k₁c₁L+k₂c₂/d_min+(1-k₁-k₂)c₃H；

其中，W为适应度值，k₁、k₂为权值系数，c₁、c₂、c₃为比例系数，L为路程，d_min为路径上的每一点离最危险点的距离，H为每一点离海平面的高度；

步骤3.6：进行全局信息素更新；

从现有的搜索到的路径中选择出最短的航迹，更新适应度值最小的航迹所经过的所有栅格的信息素值；

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