基于遗传算法的无人机航迹规划

UAV Route Planning and Obstacle Avoidance

摘要: 本项目设计融合无人飞行器，航迹规划和测距避障等技术，研发具有能自主规划航迹，实现从无人机遥控飞行技术向智能化地自主规划航迹飞行技术的转变，并且重点解决无人机在威胁代价(地形、环境等)下实现无人工控制、自主地规划航迹进行飞行并完成任务的复杂智能技术问题从而提高任务的完成率以及大大缩短任务的完成时间。

Abstract: This project is designed to integrate technologies such as unmanned aerial vehicles, trajectory planning, distance measurement and obstacle avoidance, etc. The research and development have the ability to independently plan trajectories and realize the transformation from UAV remote control flight technology to intelligently autonomous trajectory flight technology to solve the complex intelligent technical problems of unmanned aerial vehicles under the threat cost (terrain, environment, etc.) to achieve flight planning and complete tasks without artificial control, autonomously plan trajectories, thereby improving the task completion rate and greatly reducing the task completion time.

1. 引言

本项目的重点在于解决无人机在威胁代价(地形、环境等)下实现无人工控制、自主地规划航迹并完成任务的复杂智能技术问题从而提高任务的完成率以及大大缩短任务的完成时间。

上世纪八十年代以来，无人机自动航迹规划技术的研究在国外得到高度的重视。基于人工智能技术，即使遇到环境威胁问题，也可以保证一定的机身稳定性，同时利用机载传感器获取环境信息，对环境信息进行数据分析，结合导航系统数据，实时产生最优轨迹，并给出沿最优轨迹飞行的导引控制指令，具备了时效性、最优化、精确性等特点。国内西北工业大学王振华等人进行了基于改进概率地图的无人机实时避障研究，针对无人机路径规划问题，以等高线地图作为任务空间，提出一种新的采样模型，避免相交检验 [1]。另外，还有周玮等人提出的基于层次分解策略无人机编队避障的方法 [2]。张跃东等人提出了基于单目视觉的无人机障碍探测算法 [3]。本项目的创新特色在于基于优化的遗传算法进行无人机航迹规划以及基于模糊控制的智能测距以及避障系统。

同时，在无人机航迹规划的应用方面，自动航迹规划一直是人工智能和机器学习领域的研究热点，得到了许多著名研究机构和研究者的关注，出现了一些在这一领域研究实例：

Kiva Systems公司：其Raffaello教授的团队多次公开展示了，单翼到多翼无人机的稳定飞行，以及任务性自动规划飞行。

英国伦敦迈拓公司：其研发制造的基于无人机技术的运输系统，该无人机系统的研发，主要是针对在低收入国家没有适当医疗救援方式的个人，包括对需要帮助的人提供运送、诊断、治疗等全方位的服务。无人机在整个运输的过程中，并不只是简单的单独飞行，而是通过对系统的控制，让无人机在预编程序的空间轨道线路上来飞行，并配有专门的跟踪、监视装备。

2. 研究设计的总体思路

2.1. 项目研究基本内容

本项目设计融合四轴飞行器、遗传算法和Matlab等技术，研发具有能自主规划航迹，实现从无人机遥控飞行技术向智能化地自主规划航迹飞行技术的转变，并且重点解决无人机在威胁代价(地形、环境等)下实现无人工控制、自主地规划航迹并完成任务的复杂智能技术问题从而提高任务的完成率以及大大缩短任务的完成时间 [4]。该系统的组成角色包括地面站、四轴飞行器和通讯平台等，各种角色所具备的功能如下：

1) 地面站：使用者通过地面站可以浏览和查询所连接的四轴飞行器的飞行高度，状态以及位置，接收飞行器的信号，并且还可以向四轴飞行器发送飞行的指令。

2) 四轴飞行器：注册并认证后的四轴飞行器可以在此系统中的地面站进行连接，接受地面站的信号信息，同时具有可以向地面站发送讯号以及相关信息的功能。

3) 通讯平台：连接地面站和四轴飞行器，为地面站与四轴飞行器之间讯号以及相关信息相互传输提供中间平台。

2.2. 项目研究技术路线

1) 在全局规划阶段，基于等效数字地形，建立数字高程地图，数字高程地图是用地形采样点的经纬度和标高组成的数值阵列表示，要用于地形仿真、任务规划和地形的三维显示等。

2) 采用MAVLink通讯协议，来构建电脑地面站与无人机飞控之间的通讯协议，提供无人机飞行的位置信息和周围的环境信息，解决威胁飞行的信息，实现一键起飞的算法控制，提高飞行器飞行安全；

3) 本项目在C#的环境下，搭建地面站，满足硬件之上的操作系统部分，包含驱动、操作系统、应用接口，以及系统之上的应用软件部分的主要功能，有摇杆遥控、飞行数据监测、飞行姿态、飞行定位、航拍视频、数据存储、航点编辑、软件设置等。

4) 利用数字地图将三维地形上的路径寻优问题转化成状态空间最优解的搜索问题，并采用遗传算法搜索最优解，进行航迹规划。

5) 对遗传算法进行改进，对最优航迹路线进行平滑处理，实现航迹规划更加平稳，提高飞行器的安全性。

6) 对四轴无人机优化后的航迹进行仿真实验，与未经优化的航迹进行对比，最终实现飞行器稳定安全飞行。

Figure 1. Principle of UAV Route Planning

图1. 无人机航迹规划原理

3. 项目研究主要原理

3.1. 地图的建立和种群编码

飞行器进行路径规划之前首先要建立地图，本文采用栅格法建立移动飞行器的行走空间模型。栅格面积越小，空间中各种环境信息表示越精确，但同时占用的存储空间就会加大，算法所使用的搜索时间就会加大。若栅格面积越大，则空间中各种环境信息不能准确表示出来，容易出现碰撞问题。所以在建立环境模型时，需要先做以下规定：

1) 不考虑障碍物高度问题，飞行器行走空间为二维平面空间；

2) 障碍物的大小、位置已知，并且不存在动态障碍物；

3) 在规划时可以把飞行器看作质点处理。

为了方便设置栅格面积，我们通常用正方形表示飞行器的行走空间。如果障碍物面积小于正方形面积，可以将障碍物扩大为正方形；如果障碍物面积大于正方形面积，可以用两个正方形表示障碍物面积；如果障碍物面积更大 [5]，则可以用多个正方形表示。建模后飞行器行走空间如图1所示，图中白色部分为自由栅格，黑色部分为障碍物栅格。

在构建栅格地图时，首先以地图左下角第一个栅格为坐标原点建立直角坐标系。因此每一个栅格可以用(x,y)的坐标形式表示，比如左下角第一个栅格可以表示为(1,1)。并且从左下角开始每一个栅格进行编号N，编号从0开始。编号和坐标的转换公式为：

$\begin{array}{l}x=\mathrm{int}\left(N/G_{size}\right)+1\\ y=N\%Gsize+1\end{array}$

Gsize为每一行栅格数，int为取整操作。已为每一个栅格编好号码，因此可以用此编号来表示一条路径，比如起点为0终点为24的路径可以表示为(0, 6, 7, 8, 13, 19, 24)，在遗传算法中即为十进制编码。

3.2. 代码整体框架

遗传算法是一种智能优化算法，主要用来解决优化问题，其主要步骤为种群初始化、适应度函数计算、选择、交叉和变异。应用于移动飞行器路径规划时其主要步骤相同，流程图如图2所示。具体每步的方法将在以下小节做详细介绍。

3.3. 初始化种群

飞行器的起始位置为栅格0，目标位置为栅格99。初始化种群要求随机产生多条可行路径，可行路径表示不与障碍物栅格相碰撞的路径。可行路径的产生分为两个主要步骤。第一步首先产生一条间断路径。飞行器每次行走一个栅格，因此每一行至少有一个栅格在可行路径中。所以初始化时先按顺序在每一行随机取出一个无障碍栅格，形成一条间断的路径，其中为了减短路径长度路径的第一个和最后一个栅格分别为飞行器的起始位置和目标位置 [6]。

第二步是将间断的路径连接为连续路径。在这一步中首先从第一个栅格开始判断相邻的两个栅格是否为连续栅格，栅格是否连续的判断方法为：

$D=\max \left\{abs\left(x^{i+1}-x^i\right),abs\left(y^{i+1}-y^i\right)\right\}$

若D等于1则说明两个相邻栅格连续，反之不连续。对于不连续的栅格取两个栅格的中点栅格，中点栅格的坐标计算为：

$\begin{array}{l}{x}_{new}=\mathrm{int}\left(\frac{x_{i+1}+x_i}{2}\right)\\ y_{new}=\mathrm{int}\left(\frac{y_{i+1}+y_i}{2}\right)\end{array}$

若新栅格为障碍物栅格则以上下左右顺序取新栅格的相邻栅格，并判断此栅格是否已经在路径中(防止陷入死循环)，如果此栅格为无障碍栅格且不在路径中则插入路径中，如果遍历上下左右四个栅格后没有满足条件的栅格则删除这条路径；若新栅格为无障碍物栅格，则插入两个不连续栅格中间。继续判断新插入的栅格和新插入的栅格的前一个栅格是否连续，若不连续则循环以上步骤，直到两个栅格连续。当两个栅格连续后取下一个栅格，循环以上步骤，直到整条路径连续。初始化一条路径的流程图如图3所示。

Figure 2. Flow chart of genetic calculation

图2. 遗传算发流程图

Figure 3. Flow chart of initial population

图3. 初始化种群流程图

3.4. 适应度函数计算

适应度函数分成两部分，分别用来判断路径的长短和平滑程度。路径长度的计算相对简单，公式如下：

$d_{总}\text{=}{\displaystyle \underset{i=1}{\overset{end-1}{\sum }}\sqrt{{\left(x_{i+1}-x_i\right)}^2+{\left(y_{i+1}-y_i\right)}^2}}$

要求的是飞行器的路径最短，而选择方式采用的是轮盘赌的方式，所以适应度函数的第一部分为：

$fit=1/d_{总}$

飞行器由于运动学和动力学的约束，行进时拐弯不宜过大，并且相对平滑的路径有利于飞行器的行驶，因此产生的路径有平滑度的要求。路径越平滑，相邻三点形成的角度越大，角度越大相邻三点之间的距离越大，因此计算路径中所有相邻三点的距离作为适应度函数的第二部分，计算公式如下：

$d_3={\displaystyle \underset{i=1}{\overset{end-2}{\sum }}\sqrt{{\left(x_{i+2}-x\right)}^2+{\left(y_{i+2}-y_i\right)}^2}}$

d₃的值可对应路径中连续三点的夹角的四种情况分别为180度、钝角、直角和锐角，其中180度的三点成一直线，平滑度最好，钝角、直角次之。由于飞行器动力学的约束，锐角是不允许的。所以分别对除直线外的3种情况给予惩罚，分别为5、30和无穷大，可以根据实际情况改动惩罚的大小。惩罚之和的倒数即为适应度函数的第二部分。

适应度函数的两部分需要取一个权重，权重公式如下：

$fit=afi{t}_1+bfi{t}_2$

根据路径长度和平滑度之间的权重选择参数a和b。

3.5. 选择方法

选择方法采用简单的基于概率的轮盘赌方法 [7]。首先计算出所有个体的适应度函数的和，再计算每一个个体所占的比率，计算公式如下

$p_i=fit/{\displaystyle \underset{i=1}{\overset{end}{\sum }}fi{t}_i}$

然后根据每个个体的概率，以轮盘赌的方式选择出下一代个体。轮盘赌的方式保证了部分非最优的个体，可以有效的防止算法陷入局部最优解。

3.6. 交叉方式

首先需要确定一个交叉概率pc，产生0~1之间的一个随机数，并和交叉概率pc比较，若小于pc则进行交叉操作。交叉方式采用的是单点交叉的方式，具体的交叉操作是找出两条路径中所有相同的点，然后随机选择其中的一个点，将之后的路径进行交叉操作。具体的流程图如图4所示，其中n为路径数(即种群数量)。

Figure 4. Flow chart of crossover method

图4. 交叉方法流程图

3.7. 变异方式

首先需要确定一个变异概率pm，产生0~1之间的一个随机数，并和变异概率pm比较，若小于pm则进行变异操作。变异方法是随机选取路径中除起点和终点以外的两个栅格，去除这两个栅格之间的路径，然后以这两个栅格为相邻点，使用初始化路径中的第二步将这两个点进行连续操作。此时有可能无法产生连续的路径，则需要重新选择两个栅格执行以上操作，直到完成变异操作 [8]。具体的流程图如图5所示，其中n为路径数(即种群数量)。

Figure 5. Flow chart of crossover method

图5. 交叉方法流程图

4. 仿真结果

在20 × 20的栅格地图上进行实验，起点为0栅格，终点为399栅格，其中pc = 0.8，pm = 0.2，a = 1，b = 0。当b = 0即表示不考虑路径的平滑度，只考虑了路径长度作为适应度函数，栅格地图上的路径如图6(a)所示，最短路径如图6(b)所示。从图6(a)中可以看到算法成功的找到了一条无障碍路径，但路径中有多处直角的转折，不利于飞行器行驶，有时甚至出现锐角的情况，这种情况一般的飞行器是无法行驶的。从图6(b)中得出，经过10次左右迭代算法已经收敛，最优路径的长度为31.799 (相邻栅格间隔为1个单位长度)。当a = 5，b = 2时，即考虑路径平滑度时的算法结果如图6(c)、图6(d)所示，由于路径顺滑函数中对出现直角的情况加了较大的惩罚，出现锐角的情况加一个无穷大的惩罚，因此顺滑了路径且避免了锐角的出现。此时最优路径的长度也为31.799。

Figure 6. Results of genetic algorithm

图6. 遗传算法结果图

5. 创新特色

遗传算法是飞行器路径规划中应用较多的方法之一，在静态工作环境和动态工作环境都已经取得了理想的路径规划效果。本文主要介绍遗传算法在静态环境下的路径规划，主要分为地图的建立和种群编码、种群初始化、适应度函数、选择方法、变异方法和交叉方法。

6. 结论

本文的重点是如何实现基于遗传算法的路径规划算法，首先介绍栅格地图的构建和十进制种群的编码方法。然后给出了整个过程的流程图，在此基础上详细介绍了种群初始化、适应度函数、选择方法、变异方法和交叉方法，并对其中较复杂的种群初始化、变异方法和交叉方法给出了程序流程图。主要的缺点是生成的路径不够平滑，可能是间断路径连续化产生的路径不够平滑，可以在此方面改进，也可以改进适应度函数中的路径平滑的部分，设计更加合理的平滑函数。

致谢

本项目由徐州工程学院(Xuzhou University of Technology)资助，同时由鞠训光(徐州工程学院)老师指导完成。同时，作者要感谢编辑和审稿人的认真负责。