图 5.3 机器人运动方向
45
表 5-1 各个方向的偏移量
q
move[q].a
move[q].b
N
-1
0
NE
-1
1
E
0
1
SE
1
1
S
1
0
SW
1
-1
W
0
-1
NW
-1
-1
在程序中还定义了与移动机器人偏移量和方向相关的两个数据结构： struct offset { //位置在直角坐标系下的偏移 int a，b； //a 是 x 方向的偏移量 }； //b 是 y 方向的偏移量 enum directions{ N，NE，E，SE，S，SW，W，NW }；//全部 8 个方向 offsets move[8]； //各个方向的偏移表
5.2 声纳工作原理
声纳的出现最早可以追溯到 1490 年。那年意大利的达芬奇发现能用声管听到水 中的声音，这可以说是声纳的原型。1916 年，法国朗之万发明了世界上第一部声纳， 能够借助回声对目标进行定位。到了今天，声纳已经集自动化、计算机、海洋工程 等高科技于一身，能够分析和处理复杂信号，测向精度和可靠性都非常的高。
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 所示：
4．如果该点从四个方向上都不能移动，则把该点从 Close 队列中删除； 5．回到第一点，直到找到 G 点则结束； 从目标点回溯树，直到树根则可以找到最佳路径，并保存在 path[]中； } 注：S 点为移动机器人的初始点，G 点为移动机器人的目标点。
5.1.2 基于 A* 算法的路径规划实现
在移动机器人路径规划的实现中需要保存移动机器人每一步所走的位置信息和 方向信息。这里将机器人每一点的信息用一个三元组来表示，这些三元组如图 5.1 所示：
Findpath() { 把 S 点加入 Open 队列（按该点到 G 点的距离排序和走过的步数从小到大排序） 1．排序队列 Open 队列中距离最小的第一个点出列，并保存入 Close 队列中； 2．从出列的点出发，分别向 8 个方向中的一个各走出一步；
43
3．并估算第 2 步所走到的位置到目标点的距离，并把该位置按估价距离从小到 大排序后并放入 Open 队列中；
在地图已知寻找最优路径的算法有很多，这里介绍的是在程序中使用到的 A*算 法[49][50]。
A*算法是启发试搜索加动态规划。具体实现依靠两个队列 Open 队列和 Close 队 列。从一点开始试探走几个相邻的格子如果可以移动且当前移动为起点到哪个格子 的历史最佳方法则把那个格子按照估价值从小到大插入 Open 队列里面，几个方向试 探结束后取出估价值最小的节点放入 Close 队列再从这里开始试探几个相邻的方向 同样放入 Open 队列里面，放入 Open 队列的条件是：（1）这步在地图上面是可以移 动的；（2）这步所在节点在 Open 队列里面并不存在；（3）从起点到这步的实际距离 比这点的历史最小距离还短。如果满足这三个条件就把节点放入 Open 队列。具体的 算法描述如下：
当移动机器人朝每个方向的移动权值确定后，将路径搜索算法 A*引入的程序中， 在移动机器人朝目标移动前就寻找出一条最优的路径。移动机器人将沿着这条规划 好的路径从起始点移动到目标点。
另外，栅格的大小的选取也是比较重要的。因为 Pioneer II 移动机器人的外形尺 寸是 38cm×48cm，所以栅格的大小要至少选取比这个尺寸大。
NW（西北） [i-1][j-1]
N（北） [i-1][j]
NE（东北） [i-1][j+1]
W（西）[i][j-1] W（西）[i][j-1]
[i][j]
[i][j+1] E（东） [i][j+1] E（东）
[i+1][j-1] SW（西南）
[i+1][j] S（南）
[i+1][j+1] SE（东南）
5 移动机器人的避障与路径规划
5.1 地图已知的路径规划算法
地图已知的情况下进行移动机器人的路径规划相对来说比较容易些。在实验中 采用的是移动机器人走迷宫的办法，让移动机器人在一个地图已知的迷宫里由入口 寻找一条最短路径到迷宫的出口。移动机器人在行走前就将当前的已知地图进行分 析，在行走的链表里填充每一步行走的距离和角度信息，当移动机器人路径规划好 后就让移动机器人按照移动链表里面的信息一步一步的运动，从迷宫的起始点到迷 宫的目标点。 5.1.1 A* 算法原理
44
移动机器入口
11111111111 00100011001 11000110111 10110000111 11011011001 11100101101 10110010111 10001000000 11111111111
移动机器人出口
图 5.2 用二维数组表示的迷宫
设位置[i][j]标记为 X，如果 X 周围有 8 个方向的位置都是 0 值，则移动机器人 可以选择这 8 个位置中的任意一个位置作为下一个位置。但是，并不是他周围所有 的位置都是 0 值。值为 0 的相邻位置可能少于 8 个，这样移动机器人只能在这几个 相邻位置中选择下一个位置。图 5.3 给出了移动机器人下一步可能前进的方向。为 了有效地选择下一个位置，可以将位置[i][j]出发可能的前进方向预先定义在一个表 内，如表 5-1 所示，将该表设定为移动机器人在迷宫中的前进方向表，它给出了移 动机器人主动声纳（回声声纳）和被动声纳（噪声声纳）两种，主动声纳工作时 由发射基阵发射声波“照射”目标，而后由接收基阵接收目标反射的回波，处理成 各种信号。使用主动声纳会暴露自己，被探测目标发现后就会进行规避。被动声纳 的工作过程则相当于的接收过程。被动声纳不发射声波，因而不会暴露自己的位置。 声纳的工作性能不仅取决于声纳设备本身的技术状况，而且受外界条件的严重影响， 特别是环境因素、运载平台的自噪音、目标的反射强度和辐射噪音强度[51]。