1、机器人运动学方程

 为机器人的每一个连杆建立一个坐标系，并用齐次变换来描述这些坐标系间的相对关系，也叫相对位姿。通常把描述一个连杆坐标系与下一个连杆坐标系间相对关系的齐次变换矩阵叫做A变换矩阵或A矩阵。

 如果矩阵表示第一连杆坐标系相对于固定坐标系的齐次变换，则第一连杆坐标系相对于固定坐标系的位姿为

其中：

如果矩阵表示第二连杆坐标系相对于第一连杆系的齐次变换，则第二连杆坐标系在固定坐标系的位姿可用和的乘积来表示，并且应该右乘。

同理，若矩阵表示第三连杆坐标系相对于第二连杆坐标系的齐次变换，则有：

如此类推，对于六连杆机器人，有下列矩阵：

  → 机器人运动学方程

此式右边表示了从固定参考系到手部坐标系的各连杆坐标系之间的变换矩阵的连乘，左边T6表示这些变换矩阵的乘积，也就是手部坐标系相对于固定参考系的位姿。上式计算结果是一个如下的(４×４) 矩阵:

2.正向运动学及实例

正向运动学主要解决机器人运动学方程的建立及手部位姿的求解问题。

如图3.24所示，SCARA装配机器人的三个关节轴线是相互平行的，{0}、{1}、{2}、{3}分别表示固定坐标系、连杆1的动坐标系、连杆2的动坐标系、连杆3的动坐标系，分别坐落在关节1、关节2、关节3、和手部中心。坐标系3即为手部坐标系。连杆运动为旋转运动，连杆参数为变量，其余参数均为常量。该机器人的参数如表3.4所示。

图3.24  SCARA装配机器人的坐标系

表3.4  SCARA装配机器人连杆参数

该平面关节型机器人的运动学方程为

      其中：—连杆1的坐标系相对于固定坐标系的齐次变换矩阵；

         —连杆2的坐标系相对于连杆1的齐次变换矩阵；

         —手部坐标系相对于连杆2的齐次变换矩阵。

为手部坐标系(手部) 的位姿。由于其可写成(４×４)的矩阵形式，即可得向量ｐ、ｎ、ｏ、ａ，把、、代入可得。

如图3.24(ｂ)所示，当转角变量分别为＝30°，＝60°，＝30°时，则可根据平面关节型机器人运动学方程求解出运动学正解，即手部的位姿矩阵表达式

3.反向运动学

反向运动学解决的问题是：已知手部的位姿，求各个关节的变量。在机器人的控制中，往往已知手部到达的目标位姿，需要求出关节变量，以驱动各个关节，使手部的位姿得到满足，这就是运动学的反向问题，也称逆运动学。对于逆运动学问题，其求解步骤如下：

（1）根据机械手关节坐标设置确定出，由关节变量和参数确定机器人运动学方程:

（2）根据工作任务确定机器人的各连杆坐标系相对于基座坐标系的位姿。为机械手末端在直角坐标系（参考坐标或基坐标）中的位姿，由任务确定。

 （3）由和（i=1，2，…，6），求出相应的关节变量 或 。

分别用（i=1，2，…，5）的逆左乘上式有

根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可求出关节变量或。

上述求解的过程称为分离变量法，即将一个未知数由矩阵方程的右边移向左边，使其与其他未知数分开，解开这个未知数，再把下一个未知数移到左边，重复进行，直到解出所有未知数。

 应该注意，求解机器人的逆解时，可能存在的问题有：解不存在或解的多重性。由于旋转关节的活动范围很难达到360°，仅为360°的一部分，即机器人都具有一定的工作区域，当给定手部位置在工作区域外时，则解不存在。由于关节的活动范围的限制，机器人有多组解时，可能有某些解，机器人关节不能达到。一般来说，非零的连杆参数越多，达到某一目标的方式越多，运动学逆解的数目越多。所以，应该根据具体情况，在避免碰撞的前提下，按“最短行程”的原则来择优，即每个关节的移动量最小。又由于工业机器人连杆的尺寸大小不同，因此应遵循“多移动小关节，少移动大关节”的原则。

综合实例

‍斯坦福机器人的运动学分析‍

该机器人有6个自由度，除关节3为移动关节外，其余关节均为转动关节。其结构示意图如图3.25所示。其中关节1和关节2的轴线相互垂直，关节2和关节3的轴线相互垂直，关节3和关节4的轴线相互平行，关节4和关节5的轴线相互垂直，关节5和关节6的轴线相互垂直。

图3.25  斯坦福机器人

1.斯坦福机器人各连杆的参数

２.建立各连杆坐标系

按Ｄ－Ｈ方法建立斯坦福机器人各连杆坐标系,如图3.26所示。图中轴沿关节１的轴，轴沿关节ｉ＋１ 的轴，令所有轴与机座坐标系轴平行，轴按右手坐标系确定。

图3.26  斯坦福机器人的坐标系

３.两杆之间的位姿矩阵

４.机器人的运动方程

假如H=0，则n、 o 、 a三个方向矢量不变，而位置矢量的分量 、、 分别为

代入本例给出的已知参数值和变量值，求得数值解为

该(4×4)矩阵即为斯坦福机器人在题目给定情况下手部的位姿矩阵，即运动学正解。

例题：已知各关节转角，求末端位姿

（１）是从到绕旋转的角度；

（２）是从到沿测量的距离；

（３）是从到沿测量的距离；

（４）是从到绕旋转的角度。

解：

①确定D-H坐标系

②确定各连杆D-H参数和关节变量

③求出两杆间的位姿矩阵

④求末杆的位姿矩阵

⑤验证