ROS系统简介，机器人操作系统ROS的全面概述

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

        -机械臂运动规划。

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

        -是运动规划（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
  除了基于图搜索的方法，还有另外一大类基于树状结构的搜索算法，其中最著名的就是快速扩展随机树法（Randomly Exploring Randomized Trees，RRT）了。
  RRT算法是从起始点开始向外拓展一个树状结构，而树状结构的拓展方向是通过在规划空间内随机采点确定的。与PRM类似，该方法是概率完备且不最优的。
  

△ 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等。

当然，这篇推送真的只能算是运动规划入门，还有很多坑没提到：例如如何均匀采样、如何做碰撞检测、如何快速求逆解、如何在实际机器人使用等等。以后有机会再说。

文章转自微信：qRobotics

欢迎各位加入我们的机器人探讨QQ群：

奋斗的机器人：
317366356