关于MATLAB中的机械臂算法的分析和介绍

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

path

trajectory

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

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

从A到B叫path：

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

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

上面这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 看成是控制点（controlpoints）。机械臂的end-effector并不会真正通过这些controlpoints，而在他们之间找一条平滑的曲线。这样做的目的是在平滑加速度和曲线拟合的准确性之间找一个折衷方法。

(图片来自于Wikipedia)

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

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

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

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

1. Trajectory interpolated in task space

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

2.Trajectory interpolated in joint space

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

角度方向插值(orientation interpolation)

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

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

MATLAB 中的机械臂路径规划算法

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

MATLAB函数：

Simulink模块：

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

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

生成角度方向的 trajectory

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

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

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

Simulink示例

在MATLABfileexchange里搜索“TrajectoryPlanningforRobotManipulators”，会找到一个用多种方式产生trajectory的例子。

用一张表格总结如下：

这里有两点值得注意：

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

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