实例介绍机械臂运动规划及前沿研究方向（附PPT+视频）｜硬创公开课

2017 年 4 月 2 日 硬创公开课 金红

运动规划 (Motion Planning) ，有别于轨迹规划 (Path Planning)。一般来说，轨迹规划用于无人车/无人机领域，而运动规划主要用于机械臂，类人机器人领域。关于运动规划的具体定义，雷锋网本期公开课特邀请到上海交通大学机械与动力工程学院博士生邱强为我们做详细的讲解。邱强目前研究方向为机械臂运动规划，除了讲解什么是运动规划，还会介绍他做的机器人规划实例。同时，他还会讲解目前关于运动规划都有哪些前沿研究方向。

嘉宾介绍：

邱强，上海交通大学机械与动力工程学院博士生，本科毕业于清华大学机械工程系，目前研究方向为机械臂运动规划。运营个人微信公众号Nao (ID: qRobotics)，以qqfly（或fly qq）之名撰写过多篇技术文章。

公开课完整视频：

以下内容整理自邱强在硬创公开课的分享，由于内容有很多针对图或视频的细节讲解，文中做了删减，完整内容可观看视频。关注雷锋网旗下微信公众号「新智造」，回复「PPT」可获取嘉宾完整PPT。

什么是运动规划（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 等方式建立表格，这个表格纪录的是从状态到动作的映射。不过运动规划的维度这么高不可能用表格来存，所以可以通过神经网来解决这个映射问题。

Q&A 环节

Q：运动规划、轨迹规划、路径规划的区别与联系？

A：这几个概念在国内的教材里确实比较混乱，所以我按照我的理解大概说一下：①运动规划（Motion Planning）就是我这次说的主要内容；②路径规划（Path Planning）跟运动规划概念是一样的，只是我们习惯上把它放在平面机器人上；③轨迹规划（Trajectory Planning），主要是涉及速度、加速度插值，包括梯形速度曲线等底层的轨迹插值算法。

一般运动规划后的结果是离散的路径点，也会需要进行轨迹规划后发给机器人。

Q：运动规划中如何实现姿态转接，角速度，角加速度的连续。

A：首先是姿态的连续变化问题：因为我们是在C空间（也就是关节空间进行的），所以不存在这个问题；如果是对无人机进行规划（在SE(3)里），那么可以考虑看看四元数插值的算法。

第二个是角速度、角加速度连续；这个有两种方法，一个是我只规划角度，然后用轨迹规划进行插值；另一个是我同时规划速度和加速度（如优化算法），直接得到一个轨迹。

Q：移动机器人（例如AGV）的路径规划有什么推荐的软件或者平台吗？

A：AGV的路径规划，我这边做的不多，而且会根据你具体做的内容而不同（如SLAM、定位、导航等）。就我实验室来说的话，在ROS下效果就不错，可以用gmapping建图、然后navigation导航。当然，如果ROS还是比较适合实验室，商用的话我就不太了解了。

Q：学习机器人，一定要从运动学动力学轨迹规划一步一步走过来嘛？

A：因为机器人涉及的内容真的很多，如果是做机械臂的话，我还是建议把基础的东西完全弄清楚，包括空间运动、正逆解、动力学等。因为这些基础知识里很多细节，如果直接跳过，后面会遇到很多问题。这方面打好基础，后面就会轻松很多的。

Q：机械臂逆运动学推导和雅可比矩阵求解怎么在Ros上结合规划算法来做出仿真？

A：其实我们规划的过程中需要不断进行运动学求解等。当然，ROS里面的话，我们可以先不关心这个。因为ROS里的KDL、ikFast等都可以帮我们做这些工作。当然了，由于MoveIt采用的是Plugin的形式，如果是你自己算的运动学，那可以做成Plugin放进去。之后MoveIt的规划就是基于你的运动学进行计算了。

Q：请问动态环境下的运动规划有什么研究热点和方向？

A：这个有两点，一个是刚才提到的重规划re-plan问题，当环境变化或者任务变化的时候能在执行的过程中重新规划出一个新的轨迹；另一个则是在规划的过程中考虑动态情况：例如我们对无人车进行规划的时候，就应该同时估计出周围人、车的速度和运动趋势，之后在此基础上进行规划。这样可以避开运动的障碍物等。