基于蚁群算法的空中机器人三维轨迹规划

李 妍，徐向荣，徐 浩，李 双

(安徽工业大学机械工程学院，安徽马鞍山243032)

基于蚁群算法的空中机器人三维轨迹规划

李 妍，徐向荣，徐 浩，李 双

(安徽工业大学机械工程学院，安徽马鞍山243032)

轨迹规划是机器人任务规划系统的一个重要部分。在MATLAB平台下，建立空中机器人飞行三维环境模型，基于此模型利用蚁群算法对空中机器人飞行轨迹进行规划研究。首先根据限制条件对建立的环境模型进行预处理和栅格化处理，然后将蚁群算法应用于三维已知环境模型中，得到1条由起始点到目标点的最短路径,以此作为轨迹规划的最优路径。空中机器人按照求得的最优路径进行运动，完成飞行任务。在路径寻优过程中蚁群算法表现出优良的有效性和使用性。

空中机器人；蚁群算法；轨迹规划；栅格化

空中机器人轨迹规划是对空中机器人在有障碍物的环境下自主避开障碍物从起始点到达目标点的规划研究。轨迹规划可分为离线轨迹规划和在线轨迹规划，这2种规划方法在军事和民用方面均具有重要应用价值。在初始阶段，空中机器人运用人工规划的方法寻找路径，但人工规划的航迹过于粗糙，达不到轨迹规划任务的要求。对此，国内外学者提出了一系列轨迹规划算法，包括模拟退火算法、遗传算法、蚁群算法、粒子群优化算法等[1-4]，且均在轨迹规划任务中取得了显著成果。蚁群算法是意大利学者Dorigo M通过观察蚂蚁的觅食习性提出的一种仿生优化算法[3]。此后诸多学者对其进行了改进以提高蚁群算法的性能[5-9]。相对于其他算法，蚁群算法具有正反馈、分布式计算等优点，可保证算法运行的快速性，避免全局优化中早熟性收敛。文中利用蚁群算法对三维环境模型进行离线轨迹规划，将蚁群算法路径规划原理与三维环境模型相结合，说明蚁群算法不仅能求解一维、简单问题，而且在求解复杂、多维问题中也有很好应用。

1 三维环境模型的建立

空中机器人在已知的三维环境模型中飞行，完成环境监测、低空侦察、定点干扰等任务。环境模型的建立方式有多种，文中选用山峰形式的环境模型对空中机器人进行轨迹规划[10]，模型如下式

其中：x，y为在水平面上的投影坐标；z为对应于x，y坐标处山峰的高度；i为第i个山峰；x(i)，y(i)为山峰i的轮廓形状；h(i)为控制山峰i高度的参数；xi0，yi0为山峰i最高点所在的位置坐标。

利用式(1)建立的仿真环境模型如图1，平滑曲面为山地表面，曲面以下部分为障碍物，各坐标值为量纲为一的量。空中机器人具有一定的飞行速度，不能紧贴环境模型中的障碍物飞行，要与障碍物有一定的安全距离。为保证空中机器人飞行的安全性，需对三维环境模型进行预处理，扩大障碍物的范围。

1.1 环境模型预处理

将飞行中存在危险的区域作为障碍物区域处理，禁止空中机器人在此区域飞行。基于此种思想对已知环境模型进行扩展。

1)设机器人安全通过障碍物的距离为d1，d1越大，机器人安全通过障碍物的可能性就越大；d1越小，则机器人安全通过障碍物的可能性就越小。d1根据空中机器人的飞行速度合理取值。

2)空中机器人具有一定的体积，设机器人轴心到机器人实体最远点的长度为d2。

3)根据空中机器人的大小，安全距离扩张障碍物的范围d为

利用式(2)对已知环境模型进行扩展处理，得到的环境模型如图2。由图2可以看出，环境模型经过扩展后山峰的高度和体积均有所增加。在此环境模型中空中机器人可在非障碍物区域安全飞行。

1.2 栅格化环境模型

栅格化环境建模是将空中机器人进行轨迹规划的空间分解成一系列尺寸相同的网格(栅格)单元，每个网格为1个像元，像元为栅格化环境模型的基本单位[11]，如图3。利用栅格将环境模型进行均匀划分时，要保证机器人能够在每个栅格中自由活动。图3中有的栅格完全由障碍物填充，有的栅格中无障碍物，有的栅格中部分是障碍物。根据栅格与障碍物区域的交集，可将栅格分为完全可行栅格、完全不可行栅格、不完全可行栅格。

为保证机器人的飞行安全，把不完全可行栅格归为完全不可行栅格(障碍物栅格)，如图4。每个栅格都用数值0或1表示栅格的存在状态。完全可行栅格取值为0，完全不可行栅格取值为1。栅格的像元值发生变化，说明环境模型发生改变。在MATLAB仿真环境中，经过栅格化处理的已知环境模型可用只有0或1的三维矩阵表示。

1.3 栅格的标识

经栅格化的环境模型由多个栅格组成，每个栅格都有1个固定的位置坐标，对栅格位置坐标的标识有2种方法可以采用。

1)直角坐标法。以栅格左下角作为坐标原点，水平向右方向为x轴正方向、水平向上方向为y轴正方，竖直方向为z轴正方向，每个栅格都可用直角坐标(x，y，z)唯一标识。

2)序号法。对竖直方向进行由下到上的标注层次划分，按照水平向右，水平向上的顺序，从栅格阵左下角第一个栅格开始，给每个栅格1个序号(1～N)，序号与栅格一一对应。先对z方向0～1内的栅格进行标识，再对z方向1～2内的栅格进行标识，依次类推，完成对所有栅格的标识。

以上2种标识方法可用式(3)，(4)进行转换：

其中：n为用序号标识法表示的栅格位置坐标；Nx为xoy平面内每行的栅格数；Nxy为xoy平面内所有栅格的总数；mod为n/Nxy的余数；ceil为n/Nxy的整数，并且取整的方向为正无穷大[12]。

2 基于蚁群算法的三维轨迹规划

2.1 最优路径

路径规划得到路径是否最优，根据空中机器人飞行路径的长短进行判断，空中机器人由起始点到目标点飞行的距离越短，则走过的路径越优化。设从起始点开始，空中机器人到达目标点共经过w个位置点，其中第p个位置点坐标Wp=(xp,yp,zp)，这个过程中空中机器人飞行的距离用L表示。

空中机器人由起始点到目标点的路径不是唯一的，假设可行路径的条数为l。利用式(5)计算可得各路径的长度集合Lall，l条路径中最短的1条即为最优路径minL。

2.2 蚁群算法的应用

图4所示，环境模型中有8 000个栅格，用序号法对每个栅格进行标识，每个栅格对应1个(1～8 000)坐标值。设蚂蚁数为M，并且每只蚂蚁必须具备如下条件。

1)蚂蚁的爬行速度恒定，不考虑转弯、上下、左右移动时的速度变化。蚂蚁在栅格化环境模型中运动，每走1步就是从一个完全可行栅格的中心移动到另一个完全可行栅格的中心，其移动时可选择的下一栅格为与当前位置相邻接的完全可行栅格。对于三维环境模型，1个栅格的邻接栅格有26个，将1个栅格与其他各栅格间是否可以相互运动的状态用一个(0，1)邻接矩阵表示，则可形成一个8 000×8 000的二维矩阵Gd。

环境模型为无向图，第i列和第i行都表示栅格i与其他各栅格间的可运动状态。若值为1，则与对应的栅格相连接，并且与对应栅格之间可相互运动；若值为0，则与对应的栅格不连接或与对应的栅格之间相连接，但对应的栅格为完全不可行栅格。

2)每只蚂蚁在爬行过程中，都会在爬过的路径上释放信息素，假设初始状态下信息素均匀分布，表示为τij(0)=C(C为常数)。

3)在完成1次循环前，每只蚂蚁不会重复选择已经走过的栅格为下一栅格[13]。将走过的栅格坐标储存到T中，其中Tm(m=1，2，…，M)用来记录蚂蚁m目前已经走过的位置。

4)每只蚂蚁选择下一个栅格的概率，与其相连路径上信息素的浓度及下一栅格与目标点栅格的距离有关。信息素浓度越高，距离目标点栅格越近的栅格被选中的概率就越大，其他栅格被选择的概率较小。

设蚂蚁m(m=l，2，…，M)在t时刻由栅格i转移到栅格j的概率为

式中：Dm表示蚂蚁m位于栅格i时下一步允许选择的栅格；τij表示栅格i到栅格j的信息素强度；α表示控制信息素的相对重要程度，是常数；β表示控制路径长度的相对重要程度，是常数；ηij表示栅格i到栅格j的能见度，反映由栅格i转移到栅格j的启发程度[14]。

其中djE表示栅格j到目标点栅格E的距离。Pmij(t)越大，栅格j被选的概率就越大。

5)路径上存留信息素的量随着时间的推移在不断挥发、减少。用参数ρ(0≤ρ＜1)表示信息素的持久性，则1-ρ表示信息素的消失程度。u时刻后，所有蚂蚁完成1次循环，各路径上信息素的量就会发生变化，可用式(9)，(10)调整：

其中：Δτij表示本次循环中所有蚂蚁留在路径由位置点i到位置点j上的信息量；表示第m只蚂蚁在本次循环中留在路径由位置点i到位置点j上的信息量。

式中：Q表示第m只蚂蚁完成1次循环后释放在经过路径上的信息素总量；Lm表示第m只蚂蚁在本次循环中经过路径的总长度[15]。

6)M只蚂蚁经过K迭代循环，形成M×K条由起始点到目标点的路径。求解M×K条路径中的最短路径即为最优路径。

其中，Lkm表示第k次循环中第m只蚂蚁由起始点到目标点走过的路径长度。利用式(12)，(13)求解出最短路径，在MATLAB中利用plot3命令将最短路径与已知环境模型相结合绘制直观图。图5为蚁群算法求解过程流程图。

3 规划结果

在MATLAB中利用蚁群算法进行离线轨迹规划，环境模型为20×20×20的栅格模型，各栅格的存在状态可用20×20×20的三维矩阵表示，矩阵中0表示自由栅格，1表示障碍物栅格，下一步可移动位置点用矩阵Gd表示。将蚁群算法路径寻优原理与三维环境模型相结合求出最优路径minL。利用蚁群算法进行轨迹规划初始时可产生多条不同路径，但迭代到一定次数后蚂蚁的觅食路径会趋于收敛。图6为三维环境模型中，用蚁群算法求解路径时K次迭代后得出的最优路径。S表示起始点；E表示终止点。图7为其俯视图，图8为用蚁群算法求解路径时每次迭代最短路径的集合。图9为用蚁群算法求解路径时迭代次数与最短路径、平均路径的关系曲线，路径长度为量纲为一的量。由图9可知，迭代次数较少时，每次迭代后得到的最短路径各不相同，迭代到一定次数后最优路径趋于一个确定值，同时平均路径长度也在不断减小。由此可知，随着迭代次数的增加，蚁群算法求解的最短路径值趋于稳定，表明遗群算法能够很好地解决三维环境模型的轨迹规划问题。

4 结 论

在栅格化三维环境模型中，利用蚁群算法对空中机器人的轨迹进行全局规划设计，能较好地找到一条理想的规划路径，体现出蚁群算法在解决路径优化问题中的使用价值。但是蚁群算法在求解过程中需要多次迭代，花费较长时间。当迭代一定次数后，算法可能在某个或某些局部最优区域内发生停滞，或随着问题规模变大得到一个部分最优解而非全局最优解，因此，蚁群算法还需进一步改进，以找到快捷可行的路径优化方法。

[1]Kirkpatrick S,Jr Gelatt C D,Vecchi M P.Optimization by simulated annealing[J].Science,1983,220(11):650-671.

[2]Holland J H.Genetic algorithm and the optima allocations of trials[J].SIAM Journal of Computing,1973,2:88-105.

[3]Colorni A，Dorigo M，Maniezzo V.Distributed optimization by ant colonies[C]//Proc of the First European Conference on Artificial Life.Paris,France,1991:134-142.

[4]Kennedy J.The particle swarm:Social adaptation of knowledge[C]//Proceedings of the IEEE International Conference on Evolutionary Computation.Indianapolis:IEEE,1997:303-308.

[5]Gutjahr W J.Agraph-based ant system and its convergence[J].Future Generation Computer Systems,2000,16(8):873-888.

[6]Stützle T,Hoos H H.MAX-MIN ant system[J].Future Generation Computer Systems,2000,16(8):889-914.

[7]乔茹，章小兵，赵光兴.基于改进蚁群算法的移动机器人全局路径规划[J].安徽工业大学学报(自然科学版)，2009,26(1):77-80.

[8]陈崚，沈杰，秦玲，等.基于分布均匀度的自适应蚁群算法[J].软件学报，2003,14(8):1379-1387.

[9]高尚，江新姿，汤可宗.蚁群算法与遗传算法的混合算法[C]//第26届中国控制会议论文集.湖南：张家界，2007:701-704.

[10]胡志忠，沈春林.基于数字地图预处理的实时航迹规划[J].南京航空航天大学学报，2002,34(4):382-385.

[11]朱庆保，张玉兰.基于栅格法的机器人路径规划蚁群算法[J].机器人，2005,27(2):132-136.

[12]黄力，张增芳，朱亚超.基于二叉决策的机器人路径规划研究[J].机械设计与制造，2008(3):156-158.

[13]陈英俏.改进蚁群算法及其在移动机器人路径规划中的研究[D].沈阳：东北大学，2010.

[14]盛忠起，李洪峰，段晓艳，等.基于蚁群算法的一类生产链调度问题求解[J].机械设计与制造，2008(7):176-178.

[15]赵开新，魏勇，王东署.改进蚁群算法在移动机器人路径规划中的研究[J].计算机测量与控制，2014,22(11):3725-3727.

责任编辑：何莉

Three-dimensional Trajectory Planning ofAerial Robot Based onAnt ColonyAlgorithm

LI Yan，XU Xiangrong，XU Hao，LI Shuang
(1.School of Mechanical Engineering,Anhui University of Technology,Ma'anshan 243032，China)

Trajectory planning is an important part of the robot mission planning system.With MATLAB platform,establish 3D environment model of the aerial robot.Based on the model,use ant colony algorithm for aerial robot trajectory planning.Firstly,preprocess and rasterize process the established environment model according to the constraints.Then,apply the ant colony algorithm to 3D environment model to get a shortest path from the starting point to the target point as the optimal path.Aerial robot accords the obtained optimal path to complete the mission.Ant colony algorithm shows the validity and practicability during the path optimization.

aerial robots;ant colony algorithm;trajectory planning;rasterisation

TP311

A

10.3969/j.issn.1671-7872.2015.04.012

2015-03-17

科技部中欧(塞尔维亚)政府间科技合作项目((2013)2-5);安徽省教育厅高等学校自然科学研究项目(KJ2013Z022)

李妍(1987-)，女，山东滨州人，硕士生，研究方向为机器人技术及应用。

徐向荣(1962-)，男，安徽芜湖人，教授，主要研究方向为机器人运动轨迹规划、生物力学。

1671-7872(2015)-04-0360-06