MATLAB机械臂的两种路径规划

什么是 trajectory（路径）规划

中文路径在英语中可能有两种翻译：

1. path

2. trajectory

首先告诉大家，我们所说的“路径”的是后者——trajectory。我们看一下这两种“路径”在机械臂的世界里有什么区别。

设想机械臂的 end-effector 要从 A 点运动到 B 点：

从 A 到 B 叫 path：

如果我们规定从 A 到 B 这个 path，必须在特定的时间（t0 – t6），经过这 7 个点。

那么这 7 个点叫 waypoints。路径（trajectory）指的是通过这 7 个点的具体计划（例如什么时间，以什么速度等等）。规划我们如何经过这 7 个点的算法，叫做路径规划（trajectory planning）算法。

上面这 7 个点，如果机械臂的 end-effector 在通过的时候，位置连续、速度连续、甚至加速度也是连续的，那么我们说这个 trajectory 是平滑的（smooth）。

机械臂中有几种比较常见的 trajectory：

Trapezoidal Trajectories（梯形路径）

梯形路径指的是：

end-effector 在相邻 waypoints 之间的速度是象梯形一样，先线性加速，然后速度保持不变，在接近目标后线性减速。梯形路径的优点是比较简单实用，但是在每个 waypoint 会有停顿。下图是一个机械臂走梯形路径的样例：在过 waypoints 的时候，会完全停止，所以走的是直线，速度和加速度肯定不连续。

Polynomial Trajectories（多项式路径）

其中又分三次多项式和五次多项式。分别写作：

三次多项式可以记录 end-effector 的速度、位置的信息。五次多项式可以记录 end-effector 的位置、速度、加速度的信息。下图是一个五次多项式的例子，可以看到速度、加速度都是连续的。三次或者五次多项式的 trajectory 在通过 waypoints 的时候速度都是连续的，这和梯形路径是不一样的。

下图是一个机械臂走五次多项式路径的样例：为了保证过 waypoints 的时候速度不停顿，走的是曲线。

B-splines（B样条曲线）

B 样条曲线将除了起始点和终点外，将其他的 waypoints 看成是控制点（control points）。机械臂的 end-effector 并不会真正通过这些 control points，而在他们之间找一条平滑的曲线。这样做的目的是在平滑加速度和曲线拟合的准确性之间找一个折衷方法。

(图片来自于Wikipedia)

如果一个 trajectory 只有这 7 个点，无论机械臂控制器的运动控制模块如何强大，都无法保证做到“平滑”的运行。我们必须对路径点进行插值（interpolation）。

下图的橙色点为插值点示意：

在了解插值后，我们来看一下路径规划出来的信息如何和传递到反向运动学（inverse kinematics）。反向运动学模块将确保end-effector按照规划出来的路径运行，即路径跟随（trajectory following）。

这里，我们按插值类型情况分两种模式：

1. Trajectory interpolated in task space

在这种模式下，中间插值的点是在 task space（即 XYZ 坐标系，或者叫笛卡尔坐标系）进行插值。下图表明了这一过程，这时候 trajectory generation 只需要做一次，产生所有的 waypoints 和 interpolation points （插值点）。然后运动控制系统在每个控制周期都必须调用 inverse kinematics 算法去做路径跟随（trajectory following）。

2.Trajectory interpolated in joint space

在这种模式下，中间插值点的点是在 joint space（即对关节角度插值）。下图表明了这一过程。这时候 trajectory generation 也只需要做一次，但做的过程和 task space 的做法不同。即在每一个 waypoint 调用一次 inverse kinematics，然后对关节角度的进行插值。Trajectory following 模块只需执行插值后的角度指令。

角度方向插值 (orientation interpolation)

我们刚才谈到的插值，都是针对 end-effector 的位置（position，即 XYZ 的坐标）。我们不应该忽略 end-effector 的角度方向（orientation）插值。我们看一段动画来了解一下，什么是 orientation 的插值：

可以看到，随着 end-effector 的移动，它的方向也在不断做出平滑的调整，在 end-effector 运行到目标位置后，它的方向也调整到位。当然，有很多场合角度方向是不需要调整的。例如：喷漆机械臂，永远将 end-effector 指向喷漆作业面。

MATLAB 中的机械臂路径规划算法
在 R2019a 发布后 Robotics System Toolbox (RST) 中多了几个机械臂路径规划的 MATLAB 函数和 Simulink 模块。

MATLAB 函数：

Simulink 模块：

不管是 MATLAB 函数还是类似 Simulink block，大概这么几项功能：

生成梯形或者多项式的位置 trajectory

生成角度方向的 trajectory

将上两者合二为一的生成既包含位置，也包含角度方向（合称 pose）的 trajectory——即基于 homogeneous transforms 的 pose trajectory。

让我们写几行代码，来产生一个三次多项式的 trajectory：

可以看到，位置是连续的。如果我们将速度 (qd)、加速度 (qdd) 画出来，可以看到 qdd 是不连续的。

Simulink示例
在 MATLAB file exchange 里搜索 “Trajectory Planning for Robot Manipulators”，会找到一个用多种方式产生 trajectory 的例子。

用一张表格总结如下：

这里有两点值得注意：

1. 无论是 MATLAB 函数还是 Simulink block，所有角度方向的插值都是基于 SLERP (spherical linear interpolation)。这是一种基于四元数（quaternions）的插值法。不会造成下图所示的“不走近路，绕远路”的问题。例如，角度要从 0 度转到 30 度。因为圆周是 360 度，错误的方式是从 0 度转到 330 度 – 虽然也能到达指定位置或者角度，但它是错误的。在manipJointTrajectory 模型中，因为用到了基于 joint space（关节角度）的插值，读者可以注意一下如何处理这个问题。

2. Time-scaling。在 manipTransformTrajectoryTimeScaling 模型中，如果不加 time scaling 这个模块，那么所有的角度变化和位置变化都是基于线性插值。如果加入了 “想要的” 速度、加速度信息 ---即 time scaling。我们可以使角度变化和位置变化按照我们想要的梯形或者多项式形式去调整。例如，我想让角度一开始不变，然后快速变化，最后速度为0。