机器人路径规划

基于Floyd算法的机器人最短路径规划摘要：最短路径规划是一种点对点的路径规划方式，移动机器人最短路径规划研究即是实现始点和终点间最短路径规划问题的研究。首先采用栅格地图的方式对移动机器人工作环境建模，在建模的基础上，以垂线法方式选择移动机器人路径中的关键节点，确定关键节点的位置和权值关系，并根据所选节点，基于Floyd算法进行移动机器人的最短路径规划，以及对规划的路径算法进行简化改进，通过实验证明，改进的Floyd算法能实现移动机器人路径的最短和用时的相对减少。关键词：路径规划；Floyd算法；垂线法；最短路径1引言路径规划是移动机器人研究过程中的一个热门话题，如何降低移动机器人路径规划的时间复杂度和空间复杂度，是研究者积极探索的问题。移动机器人最短路径规划问题也就是寻找两点之间最短路径的问题，通常采用的路径规划方法有：平行最短路径搜寻算法[1]、蚁群算法[2]、基于矩阵负载平衡的启发算法[3]、EBSP*算法[4]、Dijkstra算法[5-7]等，其中Dijkstra算法在最短路径规划中应用比较多，但Dijkstra算法的实现形式比较复杂，Floyd算法是一种容易理解、设计方便的解决最短路径规划算法，然而，多数研究者对Floyd算法的研究主要集中在算法的应用问题上,而对Floyd算法中节点的研究和对Floyd算法改进的文献比较少。因此本文基于Floyd算法对移动机器人的最短路径规划问题进行研究，主要对移动机器人路径规划所需节点如何选择、有向图权值大小如何确定、Floyd算法的实现，以及对Floyd算法的优化改进研究作详细的介绍，并通过实验证明了Floyd算法改进的优越性以及移动机器人最短路径选择的正确性。2环境的建模2.1栅格地图的建立首先对移动机器人及工作环境作以下假设：1)工作环境是在一个面积大小为100的正方形区域；2)移动机器人形状大小为11的正方形；3)障碍物形状不作限定，所占面积为1~n，n取值范围[1,100]；将移动机器人的工作环境以栅格地图的形式进行分块，每个栅格形状大小为11，并对每个栅格进行编号，从坐标原点开始，沿X轴编号，编号形式如图1所示，图中灰色部分为障碍物位置。图1栅格地图模型2.2移动机器人在栅格地图中的移动方向假定移动机器人所在位置为点(xs,ys)，移动机器人的移动方向有8个，方向表示如图2所示，移动一个单元格后机器人的位置分别为(xs+1,ys+1)、(xs,ys+1)、(xs–1,ys+1)、(xs–1,ys)、(xs–1,ys–1)、(x,ys–1)、(xs+1,ys–1)、(xs+1,ys)，需要移动的距离分别是(2,1,2,1,2,1,2,1)。图2移动机器人移动方向和移动距离图3移动机器人最短路径规划算法的实现3.1Floyd算法的基本思想Floyd算法的基本思想是：假设求从节点vi到vj的最短路径。如果从vi到vj有弧，则从vi到vj存在一条长度为Aij的路径，该路径不一定是最短路径，尚需进行n次试探。首先考虑路径vi，v1，vj是否存在，如果存在，则比较vi，vj和vi，v1，vj的路径长度，取长度较短者为从vi到vj的中间节点的序号不大于1的最短路径。假如在路径上再增加一个节点v2，也就是说，如果vi，…，v2和v2，…，vj分别是当前找到的中间节点的序号不大于1的最短路径，那么vi，…，v2，…，vj就有可能是从vi到vj的中间节点的序号不大于2的最短路径。将它和已经得到的从vi到vj中间节点序号不大于1的最短路径相比较，从中选出中间节点的序号不大于2的最短路径之后，再增加一个v3，继续进行试探。依次类推，经过n次比较后，求得从vi到vj的最短路径。3.2移动机器人最短路径规划的流程Floyd算法是建立在已知节点的权值及方向的基础上的，因此移动机器人的最短路径规划从算法节点选择、节点的带权有向图入手进行研究。3.2.1节点的选择垂线法是指在指定连线上作垂线，通过垂线与障碍物边沿的交点，确定Floyd算法中的节点的方法，通过垂线法能有效的将环境中最短路径上节点选择出来，垂线法选择节点的流程为：1）连接始点v0、终点vt，找出连线上所经过的障碍物Oi，图3(a)中障碍物为O1、O2；2）在障碍物O1、O2上，以v0vt的连线（简称为t）作垂线m，垂线m与t的交点为Qi，垂线m与障碍物边沿的交点为Ui；3）分别选择t两边UiQi距离最大的点vi，纳入节点集合S中，图3(b)中障碍物O1选择的节点为v1、v2，障碍物O2选择的节点为v3、v4。因此，根据垂线法得到集合S中的节点为：S={v0、v1、v2、v3、v4、vt}。图3垂线法选择节点图3.2.2有向图及邻接矩阵根据垂线法选择节点后，图3(b)所示，根据节点v0、v1、v2、v3、v4、vt所在位置计算出节点间的权值大小。w01指v0、v1间的权值，w01=3+22，权值大小由两点间的移动距离得到，为方便观察，将vt用v5代表。如果两点连线间有障碍物，这两点间的权值为∞，路径的走向是沿着连线t的方向，也就是连线t同一边节点间的方向，与确定它的Qi点间的方向是相同的，在t不同边的点直接不限制相互间的方向。所以只存在有v0到v1、v0到v2的方向，而没有v1到v0、v2到v0的指向，v1与v2、v4，v2与v3、v4间是存在指向的，如果两点连线间有障碍，权值仍为∞。因此，根据节点位置、障碍物位置和连线的t方向，得到环境路径的有向带权图，如图4(a)所示，及有向图的邻接矩阵，如图4(b)所示。图4移动机器人带权路径有向图及其邻接矩阵3.2.3Floyd算法路径规划的实现移动机器人路径的邻接矩阵，也就是初始状态下vi到vj没经过其他中间节点的权值矩阵。因此，0032242202252022052520420A(1)移动机器人寻找最短路径的实现步骤如下：1）写出vi到vj初始无中间节点权值矩阵A0，如式(1)所示，从权值矩阵看出，v0、v1、v2、v3、v4、v5相互间的权值关系和方向关系。2）计算vi到vj间有1个中间节点情况下的最短权值矩阵。设vj经过一个中间点vr到达vj，则vi到vj的最短距离为：1minijirrjrAww，最短权值矩阵为11ijAA。即移动机器人中间经过1个节点的权值矩阵为：1032242253244202252722532022432052527220420A(2)3）计算vi到vj间有k个中间节点情况下的最短权值矩阵。设vj经过中间点vr到达vj，vi经过r个中间节点到达点vr的最短距离为rirA，vr经过k–r个中间节点达到点vj的最短距离为krrjA，则vi经过k个中间点到达vj的最短距离为minkrkrijirrjrAAA，最短权值矩阵为：kkijAA。则移动机器人中间经过2个节点的权值矩阵为：2032242253244285202252722532074222432052527220420A(3)经过分析，得到移动机器人中间经过3个节点、4个节点时的权值矩阵和经过2个节点时的权值矩阵相同。移动机器人从v0到v5，经过v2、v4所需距离最短，最短距离205852A。移动机器人寻找最短路径的Floyd算法非形式化描述为：a)初始化：A0b)Fork:=1tonFori:=1tonForj:=1tonIf11minkkkijirrjrAAAThen11minkkkijirrjrAAAc)算法结束语句的执行频度为：n3。3.3Floyd算法的改进根据Floyd算法推算v0到v5最短路径过程时，间接推算出所有节点间的最短路径，这样算法执行频度相对增加。移动机器人在寻找最短路径时，只需要确定从始点出发，直到终点时，相关节点的最短路径，其余节点间的最短路径不作要求。因此，需要对Floyd算法提出改进，算法的改进方法如下：1）求vi到vj之间有1个中间节点的最短权值矩阵时，中间节点及vi、vj的选择遵循以下规则：①不选择始点v0或终点v5为中间节点。②当vj=v0时，不进行中间节点的选择。③当vi=v5时，不进行中间节点的选择。2）求vi到vj之间有2~k个中间节点的最短权值矩阵时，中间节点及vi、vj的选择遵循以下规则：①不选择始点v0或终点v5为中间节点。也就是2~k个中间节点的任何一个，都不能为v0、v5。②当vj=v0时，不进行中间节点的选择。③当vi=v5时，不进行中间节点的选择。按照规则进行算法改进后，i、j、k取值变化为：(1,5),(0,4),(1,4)ijk。表1是算法改进前后，Floyd算法的性能比较表。表1Floyd算法改进前后的性能比较表n语句执行频度语句执行频度之比改进前改进后改进后/改进前32740.148464180.2815125480.38462161000.46373431800.52585290.57412497294480.6151010006480.6484实验结果Floyd算法改进前后频度随n值变化曲线如图5所示，从图上看出随着n值的增大，语句执行频度增加，改进后明显比改进前语句执行频度低，也就是改进后执行算法所用时间较少，充分证明了Floyd算法改进的优越性和合理性。图5Floyd算法改进前后频度对比图图6所示为改进后与改进前语句执行频度之比与n值的关系图，随着n值增大，比值增大，说明这种改进方式更适合节点稀疏的有向图，对于节点稠密的有向图这种改进方式不及节点稀疏的有向图优越，但从图5中改进前后频度随n值变化曲线看出，节点稠密有向图改进后仍比改进前优越。图6Floyd算法改进前后频度比值图图7中所示线路是移动机器人基于Floyd算法实现的路径规划路线，路径按照垂线法分析出的节点选择的路径，实现了路径最短的选择原则。图7基于Floyd算法的移动机器人路径选择图5结论本文对移动机器人工作环境建模，在建模的基础上，根据垂线法对路径节点进行选择，然后根据Floyd算法选择移动机器人的最短路径，并对Floyd算法进行简化改进，最后通过实验证明改进的Floyd算法实现了移动机器人路径的最短和时间的减少。今后我将对Floyd算法在移动机器人最短路径规划过程中的改进继续作深入研究。最后，衷心感谢倪建军老师在智能机器人课程上的悉心指导与帮助！参考文献[1]黄贵玲,高西全,靳松杰,等.基于蚁群算法的最短路径问题的研究和应用[J].计算机工程与应用,2007,43(13):228-235.[2]刘韵,何建农.基于交通网络最短路径搜索的改进算法[J].计算机工程与应用,2007(14):220-222.[3]胡桔州.Floyd最短路径算法在配送中心选址中的应用[J].湖南农业大学学报：自然科学版,2004,30(4):382-384.