文档介绍：基于Floyd算法的机器人最短路径规划
摘要：最短路径规划是一种点对点的路径规划方式，移动机器人最短路径规划研究即是实现始点和终点间最短路径规划问题的研究。首先采用栅格地图的方式对移动机器人工作环境建模，在建模的基础上，以垂线法方式选择移径。
ij
Floyd算法是建立在已知节点的权值及方向的基础上的，因此移动机器人的最短路径规划从算法节点选择、节点的带权有向图入手进行研究。
垂线法是指在指定连线上作垂线，通过垂线与障碍物边沿的交点，确定Floyd算法中的节点的方法，通过垂线法能有效的将环境中最短路径上节点选择出来，垂线法选择节点的流程为：
连接始点v、终点v，找出连线上所经过的障碍物0，图3(a)中障碍物为0、0；
0ti12
在障碍物0、0上，以vv的连线(简称为t)作垂线m,垂线m与t的交点为Q，
120ti
垂线m与障碍物边沿的交点为U；
i
分别选择t两边UQ距离最大的点v，纳入节点集合S中，图3(b)中障碍物0选择
iii1
的节点为v、v，障碍物0选择的节点为v、v。
12234
因此，根据垂线法得到集合S中的节点为：S={v、v、v、v、v、v}。
01234t
V。
91
92
93
94
97
98
99
vo
91
92
93
97
98
99
80
\
82
83
84
85
86
87
88
89
80
K
82
83
84
85
86
87
88
89
70
71
X
73
74
75
76
77
78
70
71
\
73
74
75
叫
77
78
60
61
62
C
65
66
67
68
60
61
62
0,
66
67
68
50
51
52
56
57
58
59
50
51
52
mm
56
57
V4
59
41
42
43
44
46
48
49
41
vi
43
49
聋
32
33
37
38
39
31
32
33
q
37
38
39
20
21
22
23
26
X
28
29
20
21
22
23
26
28
29
10
11
12
13
14
15
16
17
X
19
10
11
12
13
14
15
16
17
19
0
1
2
3
4
5
6
7
8
v<
0
1
2
3
4
5
6
7
8
片
⑷(b)
图3垂线法选择节点图
根据垂线法选择节点后，图3(b)所示，根据节点v、v、v、v、v、v所在位置计算出
01234t
节点间的权值大小。w指v、v间的权值，w=3+2込，权值大小由两点间的移动距离得到，
010101
为方便观察，将v用v代表。如果两点连线间有障碍物，这两点间的权值为8,路径的走向
t5
是沿着连线t的方向，也就是连线t同一边节点间的方向，与确定它的Q点间的方向是相同
i
的，在t不同边的点直接不限制相互间的方向。所以只存在有v到v、v到v的方向，而没
0102
有v到v、v到v的指向，v与v、v，v与v、v间是存在指向的，如果两点连线间有障碍,
1020124234
权值仍为8。因此，根据节点位置、障碍物位置和连线的t方向，得到环境路径的有向带权
图，如图4（a）所示，及有向图的邻接矩阵，如图4（b）所示。
03+2/24+2/2oo
8
OO
8082+/2^
5+J2
8
8808
271
8
OOOOOO0
oo
5+/2
OO5+J^oooo
0
4+；2
8COOOoo
8
0
(b)
图4移动机器人带权路径有向图及其邻接矩阵
移动机器人路径的邻接矩阵，也就是初始状态下v到v没经过其他中间节点的权值矩阵。ij
因此，
一0
g
3+2“2
0
4+2^2
g
g
2+%'2
g
5+『2
g
g
A=
g
g
0
g
2运
g
(1)
0
g
g
g
0
g
5+*'2
g
5+逅
g
g
0
4+*;2