运动规划

转载：http://www.javashuo.com/article/p-dueuecmk-gd.html

参考：实例介绍机械臂运动规划及前沿研究方向（附PPT+视频）

1.引言

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

2.什么是运动规划（Motion Planning）

2.1 定义

咱们能够从中给运动规划得出一个定义：

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

从数学角度上看，移动机器人的路径规划（ Path Planning ）也属于运动规划的范畴。但因为问题的维度不一样，因此使用的算法也不一样，你们习惯上将二者区分开。

2.2 为何研究路径规划

（1）社会老龄化

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

（2）市场转变

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

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

（3）示教

　　如今咱们工业机器人的使用方法一般是示教，即便像右图采用拖动示教这种比较便利的方式，效率仍是很低，由于每一台机器人的示教都须要人参与进来，而 且示教的路径没办法应对其余一些环境的变化，尤为在3C行业你每次更新一次机型，咱们就必须对流水线上全部的机器人从新示教，这样的效率确定是不够高的。

（4）加中间点

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

2.3 怎么作运动规划

对于规划器的评价标准，咱们如今有两个准则：

• Optimality（最优性）: 路径最短、规划速度最快等。
• Complete（完备性）：在有限时间内解决全部有解问题。

3.运动规划算法

3.1 二维运动规划

（1）Walk To

• 直接朝着目标走，直到到达目标点为止。
• 不少 RPG 游戏就采用了这种简单的算法
• 最优性，但不完备

（2）优化算法（蚁群等）

• 相似最优控制
• 大部分状况下效果不错，但复杂问题很容易陷入局部极值
• 不完备也不最优

（3）人工势场

• 在障碍物周围创建排斥势场
• 从起点到终点构建吸引势场
• 采用梯度降低等方式求解
• 容易实现、效果很好
• 能够与控制结合
• 可能陷入局部极值
• 不完备且不最优

（4）图搜索算法

• 将问题描述成图（节点+边）
• 用图搜索算法解决问题
• Dijkstra、A*
• 在给定的图中完备且最优

（5）可视图（Visibility Graph）

• 用封闭多面体描述障碍物
• 利用障碍物顶点间的连线构建一个图（graph），以后用图搜索算法求解
• 站在某个顶点上，环绕四周，把你能看到（无障碍物）的顶点链接起来
• 完备且最优

（6）栅格化（Cell Decomposition）

• 按必定分辨率将地图进行网格划分
• 用四连通或八连通规则创建网格图
• 分辨率完备（Resolution Complete）且最优

（7）随机路图法

• PRM（Probabilistic Road Maps）
• 经过随机采样选取不碰撞的点
• 两点链接采用简单的局部规划器如 Walk to 算法
• 将起止点连入路图
• 用图搜索求解
• 几率完备且不最优

（8）快速扩展随机树法

• RRT（Randomly Exploring Randomized Trees）
• 基于树状结构的搜索算法
• 几率完备且不最优

3.2 三维路径规划

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

（1）C空间（理论基础）

• 构形空间，Configuration Space
• 用向量描述机器人的构形
• 在C空间内，机器人是一个点
• C 空间拓扑性质与笛卡尔坐标系下的状况不一样——二自由度机械臂的C空间是一个圆环面
• 大部分机构（连续旋转关节、平动关节等）造成的构形空间均是微分流形，任一点的邻域均与欧式空间同态
• 微分流形：大部分算法效果与在笛卡尔坐标下效果相同

（2）高维度*

• 蚁群等优化算法：收敛慢，更多局部极值点
• 可视图法：在高维空间中，算法不成立
• 栅格法：理论上可行；但会计算量太大；对于一个六自由度机械臂，咱们按照6°分辨率（已是很低的分辨率了）划分网格，那么将会产生606 = 4.67 × 1010 个网格，单是对每一个网格进行碰撞检测（若是碰撞检测速度为0.1ms），就须要1296小时。
• 通常在高于三维的问题上不使用该方法。

（3）人工势场

• 在 C 空间内创建势场不方便
• 只对个别控制点进行计算，折算到每一个关节上
• 不完备且不最优，但对于简单的问题很实用

（4）PRM 和 RRT

• 不须要知道 C 空间的具体状况，只对随机采样点进行碰撞检测（判断是否在 C 空间的可行区域内）
• 两点之间采用简单的局部规划器（如 Walk to）进行链接
• PRM：得到一个图，采用图搜索算法求解
• RRT：得到一个链接到终点的树，反向搜索便可
• 在高维空间内可行，几率完备且不最优

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

（5）RRT 和 PRM 变种

• C 空间
• 随机采样（各类采样算法 T-RRT）
• 有效性判断（如碰撞检测算法 AABB、减小碰撞检测 Lazy-RRT）
• 局部规划器链接（各类链接方法、从新链接 RRT，PRM）

（6）RRT*

• 渐进最优

（7）Informed RRT*

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

4.其余

4.1 理论学习

　　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/

    容易上手+容易修改

4.2 前沿研究方向

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

（1）新问题

• 重规划 re-plan

　　这个算是蛮实用的，由于每次规划完执行的过程当中会遇到环境变化的问题，这就须要在执行过程当中从新规划。从新规划的路线与以前的路线是链接的，而不是中间停下来从新走。上面是RSS在2016年的研究，感兴趣的能够了解下。

• 考虑系统动力学

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

• 考虑接触动力学

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

• 运动规划+任务规划

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

（2）实用化

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

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

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

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

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

• 加约束

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

• 深度强化学习 DRL

　　我我的如今如今最关注的一块，目前尚未实质性的东西出来，在这里就和你们讨论下，我以为这一块将来会出来很多的研究成果。

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

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

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

5.参考网址

V-rep学习笔记：机器人路径规划1

RRT路径规划算法

基于Mathematica的机器人仿真环境（机械臂篇）

RRT基本概念

基于RRT搜索算法的六自由度机械臂