【《基于A星算法的分布式水下机器人群轨迹规划分析案例》4100字】

基于A星算法的分布式水下机器人群轨迹规划分析案例目录TOC\o"1-3"\h\u14169基于A星算法的分布式水下机器人群轨迹规划分析案例 1233701.1A*算法 1274341.1.1A*算法原理 1109131.1.2A*搜索算法步骤 3144381.2环境建模 477561.3基于A*算法的领航者轨迹粗规划 691971.4基于A*算法的分布式领航者轨迹细规划 7323211.5小结 9对于一个由领航-跟随者模型建立的多机器人编队系统，由于跟随者是参考领航者的轨迹，保持与其相对位置不变进行航行的，所以首先仅考虑领航者的轨迹优化问题。这是一个单机器人的轨迹优化问题，下面对用于求解该问题的A*算法进行介绍。A*算法A*算法原理A*搜寻算法俗称A星算法。A*算法是比较流行的启发式搜索算法之一,在路线寻优领域的研究中有着广泛的应用。它是针对每一个可能位于最短路线上的节点相对终点的距离进行估计,以此构建具有启发信息的代价函数,评估这些节点位于最短路线上的可能。常见的经典路线规划方法有Dijkstra算法、Floyd算法及启发式搜索算法，在启发式搜索算法中以启发信息为引导，不仅可以提高搜索效率，还可以减少问题的复杂性。A*算法在启发式搜索中应用比较广泛，本文以A*算法为基础，设计关于领航者的路径规划算法。A*算法是一种启发式的搜索算法,它是基于深度优先算法(DepthFirstSearch,简称DFS)和广度优先算法(BreadthFirstSearch,简称BFS)的一种融合算法,按照一定原则确定如何选取下一个节点。深度优先搜索算法,图的深度优先搜索算法类似于树的先序遍历。它的思想是,初始状态下待搜索区域内所有节点均未被访问,搜索算法从起点开始进行搜索,比较其周围所有的邻点,选择距离终点最近的点进行存储,然后再以该邻点为基础对比其周围未被访问的所有邻点,仍然选择距离终点最近的邻点存储,如此循环直到到达终点或访问完所有节点仍未到达终点搜索失败为止,若搜索成功则存储的节点就是连接起点到终点的路线。广度优先搜索算法,又称为"宽度优先搜索"或"横向优先搜索",类似于对树的层序遍历。它的原理是,从初始点出发依次访问与它相连接的邻点,访问完毕后再从这些邻点出发访问邻点的邻点,但是要保证先被访问的邻点的邻点要比后被访问的邻点的邻点先访问,直至图中所有已被访问的节点的邻点都被访问到。如果此时图中尚有未被访问的节点,则需要选取一个尚未被访问的节点作为一个新的初始点,继续搜索访问,直到图中所有的节点都被访问一遍为止。可以看出深度优先算法与广度优先搜索算法的区别是,深度优先算法优先选择离目标点最近的节点,而广度优先搜索算法优先选择离初始点最近的点。深度优先算法能以最快的速度找到一条连接初始点到目标点的路线,但是这条路线未必是最短的,而广度优先搜索算法层层搜索,假如最短路线存在的话,它一定能找到,只是它需要遍历所有的节点。A*算法正是结合了这两种算法的优点，基于启发函数构建了代价函数,使得代价函数中既要考虑新节点距离初始点的代价又要考虑与目标点的代价。A*算法的代价函数构成为(2.1)为节点到起始点的实际代价，为节点到目标点最优路线的估计代价,与之和为节点当前总的估计代价。有研究表明,估计代价如果比节点到目标节点的实际代价小或者相等,如果存在的话,则算法一定能找到一条初始节点到目标节点代价最小的路线,而比节点到目标节点的实际代价值越小,则算法搜索扩展的节点就越多,搜索的时间就越长;如果能精确的等于节点到目标节点的实际代价值,算法会迅速找出代价最小的路线,而不扩展别的节点;如果比节点到目标节点的实际代价值大,则算法不能保证所搜索到代价最小的路线,但是搜索时间相比前两种情况都要短。在A*算法用于最短路线搜索中,代价函数的代价值即为距离值。在网格地图中,常用的启发函数距离计算方法有曼哈顿方法、欧几里得方法。以欧式距离为例,其计算如下:(2.2)其中,是节点的坐标, 是目标点的坐标。A*搜索算法步骤A*算法是目前较为流行的路线规划算法之一。作为启发式搜索的一种，同样需要选取估价函数，估价函数的使用可以省略大量的无意义搜索，提高搜索效率。在基于A*的路线规划中，对位置的估价十分重要，采用不同的估价就会有不同的效果，在此每个位置的评估估价可以用如下的估价函数来表示：(2.3)其中表示对到达目标节点的总代价的一种估计，即算出的最优路线，表示从初始节点到任意节点的实际路线代价，是从节点到目标节点的最优路线的估计代价，即按某一策论计算出的估计值，从这个估价函数可以看出，搜索的启发信息体现出来，如果估价值与实际值越接近，说明估价函数取得越好。在A*算法执行的过程中用到OPEN表(用于存放搜索到的但非最小代价节点的集合)和CLOSE表(用于存放已经搜索过的代价最小节点的集合)，假设起点和终点分别用和表示，表示节点到间的任意一点，表示该节点的评估估价。其基本步骤如下所示：表2.1A*算法寻优步骤算法1：A*搜索算法基本步骤1:将开始结点放入到OPEN表中；2：从OPEN表中取出一个代价最小的节点，并从OPEN表中删除该节点；若是目标节点，则转到步骤5；否则，进入下一步骤；3:遍历的邻点，记作，计算其估价函数，并根据的属性分如下三种情况：1）既不在OPEN表中也不再CLOSE表中，将其插入OPEN表；2）在OPEN表中，则将OPEN表中已有的该节点估价函数与新的估价函数进行比较，若，则更新OPEN表中的节点估价值；3）在CLOSE表中，则将CLOSE表中已有的该节点估价函数与新的估价函数进行比较，若，则更新CLOSE表中的节点估价值，并将其从CLOSE表移入到OPEN表中；4:将节点插入到CLOSE表中，转到2；5:若终点S存在OPEN表中，表明已找到最短路线，并将S放入到CLOSE表中，并从V开始顺序搜索CLOSE表，直到S，即为所要求的最短路线。环境建模为了对机器人的路径进行规划，将实际的物理空间抽象成算法能够处理的抽象空间，实现相互间的映射，要建立其所处工作环境的模型，建立一个便于计算机进行路径规划所使用的环境模型，目前常用的建模方法有网格法、单元树法和可视图法。在本次实验过程中，考虑到水下机器人处于三维工作空间，采用网格法进行建模。网格法常常用于自主式机器人工作环境的建模，该方法本质是将机器人需要移动的区域进行等间距划分，划分成大小相等的网格进行使用。网格的大小选取是影响规划算法性能的一个很重要的因素。网格较小的话，由网格地图所表示的环境信息将会非常清晰，更能反映真实的地理环境。但由于需要存储较多的信息，会增大存储开销，同时会增大搜索算法中搜索邻域结点的计算量，规划速度会相应降低，实时性得不到保证；反之，由于信息存储量少，需要搜索的结点减少，规划速度随之增快，但环境信息划分会变得较为模糊，不利于有效路径的规划。除了对算法性能的影响以外，网格大小的选取还要考虑到机器人的尺寸大小。综合考量之下，一般会选则比机器人体积稍大的网格进行划分。划分完成后，由于地图尺寸较大，直接采用A*算法进行路径搜索时，求解时间会变得非常长，这将使得自主水下机器人的系统工作效率和工作场景大幅度缩水，因此我们往往还需要对得到的地图模型进一步处理，如下图所示，对前面建立的网格化地图进行网格压缩，如图2.1所示：图2.1示例每个复杂曲面都是由多个基本的网格曲面构成的，它一共含有个基本网格曲面，其顶点有个。这些顶点构成了网格的交点，用点集的形式表示。对该复杂曲面进行压缩，可以通过每个点可以看作是它周围四边形的平均面积中心点，并定义一个表面映射系数来表示由及其周围四边形建立起的压缩化网格属性。该压缩方法如下述公式所示：(2.4)其中，是点附近满足的所有点的高度和，可以看到，决定了压缩率，本系统用这种方式将所有点压缩为200*200以内的粗略地图，如图2.3所示。图2.2压缩前图2.3压缩后根据图2.2和图2.3所示的压缩前后的网格对比可以看到，该算法虽然将原始环境的尺寸缩小了，但却仍能从宏观上反应真实地理环境的地形特征，仅仅丢失了一些微观的细节特征。在考虑全局的路径优化问题时，这些微观特征往往是无关紧要的，但却使得A*算法求解最优路径的思路变得可行。因此，通过对网格图进行压缩的办法，能够在不丢失关键信息的情况下，实现对超大规模地图的直接最优路径搜索，虽然这种路径放在细规划的场景下可能并不是最优的，但这已经是一条能够在宏观上非常接近全局最优路径的轨迹了，下一节将会介绍如何采用基于分布式的方法，对被压缩的微观部分进行优化。基于A*算法的领航者轨迹粗规划对于上面所建立的网格图的环境模型，对代价函数的计算通常有曼哈顿方法、对角值方法和欧几里得方法，这里选择欧几里得方法，其计算公式如下：(2.5)基于欧几里得代价函数，对上面压缩后图形的领航者轨迹进行搜索的示意图如图2.4所示：图2.4粗轨迹规划示例基于A*算法的分布式领航者轨迹细规划上一小节，这条轨迹是在压缩后的粗略地图模型中进行搜索得到的。在此基础之上，为了取得更好的优化效果，我们可以将地图信息进行还原，利用通过前文的A*计算方法得到的轨迹，对地图进行切分，如图2.5所示。每一个子图中都含有200*200的粒度，并且包含一段领航者的粗规划轨迹，并且将这些子图首尾相接，可以还原领航者的粗轨迹路径。图2.5切分子图示例由于粗轨迹规划过程中，在细节上的信息是失真的，所以我们不关心每一个子图在内部的轨迹具体情况，而是将每一个子图中轨迹的起点和终点作为一个轨迹规划问题，重新用A*算法来进行求解，仍选择欧几里得方法作为代价函数。这样，原本针对整个大尺寸地图的轨迹规划问题就被分解成了若干个子问题的轨迹规划，这些拆分后的局部子问题将被送到CPU的各个核心中分别进行计算。通过A*算法得到结果后，再将这些子图拼接成完整的地图，因为它们只对子图内部进行了规划，其首尾仍是相连的。从而得到基于A*算法的分布式领航者细轨迹优化结果。图2.6是基于粗规划轨迹的基础上，对轨迹进行细规划的算法流程。图2.6细规划算法流程图如图2.7所示是细规划方法在一个（5679*4465）大小的数据测试集的运行效果，选取始点为（349，605）终点