Abstract: 在写代码的时候遇到了万向锁的问题。从头梳理了一下坐标变换和坐标系变换的相关内容。

一些约定

采用右手坐标系
大拇指指向坐标轴正向，四指方向为$\theta$角度正值（从时钟看是逆时针）

三个旋转矩阵

可见旋转矩阵，这里抄一下： $$ R_x = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(\theta) & -sin(\theta) \\ 0 & sin(\theta) & cos(\theta) \end{bmatrix} $$ $$ R_y = \begin{bmatrix} cos(\theta) & 0 & sin(\theta) \\ 0 & 1 & 0 \\ -sin(\theta) & 0 & cos(\theta) \end{bmatrix} $$ $$ R_z = \begin{bmatrix} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1 \end{bmatrix} $$

示例中坐标系的定义

定义两个坐标系，如下图，左为世界坐标系，右为相机坐标系。左的z表示朝向天空，x表示正前方。右的z表示相机正前方。

这里的推理主要由世界坐标系转换到相机坐标系，可以这么做：

world绕z轴顺时针旋转90°

所以$\theta = -90°$，旋转矩阵可以写作：

$$ R_z(\theta) = \begin{bmatrix} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix} $$

在此基础上，随后绕变换后x轴顺时针旋转90°

所以$\theta = -90°$，旋转矩阵可以写作： $$ R_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(\theta) & -sin(\theta) \\ 0 & sin(\theta) & cos(\theta) \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & -1 & 0 \end{bmatrix} $$

示例1：点随着角度绕坐标轴进行变换

这里说的是，坐标轴不变，点进行变换：

设有一个点坐标为:(1,2,3)，同样按照上述顺序进行旋转：

A点绕z轴顺时针旋转90°得B点，简单可知B点坐标为(2,-1,3)：

$$ {p}_{A} = [1,2,3]^T $$ $$ R_z(-\frac{\pi}{2}) = \begin{bmatrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix} $$

计算一下转换：

$$ R_z(-\frac{\pi}{2}) * p_A = \begin{bmatrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 \\ 2 \\ 3 \end{bmatrix} = \begin{bmatrix} 2 \\ -1 \\ 3 \end{bmatrix}=p_B $$

在此基础上，B点变换后x轴顺时针旋转90°得C点，简单可知C点坐标为(2,3,1)：

$$ {p}_{B} = [2,-1,3]^T $$ $$ R_x(-\frac{\pi}{2}) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & -1 & 0 \end{bmatrix} $$

计算一下转换：

$$ R_x(-\frac{\pi}{2}) * p_B = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & -1 & 0 \end{bmatrix} \begin{bmatrix} 2 \\ -1 \\ 3 \end{bmatrix}= \begin{bmatrix} 2 \\ 3 \\ 1 \end{bmatrix}=p_C $$

综上，当点绕坐标轴进行变换时，就直接怎么旋转就怎么写矩阵，例如：

$$ P_{new} = R_x(roll)R_z(yaw) P_{old} $$

注意：顺时针为负角度，逆时针为正角度

示例2：点不动坐标系变换

这里是说，点的绝对位置不动，坐标系旋转成一个新的坐标系，求此点在新坐标系下的坐标。

还是坐标$P_w$为(1,2,3)在camera坐标系下$P_c$是(-2,-3,1)

简单来写，坐标系的变换是world到camera，先z后x可得： $$ R_{wc} = R_z(-\pi / 2) R_x(-\pi / 2) = \begin{bmatrix} 0 & 0 & 1 \\ -1 & 0 & 0 \\ 0 & -1 & 0 \end{bmatrix} $$

可知，在camera坐标系下的点转换到world坐标系下的变换为： $$ P_w = \begin{bmatrix} 1 \\ 2 \\ 3 \end{bmatrix} = R_{wc} * P_c = \begin{bmatrix} 0 & 0 & 1 \\ -1 & 0 & 0 \\ 0 & -1 & 0 \end{bmatrix} \begin{bmatrix} -2 \\ -3 \\ 1 \end{bmatrix} $$

由于更重要的是world坐标系下的坐标变换到camera坐标系下可知： $$ P_c = R_{cw} * P_w $$ $$ R_{cw} = R_{wc}^{-1} = R_{wc}^T = (R_z(-\pi / 2) R_x(-\pi / 2))^T $$ $$ = R_x(-\pi / 2)^T R_z(-\pi / 2)^T = R_x(\pi / 2) R_z(\pi / 2) $$ $$ R_{cw} = \begin{bmatrix} 0 & -1 & 0 \\ 0 & 0 & -1 \\ 1 & 0 & 0 \end{bmatrix} $$

验证一下：

$$ P_c = \begin{bmatrix} -2 \\ -3 \\ 1 \end{bmatrix} = R_{cw} * P_w = \begin{bmatrix} 0 & -1 & 0 \\ 0 & 0 & -1 \\ 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} 1 \\ 2 \\ 3 \end{bmatrix} $$

简单总结一下，当坐标系产生变换的时候，若要把原坐标系下的点转换到新坐标系下，是连续在右边相乘矩阵，相反则取逆。

Last modified on 2026-04-09