机器人运动规划思维导图
U196265724
2025-04-17
运动规划
算法规划
算法分类
构型空间，采样规划算法，BUG算法相关内容讲解
树图思维导图提供《机器人运动规划》在线思维导图免费制作，点击“编辑”按钮，可对《机器人运动规划》进行在线思维导图编辑，本思维导图属于思维导图模板主题，文件编号是：42851bf30ac8511850a2cf0c59a6b083
思维导图大纲
相关思维导图模版
机器人运动规划思维导图模板大纲
导论
机器人的特性
执行具体的任务
具有一定的智能
运动规划包括什么
运动规划是路径规划和轨迹规划的统称
运动规划的研究领域
路径规划
机动规划
轨迹生成
运动规划的基本问题
一般问题
算法问题
模型问题
路径的概念
不与环境/场景发生冲突的、预先设定的机器人质心在空间的运动路线
轨迹的概念
轨迹是路径加上实现每个构型的具体时间
路径和轨迹的组成
路径点
路径段
路径与轨迹的区别
路径重点在路径的几何属性，具有通用性
轨迹附加动力学约束属性，个性明显
路规划方法有哪些
传统方法：Bug算法、数学规划方法、图搜索算法、图形方法、路标图法、单元分解法、势场法、随机采样方法
智能方法：遗传算法、蚁群算法、粒子群算法、神经网络
Bug算法
引言
受昆虫启发
已知目标的方向
机器人可以测量当前位置到目标点的距离
可以探测到周围的环境
墙/障碍物等
合理的环境
搜索区域有界、区域内障碍物个数有限
一条直线与障碍物的相交次数有限
Bug算法的特点
许多规划算法假设全局信息可知，Bug算法仅假设局部环境和全局目标可知。
Bug算法的两种行为
沿着障碍物/边界搜索
朝向目标搜索
三种Bug算法
Bug0算法假设可视距离无穷大，不包括记忆信息
Bug1和Bug2算法假设可视距离无穷大，且具有记忆功能
Tangent Bug 在Bug2算法基础上假设可视距离有限
Bug算法的基本概念
起始点qs和目标点qg
“撞击点”qi^H和“离开点” qi^L
路径定义为从起始点到目标点、且中间节点为撞击点和离开点的序列
Bug0算法步骤
机器人朝着目标点前进
如果遇到障碍物，沿着障碍物边界，直到可以再次朝着目标点前进
重复上述步骤直到抵达目标点
Bug0 算法可能失效
更好的Bug算法：增加记忆功能
Bug1算法
Bug1算法步骤
机器人朝着目标点前进
如果遇到障碍物，沿着障碍物边界一周，选择障碍物边界中到目标点最近的点为离开点
沿着障碍物的边界回到到离开点
重复上述步骤
Bug1算法性能分析
D: 从起始点到目标点的直线距离
P_i：第 i 个障碍物的周长
下界：D
上界：
D+1.5∑iPi
算法的优缺点
算法是完备的，但是效率不高
Bug2算法
Bug2算法步骤
沿着 m 线朝着目标点前进
如果遇到障碍物，绕过障碍物直到下次再遇到m线
离开障碍物继续沿着m线向目标点移动
Bug2算法在特殊情况下会失效
在某种情况下，重新遇到 m 线会让机器人回到起点
选择不同绕行方向的结果不同
Bug2算法性能分析
假设 ni 表示障碍物 i 与起始点qs和目标点qg连线相交的次数；
下界：D
上界：
D+0.5∑iniPi
Tangent Bug算法
Bug1和Bug2算法假设
全局信息可知、能够对障碍物实现接触式跟踪
但现实中机器人通常使用某种范围传感设备
Tangent Bug 算法假设机器人装配一个距离传感器
数学补充
距离函数
假设机器人用360°距离传感器测量距离，机器人视野范围由传感器测距边界和障碍物组成
连续区间
自由空间边界上的一组连通点
连续区间的边界点用 Oi 表示
最小化启发式函数
定义为机器人到可视的连续区间边界点的直线距离+该边界点到目标点的直线距离
向目标移动
传感器范围内没有障碍物时，向目标直线运动
传感器探测到障碍物时（单个或多个），机器人沿着使得启发式距离最小的边界点继续向着目标运动
障碍物绕行
定义df是已经感知到的边界与目标之间的最短距离
定义dl的值是目标与机器人视线内跟随障碍物上最近点之间的距离
当 dl<df时，机器人终止边界“障碍物绕行”行为
如果机器人找到启发式距离的局部最小值点M，则结束“向目标移动”，转为“障碍物绕行”
Bug0、Bug1与Bug2算法比较
Bug算法的主要行为方式
向目标移动
障碍物绕行
Bug0算法可能无法达到目标
Bug1算法是一种穷举搜索算法，是完备的
在确认最终结果之前搜索所有可能
安全、可靠
Bug1 算法的总体性能更可预测
Bug2算法是一种贪婪算法，也是完备的
选择第一种看起来不错的搜索结果
不稳定，部分情况下好、部分情况下不好
在多数情况下Bug2 算法性能优于Bug1 算法
Tangent Bug算法的特点
支持传感器
路标图法
引⾔
地图的不同表示⽅法
⼏何表示
⽹格表示
图表示
通过路径图或路径⽹络捕获⾃由空间的连通性
路标图的定义
路标图是⼀维曲线的并集，使得对于Qfree中的所有起点和⽬标点都可以通过路径连接
可达性：从构型空间的起点可以到路标图起点
可分离性：从路标图的终点可以到构型空间的终点
连通性：路标图中的起点和终点是连通的
路标图算法原理
构建路标图
节点是Qfree（或其边界）中的点
如果两个节点之间有⾃由路径，则称两个节点是连通的
将所有连通的点连接起来
将起点和终分别连接到路标图中点q'和q"
在路标图上找到⼀条在q'和q"之间的路径，结果是Qfree从开始到⽬标的路径
通视图
定义
假设空间中的障碍物都是多边形
在没有障碍物的情况下，则最短路径是连接起点到⽬标点的直线
在有障碍物的情况下，最佳路径是连接起点、障碍物顶点以及⽬标点的⼀系列线段组成的序列
形成⽅式
通过将所有“可⻅”顶点、起点和终点彼此连接起来⽽形成
特性
可⻅意味着⽆障碍，障碍周围存在可⾏路径
构建通视图
通视图G的顶点定义为所
有障碍物顶点、起点和终点可视线段的集合
如果 q'从q"可⻅，则连接
节点 q'与节点q"
通视图算法的效率复杂度
为O(n^3),⽐较⾼效
通视图剪枝
⼀条⽀撑线与两个障碍物相切，使得两个障碍物都位于线的同⼀侧
⼀条分隔线与两个障碍物相切，使得障碍物位于线的相对两侧
通视图剪枝就是删除通视图中不在⽀撑线或分隔线上的所有边
通视图构建算法
⽬标：计算从v可⻅的顶点集vi
可⻅性：线段vvi是可⻅的
线段不在障碍物内
与 vvi相交的最近线在vi之外
构建的复杂度为O(n^3)
计算线段vvi的⻆度ai ，对顶点进⾏排序并按此创建列
表E
创建活动列表S，包含v发出的⽔平半线相交的已排序的边列表
对于每个ai
如果vi对v可⻅，则将vvi添加到图中
如果vi是边E的“头”且不在S中，则将E插⼊S
如果vi是边E的“尾”且在S中，则从S中删除E
总结
能够得到最短路径
找到的路径是“半⾃由”的，任何执⾏误差都会可能会导致碰撞
通视图在更⾼维度上不⼀定保持最优性
只要能找到安全路径，可能不关⼼严格的最优性
远离障碍⽐找到最短路线更重要
⼴义Voronoi图法
Voronoi 图
给定平⾯中的⼀组数据点
给整个平⾯上⾊，使得平⾯中任何点的颜⾊与其最近邻点的颜⾊相同
构成 Voronoi 图的线段隔离了⼀组点
Voronoi 图的特性
线段：与2 个点等距的点
顶点：与2 个以上点等距的点
性质：最⼤化点与障碍物之间的距离
⼴义Voronoi图
GVD 由距离最近的两个物体等距的路径形成
最⼤化障碍物之间距离
⽣成⼀个⾮常安全的路标图，尽可能避开障碍物
组成
边是直线段和⼆次曲线段的组合
直边：与两条线等距的点
曲线边：与⼀个⻆和⼀条线等距的点
构造GVD
基于传感器
允许机器⼈在未知空间中构建 GVD
选择边界点为起始点
沿着⼆等距⾯搜索直到到达交会点或者边界点
当遇到新的交会点时，标记搜索⽅向，然后识别从它出发的所有新的GVD边，继续探索新的GVD边，直到检测到另⼀个交会点或者边界点
当遇到旧的交会点，则机器⼈已经完成了GVD图中的⼀个环，继续搜索与当前交会点相关的⼀个未搜索的边
当机器⼈到达边界点时，则返回到具有未搜索GVD边的交会点
当所有的交汇点都没有未搜索的边与之关联时，搜索就完成了
基于多边形
在这种情况下我们可以计算 GVD 构造算法的复杂性
空间的组成是点和线段
到两个顶点等距的点集是⼀条线
与两条边等距的点集是⼀条线
并且与顶点和边等距的点集是抛物线
⽹格构型空间：Brushfire ⽅法
假设世界是⼀个允许⾼效计算的⽹格
构成方式与brushfire ⽅法一样
brushfire ⽅法的输出是⼀个离散图，其中⽹格中的每个像素的值等于到最近障碍物上最近点的距离
距离的不同定义
绝对值相加
平方和
⼴义Voronoi图的特性
关键属性：Voronoi图边上的点距离障碍物最远
思路：通过跟随Voronoi图上的边在起点和终点之间构造⼀条路径
总结
难以在更⾼维度或⾮多边形构型空间中使⽤
可能获得过于保守的路径
可能不稳定：障碍物的微⼩变化可能导致图的巨⼤变化
对于⾼维空间存在近似算法
采样规划算法
引⾔
传统路标图⽅法的缺点
构型空间可能⼗分复杂，障碍物可能⼗分复杂
传统的规划⽅法需要对构型空间结构有深⼊了解，否则规划很难进⾏
蒙特卡罗技术的兴起
核⼼思想：与其穷尽地探索所有可能性，不如随机探索较⼩的可能性⼦集
⽐任何穷举算法都更快地在构型空间中搜索到⽬标
通常必须牺牲完备性和最优性
需要权衡路径规划质量和算法性能
采样规划⽅法
各种采样规划⽅法均是对⾃由空间的离散采样
完全描述和优化探索⾼维构型空间太复杂且没有必要→只寻找构型空间的“好”采样
特点
掌握构型空间的连通性时不需要对其进⾏显式构造，仅仅在有限采样集合内研究连通性和碰撞检测
碰撞检测算法成熟、计算时间短
⽐其它⼏何⽅法效率⾼
使得复杂问题的简单化、局部化、规模降低
传统⽅法：构型空间离散，取点→也是⼀种采样，但这类⽅法对构型空间的类别有⼀定的要求，适⽤范围有限
基本步骤
建⽴构型空间的模型
关键
采样
连接
连通性和碰撞检测
运动规划
优点
概率完备
不需要构造整个构型空间
容易应⽤于⾼维构型空间
⽀持快速查询
缺点
对某些问题效果不佳
不适⽤狭窄通道
难以采样/连接约束⾯上的节点
不具有最优性和完备性
概率路标图法（PRM）
PRM是基于随机采样的规划⽅法
步骤
在⾃由空间进⾏路点（顶点）的随机采样
然后连接顶点并进⾏碰撞分析，再确定实际可⾏的顶点，并且将顶点加⼊到路标图中
将邻近的顶点连接起来，确认是否与障碍物相交，不相交的加⼊边集中
最后进⾏搜索路径
PRM适合各种复杂空间的运动规划问题
基本概念
⽬标
构造⼀个图顶点 q'到 q" 之间存在⽆碰
撞路径当且仅当 e = (q', q") ∈ Qfree
局部规划算法
设 Δ 是⼀个确定的局部规划算法，实现构型点的采样和边的连接
度量函数
度量函数D: Q × Q → [0，∞] 上的距离函数
⽣成步骤
初始化：图 G = (V, E) 是空的
采样构型点
采⽤局部规划算法，重复地从 Q 中按照⼀定的策略采样，直到采样到⾃由空间中的 n个构型时停⽌
连接构型
对于每个属于 V 的点q，将该点与周围的邻近点进⾏局部连接
路标图检查
删除路标图中与障碍物相交的边，检查路
标图的连通性
采样构型点
PRM采样策略属于多⽬标查询规划技术，通过预处理得到路标图
⽆效点在边界上或不符合构形要求（对运动体⽽⾔）
按照均匀分布采样策略随机采样 n 个点
⽆效点在边界上或不符合构形要求（对运动体⽽⾔）
也可以使⽤其他采样策略进⾏采样
⽆效点在边界上或不符合构形要求（对运动体⽽⾔）
去掉冲突点
冲突点在禁⽌区域（对障碍⽽⾔）
去掉⽆效点
⽆效点在边界上或不符合构形要求（对运动体⽽⾔）
连接采样点
搜索当前采样点的 k 个邻居
根据度量函数计算与该构型距离最近的 k 个点作为 q 的邻居集
将 q 与其邻居集之间的边加⼊到边集 E 中
采⽤kd树进⾏邻居搜索
kd 树：为了避免每次都重新计算⼀遍距离，算法会把距离信息保存在⼀棵树⾥，这样在计算之前从树⾥查询距离信息，尽量避免重新计算
kd 树是⼀种⼆叉树，表示对k 维空间的⼀个划分，构造 kd 树相当于不断地⽤垂直于坐标轴的超平⾯将k维空间切分，构成⼀系列的k维超矩形区域
超平⾯在⼀维中是⼀个点，⼆维中是线，三维的是⾯
kd树的构造过程
构造根节点
通过递归的⽅法，不断地对 k 维空间进⾏切分，⽣成⼦节点
重复上述步骤，直到⼦区域中没有示例时终⽌
距离函数
理想情况下，距离与机器⼈扫过的区域有关
很难精确计算
通常使⽤启发式函数
两种定义距离的⽅式
嵌⼊距离
将⾮欧⽒空间的距离通过映射嵌⼊到欧⽒空间中
叠加距离
对于同时可以旋转和平移的刚体，可以分别定义旋转和平移距离，然后对两个距离进⾏叠加获得整个构型之间的距离
平移距离有欧⽒空间内的距离定义，旋转距离可以有旋转矩阵或者四元数等的定义来定义
路标图检查
去掉冲突连接路段
对路标图的连通性进⾏检查
路标图不完备
碰撞检查
增量碰撞检查
细分碰撞检查
路径查询
将起始点、⽬标点与概率图连接
使⽤ A* 等图搜索算法进⾏搜索
路径后处理
路径缩短
路径平滑
采样策略
障碍物附近采样（OBPRM算法）
ሀ窄的通道可以被视为⾃由构型空间中被障碍物包围的细⻓⾛廊
狭窄通道内采样（桥梁规划算法）
可⻅性的采样
构型 q 的可⻅域包括局部规划算法可以连接到 q 的所有构型
将满⾜条件的那些构型 q 添加到路标图中
q不能连接到任何现有节点，即，q 是⼀个新⼦图
q连接⾄少两个现有⼦图
可操作性采样
可操作性采样利⽤与机械臂雅可⽐矩阵相关的可操作性度量，可操作性表征了机械臂在给定构型下的运动⾃由度
在可操作性⾼的构型空间区域，机器⼈具有很⾼的灵活性，因此这些区域需要的样本相对较少
在可操作性低的构型空间区域往往靠近(或包括)机械臂的奇异构型
在奇点附近，可能的运动范围会缩⼩，因此应该对这些区域进⾏更密集的采样
准随机采样
准随机采样最初是在蒙特卡洛积分的背景下引⼊的，旨在优化样本分布的各种特性
使⽤准随机序列的优点是保证算法的运⾏时间相同
⽹格采样
对构型空间进⾏⽹格划分
构型点可以⼀次性添加，也可以通过计算空间内最⼤空⽩处进⾏逐步添加
⽹格节点 q 移动到相邻的⽹格节点 q′ 需要碰撞检查
在查询阶段，尝试将 起点和终点连接到附近的⽹格点
连接采样
连接采样⽣成有助于路标图连接，并且可以与其他所有采样⽅法结合使⽤
选择⼀些可能位于ሀ窄区域的构型并扩展它们
在 q 的邻域内选择⼀个新的⾃由构型，将此构型添加到路标图中，并尝试其连接到路标图的其他构型上
连接采样步骤增加了自由空间中困难区域的路标图的密度
连接采样永远不会在路标图中创建新⼦图
采样策略选择
对于在实践中发现的涉及 3−7 个⾃由度的许多问题，均匀随机采样效果很好
如果需要运⾏时间⼀致，则可以采⽤准随机采样和基于栅格的采样
当考虑具有ሀ窄通道的问题时，应使⽤考虑到ሀ窄通道采样策略
不同采样⽅法的组合是可能的，并且在许多情况下能够提升算法的效率
连接策略
创建稀疏路标图
避免计算属于同⼀连接⼦图的边，将构型与每个⼦图中距离⾜够近的最近节点连接起来
连接连通⼦图
在某些情况下，路标图可能由⼏个连接的⼦图组成，可以通过采⽤旨在连接路标图不同组成 部分的策略来提⾼路标图的质量
特性
使⽤较少的随机点可获得解
较少的采样点就能100%覆盖可⾏空间
对于⼤多数问题：寻找到路径的概率随样本数量呈指数增⻓
⽆法检测到路径不存在
单⽬标采样规划算法
多⽬标查询规划算法
⽬标是创建⼀个路标图来捕获自由空间的连通性，然后⾮常快速地解决任意两个起点和终点的查询
试图尽可能快地解决路径搜索问题，⽽不是专注于探索整个自由空间
对于运动动⼒学规划⾮常有效
步骤
分别建⽴以起点和终点为根的两棵树
通过扩展两棵树向彼此⽣⻓
采样过程中选择靠近两棵树边界的区域采样
树合并过程中，局部规划算法尝试将两个树的两个构型进⾏连接
扩展空间树算法（EST）
基本思想
EST是⼀种采样式运动规划算法，旨在⾼效探索⾼维构型空间中的可⾏路径
其核⼼理念是通过在“稀疏区域”中优先扩展已有树结构，从⽽更容易穿越障碍物密集或瓶颈区域
流程
初始化：从起始状态开始构造⼀棵树（或图），初始节点为起点
节点采样：从当前树结构中按照某种“概率密度估计”选择扩展节点，倾向选择邻域稀疏的节点
状态扩展：从该节点出发，向随机采样的⽬标状态进⾏⼀次前向推进（如仿真动⼒学或简单线性推进），得到⼀个新的候选状态
可⾏性检查：判断新状态是否在⾃由空间中（⽆碰撞）如果是，则将其加⼊树中
终⽌条件：达到⽬标区域或满⾜最⼤迭代次数后终⽌
引导采样
理想情况下，应该选择函数使得采样构型构成包含起点和终点的连接子图，并相当均匀地覆盖自由空间
合并树
Ti（起点树）中⽣成新构型q
尝试将q 连接到 Tg（目标树）中 k 个最邻近的构型
如果连接成功，则合并两棵树
否则，将交换树并重复该过程指定的次数
优点
适⽤于⾼维空间，尤其在⾃由空间中稀疏或ሀ窄通道的情况表现优于随机快速扩展树（RRT）
利⽤密度函数引导搜索，提⾼穿越瓶颈区域的概率
易于实现，适配多种机器⼈系统和状态空间模型
缺点
样本效率较低，搜索过程中可能产⽣⼤量冗余节点
依赖于合适的概率密度估计⽅法，参数选取敏感
相⽐ RRT 类算法，路径质量不稳定、可重复性差
快速扩展随机树算法（RRT）
基本思想
快速扩展随机树（RRT）是⼀种典型的采样式运动规划算法，旨在快速探索⾼维空间中的可达区域。其主要思想是利⽤随机采样与渐进构树，使得树状结构⾃然“向前扩展”到未探
索区域
算法流程
初始化：从初始点开始构建⼀棵以其为根节点的搜索树
随机采样：在状态空间中均匀或按⼀定策略随机采样⼀个状态
寻找最近节点：在已有树中找到采样构型最近的节点
扩展节点：从 最近的节点向 采样节点沿着可⾏路径前进⼀个步⻓（即⽣成⼀个新节点，加⼊树中
⽬标检测：若新节点落在⽬标区域附近则认为找到路径
重复执⾏：直到达到终⽌条件，如时间限制或树命中⽬标区域
确定步长
⼀种可能的⽅法是基于已有构型和随机构型 之间的距离动态地选择 step_size
如果两个构型远未发⽣碰撞，那么为step_size 选择⼀个较⼤的值， 否则选择⼩步⻓
如果 step_size ⼩，则探索步骤短，树的节点靠得很近
基于⽬标的快速 RRT
改进的⽬的：解决随机选择采样点导致效率低的问题
引导采样
在均匀样本和偏向包含初始或⽬标构型的区域的样本之间交替
将随机构型点设置为目标函数点的概率为 p，或者随机⽣成随机构型点概率为1 − p 来⾃均匀分布
缺点
如果⽬标概率的不断加⼤，当障碍物遮挡⽬标时容易陷⼊局部搜索⽆法跳出
为了保持随机树对未知空间的扩展能⼒，概率通常不宜选择的过⼤(通常0.05 - 0.1)
RRT Connect算法
RRT Connect 算法从初始状态点和⽬标状态点同时扩展随机树从⽽实现对状态空间的快速搜索
步骤
随机⽣成⼀个随机构型
RRT 将⼀棵树向 随机构型扩展，获得了⼀个新构型qnew
尝试将另⼀棵树中距离 qnew最近的节点朝向 qnew扩展
如果成功（即终点起点已连接），规划算法终⽌
否则交换两棵树并重复该过程⼀定次数
优点
⽐ RRT 更快收敛：双向扩展+连接机制⼤幅提升搜索效率
结构简洁⾼效，易于实现和调试
适⽤于⾼维空间：⼴泛⽤于移动机器⼈、机械臂、⽆⼈机等
缺点
仍然不能保证最优性（除⾮结合 RRT* 的重接策略）
连接失败时退化为普通双向 RRT
路径质量需后处理优化（如平滑、简化）
RRT*算法
RRT算法虽然能快速地找到路径，但是得到的路径并不光滑，对机器⼈移动⽽⾔不是最优路径
重新为 xnew 选择⽗节点（选择半径为r）
降低起点到 xnew 的路径代价
将临近节点作为新节点的父节点
重新对随机树进⾏布线
尽可能的减⼩最终路径代价
将新节点作为临近节点的父节点
半径R对算法的影响
R太⼩：每次优化的邻居点很少，优化⼒度太⼩，路径类似与 RRT
R太⼤：每次优化的邻居点⾮常多(⽐如包括起点)，优化⼒度太⼤
优点
在⾼维空间中具有很强的探索能⼒
算法简单，适配多种动⼒学系统
可扩展性强，易与其他策略结合（如路径平滑、启发式
引导）
缺点
路径不最优，通常需要后处理优化
在ሀ窄通道区域的采样效率较低
原始 RRT 对采样策略敏感，导致性能不稳定
采样树路标图法（SRT）
采样树路标图法利⽤树规划算法的局部采样⽅案来填充类似 PRM 的路标图
路标图的节点是树
流程
在空间中随机产生几个构型，作为树的顶点
基于树规划进行扩展
每棵树（尤其是起点树和目标树）尝试和最近的邻居树合并，若起点和终点能连上同一颗树就视为连接成功
并⾏计算
SRT 的树可以彼此独⽴地⽣成。这种解耦使得 SRT 的并⾏ 实现成为可能
其他采样规划算法
基于控制的规划
多个机器⼈运动规划
⽆碰撞路径意味着在每⼀步机器⼈和障碍物之间或机器⼈和另⼀个机器⼈之间没有碰撞
两个问题
为每个机器⼈设计路径，使得机器⼈与障碍物⽆碰撞
不同路径之间需要协同，使得机器⼈之间⽆碰撞
两种规划⽅法
集中式规划
解耦规划
机械臂运动规划
基于控制的运动规划
基于PRM的运动规划
柔性机器⼈运动规划
难点是构型空间可能具有⽆限的
维度
单元分解法
引⾔
原理
单元分解是连续⾃由空间离散化的⼀种⽅法。它将⾃由空间划分为⼀定形状且数量有限的单元进⾏路标图的⽣成
关键点：将构型空间分解为单元使得单元内的路径不会与障碍冲突
单元分解有两种⽅法
近似单元分解：相对原⾃由空间分解有误差（离散化误差）
准确单元分解：相对原⾃由空间离散化分解⽆误差（精确）
邻接图
节点对应于⼀个单元格
边连接相邻单元格的节点
如果两个单元格共享⼀个公共边界，则它们是相邻的
基于单元分解的路径规划
规划步骤
确定包含开始和⽬标的单元格
在邻接图中搜索路径
梯形分解法
原理
分解单元采⽤多边形形状准确刻画⾃由空间（⼀般为梯形和三⻆形）
分解是完备的，但从路径⽣成看不是最优的
路标图⽣成
单元图是路标图的基
础
单元序列可⽣成⾃由路径（连续单元中点相连）
图的连通性表示单元
的邻接关系
分解是完备的，但从路
径⽣成看不是最优的
梯形分解构造
对所有拐角或折线处做分解
缺点
从根本上需要⼀个多边
形⼯作空间，这对于许
多应⽤程序来说并不是
⼀个现实的假设
莫尔斯单元分解法
区域覆盖路径规划
沿着⼀条路径覆盖空间
解决⽅法
简单地覆盖每个单元
使⽤邻接图确保每个单元都被访问并因此被覆盖
⽜耕法分解
在梯形分解构造的基础上，只有当前后两篇区域连通性发生变化时才会进行分解
可以通过来回运动覆盖每个单元格
基于传感器的覆盖算法
基于牛耕法的分解结果
覆盖⼀个单元格直到检测到闭合临界点
如果闭合临界点有与之关联的“未清理”单元格，选择⼀个并覆盖，重复
如果闭合临界点没有未清理单元格
在单元拓扑图中搜索关键点带有未清理单元格的点
规划到临界点的路径
覆盖单元格，重复
否则覆盖完成
近似单元分解法
特点
选择离散化栅格⼤⼩，进⾏等栅格划分
标注栅格
路径搜索
栅格细化
以防即使存在路径，也可能找不到路径
解决方案
区分完全包含在某些构型空间障碍物中的单元格和部分与构型空间障碍物相交的单元格
尝试使⽤当前的单元格集找到路径
如果没有找到路径：细分混合单元格再次尝试使⽤新的一组单元格
直到⼀些合理的单元格⼤⼩然后因失败⽽停⽌
格栅四树分解
对混合区反复进⾏四⼦区分解
每个⽅形单元分解为更⼩的⽅形单元
细化精度要求：所有单元均处在⾃由空间
这种分解⼀定存在⽆碰撞⾃由路径
优势
对障碍形状限制少
实际使⽤多
路径搜索速度快
缺点
与障碍间隙未知
权衡完整性和计算复杂度
仍然难以在⾼维度上使⽤
动力学约束下的运动规划
引言
由于系统的差分约束，状态对之间的直线连接通常不是有效的轨迹
规划是由粗到细的工艺
轨迹优化只进行局部的轨迹优化
状态栅格规划
基本概念
有很多方法来解决图搜索问题
假设机器人不再是一个质点
现在需要一个具有可行运动连接的图
手动创建（构建）一个包含机器人所有可执行的边的图
正向：在控制输入的空间中离散样本
反向：在状态空间中离散样本
是所有动力学规划的基本动机
状态网格规划是最直接的规划
控制空间采样vs状态空间采样
控制输入空间采样
选择u，确定时间长度T，对系统进行正向模拟（数值积分）
特点
正向模拟
固定u、T
无任务引导
容易实施
规划效率低
计算方式
状态s=(x,y,θ)
控制输入:u=(v,ϕ)
运动学方程[x',y',0']=[vcos0,vsin0,(r/l)tanQ]
步骤
在T中选择一个s∈T
选择v、ϕ和tau
对来自s的运动进行积分
如果无碰撞，则添加结果
状态空间采样
选择sf，找到s0和sf之间的连接（轨迹）
特点
逆向计算
需要计算u,T
良好的任务指导
难以实施
规划效率高
步骤
构建栅格图
给定一个原点
为原点周围的8个相邻节点找到可行路径
向外延伸至24个邻居
完成网格
对于两层栅格图
初始状态不同时，只有第一层状态不同
两种方式对比
状态空间采样轨迹在初始角v方向更密集
对于几种不同的输入，输出结果非常相似
边界值问题（BVP）
边界值问题是状态采样网格规划的基础
没有通用解决方案,针对具体问题设计
通常需要进行复杂的数值优化
最优边界值问题（OBVP）
目标：最大限度的减小变加速度的积分
混合A*算法
节点扩展：连续状态与运动原语
状态表示：节点包含位置（x,y）和方向（θ），形成连续状态空间
运动原语（Motion Primitives）：预定义的车辆运动模型（如前进 + 左转、倒车 + 右转），模拟真实转向半径和轨迹
启发式函数：双重引导
非完整启发式
快速逼近目标
障碍物启发式
避开障碍区域
路径生成与优化
Reeds-Shepp 曲线：在开放区域直接连接节点，减少搜索步数
闭环优化：搜索完成后，对路径进行平滑处理（如梯度下降），消除冗余节点
步骤
初始化
节点扩展
弹出代价最低的节点
生成运动原语对应的子节点，计算实际代价（g 值）和启发式代价（h 值）
碰撞检测
更新列表
终止条件
路径提取
反向追踪父节点，应用 Reeds-Shepp 曲线优化
定义修剪规则
使用网格图
基于动力学的RRT*
步骤
初始化
随机采样
在状态空间中采样目标状态（x, y, θ, v, …），或结合控制输入生成可达状态
最近邻搜索
在树中找到与目标状态最近的节点（需定义状态距离度量，如欧氏距离+角度权重）
轨迹扩展
施加控制输入，通过动力学模型（如车辆运动学方程）生成局部轨迹
轨迹需离散化检查碰撞和约束
代价优化
终止条件
如何采样
要求在全状态空间采样，而不是在欧几
里得空间（如 RRT）采样
距离定义
没有运动限制，可以使用欧氏距离或
曼哈顿距离
有运动约束条件的状态空
间中，可采用最优控制计算最近的结点
通常采用时间-能量最优的二次方形式
选择父节点
对状态进行随机抽样，就可以计算出从树中的状态节点到抽样状态的控制策略和成本
查找附近的节点
设定一个代价容忍度 r，就可以计算出 x_ran
所能到达的状态的边界，以及以小于 r 的成本到达x_rand 的状态的边界
重新接线
在” 重新布线” 时，我们会计算 x_rand
的前向可到达集，并求解 OBVP
解耦轨迹规划
时间标度 s(t) 是一个函数 s : [0, tf
] → [0, 1] ，相当于每个时间 t ∈ [0, tf
] 分配一个 s 值
路径和时间标度一起指定轨迹 q(s(t))，或简称 q(t)
时间标度 s(t) 对于所有 t ∈ (0, tf
) 应该是二次可微且单调的 ˙s(t) > 0
s(t) 的二次可微性确保加速度 ¨q(t) 是有界的
最优控制
对一个动力学系统，在考虑障碍物、执行器约束、动力学约束和边界条件的基础上求解，并可以找到一个最小化某个目标函数 J 的轨迹
集群机器人运动规划
多机器人路径规划（MAPF）
k个智能体a1,a2,···,ak,，每个智能体都有一个起始位置si∈V和一个目标位置gi∈V
目标
为所有智能体找到无碰撞路径
使其运动时间之和最小
基本动作
移动：移动到邻近位置
等待：在当前位置等待
两个操作都需要一个时间步长，成本为一个单位
冲突
顶点冲突：两个智能体在同一时间点停留在同一位置
边冲突：两个智能体在同一时间步以相反方向穿越同一条边
基于规则的次优解算器
推动和交换
具有较高优先级的智能体s将智能体r从三度边v推开
被推开的点的度至少为3
非完备，非最优
分层协同A*算法
原理
智能体按照某种预定顺序逐个进行规划,将其停留的顶点标记为在该时间点被占据
同样具有优先级,高优先级的智能体先规划
易于部署,在传统A*算法基础上进行简单改进就可以
非完备、非最优
单一智能体A*算法
将所有智能体视为单一的”联合智能体”
状态空间包括将k个智能体放置在V个位置的所有排列组合
扩展性差（指数爆炸）
独立性检测(ID)
ID法试图检测独立的智能体群体，在这些群体中，每个群体都有一个最佳解决方案，使两个解决方案不发生冲突
M∗：增强型单一智能体A*算法
主要思路
对有冲突的智能体向所有方向扩展子节点，无冲突的智能体只扩展一个子节点
一般来说，节点只扩展一个子节点，每个智能体都在该节点上进行最优移动
当节点n的q个智能体之间发生冲突时，M*会从n追溯到所有父节点，直到根节点，并将所有这些节点放回OPEN集
然后，q个冲突智能体将扩展其所有分支
与ID算法类似
基于冲突的搜索(CBS)
CBS是一种双层搜索算法，能以最佳方式求解MAPF
下层运行A*为单个智能体规划路径
上层在约束树上运行A*来解决碰撞问题
增强型CBS（ECBS）
根据策略（函数）f1在OPEN中选择一个有希望的子集作为FOCAL
根据另一个策略（函数）f2从FOCAL中选择最佳节点进行评估
优点
有界最优
更快的搜索
速度障碍（VO）
VO(速度障碍)
控制环路
感知其他机器人的
位置和速度
计算VOA|B与其他机器人B之
间的关系
使用适当的策略（如线性规划）选出新速度
将速度应用于机器人执行器/更新位置
缺点
会产生振荡
RVO(互易速度障碍)
主要思路
每个智能体对速度变化承担有限的责任。我们选择的新速度v′A是其当前速度vA 和速度障碍外的速度v的平均值
RVO是一条决定每个智能体应承担多少责任的规则
ORCA(最优互避碰撞)
主要思路
RVO的基础上让每个智能体只需承担一半责任
鸟群模型
每个智能体都受到三种力的控制
短期：对邻居和障碍物的排斥力vtextrep
中期：速度对齐力vtextfrict，使运动与邻居对齐
长期：对目标的吸引力vfock
合力是上述力的和
优点
这一策略很好地模拟了自然动物群落的运动
缺点
性能对参数设置很敏感
候选解决方案
使用进化策略自动调整参数
蜂群的轨迹规划
基于优化的轨迹规划
最优控制理论为基础的优化规划
标准互避碰撞惩罚：避免在任何时间点上彼此靠得太近
互避公式
离散化
安全性低
计算效率高（简单）
不保守
凸面表示法
安全
计算昂贵（复杂）
保守
优化工程策略
多目标优化
安全检查
防撞工程策略
离散化
安全检查
工程战略
系统架构
编队
一致性算法
一致是指所有智能体从任意初始状态出发，通过与相邻智能体协商，就某些特征达成一致
基于位置偏差的编队
要求：拓扑网络连通
属性：平移不变性
基于距离的编队（梯度控制法）
要求：拓扑网络具有刚性
属性：平移和旋转不变性
虚拟结构法
虚拟刚体
将整个编队视为一个刚体
远程操作 -> 虚拟刚体 -> 智能体
虚拟结构 + 势场（矢量场）（由多个独立的矢量向量场叠加得到）
编队重构
队形相似度指标
性质
平移、旋转和缩放不变性
平移和旋转不变性：该度量基于欧氏距离
缩放不变性：拉普拉斯矩阵通过度矩阵进行归一化
可以实现更灵活的编队机动，例如通过走廊
智能体位置可变
构型空间（？）
引言
如何指定机器人的位置，避免机器人与障碍物碰撞
需要多少信息才能完全指定机器人上每个点的位置
这些信息应该如何表示
这些表示的数学特性是什么
在规划机器人路径时如何考虑机器人世界中的障碍物
机器人的构型空间
构型空间
机器人系统的构型是该系统每个点位置的完整描述
机器人系统的构型空间是指系统所有可能构型所组成的空间
用 q 来表示构型, 用 Q 表示构型空间
工作空间
机器人的工作空间定义为机器人实际运动的空间，也就是二维、三维欧几里得空间
障碍物与构型空间
构型空间障碍物
机器人沿障碍物边缘移动的边界
自由构型空间
除去障碍物与构形空间的边界后的区域
自由路径
构型空间的拓扑
拓扑
拓扑是空间的“内在特征”
如果需要剪切和粘贴以使两个空间相同，则两个空间具有不同的拓扑
如果我们可以拉伸并且在不撕裂的情况下“连续”重塑，一个变成另一个，它们具有相同的拓扑结构
讨论拓扑结构的一个基本数学机制是同胚
研究构型空间拓扑的目的
拓扑会影响对构型空间的表示
一种适用于拓扑空间A的路径规划算法，可能迁移到同构的其他拓扑中
映射（对于映射φ:S→T）
如果 φ 的使得 T 中的每个元素与 S 中的至多一个元素对应，则 φ 是单射
如果 T 的每个元素在 S 中都有对应的原像，则 φ 是满射
如果 φ 既是满射又是单射，那么它是双射（存在反函数）
如果所有阶的导数都存在，则 φ 是平滑的
同胚和微分同胚
若φ: S → T 为双射，且 φ和 φ^−1连续，则称映射 φ 为同胚的；如果存在这样的 φ ，则 S 和 T 是同胚的
如果φ 和φ^−1都光滑的同胚是微分同胚
局部同胚和微分同胚
如果对于任意 p∈S 都有一个包含它的邻域与空间 T 的某个邻域微分同胚（同胚），则称空间 S 和空间 T 局部微分同胚（同胚）
S^1 对 R^1局部微分同胚
球面对平面局部微分同胚（环面也是）
流形和可微流形
如果集合 S 对 R^k 局部同胚，则它是一个 k 维流形
坐标卡是一个二元组 (U,φ)， U 是 k 维流形中的一个开集，φ 是从 U 到 R^k 中某个开集的微分同胚
许多流形需要多个坐标卡来覆盖
图册是一组坐标卡
覆盖流形
在它们重叠的地方是平滑的
如果存在从 S 到 R^n 的图册，则集合 S 是维数为 n 的可微流形
常见流形
R^1×R^1×⋯×R^1=R^n
S^1×S^1×⋯×S^1≠S^n （ =T^n，n 维环面）
S^n 是 n 维球面
尽管 S^n 是一个 n 维流形，但它不是单个坐标卡的流形
平面内平移机器人
R^2
平面内平移和旋转的机器人
SE(2)或者R^2×S^1
三维空间中平移的刚体
R^3
航天器
SE(3)或者R^3×SO(3)
一个 n 关节旋转臂
T^n
一个附有 n 关节机械臂的平面移动机器人
SE(2)×T^n
构型空间的维度
维度
指的是构型空间的维数，或者确定构型所需的最小参数个数
确定维度的方法
显式生成参数化
从所有参数开始并添加（独立）约束
平面机器人（可抽象为点）
构型可以由两个坐标给出：q=(x,y)∈Q=R^2
即该机器人有两个自由度；构型空间是二维的
平面机器人（不可抽象为点）
假设机器人是一个平面刚体，可以在平面内平移和旋转，故还需要一个额外的旋转参数
即该平面机器人具有三个自由度
刚体构型的矩阵表示
采用矩阵的原因
矩阵乘法可用于变换参考系或对构型空间进行旋转和平移
矩阵组 SO(n) 描述刚体在 n 维空间(n=2, 3)中的方向，矩阵组 SE(n) 描述其位置和方向
几何变换（矩阵组 SO(n) 和 SE(n) 可用于）
当旋转矩阵用于表示构型时，我们通常称其为坐标系
表示刚体的构型
更改构型或点表示的参考系
当它用于位移或坐标变化时，我们通常称其为变换
变换（移动）构型或一个点
构型空间的变换
雅克比矩阵
通过雅可比矩阵可以将机械臂末端位置/速度和各关节角度/角速度联系起来
势函数法
引言
想象机器人是一个正电荷
目标是负电荷
障碍物为正电荷
势函数
沿着势函数的负梯度方向可以使能量最小化最快
每个时刻机器人都会根据当前位置的负梯度方向前进
吸引势函数
锥形势函数
二次型势函数
排斥势函数
针对每个障碍物定义距离
因最近的障碍物可能变化，因此排斥势函数可能不连续（突变）
势函数叠加
直接相加
梯度下降
梯度下降定义为沿着梯度减小最快的方向
临界点 q∗ 定义为梯度为0的点
海森矩阵
Hessian 矩阵是势函数的二阶导数
如果 Hessian 矩阵是非奇异的 (Det(H) ≠ 0)，则临界点是唯一的
如果 H 是正定的，则存在最小值
如果 H 是负定的，则存在最大值
如果 H 不定，则存在鞍点
Brushfire 算法
原理
需要在空间上定义一个网格
需要定义网格的连通性
四连通
八连通
使用网格来近似距离
流程
将构型空间划分为网格，其中有障碍物的网格标记为1，其他网格标记为0
创建一个队列 L，初始化为与障碍物相邻的所有网格
当L非空时
取出队列 L 中的元素 t
如果该格标记为0
令该格标记变为：与之连通的网格的最小的标记值+1
将所有标记为0并与标记不为0连通的网格加入队列 L
当所有网格都具有指定值时，结束程序
波前传播算法
局部最小值
势场中存在局部最小值（排斥梯度和吸引梯度相等），机器人移动到该点后无法离开
波前传播算法是从目标点开始的 Brushfire 算法
将构型空间划分为网格，其中有障碍物的网格标记为 1，其他网格标记为 0
将目标点标记为 2，创建一个与目标点相邻的网格的队列 L
从目标开始，其相邻单元（连通）在当前单元基础上加1，直到没有“0”值单元存在
最短路径按照标记从高到低选择
波前传播算法同样适用于三维环境
连通性
六个面
二十六点（3*3*3-1）
更高维度的波前规划器的实现在计算上变得难以处理
波前传播算法的优缺点
波前规划器实质上在具有一个局部最小值的网格上形成一个势函数，因此算法是完备的
能够获得最短路径
所规划的路径离障碍物太近，成熟性不足
规划器必须在整个空间中搜索路径，比较耗时
导航函数
定义一个函数为导航函数要求
是光滑的
在构型空间中有唯一的最小值 qg
在自由空间的边界上取相同的最大值
是 Morse函数（Morse函数：如果函数中每个临界点（梯度为零的点）都是孤立的）
非欧几里德空间中的势函数
工作空间和构型空间中的力的关系
在（工作空间）欧几里得空间设计势函数
将工作空间的势函数转换到构型空间
子主题 8
图搜索算法
引言
路径规划的两个步骤
构造图
构造⼀个描述运动规划问题的图
常⻅的⽅法包括单元分解法、路标图法等
图搜索
在图中搜索⼀条（希望接近最优的）路径
本章的主要内容
图的基本概念
图
G(V, E)
⽆向图
赋权图
有向图
顶点集
V = {1, 2,...,n}
边集
E = {(i, j)} 包含于 V * V
邻居集
Ni = {j | (i, j) 属于 E}
路
(i1, i2),(i2, i3),...
连通图
图中任意两个顶点之间
存在着⼀条路
图的表示
邻接链
邻接矩阵
图构造
路标图法
单元分解法
图搜索算法
⽆信息搜索
节点扩展不使⽤其它启发式的知识
仅有搜索问题的知识
节点扩展只有如深度类特征
常⻅的⽅法包括⼴度优先算法和深度优先算法
有信息搜索（启发式搜索）
节点扩展使⽤启发式知识
利⽤特定知识进⾏节点扩展选择
利⽤启发式函数估计到⽬标点的最短路径
常⻅的⽅法如A*、D*算法
图搜索算法基本流程
创建容器：⽤⼀个容器⽤来存放将要访问的节点
初始化：初始时刻容器仅包含起始点
进⾏循环
弹出：根据特定的函数从容器中选取下⼀个访问的节
点
扩展：搜索所访问节点的所有邻居，并将这些节点加
⼊到容器中
结束循环
深度优先算法
基本思路
先选择某⼀种可能情况向前（⼦结点）探索
⼀旦发现原来的选择不符合要求，就回溯⾄⽗结点重新
选择另⼀结点继续向前探索
如此反复进⾏直⾄求得最优解
深度优先搜索可⽤递归法实现
深度优先的搜索策略是尽可能“深”地搜索树
深度优先搜索算法遍历
深度优先搜索在回溯和搜索另
⼀条路径之前搜索⼀直到叶⼦
的路径
深度优先搜索算法实现
使⽤堆栈（先入后出）来实现深度优先搜索算法，堆栈是⼀种先进后出的容器
深度优先搜索算法步骤
⾸先将根节点放⼊堆栈中
2，从堆栈中取出第⼀个节点，并检验它是否为⽬标
如果找到⽬标，则结束搜寻并回传结果
否则将它某⼀个尚未检验过的⼦节点加⼊堆栈中
重复步骤2
4，如果不存在未检测过的直接⼦节点
将上⼀级节点加⼊堆栈中
步骤2
重复步骤4
若堆栈为空，表示整张图都检查过了——亦即图中没有欲搜寻的⽬标。结束搜寻并回传“找不到⽬标”
⼴度优先算法
基本思路
⾸先访问初始点Pi，并标记为已访问
接着访问Pi的所有未被访问的可达邻居点，并均标记为已访问
然后再按照访问的次序，访问每个未被
访问过的邻接点，并均标记为已访问
依此类推直到访问完与初始点Pi相通的所有顶点为⽌
⼴度优先搜索类似树的按层遍历
⼴度优先搜索
⼴度优先搜索 (BFS) 在探索更
远的节点之前先探索距离根最
近的节点
⼴度优先搜索算法实现
为了实现波状推进搜索特性，BFS采⽤队列（先进先出）作为容器的数据结构
⼴度优先搜索算法步骤
⾸先将根节点放⼊队列中
2.从队列中取出第⼀个节点，并检验它是否为⽬标
如果找到⽬标，则结束搜寻并回传结果
否则将它所有尚未检验过的直接⼦节点加⼊队列中
若队列为空，表示整张图都检查过了——亦即图中没有欲搜寻的⽬标。结束搜寻并回传“找不到⽬标”
重复步骤2
深度优先与⼴度优先⽐较
⼴度优先搜索可找到最⼩深度的⽬标点
深度优先搜索⽬标点不⼀定深度最⼩
⼴度优先搜索存贮量要求⼤
深度优先搜索耗时⻓
启发式搜索算法
启发式搜索
在搜索中，如果计算当前节点与⽬标点的某种代价估计值，则称为启发式搜索。
搜索总是从⽗节点开始向⼦节点进⾏扩展，每次搜索只有⼀个⽗节点 ，以免陷⼊死循环
搜索算法实际上是根据初始条件和扩展规则构造⼀棵“解答树”并寻找符合⽬标状态的节点的过程
所有的搜索算法从最终的算法实现上来看，都可以划分成两个部分—弹出和扩展
贪婪最佳优先算法(GBFS)
使⽤的是优先队列
普通队列是⼀种先进先出的数据结构
优先队列中元素被赋予了优先级，最⾼优先级元素优先出列，也就是“先进，最优的先出“
GBFS算法作为⼀种启发式搜索算法，使⽤启发式评估函数ℎ(n)来作为代价函数，也就是
f（n）= ℎ(n)
ℎ(n)为当前节点到⽬标点的最短距离（欧⽒距离或者曼哈顿距离）
Dijkstra算法
流程上与启发式搜索类似，只是代价函数变为从起点到当前点的移动代价
原理
初始时，S只包含起点s；U包含除s外的其他顶点,且U 中顶点的距离为“起点 s到该顶点的距离”
从U中选出离扩展结点距离近的顶点k，并将顶点 k加⼊到 S 中，同时从 U 中移除顶点k，并且将扩展节点标记为已扩展
更U中各个顶点到起点s的距离
重复上述步骤，直到遍历完所有顶点
A*算法
原理
A*算法也是⼀种启发式算法，它的代价函数
为启发式搜索算法与Dijkstra算法的和
⽤于找到有希望遍历的节点的信息
可接受→永远不要⾼于实际路径成本
A*算法步骤
将起始点加⼊开放列表O（设置⽗亲节点为空）
在 O 中选着⼀个F值最⼩的节点作为当前节点
将其添加到封闭列表C中，并从O中移除
如果为终点节点，那么结束搜索
处理当前节点的所有邻接节点
如果不在O中，那么就将其添加到O中
如果已经添加到O中，重新计算G值，如果G值⼩于先前的G值，那么就更新此值(同时跟新⽗节点)
如果该节点不可通过或者已经被添加到C中，那么不予处理
如果O不为空，重复上述步骤继续执⾏
查看更多
相似思维导图模版
首页
我的文件
我的团队
个人中心