-
移动机器人路径规划的研究
移动机
器人路径规划的目标是寻求一条从初始点到终点的路径，
使得机器人
沿规划出的路径运动时不会与环境中的障碍物碰撞，
其实质就是移动机器人运动
过程中的导航和避碰。
路径规划是自主式
移动机器人导航的基本环节之一，
它是按照某一性能指标
搜索一
条从起始状态到目标状态的最优或近似最优的无碰路径
。
根据机
器人对环
境信息知道的程度不同，
可分为两种类型：
环境信息完全知道的全局路径规划和
环境信息完全未知或部分未知，通过传
感器在线地对机器人的工作环境进行探
测，以获取障碍物的位置、形状和尺寸等信息的局
部路径规划。
1
全局路径规划
全局路径规划又称静态路径规划，是基于环境先验完全信息的
路径规划，
是指根据环境模型找出从起始点到目标点的符合一定性能的可行或最优的路径
，
它涉及的基本问题是世界模型的表达和搜索策略。
全局路径规
划包括环境建模和
路径搜索策略两个子问题。其中环境建模的主要方法有
:
可视图法
(V-Graph)
、栅
格法
(Grids)
和拓扑法等
。
2
局部路径规划
局部路径规划又称动态路径规划，
依赖于传感器的信息，
是未知或部分未知，
即障碍物的尺寸、
行状和位置
等信息必须通过传感器获得。
局部路径规划的主要
方法有人工势
场法
Artificial Potential
Field
。遗传算法（
Genetic-algorithm
）
、模糊
逻辑算法（
< br>Fuzzy Logic Algorithm
）和滚动窗口法等。
3
．环境建模
移动机器人路径规划中，包括环境建模和路径搜索策略两个子问题。
移动机器人环境建模方法：可视图法（
V-Graph
）
，自由空间法（
Free
Spac
eApproach
）和栅格法（
Grids
< br>）等。
可视图法把机器人看作一点，
< br>将机器人、
目标点和多边形障碍物的各顶点进
行组合连接
，
并保证这些直线均不与障碍物相交，
这就形成了一张图，
称为可视
图。
即要求机器人和障碍物各顶点
之间、
目标点和障碍物各顶点以及各障碍物顶
点与顶点之间的连
线均不能穿越障碍物
,
也即直线是可视的。从而搜索最优路径<
/p>
的问题就转化为求经过这些可视直线从起始点到目标点的最短距离问题。
< br>优化算
法可以删除一些不必要的连线以简化可视图
,
从而缩短搜索时间
,
求得最短路径。
可视图法能够求得最短路径，
但这种方法忽略了机器人的尺寸大小，<
/p>
使得机器人
通过障碍物顶点时离障碍物太近，甚至接触，并且搜索
时间长，对于
N
条连线
的搜索时间为<
/p>
TN
；自由空间法应用于机器人路径规划，采用预先定义的如广义
锥形和凸多边形等基本形状构造自由空间，
并将自由空间表示为
连通图，
通过搜
索连通图来进行路径规划。
其优点是比较灵活，
起始点和目标点的改变不会造成
连通图
的重构，
缺点是复杂程度与障碍物的多少成正比，
且有时无法获
得最短路
径。
栅格法将机器人工作环
境分解成一系列具有二值信息的网格单元，
多采用四
叉树或八叉
树表示工作环境
[6]
，并通过优化算法完成路径搜索。该法以
栅格为
单位记录环境信息，
环境被量化成具有一定分辨率的栅格
，
栅格的大小直接影响
着环境信息存储量的大小和规划时间的长
短。
栅格划分大了，
环境信息存储量小，
规划时间短，
但分辨率下降，
在密集环境下发现路径的能力减
弱；
栅格划分小了，
环境分辨率高，
在
密集环境下发现路径的能力强，
但环境信息存储量大，
规划时<
/p>
间长，可以采用改进的栅格法弥补栅格法的不足。
栅格解耦法是目前研究较为广泛的路径规划方法。
该方法将机器人的工作空<
/p>
间解耦为多个简单的区域
,
一般称为栅格
。
栅格大多采用四叉树或八叉树来表示，
然后通过优化算法在栅
格图中搜索一条从起始栅格到目标栅格的路径来完成路
径搜索。栅格解耦法包括确切的和
不确切的两种。
确切的解耦法用来
描述整个自由空间
,
这将使复杂环境的解耦速度变慢
,
其原因是
许多复杂的多边形可能需要与障碍物的
边界相匹配。
这种方法可以保证只要起始
点到目标点之间存在路
径
,
就完全能搜索到这条路径。在不确切的解耦法中
,
所有
的栅格都是预定的形状
,
为了研究方便假设全部为矩形。整个图被分割成多个较
大
的矩形
,
每个矩形之间都是连续的。如果大矩形内部包含障碍物
或者边界
,
则又
被分割成
4
个小矩形
,
对所有稍大的
栅格都进行这种划分
,
然后在划分的最后界限
< br>内形成的小栅格间重复执行程序
,
直到达到解的界限为止
。在进行下一层更细的
划分之前
,
应在
每一层上的起始点和目标点间找到一条路径
,
如果该路径满足起
始
点到目标点间无障碍物的要求
,
则停
止搜索。不确切的解耦方法比确切的解耦方
法在数学计算上要简单的多
< br>,
因此也比较容易实现。
4
路径搜索算法
移动机器人路径规划的方法有很多，
可以说各有优缺点。
同
时，
随着各种新
方法和新技术的不断出现，
不断吸收新的理论，
极大的促进了机器人足球的蓬勃
发展。
足球机器人在静态环境下的路径规划已经比较成熟，
但是在动态
环境下的
全局最优路径规划方法还是不太成熟，
动态环境和未知
环境的路径规划一直以来
都是学术界研究的一个难点和重点。
4.1Dijkstra
算法
由荷兰计算机科学家艾兹格
·
迪科斯彻发现的
。算法解决的是有向图中最短路径问题。
举例来说，如果图中
的顶点表示城市，而边上的权重表示著城市间开车行经的距离。
Dijkstra
算法可以用来找到两个城市之间的最短路径。
Dijkstra
算法的输入包含了一个有权重的有向图<
/p>
G
，
以及
G
中的一个来源顶点
S
。
我们以
V
表示
< br>G
中所有顶点的集合。
每一个
图中的边，
都是两个顶点所形成的有序元素对。
(
u
,
v
)
< br>表示从顶点
u
到
v
有路径相连。
我们以
E<
/p>
所有边的集合，
而边的权重则由权重函数
w
:
E
→
[0,
∞]
定义。
因此，
< br>w
(
u
,
v
)
就是从顶点
u
< br>到顶点
v
的非负花费值
(cos
t)
。
边的花费可以
想像成两个顶点之间的距离。任两点间路径的花费值，就是该路径上所有边的花费值总和。
已知有
V
中有顶点
s
及
t
，
Dijkstra
算法可以找到
s
到
t
的最低花费路径
(i.e
.
最短路径
)
。
这个算法也可以在一个图中，找到从一个顶点
s
到任何其他顶点的最短路径。
算法描述
这个算法是通过为每个顶点
v
保留目前为止所找到的从
s
到
v
的最短路径来工作的。初始
时，源点
s
的路径长度值被赋为
0
（
d[s]
=0
< br>），
同时把所有其他顶点的路径长度设为无穷
大，
即表示我们不知道任何通向这些顶点的路径
（对于
V
中所有顶点
v
除
s
外
d[v]
= ∞
）
。
当算法结束时，
d[v]
中储存的便是从
s
到
v
的最短路径，或者如果路径不存在的话是无穷
大。
Dijstra
算法的基础操作是边的拓展：如果存在一条从
u
到
v
的边，那么从
s
到
u
的
最短路径可以通过将边
(
u
,
v
)
添加到尾部来拓展一条从
s
到
v
的路径。这条路径的长度是
d[
u]+w(u,v)
。
如果这个值比目前已知的
d[v]
的值要小，
我们可以用新值来替代当前
d[v]
中的值。拓展边的操作一直执行到所有的
d[v]
都代表从
s
到
v
最短路径的花费。这个算法
经过组织因而当
d[u]
达到它最终的值的时候没条边
(<
/p>
u
,
v
)
都只被拓展一次。
算法维护两个顶点集
S
和
Q
。
集合
S
保留了我们已知的所有
< br>d[v]
的值已经是最短路径的值
顶点，而集合
Q
则保留其他所有顶点。集合
S
初始状态为空，而后每一步都有一个顶点从
Q
移动到<
/p>
S
。这个被选择的顶点是
Q
中拥有最小的
d[u]
值的顶点。当一个顶点
u
从
Q
中
转移到了
S
中，算法对每条外接边
(u,v)
进行拓展。
伪码
在下面的算法中
,u:=Extract_Min(Q)
在在顶点集
Q
中搜索有最小的
d
[
< br>u
]
值的顶点
u
。这
个顶点被从集合
Q
中删除
并返回给用户。
1
function
Dijkstra(G, w, s)
2
for each
vertex v in V[G]
//
初始化
3 d[v] := infinity
4 previous[v] := undefined
5 d[s] := 0
6 S :=
empty set
7 Q := set of all
vertices
8
while
Q is not an empty set
//
Dijstra
算
法主体
9 u := Extract_Min(Q)
10 S := S union {u}
11
for
each edge (u,v) outgoing from u
12
if
d[v] > d[u] + w(u,v)
//
拓展边
(u,v)
13 d[v] := d[u]
+ w(u,v)
14
previous[v] := u
如果我们只对在
s
和
t
之间寻找一条最短路径的话，我们可以在
第
9
行添加条件如果满足
u
=
t
的话终止程序。
<
/p>
现在我们可以通过迭代来回溯出
s
到
t
的最短路径
1
S := empty sequence
2 u := t
3
while
defined u
4 insert u to the beginning of S
5 u := previous[u]
现在序
列
S
就是从
s
到
t
的最短路径的顶点集
.
时间复杂度
我们可以用大
O
符号将
Dijkstra
算法的运行时间表示为边数
m
和顶点数
n
的函数。
Dijkstra
算法最简单的实现方法是用一个链表或者数组来存储所有顶点的集合
Q
,
所以搜索
Q
中最小元素的运算
(Extract-Min(
Q
))
只需要线性搜索
Q
中
的所有元素。
这样的话算法的
运行时间是
O
(
n
2
)
。
对于边数少于
n
2
稀疏图来说，
我们可以用邻接
表来更有效的实现
Dijkstra
算法。
同时需要
将一个二叉堆或者斐波纳契堆用作优先队列来寻找最小的顶点
(Extract-Min)
。
当用到二叉
堆的时候，算法所需的时间为
O
((
m
+
n
)log
n
)
，斐波纳契堆能稍微提高一些性能，让算
法
运行时间达到
O
(
< br>m
+
n
log
n
)
。
相关问题和算法
在
< br>Dijkstra
算法的基础上作一些改动，可以扩展其功能。例如，有时希望在
求得最短路径
的基础上再列出一些次短的路径。
为此，
可先在原图上计算出最短路径，
然后从图中删去该
路径中的某一条边，
在余下的子图中重新计算最短路径。
对于
原最短路径中的每一条边，
均
可求得一条删去该边后子图的最短
路径，这些路径经排序后即为原图的一系列次短路径。
OSPF
（
open
shortest path first,
开放最短路径优先）算法是
Dijkstra
算法在网络路由
中的一个具体实现
。
与
Dijkstra
算法不同，
Bellman-Ford
算法可用于具
有负花费边的图，只要图中不存在总
花费为负值且从源点
s
可达的环路（如果有这样的环路，
则最短路径不存在，因为沿环路
循环多次即可无限制的降低总花费）。
< br>
与最短路径问题有关的一个问题是旅行商问题
(tra
veling salesman problem)
，
它要求
找
出通过所有顶点恰好一次且最终回到源点的最短路径。该问题是
NP
难的；换言之，与最短
路径问题不同，旅行商问题不太可
能具有多项式时间算法。
如果有已知信息可用来估计某一点到
目标点的距离，则可改用
A*
算法
，以
减小最短路径的
搜索范围。
4.2
、
A*
算法的
程序编写原理
< br>A*
（
A-Star)
算法是一
种静态路网中求解最短路最有效的方法。
公式表示为：
f(n)=g(n)+h(n),
其中
f(n)
是节点
n
从初始点到目标点的估价函数，
g(n)
是在状态空间中从初始节点到<
/p>
n
节点的实际代价，
< br>h(n)
是从
n
到目标节点最佳
路径的估计代价。
保证找到最短路径（最优解的）条件，关键
在于估价函数
h(n)
的选取：
估价值
h(n)<= n
到目标节点的
距离实际值，
这种情况下，
搜索的点数多，
搜索范围大，
效率
低。但能得到最优解。
< br>
如果
估价值
>
实际值
,
搜索的点数少，搜索范围小，效率高，但不能保证得到最优解。
估价值与实际值越接近，估价函数取得就越好。
例如对于几何路网来说，可以取两节点间欧几理德距离（直线距离）做为估价值，
即
f=g(n)+sqrt((dx-nx)*(dx-nx)+(dy-
ny)*(dy-ny))
；这样估价函数
f
< br>在
g
值一定的情况下，会或多
或
少的受估价值
h
的制约，节点距目标点近，
h
值小，
f
值相对就小，能保证最
短路的搜索
向终点的方向进行。明显优于
Dijstra
算法的毫无无方向的向四周搜索。
A*
算法是
一个可采
纳的。
< br>举一个例子，其实广度优先算法就是
A*
算法的特例。其
中
g(n)
是节点所在的层
数，
h(n)=0
，这种
h(n)
肯定小于
h'(n)
，
所以由
前述可知广度优先算法是一种可采纳的。
实
际也是。当然它是一
种最臭的
A*
算法。我在《初识
A*<
/p>
算法》中说过，
A*
算法是最好优先算<
/p>
法的一种。
只是有一些约束条件而已。
我
们先来看看最好优先算法是如何编写的吧。
如图有
如下的状态空
间：（起始位置是
A
，目标位置是
P<
/p>
，字母后的数字表示节点的估价值）。
如图有如下的状态空间：（起始位置是
A
，目标位置是
P
，字母后的数字表
示节点的估
价值）
图
1
状态空间图
搜索过程中设置两个表：
OPEN
和
CLOSED
。
OPEN
表保存了所有已生成而未考察的节点，
CLOSED
表中记录已访问过的节点。算法中有一步是根据估价函数重排
OPEN
表。这样循环中
的每一步只考
虑
OPEN
表中状态最好的节点。具体搜索过程如下：
1
）初始状态：
OPEN=[A5];
CLOSED=[];
2
）估算
A5
，取得搜有子节点，并放入
OPE
N
表中；
OPEN=[B4, C4, D6];
CLOSED=[A5]
3
）估算
B
4
，取得搜有子节点，并放入
OPEN
表中；
OPEN=[C4, E5,
F5, D6];
CLOSED=[B4, A5]
4
）估算
C4
；取得搜有子节点，并放入
OPE
N
表中；
OPEN=[H3, G4, E5, F5, D6]
CLOSED=[C4, B4, A5]
5
）估算
H3
，取得搜有子节点，并放入
OPE
N
表中；
OPEN=[O2, P3, G4, E5, F5, D6];
CLOSED=H3C4, B4, A5]
6
）估算
O
2
，取得搜有子节点，并放入
OPEN
表中；
OPEN=[P3, G4,
E5, F5, D6];
CLOSED=[O2, H3, C4, B4, A5]
7
）估算
P
3
，已得到解；
看了具体的过程，再看看伪程序吧。算法的伪程序如下：
Best_First_Search()
{
Open =
[
起始节点
];
Closed = [];
while (
Open
表非空
)
{
从
Ope
n
中取得一个节点
X,
并从
OPEN
表中删除
.
if
(X
是目标节点
)
{
求得路径
PATH;
返回路径
PATH;
}
for (
每一个
X
< br>的子节点
Y)
{