3D坐标变换
引言：
在现实世界中，我们更多处理的是三维的坐标以及姿态数据，当我们处在一个场景中时，我们会有自己的三维坐标和姿态，与二维一样，相同的目标在不同的坐标系下，就会有不同的表达。那么，如何将这些表达在不同的坐标系之间进行变换，就是我们将要研究掌握的知识。
接下来我会带着大家介绍关于3D坐标系相关的知识。
学习目标：
了解三维坐标系下姿态的表达方式
能够描述目标的三维位置和姿态
能够编写程序描述三维坐标系的变换矩阵
能够编写程序计算目标在不同的三维坐标系下的表达
3D坐标系变换场景
假如我们使用了一个无人机在三维场景下进行目标侦测，无人机相对于世界坐标系的位置已知。
已知无人机在世界坐标系下的位置为 ({\delta}x,{\delta}y,{\delta}z) ，并且发现目标相对于无人机坐标系的位置为 (x_r, y_r, z_r)。如何计算目标点（下图红点）在世界坐标系的位置？
当无人机的坐标系在原点时，以自己为中心绕着的世界坐标系X轴进行了 {\gamma} 度的旋转，得到了目标相对于无人机的位置 。然后接着又以自己为中心绕着世界坐标系的Z轴进行了 \alpha 度的旋转，得到了目标相对于无人机的位置，那么如何根据这些位置得到目标在世界坐标系下的位置？
- 先绕X轴旋转 \gamma 度、接着绕Z轴旋转 \alpha 度
那么再进一步，如果无人机的变换既包含了旋转也包含了平移，那么其观察到的目标点位置 (x_r, y_r, z_r)，根据当前无人机的旋转平移信息，如何转换得到目标点在世界坐标系下的坐标(x_w, y_w, z_w) ?
3D坐标系变换公式及示例
右手定则及旋转规则
右手定则
经典的右手定则如下：
三个轴相互垂直，且从X->Y->Z 为逆时针方向旋转。
我们也可能见到其他的右手坐标对应方式，如下，他们都满足坐标系逆时针旋转规则。
坐标系的旋转
如下图：
平移变换
我们依然先考虑最简单的平移关系，如下图所示，假如 红点目标 在 无人机坐标系 O_r 中的坐标为 (0, 3, 2) ， 无人机 在 世界坐标系 O_w 中的坐标为 (1,2,2.5) 。
则根据简单的空间几何关系，就可以知道目标在 世界坐标系 O_w 中的坐标如下：
\left\{
\begin{aligned}
x_w = 1.0 + 0.0 = 1.0 \\
y_w = 2.0 + 3.0 = 5.0 \\
z_w = 2.5 + 2.0 = 4.5
\end{aligned}
\right.
转换成符号表达如下：
\left\{
\begin{aligned}
x_w = \delta x + x_r \\
y_w = \delta y + y_r \\
z_w = \delta z + z_r
\end{aligned}
\right.
写成向量求和的表达如下：
\begin{bmatrix} x_{w} \\ y_{w} \\ z_w \end{bmatrix}=
\begin{bmatrix} x_{r} \\ y_{r} \\ z_w \end{bmatrix}+
\begin{bmatrix} {\delta}x \\ {\delta}y \\ {\delta}z \end{bmatrix}
写成齐次矩阵相乘的表达如下（详细推导可参见此链接： 机器人运动学3D坐标系平移）：
\left[\begin{matrix}
x_w \\
y_w \\
z_w \\
1
\end{matrix}
\right]
=
\left[\begin{matrix}
1 & 0 & 0 & \delta x \\
0 & 1 & 0 & \delta y \\
0 & 0 & 1 & \delta z \\
0 & 0 & 0 & 1
\end{matrix}
\right]
\cdot
\left[\begin{matrix}
x_r \\
y_r \\
z_r \\
1
\end{matrix}
\right]
旋转变换
我们先考虑一个轴的旋转。如下图所示，无人机坐标系 O_r 是由 世界坐标系 O_w 经过绕Z轴逆时针旋转 \alpha 度而来。
如果已知目标在 无人机坐标系 O_r 的位置 (x_r, y_r, z_r) 那么如何得到其在 世界坐标系 O_w 下的位置呢？
这里可以认为其Z轴的坐标与旋转无关，则根据二维可以将多元方程写成如下形式：
\begin{cases}
x_{w}=\cos \theta \cdot x_{r}-\sin \theta \cdot y_{r} + 0 \cdot z_r\\
y_{w}=\sin \theta \cdot x_{r}+\cos \theta \cdot y_{r} + 0 \cdot z_r\\
z_{w}=0 \cdot x_r + 0 \cdot y_r + 1 \cdot z_r
\end{cases}
以矩阵点乘的形式，变换公式如下：
\left[\begin{matrix}
x_w \\
y_w \\
z_w
\end{matrix}\right]
=
\left[\begin{matrix}
cos(\alpha) & -sin(\alpha) & 0 \\
sin(\alpha) & cos(\alpha) & 0 \\
0 & 0 & 1 \\
\end{matrix}\right]
\cdot
\left[\begin{matrix}
x_r \\
y_r \\
z_r
\end{matrix}\right]
我们将其中的 3x3 的旋转矩阵称命名为 R_z(\alpha)
我们把之前绕Z轴旋转的所有x，y，z依次换成y，z，x则可得到如下多元方程式：
\begin{cases}
y_{w}=\cos \theta \cdot y_{r}-\sin \theta \cdot z_{r} + 0 \cdot x_r\\
z_{w}=\sin \theta \cdot y_{r}+\cos \theta \cdot z_{r} + 0 \cdot x_r\\
x_{w}=0 \cdot y_r + 0 \cdot z_r + 1 \cdot x_r
\end{cases}
整理后可得：
\begin{cases}
x_{w}= 1 \cdot x_r + 0 \cdot y_r + 0 \cdot z_r\\
y_{w}= 0 \cdot x_r + \cos \theta \cdot y_{r}-\sin \theta \cdot z_{r} \\
z_{w}= 0 \cdot y_r + \sin \theta \cdot y_{r}+\cos \theta \cdot z_{r}
\end{cases}
以矩阵点乘的形式，变换公式如下：
\left[\begin{matrix}
x_w \\
y_w \\
z_w
\end{matrix}\right]
=
\left[\begin{matrix}
1 & 0 & 0 \\
0 & cos(\gamma) & -sin(\gamma) \\
0 & sin(\gamma) & cos(\gamma) \\
\end{matrix}\right]
\cdot
\left[\begin{matrix}
x_r \\
y_r \\
z_r
\end{matrix}\right]
我们将其中的 3x3 的旋转矩阵称命名为 R_x(\gamma)
- 先绕X轴旋转 \gamma 度、接着绕Z轴旋转 \alpha 度
这里并没有绕Y轴旋转，我们可以等同于认为绕Y轴的旋转为3x3的单位矩阵，即 \beta = 0 。且此时：
R_y(\beta) =
\left[\begin{matrix}
cos(\beta) & 0 & sin(\beta) \\
0 & 1 & 0 \\
-sin(\beta) & 0 & cos(\beta) \\
\end{matrix}\right]
=
\left[\begin{matrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1 \\
\end{matrix}\right]
我们可以认为这个过程是 先绕X轴旋转 \gamma 度、接着绕Y轴旋转 \beta = 0 度、最后绕Z轴旋转 \alpha 度 。我们将之总结为如下变换：
\left[\begin{matrix}
x_w \\
y_w \\
z_w
\end{matrix}\right]
=
R_z(\alpha) \cdot R_y(\beta) \cdot R_x(\gamma)
\cdot
\left[\begin{matrix}
x_r \\
y_r \\
z_r
\end{matrix}\right]
这里
由于无人机坐标系是由世界坐标系变换而来，则其变换矩阵又可以可记做 {^w_r}R_{zyx}，即以上变换可以简化为：
P_w = {^w_r}R_{zyx} \cdot P_r
其中，根据基本的矩阵相乘知识，可知 {^w_r}R_{zyx} 仍然是个 3x3 的矩阵，即可以写成如下形式：
{^w_r}R_{zyx} =
\left[\begin{matrix}
R_{11} & R_{12} & R_{13} \\
R_{21} & R_{22} & R_{23} \\
R_{31} & R_{32} & R_{33} \\
\end{matrix}\right]
完整的推导及计算可参见：
机器人运动学 | 3D坐标系旋转Z轴
机器人运动学 | 3D坐标系旋转综合
旋转+平移变换
实际应用中，绝大部分情况都是旋转和平移同时出现的，如下：
我们可以认为新的无人机坐标系是由世界坐标系，先经过旋转，后经过平移得到的。那么结合上边的旋转和平移矩阵：
\left[\begin{matrix}
x_w \\
y_w \\
z_w
\end{matrix}\right]
=
\left[\begin{matrix}
R_{11} & R_{12} & R_{13} \\
R_{21} & R_{22} & R_{23} \\
R_{31} & R_{32} & R_{33} \\
\end{matrix}\right]
\cdot
\left[\begin{matrix}
x_r \\
y_r \\
z_r
\end{matrix}\right]
\left[\begin{matrix}
x_w \\
y_w \\
z_w \\
1
\end{matrix}
\right]
=
\left[\begin{matrix}
1 & 0 & 0 & \delta x \\
0 & 1 & 0 & \delta y \\
0 & 0 & 1 & \delta z \\
0 & 0 & 0 & 1
\end{matrix}
\right]
\cdot
\left[\begin{matrix}
x_r \\
y_r \\
z_r \\
1
\end{matrix}
\right]
可以得到如下变换：
{^w_r}T
=
\left[\begin{matrix}
R_{11} & R_{12} & R_{13} & \delta x \\
R_{21} & R_{22} & R_{23} & \delta y \\
R_{31} & R_{32} & R_{33} & \delta z \\
0 & 0 & 0 & 1
\end{matrix}\right]
完整的推导及计算可参见：
机器人运动学 | 3D坐标系旋转平移叠加