基于蚁群算法和Maklink图的机器人路径规划方法与流程

本发明涉及机器人路径规划领域，尤其涉及一种基于蚁群算法和Maklink图的机器人路径规划方法。

背景技术：

机器人尤其是智能移动机器人技术是国内外各国重点发展的热门技术。无人驾驶汽车、绝大部分的服务类机器人、特殊环境下的监控或探测机器人都属于可以移动的智能移动机器人，而路径规划作为移动机器人自主导航技术的重要组成部分，对其进行深入的研究具有非常重要的意义。所谓移动机器人路径规划技术，就是移动机器人根据自身传感器对环境的感知，自行规划出一条安全的运行路线，同时高效完成作业任务^[1]。在这个过程中，机器人同时要完成三个任务，从起点运动到终点、完成避障、在此基础上尽量优化轨迹。

路径规划是移动机器人完成导航的前提，是移动机器人的关键技术之一，从上世纪70年代起，国内外大量学者对其进行了大量的研究工作。在此期间，诞生了大量的移动机器人和路径规划算法，具有代表性的移动机器人有：1966年斯坦福研究院的SHAKEY，1997年美国的火星探测者机器人，20世纪后的日本本田公司的ASIMO、美国谷歌公司ATLAS、BIGDOG等等；具有代表性路径规划算法的有A*算法^[2]、Dijstra算法、人工势场法^[3]等算法，然后随着智能技术的发展，像遗传算法、粒子群算法、蚁群技术、甚至神经网络算法^[4]等也被引入到路径规划中来，传统技术与智能技术的结合为移动机器人路径规划技术打开了一个新的方向。

移动机器人规划技术可以分成四类：模版匹配路径规划技术、人工势场路径规划技术、地图构建路径规划技术、人工智能路径规划技术。目前移动机器人应用最多的是地图构建路径规划技术，也是目前研究的一个热点，地图构建路径规划技术可分为路标法和栅格法，路标法可分为可视线法、切线图法、Voronoi图法、Maklink图法等。栅格法复杂环境下地图建模信息量大，决策效率低，移动机器人传感器资源有限，难以兼顾分辨率与实时性。Maklink图法使用灵活，起点与终点发生改变时，不必改变连通图，并且在复杂环境下具有较低的计算复杂度。

现有的在Maklink地图环境下进行路径规划的最早也是最简单的方法是直接使用Dijstra算法进行路径规划^[5]。对于工作环境中的障碍物，采用多边形进行代替，接着建立起不同多边形顶点之间，以及多边形顶点与边界之间的自由链接线，取自由链接线中点为路径点，连接路径点形成连通图。Dijstra算法选择一系列的路径点形成的连通图路径，使路径的长度最小，以此来达到路径规划的目标。该技术存在如下的缺点：1)不能求取全局最优路径。Dijstra算法求取的是一系列路径点连成的最短路径，但是考虑到机器人可以沿着障碍物边缘走，并不是非得沿着连通图路径，因此，Dijstra算法求取的只是连通图下的最优路径，并不是全局最优路径；2)效率低。Dijstra算法在计算过程中，会遍历大量的路径点，有很多路径点是没有必要遍历的，这会导致Dijstra算法的计算效率低下。

目前另外一种存在于理论阶段的方法是，在Dijstra算法求取“全局次优路径”的基础上，采用其它智能算法进行二次优化，将优化后的路径作为“全局最优路径”，使用的算法如蚁群算法^[6]、粒子群算法^[7]、遗传算法^[8]、萤火虫算法^[9]等。具体操作是，使路径点在自由链接线上移动，数学模型就是N元函数的优化问题(这里的N代表Dijstra算法求取的全局次优路径的路径点数目)。该技术存在如下的缺点：1)不能求取全局最优路径。即使使用智能算法进行二次优化，但是优化后的路径很有很可能不是全局最优路径，因为其它路径点组成的路径也很有可能是全局最优路径；2)存在搜索速度慢、搜索停滞、局部最优情况。现在的大部分智能算法普遍存在着收敛速度慢、收敛到局部最优的情况。

参考文献

[1]朱大奇,颜明重.移动机器人路径规划技术综述[J].控制与决策,2010,25(07):961-967.

[2]赵晓,王铮,黄程侃,赵燕伟.基于改进A*算法的移动机器人路径规划[J].机器人,2018,40(06):903-910.

[3]Chen T B,Zhang Q S.Robot motion planning based on improved artificial potential field[C].Proceedings of 2013 3rd International Conference on Computer Science and Network Technology.Dalian:IEEE,2013:1208-1211.

[4]Hwu T,Wang A Y,Oros N,Krichmar J L.Adaptive Robot Path Planning Using a Spiking Neuron Algorithm With Axonal Delays[J].in IEEE Transactions on Cognitive and Developmental Systems,2018,10(2):126-137.

[5]Habib M K,Asama H.Efficient method to generate collision free paths for an autonomous mobile robot based on new free space structuring approach[C].Proceedings IROS'91:IEEE/RSJ International Workshop on Intelligent Robots and Systems'91.Osaka,Japan:IEEE,1991:563-567.Guo W,Liang R Y,Wang L,et al.Exploring sustained participation in firm-hosted communities in China:the effects of social capital and active degree[J].Behaviour&Information Technology,2017,36(3):223-242.

[6]陈晓,戴冉,陈昌源.基于Maklink图和蚁群算法的航线规划[J].中国航海,2017,40(03):9-13.

[7]李明富,张玉彦,马建华,周友行.基于变参数萤火虫算法和Maklink图的路径规划研究[J].机械科学与技术,2015,34(11):1728-1732.

[8]王飞,王红勇.基于Maklink图和遗传算法的改航路径规划方法研究[J].交通运输系统工程与信息,2014,14(05):154-160.

[9]秦元庆,孙德宝,李宁,马强.基于粒子群算法的移动机器人路径规划[J].机器人,2004(03):222-225.

技术实现要素：

本发明提供了一种基于蚁群算法和Maklink图的机器人路径规划方法，本发明建立自由链接线组成移动机器人可以移动的网络图，实时规划出最短的运动路径，在最短的时间内到达任务点，详见下文描述：

一种基于蚁群算法和Maklink图的机器人路径规划方法，所述方法包括以下步骤：

根据移动机器人工作环境建立相应的Maklink地图，采用多边形代表环境中的障碍物，通过自由链接线组成移动机器人自由移动的网络图；

使用改进Dijstra算法在Maklink地图上求取前K条最优路径；

使用改进的蚁群算法对前K条最优路径进行二次寻优，取最优路径作为全局最优路径。

其中，所述方法采用链接图建立环境模型；

障碍物用顶点表示，第i个障碍物Oi有ni个顶点，障碍物和环境分别表示为；

Oi＝{(x1i,y1i),(x2i,y2i),…,(xni,yni)}

W＝{WSB,O1,O2,…,Om}

其中：WSB表示无障碍物的环境，(xdi,ydix,(d＝1,2,…,n)表示障碍物Oi顶点的坐标。。

进一步地，所述使用改进Dijstra算法在Maklink地图上求取前K条最优路径具体为：

设定起点S，终点T，备选路径数K，然后以起点为根构造正序树，以终点为根构造逆序树，以设定的中间节点向两棵最短路径树回溯，剔除重复路径，即可得到前K条最优路径。

进一步地，所述前K条最优路径具体为：

P＝{p1,p2,…,pk}

pi＝{S,V1,V2,…,Vd,T}

其中：V1,V2,…,Vd为路径依次通过的节点，对应的自由链接线为Li＝(i＝1,2,…,d),S,T分别为起点和终点；

设与分别为Li的两个端点，则链接线上其它点表示为：

其中：hi为比例参数，hi∈[0，1]，i＝1，2，...，d，则一组(h1，h2，...，hd)对应一组路径，(h1，h2，...，hd)为ACO算法的优化后的解。

其中，所述使用改进的蚁群算法对前K条最优路径进行二次寻优具体为：

划分距离为ξ，则每条自由链接线的划分数为：

从链接线Li-1到链接线Li有πi条路径选择；

共有m只蚂蚁从起点出发到达终点，路径为{S,n1j,n2j,…,ndj,T}，ndj表示路径点在第d条自由链接线的第j个等分点上，在t时刻，蚂蚁k在从Li移动到Li+1上节点j的选择方法为：

其中：I为Li+1链接线上所有点的集合，q为[0,1]之间的随机数，q0为[0,1]之间可以调节的参数，τi，k为信息素，ηi，k为启发值，α为信息启发因子，β为期望启发式因子，J为设定的常数。

本发明的有益效果如下：

1、本发明针对移动机器人工作环境建立相应的Maklink地图，采用多边形代表环境中的障碍物，建立自由链接线组成移动机器人可以移动的网络图；

2、本发明使用改进的Dijstra与蚁群算法在Maklink地图上按次序分步寻优，取蚁群算法优化后的最优路径作为全局最优路径；

3、通过采用本方法移动机器人可基本实时规划出最短的运动路径，在最短的时间内到达任务点，且移动机器人的续航里程更长，使用寿命延长。

附图说明

图1为一种基于蚁群算法和Maklink图的机器人路径规划方法的流程图；

图2为机器人二维工作环境建模的示意图；

图3为改进Dijstra算法一次寻优的前四条最短路径的示意图；

图4为蚁群算法二次寻优的全局最短路径的示意图；

图5为改变起始点后采用本方法计算的全局最短路径的示意图；

图6为采用本方法与PSO算法路径规划对比的示意图；

图7为采用本方法与PSO算法收敛对比的示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面对本发明实施方式作进一步地详细描述。

实施例1

本发明实施例提供了一种基于蚁群算法和Maklink图的机器人路径规划方法，参见图1，该方法包括以下步骤：

101：根据移动机器人工作环境建立相应的Maklink地图，采用多边形代表环境中的障碍物，通过自由链接线组成移动机器人可自由移动的网络图；

其中，通过采用多边形代表环境中的障碍物使得多个多边形组成工作环境的障碍区域。

102：使用改进Dijstra算法在Maklink地图上求取前K条最优路径；

具体实现时，第一次优化也可以采用删除算法等前K最短路径算法，本发明实施例对此不做限制。

103：使用改进的蚁群算法对前K条最优路径进行二次寻优，取最优路径作为全局最优路径。

具体实现时，第二次优化也可以采用其他的遗传算法等智能算法，本发明实施例对此不做限制。

通过上述步骤102和步骤103中的两次优化，从而实现在Maklink图下理论上100％的全局寻优，达到全局最优的搜索思路。

实施例2

下面结合具体的计算公式、实例对实施例1中的方案进行进一步地介绍，详见下文描述：

本路径规划仿真实验的运行环境为：Windows7 64bit；MatlabR2014a；处理器Intel(R)Core(TM)i5-3230M；主频2.6GHz；内存4GB。基于蚁群算法和Maklink图的机器人路径规划方法分为以下步骤：

一、移动机器人工作环境建模

目前移动机器人常用的环境建模方法有栅格图法(Grids)、链接图法(MAKLINK graph)、可视图法(Visibility Graph)，栅格图法在复杂环境下存在地图信息量大、决策效率低下的问题，本发明实施例采用链接图法建立环境模型。

假设移动机器人在静态二维空间中移动，障碍物用多边形表示，多边形的高度垂直于XY平面，将障碍物的边界依据机器人的最大尺寸和机器人正常感知所需的最小范围进行扩展，并将移动机器人简化为一个质点，障碍物用顶点表示，假设第i个障碍物Oi有ni个顶点，障碍物可以表示为式(1)，整个环境W可以表示为式(2)。

Oi＝{(x1i,y1i),(x2i,y2i),…,(xni,yni)} (1)

W＝{WSB,O1,O2,…,Om} (2)

其中：Oi表示第i个障碍物，WSB表示无障碍物的环境。然后建立自由链接线，自由链接线的建立参考Maki K HABIB教授在1991的研究，最后取自由链接线的中点为路径点，在每个自由空间内连接路径点，就组成了一个移动机器人可以自由移动的网格，如图2所示，虚线表示自由链接线，实线连通的网格表示移动机器人可自由移动的路径。

二、建立改进的Dijstra算法

改进Dijstra算法采用基于双向搜索的K则最优路径算法，这个算法以Dijstra算法为基础，融合双向搜索策略，全程只需要运行两次Dijstra算法，大大降低了算法的复杂度。具体操作步骤是：设定起点S，终点T，备选路径数K，然后以起点为根构造正序树，以终点为根构造逆序树，以设定的中间节点向两棵最短路径树回溯，剔除重复路径，即可得到前K条最短路径。

三、建立蚁群算法

蚂蚁k(k＝1,2,…,m)觅食过程中，根据环境中各条路径上的信息素的量τi，j(t)和从当前节点i选择下一个节点j的期望程度ηi，j(t)来决定下一步路径搜索的概率

式(3)中：j∈allowedk表示蚂蚁k从当前节点i出发可选择的路径点的集合。ηi，j(t)是局部启发式函数，通常ηi，j(t)＝1/di，j，为蚂蚁在此次循环中选择路径di，j长度的倒数。α为信息启发因子，表示轨迹的相对重要性，反映了蚂蚁在运动过程中所积累信息在蚂蚁运动时起的作用，值越大，该蚂蚁越倾向于选择其它蚂蚁经过的路径，蚂蚁之间协作性越强，β为期望启发式因子，反映了启发信息在蚂蚁选择路径中的受重视程度，值越大，则该状态转移概率越接近于贪心规则。

t+n时刻路径点(i,j)上的信息素量可按如下的规则进行调整：

τi，j(t+n)＝(1-ρ)τi，j(t)+Δτi，j(t) (4)

式(4)中：ρ表示信息素挥发系数，1-ρ表示信息素残留因子，Δτi，j(t)表示本次循环中路径点(i,j)上的信息素增量，初始值为0，表示第k只蚂蚁在本次循环中留在路径点(i,j)上的信息素量，为：

式(6)中：Q表示信息素强度，Lk表示第k只蚂蚁在本次循环中所走路径的总长度。

四、基于蚁群算法和Maklink图的移动机器人路径规划

本发明实施例采用了分步寻优策略，首先采用改进Dijstra算法在Maklink图上一次寻优，计算得到前K条最短路径，然后使用蚁群算法对这前K条最短路径进行二次优化，取最短路径为全局最优路径。下面，结合实例地图(2)，详细分析一下该过程。

1)改进Dijstra算法在Maklink图上一次优化

改进Dijstra算法原理较为简单，图3是K＝3时，由改进Dijstra算法计算出的前4条最短路径。路径1、2、3、4分别对应点划线、点线、实线、虚线，长度为路径1<路径2<路径3<路径4，长度依次为135.74m，136.8900m，137.3600m，140.1982m。

2)ACO算法二次优化

利用改进Dijstra算法计算出的前K条最短路径可以表示为式(7)，第K条最短路径可以表示为式(8)，如下：

P＝{p1,p2,…,pk} (7)

pi＝{S,V1,V2,…,Vd,T} (8)

其中：V1,V2,…,Vd为路径依次通过的节点，对应的自由链接线为Li＝(i＝1,2,…,d),S,T分别为起点和终点。设与分别为Li的两个端点，则链接线上其它点可以表示为：

其中：hi为比例参数，hi∈[0，1]，i＝1，2，...，d，则一组(h1，h2，...，hd)对应一组路径，(h1，h2，...，hd)为ACO算法的优化后的解。

使用ACO算法对前K条最短路径进行优化，首先应该先离散化工作空间，这里需要离散自由链接线，对自由链接线的划分采用固定距离划分法，设划分距离为ξ，则每条自由链接线的划分数为：

从链接线Li-1到链接线Li有πi条路径可以选择。

然后需要选择节点，蚁群算法优化寻找路径参数集合(h1，h2，...，hd)，使得在离散化的空间里得到最短的路径。假设共有m只蚂蚁从起点出发到达终点，路径为{S,n1j,n2j,…,ndj,T}，其中，ndj表示路径点在第d条自由链接线的第j个等分点上，在t时刻，蚂蚁k在从Li移动到Li+1上节点j的选择方法为：

其中：I为Li+1链接线上所有点的集合，q为[0,1]之间的随机数，q0为[0,1]之间可以调节的参数，τi，k为信息素，ηi，k为启发值，α为信息启发因子，β为期望启发式因子，J为设定的常数。

其中，j的计算方法为：首先依次计算当前节点i到下条链接线节点j的选择概率pi，j，然后根据选择概率pi，j采用轮盘赌法找出下一个节点j，pi，j的计算公式为：

其中，j∈allowedk表示蚂蚁k从当前节点i出发可选择的路径点的集合。t+n时刻路径点(i,j)上的信息素量按照以下方式更新。

τi，j(t+n)＝(1-ρ)τi，j(t)+Δτi，j(t) (13)

式(13)(14)(15)中：ρ表示信息素挥发系数，1-ρ表示信息素残留因子，Δτi，j(t)表示本次循环中路径点(i,j)上的信息素增量，初始值为0,表示第k只蚂蚁在本次循环中留在路径点(i,j)上的信息素量,Q表示信息素强度，这里取为1，Lk表示第k只蚂蚁在本次循环中所走路径的总长度。

经ACO算法二次优化后的结果如图3所示，优化后的最优路径长度为121.8998m，对应图3中的路径4。可知，二次优化的效果明显，全局最优路径并不是Dijstra算法计算的“最优路径”，而是对应‘次次次最优路径4’。

图5的起点和终点分别为T1、S1，T2、S2，本发明实施例的算法优化结果。

实施例3

为了进一步验证本发明实施例提出算法的有效性，将本发明实施例与PSO算法进行了比较，PSO算法参数设置如下：迭代次数maxgen＝100，种群规模sizepop＝20，加速度因子c1＝c2＝1.49445，仿真结果如图6所示，收敛速度如图7所示。

其中，由图6可知，蚁群算法和PSO算法都找到了全局最优路径，分别对应虚线和实线，长度分别为121.8998m、124.0313m，因此，本发明实施例搜寻路径的长度要小于PSO算法，本发明实施例的全局寻优能力更强；由图7可知，本发明实施例的收敛速度要快于PSO算法，只需迭代20代左右就能达到稳定，而PSO算法则需要大约30次迭代才能稳定下来。因此，本发明实施例的全局搜索能力强于PSO算法，收敛速度快于PSO算法。

本领域技术人员可以理解附图只是一个优选实施例的示意图，上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。