基于改进Dijkstra算法的机器人路径规划方法

（河南财经学院）张 巧 荣 崔 明 义
ＱＩＡＯ ＲＯＮＧ ＣＵＩ ＭＩＮＧＹＩ
摘要：本 文 提 出 一 种 利 用 栅 格 法 和 改 进 的 Ｄｉｊｋｓｔｒａ 算 法 进 行 机 器 人 路 径 规 划 的 方 法 。该 方 法 利 用 栅 格 法 对 机 器 人 的 工 作 环 境
进 行 表 示 ， 利 用 改 进 的 Ｄｉｊｋｓｔｒａ 算 法 进 行 最 短 路 径 的 搜 索 。 应 用 该 方 法 在 对 环 境 细 化 到 包 含 １００００ 个 栅 格 节 点 的 情 况 下 ， 在
本 文 提 出 一 种 基 于 栅 格 法 和 Ｄｉｊｋｓｔｒａ 算 法 的 路 径 规 划 方 法。该方法使用栅格法来对机器人的工作空间进行表示， 使用 改进的 Ｄｉｊｋｓｔｒａ 算法 进 行最 短 路 径的 搜 索 。该方 法 即 利用 了 栅 格法简单易于实现的特点， 有很好的解决了栅格粒度和时空开 销之间的矛盾。
术 Ｋｅｙ ｗｏｒ ｄｓ：ｇｒ ｉｄ，Ｄｉｊｋｓｔｒ ａ ａｌｇｏｒ ｉｔｈｍ，ｐａｔｈ ｐｌａｎｎｉｎｇ
创 １ 引言
新
路径规划技术是智能机器人领域中的核心问题之一， 也是
机器人学中研究人工智能问题的一个重要方面， 它的任务就是
在具有障碍物的环境内， 按照一定的评价标准， 寻找一条从起
始状态（ 包括位置和姿态） 到达目标状态（ 位置和姿态） 的与障
张巧荣：讲师 硕士研究生 河 南 省 自 然 科 学 基 金 资 助 项 目 （０４１１０１３８００）
－ ２８６ － ３６０元 ／ 年 邮局订阅号： ８２－９４６
图 １ 机器人工作环境的栅格法表示 《 现场总线技术应用 ２００ 例》
您的论文得到两院院士关注
机器人技术
采用序号对栅格进行标识。如图 ｌ 所示， 按从左到右， 从上
２） 传统的 Ｄｉｊｋｓｔｒａ 算法需要用邻接表或邻接矩阵来表示结 点之间的邻接关系。考虑到栅格化之后的机器人的工作环境， 每 个栅格只可能和它周围的 ８ 个栅格邻接， 它们之间的连接权值 就是它们之间的距离， 如图 ２ 所示， 而且只有自由栅格才可能成 为路径中的结点， 所以在算法的实现中完全可以不用邻接矩阵 和邻接表存储栅格之间的关系， 这样就大大节省了存储空间。
碍 物 无 碰 的 路 径 。路 径 规 划 问 题 根 据 机 器 人 的 工 作 环 境 模 型 可
分为两 种， 一种是基于环境地图的全局路径规划， 作业环境的
全部信息都是预知的； 另一种是基于传感器的局部路径规划，
作业环境的信息是全部未知或部分未知的。
静态环境中机器人全局路径规划一直是路径规划中的一
３ 改进的 Ｄｉｊｋｓｔｒ ａ 算 法
文 献 中 提 出 一 种 双 Байду номын сангаас Ｄｉｊｋｓｔｒａ 算 法 来 进 行 最 短 路 径 的 搜 索， 以减少传统的 Ｄｉｊｓｔｒａ 算法的搜索时间。
３．１ 算法描述 假 设 每 个 栅 格 都 包 含 一 组 信 息 （ｄｓｊ，ｐｓｊ，ｄｔｊ，ｐｔｊ，ｆｓｊ，ｆｔｊ）， 其 中 ｄｓｊ 、ｄｔｊ 分 别 表 示 从 起 点 ｓｏｕｒｃｅ、 终 点 ｔａｒｇｅｔ 到 顶 点 ｊ 的 最 短 路 径 的 长 度（从顶 点 到 其本 身 的 最短 路 径 长度 设 为 零） ；ｐｓｊ、ｐｔｊ 则 分 别 表 示 从 ｓｏｕｒｃｅ、ｔａｒｇｅｔ 到 ｊ 的最 短 路 径中 ｊ 点 的 前一 顶 点；ｆｓｊ， ｆｔｊ 分别 标 记是否已经求出了该顶点至 ｓｏｕｒｃｅ、ｔａｒｇｅｔ 的最短路径。为起点 ｓｏｕｒｃｅ 和终 点 ｔａｒｇｅｔ 设 置 两 个 中 间 链 表， 分 别 存 储 和 ｆｓｊ＝ｔｒｕｅ 和 ｆｔｊ＝ｔｒｕｅ 的栅格结点相邻接 的 ｆｓｊ＝ｆａｌｓｅ 和 ｆｔｊ＝ｆａｌｓｅ 的栅 格 结 点。算 法的基本过程如下： １）初始化。起点设置为： ①ｄｓｓ ＝ｄｔｔ＝０，ｐｓｓ、ｐｔｔ 为空； ②所有其他 顶 点： ｄｓｊ＝ ｄｔｊ ＝ ∞， ｐｓｊ＝ｐｔｊ＝？； ③ｆｓｓ＝ｔｒｕｅ，ｆｔｔ＝ｔｒｕｅ，其 他 所 有 栅 格 结 点 ｆｓｊ＝ｆａｌｓｅ， ｆｔｊ＝ｆａｌｓｅ。④清 空 两 个 链 表 ，然 后 分 别 把 与 ｓｏｕｒｃｅ、ｔａｒｇｅｔ 直接连接的顶点加入两个链表。 ２） 分 别 检 验 从 所 有 ｆｓｋ＝ｔｒｕｅ，ｆｔｋ＝ｔｒｕｅ 的 顶 点 ｋｓ、ｋｔ 到 其 他 ｆｓｉ＝ ｆａｌｓｅ， ｆｔｊ＝ｆａｌｓｅ 的邻接顶点 ｉ 、ｊ 的 距离（这 些 顶点 都 存 储于 相 应 的 链表中） ，并设置： ｄｓｉ ＝ ｍｉｎ［ｄｓｉ，ｄｋｉ＋ｌｋｉ］ ｄｔｊ ＝ ｍｉｎ［ｄｔｊ，ｄｋｊ＋ｌｋｊ］ 式中，ｌｋｊ 表示从顶点 ｋ 到顶点 ｊ 的直接连接距离。 ３） 从 相 应 的 链 表 结 点 中，选 取 ｄｓｊ 中 最 小 的 一 个 顶 点 ｍ ，选 取 ｄｔｊ 中最小的一个顶点 ｎ ，则 ｍ、ｎ 各 为到 起 点 ｓｏｕｒｃｅ、终点 ｔａｒ－ ｇｅｔ 的 最 短路 径 中 的 一 个 顶 点，分 别 找 到 ｍ、ｎ 的 前 一 顶 点 ｍ’、 ｎ’， 设 置：ｐｓｍ ＝ ｍ’， ｐｔｎ ＝ ｎ’， ｆｓｍ＝ｔｒｕｅ， ｆｔｎ＝ｔｒｕｅ，如 果 发 现 某 一 顶 点 ｉ （相 遇点） ｆｓｉ＝ｔｒｕｅ 和 ｆｔｉ＝ｔｒｕｅ 同 时成 立 ，则 执行 ４） ，否 则，将 ｍ 从 链 表 ｓ 中删 除 ， 把 与 ｍ 相 邻 的 所有 ｆｓｊ＝ｆａｌｓｅ 的 结 点 ｊ 加 入 到 链 表 ｓ 中， 将 ｎ 从链表 ｔ 中删除， 把 与 ｎ 相邻 的 所 有 ｆｔｊ＝ｆａｌｓｅ 的 结 点 ｊ 加入到链表 ｔ 中， 转到 ２） 继续。 ４） 在链 表 ｓ （也可 以 用 链表 ｔ） 中 找 出所 有 ｆｔｑ＝ｔｒｕｅ 的 顶 点 ｑ ，并找出其上一顶点 ｐ ，记： ｄｓｑ ＝ ｍｉｎ［所有 的（ｄｓｑ ＋ ｄｐｑ） ］ ５） 比较 ｄｓｑ ＋ ｄｔｑ 及 相 遇 点 到 两 个 端 点 的 最 短 路 径 长 度 和， 即可得出两个端点间的最短路径。 ３．１ 算法分析 １） 传 统 的 Ｄｉｊｋｓｔｒａ 算 法 在 第 ２） 步 中 要 检 测 所 有 标 记 位＝ ｆａｌｓｅ（ 未 找 到到 起 点 的 最 短 路 径 ） 的 结 点 ， 但 是 在 实 际 执 行 过 程 中， 只有和标记位＝ｔｒｕｅ（ 已找 到 到 起点 的 最 短路 径 ） 的 结点 相 邻
技 ｍａｒｙ ｆｒｅｑｕｅｎｃｙ ｏｆ １．７ ＧＨｚ ， ｔｈｅ ｍａｘｉｍｕｍ ｔｉｍｅ ｎｅｅｄｅｄ ｉｓ ｌｅｓｓ ｔｈａｎ ０．３ ｓｅｃｏｎｄｓ． Ｔｈｅ ａｐｐｒｏａｃｈ ｐｒｏｖｅｓ ｔｏ ｂｅ ｆｌｅｘｉｂｌｅ ａｎｄ ｐｒａｃｔｉｃａｂｌｅ，
ａｎｄ ｃａｎ ｇｕａｒａｎｔｅｅ ｔｈｅ ｐａｔｈ ｉｓ ｔｈｅ ｓｈｏｒｔｅｓｔ．
（１）
ｘ＝ｍｏｄ（ｐ，ｋ）
（２）
ｙ＝ｉｎｔ（ｐ，ｋ）
（３）
其中， ｋ 为在横坐标方向上划分的栅格的数量， 图 １ 中 ｋ＝
１０， ｍｏｄ（ｐ，ｋ）表示 ｐ 除以 ｋ 取余数， ｉｎｔ（ｐ，ｋ）表示 ｐ 除以 ｋ 取整。
接的结点的 ｄ 值 才 有可 能 改 变， 所 以 在 改进 的 Ｄｉｊｋｓｔｒａ 算 法中 引入了中间链表用来存放和 标 记 位＝ｔｒｕｅ 相 邻 接 的标 记 位＝ｆａｌｓｅ 的结点。这样在第 ２） 步中只需要检测中间链表中的结点就可以 了， 减少了搜索时间。
主 频 １．７ＧＨＺ 的 计 算 机 上 规 划 路 径 的 时 间 最 长 不 超 过 ０．３ 秒 。 实 践 证 明 该 方 法 具 有 实 时 性 和 路 径 最 优 性 。
关键词：栅格法；Ｄｉｊｋｓｔｒ ａ 算法；路径规划
中 图 分 类 号 ： Ｔ Ｐ ２４２
文献标识码：Ａ
Ａｂｓｔｒ ａｃｔ：Ｔｈｉｓ ｐａｐｅｒ ｐｒｏｐｏｓｅｓ ａ ｒｏｂｏｔ ｐａｔｈ ｐｌａｎｎｉｎｇ ａｐｐｒｏａｃｈ ｂａｓｅｄ ｏｎ ｇｒｉｄ ｍｅｔｈｏｄ ａｎｄ ｉｍｐｒｏｖｅｄ Ｄｉｊｋｓｔｒａ ａｌｇｏｒｉｔｈｍ． Ｔｈｅ ｅｎｖｉｒｏｎ－
把整个空间用固定大小的栅格分成许多小空间， 所有节点可分
为 障 碍 物 节 点 和 自 由 节 点 两 大 类 。每 个 自 由 节 点 连 接 相 邻 的 自
由节点组成一个图， 用搜索算法生成从初始节点到目标节点的
路径。该方法的缺点是表示效率不高， 存在着时空开销与求解 精度之间的矛盾， 但是它具有简单， 易于实现， 并且易于扩到三 维环境的优点， 而且栅格法能保证找到的路径都是最短路径。
２ 利用栅格法进行环境建模
不失一般性， 本文对机器人的工作环境作如下假设： （１） 移动机器人在有限二维空间中运动； （２） 把 障 碍 物 边 界 向 外 扩 展 机 器 人 本 体 在 长 、宽 方 向 上 最 大尺寸的 １／２， 则机器人可看作质点忽略不计； （３） 工作空间中的障碍物可用凸多边形来描述。 设机器人工作空间如图 １ 所示， 其中阴影部分表示障碍物。
为 障 碍 物空 间 、混 合型 空 间 。对于 混 合 型空 间 按 照前 面 的 方法
继续进行划分， 直到一个预先设定好的精度为止。该方法的主
要缺点是计算单元之间的邻接关系时的开销较大， 而且用这种
方 法 找 到 的 路 径 不 能 保 证 是 最 短 路 径 。而 栅 格 法 的 基 本 思 想 是
到下的顺序， 从栅格阵左上角第一个栅格开始， 给每一个栅
格—个序号 ｐ（从 ０ 开始）， 则序号 ｐ 与栅格— ——对应。采用序号
来标识栅格比采用栅格的左上角顶点的直角坐标来标识栅格
简单明了， 节省内存， 而且栅格序号和栅格的左上角顶点的直
角坐标之间存在着如下的关系， 可以
很方便的进行转换：
ｐ＝ｘ＋ｋｙ
个重要问题， 具有广阔的应用范围。用于全局路径规划的典型
方 法 有可 视 图 法、自 由 空 间法 和 单 元分 解 法 等。其 中 可 视图 法