基于改进A星算法的仿生机器鱼全局路径规划

基于改进A*算法的仿生机器鱼全局路径规划摘要：针对传统A*算法搜索速度慢和规划路径不够优化的缺点,引入可扩展节点，对A*算法流程进行改进，以减少时间和空间复杂度，提高搜索效率，并对规划路径进行优化处理，以有效的缩短路径。对存在凹形障碍物的地图，采用后退-尝试的方法解决路径规划的失败问题，使之绕过凹形障碍物取向目标点，达到输出最短路径的目标。关键词：仿生机器鱼；路径规划；A*算法；可扩展节点；凹形障碍物ImprovedA*AlgorithmBasedGlobalPathPlanningforBio-mimeticRoboticFishAbstract:A*algorithmisimprovedtosolveitslowefficiencyandnon-optimalpath.Introducingsuccessor,theimprovedalgorithmissimplifiedinitstimeandspacecomplexity,andtheplannedpathisoptimizedtomakeitshorterthanbefore.Consideringtheconcavityobstacles,theplanningpathmaygetstuckinconcavitytraps.Aback-trymethodisgiventosolvetheproblem,ismaketheplanningpathroundthetrapinsteadofgettingintoit.Keywords：bio-mimeticroboticfish；pathplanning；A*algorithm；developed-nodes;concavityobstacle仿生机器鱼是模仿鱼类的游动机理，并结合机械、电子等方面学科，实现能够进行水下推进的机器人。它具有机动性能好、噪声低、效率高、鲁棒性强等诸2多优点。并在海洋勘探、水下考古、海洋生物观察、管道检测和娱乐等方面具有广泛应用。路径规划是仿生机器鱼应用领域一项重要研究内容，它是指在有障碍物的有限空间内，依据任务的实际需要，搜索出从起点到目标点的一条行走路径，让机器人在行走过程中安全、无碰撞的绕过所有障碍物到达目标点[1]。全局路径规划是基于环境信息全部已知情况下，依据某个或几个优化准则(如路径最短、时间最少等)来寻找全局最优路径。有多种全局路径规划算法，如蚁群算法、遗传算法、波形算法、神经网络算法和A*算法[2]。但是，神经网络算法的不足之处是需要大量的训练数据，导致其收敛速度慢，搜索效率低，不可避免地存在局部极小和动态特性不够理想等；而遗传算法也有计算速度过慢，占用大量存储量空间、运算时间和搜索效率低等缺点。与前两种算法相比，A*算法具有以下两个优点：（1）可采纳性，即一种搜索算法能在有限的时间内终止，并能找到最优解，提高搜索效率；（2）单调性，对A*算法的估值函数加以适当的限制性条件，就可以使它对所扩展的一系列节点估值函数单调递增或非递减，从而减少对OPEN列表和CLOSE列表的检查和调整，从而提高搜索效率。其A*搜索效率相对较高而且易于实现，因此得到广泛的应用。当环境障碍物较多时，特别是在存在凹形障碍物的情况下，由于A*算法需要搜索的节点数增加，大大增加路径规划的时间代价并使路径变的更为曲折[3]。本文在环境信息全部已知的情况下对仿生机器鱼进行全局路径规划，针对传统A*算法搜索速度慢和规划路径不平滑等不足，先提出可扩展节点概念，再对传统的A*算法的流程进行改进，提高搜索效率，优化行走路径，并解决凹形障碍物中路径规划失败的问题。31A*算法的改进1.1A*算法的思想和算法流程A*算法是在广度搜索基础之上引入估值函数，每走一步都用这个估值函数对所有节点进行评估，通过比较从中找出最优节点，并将其命名为父节点，再从这个父节点搜索下一个节点直至到达目标点[4]。估值函数描述如下：f（n）=g(n)+h(n),要求启发函数h(n)≤h*（n）。f（n）是从起点经过当前节点n到达目标点的全程路径代价预测值；g(n)是从起点到达当前节点n的路径代价的实际值；h(n)是从当前节点n到达目标路径代价的估计值；h*（n）是从当前节点n到达目标点的路径代价的实际值。f值最小的节点视为最优节点。传统的A*算法流程[5]如下：step1初始化，生成一个OPEN列表、一个CLOSED列表。step2把起点加入OPEN列表，OPEN列表中仅包含起始节点，记f=h。step3如果OPEN列表为空，则失败退出，否则进行Step4。step4遍历OPEN列表，查找f值最小的点为最佳节点（BESTNODE），将其移入CLOSED列表，并把它作为当前节点。step5判断当前节点是否为目标点。若是，则路径规划结束，并输出路径；否则将当前点的相邻点投入OPEN列表，进行step3。step6保存并输出CLOSED列表中的路径节点。1.2可扩展节点1.2.1定义可扩展节点定义：AB和AC为搜索路径上任意两条相连的线段，如果存在障碍物位于AB和AC小于或等于的夹角的内部，则称顶点C为可扩展节点[6]，如图1所示。4图1、障碍物顶点间的夹角和障碍物的关系A*算法是在隐式上进行搜索，一边搜索一边建立新的节点。因此路径的建立和搜索同时进行，这样只有对路径规划有用的节点才被放入OPEN列表。在每次扩展节点时，所选节点是障碍物的顶点并且是可扩展节点（根据定义），连接该节点和扩展子节点就是无碰撞路径。因此在所有的节点中，只有满足以上条件的节点才被选作扩展节点。这样每次需要计算的节点数大大减少，在A*算法中尤为显著，路径搜索效率按指数级别提高。1.2.2分析时间复杂度在A*算法中，每次从当前节点开始扩展后续节点集的最坏时间复杂度为O（MN），N表示障碍物顶点的个数，M表示障碍物的个数。由于选择扩展节点与原节点的连线就是与所选扩展节点所在的障碍物的顶点相切的切线，从当前节点出发与一个凸多边形最多有两条切线，所选择的扩展节点的个数为O（M），因此计算从当前节点到每个扩展节点之间的线段是否是无碰撞路径的时间复杂度为O（MN）。（同意修改）当引入可扩展节点后只有满足可扩展节点条件的节点才5被移入OPEN列表，再判断当前节点是否在可扩展节点集中，减少了需要判断是否是无碰撞路径的节点的数量。从当前节点开始扩展后续节点的时间复杂度远远优于O（MN）。A*算法的时间复杂度主要取决于扩展节点的个数，当引入可扩展节点后，在所有的节点中满足可扩展节点条件的节点数和以前相比就变的非常少了，进而大大提高A*算法的路径规划效率，如图2所示。图2（a）无可扩展节点限制的搜索图图2（b）有可扩展节点条件限制的搜索图图2可扩展节点图2（a）是在无扩展节点限制条件下的搜索图，图2（b）是在可扩展节点限制条件下的搜索图，从图2中可以明显的看出，当引入可扩展节点后，搜索的节点数少于无可扩展节点限制条件下的节点数。由A*算法的特征可知，在环境越复杂的情况下，这种搜索效率提高的越明显。2改进的A*算法改进后的A*算法步骤如下：step1初始化，生成一个OPEN列表、一个CLOSED列表，标记所有节点的OPEN值6为0，记f=h。step2把起始节点加入OPEN列表，标记该节点的OPEN值为1(表示该点曾加入过OPEN列表)，记为OLD，重复以下过程，直至找到目标节点；若OPEN列表为空，则宣告失败。step3如果OPEN列表为空，记下当前节点为死点，即此点无法通过。若起始点为死点，则路径规划失败退出，否则将此死点的父节点置为当前节点，跳过Step4直接执行Step5。step4遍历OPEN列表，查找f值最小的节点，将其移人CLOSED列表，并把它作为当前点，同时清空OPEN列表。step5判断当前节点是否为目标点。若是，则路径规划结束，并输出路径；否则对于当前节点展开的每一个可扩展节点(DEVEIOPED-NODE)进行如下操作。情况1：它是障碍点或者在CLOSED列表中，将其剔除，根据可扩展节点的条件查找下一个相邻节点。情况2：它的OPEN值为1(说明它曾加入过OPEN列表，其父节点已经设置)，计算该节点的f，g和h值。情况3：它的OPEN值为0，把它加入OPEN列表，同时将其OPEN记为1，并且把当前点设置为它的父节点，计算该节点的f，g和h值。然后进行Step3。step6对每个可扩展节点进行下列过程：1）建立指向最佳节点（BESTNODE）的指针；2）计算g(DEVEIOPED-NODE)=g(BESTNODE)+g(BESTNODE,DEVEIOPED-NODE)；3）如果可扩展节点已经在开启列表中，即为OLD；4）比较新旧路径的时间复杂度，如果g(DEVEIOPED-NODE)g(OLD)，则重新定义OLD的父辈节点为最佳节点，记下较小代价g(OLD)，并修正f(OLD)值；75）若得到OLD节点的时间复杂度较低或相同，立即停止修正；6）若后继的可扩展节点（DEVEIOPED-NODE）不在开启列表（OPEN）中，则看其是否在关闭列表（CLOSED）中；7）若后继节点（DEVEIOPED-NODE）在关闭列表（CLOSED）中，则转向Step3；8）若后继的可扩展节点（DEVEIOPED-NODE）既不在开启列表（OPEN）中，也不在关闭列表（CLOSED）中，则把它放入开启列表（OPEN）中，然后转向Step7；Step7计算f值。Step8回步骤Step2。3改进说明比较改进前和改进后的A*算法流程，本文主要做了以下4点改进。1）为了减少搜索节点的个数，提高A*算法的执行效率，根据可扩展节点的定义来判断当前节点是不是可扩展节点，若是将其考虑进来，否则不予考虑；在对当前节点进行扩展时，可以先排除曾经已经进入过CLOSED列表中的节点[7]，这在路径规划上体现为不允许仿生机器鱼返回重走已经走过的路径，此方法可以有效的避免路径规划中出现迂回现象。在没有排除可扩展节点中已经存在于CLOSED列表中的节点时，在路径规划中常会出现路径在某两个相邻节点之间来回振荡的现象，致使程序进入死循环，通过改进A*算法流程，从根本上避免上述现象的发生，提高路径规划效率。2）对当前节点的相邻节点进行扩展时，先将OPEN列表清空，再把这些节点存入OPEN列表中。这样就使得OPEN列表中的元素有一个上限，这个上限就是当前节点的可扩展节点，而不会由于累加而越来越多。因此在选择OPEN列表中f（n）最小的节点时，将会显著减少循环次数，从而提高算法执行的速度。从OPEN表中投入CLOSED列表中的相邻节点也是物理上的相邻节点，这样就不会跳跃现象，确保路径规划的连续性和平滑性。83）关于改进后的Step5中的情况2，文献[8]已经指出：如果展开后的节点曾进入过OPEN表，用g值做参考来检查这条路径是否是更好的路径，如果g值更小就表示该路径是更好的路径，就把当前节点设置为其父节点，并重新计算它的g和f值，否则g和f的值都不变。实际上不会出现g值更小的情况，因为先搜索到节点的g值都比后搜索到的g值小，因此g和f的值总是保持最小值。在仿生机器鱼路径规划上的体现为，仿生机器鱼每走一步都是最优的路径，此步最优是建立在上步最优的基础之上。4）关于改进后的Step3，是针对在路径规划中存在凹形障碍物的情况。当路径规划中存在凹形障碍物时，搜索路径常常会掉入凹形陷阱中，由于在程序中后搜索的节点不会是先前进入过CLOSED列表中的节点，造成搜索路径陷入陷阱中，导致路径规划失败。在这种特殊情况下，采取的策略是从当前节点后退一步到达其父节点，尝试其他节点，如果还是失败则以该节点为父节点再后退一步接着寻找可能的节点，直至到达目标节点。4仿真实例4.1建立路径规划模型仿真实例是在全局视觉的前提下进行的。仿真是输入一个二维的栅格地图，用栅格的几何中心点代表整个栅格，称为节点[8]。每个节点有两种状态：可通行节点成为自由节点，不可通行节点成为障碍节点。如图3所示，“点”表示自由节点[8]，上三角表示障碍节点(下同)。