一种融合改进A_算法与改进动态窗口法的AGV路径规划_潘富强.pdf

第 卷 第 期 年 月传 感 技 术 学 报 .项目来源：国家自然科学基金资助项目（）；江西省重点研发计划资助项目（）；南昌市高层次科技创新人才“双百计划”资助项目（）收稿日期：修改日期：，（，）：（），：；：一种融合改进 算法与改进动态窗口法的 路径规划潘富强，曾 成，马国红，刘继忠（南昌大学先进制造学院，江西 南昌）摘 要：为满足 路径规划中路径的全局最优性和实时避障的需要，提出了一种基于改进 算法与改进动态窗口法的融合算法。首先，基于传统 算法，提出了一种基于障碍物类型的关键路径点提取策略，剔除冗余路径点，从而减少路径总转角，缩短全局路径长度；其次，根据提取的关键点计算贝塞尔曲线，生成一条符合机器人运动学的路径；最后，将改进 算法与改进动态窗口法进行融合，将提取的关键路径点作为改进动态窗口法的过程目标点，并向动态窗口法的评价函数中加入全局路径代价函数，从而使融合算法同时具备全局路径最优性和动态避障能力。仿真实验结果表明：与传统 算法相比，改进算法减少了 的路径点，减少了 的路径总转角，缩短了 的全局路径总长度，而且基于改进 算法的融合算法相较于基于传统 算法的融合算法，效率提高了，能够很好地满足 路径规划的需要。最后完成了实际场景下的运行实验验证。关键词：自动导引运输车；路径规划；实时避障；算法；动态窗口法中图分类号：文献标识码：文章编号：（）路径规划作为自动导引运输车（，）研究中的关键技术之一，一直受到国内外学者的关注。路径规划是解决机器人从起始点到目标点移动过程中以最优路径行驶的问题，从机器人问世以来，路径规划逐渐成为机器人领域的一个研究热点。为了在 移动过程中得到一条无碰撞路径，研究学者们多是以某些优化准则对 进行约束进而来指导机器人行驶。近年来，对机器人路径规划的研究主要以、蚁群算法等为代表性的全局路径规划和以第 期潘富强，曾 成等：一种融合改进 算法与改进动态窗口法的 路径规划 人工势场、为代表性的局部路径规划为主，全局路径规划和局部路径规划的区别在于移动机器人对周围环境信息的知悉程度。算法是机器人路径规划领域使用较广泛的算法之一，它的核心思想是引入一种启发式搜索函数，用来指导机器人的路径搜索。等将传统 算法的启发函数稍作改变，加入余弦相似性和方向信息，使搜索方向更接近目标方向，提高了 算法的搜索速度；等提出了一种融合算法策略，将 算法和蚁群算法进行融合。面对复杂的动态环境，局部路径规划可显示其独特的优势，局部路径规划算法通过在线实时规划路径，因而可以显示其良好的局部避障能力，但是在这种情况下规划出来的路径，往往不是全局意义上的最优路径。为了解决上述问题，本文提出了一种融合改进 算法和动态窗口法的路径规划方法。基于传统 算法设计了一种提取关键路径点的策略，减少了 算法中的冗余路径点，从而缩短了全局路径的长度，并结合动态窗口法，将提取的关键路径点作为过程目标点，保障规划出来的全局路径在最优的情况下，路径平滑且能够有效避开新出现的障碍物。改进 算法为了减少 算法冗余的路径点和保证路径的平滑性，提出了一种关键点搜索策略，并结合贝塞尔曲线优化来对 算法进行改进。传统 算法算法是一种图搜索算法，该算法在进行路径规划时，从起始节点开始对相邻节点进行遍历，并计算当前节点到相邻节点的代价值和当前节点到目标节点的代价值之和作为启发式函数的评价指标，比较后选择代价值最低的节点加入搜索空间，将添加到搜索空间的新节点作为当前节点，然后通过启发式函数找到下一个最优节点，直到搜索到目标点。最后根据搜索空间中节点的代价值来搜索一条最优路径。它的启发式函数可以表示为：（）（）（）（）式中：代表路径搜索过程之中的一个节点，（）代表从起始点到 节点的最小代价值，（）代表从 节点到目标节点的估计代价值。（）通常为单步欧氏距离的累加，公式可以表示为：（）（）（）（），（）式中：（，）为当前点的坐标，（，）为上一节点的坐标。（）通常由当前点到目标点的曼哈顿距离或欧氏距离来表示，但欧式距离更符合现实情况下的距离表示，故本文（）用当前点与目标点的欧氏距离来表示，公式为：（）（）（）（）式中：（，）为目标点坐标。基于障碍物类型的关键路径点提取传统 算法规划出的最终路径中包含了很多冗余的路径点，导致了路径变长且路径总转角变大，而且将 算法与动态窗口法融合时会将 算法得出的路径点作为动态窗口法的过程目标点，过多的路径点会增加动态窗口法对过程目标点的判断时间，又因为 算法生成的路径点之间的距离都很近，而过近的路径点会导致速度变化过于频繁，这样会降低总体平均速度，从而降低动态窗口法运行的效率，这对后续机器人进行局部路径规划很不利。因此针对该问题提出了一种基于障碍物类型的关键路径点选取策略来去除冗余的路径点。由于移动机器人在运行时总是在障碍物旁边转弯，因此只需要保留障碍物附近的路径点即可去除大量冗余路径点。障碍物类型大致可分为点类型障碍物、线类型障碍物和角类型障碍物：点类型障碍物：该类型障碍物通常表现为一些小的可移动的物体，如小的工具箱，易拉罐，形状规则的机械零件等；或者为四周都是可通行区域的柱状障碍物，如桌腿，人腿，灯柱等。线类型障碍物：该类型障碍物在现实中通常表现为围墙尽头、出入口、门框等。角类型障碍物：该类型障碍物在现实中通常表现为墙角、机床底座的边角、大型集装箱边角等。上述三种类型的障碍物和关键路径点在栅格地图中的表示如图 所示。图 障碍物类型及关键路径点其中，黑色的方块表示存在障碍物，黑色方块的传 感 技 术 学 报第 卷四个方向所占据的位置表示障碍物附近可能存在的路径点的位置。根据障碍物类型提取关键路径点的具体步骤如下：根据传统 算法得出的路径点坐标集合，来搜索附近存在障碍物的路径点，并获得路径子集合 ，和路径附近的障碍物集合 ，这样可以去除处于栅格地图空旷处的路径点。计算路径子集合 中每个路径点的方位角，计算公式为：（）式中：（，）为路径点的坐标。得到方位角集合，。根据方位角集合 来去除具有相同方位角的路径点，这样可以去除附近存在障碍物，但是对移动机器人行进路径意义不大的路径点。得到最终路径点集合 ，。关键路径点提取效果如图 所示。图 关键路径点提取图 中，圆点表示 算法得出的路径点，矩形点表示基于障碍物类型提取的关键路径点。可以看出本文提出的关键点提取策略能够很好地达到去除冗余路径点的目的。贝塞尔曲线优化贝塞尔曲线（）是由多个控制点来决定的高阶可导的数学曲线。由于关键路径点得出的路径会出现路径转角突变很大的情况，这样会导致移动机器人运行速度的急剧变化，故使用三阶贝塞尔曲线对路径进行优化，使全局路径更加平滑，这样可以使移动机器人的运行速度变化更加平缓，从而提高运行效率。三阶贝塞尔曲线公式为：（）（）（）（），（）式中：，为曲线的四个控制点，为归一化变量。假设存在两个路径点，的坐标为（，），（，），两个路径点的方位角分别为，计算这两点间贝塞尔曲线的四个控制点坐标，为：（）（）（）（）（）将式（）得出的四个控制点坐标代入式（），即可以得出两个路径点之间的三阶贝塞尔曲线，如图 所示。图 贝塞尔曲线因此根据方位角集合 和路径点集合迭代求出每一段贝塞尔曲线，再进行拼接，即可求出一条平滑的符合移动机器人运动规律的全局路径，如图 所示。图 改进 算法路径 改进动态窗口法动态窗口法（，）是一种动态避障方法，其主要过程如下：首先对移动机器人运动过程中的速度空间（，）进行多组采第 期潘富强，曾 成等：一种融合改进 算法与改进动态窗口法的 路径规划 样；然后通过这些速度对机器人在下一时刻的轨迹进行模拟；最后使用评价函数对模拟的轨迹进行评估，从中选出一条最优轨迹，并将最优轨迹所对应的速度作为移动机器人的运动速度。本文基于机器人运动模型和速度采样，通过改进动态窗口法的评价函数实现动态窗口法改进，再通过改进的动态窗口法结合全局路径信息使移动机器人能够在避开动态障碍物的同时遵循最优路径。机器人运动模型移动机器人在运动过程中，它的速度为线速度 和角速度，其中在 世界坐标系下把线速度分解到 和 轴方向，记为 和。在较短时间间隔内，计算机器人在两个相邻时刻的轨迹，那么对应这两点的圆弧轨迹可以看成直线。假设在 时刻，机器人的线速度为 和，角速度为，其在 轴方向的增量即为，轴方向的增量即为。则在 时刻到 时刻下，机器人在世界坐标系中运动的位移和角度关系可由下式表示：（）（）（）（）（）机器人运动模型如图 所示。图 机器人运动模型 速度采样在建好移动机器人的运动模型后，通过机器人的速度采样便能预估它的运动轨迹。在进行速度采样时，通常会获得许多组采样速度构成的速度动态窗口，再根据评价函数的设定阈值来对获得的速度进行评估，以此来获得相对最优的速度，并根据最优速度预测出运动轨迹。在速度空间（，）中有很多组速度，由于机器人和所处环境的制约因素，会将采样速度限制在一定范围内。机器人的速度限制 （，），（）机器人的自身条件制约由于机器人电机的力矩限制，会存在最大加速度。在 时间间隔内，机器人的实际运动速度会限定在一定范围，可以表示为：（，）?，?，?（）式中：、表示机器人当前速度；?、?为最大加速度；?、?为最大减速度。机器人安全距离约束为确保机器人在运行过程中的安全，需要避免与障碍物发生碰撞。当预测轨迹上检测出障碍物时，在最大减速度约束下，能够让机器人在撞上障碍物之前停止。所以在这种情况下需要对机器人速度做进一步约束：（，）（，），（，）?）（）式中：（，）表示当移动机器人速度为（，）时，在该速度下推算出的机器人运行轨迹上离障碍物最近的安全距离。速度采样空间如图 所示。图 速度采样空间图 中，矩形框为移动机器人，曲线为移动机器人在一个采样周期内所能到达的位置和路径。改进评价函数经过对机器人的速度采样后，符合条件的速度轨迹不止一组，故需要经过评价函数对其评估选择出最合适的轨迹。因此需要设计评价函数对其进行评价，用以选出最优路径。传统动态窗口法评价函数的定义如下：（，）（，）（，）（，）（）式中：（，）用来对角度差距进行评价，指导机器人运动，迫使机器人移动到推演轨迹终点时的行传 感 技 术 学 报第 卷驶方向与目标位置的角度保持在合理范围，避免偏离目标点，通常表示为当前机器人方位角与最终目标点方位角的差；（，）表示机器人在以速度（，）运行时，在该速度下预测的轨迹与最近障碍物之间的距离；（，）用来评价机器人在当前轨迹中运行时的速度大小；为平滑系数，、为相应函数的加权系数。由式（）可以看出，传统动态窗口法并没有考虑全局环境信息，这样很难得出全局最优路径，因此本文对该评价函数进行改进，向评价函数中添加全局路径信息。首先，将评价函数中的方位角代价函数（，）更改为当前方位角与当前改进 算法生成的全局路径的切线角度的差。对式（）进行求导，即可得到当前段全局路径的切线斜率函数：?（）（）（）（），（）由式（）可以求出机器人当前位置与全局路径最近之处的全局路径的方位角：（?（）（）再将 与移动机器人速度采样空间中预测轨迹终点方位角 做差，得到改进方位角代价函数：（，）（）新的方位角代价函数可以使机器人运行时的前进方向更好地贴近全局路径切线方向。其次，为了使机器人能够更迅速地到达过程子目标点，再向评价函数中添加全局路径的距离代价函数：（，）（）（）（）式中：（，）为预测轨迹终点的坐标，（，）为当前子目标点的坐标。改进后的动态窗口法评价函数为：（，）（，）（，）（，）（，）（）融合算法动态窗口法具备良好的局部避障能力，但是容易陷入局部最优且最终生成的路径很难满足全局最优的要求。因此本文中，实现将 算法与动态窗口法相结合，使得融合算法既能得到全局最优解，又具备局部避障的能力。具体的实现步骤如下：移动机器人获得周围场景信息并将场景信息转化为栅格地图 ，；，。设定起始点（，）和 目 标 点（，），将 中 和 位置的状态设置为，若该位置之前的状态为，则返回路径规划失败。运用传统 算法进行路径规划，得到路径点集合 ，和搜索区域内的障碍物集合 ，并将 中的点在 中的状态更新为。根据基于障碍物类型的关键路径点提取策略得到关键路径点集合 ，并将这些关键路径点作为 算法的子目标点。由 中的路径点根据式（）和式（）求取每一段贝塞尔曲线的轨迹点 ，再将每一段 进行拼接，得到平滑路径 ，。运行 算法进行速度采样，并在速度采样空间中搜索预测轨迹，根据 来计算式（）中的四个代价。（，）（，）()（，）（，）（）（）（）将式（）代入式（），得到每段 的（，），取（，）值最小的 作为最佳运行轨迹 ，。移动机器人运行至 终点，并判断是否到达子目标点，若到达，则更新 为，并 判 断 是 否 为 状 态，若 为 状态，则迭代更新 为，依此类推，直至更新到终点；若未到达或不为 状态，则运行步骤。融合改进 算法和 算法的路径规划算法流程图如图 所示。第 期潘富强，曾 成等：一种融合改进 算法与改进动态窗口法的 路径规划 图 融合改进 算法和动态窗口法的路径规划流程图 运行结果分析为验证算法有效性，进行了仿真运行实验和实际环境下的运行实验。仿真运行实验是在 编程环境下运用 编程进行。实际场景下的运行实验使用的移动机器人平台是 ，搭载了 激光雷达和 深度相机，使用 主频的 机控制移动机器人运动，实验用软件系统为 系统下 版本的 平台，在同一无线局域网环境中，机使用 远程控制工具输入 的 与之进行网络通信。移动机器人平台如图 所示。图 移动机器人 融合算法仿真实验为了验证改进 算法与改进动态窗口法的融合算法对不同类型障碍物场景的适应性，分别将本文改进 算法生成的路径与传统 算法生成的路径进行比较，再比较传统 算法与改进 算法对融合算法的效率提升，最后将本文融合算法与其他算法进行比较。传统 算法与改进 算法的路径比较首先分别设置点类型障碍物场景、线类型障碍物场景、角类型障碍物场景、多类型障碍物场景以及出现动态类型障碍物的场景，然后在这五个场景中进行基于改进 算法和改进动态窗口法的融合算法仿真实验，并与传统的 算法和改进 算法进行对比，具体运行结果如图 所示。图 中，左下角路径端点为起始点，右上角路径端点为目标点，场景一为点类型障碍物场景，场景二为线类型障碍物场景，场景三为角类型障碍物场景，场景四为多类型障碍物场景，场景五为出现动态类型障碍物的场景，其中由三角形标注的矩形为出现的动态障碍物。由图 中的五个实验可知融合算法能够很好地适应具有不同类型障碍物的场景，生成的路径曲线非常平滑且符合移动机器人运动规律。从图（）中可以看出融合算法能够在保证全局路径最优的情况下很好地对突然出现的障碍物进行规避，因此能够很好地满足路径规划中路径的全局最优性和实时避障的需要。由于 算法并不能预知突然出现的障碍物，故场景五的实验中 算法路径规划失败，场景一到场景四中传统 算法与改进 算法的数据如表 所示，可以得出改进 算法相较于传统 算法，路径点减少了 以上，累计总转角减少了以上，路径总长度减少了以上，因此改进 算法生成的路径不仅路径点大幅减少，累计转角和路径长度也减少了，路径点和路径长度的减少可以提高后续动态窗口法的运行效率，转角的减少可以使后续动态窗口法运行时的速度变化更加平缓。传统 算法与改进 算法对融合算法的效率提升为了比较传统 算法和改进 算法对融合算法的效率提升效果，分别将传统 算法与改进 算法和改进动态窗口法进行融合，并在五个场景中运行传统 融合算法和改进 融合算法，得出的数据如表 所示，角速度和线速度曲线如图 所示。由表 可知改进 融合算法相较于传统 融合算法减少了 以上的运行时间。由图 可传 感 技 术 学 报第 卷知改进 融合算法的角速度和线速度曲线相较于传统 算法的角速度和线速度曲线变化幅度更小，且变化更加平缓，因此基于改进 算法与改进动态窗口法的融合算法更加有利于移动机器人控制。由于传统 算法生成的路径包含很多冗余的路径点，且相邻路径点之间的距离太近，导致传统 算法与改进动态窗口法融合后的角速度曲线变化过于频繁，从而使线速度变化过于频繁，最终导致平均线速度明显小于改进 融合算法的平均线速度，这也是改进融合算法效率更高的原因。图 不同类型障碍物场景融合算法路径规划表 传统 算法与改进 算法数据地图场景路径点算法改进 算法累计转角（）算法改进 算法路径长度 算法改进 算法场景一场景二场景三场景四表 传统 融合算法与改进 融合算法对比运行时间 场景一场景二场景三场景四场景五传统 算法本文融合算法效率提升 本文融合算法与其他算法进行比较为了比较本文融合算法与其他算法之间的性能，分别将本文融合算法与 算法、改进蚁群算法、人工势场法（，）以及 算法在图 五个场景中运行，运行结果如图 所示，并运用模型预测控制（，）的轨迹跟踪方法对其他各个算法得出的路径进行模拟运行，最终得出的数据如表 所示。由表 可知本文提出的融合算法在五个场景中都能够进行有效的路径规划，其他三种算法均不能在场第 期潘富强，曾 成等：一种融合改进 算法与改进动态窗口法的 路径规划 景五的环境下进行有效的路径规划，而 算法在场景三和场景四中也无法得出有效路径。相比于其他三种算法，本文融合算法生成的路径总转角更小，路径长度仅在场景一中大于 算法，故相较于其他算法，本文算法具有更好的适应性和更高的效率。图 角速度与线速度曲线图 本文融合算法与其他算法在不同场景下运行结果传 感 技 术 学 报第 卷表 本文融合算法与其他算法运行数据比较算法指标场景一场景二场景三场景四场景五本文融合算法路径长度 路径总转角（）总运行时间 文献算法路径长度 路径总转角（）总运行时间 算法路径长度 路径总转角（）总运行时间 算法路径长度 路径总转角（）总运行时间 图 规避过程 实际场景下的融合算法验证为进一步验证算法的有效性，在人为设置的实际场景中，进行了运行实验。首先通过激光雷达和深度相机运用 算法构建实验场景的二维栅格地图，如图（）和图（）的右侧子图所示，构建的二维栅格地图以图片的形式进行保存，地图中白色部分为已探索且未被占据的区域，黑色部分为已探索到的障碍物，灰色部分为未知区域。然后在构建好的栅格地图中对基于改进 算法和改进动态窗口法的融合算法进行验证，实验场景和验证结果如图 所示。图中验证实验分为两部分，图（）在无新增障碍物的环境下进行实验，由实验结果可知融合算法能够在多类型障碍物环境下生成一条平滑且有效的路径，图（）在出现新障碍物的环境下进行实验，新增障碍已在图中用箭头标出，由实验结果可知融合算法能够在保证路径最优的情况下对新出现的障碍物进行有效的规避，规避过程如图 所示。图 实验环境和验证结果第 期潘富强，曾 成等：一种融合改进 算法与改进动态窗口法的 路径规划 总结针对 路径规划需要在路径全局最优的情况下进行实时避障问题，提出了一种基于改进 算法和改进动态窗口法的融合算法。提出了一种基于障碍物类型的关键路径点提取策略的改进 算法。该算法有效减少了全局路径中的冗余路径点，缩短了全局路径长度，减少了路径总转角，并根据关键路径点计算贝塞尔曲线，从而得到一条平滑的路径曲线，使路径更加符合机器人运动规律。提出了一种改进 算法与改进动态窗口法融合的路径规划方法。通过向动态窗口法的评价函数中加入全局路径信息得到改进动态窗口法，使动态窗口法能够在顾及全局路径信息的情况下对障碍物进行规避，从而赋予融合算法实时避障能力，保障机器人能够在全局路径最优的情况下避开未知的障碍物。完成了所提出算法的仿真实验和实际场景下运行实验。仿真实验结果表明：与传统 算法相比，改进 算法减少了 的路径点，减少了的路径总转角，缩短了 的全局路径总长度，而且基于改进 算法与改进动态窗口法的融合算法相较于基于传统 算法的融合算法，效率提高了，相较于其他算法具有更好的适应性和更高的效率。实际场景下的运行实验，验证了所提出算法的可行性和有效性。参考文献：，（）：，：陈荣军，谢永平，于永兴，等 基于改进 算法的无人配送车避障最优路径规划 广东技术师范大学学报，（）：马全坤，张彦斐，宫金良 基于记忆模拟退火和 算法的农业机器人遍历路径规划 华南农业大学学报，（）：刘军，冯硕，任建华 移动机器人路径动态规划有向 算法 浙江大学学报（工学版），（）：，（），：，：，（）：张丹红，陈文文，张华军，等 算法与蚁群算法相结合的无人艇巡逻路径规划 华中科技大学学报（自然科学版），（）：，（）：，（），：，（），：，（）：潘富强（），男，江西南昌人，南昌大学在读硕士生，主要研究方向为智能机器人与路径规划，；刘继忠（），男，山东成武人，博士，教授，博士生导师，通讯作者，主要研究方向为智能机电系统与机器人，机器视觉与图像处理，。