论文1解析（复现）：五由度采摘机械臂关节空间轨迹规划

本论文是：五自由度采摘机械臂关节空间轨迹规划，卢君宜，熊晓松，朱东

关键词：DH，正逆运动学，摆线轨迹规划，无振动，无过冲。

首先什么机械臂的轨迹规划：

机械臂轨迹规划是指根据操作任 务 的要求 ，计算出预期的运动轨迹 ，然后根据此预期 轨迹 ，实时计算机械臂运动时的位移 、速度和加速度 ，从而生成运动轨迹。

机械臂的运动轨迹算法很多，

比如（1）采用 连续遗传算法规划了 6R 操作臂的运动轨迹，（2）使用 齐次变换矩阵表示机械臂末端执行器运动到达的 目标位置 ，并在关节空 间 内采用二次多 项式插 补程序 来实 现相 邻 轨迹 的平 滑过 渡，（3）为求更 好 、更均 匀 的运动 用 四元素法改进 Paul的方法，此外还有多项式，三次样条，B样条等运动轨迹算法。

而本文采用的是摆线的运动策略，简单可靠，在轨迹函数、速度函数、加速度函数连续、并且从加速度函数入手，选取计算简单的摆线函数作为轨迹规划的函数。

1. 正运动学

采用D-H来求五自由度的正运动学算法（DH法网上很多，这里不详细介绍了）。建立坐标系

DH参数表为：

从而得到：

这里通过几何求出来的末端坐标，可以根据二连杆的方式进行迭代，最终求出xyz坐标。这里需要注意的是通过几何的方法可以求出坐标A，B，C三点的（x，y，z），这对后面的运算非常重要。

2. 逆运动学

 此机械臂并没有采用传统的解析法进行求解，而是采用了几何法进行求解，我们来讨论一下。

是不是乍一看很唬人，不要拍，一点点来分析。

第一：我们知道一看到这样的公式，首先就排除了传统的利用齐次变换的方式进行求解逆矩阵，有好处也有坏处：好处：根据已知条件和机械臂的位姿可以求出唯一解，但是这样也是坏处，《机器人导论》等一些书籍利用求齐次变换求转置的方式进行求解，当然，解出来的解，可能不知一个，那么就需要进行寻找最小的值，最优解，还有验证行列式不为0（防止奇异解）。

第二：几何的方式，就不得不利用三角函数的知识和向量的点积、叉乘的这几个本质的概念，（有些时候并不是简单所理解的那样，而是空间，机器人学的坐标系方式去理解）

第三：还有最重要的一点，需要进行画辅助线，来进行求解。

好了，我们来顺理一下

theta1 = ractan（y，x）这个应该没有疑问。

theta2 = arccos（[L1^2+|AC|^2-L2^2]/2L1|AC|）-.......

画上辅助线来分析：

式子的第一个是在求角CAB = theta2+角1+角4。角4的A、C、T点都已知，所以可求。角1， 和和红线进行平行的，注意并没有构成一个平行四边形，看着很像其实并不是，所以角1等于角2，角2等于角3，这里为啥说角2等于角3，因为T是在3维空间中，T平面的夹角一定等于他的arctan（z坐标，根号下x^2+y^2）,所以式子是正确的。

theta3  =....

这个很好求，因为ABC点都知道了，说以theta3就利用三角公式进行求解了。

theta4 = ....

类似在做一下辅助线。

这样子看很清楚了把，所以就可以求出theta4的坐标了 ，等于角ACB+角ACT。到此正逆解的算法已全部ok。

3.轨迹规划

当我们有T从一个点到另一个点进行运行，此时也就知道了起始点和终止点的坐标信息。比如P1（628,228，-306）和P2点（765，-391，-106）。也就是从P1点运动到P2点。

我们利用摆线的方式进行轨迹规划，它的优点是简单，使角度位置和速度曲线平滑，为 了使采摘机械 臂能 够平稳 地从 当 前位姿运 动到 指定位姿 ，要求 机械臂 在启 动 、运 行和停止 时 都应平 滑稳定 。 南于摆线运动 曲线光 滑 、速 度 和加速 度连续 ，且 能在 有限 区间的端点处产生零速度和零加速度 ，非常适用 于采摘机械 臂的点到 点的关 节空 间 轨迹 规划。

这一部分，买了书，还没有到，到了给补上。

 怎么实现呢？这样子实现，首先我们知道了起始点到终止点的各个关节的角度变化，之后，我们需要设定一个时间走过这个过程，即

而此时就能求出，关节角位置的曲线方程，同时关节角速度，角加速度都可以求出来了，如图：

4.雅克比矩阵的应用

我们知道，根据串联机械臂的正逆运动学可以求出他的雅克比矩阵，求雅克比矩阵常用的有3种，即

其实用那种并不介意，结果一样的，我们就可以求出串联机器人末端空间与关节空间的速度之间的关系（线速度和角速度）。

之后我们利用J雅克比矩阵，求解出末端的线速度关系（这里不细说雅克比了）。即