运动规划
转载：https://www.cnblogs.com/lvchaoshun/p/6681541.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.参考网址
