7机器人运动规划

势场
由障碍物产 生的斥力场 由目标点产 生的引力场
• 远离障碍物：设想障碍物是能产生一种斥 力场的物质 • 移近目标：设想目标位置是一个能产生一 种引力场粒子
移向最低势能 势场中的最陡下降， 也即最佳优先搜索
• 目标引力场：
U g q d q, q goal
2
到达目标 态的距离
• 障碍物斥力场：
生物
机器人仅是空间推理的一种应用
动漫
自由度
• 一个机器人的几何构形 是由p个自由度（DOF） 来定义的。 • 假设p个DOF，一个机 器人的几何构形A是由 p个变量来定义的： A(q)，q={q1,…,qp} • 例子：
– 棱柱（平移）DOF：qi 是沿某方向的平移量。 – 旋转DOF ：qi是绕某轴 的旋转量。
– 在相应x位置构建平面 – 依据事件类型：
• 将一个当前区域划分为两个新区域，或着 • 合并两个当前区域
– 产生一个新区域
• 2D复杂性
– 时间：O(NlogN) – 空间：O(N)
准确区域划分
• 准确区域划分能扩 展到较高维及非多 边形边界，称柱形 区划分。 • 提供准确的解，因 而具有完全性。 • 在较高维下昂贵且 难实现。
可见图
• 无障碍时，最佳路径就是qstart与qgoal之间的 直线
可见图
• 多边形障碍物时，最短路径似乎是一系列 连接障碍物顶点的直线段。 • 总对吗？
可见图
• 可见图G=障碍物顶点(外加上qstart和qgoal )之间无 遮挡线段的集。 • 如果从一结点P可看见另一结点P’，则将P与P’相 连接。 • 解=可见图中的最短路径。
– 区别下面两者
完全位于Cobs内的填满区域，以及 部分与Cobs相交的混合区域
– 在当前区域集中试找一条路径 – 如果找不到路径
再细划分混合区域，并在新区域集中再试
例子
近似区域划分：局限
• 优点：
– 对障碍物构形的要求少 – 方法实用 – 能快速找到明显的路径
• 缺点：
– 没有清楚的关于最佳性的概念 – 完全性与计算之间权衡 – 在高维时应用仍有困难
– U(qgoal)=0 – 对任何与qgoal不同的q，存在一个近邻q’，使得 U(q)>U(q’)
摆脱局域极小1
• 重复
– if U(q)=0 返回成功 – else if 迭代太多，返回失败 – else
• 寻找q的近邻qn，且U(qn)为极小 • if U(q)>U(qn)或者qn还未访问过
性质
• • • •
能在空间的所有方向快速探索 不需一个大范围的预处理 单个最近邻查询 仅需冲突检测，即不需表征和（或）预先计算整 个C空间。
总结
采样 势场 近似区 域划分 Voronoi图 可见图
2D或3D中的实 用性
>2D或3D中的实 用性
是
是
是
是(用随机 版本)
是
?
是
否
是
否
快速
在线扩展 完全性
Voronoi图：规划
• 寻找Voronoi图中qstart的最接近点q*start • 寻找Voronoi图中qgoal的最接近点q*goal • 计算Voronoi图中从q*start到q*goal的最短路 径
实例
Voronoi图：弱点
• 在高维或非多边形场合下，难计算 • 近似算法 • Voronoi不一定是最佳启发方式。能导致很 保守的路径 • 可能不稳定。障碍构形的小变化能导致图 形的大改变
• else
– 从qn开始，随机行走T步 – 将q置为随机行走到达的构形
注：类似于随机搜索和模拟退火，因此能较快脱离局域极小。
高维C空间
多节足虫机器人，约13,000个自由度
高维C空间处理
近邻的全集
近邻的随机子集
• 应评估当前态的所有近邻态，但是： • 邻域尺寸随维数呈指数增长 • 高维时，很昂贵 解决方法： • 只评估近邻的一个K大小的随机子集 • 移向势能最低的近邻
机器人运动规划
轨迹规划
概要
• 问题提出 • 基本方法：
– 路线图
• 可见图 • Voronoi图
– 区域划分 – 势场
• 方法扩展
– 采样技术 – 在线算法
机器人运动规划
• • • • 搜索，如A*、随机搜索等方法的应用 几何构型中搜索 空间推理 挑战：
– 连续状态空间 – 高维空间
加工工程与设计
示例
允许沿x与y移动：2DOF
允许沿x与y移动和转动： 3DOF(x,y,)
示例
固定在底座
自由
固定：虚线受水平约束
构形空间（C空间）
q=(x,y,) C=R2 2D旋转集
q=(q1,q2) C=2D旋转集2D旋转集
• 构形空间C=机器人合法构形的q值的集 • 定义可能参数的集（搜索空间）和允许路径的集
是
是 概率完 全
是
是 概率分辨 率完全
是
? 分辨率 完全
低维
? 是
2D
否 是
更准确/完全 更快/高维下更实用
更复杂的C空间
在所有场合下，通过扩张障碍物，就把问 题简化为寻找一个点穿越构形空间的路经
运动规划问题
• • • • • 轨迹规划：对求解问题的空间几何轨迹及其生成进行规划 A=在2D或3D中有p个自由度的机器人 CB=障碍物集 如果不导致机器人与障碍物交叉，则构形q是合法的。 已知起始与最终构形(qstart, qgoal)，寻找一个从qstart到qgoal 的合法构形的连续序列。 • 如果没找到路径，则报告失败。
Voronoi图
• 复杂性：
– 时间：O(NlogN)，等同于给N个点排序 – 空间：O(N)
非点的Voronoi图
• 边是直线段与二次曲线段的组合 • 直线边：与两线等距的点 • 曲线边：与一个角点和一条线等距的点
Voronoi图（多边形）
Voronoi图（多边形）
• 主要性质：Voronoi图中的边点离障碍物最远。 • 思路：沿着Voronoi图中的边构建一条由qstart到 qgoal的路径。 • 把Voronoi图作为路线图。
准确区域划分
在一个区域内的任何路径 保证不与任何障碍相交
准确区域划分
• 区域图形定义一个路线图
准确区域划分
qstart
qgoal
• 路线图能用来寻找任何两个构形之间的一 条路径
准确区域划分
关键事件： 划分当前区与合并当前区
平面扫描算法
• 将当前区域序列初始化为空 • 将Cobs中的顶点沿x方向排序 • 对每个顶点：
形式化保证：普适钢琴搬动者问题
• 形式化结论（但在实际算法上不太有用）：
– p：C的维数 – m：用来描述Cfree的多项式的数目 – d：多项式的最高次方
• 如路径存在，则能按p的指数时间，以及m与 d的多项式时间来找到该条路径。
处理方法
• 基本方法：
– 路线图
可见图 Voronoi图
扫描算法
• 绕着每个顶点，扫动一条源于该点的射线。 • 记录那些到达其它可见顶点的线段。
复杂性
• 构建可视图是昂贵的：
– N=多边形障碍物的顶点总数 – 简单：O(N3)，因为有O(N2)对顶点，多边形障碍物有O(N)条边 – 扫描：O(N2logN)，因为每个顶点要访问(N-1)个其它顶点，并且 在每次访问中，要依据在该方向上的障碍物序列来确定其可见性， 开销为O(logN)。 – 最佳：O(N2)，因为每个顶点都有一条从-/2移到/2的扫描线，而 所有扫描线都同时移动（旋转树）。
8个自由度
10个自由度
采样技术
• 在高维时，要完全地描述与最佳地探索C空 间是很难的，并且也无必要。 • 只需寻找C空间的一个好样本。
采样技术
Rp
禁止空间
自由空间
采样技术
随机采样位置
采样技术
去掉禁止区的样本
采样技术
将每个样本与其k个最近邻相连（k最近邻查询）
采样技术
去掉穿越禁止区的连接
可见图：弱点
• 最短路径，但：
– 试着尽可能靠近障碍物 – 任何执行错误将引起碰撞 – 2D以上空间中很复杂
• 只要能找到一条安全路径，可不严格介意 最佳性。因为规避障碍比寻找最短路径更 重要。 • 需要定义其它类型的路线图。
Voronoi图
• 已知平面上一组数据点
– 着色整个面，使得平面上任何点的颜色与其最近邻点 的颜色是相同的。
采样技术
• 能用相对少的随机采样点来找到一个解。 • 对多数问题而言，相对少的样本是足以覆 盖大部分可行的空间的，并且找到路径的 概率为1。 • 对于一大类问题而言：
– 随采样数增加，Prob(找到一条路径)指数地趋 向于1
• 但是，不能用来检查路径的不存在性。
快速探索随机树（RRT）
• 将图形初始化为{qstart} • 重复：
自由空间：点机器人
• Cfree={A(q)不与障碍物交叉的参数的集ห้องสมุดไป่ตู้ • 例子：
– 对2D平面上的一个点机器人：R2减去障碍物区
自由空间：对称机器人
自由空间：对称机器人
• C=R2，因为取向无关 • 以机器人半径来扩大障碍物尺寸，则该问 题可简化为点机器人问题
自由空间：非对称机器人
• 构形空间是3D（x,y,） • 对每个值，需对障碍物实施一个相应的（不同的）扩张 • 仍可通过扩张障碍物，把问题简化为点机器人问题
区域划分
• 把空间划分为区域，在区域内的路径是无 障碍物的。
近似区域划分
• 定义一个C空间的离散网格 • 标记与Cobs相交的网格区为遮挡区 • 可用A*（其中，可用直线距离作为启发）来寻找 穿越剩余区域的路径。 • 上述算法是不可能有完全性的。为什么？
近似区域划分
• 不完全性：甚至在存在路径时，也不能找到一条路径 • 解决办法：
– 移向qn(qqn) – 记录qn
注：可能需花长时间来探索局域极小的邻域。
摆脱局域极小2
• 重复
– if U(q)=0 返回成功 – else if 迭代太多，返回失败 – else
• 寻找q的近邻qn，且U(qn)为极小 • if U(q)>U(qn)或者qn还未访问过
– 移向qn(qqn)
采样技术
结果得到一幅似然（概率）路线图（PRM）
采样技术
将起点和终点与PRM相连，并用A*来搜索
采样技术
• 方法：
– 将连续空间转换成离散空间，再用A*搜索在似 然路线图上寻找路径
• 好的采样策略是重要的：
– 均匀采样 – 在无近邻的点的附近多采样 – 在障碍物附近多采样 – 采用预先计算过的样本序列
Voronoi图
• Voronoi图=用来划分不同颜色区域的线段集
– 线段（边沿）=与两个数据点等距离的点 – 顶点=与两个以上数据点等距离的点
Voronoi图
边点到蓝点 和红点是等 距离的
顶点离开三 点的距离是 相等的
• Voronoi图=划分不同颜色区的线段集
– 线段（边沿）=与两个数据点等距离的点 – 顶点=与两个以上数据点等距离的点
– 区域划分 – 势场
• 扩展
– 采样技术 – 在线算法
注：在所有场合，先将在连续C空间中难处理的 问题简化为在离散空间中可处理的问题。则可以 使用已知的技术，如，A*，随机搜索等。
路线图
• 路线图：自由空间中的一维曲线网络 • 一般思路：
– 避免搜索整个空间 – 预先计算一幅路线图，呆在路线图所指明的道路上能保证规避障 碍物。 – 在路线图上寻找一条qstart到达qgoal的路径。
– 选择随机新样本qrand – 寻找离qrand最近的结点qnear – 如果无冲突，则产生边(qnear,qnew)
注： • qnew = qnear + ， 为(qnear,qrand)两构型之间最佳路径的代价的度量 • 如果qnear到qrand的连线和障碍物相交，则qnew是在该障碍物前方（即 在qnear一侧），并且最接近于qrand的点。否则，qnew就是qrand
1 U o q 2 d q, q obstacle
• 总势场：
到达最近障碍点的 距离，可用距离变 换来效地计算
控制离开障
碍物的远近
U q U g q Uo q
势场：局限
• 完全性？ • 较高维下有问题
局域极小问题
qgoal
势能局 域极小
• 势场一般会出现局域极小 • 特殊场合：导航函数