“

如今，机器人正在更深入的服务于人类，它们要想完成的一些力所能及的操作，都需要依托于机器人的运动规划。本期技术干货，我们邀请到了小米工程师——‍孙国康，向我们介绍机器人的运动规划方法和应用现状。

”

一、前言

近年来，机器人技术进一步发展，机器人也越来越接近我们的生活。北京冬奥会的智能餐厅里， 机器人自主完成炒菜、出餐、送餐；布拉格酒吧昏暗的灯光下，机器人调酒师正优雅的完成一杯杯鸡尾酒的制作。早已不止在工厂，机器人正在更深入的服务于人类，完成它们力所能及的事情。那机器人是如何完成的这些操作功能呢？

视频1：几种机器人在生活场景中的应用

视频来源：7 Amazing Robotic ARMS (Chef, Barista, Barteder) for Food and Beverage Industry - YouTube

机器人完成上述功能的基础之一是产生和上述功能相匹配的运动，而控制机器人的运动离不开机器人的运动规划。机器人的运动规划是使机器人从当前点避开所有可能的障碍物到达目标点的过程，这个过程可以分成两部分，一是无碰撞路径的生成，二是路径的光滑化。无碰撞路径的生成又叫路径规划，路径的光滑化又叫轨迹规划。

路径规划一般可以得到从起点到终点的一系列路径的散点，轨迹规划是将路径的散点赋予时间信息，得到机器人可执行的控制指令。这篇文章将通过路径规划和轨迹规划原理的介绍及应用，对机器人运动规划方法进行浅析。

二、运动轨迹之路径规划

>>>> 2.1 原理介绍

本文首先通过点状物体在二维空间中的路径规划来描述清楚常用几种路径规划方法是如何工作的，再将其扩展到高维空间，使读者对路径规划方法有一个大体的认识。

▍2.1.1 基于网格的方法

基于网格的方法首先将机器人所在的空间离散为网格，并且通过碰撞检测，得到机器人可以运动的自由空间。然后在自由空间中进行路径搜索，搜索出一个从初始状态到目标区域的某一网格点的运动。基于网格使用图搜索算法，能够在给定的图中得到完备且最优的路径。

图片：基于网格方法路径规划示意

图片来源：https://ai.nju.edu.cn/_upload/tpl/00/d7/215/template215/Robot.html

▍2.1.2 基于采样的方法

基于采样的方法为了提高求解速度，放弃进行最优解的求取，而只求取满意解。基于采样的方法不再将空间进行网格划分，而是通过随机选点的方法进行空间探索。主要有随机路图法(PRM)和快速搜索随机树法（RRT）两类方法。

随机路图法的原理是在空间中通过随机采样选取不碰撞的点，然后再将两点连接，得到随机路图。得到随机路图后，采用路径搜索算法搜索得到规划的无碰撞路径。下面的视频能很好的使这一过程可视化。

视频：随机路图法路径规划过程示意

视频来源：https://www.youtube.com/watch?v=gP6MRe_IHFo

快速搜索随机树法方法是用树来表示这个过程。该方法是先在空间中随机采样采一个点，然后将这个点与原来树的最近点连在一起，若连接线没有发生碰撞，则将该采样点添加到搜索树。按照这种方法不断扩展搜索树，并且最终连到目标位置。基于采样的方法在采样点足够多时总能找到一组无碰撞的路径，但它不一定是最优的。

视频：快速搜索随机树法路径规划过程示意

视频来源：https://www.youtube.com/watch?v=Ob3BIJkQJEw

▍2.1.3 人工势场法

人工势场法进行规划的基本方法是：以机器人目标位置做为引力场，以障碍物作为斥力场，基于引力场和斥力场之间的关系计算机器人所受的合力，合力的方向以及大小为机器人所需的速度方向和大小。

如下面的视频所示，根据目标点和障碍物建立起势场，而机器人在势场的作用力的引导下走到目标点。人工势场法在复杂环境下，容易陷入局部极值的情况，可能找不到解，也求不出最优解。但人工势场法的计算速度快，容易和控制算法相结合。

视频：人工势场法规划过程示意

视频链接：https://www.youtube.com/watch?v=O17-hhekXSY

‍

 >>>> 2.2 在机械臂运控中的应用

▍2.2.1 C空间

前述路径规划算法是针对一个点在二维空间来进行路径规划的，而实际的机器人有体积而且自由度更高。实际机器人的路径规划一般在位形空间（C空间）中进行规划，来满足对高自由度机器人的路径规划需求。

位形是指机器人上每个点位的集合，包含所有可能位形的空间称为位形空间。如下面的视频所示，2DOF的机械臂位形空间可以用它的两个关节角来表示，它的位形空间可以被表示为圆环面，在工作空间中的障碍物也可以通过映射，映射到C空间中。机器人的自由度数等于其C空间的维度，一个7自由度的机械臂，意味着需要在7维的空间中规划其运动。现有的可视化手段已经较难去进行可视化了。

视频：C空间拖动机械臂   

视频：2DOF机械臂的位形拓扑

视频：障碍物映射到C空间

以上三个视频来源：https://www.youtube.com/watch?v=tlFVbHENPCI

▍2.2.2 路径规划方法在高维C空间的应用

前面提到基于网格的方法，在高维空间中，理论上仍然可行，但引入的计算量过大。如7自由度机械臂，按照5度的分辨率（已经是很差的分辨率了）进行网格划分，将会产生72^7=1.003*10^13个网格（^表意为次方），因为需要对每个网格进行判断该网格是否产生碰撞，就需要进行碰撞检测，仅是对每个网格进行碰撞检测（若碰撞检测的时间为0.01ms）,就需要27863小时。所以一般在高维空间中不使用该方法。

在高维空间中建立势场同样也不方便，人工势场法在应用时通常先在工作空间中建立势场，再计算机器人上每个连杆在势场中受到的作用力，然后通过雅可比矩阵将作用力其映射到每个关节上，进而得到C空间的力矩，这样就完成了人工势场法在高维空间中的应用。

视频：人工势场法在2DOF机械臂的应用示意

视频来源：https://www.youtube.com/watch?v=FA10-Guhbqs

随机路图和快速搜索随机树法不需要知道C空间的具体情况，只对随机采样点进行碰撞检测（判断是否在C空间的可行区域内），然后两点之间采用简单的局部规划器进行连接。如下面两个这两种方法均能在高维空间中执行，并且采样点足够多时总能找到一组满意解。

视频：随机路图法在3DOF机械臂上的应用示意

视频：快速搜索随机树法在4DOF机械臂上的应用示意

视频来源：https://www.youtube.com/watch?v=Ob3BIJkQJEw

基于速度的方法能在寻找到一条机器人路径的同时，考虑机器人的动力学、障碍物的位置和速度等动态约束。如利用速度障碍的概念，可以将障碍物约束转换为一个速度级的不等式约束，作为控制器的一个约束条件。这类方法一般可以直接得到机器人执行所需的轨迹。但基于速度的方法有和人工势场法类似的问题，当环境中障碍物稍微变得复杂时，该方法可能找不到一条有效路径。

基于速度的方法往往需要和采样方法相结合，先通过采样的方法规划出一条初始轨迹，再通过考虑机器人的动力学、障碍物的位置和速度等动态约束，来控制机器人完成轨迹的执行。

视频：基于速度的方法在机械臂运动规划上的应用

视频来源：https://www.youtube.com/watch?v=cTXytsWyFxE

上面介绍的几种方法均在以机械臂为代表的高自由度机器人中有较多应用，而且有很多改进的变种。在不考虑机器人动力学的情况下，基于采样的RRT和PRM等方法几乎已经成为路径规划的主流方法。但是在动态环境和一些复杂操作场景中，人工势场法和一些基于速度的方法则能够直接和机器人的动力学控制相结合，得到更好的控制效果，也是目前研究和应用的重点。

三、运动规划之轨迹规划

路径规划后的路径没有时间信息，直接使用会使机器人急起急停，对机器人本体的冲击很大，甚至机器人根本无法执行，因此机器人为了完成对路径的顺利执行还需要进行轨迹规划。

轨迹规划就是给路径规划所得到的路径点进行平滑，赋予时间信息，使路径变成机器人可执行的轨迹。如下图所示，图中展示了机器人执行相同的路径，而采用不同轨迹的运动效果。从左到右依次是时间最优、固定节拍和末端匀速机器人，从图中可以看出不同的轨迹规划方法对机器人执行的影响。通过轨迹规划，机器人可以得到加速度和冲击更小、能量消耗更小同时时间花费更少的轨迹。

视频：同一路径下不同轨迹规划方法（左到右依次是：时间最优、固定节拍、末端匀速）的执行效果

视频来源：https://www.zhihu.com/zvideo/1316437536278429696

 >>>> 3.1 轨迹规划

路径规划后的路径点在关节空间，关节空间轨迹规划直接规划关节的运动，需要求解的是关节角度、角速度、角加速度与时间的函数关系。轨迹规划是已知关节始末位置关节角度以及中间节点对应的关节角度，求解连续轨迹的过程，因此，轨迹规划转化成插值拟合问题。

图片：曲线拟合

目前开展的轨迹平滑主要采用的方法有 B 样条曲线、NURBS 曲线等。样条曲线拥有许多优良性质，如几何不变性、局部支撑性等，使其在多段轨迹过渡时连接的平滑以及整个曲线的平滑性上有很好的保证。采用样条曲线在关节空间中进行轨迹规划时能减少关节冲击，同时使得轨迹适应性更好。

图片：样条曲线拟合的局部支撑性

轨迹规划算法主要是利用样条曲线的优良性质，保证速度、加速度、加加速度的连续， 改善了轨迹连接过渡的平滑性，并且使轨迹具有局部性，方便对轨迹进行局部修改，避免了对轨迹的整体影响。在精度要求越来越高的需要下，样条曲线将更多地用于轨迹规划。

但轨迹规划算法只考虑了轨迹的连续、平滑，未考虑实际作业要求中的效率、能量、冲击等问题。为此，对轨迹尚待进一步优化，以期提高轨迹的质量。

>>>> 3.2 轨迹优化

轨迹优化的主要是使关节运动能够提高效率、降低能耗，减少冲击等，以满足实际要求。优化目标一般可分为：时间最优、能量最优、冲击最优以及综合最优。

关节空间轨迹时间优化的目的是在满足约束条件下，尽可能地缩短运行时间，提高工作效率。对于轨迹时间的优化，本质上是以时间最短为优化目标，在运动学或动力学约束条件下，寻找最优解的数学优化问题，所以，对时间的优化就集中在对于优化算法的研究上。大多数的方法都是在满足关节速度、加速度、加加速度约束下，采用不同的优化算法进行寻优。

能量优化的大致思路与时间优化类似，在运动学、动力学约束下，建立能量消耗函数，采取优化算法进行求解。相关方法的不同之处在于对能耗函数及优化算法的选择上。不同的能耗函数反映的是不同形式的能量消耗指标，如何建立统一的能量消耗函数是今后的研究方向。同时，考虑单一的能量优化应用较少，更多的情况需要综合考虑其他因素（如时间、冲击等），综合优化更符合实际需求。

关节冲击使关节运动不平稳，严重的会增加振动、机械磨损，导致结构破坏。对于轨迹冲击优化的研究，既有对评价冲击大小的指标设定研究，也有对优化算法的具体应用的探讨。现阶段的主要研究集中在建立冲击评价指标及优化算法的使用上，实际中大多是考虑多方面目标的优化，单独考虑冲击最优对于实际意义不大，冲击最优的同时也需要考虑效率、能量问题。

四、落地应用

>>>> 4.1 在拖动示教中的应用

前面提到运动规划包含了路径规划和轨迹规划两个部分。在多数应用场景下，路径规划的部分是由人“手把手”教给机器人的，而不是由机器人自己生成的，这里的人工教授即为拖动示教。具体来看拖动示教的实现过程如下，操作员先拖动机器人执行期望路径，控制器记下对应的路径后，进行轨迹规划，控制机器人执行该路径。如下面的应用Demo所示，拖动示教操作简单，却能完成相当多的工作，实现机器人的快速部署，几乎可以称得上是现有协作机器人的标配。运动规划在拖动示教中的应用主要体现在轨迹规划上。

视频9：机械臂拖动示教的应用实例

视频来源：https://www.youtube.com/watch?v=RWzFS0PkHuo

>>>> 4.2 搬运、码垛中的应用

在机器人实现搬运和码垛的过程中，通过视觉获取目标点位置，再通过人为的给定路径的中间点，完成搬运和码垛的操作。

视频：机械臂在码垛应用实例

视频来源：https://www.youtube.com/watch?v=DVnFT9audks

下面视频是一个移动机器人操作的场景，涉及了移动机器人导航和机械臂操作的运动规划。

视频：移动机器人操作应用实例

视频来源：https://www.youtube.com/watch?v=BiemOdLYf48

>>>> 4.3 生活场景中的应用

相比于上述两类工业应用场景而言，生活应用场景的环境更加开放，对机器人运动规划的能力要求也更高。虽然当前大规模落地的应用还不多见，但机器人已经能够成功胜任一些特定场景需求，下面给出几个较为经典的例子。

▍4.3.1 障碍物环境下的目标跟踪和抓取操作

下面视频给出了YUMI和BAXTER两个机器人在障碍物环境下完成目标物体跟踪和取放任务的实例。

视频：目标跟踪和抓取应用实例

视频来源：https://www.youtube.com/watch?v=Fl4WvsXQDzo

▍4.3.2 咖啡制作与拉花

下面的视频是Rethink（老牌协作机械臂生产商），展示的是协作机械臂在制作咖啡+拉花这样一个商业场景中的应用，得益于协作械臂安全且丰富的人机交互能力。

视频：机器人进行咖啡制作与拉花应用实例

视频来源：https://www.youtube.com/watch?v=rRSC2XZga98

▍4.3.3 机器人端水杯+避障

机器人端水杯需要满足水不外撒所需的动力学约束，同时进行避障。这时运动规划和轨迹规划往往会合二为一，直接计算出所需的轨迹。

视频：机器人进行端水杯操作

视频来源：https://www.youtube.com/watch?v=K_rOgjixkOs

>>>> 4.4 其他应用

下面这个视频是目前最广泛的运动规划平台MoveIt前几年给出的一些应用示例，可以从实例中看出一些机器人运动规划的应用情况。

视频：运动规划的一些应用集合

视频来源：https://www.youtube.com/watch?v=0og1SaZYtR

五、结语

尽管现阶段运动规划方法取得了一定的发展，甚至理论上已经较为完备，但由于现有机器人的操作环境单一，运动规划在实际应用中的场景依然比较有限，未来尚有很大的发展空间。相信随着机器人应用场景的进一步开放，机器人运动规划终将在各个领域大放异彩。

参考文献

[1] Mohanan M G ,  Salgoankar A . A survey of robotic motion planning in dynamic environments[J]. Robotics and Autonomous Systems, 2017, 100:171-185.

[2] Tsardoulias E G ,  Iliakopoulou A ,  Kargakos A , et al. A Review of Global Path Planning Methods for Occupancy Grid Maps Regardless of Obstacle Density[J]. Journal of Intelligent & Robotic Systems, 2016, 84(1-4):829-858.

[3] H. Choset, K. M. Lynch, S. Hutchinson. Principles of Robot Motion: Theory, Algorithms, and Implementations[M]. 2005.

[4] Pankert J ,  Hutter M . Perceptive Model Predictive Control for Continuous Mobile Manipulation[C]// International Conference on Robotics and Automation. IEEE, 2020.

[5] Kevin M. Lynch, Frank C. Park. Modern Robots Mechanics, Planning, and Control[M]. 2017