1、采样法
利用网格法可以得到对特定离散化后C-space的最优解。但网格法的缺点是计算复杂度高,适用于自由度较低的机器人。本文简要介绍另一种基于采样的规划器,它涉及以下函数,通过这些函数构建一个图(graph)或树(tree),用于表达机器人的可行运动:
一个随机或者确定函数从C-space或state space中选择样本点
一个用于评估样本点或者运动是否在
空间的函数
一个选择之前自由空间里邻近样本点的函数
一个局部规划器,用于连接新的样本点
采样法放弃了网格法得到的分辨率最优解(resolution-optimal solutions),由此换来了在处理高自由度机器人规划问题的满意解(satisficing solution),且求解速度快速。所选择的样本点构成了一个路图或搜索树。相比网格法,采样法可通过更少的样本点来逼近
,因为随着搜索空间自由度的增加,网格法的样本点数目是以指数形式增长的。大部分采样法是概率完备的,也就是说,在有解的情况下,当样本点数目趋于无穷大,找到解的概率趋于100%。
目前有两类算法基于采样法:快速搜索随机树(RRT)和 概率路图(PRM)。RRT采用了树结构在C-space或state space中进行单次规划(single-query planning),PRM主要是在C-space中创建路图,可实现多次规划(multiple-query planning)。下面分别介绍这两种算法的原理和Python代码实现。
2、RRT算法
RRT算法搜索了一条无碰撞运动,用于从初始状态
到达目标集合
。当用于运动学问题时,
代表了位型
,当用于动力学问题时,
则同时包含了速度。RRT算法从
开始生成一个树,以下是RRT算法的伪代码:
1: initialize search tree T with xstart
2: while T is less than the maximum tree size do
3: xsamp sample from X
4: xnearest nearest node in T to xsamp
5: employ a local planner to find a motion from xnearest to xnew in the direction of xsamp
6: if the motion is collision-free then
7: add xnew to T with an edge from xnearest to xnew
8: if xnew is in Xgoal then
9: return SUCCESS and the motion to xnew
10: end if
11: end if
12: end while
13: return FAILURE
以一个典型的运动学问题为例(x代表q),第3行将从均匀分布的
中随机地(略倾向于
)选取一个
。第4行中的
是搜索树
中距离
最近的节点。第5行中的
可以选择[
,
]线段上的一点,距离
有一小段距离
。由于
很小,可以用一个简单的局部规划器返回一条直线运动,连接
到
,如果运动是无碰撞的,
将被加入到搜索树
中。
均匀分布的样本点会让生成的树快速探索
空间,下图是由RRT生成的一个树:
RRT算法生成树
从上面的伪代码可以看出在编程时我们有较多的选择,包括如何选择样本点(第3行),如何定义最近点(第4行),如何规划向的运动(第5行)。即便对采样法做很小的改变,都可能对规划时间产生很大的影响。下面分别对这三行做进一步描述。
2.1、第3行:采样器(Sampler)
最常见的方法是随机地从均匀分布的
空间采样。比如,对于欧几里德C-space
,以及
关节机器人的C-space
,我们可以在每个关节上均匀采样,对于平面移动机器人的C-space
,可以分别在
和
上均匀采样。
对于动态系统,则是在状态空间中均匀采样,分为两部分:在C-space中均匀采样和在有界速度集上均匀采样(即
)。
实际上,样本点的也不一定需要随机选择。比如可以采用一个确定的采样方案,在
空间生成逐渐细化的网格,最终会在状态空间产生浓密的样本点,当样本点无穷多时,样本将与
空间无限接近。
2.2、第4行:定义最近节点(Nearest Node)
寻找最近节点依赖于在
空间中如何定义距离。对于在
空间上无运动约束的机器人来说,一个很自然的选择就是采用欧几里德距离。对于某些空间,距离的定义就不是这么明显了。
比如平面上的小车机器人,它的C-space表达为
,下图中哪一个与
更近呢?
哪一个位型和灰色的最近?
由于运动约束的存在,小车无法原地转动或侧向移动,为了朝着移动,那么在它后面的那个位型是最佳的位置。因此应该被定义为去往最快速的节点,然而要计算出来是很困难的。一种简单的选择是在每个维度上设置权重,然后计算的加权和。权重描述了不同维度的重要性 。
2.3、第5行:局部规划器(Local Planner)
局部规划器的作用是寻找到一条路径,从