空间描述与变换

1.位姿与坐标系描述

（1）位置描述

矢量 Ap 表示箭头指向点的位置矢量，其中右上角标“A”表示该点是用{A}坐标系描述的。

 

数学表达形式如下：

 

（2）姿态描述

坐标系{B}与机械手末端工具固连，工具的姿态可以由坐标系{B}的方向来描述。而坐标系{B}的方向可以用沿三个坐标轴的单位矢量来表示

 

​​​

（3）位姿描述

固连坐标系把刚体位姿描述问题转化为坐标系的描述问题。图2-3中坐标系{B}可以在固定坐标系{A}中描述为

 

旋转矩阵 描述坐标系{B}的姿态，矢量 描述坐标系{B}的原点位置。

程序示例

例如绕x、y、z轴旋转做旋转变换，输入MATLAB程序

R = rotx(pi/2)

R = roty(pi/2)

R = rotz(pi/2)

输出结果如下：

R =

    1.0000         0         0

         0    0.0000   -1.0000

         0    1.0000    0.0000

R =

    0.0000         0    1.0000

         0    1.0000         0

   -1.0000         0    0.0000

R =

    0.0000   -1.0000         0

    1.0000    0.0000         0

         0         0    1.0000

如果改变角度的输入方式，可以使用如下指令

R = rotx(30, 'deg')

R = roty(30, 'deg')

R = rotz(30, 'deg')

输出结果如下：

R =

    1.0000         0         0

         0    0.8660   -0.5000

         0    0.5000    0.8660

R =

    0.8660         0    0.5000

         0    1.0000         0

   -0.5000         0    0.8660

R =

    0.8660   -0.5000         0

    0.5000    0.8660         0

         0         0    1.0000

我们可以使用trplot（）函数查看图形表示相应的3D坐标系，使用tranimate（）函数可以用动画的方式展示世界坐标系旋转为3D坐标系的过程

trplot（R）

显示结果如下：

输入

tranimate（R）

输出图形如下

 

2.平移和坐标系映射

（1）平移坐标变换

BP为坐标系{B}描述的某一空间位置，我们也可以用AP（坐标系{A}）描述同一空间位置。因为两个坐标系具有相同的姿态，同一个点在不同坐标系下的描述满足以下关系

 

MATLAB中坐标平移的函数为transl(x, y, z)

输入

T = transl(5, 15, 5)

则平移变化矩阵为：

T =

     1     0     0     5

     0     1     0    15

     0     0     1     5

     0     0     0     1

（3）旋转坐标变换

旋转坐标变换的任务是已知坐标系{B}描述的一个点的位置矢量BP和旋转矩阵，

​​​​​​​ 

求在坐标系{A}下描述同一个点的位置矢量AP。

 

将上式写成矩阵形式得：

 

上式即为我们要求的旋转变换关系，该变换是通过两个坐标系之间的旋转变换实现的。

在机器人工具箱中，提供了trotx（）、troty（）、trotz（）函数计算旋转变换矩阵。

输入

T = trotx(pi/2)

运行结果为

T =

    1.0000         0         0         0

         0    0.0000   -1.0000         0

         0    1.0000    0.0000         0

         0         0         0    1.0000

（3）复合变换

如果两个坐标系之间即存在平移又存在旋转，如何计算同一个空间点在两个坐标系下描述的变换关系？

 

为了得到位置矢量BP和AP之间的变换关系，我们建立一个中间坐标系{C}。

为了得到位置矢量BP和AP之间的变换关系，只需坐标系{B}在坐标系下{A}的描述。

3.齐次坐标变换

（1）齐次变换

坐标变换可以写成以下形式

 

将位置矢量用4´1矢量表示，增加1维的数值恒为1，我们仍然用原来的符号表示4维位置矢量并采用以下符号表示坐标变换矩阵

 

是4´4矩阵，称为齐次坐标变换矩阵。可以理解为坐标系{B}在固定坐标系{A}中的描述

齐次坐标变换的主要作用是表达简洁，同时在表示多个坐标变换的时候比较方便。

示例

先给出旋转矩阵R（x，60。），平移矢量p=[1 2 3]T

输入

T=transl(1,2,3)*trotx(pi/3)

运行结果为：

T =

    1.0000         0         0    1.0000

         0    0.5000   -0.8660    2.0000

         0    0.8660    0.5000    3.0000

         0         0         0    1.0000

(2) 齐次变换算子

在机器人学中还经常用到下面的变换，如图，矢量AP1沿矢量AQ平移至的AQ终点，得一矢量AP2。已知AP1和AQ求AP2的过程称之为平移变换，与前面不同，这里只涉及单一坐标系。

​​​​​​​

 

 

可以采用齐次变换矩阵表示平移变换

称为平移算子，其表达式为

 

 

其中I是3´3单位矩阵。例如若AQ=ai+bj+ck，其中i、j和k分别表示坐标系{A}三个坐标轴的单位矢量，则平移算子表示为

 

 

同样，我们可以研究矢量在同一坐标系下的旋转变换，如图，AP1绕Z轴转q角得到AP2。则

 

Rot(z,q)称为旋转算子，其表达式为

 

同理，可以得到绕X轴和Y轴的旋转算子

    

定义了平移算子和旋转算子以后，可以将它们复合实现复杂的映射关系。变换算子与前面介绍的坐标变换矩阵形式完全相同，因为所有描述均在同一坐标系下，所以不需上下标描述（坐标系）。

 

(3) 复合变换

复合变换主要有两种应用形式，一种是建立了多个坐标系描述机器人的位姿，任务是确定不同坐标系下对同一个量描述之间的关系；另一种是一个空间点在同一个坐标系内顺序经过多次平移或旋转变换，任务是确定多次变换后点的位置。

 

如图表示的三个坐标系，已知坐标系{A}、{B}和{C}之间的变换矩阵        和  位置矢量CP，求在坐标系{A}下表示同一个点的位置矢量AP。

 

根据坐标变换的定义得

 

解析：

输入

>> R = rotz(pi/3)

输出

R =

    0.5000   -0.8660         0

    0.8660    0.5000         0

         0         0    1.0000

（3）

 

输入

>> P = rotz(pi/3)* [3;7;0] + [10;5;0]

输出

P =

    5.4378

   11.0981

         0

 

解析

（1）  

输入

>> T = transl(4,3,0) * trotz(pi/6)

输出

T =

    0.8660   -0.5000         0    4.0000

    0.5000    0.8660         0    3.0000

         0         0    1.0000         0

         0         0         0    1.0000

（2）  

输入

>> T0 = trotz(-pi/6) * transl(-4,-3,0)

输出

T0 =

    0.8660    0.5000         0   -4.9641

   -0.5000    0.8660         0   -0.5981

         0         0    1.0000         0

         0         0         0    1.0000

4.刚体的旋转

（1）rpy角

rpy角是描述船舶航行时的姿态的一种方法，将船的行驶方向作为Z轴，则绕Z轴旋转称为滚动（Roll）角α，将绕Y轴(与海面平行)方向的旋转称为俯仰（Pitch）角β，取X轴与海面垂直方向，将绕X轴的旋转称为偏转（Yaw）角γ。机械臂末端的定义类似，故习惯上称为rpy角。

描述运动坐标系的规则是：首先使运动坐标系的初始方位与固定坐标系重合，将运动坐标系绕固定坐标系X轴转动γ，再将运动坐标系绕固定坐标系Y轴转动β，最后将运动坐标系绕固定坐标系Z轴转动α。因为三次转动都是相对固定坐标系的，所以相应的旋转矩阵。

 

将三个矩阵相乘得到

 

它表示绕固定坐标系的三个轴依次旋转得到的旋转矩阵，称为绕固定轴XYZ旋转的rpy方法。下面讨论逆问题：从给定的旋转矩阵得到绕固定轴XYZ旋转的rpy角γβα。令

 

上式中有3个未知数，9个方程，其中6个不独立，因此可利用其中3个解出未知数。

，如果cos(β)不为零，则可以得到  

 

 

>> R = rotx(0.2)*roty(0.3)*rotz(0.1)

采用rpy角的运算方式为

>> R = rpy2r(0.2,0.3,0.1)

输出

R =

    0.9506   -0.0954    0.2955

    0.1563    0.9693   -0.1898

   -0.2683    0.2266    0.9363

其逆运算为

>> gamma = tr2rpy(R)

输出

gamma =

    0.2000    0.3000    0.1000

（2）绕运动系ZYX转动的euler角

描述运动坐标系的规则是：运动坐标系的初始方位与参考坐标系重合，首先将运动坐标系绕Z轴转动α，再将运动坐标系绕Y轴转动β，最后将运动坐标系绕X轴转动γ。这种描述方法中各次的转动都是相对运动系的，而不是相对固定坐标系的。相应的旋转矩阵为

 

将三个矩阵相乘得到

 

结果与绕固定轴XYZ旋转相同，这是因为绕固定轴旋转的顺序与绕运动轴旋转的顺序相反，且旋转角度对应相等。因此，用ZYX euler角与XYZ rpy角的描述方法是等价的。

另外一种常用的euler角方法是ZYZ方法，首先使运动坐标系与参考坐标系重合，将运动坐标系绕Z轴转动α，再将运动坐标系绕Y轴转动β，最后将运动坐标系绕Z轴转动γ。

 

将三个矩阵相乘得到

 

如果sin(β)不为零，ZYZ方式euler角逆解是

 

>> R = rotx(0.2)*roty(0.3)*rotz(0.1)

采用欧拉角角的运算方式为

>> R = eul2r(0.2,0.3,0.1)

输出

R =

    0.9506   -0.0954    0.2955

    0.1563    0.9693   -0.1898

   -0.2683    0.2266    0.9363

其逆运算为

>> gamma = tr2eul(R)

输出

gamma =

    0.2000    0.3000    0.1000