对直角坐标系位姿变换及描述的理解

任一刚性物体在空间中的静止状态需要由位置及姿态唯一确定，位置描述涉及平移变换，而姿态描述涉及旋转变换，如何在空间中准确描述一物体的位姿，需要了解物体相对于某个确定坐标系的位姿信息，如若知道多个坐标系之间的相对位姿信息，即可获得该物体相对于中间任意坐标系的位姿描述，这也是坐标系转换的前提。

空间描述

位置描述：平移变换

若仅描述两点之间的相对位置，用向量描述即可。首先需确定参考坐标系，才能确定 XYZ 各轴分量，其矩阵描述如下：

姿态描述：旋转变换

要想描述刚性物体在空间中的状态，仅有位置信息是不够的，还需要有姿态信息，即确定与物体固连的坐标系相对与参考坐标系的位姿。通常我们给出旋转矩阵来描述这种相对姿态信息，而这仅仅用于表示两者之间的相对姿态，并不体现变换过程。

两坐标系 ， 之间的旋转矩阵 ，所指的就是 在 中的姿态描述：

，， 为坐标系 三坐标轴上的正单位向量；，， 为坐标系 三坐标轴上的正单位向量。我们可以清晰地看出旋转矩阵是由 系中各主轴单位向量分别在 系中的投影（模长均为 1）。

理解两坐标系的变换关系： 是描述 系通过外旋或者内旋变换后与 系一致的旋转矩阵。

旋转矩阵具有一些性质例如：旋转矩阵是正交阵，即旋转矩阵的逆等于旋转矩阵的转置

坐标系描述

若想完全表示两坐标系的相对状态，需要位置以及姿态信息，所以我们将两者组合用于准确描述两坐标系之间的相对位姿，例如 在 中的位姿表示。

混合变换

如下图，描述坐标系 在坐标系 下的位姿

坐标系 中 P 向量的位姿描述转换到坐标系 中的位姿描述：（ 为 在 中的方向描述， 为 在 中的方向描述），列出下列式子：

求得坐标系中的向量转换到坐标系中的表示，

逆变换

若已知 相对于 的变换 ，要求 相对于 的变换 ，即求矩阵的逆。直接的办法就是对转换矩阵求逆，也可以利用 变换的性质进行推导：

旋转矩阵：

平移矩阵：

变换矩阵：

姿态的其他描述方法（欧拉角、四元数、旋转向量（或称为轴角））

欧拉角

欧拉角的描述如下图所示，表示两个物体间的相对姿态 (通过三个角度 、、 描述姿态)，一般以自身坐标进行旋转的角度称为欧拉角，与以固定参考系进行旋转的角度 RPY 角有所不同。

欧拉角的旋转轴和旋转顺序

表示旋转的欧拉角旋转顺序有 12 种：

Proper/classic Euler angle:z-x-z，x-y-x，y-z-y，z-y-z, x-z-x，y-x-y

Tait-Bryan angle（也称作 RPY 角）：x-y-z，y-z-x，z-x-y，x-z-y，z-y-x，y-x-z

Proper/classic Euler angle 说明这些角度并不是独立的，例如当下面的旋转组合：先绕 x 轴旋转 90 度，再绕 y 轴旋转 90 度，最后绕 x 轴旋转 -90 度，这一系列组合得到的效果与只绕 z 轴旋转 -90 度是一样的。也就是说我们仅仅在 2 个平面上进行旋转（其中一个平面上必须进行两次旋转）同样可以得到任意的三维旋转！

欧拉角内旋和外旋

内旋指绕着动坐标系主轴旋转，外旋指绕着静坐标系主轴旋转，内旋右乘，外旋左乘。欧拉角的表述用于确定姿态变换的过程，不同的变换过程确定不同的绕轴旋转顺序以及角度。下列表给出欧拉角的旋转顺序及角度计算

四元数

一般情况下，两个复数相乘 ，其实就是旋转 + 伸缩两种变换，也就是两个复数的模相乘（伸缩），辐角相加（旋转），而为了探究三维空间中的旋转变换，爱尔兰数学家哈密顿利用复数发明了四元数，用于描述旋转变换。四元数并不是生来为了解决三维旋转，而是它的性质非常有利于表达旋转信息。四元数相比其他描述形式的优点：

• 解决万向节死锁（Gimbal Lock）问题
• 仅需存储 4 个浮点数，相比矩阵更加轻量
• 四元数无论是求逆、串联等操作，相比矩阵更加高效

四元数组成

四元数从表达意义上理解和轴角相差无几，也是使用 4 个量表示绕此转轴的旋转角度。四元数由 1 个实数和 3 个虚数组成，其表达为 ，其中 ，，，。四元数表达中的 代表旋转轴单位矢量， 代表绕旋转轴旋转的角度大小，并且 ，这便是四元数的常规表达形式

四元数中的每个数都是经过"处理"的轴和角，轴角描述的四个量并不是一个空间下的东西，首先 是一个 3 维坐标下的矢量，而 则是极坐标下的角度，简单地将它们组合到一起并不能保证它们插值结果的稳定性，因为它们无法归一化，所以不能保证最终插值后得到的矢量长度（经过旋转变换后两点之间的距离）相等，而四元数是在一个统一的 4 维空间中，方便归一化来插值，又能方便地得到轴、角这样用于 3D 信息数据，所以用四元数再合适不过了。相比于矩阵，四元数也只要存储 4 个浮点数，优势很明显。

四元数使用

四元数的共轭：

四元数的模长：

四元数的逆：

单位四元数：，逆等于其共轭

以 为轴，将 旋转 度（右手定则），得到的新向量为

旋转向量

旋转向量也被称为轴角，其表达 中 代表旋转轴单位矢量（即 ），而 代表绕着旋转轴旋转的角度大小，四元数的组成意义虽和轴角相同，但是其表达形式却不相同。利用轴角进行姿态变换的方法称为轴角法，其具有两个优点也有两个缺点：

优点 1：同轴的两次旋转可以直接相加来等效为一次旋转；

优点 2：定义简单，相对直观；

缺点 1：轴角法表示不唯一，当旋转角度是 0 的时候，旋转轴可以是任意的；

缺点 2：两次任意的连续旋转无法合成；

欧拉角、旋转矩阵、四元数、旋转向量之间的转换关系（右手系最常用，教材、OpenCV 以及 Eigen 中都是用右手系，本文也以右手系为准）

先导知识

有一些几何变换的推导文章，公式都是正确的，但是很多时候可能会很迷惑，因为他们的公式中的符号可能是相反的。在我们初识三维坐标系时，大都是右手坐标系，但是有些理论是以左手笛卡尔坐标系为基础，所以时常需要进行左手系与右手系的相互转换。

不管是左手系转右手系还是右手系转左手系，我们都需要这样一个转换矩阵：(此转换矩阵需根据两坐标系之间的轴对应关系确定，在此因两坐标系仅 Z 轴相反，所以转换矩阵如下)

左手系转右手系

对于向量 ，将其从左手坐标系转换到右手坐标系：

对于旋转矩阵 ，将其从左手坐标系转换到右手坐标系：

右手系转左手系

对于向量 ，将其从右手坐标系转换到左手坐标系：

对于旋转矩阵 ，将其从右手坐标系转换到左手坐标系：

左手系用左手确定正方向，右手系用右手确定正方向。

欧拉角 ↔︎ 旋转矩阵

欧拉角 → 旋转矩阵

以右手系下的单轴旋转矩阵获得 顺序欧拉角旋转矩阵

旋转矩阵 → 欧拉角

利用欧拉角转旋转矩阵求反解：

观察旋转矩阵各元素特点求解 顺序下各轴旋转角度 ，如下所示，其他旋转顺序的欧拉角可同理求得。

四元数 ↔︎ 旋转矩阵

四元数 → 旋转矩阵

对于三维坐标的旋转，可以通过四元数乘法直接操作，与旋转矩阵操作可以等价，但是表示方式更加紧凑，计算量也可以小一些。

若已知四元数

利用 Rodrigues 公式(公式为右手系公式)，将右手系罗德里格公式中的元素按 在前的的格式进行整理后，最终可得右手系下的旋转矩阵：

其中 ，，，，，，， 为转动角度。

旋转矩阵 → 四元数

由上式（4.11）的旋转矩阵 R，利用矩阵中元素可反向计算得出以下 4 组计算式

那么应该选以上 4 种的哪一种呢？有学者建议首先判断 中哪一个最大，则先计算该元素，然后再通过计算方式二中公式计算其他三个分量。因为如果 ，除法就没有意义 (分母不能为 0)；如果 非常小，将会出现数值不稳定。为什么这里可以不用考虑开方取正根还是负根的问题？因为此处用到了负四元数的性质，那就是 和 代表相同的旋转。所以只要保证他们的符号是相对一致即可，并且负号并不影响结果。

旋转向量（轴角）↔︎ 旋转矩阵

旋转向量（轴角）→ 旋转矩阵

假设有旋转向量 (轴角) 中，旋转轴单位向量 ，旋转角度为 ，转换后的旋转矩阵为 ，那么 到 的转换其实可以参照四元数转旋转矩阵：

旋转矩阵 → 旋转向量（轴角）

根据上述旋转矩阵元素排列特点，可得出下述计算式：

根据矩阵各元素对应相等，可求解 。

欧拉角 ↔︎ 四元数

欧拉角 → 四元数

欧拉角构造四元数，跟欧拉角构造旋转矩阵一样，就是把三个基础旋转组合在一起，将 欧拉角（或 角：绕固定坐标系的 依次旋转 角）转换为四元数：

四元数 → 欧拉角

根据上述欧拉角转四元数公式（4.15）可以求出逆解，即由四元数 到欧拉角 的转换为：

由于 和 的取值范围在 之间，只有 180°，而绕某个轴旋转时范围是 360°，因此采用计算机计算时要使用 函数代替 函数：

四元数转欧拉角计算过程中可能会出现旋转自由度丢失的情况，例如俯仰角为 90 度时，会出现 无法计算，所以需要对具体情况进行阈值判断。