基于蚁群算法的机械臂打孔路径规划

问题描述

  该问题来源于参加某知名外企的校招面试。根据面试官描述，一块木板有数百个小孔（坐标已知），现在需要通过机械臂在木板上钻孔，要求对打孔路径进行规划，力求使打孔总路径最短，这对于提高机械臂打孔的生产效能、降低生产成本具有重要的意义。

数学模型建立

问题分析

  机械臂打孔生产效能主要取决于以下三个方面：

1. 单个孔的钻孔作业时间，这是由生产工艺所决定的，不在优化范围内，本文假定对于同一孔型钻孔的作业时间是相同的。
2. 打孔机在加工作业时，钻头的行进时间。
3. 针对不同孔型加工作业时间，刀具的转换时间。

  在机械臂打孔生产效能的三个重要因素中，单孔作业时间因生产工艺无法优化，刀具切换时间因生产流程无法优化，所以可优化的主要是机械臂行进时间，这直接受到打孔路径规划的影响，并与路径长度正相关，所以设计出合理的较短的打孔路径，对于提高机械臂打孔的生成效能具有重要意义。   打孔的路径规划问题，可以转换为旅行商问题TSP（一个旅行商人要拜访n个城市，他必须选择所要走的路径，路径的限制是每个城市只能拜访一次，而且最后回到原来出发的城市）来分析求解。   在实际应用中，因为机械臂连续作业，那么一块木板打孔完毕后，机械臂是否回到起始点需要对TSP进行改造。

最佳规划路径

  采用0-1变量来确定规划路径上两点的情况，即

  那么刀具行进时间为

  其中，n为所有的打孔数目，(xi,xj)和(yi,yj) 为任意两孔，v为刀具行进的速度,假设两点距离采用欧氏距离公式。

算法选型

  TSP问题是非常典型的NP（Nondeterministic Polynomial）难问题，对于大规模的TSP问题，目前没有完美的解法，所有的智能算法只能在一定程度上近似逼近最优结果。其中常用的算法有遗传算法、模拟退火算法、蚁群算法等。   由文献可以得到，==蚁群算法适用于缓慢地精确的求解场合；模拟退火算法适用于快速较精确地求解；遗传算法适用于快速地求解，但是准确度不高==。所以，本文在保证精确度的要求下，以蚁群算法为基础，探讨打孔路径规划的问题。   蚁群算法（Ant Colony Algorithm，ACA），最初是由意大利学者Dorigo M.博士于1991年首次提出，其本质是一个复杂的智能系统，且具有较强的鲁棒性，优良的分布式计算机制等优点。该算法经过十多年的发展，已被广大的科学研究人员应用于各种问题的研究，如旅行商问题，二次规划问题，生产调度问题等。   针对多孔的全局路径规划问题，改进的蚁群算法可以描述为：

  信息素更新：为了避免残留信息素过多引起残留信息淹没启发信息，在每只蚂蚁走完一步或者完成对所有 n个任务点的遍历后，要对残留信息进行更新处理。

算法设计

  结合实际应用场景，本文主要在蚁群算法的基础上，考虑传统旅行商问题，不回起始点的遍历路径，融入高度信息的三维情形等三种情形考虑。

二维路径计算

  考虑到机械臂的运动状态，如机械臂可能任意角度的斜线，或者只可以走固定角度的路线（比如3D打印机），所以本文定义两种计算两点之间距离的方法。

1. 曼哈顿距离：即两点在南北方向上的距离加上在东西方向上的距离。 H(n) = D * (abs(n.x – goal.x ) + abs(n.y – goal.y ) )
2. 欧几里得距离： H(n) = D * sqrt((n.x-goal.x)^2 + (n.y-goal.y)^2)

  补充知识：曼哈顿距离，欧式距离，明式距离，切比雪夫距离区别

三维路径计算

  为适应应用场景的复杂性，本文简单讨论在凹凸不平的木板上打孔的路径规划问题，木板网格化后每一个网格的高度已知且不同，那么设计可以不碰撞模板的安全路径。   针对多个3D任务孔，首先设计启发函数，利用A*算法得到单孔与单孔之间的无碰撞最短路径作为两点之间的路径，然后应用蚁群算法，得到遍历所有孔的最短无碰撞路径。   三维多任务孔的路径规划可以抽象为网络最短路径问题，从抽象的数学观点来看，网络实质上是一个有权值的有向图，它由节点和连接这些节点的弧及其方向组成。如下图所示，在复杂任务应用场景下，节点是指起始点、目标点和任务点，节点之间的弧是指节点之间的路径，两点之间的路径长度可以作为弧的权值，因为节点与节点之间可以互相抵达，方向是双向的，所以求多任务孔间的最短路径就是在网络图中寻求航行代价和最小的路径。

求遍历所有节点的最短路径

  根据应用场景，假设对多个木板执行一样的打孔操作，那么当对一块模板完成任务后不需要再返回起始点，可以逆着规划航路直接打孔，回到起始点后可以再完成下一木板的打孔操作，提高应用效率。这种应用情形和TSP问题不一样的地方是路径不闭环，最后不需要直接回到起始点。   基本蚁群算法最早是用来求网络中的最短回路的，因此可以通过增加一个连接网络输入节点与输出节点的虚边，在搜索过程中规定必须经过虚边，变遍历所有节点的最短路径问题为最短回路问题。根据蚁群算法的搜索原理，设虚边的权小于或等于网络所有边权的最小值即可符合上述要求。   本文引入出发点和目标点间的虚边，在搜索过程中要求必须经过虚边，变遍历所有节点的最短路径问题为最短回路问题，设虚边的权小于或等于网络所有边权的最小值。

算法实现流程

可行性分析

  为客观地验证多任务孔的路径规划系统的有效性，评价路径规划系统中算法的性能和优缺点，本文针对路径规划系统的环境模型、两孔之间的路径规划和多任务孔间的路径规划算法进行验证。   本文主要使用Python语言对算法进行快速实现，Python语言开发效率优于C++语言，可以快速实现和验证算法的优缺点，但是Python是解释型语言，运行效率慢。C++语言一般是Python运行效率的5~10倍，所以Python语言的运行时间除以5，一般不小于C++语言的实现时间。

传统旅行商问题仿真结果

遍历所有节点的最短路径仿真结果

3维的最短路径仿真结果

  本文提供上述仿真的源代码，因为目前实现的代码是一种比较理想的场景，和实际应用场景仍有比较大的差距，希望提出建议，共同完善！附github上的源代码

下一步优化

  在路径规划问题抽象模型基础上，本文利用蚁群算法求解遍历所有任务孔的最短路径。基本蚁群算法在处理该类问题时会出现收敛速度慢且容易陷入局部最优解的缺陷,下一步可以对信息素和信息素挥发系数进行了改进,采用一种动态自适应调整信息素和挥发因子的蚁群算法,以求在路径规划方面获得更好的效果。   在“改进的智能蚁群算法在TSP问题中的应用”文献中，动态自适应调整信息素和挥发因子的策略可以描述为：传统蚁群算法中，往往会出现信息素分布过度集中在某一条路径，使得大多数蚂蚁仅通过此一条路径，导致早熟的现象;或者是信息素分布过度分散到各个路径中，使得蚂蚁搜索最优路径耗时相对较长而减缓收敛速度。本文采用自适应的信息素调节机制，使得信息素分布相对均匀，从而使算法跳离局部最优解。另外，信息素挥发系数直接关系到蚁群算法的全局搜索能力及其收敛速度，动态调整信息素挥发系数具有很明显优势，不仅可以加快收敛速度，而且能够提高搜索质量。   在三维路径规划中，点与点之间的最短路径实现效率相对较低，可以优化启发式函数，采用C++语言实现，提高运算速度。