大界成立的RoLAP实验室（RoboticPlus Laboratory for Autonomy and Perception），由中科院博士后、加州理工物理学博士、大界首席科学家周诚喆领衔，聚集了一批专业的硕博团队，致力于研究工业机器人在智能制造场景下的视觉感知（眼）、运动规划（手）、场景理解（大脑）的协同闭环系统。本文将基于RoLAP实验室的研究成果，为各位读者深度解析机械臂运动规划的关键技术。
二. 运动规划：采样 VS 优化
轨迹是否能完成指定任务？ 轨迹是否和物体产生碰撞？ 规划时间是否合理？ 是否能自动处理冗余自由度？ ……
下方表格简单地将三种常见的规划策略做了一个对比。
现在最常用的运动规划大多是基于采样来规划。这类算法实现起来相对简单，不需要对场景和机械臂建立复杂的数学物理模型，规划过程中只需要回答简单的“是”或“否”，比如“该采样点是否发生碰撞”、“该采样点机械臂是否可达”等等。在这些“是或否”的二元问题指引下，采样类规划算法会快速扩张并尝试搜索关节空间中每个角落。这种随机生长的方式对于低自由问题非常有效，甚至可以保证概率完备性，即在无穷大的采样数量下能保证搜索到可行路线。不过这些基于采样的规划方法其设计初衷只是为了寻找一条可行轨迹而并非最优轨迹。因此该类算法往往需要通过后处理步骤来平滑和缩短输出的轨迹，在后处理过程中甚至可能会产生新的碰撞。同时，大量的计算时间被花费在针对关节空间中和目标任务无关的区域的随机采样以及采样点之间的连边上。采样类规划算法在计算效率上的缺陷会随着机械臂自由度的增加变得更加严重。 另一方面，上文已经提到采样式运动规划主要是寻找一条可以完成目标任务的可行路径。这是一种相对独立的规划范式，对于任务和环境的历史信息并没有记忆。这类方法对于完全独立的规划任务可能是必须的甚至是唯一的解决方案。但是对于一个变化中的“陌生”环境，即使只是发生了微小变化，采样式运动规划也必须从头开始搜索可行路径。这种规划范式无法满足对实时性较高的任务。 本系列探讨的则是运动规划的另外一条技术路线，即轨迹优化算法。从控制角度看，轨迹优化本质上是最优控制论的应用，即通过计算出一连串合适的系统状态和控制，在实现任务的同时尽可能降低完成任务需要付出的代价。在机器人运动规划中，优化类算法扮演了两个重要的角色。一是作为后处理算法，将其他运动规划器的输出路径（比如采样类算法输出的折线轨迹）作平滑处理或者细节简化。二是作为轨迹重规划算法，可以将一条直观但是存在碰撞（比如简单的点到点直线路径）的运动轨迹，转变成无碰撞且任务代价尽可能低的局部最优路径。轨迹优化类规划算法最大的特点是利用了实际生产（或者设计）场景中物理（或者仿真）环境的变化往往是连续的这一特质。换句话说，当场景和场景之间存在大量共同点时，从头开始采样搜索不应该是最佳的规划策略。基于轨迹优化的运动规划范式会利用环境和机械臂的历史状态甚至专家库中的工艺知识，快速计算出当前状态下最优的运动规划。 轨迹优化方法作为局部最优的运动规划器在一定程度上可以满足上文提到的全部规划器指标。当然使用轨迹优化算法的代价是其背后复杂的物理建模和数值求解技巧。实现这类基于优化的算法通常需要将机械臂、环境碰撞、目标工艺等工程问题通过高保真的方式翻译成由矩阵描述的优化问题并通过合适的数值方法进行求解。这其中就涉及到如下三大主要模块，其中每个模块都对应一种"凸"的概念（本文标题中的“凸凸凸”便来自于此）： 层级凸包碰撞模型（描述物理世界和机械臂之间的准静态关系） 优化问题的运动学近似（描述物理世界和机械臂之间的动态关系及工艺任务） 逐步二次规划求解器（调整机械臂状态来满足动/静态关系和工艺任务） 轨迹优化算法的整体实现框架如下，在接下来的章节中会依次介绍大界对于这些模块的处理方案。
大界在碰撞处理上采取了层级式的凸包碰撞模型。为了高效处理三维模型之间的碰撞，对原始精细的三维模型作凸包近似是现代三维引擎必不可少的步骤。凸包近似的作用是将原始三维模型通过一个或者多个凸多面体的集合完全包裹住。这样模型与模型之间的原始碰撞问题简化为模型凸包之间的碰撞问题。利用凸多面体的良好数学特性，凸包之间的碰撞问题已有许多高效解决方案，是现代物理仿真（游戏）引擎必备的工具。当然凸包近似也不是万能的。基于凸包的高效碰撞计算是以牺牲原始模型精度为代价的，很多情况下凸包仅仅是对原始模型的一种保守近似。因此一个高效且高精度的碰撞系统一般会通过层级式的凸包模型来处理空间重叠问题。大界的实现方案包括如下三个尺度的碰撞模型： 大尺度阶段使用轴向包围盒（也是凸包）等方法快速粗筛，得到可能发生交互的个体。 中尺度阶段使用原始模型的完整凸包，计算相对保守的运动规划。 小尺度阶段基于中尺度提供的规划结果，使用细粒度更高的凸分解近似，实现更为精细的工艺规划。 上图中展示了不同细粒度下的碰撞模型。最左边为机械臂各关节部件的原始三维模型网格。左起第二个图展示了在三个简单的轴向包围盒（长方体各边必须和坐标系平行）包裹下的原始机械臂模型。轴向包围盒在碰撞引擎中极为常见，在碰撞初筛阶段可以快速判断大量盒与盒之间的重叠情况。不过由于其轴向特点以及方正的型态，不适合做进一步碰撞分析。左起第三个图展示了机械臂每个零部件由顶点形成的单个整体凸包。这种方法比较简单，生成的面的数量也较少。当物体和机械臂距离不是特别近的时候，凸包碰撞检测的结果和原始模型基本保持一致。不过整体凸包法也存在一定问题，比如对于三号轴（绿色臂）的单个凸包近似过于保守，导致障碍物无法穿过凸包靠下的部分却不会和三号轴的原始模型产生碰撞。这种情况对于更紧凑的场景可能会导致找不到可行路径。图中最右的机械臂模型展示了凸分解是如何通过将原始模型分解为数个更小的凸包来避免这种情况发生。当然一个几何体的凸分解方式有无数种，一种比较好的凸分解其空间浪费率应该是较低的。然而图中实际展示的是原始模型的伪凸分解。为什么说是”伪”？因为在给定子凸包数量的情况下求原始模型最优凸分解（即空间浪费率最低）的问题是 NP-HARD。 因此一种更为讨巧的做法是伪凸分解，即生成的子凸包集合无法完全包裹住原始模型但是被控制在一定阈值范围内。虽然失去了完美的凸包覆盖，但是伪凸分解能快速生成更为保真的子凸包集合。这些小尺度的失真在实际应用中会被规避，因为规划算法往往要求机械臂和障碍物之间存在一定的安全距离，一般远大于伪凸分解的失真阈值。 凸分解在一定程度上弥补了整体凸包 对于非凸几何过于保守的碰撞模型 多面体碰撞算法中往往涉及两个重要的概念，一个是距离矢量，一个是穿透（又称分离）矢量，分别用来量化两个多面体还未碰撞或已产生空间重叠的情况下的几何关系。距离矢量非常好理解，即两个多面体A和B的最短距离。如果将多面体A中任意一点和多面体B中任意一点记作一组点对，那么距离矢量就是所有可能的点对中距离最短的那个矢量。通常情况下，对于任意一对多面体求距离矢量是一个复杂度非常高的计算过程。但如果两者都是凸多面体，利用几何体的凸性质可以在较少的计算时间内得到两个问题的答案： 是否构成碰撞？ 在没有碰撞的情况下，距离矢量是什么？ 下图中直观地展示了距离矢量的物理意义。蓝色多面体沿着距离矢量平移后，和红色多面体存在有且仅有一个共同点。 在距离矢量的平移作用下，两个凸包发生接触 另一方面，若两个多面体已经发生碰撞，无论两者以怎样的方式重叠，在这种情况下距离矢量都是零，无法提供其他有意义的信息。穿透矢量定义为将两个多面体完全分开的最短矢量。利用凸多面体的性质，穿透矢量同样存在高效的计算方法。注意距离矢量和穿透矢量的定义对于凸多面体来说是相容的。比如下图中的红色多面体，如果在两者完全分离之后继续沿着穿透矢量的方法平移，则会产生和穿透矢量方向相同的距离矢量。换句话说，距离矢量和穿透矢量在某种程度上是连续的，在发生碰撞的时刻可以无缝切换。这种连续性对于轨迹优化算法是极为友好的。 在穿透矢量的平移作用下，两个凸包被完全分离 凸包碰撞模型和其他常见的碰撞检测算法都可以检测出机械臂和其他物体是否发生空间重叠。基于随机采样的路径规划算法会大量使用“是”或“否”的碰撞检测来判断采样点和连边路径是否可行。但是凸包碰撞模型利用距离矢量和穿透矢量还可以提供发生碰撞和解耦碰撞的趋势。下图中展示了一个机械臂和环境物体（三十八面扭棱立方体）的简单碰撞场景。对于一个给定机械臂状态，常见的碰撞检测算法可以计算在当前状态下是否会发生“穿模”现象。采样类运动规划算法会基于这个结果，剔除关节空间中所有发生碰撞的采样点。 原始模型和障碍物之间产生碰撞 同样一个场景在凸包碰撞模型中则包含了更多的信息。下图中展示了机械臂各关节的整体凸包和环境物体（扭棱立方体是凸的）的交互细节。为了更好地展现碰撞矢量，图中只显示了中尺度（即单体凸包）的碰撞结果，每条线段表示了对应颜色的关节凸包和环境物体之间的距离矢量（如果没有碰撞发生）或穿透矢量（如果已经穿模）。当某个关节凸包离环境物体越来越近的时候，对应的距离矢量也变得越来越短。同理，在关节凸包逐渐和环境物体分离的过程中，穿透矢量的长度也逐渐趋向于零。这些矢量的方向和变化就是轨迹优化算法中依赖的趋势信息。当关节凸包和物体之间的距离小于某个安全距离时，轨迹优化算法会参考距离矢量来调整各个关节转角以防止距离发生进一步减小。当两者已经发生碰撞时，穿透矢量对于如何解耦机械臂和物体之间的空间重叠提供了几何上的参考（采样类方法在这里则完全失效）。当两者距离超过一定阈值的时候，处于效率考量轨迹优化算法会认为这是两个完全独立的个体不再做任何处理。 值得注意的是，在规划中常见的人工势场法中，无论物体距离机械臂多远都会受到一定的影响。比如下图中的紫色轴多多少少会被图中扭棱立方体的人工势场所影响，从而对紫色轴的转动产生不必要的影响。而在凸包碰撞模型中这些影响是完全可以避免的，因此可以实现一些更加精细的工艺路径规划。 凸包碰撞模型捕捉到的细节 利用几何模型中的凸性质来高效处理运动规划中的碰撞问题，这便是轨迹优化算法的第一个凸。 四. 优化问题中的凸：运动学近似 机械臂轨迹优化算法本质上是在求解一个非线性优化问题。那么非线性优化问题的一般形式如下： 这里 f 为非线性函数，g 为非线性不等式约束，l 为线性不等式约束。机械臂轨迹优化问题中的各种物理上的诉求可以转换成上述标准形式。 х 为机械臂状态向量，即所有关节空间的取值。注意运动规划中，х 不是系统在某一时刻的单个状态，而是包括了整条规划路径上的全部状态，即 х=х0,…,хt ,…,хT f 为代价函数，即完成任务系统所要付出的且用户在意的代价。代价函数的定义非常自由，一些常见形式包括机械臂相关的比如关节移动距离最短，关节输出力矩最小，电池能耗最低，末端轨迹平滑程度、场景相关的比如距离某个障碍物尽可能远，甚至与工艺先验知识相关的代价比如工具头与工作台尽可能保持垂直等。 l 为线性不等式约束，目的在于严格控制机械臂轨迹的可行性。这是处于两个考量，一是机械臂本体对于每个关节电机能转动的范围都存在不同程度限制。二是机械臂姿态存在跨相限问题，即同一个末端姿态对应多个正运动学解。这些解之间往往存在巨大的关节轴变换。由于直接对系统状态 х 进行限制，因此 l 的数学形式为线性（仿射）不等式。 h 为等式约束，即机械臂必须要完成的任务。这类约束属于硬约束，无论优化过程是否能找到局部最优解，机械臂都必须完成指定的任务。比如运动规划中必须要经过的路径点，或者自由规划路径的起始和终点位置必须要保持的工具姿态等等。由于用户一般更加在意工作空间而非关节空间中的任务完成情况，这类约束经过机械臂运动学模型的转换后往往是非线性的。 g 为非线性不等式约束，大多数情况下用来描述机械臂和环境物体的碰撞。在碰撞处理中，约束 g 可软可硬。硬约束即必须要满足的约束，比如机械臂任何时刻任何部件都禁止与工作台发生碰撞。而软约束则允许一些小幅度的违例发生，比如工艺中一些不需要绝对严格遵守的加工姿态限制等。软约束的好处是通过牺牲少量无关紧要的约束，来极大地提高路径规划的成功率和效率。其实上文中提到的伪凸分解也是基于软约束的思想，通过将原本严格的最优凸分解问题松弛成一定误差范围内的非严格凸包覆盖问题，从而大大提升了凸分解的效率。 一个优化问题的可行解定义为满足所有硬约束的解，最优解则是在满足约束的基础上同时具有最低的代价。就像山川地貌存在各种不同高低的盆地，有些最优解是局部的，对应某个山头附近的最低海拔。而有些解则是全局最优，即整个山川中海拔最低的盆地。对于一般的非线性问题寻找全局最优解所需要的计算资源可能非常巨大。因此在实际工程应用中，一般退而求其次寻找局部最优解。 具体到实际机械臂规划问题中，每个关节都对应一个转角度数和角速度，再加上和外部环境的碰撞交互，形成了巨大的关节搜索空间和约束检测。因此传统采样方法的效率随着机械臂自由度和环境复杂程度的上升而迅速下降。其中一个主要原因是采样算法无法有效利用关节空间中距离相近的采样点，从而造成计算资源上的浪费。然而机械臂路径规划是在一个具有连续性和确定性（考虑静态环境）的系统下进行的。换句话说，系统任何一个状态都可以通过该状态一定邻域范围内的其他状态来估计（确定性）并且误差是可控的（连续性）。轨迹优化算法正是利用了这个思想，将原本庞大复杂的非线性优化问题，拆分成数个小规模的邻域子问题并依次迭代求解，最终得到原始非线优化问题的局部最优解。这里必须重点强调解的局部最优特性。因为不像采样类方法会随机探索整个关节空间，轨迹规划一般会从一个给定的初始状态开始迭代优化，因此大多数情况下只能找到距离初始状态最近的那个局部最优解。但是对于机械臂路径规划这个问题，局部最优解往往可以应付绝大多数问题，原因如下： 代价并不是用户所严格追求的目标，比如关节空间中的路径长度是否达到全局最短并不重要，只要整个路径中存在的弯路尽量少即可。 与其他机器学习问题不同，轨迹优化问题存杂大量可靠的初始条件可以利用，比如人工示教或者专家库的经验，关节转角线性插值，步长较大的粗采样轨迹输出等等。 不过由于优化变量是关节空间中的向量，而碰撞约束和任务约束则一般定义在工作空间中，因此关节空间中的邻域需要转换成到工作空间中的邻域作进一步计算。设当前关节状态向量为 x，机械臂运动学模型 Fm(x) 输出机械臂第 m 个轴在基坐标系下的六自由度末端位姿。引入关节状态扰动量 δx，则机械臂各轴在工作空间中产生的位姿扰动可以通过关节雅可比获得。 这里有一个技术细节问题。由于末端姿态是通过齐次欧式变换矩阵描述的，简单地将两个欧式变换矩阵相加一般并不能得到一个齐次欧式变换矩阵。这是因为所有齐次欧式变换矩阵都是被约束在 SE3 李群空间中而非自由矩阵。为了处理这种情况，机器人运动学模引入了旋量坐标，即欧式变换群的切空间。如果机械臂状态向量的每个轴分量为 q，即 x=( q1,q2,… )。那么机械臂正运动学姿态可以通过各个关节的螺旋轴 ω 的指数乘积形式获得。 在旋量框架下，雅可比矩阵是通过乘积方式定义的，从而解决了在加法定义下欧式变换约束被破坏的情况。由于其良好的数学性质，基于 SE3 旋量的运动学计算在导航建图 SLAM 领域中已逐渐成为主流。 回到机械臂轨迹优化问题上，给定一个机械臂关节状态 x，利用旋量雅可比矩 Jm [x] 可以通过简单的矩阵运算近似得到任意扰动下的关节状态 x+δx 所对应的每个轴部件的位姿，避免了大量多余的采样。雅可比矩阵在轨迹优化算法中意义重大，尤其是对于各种复杂的工作空间约束起到了加速的作用。比如，当关节状态发生扰动时，每个轴的凸包产生的变化便可以通过对应的关节雅可比矩阵进行估计。这种线性近似的思想极大地加速了碰撞矢量的计算，仅仅需要当前的距离矢量和穿透矢量，结合当前关节凸包模型的雅可比矩阵，便可估计出当关节状态发生任意扰动变化时距离矢量和穿透矢量的变化量，无需反复进行碰撞计算。 将局部关节空间中的运动规划问题在运动学近似下简化为一个局部凸问题，这是轨迹优化算法的第二个凸。 五. 规划计算中的凸：逐步二次规划 对于各类约束 g (x) 则采用线性近似 G ( p; X ) 处理。 通过上述局部凸近似，原本针对变量 x 的全局优化问题转换为一系列关于当前状态 X 扰动量 p 的局部优化问题，其数学上的描述为：给定当前代价二次近似 F ( p ; X ) 、等式线性约束近似 H ( p ; X ) 、不等式线性约束近似 G ( p ; X ) 、全局线性约束 l ( X+p ) 以及信赖域常数 Δ，求解如下线性约束二次规划子问题： 其中一些关键细节的解释如下: 原始问题的非线性代价函数被局部二次近似代替，线性化后的约束则通过惩罚项来落实。 自适应常数为信赖域大小，用来控制每次迭代更新的步长。这里使用了无穷范数，即向量的每个分量的绝对值都必须小于信赖域范围。Δ 的大小会根据每次迭代中的局部凸近似和原始非线性问题之间的吻合程度来自动调整。 自适应常数 μ 为惩罚系数。当 μ → +∞ 时，约束下的线性约束二次规划问题和原始问题的最优解是等价的。μ 的大小会根据当前约束满足情况自适应放大。 | а |+ = max (0,a) 为有效约束的激活函数。如果某个约束已经满足（小于零）则不会激活，反之会根据当前约束偏差大小来惩罚违背约束的分量。另一方面，对于等式约束来说，惩罚 | а | 是精确的因此不需要激活函数。 上述局部二次形式虽然是凸问题，但由于惩罚项的存在导致子问题是不可微分的，可以通过引入松弛变量将问题转换为解决。 结合上述各个模块，逐步二次规划求解器的算法流程伪代码如下: while 原始优化问题没有收敛 do while 迭代凸近似没有收敛 do 计算当前代价和凸近似 while 信赖域大小合理 do 求解凸近似松弛形式得到更新量p 计算能使凸近似和原始问题保持一致的最大步长ρ 如果步长被接受 扩张Δ 退出信赖域循环 否则 缩小Δ 如果信赖域大小小于阈值 直接退出当前循环，跳到步骤 (✦) 如果代价与惩罚的合函数下降幅度或者信赖域大小小于阈值 判定迭代凸近似收敛 如果约束满足容忍阈值 (✦) 判定整个优化过程收敛 否则 放大惩罚系数 μ 通过迭代求解多个凸优化子问题，求解原始非线性问题的局部最优解，这便是轨迹优化算法的第三个凸。 六. 大界工业臂柔性仿真系统案例 在该演示中，机械臂轨迹的初始和终止状态被限定在工作台上两个相隔数米的位置。运动规划器需要在实现初始和终止状态之间自由路径规划的同时保证机械臂和正在被用户实时调整姿态的工件之间不发生碰撞。这里红色粗线为轨迹优化系统根据环境中的改变实时计算出的轨迹。为了展示规划效果，图中机械臂被设定为沿着规划出的状态路径来回移动。当工件离机械臂距离大于安全阈值的时候，机械臂的运动轨迹并不会被工件的空间位置所影响。一旦工件进入可能的碰撞区域，轨迹优化算法会迅速作出反应，在不影响完成任务的情况下，尽可能和工件保持距离。 在该演示中，导轨上的机械臂需要绕过工件到工作台的另一边去实现某焊接任务（轨迹终点处的直线运动）。同时在运动规划的过程中，工件上某一块板筋在用户的设计操作下发生位置和高度上的变化。为了防止在自由路径中发生碰撞，大界实现的轨迹优化算法会根据板筋和机械臂之间的安全碰撞距离自动实时调节自由段路径。 相比之下，采样类算法对于相同的场景则无法实现实时轨迹规划，仅能对整个交互过程中少数的几个工件状态作出反应。同时路径采样的随机性非常大，存在不必要的冗余路径。 本期轨迹优化算法的技术详解到此结束，如果您有任何疑问或想法，欢迎与RoLAP实验室取得联系，RoLAP将坚持开放合作，积极与行业内的专家学者及企业共同交流和探索智能制造创新之路，同时实验室诚邀对机械臂算法、视觉算法、图形算法、软件开发等感兴趣的优秀人才加入，一起玩转硬核研究。三. 几何模型中的凸：层级凸包碰撞
仿真计算中对于物理世界建模的精细程度直接决定了运动规划算法的可行性和最优性。尤其是对于运动机构和物理世界的碰撞处理更是机械臂规划中的难点。区别于无人小车无人机等自主运动平台，机械臂占用的空间无法通过一个简单的质点或者球包来近似。机械臂运动规划算法需要严格计算各个运动机构和障碍物之间的几何关系。
通过上文描述的碰撞模型和运动学近似处理，物理世界中的机械臂规划问题已经被翻译成纯粹的数学优化问题。大界在这里采用了逐步二次规划算法求解这类问题。逐步二次规划算法作为优化领域中最常用的数值求解算法之一，通过反复迭代求解一系列更简单的二次规划子问题，可以在满足约束的情况下驱使原始代价函数收敛到局部最优解。所有逐步二次规划算法都用到了原始问题的局部凸近似，即给定当前系统状态 X ，原始问题在局部扰动状态 x = X+p 下的近似。当 p 扰动足够小的时候，代价函数 f (x) 可以使用局部二次近似 F ( p ; X ) 来逼近。
基于上文讨论的三大模块，大界研发了一套基于轨迹优化的工业机械臂柔性仿真系统，实现了冗余自由度机械臂和工件之间的实时规划与交互。本文的结尾通过两个仿真案例来展示这套系统，演示场景为大界现有的切割单站，主要由一个吊装在龙门线轨上的六轴工业机械臂和工作台上的工件组成。演示中的路径规划的代价函数设定为关节空间中的角度转动距离。换句话说，优化算法总是尝试寻找一条能使关节变化最少的路径。
6+2 机械臂导轨工作站和工件模型之间的实时交互
2023-07-11
