三维情况是上一节的二维情况（二维空间位姿描述）的延伸。在二维坐标系上增加一个额外的坐标轴即可，通常用z表示，它同时与x轴和y轴正交。z轴的方向服从右手规则，并构成右手坐标系。与各坐标轴平行的单位向量记作$\hat{x}$、$\hat{y}$和$\hat{z}$：
$$\hat{z}=\hat{x}\times \hat{y},\hat{x}=\hat{y}\times \hat{z},\hat{y}=\hat{z}\times \hat{x}$$
坐标系中的一个点P可用其x，y和z的坐标值（x，y，z）或者一个约束向量表示：
$$P=x\hat{x}+y\hat{y}+z\hat{z}$$

图1 展示了一个相对于参考坐标系{A}的坐标系{B}。显然，{B}的原点通过向量t=（x,y,z）进行平移，然后再通过某种复杂的方式进行旋转。做法还是同前面一样，从任意一点P出发，然后再确定${}^{A}p$和${}^{B}p$之间的关系。仍然是两方面考虑：旋转和平移。

1.2.1 三维空间姿态描述

图1中显示了两个右手坐标系，其方向差异很大，将坐标系{A}旋转，直到与坐标系{B}方向一致。首先考虑绕单个坐标轴的旋转。下图所示显示了一个右手坐标系，以及它绕不同坐标轴旋转不同角度的情形。

但绕坐标轴连续旋转就没那么简单了，下图演示了一个坐标系按不同的顺序旋转的情形，可以看到虽然旋转角度相同，但最终的坐标系完全不同，它取决于旋转的顺序。

目前有很多种表示旋转的方法，后面将会介绍几种：正交旋转矩阵，欧拉和卡尔丹角，旋转轴与角度，以及单元四位数。他们都可以表示为向量或矩阵，即MATLAB的自然数据类型，或者表示为一个工具箱自定义的类。

1.2.1.1正交旋转矩阵

用相对于参考坐标系的坐标轴单位向量表示它们所在坐标系的方向。每个单位向量有三个元素，他们组成了$3\times 3$阶正交矩阵${}^A{R}_B$:
$$\left(\begin{array}{c}{}^{A}x\\ {}^{A}y\\ {}^{A}z\end{array}\right)={}^A{R}_B\left(\begin{array}{c}{}^{B}x\\ {}^{B}y\\ {}^{B}z\end{array}\right)$$
上式将一个相对坐标系{B}的向量旋转为相对于坐标系{A}的向量。矩阵R属于属于特殊三维正交群。它具有正交矩阵的性质，如$R^T=R^{-1}$以及$det(R)=1$。
分别绕x,y,z轴旋转$\theta$角后的标准正交矩阵可表示为：
$$R_x(\theta )=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right)$$
$$R_y(\theta )=\left(\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right)$$
$$R_y(\theta )=\left(\begin{array}{cccc}\cos \theta & -\sin \theta & 0& \\ \sin \theta & \cos \theta & 0& \\ 0& 0& 1& \end{array}\right)$$

• MATLAB程序实现：

1. 绕x轴旋转

>>R = rotx(pi/2);   #绕x轴旋转π/2
>>trplot(R);    #绘制相应的坐标系
>>tranimate(R);    #旋转动画，展示了世界坐标系旋转到指定坐标系的过程
R =

    1.0000         0         0
         0    0.0000   -1.0000
         0    1.0000    0.0000

2. 先绕x轴旋转，再绕y轴旋转。

>>R = rotx(pi/2) * roty(pi/2)
>>trplot(R)
R =

    0.0000         0    1.0000
    1.0000    0.0000   -0.0000
   -0.0000    1.0000    0.0000

3. 如果颠倒以上旋转顺序，可以看出旋转的不可交换性

>>R = roty(pi/2) * rotx(pi/2)
R =

    0.0000    1.0000    0.0000
         0    0.0000   -1.0000
   -1.0000    0.0000    0.0000

得到的结果完全不同

1.2.1.2 三角度表示法

欧拉旋转定理要求绕3个轴依次旋转，但不能绕同一轴连续旋转两次。旋转顺序分为两种：欧拉式和卡尔丹式。

• 欧拉式：绕一个特定的轴重复旋转，但不是连续的：XYX、XZX、YXY、YZY、ZXZ、或ZYZ。
• 卡尔丹式：绕三个不同的轴旋转：XYZ、XZY、YZX、YXZ、ZXY或ZYX。
  一般来说，所有这些序列均被称为欧拉角，共有12种形式可供选择。

ZYZ序列的欧拉角表示为
$$R=R_z(\varphi )R_y(\theta )R_z(\psi )\text{\hspace{1em}(1)}$$
欧拉角是一个三维向量$\Gamma =(\varphi ,\theta ,\psi )$。
例 1：要计算$\Gamma =(0.1,0.2,0.3）$的等价旋转矩阵，可以表示为：

>>R = rotz(0.1) * roty(0.2) * rotz(0.3)
>>R = eu12r(0.1, 0.2, 0.3)      #两个表达都可
R =

    0.9021   -0.3836    0.1977
    0.3875    0.9216    0.0198
   -0.1898    0.0587    0.9801

上述问题的逆命题就是找到给定旋转矩阵的欧拉角：

>>gamma = tr2eul(R)
gamma =

    0.1000    0.2000    0.3000

但是，如果$\theta$为负时，其反函数结果与原来的数值不同，它返回了一个正的$\theta$值和两个不同的$\varphi 、\psi$值，但这一组欧拉角对应的旋转矩阵仍与前一组是相同的，两组不同的欧拉角对应同一个旋转矩阵，说明从旋转矩阵到欧拉角的映射不是唯一的，而工具箱函数返回的角度始终为正。

>>R = eu12r(0.1, -0.2, 0.3)
R =

    0.9021   -0.3836   -0.1977
    0.3875    0.9216   -0.0198
    0.1898   -0.0587    0.9801
#反函数结果为
>>tr2eul(R)
ans =

   -3.0416    0.2000   -2.8416
>>eu12r(ans)
ans =

    0.9021   -0.3836   -0.1977
    0.3875    0.9216   -0.0198
    0.1898   -0.0587    0.9801

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

    0.9211   -0.3894         0
    0.3894    0.9211         0
         0         0    1.0000
>>tr2eul(R)
ans =

         0         0    0.4000

对于$\theta =0$的情况，反函数返回的角度值与原值完全不同。在这种情况下，从式（1）得出的旋转矩阵是
$$R=R_z(\varphi )R_z(\psi )=R_z(\varphi +\psi )$$
由于$R_y=I$，所以可简化为只是$\varphi +\psi$的一个函数。对于逆运算而言，只能确定这个和的值。要想得到其中每个值，只能按惯例取$\varphi =0$。$\theta =0$的情况实际上是一个奇异点。
另一种广泛使用的旋转角顺序是横滚-俯仰-偏航角，即
$$R=R_x({\theta }_r)R_y({\theta }_p)R_z({\theta }_y)$$
它用于描述船舶、飞机和车辆的姿态时非常直观。横滚、俯仰和偏航（也称为倾斜、姿态和航向）是指分别绕x、y、z轴的旋转。这个xyz角序列，即专业上的卡尔丹角，也被称为泰特-布莱恩角或导航角。
例2：对于航空及地面车辆而言，通常定义x轴为前进的方向、z轴垂直向下、y轴指向右手方向。

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

    0.9363   -0.2751    0.2184
    0.2896    0.9564   -0.0370
   -0.1987    0.0978    0.9752
>>gamma = tr2rpy(R)    #反函数结果
gamma =

    0.1000    0.2000    0.3000

1.2.1.3绕任意向量旋转

对于空间中的两个任意姿态的坐标系，总是可以找到一个旋转轴，使其中一个坐标系绕这个轴旋转可以和另一个坐标系的姿态重合。

>>R = rpy2r(0.1, 0.2, 0.3)
R =
    0.9363   -0.2751    0.2184
    0.2896    0.9564   -0.0370
   -0.1987    0.0978    0.9752
>>[theta, v] = tr2angvec(R)    #theta是旋转的角度大小，v是绕其旋转的向量
theta =
    0.3655
v =
    0.1886    0.5834    0.7900
>>[v, lambda] = eig(R)    #求取R矩阵的特征值和特征向量
v =
   0.6944 + 0.0000i   0.6944 + 0.0000i   0.1886 + 0.0000i
  -0.0792 - 0.5688i  -0.0792 + 0.5688i   0.5834 + 0.0000i
  -0.1073 + 0.4200i  -0.1073 - 0.4200i   0.7900 + 0.0000i
lambda =
   0.9339 + 0.3574i   0.0000 + 0.0000i   0.0000 + 0.0000i
   0.0000 + 0.0000i   0.9339 - 0.3574i   0.0000 + 0.0000i
   0.0000 + 0.0000i   0.0000 + 0.0000i   1.0000 + 0.0000i

一个正交矩阵总有一个实特征值$\lambda =1$，以及一对共轭复特征值$\lambda =\cos \theta \pm i\sin \theta$,其中$\theta$代表旋转的角度。根据特征值和特征向量的定义有
$$Rv=\lambda v$$
其中，$v$是对应于$\lambda$的特征向量。当$\lambda =1$时：
$$Rv=v$$
意味着对应的这个特征向量$v$是不随旋转而改变的。这样的向量只有一个，而旋转就是以这个轴为向量发生的。上面的例子中第三个特征值等于1，所以旋转轴应是矩阵v中的第三列。