最近拿到了个六轴机械臂，需要在上面搞些事情。正好重新整理一下之前看过的东西，特此根据自己的理解记录下来。仅供参考，非专业且错误地方尽管指正。

机器人分很多类，无人机，移动机器人，机械臂，聊天机器人，下棋机器人等等，然而我们描述的是可做出动作的机器人。由于本人的原因，将会围绕一款innfos-Gluon-6L3六轴机械臂进行说明。此款机器人3d模型和细节可从此链接下载 wiki.innfos.com/wiki/cn

对于机器人工程师来说，一个机器人可以是一种由不同机构组成的，在给定的约束下进行一定的动作的机械模型。所以描述机器人时，便可利用机械结构之间的约束来确定机器人的几何关系，并构建机器人运动学模型，动力学模型，再规划运动轨迹。

所以机器人工程师的初级进阶之路就是，位置->速度->加速度->路径规划->轨迹规划->运动规划->....。接下来，我将一步步进行攻破。

一、位姿（Pose、Configuration）

首先是位置。当描述一个空间点时，我们经常使用位置（Position）。然而对于机器人，大部分是由几个转动副和几个移动副组成的机械机构。所以光有位置还不够，还需要姿态（Orientation），用来确定方向。

1）位置：在三维空间可以用笛卡尔坐标系位置坐标XYZ，3个量来表示。

2）姿态：在三维空间可以用RPY来分别表示关节点按照x轴、y轴、z轴旋转roll（翻滚角）、pitch（俯仰角）、yaw（偏航角）个角度。

这3+3个量就描述了一个关节在空间的6个自由度（DoF），合称为位姿。位姿是相对的，例如下图，

Gluon-6L3的3D图

在固定的基坐标系{B}（Base Frame）下，转动关节（Revolute Joint）的末端坐标系{E}（End Frame）的位姿可以描述为「相对于{B}的位姿为（X，Y，Z，roll，pitch，yaw）」。一般来说是相对于全局坐标系{U}（Universal Frame）的，若{B}与{U}重合，则可省略相对于。

有了一个关节的位姿之后还不够，我们还需要了解每个关节位姿之间的几何关系。

目前已经有很多种成熟的机器人的几何关系描述方法，T矩阵描述法、D-H（Denavit-Hartenberg）法，MDH（Modified D-H）法、MCPC（Modified Complete and Parametrically Continuous）法、旋量法等等。

本篇先介绍T矩阵法。

二、T矩阵法

T矩阵法为机械臂最基本的建系方法。是通过关节的6个自由度的位姿信息来表示两个坐标系之间的齐次变换矩阵（Homogenous Transformation Matrix），通过矩阵相乘的形式对各个坐标系间的映射关系（Mapping）。

上式表示坐标系i-1到坐标系i的变换矩阵，也可认为是关节点i在关节点i-1坐标系下的坐标。其中由3个量表示的位移矩阵（Translate Vector） [公式]，3个向量组成的旋转矩阵（Rotation Matrix）R来表示方向信息，分别是法线（normal） [公式]、指向（orientation） [公式] 、接近（approach） [公式] 向量，分别对应自身运动坐标系的x、y、z轴。

当我们知道了关节两两之间的变换关系后，就可以通过串串的方式求出从头到尾或者过程之间任意两个关节之间的关系了。

上式表示基坐标系{B}到末端坐标系{E}的变换关系。

三、基础T矩阵

位姿有6个自由度，所以可以对应4个基础T矩阵，分为两组：

1）1个位移矩阵

2）3个旋转矩阵

他们之间按顺序搭配相乘可算出T矩阵。此时顺序分为两组：

1）以固定坐标系为参考，进行坐标变换。

例如初始位姿为(X:0,Y:0,Z:0,roll:0,pitch:0,yaw:0)要相对于固定坐标系（x，y，z）依次进行变换（1）绕z轴转90度；（2）绕y轴转90度；（3）接着平移[4,-3,7]。

初始位姿矩阵为

则 [公式] ，新的位姿为 [公式]

可看出新的动作矩阵要依次左乘。

2）以当前坐标系为参考，进行坐标变换。

例如初始位姿为(X:0,Y:0,Z:0,roll:0,pitch:0,yaw:0)要相对于当前旋转坐标系 [公式] 依次进行变换（1）绕 [公式] 轴转90度；（2）绕 [公式] 轴转90度;（3）接着沿[公式]轴平移[4,-3,7]。

初始位姿矩阵为

则 [公式] ，新的位姿为 [公式]

可看出新的动作矩阵依次右乘。