机械臂的运动规划

如果你想要让机器人能帮你拿瓶子、做饭、收拾屋子等，就必须赋予机器人快速生成无碰撞、最优运动轨迹的能力，这就需要靠运动规划了。有人觉得运动规划已经很成熟了，无需再研究，但实际上，机械臂运动规划非常难……

如 果你想要让机器人能帮你拿瓶子、做饭、收拾屋子等，就必须赋予机器人快速生成无碰撞、最优运动轨迹的能力，这就需要靠运动规划了。有人觉得运动规划已经很 成熟了，无需再研究，但实际上，机械臂运动规划非常难，之所以这么难，主要是因为规划问题的维度太高（具体后面分析），目前暂无兼顾实时性与最优性的规划 算法。

什么是运动规划（Motion Planning）

在上面四张图片中，左上角是机器人在抓取桌上的东西，这是我们实验室之前一个博士师兄的课题，主要就是机械臂通过轨迹规划抓取识别到的物体而不碰到 障碍物。右上角是蛋白质折叠过程，使用的是我们之后会提到的算法去规划它空间变化。左下角是《帝国时代2》的场景，我们在玩这类游戏的时候只需要点击一个 目标点，游戏人物就会自行找到可行的路径。最后一个是我们之前做过的一个机器人导航项目，通过激光雷达和算法机器人可在室内找到路径。从这四张图片我们可 以从中给运动规划得出一个定义：

• 在给定环境中，指定机器人起点与终点，计算出连接起点与终点，并满足一定约束条件（如避障）的轨迹。

• 从数学角度上看，移动机器人的路径规划（ Path Planning ）也属于运动规划的范畴。但由于问题的维度不同，所以使用的算法也不同，大家习惯上将两者区分开。

为什么研究路径规划

• 社会老龄化

这是世界银行发布的关于中日两国国内生产总值（GDP）变化曲线图，小图是中日两国的人口结构，可以看到2000年日本和中国2015年的人口结构 已经很接近了，所以未来中国劳动力数量会减少，我们必须提高平均劳动生产力，这样才能防止GDP的增速减缓。机器人是可以解决这些问题的。

• 市场转变

传统工业机器人主要应用在汽车行业，而这个行业的特点是一个车型可以生产很多年，同时每台车的利润也会相较较高，但是从目前来看机器人在汽车行业已 经基本饱和，所以大家的关注点开始转向3C（Computer、Communication、ConsumerElectronics）行业。

3C产品具备这些特点：更新周期短、款式种类多、单件利润低、整体市场大、劳动力成本增加、对自动化需要加大。

• 示教

现在我们工业机器人的使用方法通常是示教，即使像右图采用拖动示教这种比较便利的方式，效率还是很低，因为每一台机器人的示教都需要人参与进来，而 且示教的路径没办法应对其他一些环境的变化，尤其在3C行业你每次更新一次机型，我们就必须对流水线上所有的机器人重新示教，这样的效率肯定是不够高的。

• 加中间点

当然，目前有些机器人应用是加入了机器视觉等技术，就是在检测之后让机器人应对一些变化情况。左边码垛机器人就是通过视觉可以抓取东西，但它的路径 是人工指定中间点。右边是我做过的类似插秧机器人，原理与前面码垛机器人类似。这类机器人想要在3C行业被灵活运用肯定是不行的，所以如果运动规划研究成 熟算法比较稳定的话，就可以用高级编程语言去编程，比如我们的指令让它抓取零件A然后加工零件B的某一面，这种下达指令的方式就不需要每一步都示教了。

怎么做运动规划

对于规划器的评价标准，我们现在有两个准则：

Optimality（最优性）: 路径最短、规划速度最快等。

Complete（完备性）：在有限时间内解决所有有解问题。

然后，我们从最基础的问题入手，也就是2维环境中的点状机器人（point agent），点状机器人是没有实体的。接下来介绍下点状机器人的路径规划算法都有哪些。

• Walk To

  直接朝着目标走，直到到达目标点为止。

  很多 RPG 游戏就采用了这种简单的算法

  最优性，但不完备

• 优化算法（蚁群等）

  类似最优控制

  大部分情况下效果不错，但复杂问题很容易陷入局部极值

  不完备也不最优

• 人工势场

  在障碍物周围建立排斥势场

  从起点到终点构建吸引势场

  采用梯度下降等方式求解

  容易实现、效果很好

  可以与控制结合

  可能陷入局部极值

  不完备且不最优

• 图搜索算法

  将问题描述成图（节点+边）

  用图搜索算法解决问题

  Dijkstra、A*

  在给定的图中完备且最优

• 可视图（Visibility Graph）

  用封闭多面体描述障碍物

  利用障碍物顶点间的连线构建一个图（graph），之后用图搜索算法求解

  站在某个顶点上，环绕四周，把你能看到（无障碍物）的顶点连接起来

  完备且最优

• 栅格化（Cell Decomposition）

  按一定分辨率将地图进行网格划分

  用四连通或八连通规则建立网格图

  分辨率完备（Resolution Complete）且最优

• 随机路图法

  PRM（Probabilistic Road Maps）

  通过随机采样选取不碰撞的点

  两点连接采用简单的局部规划器如 Walk to 算法

  将起止点连入路图

  用图搜索求解

  概率完备且不最优

• 快速扩展随机树法

  RRT（Randomly Exploring Randomized Trees）

  基于树状结构的搜索算法

  概率完备且不最优

前面我们讲的都是2D点状机器人的情况，现在我们想怎么把这些问题推广到实际机器人上。实际机器人有两个问题，一个是机器人不再是一个点，需要将机器人的体积考虑在内，另外，机器人的自由度更高，原本的算法是否都还可用？

• C空间（理论基础）

  构形空间，Configuration Space

  用向量描述机器人的构形

  在C空间内，机器人是一个点

  C 空间拓扑性质与笛卡尔坐标系下的情况不同——二自由度机械臂的C空间是一个圆环面

  大部分机构（连续旋转关节、平动关节等）形成的构形空间均是微分流形，任一点的邻域均与欧式空间同态

  微分流形：大部分算法效果与在笛卡尔坐标下效果相同

• 高维度*

  蚁群等优化算法：收敛慢，更多局部极值点

  可视图法：在高维空间中，算法不成立

  栅格法：理论上可行；但会计算量太大；对于一个六自由度机械臂，我们按照6°分辨率（已经是很低的分辨率了）划分网格，那么将会产生606 = 4.67 × 1010 个网格，单是对每个网格进行碰撞检测（如果碰撞检测速度为0.1ms），就需要1296小时。

  一般在高于三维的问题上不使用该方法。

• 人工势场

  在 C 空间内建立势场不方便

  只对个别控制点进行计算，折算到每个关节上

  不完备且不最优，但对于简单的问题很实用

• PRM 和 RRT

  不需要知道 C 空间的具体情况，只对随机采样点进行碰撞检测（判断是否在 C 空间的可行区域内）

  两点之间采用简单的局部规划器（如 Walk to）进行连接

  PRM：获得一个图，采用图搜索算法求解

  RRT：获得一个连接到终点的树，反向搜索即可

  在高维空间内可行，概率完备且不最优

现状：主要使用 RRT 和 PRM 等 Sampling-based methods；这些算法计算的结果一般需要进行后处理（smoothing等）。

• RRT 和 PRM 变种

  C 空间

  随机采样（各种采样算法 T-RRT）

  有效性判断（如碰撞检测算法 AABB、减少碰撞检测 Lazy-RRT）

  局部规划器连接（各种连接方法、重新连接 RRT，PRM）

  …

• RRT*

  渐进最优

• Informed RRT*

  先验知识——只在sub-problem下采样

• 理论学习

  Coursera: 宾大 Robotics: Computational Motion Planning （简单编程）

  Choset, Howie M. Principles of robot motion: theory, algorithms, and implementation. MIT press, 2005.

  经典论文+编程实现经典算法。

• 实践

  ROS MoveIt!：http://moveit.ros.org/

  容易上手+容易修改

前沿研究方向

理论现状是，从运动学规划角度，给定足够多的时间一定能够最优且完备地求解到轨迹。从理论的角度而言，这个问题已经解决了。现在研究方向主要在这两个方面，探索新问题和做一些实用化工作。

新问题

• 重规划 re-plan

这个算是蛮实用的，因为每次规划完执行的过程中会遇到环境变化的问题，这就需要在执行过程中重新规划。重新规划的路线与之前的路线是连接的，而不是中间停下来重新走。上面是RSS在2016年的研究，感兴趣的可以了解下。

• 考虑系统动力学

理想状态下机器人在运动规划下直接端一杯水到一个地方就行了，但实际情况下这个过程是有动力学在里面，如果不做任何处理，这个杯子会掉。所以，在考虑了动力学之后，重新进行运动规划，这时候杯子才不会掉。这个问题还是比较简单的，因为你只需要把它变成一个约束就好了。

• 考虑接触动力学

因为我没有做这块东西，所以不太清楚它是怎么运作的，但是这个问题是存在的，因为在规划的时候会跟环境接触，例如这个机器人攀爬杆子然后落地，涉及 到整个身体动力学跟你身体运动的协调过程，这个工作是MIT计算机科学与人工智能实验室在2014年的实验。接触动力学比传统的单体动力学复杂很多，因为 我们不知道它接触的碰撞摩擦力这些不好建模。

• 运动规划+任务规划

运动规划是指我给你一个大任务，你自动生成一些小任务。这是IROS在2016年的一个工作，它的目标是让机器人到达对面这个点，而它的路径被障碍 物挡住了，这个时候把运动规划加进来，从更高一个空间维度去求解这个问题。第一步，它把这个桌子往前推，发现桌子推不动的时候对任务进行重规划，然后规划 到去推这个桌子，然后发现执行的效果与预计的不一样，所以它又生成新的任务，然后它拉开桌子之后就走到了对面实现了工作。这只是一个很简单的demo，但 实际上生活中会遇到很多这样的问题，比如我想从这个房间到另一个房间，而门是关着的，这个时候就需要把门打开。所以说，不是要给机器人生成很多子任务，而 是一个大任务，未来服务机器人想要做好这块是必须要做的。

实用化

另外大部分时间大家都用在了实用化上，虽然说只要有足够时间它一定能求解出来，但实际情况下我们不可能给它无限的时间。另外RRT这些算法生成轨迹 很奇怪， 你可以看右边这个视频，只是让它敲这个东西它要画一大圈，所以这也是一个问题，就是怎么优化它的轨迹。所以需要将研究领域好的算法往工业领域推，目前两者 之间是存在很大缺口的。

• 轨迹复用（相对固定的动作）

这个工作是想办法把旧的轨迹给用起来，通过人工的方式指定一个运动微元，也就是原始轨迹，等到了新的环境后再进行改变。当然，这个爬楼梯的过程，环 境和动作基本上都相同，所以可以在这个微元的基础上进行改变。首先，通过变形的工作拉到现在起始点位置，部分起始点会重合，然后对这些新起始点进行重复利 用，它会形成一个好的轨迹。这个工作是Hauser et al在2008年发布的论文。现在存在的问题是运动微元必须由人工来指定，所以研究方向是由系统自动生成运动微元。

• 旧轨迹信息（相对固定的环境）

这是之前做的一个内容，比较简单但在相对固定的环境比较好用。大概原理就是根据人工示教的路径，通过高斯混合模型（GMM）对可行C空间进行建模， 之后在这个GMM-C空间内进行规划。这个方法有点类似Learning From Demonstration 的工作，但我只用了它们前面一半的步骤，后面一半还是采用采样的方法。

• 加约束

这个是我针对加工过程做的另一个工作。我们在工业领域用机器人往往期望的不是整个机械臂的动作，而只是末端的动作。假设我要抛光一个面，首先我要对 末端进行规划，用CAD模型就可以计算实现；得到路径后发给机器人，之后直接求逆解或者用雅克比迭代过去。当然，这种方法大部分时候够用，但有时候也会遇 到奇异点或者碰到障碍物。我就是针对这个七轴的机械臂，利用它的一个冗余自由度进行规划。因为末端是固定的轨迹，这个时候，只要找到冗余自由度对应的C空 间流形，我们就可以在这么一个低维（2维）流形内进行很快速的规划，实现末端固定轨迹，且关节避障避奇异。

• 深度强化学习 DRL

我个人现在现在最关注的一块，目前还没有实质性的东西出来，在这里就和大家讨论下，我觉得这一块未来会出来不少的研究成果。

假设深度学习做运动规划，那么它进行一次运动规划的时间就是一次网络正向传播时间，这个时间非常短的，所以只要网络训练好后，运动规划需要耗费很长 时间的问题就没有了。目前这块也有一些这方面的研究，上面左边图是用深度学习玩游戏，Nature上的一篇论文，效果比人还厉害；右上角是谷歌用深度学习 来开门；右下角就是AlphaGo下围棋了。这个是很有意思的，它也是运动规划和控制的问题，但它是用网络来做的映射。

我为什么对这方面很感兴趣呢？首先，CNN已经具备强大的环境理解能力，很容易从观测估计状态，观测是图片这类，而状态，如果是物体识别，就是是什 么物体，如果是定位，那就是物体在什么地方。也就是说，在给定信息满足系统状态可观性的前提下，CNN环境理解能力是非常强大的。

第二个就是RL（强化学习）可以进行路径规划，通过 value iteration 等方式建立表格，这个表格纪录的是从状态到动作的映射。不过运动规划的维度这么高不可能用表格来存，所以可以通过神经网来解决这个映射问题。