移动机器人的避障与路径规划-Read.PDFVIP

5 移动机器人的避障与路径规划 5.1 地图已知的路径规划算法 地图已知的情况下进行移动机器人的路径规划相对来说比较容易些。在实验中 采用的是移动机器人走迷宫的办法，让移动机器人在一个地图已知的迷宫里由入口 寻找一条最短路径到迷宫的出口。移动机器人在行走前就将当前的已知地图进行分 析，在行走的链表里填充每一步行走的距离和角度信息，当移动机器人路径规划好 后就让移动机器人按照移动链表里面的信息一步一步的运动，从迷宫的起始点到迷 宫的目标点。 * 5.1.1 A 算法原理 * 在地图已知寻找最优路径的算法有很多，这里介绍的是在程序中使用到的 A 算 法[49][50] 。 A*算法是启发试有哪些信誉好的足球投注网站加动态规划。具体实现依靠两个队列 Open 队列和 Close 队 列。从一点开始试探走几个相邻的格子如果可以移动且当前移动为起点到哪个格子 的历史最佳方法则把那个格子按照估价值从小到大插入Open 队列里面，几个方向试 探结束后取出估价值最小的节点放入 Close 队列再从这里开始试探几个相邻的方向 同样放入Open 队列里面，放入 Open 队列的条件是：（1）这步在地图上面是可以移 Open 队列里面并不存在；（3）从起点到这步的实际距离 动的；（2）这步所在节点在 比这点的历史最小距离还短。如果满足这三个条件就把节点放入Open 队列。具体的 算法描述如下： Findpath() { 把 S 点加入 Open 队列（按该点到 G 点的距离排序和走过的步数从小到大排序） 1．排序队列 Open 队列中距离最小的第一个点出列，并保存入 Close 队列中； 2．从出列的点出发，分别向8个方向中的一个各走出一步； 43 3．并估算第2步所走到的位置到目标点的距离，并把该位置按估价距离从小到 大排序后并放入 Open 队列中； 4．如果该点从四个方向上都不能移动，则把该点从 Close 队列中删除； 5．回到第一点，直到找到 G 点则结束； 从目标点回溯树，直到树根则可以找到最佳路径，并保存在 path []中； } 注：S 点为移动机器人的初始点，G 点为移动机器人的目标点。 5.1.2 基于A* 算法的路径规划实现 在移动机器人路径规划的实现中需要保存移动机器人每一步所走的位置信息和 方向信息。这里将机器人每一点的信息用一个三元组来表示，这些三元组如图 5.1 所示： x y dir 图5.1 三元组 图 5.1 中，x ，y 表示移动机器人的坐标值，dir 表示移动机器人的方向信息，在 程序的实现中我们用一个数据结构来描述： struct items { int x，y ； int dir ； }； 可以用一个二维数组 maze[m+2][p +2]来表示迷宫，当数组元素 maze[i][j ] = 1 时， 表示该位置是墙壁，不能通行；当 maze[i][j ] = 0 时，表示该位置是通路。 1≤i ≤m,1≤ j ≤ p 。数组的第0 行、第 m ＋1 行，以及第 0 列和第 p ＋1 列是迷宫的围 墙，如图 5.2 所示： 44 1 1 1 1 1 1 1 1 1 1 1 移动机器入口 0 0 1 0 0 0 1 1 0 0 1 1 1 0 0 0 1 1 0 1 1 1