动态环境中的规划

if g(s’)>g(s)+c(s,s’) g(s’)=g(s)+c(s,s’)； 把s’ 插入OPEN；
CLOSED={sstart,s2,s1} OPEN={s4,sgoal} 下一个扩展状态：s4
A*搜索：例子
• 计算相关态的最佳g值 ComputePath函数： while (sgoal没有被扩展)
执行慢
4D（x,y,,V）规划
超过2千万个状态 规划慢
执行快
基于高维搜索的规划
6DOF机器人手臂 >3x109个状态
20DOF机器人手臂 >1026个状态
实际规划
• 由于下面原因，需多次再规划
– 环境变化
• 导航时，有人在附近 • 自动驾驶时，有其它车辆在路上
– 环境模型不精确 – 位置估计有误差
A*搜索
• 计算相关态的最佳g值 ComputePath函数： while (sgoal没有被扩展过)
从OPEN中移去f(s)( = g(s)+h(s))最小的s； 把s插入CLOSED； 对s的每个不在CLOSED中的后续态s’
if g(s’)>g(s)+c(s,s’) g(s’)=g(s)+c(s,s’)； 把s’ 插入OPEN；
• 在解许多问题时，比A*快得多。
A*的最佳性质
• f(s) = g(s) + h(s)为次序来扩展状态。 • C*为最佳路径的代价，A*搜索：
– 将扩展f(s) < C*的所有结点 – 可能扩展一些f(s) = C*的结点 – 不扩展f(s) > C*的任何结点
• 特例：
– h(s) = 0，f(s) = g(s) UCS搜索，需扩展当前态的所 有后续态
从OPEN中移去f(s)( = g(s)+h(s))最小的s；
把s插入CLOSED； (s)=g(s)； 对s的每个不在CLOSED中的后续态s’
if g(s’)>g(s)+c(s,s’) g(s’)=g(s)+c(s,s’)； 把s’ 插入OPEN；
注：
• 值是一个状态在其扩展过程中的值。
对s的每个不在CLOSED中的后续态s’ if g(s’)>g(s)+c(s,s’)
g(s2)>g(sstart)+c(sstart,s2)
g(s’)=g(s)+c(s,s’)；
把s’ 插入OPEN；
CLOSED={} OPEN={sstart} 下一个扩展状态：sstart
A*搜索：例子
• 计算相关态的最佳g值 ComputePath函数： while (sgoal没有被扩展)
A*搜索
• 计算相关态的最佳g值 主函数： g(sstart)=0；所有其它g值是无穷；OPEN={sstart}； ComputePath()； 给出结果；
ComputePath函数： while (sgoal没有被扩展)
从OPEN中移去f(s)( = g(s)+h(s))最小的s； 扩展s；
注： OPEN是扩展候选态的集。 如果启发方式是一致性的，则每个扩展 态的g(s)都是最佳的。
if g(s’)>g(s)+c(s,s’) g(s’)=g(s)+c(s,s’)； 把s’ 插入OPEN；
对每个已扩展状态，g(s)是最佳的。 对每个其它状态，g(s)是一个上限。 现在能计算一个最小代价路径。
加权A*
• 以f(s)= g(s)+h(s)(>1)为次序来扩展状态。 • 亚优：cost(解) cost(最佳解)。
注： CLOSED是已扩展状态的集。 if体中重新给g(s’)赋值，是试图用找到的 从sstart到s的路径来降低g(s’)。
A*搜索：例子
• 计算相关态的最佳g值
ComputePath函数：
while (sgoal没有被扩展) 从OPEN中移去f(s)( = g(s)+h(s))最小的s；
把s插入CLOSED；
CLOSED={sstart,s2,s1,s4,sgoal} OPEN={s3} 结束
A*搜索：例子
• 计算相关态的最佳g值 ComputePath函数： while (sgoal没有被扩展)
从OPEN中移去f(s)( = g(s)+h(s))最小的s； 把s插入CLOSED； 对s的每个不在CLOSED中的后续态s’
A*搜索：例子
• 计算相关态的最佳g值 ComputePath函数： while (sgoal没有被扩展)
从OPEN中移去f(s)( = g(s)+h(s))最小的s； 把s插入CLOSED； 对s的每个不在CLOSED中的后续态s’
if g(s’)>g(s)+c(s,s’) g(s’)=g(s)+c(s,s’)； 把s’ 插入OPEN；
动态环境中的规划
路径规划
概要
• 规划经常是一个重复过程，且要求快速。
– 动态环境 – 不精确的初始模型 – 真体位置有误差
• 基于A*的规划器类型：
– ARA*
• 随时A*搜索 • 输出 亚优解 • 能在有时间约束下使用
– D*与D*精简版
• 递增A*搜索 • 通过复用前次搜索结果来计算最佳解 • 常常能显著加速重复规划
– 结合上述两者的优点。
搜索最小代价路径
• 计算相关态的g值
– g(s)：一条从sstart到s最小代价路径的代价估值。 – 最佳值满足： g(s)=mins”pred(s)(g(s”)+c(s”,s))
由s3到sgoal边的 代价c(s3,sgoal)
搜索最小代价路径
• 最小代价路经是由回溯（backtracking）获 得的一条的贪婪路径
CLOSED={s4} OPEN={sgoal,s3} 下一个扩展状态：sgoal
示例：复用A*（ =1）
CLOSED={s4,sgoal} OPEN={s3} 结束
现在能够计算一个最小代 当ComputePath终止后：
价路径
所有状态的g值都等于最终A*的g值
回到实例
• 执行一系列降低的加权A*搜索：
A*搜索：例子
• 计算相关态的最佳g值 ComputePath函数： while (sgoal没有被扩展)
从OPEN中移去f(s)( = g(s)+h(s))最小的s； 把s插入CLOSED； 对s的每个不在CLOSED中的后续态s’
if g(s’)>g(s)+c(s,s’) g(s’)=g(s)+c(s,s’)； 把s’ 插入OPEN；
(s)=g(s) （即一致性）。
• 初始化OPEN时，使用上次搜索结果。
示例：复用A*（ =1）
CLOSED={} OPEN={s4,sgoal} 下一个扩展状态：s4
g(s’)=mins”pred(s’)(v(s”)+c(s”,s’)) 初始的OPEN包含所有不一致性的状态
示例：复用A*（ =1）
– h(s) = h*(s)，f(s) = g(s) + h*(s)，只扩展当前态的最佳 后续态
加权A*：示例
A*
11,054次扩展 代价=168,204
=10的加权A*
1,138次扩展 代价=177,876
构建随时搜索
• 执行一系列降低的加权A*搜索： 置为大值； while 1，并且仍留有时间来规划
– 随时D*(AD*)
• 随时递增A*搜索 • 输出 亚优解 • 能在有时间约束下使用 • 常常能显著加速重复规划
– 所有都基于ComputePathWithReuse函数
动态环境中的自动真体
ATRV机器人 Segbot机器人
2D地图 3D地图
规划（Planning）
• 规划
– 利用一个问题的结构来构造一个到达目的行动 计划
• 需快速再规划
实际规划
• 由于下面原因，需多次再规划
– 环境变化
• 导航时，有人在附近 • 自动驾驶时，有其它车辆在路上
– 环境模型不精确 – 位置估计有误差
• 需快速再规划！
用随时D*（即随时动态A*）来做4D规划
实际规划
用随时D*（即随 时动态A*）来做 4D规划
用随时D*（即随时动态A*）来做3D停车规划
• g(s’)=mins”pred(s’)(v(s”)+c(s”,s’)) • OPEN：一个(s)>g(s)（即不一致性）状态的集，其它所有状态有
(s)=g(s) （即一致性）。
复用加权A*搜索
用所有的不一致性的状态来初值化OPEN；
ComputePathWithReuse函数：
while (sgoal没有被扩展)
CLOSED={sstart,s2} OPEN={s1,s4} 下一个扩展状态：s1
A*搜索：例子
• 计算相关态的最佳g值 ComputePath函数： while (sgoal没有被扩展)
从OPEN中移去f(s)( = g(s)+h(s))最小的s； 把s插入CLOSED； 对s的每个不在CLOSED中的后续态s’
实际规划
• 随时规划算法，例如， A*的随时复用版，即 ARA*
– 快速找到第一个可能的亚优解，然后用其余时间来改 进它。
– 允许满足时间约束。
• 再规划算法，例如， A*递增版，也即D*与D*精简 版
– 复用以前规划来加速再规划 – 很适合于动态和/或部分已知的环境。
• 随时再规划算法，例如，随时递增A*，即随时D*
=2.5
=1.5
=1.0
13次扩展，解=11次移动 15次扩展，解=11次移动 20次扩展，解=10次移动