开始学习《Robotics，Vision and Control》，今天是学习的第一篇章，因为项目需要，

需要在Matlab上做一个仿真实验，由于是串联机器人系统，不涉及到移动机器人系统，因此

本系列文章主要是学习和介绍书中相关章节。

位姿的表示

主要讲述了怎么去描述2D点和3D点的位姿，重点是相对位姿的表达。

1)一个点可以用一个坐标向量来描述，此坐标向量表示了此点相对于参考坐标系的位移

2)由一系列点组成的刚体可以由一个单独的坐标系来描述，并且它的成员点可以用相对于此坐标系

的位移来描述

3)一个目标坐标系的位置和姿态称为它的位姿

4)一个坐标系相对于另一个坐标系的相对位姿可以用ξ来表示

5)一个点相对于一个坐标系的坐标向量可以通过点乘一个相对位姿转换到另一个坐标系下

在2D空间表达位姿

在2D空间中一般使用平移量和一个旋转量来表示点的位姿，R∼(x,y,θ)，如下图所示：

将{B}中的点P在{A}中表示：

其中相对位姿的表达：

在Matlab中变换矩阵的计算：

其中1，2分别表示平移量x,y。30*pi/180表示的是旋转量，表示旋转了30°。

我们可以在matlab中把这种旋转变换绘制出来，有两种方式，一种是静态的，只显示变换之后

的结果，用trplot2;另一种是动态的显示，它可以动态地显示出变换的过程，用tranimate函数(只适用于三维)：

其中T1表示要绘制的变换矩阵，注意这里的变换都是相对于世界坐标系的变换后的结果，可选项

‘frame’，‘1’，表示此结果坐标系我们用{1}来标识它，后面的该坐标系的颜色。

注意：坐标变换的先后顺序是不能更换的，不同的变换顺序会得到完全不一样的结果

Matlab中点的绘制方式：

此外我们还需要注意几个Matlab函数，因为一般的坐标都做过齐次处理：

有时候我们仅仅只需要前两个坐标值，而不需要齐次项，这时候我们可以用h2e函数和homtrans函数

描述三维位姿

三维坐标系是二维坐标系的扩展，多了一个z来表达，三维空间中的点自然就用(x,y,z)来表示，也可以表示成

向量t(x,y,z)，如图所示：

在三维空间中也从两个方面来考虑：平移和旋转。

1、三维空间中的姿态

三维空间中的变换顺序和二维的一样是不能交换顺序的，变换顺序将影响最终结果。数学上有几种方式来描述

旋转：正交旋转矩阵、欧拉角、旋转轴和单位四元素。

正交旋转矩阵

关于x,y,z轴旋转一个角度的正交旋转矩阵分别为：

在Matlab中很方便地可以利用一些函数计算旋转矩阵：rotx(a)、roty(a)、rotz(a)

同样地，我们可以用trplot(R)和tranimate(R)来静态地或动态地显示变换结果。

**从以上可以看出，正交矩阵有9个元素，但它们不是独立的，每个列向量的模是1和三个

列向量相互正交各有3个约束，因此正交矩阵只有3个独立的变量**

欧拉角

根据旋转的顺序不同欧拉角有两种：一种是有绕重复轴旋转的欧拉角，如XYX,XZX等6种，

另一种是每次旋转都绕不同轴的卡尔达纳，如XYZ,XZY等6种。

每一种形式都应用于特定的领域，在航空和机械领域用得比较多的就是ZYZ形式，在Matlab机器人工具箱中也是用的这种形式的欧拉角

在matlab中可以用相应的函数很方便地计算：

上述两种方式完全等价

有时候我们知道变换矩阵了，我们需要知道对应这个矩阵的欧拉角，可以用tr2eul()函数

注意欧拉角中的Y方向旋转的角度不能是0或负值，或kpi,会奇异

另一种被广泛使用的欧拉角是XYZ(roll-pitch-yaw)

在matlab中相应的函数是：rpy2r(x,y,z)以及tr2rpy(R)

这种XYZ形式的欧拉角在Y旋转角度为(2k+1)pi/2的情况下会奇异

旋转轴

两个特定姿态的坐标系是通过空间中某个轴的旋转而联系在一起的。

R就是世界坐标系绕着轴V旋转theta角得到的。其中theta和V分别是R的特征值和对应的特征向量。

在matlab中可以利用eig函数来求一个矩阵的特征值和对应的特征向量的：

其中V的列向量就是lambda中对应的列特征值所对应的特征向量，对于每一个正交旋转矩阵来说，总是有一个特

征值为1，其所对应的特征向量就是对应的旋转轴

在Matlab中可以用angvec2r()函数来得到旋转轴对应的变换矩阵

单位四元素

四元素一直以来都备受争议，但它却在机器人中得到了很广泛地应用。四元素是复数的扩展，是一个标量加一个矢量组成：

Matlab中可以直接用Quaternion()来将矩阵转换为四元素：

所有和四元素有关的操作都在Quaternion这个类中，其有很多的成员函数，如q.R将四元素转换为矩阵形式，

q.plot将其绘制出来。

将平移和旋转组合起来

前面讨论的都是只考虑旋转，而没有考虑平移，接下来将结合两者一起来讨论。

有两种非常实用的表示方式：四元素和4x4的齐次变换矩阵

在Matlab中，用transl()和trotx()等函数来进行计算：

其中transl只有旋转，trotx只有旋转，相应的troty(),和trotz()

我们可以使用t2r(T)将T中旋转部分提取出来，使用transl(T)'将平移部分提取出来：

此次只是一些基础知识，二维和三维中位姿的描述方式，但却是后面一切学习的根基。