运动规划最全简介

http://blog.csdn.net/jiakeyouwe/article/details/53462920

-你的博士课题是关于什么的？

-机械臂运动规划。

-运动规划不是已经很成熟了吗，还有什么可研究的？

-是运动规划（Motion Planning），并不是轨迹规划（Trajectory Planning）。

Balabala…

上述对话曾多次出现在我与其他机器人从业人员的对话中。所以，我觉得有必要聊聊什么是运动规划了。但，或许是

因为暑假在家比较懒散，又或许是写课题相关的东西会产生“论文拖延症”，这篇推送足足拖了三周才开始写。

是什么

简单地说，运动规划就是在给定的位置A 与位置 B 之间为机器人找到一条符合约束条件的路径。

具体的案例可以是为移动机器人规划出到达指定地点的最短距离，或者是为机械臂规划出一条无碰撞的运动轨迹，从

而实现物体抓取等。

△ 嗯，这是图例

　　当然，有些人喜欢将前者称为路径规划（Path Planning）。但是，后面讲完构形空间（Configuration Space

，如果会讲的话），我们就会发现，其实两者是完全相同的问题，只是规划的空间不同而已（一个是三维欧式空间

，一个是高维流形）。

      看到这，不明真相的吃瓜群众们就坐不住了：“不就是随便抓瓶水这么简单的事嘛，有什么难的？电影上的机器

人都这么高级。”

　　对的，就算是量子卫星已经升空的今天，我们也不能很好地解决让机器人拿瓶水这么简单的任务。目前绝大多数

的工业机器人工作都是通过人工一个点一个点示教实现的，而服务机器人更是只局限在实验室中。

　　机械臂运动规划之所以这么难，主要是因为规划问题的维度太高（具体后面分析），目前暂无兼顾实时性与最优

性的规划算法。

为什么要研究运动规划？

这个问题其实没什么好说的，你要想让机器人能帮你拿瓶子、做饭、收拾屋子等，就必须赋予机器人快速生成无碰撞

、最优运动轨迹的能力。

当然对于工业界，他们还有更明确的需求。

△ the Gap between Industry Robotics and Research Robotics

　　先上个图，好像是在ROS-I内部材料里看到过，但找不到了，所以大致画了一个。

　　从图上可以看出，目前工业机器人灵活性普遍很差，基本依靠人工示教，也难以应对订单变化等情况，而学术研究

却主要集中在更加灵活的机器人应用上。工业应用与学术研究中间存在巨大的缺口。

△ 汽车生产流水线

目前工业机器人主要用在汽车行业。一种车型往往能够生产好几年，每部车的利润也较为可观，所以一开始花点时间

给机器人示教也没关系。但是，现在机器人厂商们想开始切3C产业（结合电脑、通讯、和消费性电子三大科技产品整

合应用的资讯家电产业）的蛋糕，于是问题就出来了。手机等产品的更新换代周期极短，且单个产品收益远不如汽车，

为了能快速调整生产线，以应对新款产品，传统的人工示教方法就显得有点力不从心了。

目前工业机器人虽然已经很成熟了，也有很多应用实例，但是也存在不少问题：

①人工示教费时费力；

②无法应对变化环境；

③更新流水线成本高；

④应用场景较为简单。

随着手机等3C产业（更新换代快）的发展，人工费用的上升，工业界开始追求一种更加灵活的机器人：要是机器人可以

进行任务层次的编程（例如“抓取A放到B”），而不是手把手示教（从A-->001-->002-->张开手爪--->200-->...-->

关闭手爪-->...--->666--->B-->打开手爪），那岂不是太amazing了！

正如我之前在知乎问题“mujin 工业机器人设计公司，他的优势在哪里，在算法上获得了哪些突破？”下的回答所描述的那

样（具体内容点击文末阅读原文查看），运动规划能够实现任务层次的高级编程，而不需繁复的人工示教过程。

怎么样

这部分主要整合了Coursera上宾大的公开课《Robotics: Computational Motion Planning》、《Planning Algorithms》

、《Principles of Robot Motion Theory, Algorithms, and Implementations》、本人去年开题报告等内容。

先说明一下，运动规划算法通常有两个评价指标：

• 完备性Complete：利用该算法，在有限时间内能解决所有有解问题；

• 最优性Optimality：利用该算法，能找到最优路径（路径最短、能量最少等）；

1、从游戏说起(2D)

运动规划算法的应用领域非常广，除了机器人外，另一个很大的应用领域就是电子游戏了。

△ 红警2：尤里的复仇，游戏截图

在玩像红警（暴露年龄系列）这类的即时战略游戏时，我们往往只需为游戏单位指定一个目标点，游戏中的人物就能够绕过

障碍物自动走到目标点。要想实现这一功能，这里面就需要应用到运动规划算法。为了简单，我们先假设我们要规划的机器

人/游戏人物在地图上只是一个点，尺寸为0。

• Walk To算法

  最简单的“算法”应当数Walk To算法了，顾名思义，就是让机器人直接从当前位置朝目标点走，直到到达目标点为止。

△ Walk To算法示意

　　一些RPG游戏（如同样暴露年龄的传奇）就采用了这种简单的算法，用户通过鼠标点击给定人物前进方向。

△ 热血传奇游戏截图

　　显然，这个算法找到的路径是最短的（最优性），但无法应对路径上有障碍物的问题（不完备），全凭玩家操作

避开障碍物。

• Bug算法

  Bug算法就是为了应对一些简单的障碍物而提出的，其说明如下：

  沿着起始点与终点的连线M运动；

  遇到障碍物，则绕着障碍物顺（或逆）时针运动，直到回到连线M。
  

△ Bug算法示意

显然，这个算法就是传说中的二维迷宫通用解法。所以这个算法在二维空间内是完备的，但其路径不能保证最优。

• 　　蚁群算法

  　　蚁群算法（Ant Colony Optimization）其实是一种优化算法，但完全可以直接用到路径规划问题上。这种算法

• 能够得到既完备又最优的路径，但由于收敛速度不快，并未得到太广泛应用。

△ 蚁群算法示意图，出处如图

• 人工势场法

  势场对应着能量分布，最常见的势场就是重力场了，我们在不同高度会拥有不同重力势能。斜面上的球会自然沿着

  受此现象的启发，人们便想到人工势场法，人工添加势场函数，让目标点处于谷底，距离目标点越远的势场越高，

• 同时，为了避免发生碰撞，可以在障碍物四周添加排斥势场（障碍物处势能最大）。

• 斜面往下滚。

△ 人工势场示意图，出处Coursera

人工势场非常直观，且对运算量要求不高，可以跟机器人的控制相结合。实验室的师兄们曾在中型组足球机器人比赛

中使用过这个方法。

当然，势场法的一个问题就是没办法避开局部极小值问题，所以该方法是不完备的，同时也是非最优化的。

====图搜索介绍 分割线====

另外有一大类算法则是先将运动规划问题转换成图（graph），之后利用各种图搜索算法解决问题。这里简单介绍一下

图搜索算法。

图是图论（Graph Theory）里的一个概念，它表示一类用若干离散节点（vertices、node、points）与连接节点的边

线（edges，lines，arcs）表示的拓扑结构。

△ 著名的邮差问题就可以转化成一个图论问题

对于在一个图上寻找到一条最短路径的问题，图论中已经有很多方法了，其中在规划领域最著名的两个分别是

Dijkstra算法和A*算法。

△ Dijkstra和A*算法伪代码

上图给出了两种算法伪代码，按照这个说明基本可以自己写出两种算法了。下面动图则是我利用Dijkstra和A*算法做

的效果演示动画。因为A*算法加入了启发函数，用于引导其搜索方向，所以大部分情况下A*算法会比Dijkstra算法规

划速度快不少。

△ Dijkstra和A*算法演示

====图搜索介绍 分割线====

要将图搜索算法用在运动规划里，就必须想办法将规划问题转换成图。

• Visibility Graph可视图法

  可视图法用封闭多面体描述障碍物；并将连接所有多面体的所有顶点，去除与障碍物相交的连线；之后将起始点与

• 目标点与所有顶点连接，并去除与障碍物相交的连线。由此可得到一个图。

△ 可视图

之所以叫做可视图，大概可以这样理解：站在某个顶点上，环绕四周，把你能看到（无障碍物）的顶点连接起来。可以

证明，该方法完备且最优。

• 空间离散法

  空间离散法就更简单了，按照某一尺寸划分网格，包含障碍物的网格认为不可通过，这样便得到一个网格图，之后

  显然如果网格尺寸太大的话，可能会造成连通路径堵塞，因此该方法是分辨率完备（Resolution Complete）且最优的。

• 按照四连通或八连通的方法得到一个图。

△ 空间离散法示意图

这个算法在即时战略游戏里面用得非常多，虽然我不知道红警是不是这样实现的，但我知道开源红警OpenRA（openra.

net）是采用的空间离散+A*算法（github上有源码）。

△ 开源红警OpenRAve截图，出处见水印

• 随机路图法PRM

  随机路图（Probabilistic Road Maps，PRM）就是在规划空间内随机选取N个节点，之后连接各节点，并去除与

  显然，当采样点太少，或者分布不合理时，PRM算法是不完备的，但是随着采用点的增加，也可以达到完备。所

• 以PRM是概率完备且不最优的。
  

• 障碍物接触的连线，由此得到一个随机路图。

△ PRM算法示意图

除了上述方法外，还有很多其他构建搜索图的方法，如Voronoi图法、Cell Decomposition等。

• 快速扩展随机树法RRT

  除了基于图搜索的方法，还有另外一大类基于树状结构的搜索算法，其中最著名的就是快速扩展随机树法

  RRT算法是从起始点开始向外拓展一个树状结构，而树状结构的拓展方向是通过在规划空间内随机采点确

• 定的。与PRM类似，该方法是概率完备且不最优的。

• （Randomly Exploring Randomized Trees，RRT）了。

△ RRT算法示意图

2、拓展到机械臂(CSpace+多维)

　　机械臂与平面机器人的区别主要在两个部分，一个是规划空间不同，另一个是机械臂往往具有更高的自

由度。

• 构形空间C-Space
  

以上算法都将机器人看做一个点，要想对机械臂进行规划，我们就应该想办法将机械臂用一个点来描述。

于是，我们就要简单说一下构形空间（Configuration Space，或C-Space）了。

构形空间，顾名思义就是与机器人构形相关的空间了。

对于平面移动机器人，由于它具有一定尺寸，所以也不能直接当做点来处理。直观的看，如果我们把机器人

当做一个点，就应该相应地将障碍物进行膨胀，这个膨胀处理的拓扑方法叫做闵科夫斯基和（Minkowski Sum）。

△ 平面机器人的C-Space

但是，机械臂的构形空间就没办法简单地用闵科夫斯基和来处理了。我们知道，用广义坐标（通常为各关节角

度）可以将机械臂用一个点描述，如六自由度机器人可用六维向量空间的一个点(θ1,θ2,θ3,θ4, θ5, θ6)描述。

但是，相应的障碍物的描述就比较麻烦了，由于机械臂逆解存在多解和奇异等问题，所以从工作空间到构形空

间的映射是非线性的，目前没有很好的方法将工作空间的障碍物直接映射到构形空间。对此，一般做法是对构

形空间离散化，对构形空间的每个网格判断是否存在障碍物。

△ 两自由度机械臂构形空间

当然，上图只是将机械臂的构形空间投影到了平面上，由于机械臂旋转关节的连续性（0=2π），所以机械臂的

构形空间并不是如上图所示的平面，而是类似一个圆环面。

△ 两自由度机械臂构形空间

　　可以看到，这个圆环面（本身只有两自由度）是嵌入到三维空间的。对于嵌入的概念，可以这样理解：

就像是三维宇宙中被二向箔二维化后的太阳系。三自由度以上机械臂的构形空间就无法画出来了。

　　由于构形空间的拓扑结构发生了根本性的变化，所以研究人员第一个需要解决的问题就是上述这些规划

算法在构形空间内是否还有效。但是，由于①做机器人的精通太高级数学的并不多；②大部分机构（连续旋

转关节、平动关节等）形成的构形空间均是微分流形，在任一点的局部均与欧式空间同态，也就是说大部分

算法都可以用。所以，在机器人运动规划方面较少看到算法完备性等数学证明。

△ 当然，这种机构的构形空间就不是微分流形了

　　对于二自由度机械臂，上述大多数算法都是可以直接使用的：

△Bug算法在两自由度机械臂中的应用，出处如图

△Dijkstra算法在两自由度机械臂中的应用，出处Cousera

• 　　高维度

　　高维度将会带来两个问题：①求解计算量增加；②障碍物无法描述。要看一个规划算法能否用在机械

臂，就应该看它能否应对这两个问题。我们一个个看。

　　Walk To：这个算法当然在任何维度下都能直接使用，当然，它完全无法应对障碍物等情况，我们甚

至不把它称为规划算法。

　　Bug：因为Bug算法需要绕障碍物顺（或逆）时针旋转，高维空间不存在这样的方向，所以自然是无

法使用的。

　　蚁群算法：依旧是收敛慢，用的人不多。

　　人工势场法：由于势场法需要在C-Space中构建人工势场（与障碍物在C-Space中的分布有关），所

以经典的人工势场法无法直接用在高维机械臂上。

　　当然，研究人员想出了解决方法，例如在机械臂上放几个控制点（Control Pointss）；在工作空间建

立势场函数后，对每个控制点施加势场力；最后通过力与雅克比的关系（可由虚功原理推导出），将控制

点处的力转换到每个关节力矩；关节力矩就是构形空间的势场力。

△基于控制点的势场法，图片出自本人论文

Hand-eye coordinative remote maintenance in a tokamak vessel

　　对于人工势场法，Khatib是这领域的大拿，他将其用于人形机器人的运动规划。

△势场法规划机器人，图片出自Khatib课件

　　人工势场法在多自由度机械臂中实时性好，可以与控制算法结合在一起。但是，依旧是既不完备也不

最优，所以很有可能遇到无法求解的情况。

　　可视图法：与Bug算法类似，可视图法需要确定障碍物在C-Space中的分布情况，因此无法用在高维

空间中。

　　空间离散法：理论上讲，空间离散法可以适用于任何维度的空间，但问题是它会引入很大的计算量。

例如对于一个六自由度机械臂，我们按照6°分辨率（已经是很低的分辨率了）划分网格，那么将会产生

60^6=4.67×10^10个网格，单是对每个网格进行碰撞检测（如果碰撞检测速度为0.1ms），就需要1296

小时。除了碰撞检测，规划算法的计算量也是随着空间维度指数增长的。所以一般大于3自由度的规划问题

，空间离散法就不具有实际应用价值了。

　　概率路图法PRM和快速拓展随机树法RRT：这两种算法的相同点是，他们均不考虑障碍物在C-Space中

的分布情况，而是采用碰撞检测函数对几个随机采样点进行碰撞检测。因为被催着要去吃饭了，所以直接说

结论，PRM和RRT算法在高维空间中都可以使用，且满足概率完备性（非最优），规划速度相当快。

△用RRT规划的机械臂

虽然基于采样的规划算法速度很快，但他们也有致命的缺点，那就是由随机采样引入的随机性。利用RRT和

PRM算法进行运动规划，用户无法对规划结果进行预判，每次规划的结果都不一样，这就使得自动规划的机

器人无法进入工业领域（极端追求稳定性）。

△RRT每次规划结果不同

所以目前规划领域也主要集中在对PRM和RRT的改进上，大家都想要尽可能解决这类算法的不确定性，甚至

能实现一些优化目标，如RRT*，Informed-RRT*，SBL等。

当然，这篇推送真的只能算是运动规划入门，还有很多坑没提到：例如如何均匀采样、如何做碰撞检测、如何

快速求逆解、如何在实际机器人使用等等。以后有机会再说。