2D坐标变换

引言：

当我们处在一个场景中时，我们首先要做的事情是确定我们自己的位置坐标。

事实上，这个就是无人车定位和导航的基本问题。为了定位无人车，我们必须先使用一个确定的坐标系作为参考。接下来我会带着大家介绍关于坐标系相关的知识。

学习目标：

了解位置和姿态（位姿）与坐标系相关概念

能够描述目标在二维坐标系下的位置和姿态

能够编写程序描述二维坐标系的变换矩阵

能够编写程序计算目标在不同二维坐标系下的表达

目标的位置和姿态

假如我们在场景里有这样一辆小汽车，他有固定的位置和方向，我们将其位置（Position）和方向（Orientation）合并在一起称之为位姿（Pose）。那么这里请问：这辆小车的位姿是多少呢？如果没有任何参照物或坐标系的情况下，我们就很难描述小车的位置和方向。

所以，我们为之添加坐标系 $O_a$ ，则可以看出来，其位置为 $(8, 6)$，旋转角度为 $30°$ （逆时针）。

2D坐标系变换场景

如果小车在如下地图中，并且方向向右，则其位姿为 $(6, 4, 0)$

此时，想象我们有一个原始坐标系B，此时可以认为坐标系A是坐标系B经过平移得到的。那么我们的小车在坐标系B下的位姿会是怎么样的呢？

如果有一个原始坐标系C，可以认为坐标系A是坐标系C经过旋转得到的，如下，那么小车相对于坐标系C的位姿是怎么样的？

如果坐标系A是其他通过平移加上旋转得到的呢，在下边这种情况下，小车相对于原始的坐标系C的位姿又该是怎么样的呢？

为了总结通用的解决方案，问题就来了，当我们知道小车在坐标系A下的位姿，如何确定小车在坐标系B和坐标系C下的位姿的。这就是我们接下来要介绍的坐标系变换。

实际上，知道了从坐标系B和C到坐标系A的变换关系，就可以帮我们确定目标小车在原始的坐标系B和C下的位姿。

2D世界与小车坐标系示例

为什么我们要获取目标在不同的坐标系下的位姿呢？我们通过以下的简单案例进行讲解并计算。

假如在一个世界坐标系 $O_w$ 下有一个小车和一个人，则很容易看出来他们两个在世界坐标系下的位置。

但事实上，我们通常能通过小车的摄像头或雷达等传感器，获取到人在小车坐标系 $O_r$ (下图的蓝色坐标系)下的位置 $(5.5, 4)$ ，那么如果我们只知道人在小车坐标系下的位置以及小车在世界坐标系下的位置，是否能够算出人在世界坐标系 $O_w$ 下的位置呢？

那么，我们将两个坐标系进行合并，如果已知小车在世界坐标系下的位姿，并且小车可以通过传感器获取目标在小车坐标系下的 $O_r$ 的位姿，我们就可以算出来人在世界坐标系 $O_w$ 的位姿。

当然，就如同刚刚讲过的坐标系变换场景问题，坐标变换除了纯粹的平移关系，还会有纯粹的旋转关系，以及最常见两者的结合（平移+旋转）。接下来我们的目标就是求出再不同的坐标系变换关系下，目标的位姿以及其变换关系。

平移变换

我们首先考虑平移的变换关系，如下图所示，目标在小车坐标系 $O_r$ （蓝色）的坐标为 $(5.5, 4)$，在世界坐标系 $O_w$ （黑色）的坐标为 $(9.5, 7)$ 。

则可知目标在世界坐标系 $O_w$ 下的位置与小车坐标系 $O_r$ 下的位置关系如下：

$$\begin{cases} x_{w}=x_{r} + 4\\ y_{w}=y_{r} + 3 \end{cases}$$

以上等式可以写成以下的向量求和的方式：

$$\begin{bmatrix} x_{w} \\ y_{w} \end{bmatrix}= \begin{bmatrix} x_{r} \\ y_{r} \end{bmatrix}+ \begin{bmatrix} {\delta}x \\ {\delta}y \end{bmatrix}$$

其中的向量 $\begin{bmatrix} {\delta}x \\ {\delta}y \end{bmatrix} = \begin{bmatrix} 4 \\ 3 \end{bmatrix}$ 描述了从世界坐标系$O_w$到小车坐标系$O_r$的平移关系。

旋转变换

接下来我们考虑两个坐标系之间的旋转变换关系。如下图所示，小车坐标系 $O_r$ （蓝色） 是由世界坐标系 $O_w$ （黑色）经过逆时针旋转 $\theta$ 度而来。

如果已知目标在小车坐标系 $O_r$ 的位置 $(x_r, y_r)$ 那么如何得到其在世界坐标系 $O_w$ 下的位置呢？

这里的小车坐标系可以认为是由世界坐标系通过逆时针旋转了θ角度而来，此时θ为正值。当然，也可以认为世界坐标系是由小车坐标系顺时针旋转θ角而来。根据几何或向量相关知识（详细可推导参见此链接：机器人运动学 | 2D坐标系旋转原理），可知：

$$\begin{cases} x_{w}=\cos \theta \cdot x_{r}-\sin \theta \cdot y_{r}\\ y_{w}=\sin \theta \cdot x_{r}+\cos \theta \cdot y_{r} \end{cases}$$

以上等式可以写成以下的矩阵相乘的方式：

$$\left[\begin{array}{l} x_{w} \\ y_{w} \end{array}\right] = \left[\begin{array}{cc} \cos (\theta) & -\sin (\theta) \\ \sin (\theta) & \cos (\theta) \end{array}\right] \cdot\left[\begin{array}{l} x_{r} \\ y_{r} \end{array}\right]$$

其中的 2x2 矩阵我们称之为旋转矩阵，并且只有旋转的角度θ有关，此矩阵有两个意义：

1. 该旋转矩阵描述了从一个坐标系到另一个坐标系的变换（逆时针旋转θ为正，顺时针旋转θ为负）
2. 旋转后的坐标系中的某一点通过左乘此旋转矩阵，可以得到其在旋转之前坐标系下的坐标

推导过程：

• 旋转前坐标系：

在黑色坐标系中(A坐标系)， $\vec {OP}$ 可以分解为 水平坐标方向的两个向量：

$$\vec {OP} = \vec {X_a} + \vec {Y_a}$$

每个方向上的向量又可以分解为标量和单位向量的乘积，$\vec {X_a} = x_a \cdot \vec {i_a}$ 和 $\vec {Y_a} = y_a \cdot \vec {j_a}$ 即有：

$$\vec {OP} = x_a \cdot \vec {i_a} + y_a \cdot \vec {j_a}$$

此时，$x_a$是x方向的标量，即p点在A坐标系中的x坐标位置，$\vec {i_a}$ 为x轴方向的单位向量； $y_a$是y方向的标量，即p点在A坐标系中的y坐标位置，$\vec {j_a}$ 为y轴方向的单位向量；

• 旋转后坐标系

同样有：

$$\vec {OP} = x_b \cdot \vec {i_b} + y_b \cdot \vec {j_b}$$

• 建立等式

对于向量$\vec {OP}$而言，无论在A坐标系还是B坐标系，他的方向和模长是没有变的，只是观察坐标系不同。那么：

$$x_a \cdot \vec {i_a} + y_a \cdot \vec {j_a} = x_b \cdot \vec {i_b} + y_b \cdot \vec {j_b}$$

由上图关系可推导：

两边同时右乘 $\vec i_a$ ：

$$x_a \cdot \vec {i_a} \cdot \vec {i_a}+ y_a \cdot \vec {j_a} \cdot \vec {i_a} = x_b \cdot \vec {i_b} \cdot \vec {i_a} + y_b \cdot \vec {j_b} \cdot \vec {i_a} \\ x_a = x_b \cdot \vec {i_b} \cdot \vec {i_a} + y_b \cdot \vec {j_b} \cdot \vec {i_a} \\ x_a = cos(\theta) \cdot x_b - sin(\theta) \cdot y_b$$

两边同时右乘 $\vec j_a$ ：

$$x_a \cdot \vec {i_a} \cdot \vec {j_a}+y_a \cdot \vec {j_a} \cdot \vec {j_a} = x_b \cdot \vec {i_b} \cdot \vec {j_a}+ y_b \cdot \vec {j_b} \cdot \vec {j_a} \\ y_a = x_b \cdot \vec {i_b} \cdot \vec {j_a} + y_b \cdot \vec {j_b} \cdot \vec {j_a} \\ y_a = sin(\theta) \cdot x_b + cos(\theta) \cdot y_b$$

注

$\vec{i_a} \cdot \vec{i_a} = |\vec{i_a}| \cdot |\vec{i_a}| \cdot cos(0) = 1$
$\vec{i_a} \cdot \vec{j_a} = |\vec{i_a}| \cdot |\vec{j_a}| \cdot cos(\pi / 2) = 0$
$\vec {j_b} \cdot \vec {i_a} = |\vec {i_b}| \cdot |\vec {i_a}| \cdot cos(90 + θ) = -sin(\theta)$
$\vec {i_b} \cdot \vec {j_a} = |\vec {i_b}| \cdot |\vec {j_a}| \cdot cos(90 - θ) = sin(\theta)$

将两式合并为矩阵形式：

$$\left[\begin{array}{l} x_{a} \\ y_{a} \end{array}\right] = \left[\begin{array}{cc} \cos (\theta) & -\sin (\theta) \\ \sin (\theta) & \cos (\theta) \end{array}\right] \cdot\left[\begin{array}{l} x_{b} \\ y_{b} \end{array}\right]$$

旋转+平移变换

我们已经知道了单纯平移和单纯旋转的变换关系是怎么样的了，但是现实世界中，往往是平移和旋转是共同发生的。接下来我们学习如何将这两种变换合并成同一个变换。

理论阐述：

如上图所示，小车坐标系 $O_r$ （蓝色） 是由 世界坐标系 $O_w$ （黑色）先经过逆时针旋转 $\theta$ 度（若 $\theta$ 为负则顺时针旋转），再按照世界坐标系进行平移 $({\delta}x, {\delta}y)$ 而来。

若已知目标在小车坐标系下的坐标为 $(x_r, y_r)$ ，则目标在 世界坐标系 $O_w$ 下的坐标 $(x_w, y_w)$ 为：

$$\left[\begin{matrix} x_{w} \\ y_{w}\end{matrix}\right]= \left[\begin{matrix} \cos (\theta) & -\sin (\theta) \\ \sin (\theta) & \cos (\theta) \end{matrix}\right] \cdot \left[\begin{matrix} x_{r} \\ y_{r} \end{matrix}\right] + \left[\begin{matrix} {\delta}x \\ {\delta}y\end{matrix}\right]$$

我们可以将之合并为一个 3x3 矩阵的形式（详细推导可参见此链接： 机器人运动学2D坐标系旋转平移总结）：

$$\left[\begin{array}{c} x_{w} \\ y_{w} \\ 1 \end{array}\right]=\left[\begin{array}{ccc} \cos (\theta) & -\sin (\theta) & {\delta}x \\ \sin (\theta) & \cos (\theta) & {\delta}y \\ 0 & 0 & 1 \end{array}\right] \cdot\left[\begin{array}{c} x_{r} \\ y_{r} \\ 1 \end{array}\right]$$

其中的数字1组成的 3x1 向量我们称之为齐次坐标，数字1组成的 3x3 矩阵，称之为齐次矩阵。目的是为了将旋转和平移进行合并计算。

以上的 3x3 矩阵我们可以将之写作 $^w_rT$，即由世界坐标系经过这个旋转平移可以得到小车坐标系，可以认为是小车坐标系的点，经过左乘此矩阵，得到其在世界坐标系的位置。

案例计算：

这里我们以上图为例，将数据 $\theta = 30°, ({\delta}x, {\delta}y) = (6, 2), (x_r, y_r) = (5.5, 4)$ 套入公式：可得

$$\begin{aligned} \left[\begin{array}{c} x_{w} \\ y_{w} \\ 1 \end{array}\right] & =\left[\begin{array}{ccc} \cos (\theta) & -\sin (\theta) & {\delta}x \\ \sin (\theta) & \cos (\theta) & {\delta}y \\ 0 & 0 & 1 \end{array}\right] \cdot\left[\begin{array}{c} x_{r} \\ y_{r} \\ 1 \end{array}\right]\\ & = \left[\begin{array}{c} \cos (\theta) \cdot x_r - \sin (\theta) \cdot y_r + {\delta}x \cdot 1 \\ \sin (\theta) \cdot x_r + \cos (\theta) \cdot y_r + {\delta}y \cdot 1 \\ 0 \cdot x_r + 0 \cdot y_r + 1 \cdot 1 \end{array}\right] \\ & = \begin{bmatrix} \dfrac{\sqrt{3}}{2}\times 5.5-\dfrac{1}{2}\times 4+6 \\ \dfrac{1}{2}\times 5.5+\dfrac{\sqrt{3}}{2}\times 4+2 \\ 1 \end{bmatrix}\\ & = \begin{bmatrix} 8.76313972 \\ 8.21410162 \\ 1 \end{bmatrix}\\ \end{aligned}$$

2D一般情况扩展

我们将之扩展为一般情况： 在二维空间中，如果我们希望求解一个点在不同坐标下的位置，我们可以采用如下变换矩阵：

$$^A_BT=\left[\begin{array}{ccc} \cos (\theta) & -\sin (\theta) & {\delta}x \\ \sin (\theta) & \cos (\theta) & {\delta}y \\ 0 & 0 & 1 \end{array}\right]$$

该矩阵描述了以下信息：

1. $T$ 表示变换Transformation，包含了旋转Rotation和平移Translation
2. $^A_BT$ 表示坐标系A经过此旋转平移得到了坐标系B

3. 坐标系B中的点 $P_b$ ，可以通过左乘此矩阵$^A_BT$ ，得到其在坐标系A中的位置 $P_a$ ，即 $P_a={^A_B}T \cdot P_b$

4. 如果希望将坐标系A下的点 $P_a$ 得到其在坐标系B中的坐标 $P_b$ ，则使用其逆矩阵即可，即 $P_b = {^B_A}T \cdot P_a$ 。这里 ${^B_A}T = {^A_B}T^{-1}$