​机器人学是一门很有趣的科学，因为它研究和控制的是实际物理世界中的对象，我们必须要遵照物理世界的一些规则来设计机器人的控制策略，才可能尽可能实现想要的控制效果。

前面，我们介绍过李群理论在视觉伺服与处理动力学约束上的应用。这次，我们来介绍一下李群理论在轨迹规划方面的应用。

视频放前面：

轨迹规划可以分为路径插补和时间计算两部分。

假设，我们通过人工示教的形式，得到了机器人要运行经过的几个路径点。但，由于受到物理世界的限制，我们无法让机器人在这几个路径点之间瞬移。我们必须要找到一条连续的路径，连接这些路径点。这个过程就是路径插补，通过路径规划，我们可以得到一条几何路径 ， 是 范围内的标量。

有了几何路径后，我们就可以以不同的速度沿着这条路径上运动，我们可以得到一个速度率 ，其中 是时间。这个过程就是时间计算，通过时间计算，我们就得到了一条连续、带时间戳的轨迹 。

上面简单介绍了一下轨迹规划的定义。具体的其他更详细介绍，推荐一本教材：

Biagiotti, Luigi, Melchiorri, Claudio. Trajectory Planning for Automatic Machines and Robots [J]. 10.1007/978-3-540-85629-0.

这里，我想讨论的是，路径插补对时间计算的影响。

我们知道，速度是一个矢量，受物理世界影响，速度的大小与方向都不可以发生突变（惯性）。

如果，我们采用最简单的线性插值连接每个路径点。那么，如上图所示，在过渡点处的速度方向必然会发生突变。在实际物理世界中，要么最终的轨迹偏离上述路径，要么机器人的 B 点处的速度降为 0。从而影响控制的精度与速度。

当然，见多识广的读者们肯定会想到说，机器人学的教材中介绍过，可以在 B 点处用圆弧或者多项式曲线进行过渡，从而让路径的切向不会发生突变。

于是，问题来了。姿态怎么过渡？或者说，姿态空间的圆弧或者多项式曲线是怎么定义的？（这其实是 RVBUST INC. 之前的一道面试题）

有人可能会说，四元数有一个 slerp 插值，可以做姿态空间的插补。是真的吗？

例如上图所示，一个刚体要在三个姿态之间插补出一条连续路径（位置相同）。Slerp 插值就是计算出两个姿态间的旋转轴，然后绕着旋转轴旋转。上面的例子用 Slerp 插值的效果如下：

Slerp 插值结果，图中绿色箭头是角速度方向

Slerp 插值实际上就是姿态空间的直线插值。所以，在中间过渡点处，角速度的方向会发现突变。

为了解决姿态插值问题，学术界（包括近两年 IROS 和 ICRA 等顶会）不断有关于高阶连续姿态插值的成果发布。在这里，我们不妨用李群表示论来看待这个问题：

对于线性空间（笛卡尔位置，关节空间等），我们可以很容易定义出两个状态 和 之间的直线插值：

对于姿态空间，两个状态 和 ，我们利用李群定义群内的运算：

• 加法：
• 逆运算：

于是可以推导出群空间的“减法”运算：

Logm 是矩阵的对数运算，具体可以参考一些介绍李群李代数基本运算的书，这里不赘述。

我们可以发现姿态空间的 Slerp 插值（直线插值）也可以表示成与线性空间类似的结构：

有了上面的知识，我们再来看轨迹插补。对于线性空间，我们可以利用多项式进行插补。而 Bezier 曲线就是一种构造多项式曲线的方式：

2 阶 Bezier 曲线5 阶 Bezier 曲线

从上面两个动图（出自 Wikipedia），我们可以发现，n 阶的 Bezier 曲线就是通过 n 个线性插值构造出来的。

我们有了任意李群空间的线性插值定义，就可以很自然地推导出任意李群空间的 Bezier 曲线定义 （多项式）。

于是，我们就可以得到 Bezier 过渡的姿态插值：

（左）Slerp 插值，（右）5 阶 Bezier 过渡插值。绿色箭头为角速度方向

如上动图所示，利用李群空间表示法获得的 Bezier 过渡，可以保证姿态轨迹的切向不会发生突变。

前面，我们在做机器人高速动态搬运的时候，其实也需要保证整个 SE(3) 空间的轨迹插补连续性，也是用到了这些技术。

末端姿态的角速度方向变化连续一些东西乱入了

点击链接，来如本与我们一起玩。