[自动驾驶算法][从0开始轨迹预测]：一、坐标系和坐标系变换

既然要从0开始轨迹预测，那从哪开始写起呢？回想下自己的学习历程，真正有挑战性的不是模型结构，不是繁琐的训练和调参，而是数据的制作！！！

笔者自认为不是一个数学基础牢固的人，那么我们的轨迹预测之旅就从坐标转换开始吧～～～

由难至简，才能做到【删繁就简三秋树，领异标新二月花】，专注于轨迹预测的核心算法。

---

1. 坐标系：

二维直角坐标系，笛卡尔坐标系（Cartesian coordinate system），由两条相互垂直、相交于原点的数线构成的。

直角坐标系也可以推广至三维空间与高维空间 (higher dimension)。在原本的二维直角坐标系，再添加一个垂直于x-轴，y-轴的坐标轴，称为z-轴，叫做三维直角坐标系。

那么三维直角坐标系的x，y，z-轴的指向，以及绕x，y，z-轴旋转的方向该如何确定呢？？

三维直角坐标系分两种，左手坐标系和右手坐标系，那么为什么要用左手和右手来区分呢？这是因为当确定了x-轴，y-轴方向之后,z-轴的方向有两种可能，它可以通过左手或右手来确定。

下面就是这两个坐标系 x，y，z-轴指向 的规则示意图（图中固定了x轴的正方向向右，y轴的正方向向上），其中大拇指、食指、中指分别对应于x-轴、y-轴、z-轴：

对坐标系使用左手与右手的命名，有一个作用就是用来方便 判断旋转的正方向，这就是左手螺旋法则和右手螺旋法则。例如对左手坐标系，确定一个旋转轴后，左手握住拳头，拇指指向旋转轴的正方向，四指弯曲的方向为旋转的正方向。相应地，右手坐标系就用右手来判定。确定了旋转的正方向后，在公式计算中就很容易知道是该使用正角度还是负角度了。下图就是右手的例子：

给定任意一个旋转角度的三维坐标系，如果按上面的方法判断旋转正方向，首先，你得确定这个坐标系是左手坐标系还是右手坐标系，这时你会先拿出一只手来，像上图一样摆好三根手指的姿势来比对给定坐标系的x、y、z轴正方向看是否一致。然后根据旋转轴的正方向，用相应的手来判断旋转正方向。

那么有什么快速判断的方法吗？

上图给出左右手坐标系绕z轴的旋转方向，从我们眼睛看屏幕的角度来看，它们绕z轴旋转的正方向都是逆时针。同理，绕y-轴和x-轴也可以获得相同的结论。则：对于任意旋转角度的三维坐标系（无需区分左右手），绕某一坐标轴旋转的正方向，与另外两个坐标轴的正方向顶端按X—>Y—>Z—>X的顺序进行指向的方向一致。

---

2. 坐标转换与坐标系转换

自由度的定义：自由度（Degree of Freedom，简称DOF）是指系统中可以自由变化的独立参数的数量，也就是系统的状态空间维度（有几个量可以调节）。

公式预警！！！ 都是超级简单的向量的相加，相信我读完你会有收获哒！！！

2.1 2D坐标转换

1. 平移（Translation）

在2D空间中，我们经常需要将一个点平移到另一个位置(如下图所示)。假设空间中的一点P，其用坐标表示为（x,y）；将其向 x方向平移 tx，向y方向平移ty, 假设平移后点的坐标为（x’,y’）,则上述点的平移操作可以归纳为：
$$\begin{gathered} x^{\prime }=x+t_x \\ y’=y+t_y \\ \overrightarrow{O{P}^{\prime }}=\overrightarrow{OP}+\overrightarrow{P{P}^{\prime }} \end{gathered}$$

公式（1）又可以采用矩阵表述如下：

$$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\end{array}\right]=\left[\begin{array}{ccc}1& 0& t_x\\ 0& 1& t_y\end{array}\right]\left[\begin{array}{c}x\\ y\end{array}\right]$$

2. 缩放（Scaling）

​ 其中，s_x和s_y分别是沿x和y轴的缩放因子。
$$\begin{gathered} x^{\prime }=s_{x}x \\ y’=s_{y}s \\ \overrightarrow{O{P}^{\prime }}=s\overrightarrow{OP} \end{gathered}$$

​ 齐次坐标的形式：
$$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}{s}_x& 0& 1\\ 0& s_y& 1\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$

3. 旋转（Rotation）

点P（x，y）绕坐标系原点旋转$\theta$角得到点P’（x‘，y’）有：

将OP与x-轴正方向的夹角记做$\beta$，OP的长度为r，且$rcos\beta =x$,$rsin\beta =y$,则P‘的坐标可推导为：
$$\begin{gathered} x^{\prime }=rcos(\theta +\beta )=r(cos\theta cos\beta -sin\theta sin\beta )=rcos\beta cos\theta -rsin\beta sin\theta \\ {y}^{\prime }=rsin(\theta +\beta )=r(sin\theta cos\beta +cos\theta sin\beta )=rcos\beta sin\theta +rsin\beta cos\theta \end{gathered}$$

$$\begin{gathered} x^{\prime }=xcos\theta -ysin\theta \\ {y}^{\prime }=xsin\theta +ycos\theta \end{gathered}$$

同理写成齐次坐标的形式：
$$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}cos\theta & -sin\theta & 1\\ sin\theta & cos\theta & 1\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$

2.2 2D坐标系转换

理解了2D坐标转换，2D坐标系的转换（坐标点不动，坐标系动-横看成岭侧成峰）可以看成2D坐标转换的逆转换：

​ 坐标系向左平移 = 坐标点在原坐标系基础上向右平移；

​ 坐标系绕轴顺时针旋转 = 坐标点在原坐标系的基础上绕轴逆时针旋转；

1. 坐标系平移

红色坐标系相对于黑色坐标系中平移的距离为(i, j)

红色点在红色坐标系的位置为(x, y)，则红色点在黑色坐标系的表示如下图所示：

图中示例，坐标系向左向下平移，相当于P点像右像上平移。

$$\begin{gathered} x^{\prime }=i+x \\ {y}^{\prime }=j+y \\ \overrightarrow{O{P}^{\prime }}=\overrightarrow{O{O}^{\prime }}+\overrightarrow{O^{\prime }P}=(i,j)+(x,y)=(i+x,j+y) \end{gathered}$$
​ 将坐标系的平移写成齐次坐标的形式：
$$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}1& 0& i\\ 0& 1& j\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$

2. 坐标系旋转

已知 红点P在蓝色坐标系的位置(x, y)，也知道蓝色坐标系相较于黑色坐标系顺时针旋转的角度θ。

求解： 红点在黑色坐标系同的位置(X‘, Y’)？

蓝色坐标系相较于黑色坐标系顺时针旋转的角度θ，相当于计算P点在蓝色系内绕远点逆时针旋转θ

向量分解的方法推导：

将P点的坐标，沿黑色坐标系分解：

$$\begin{gathered} x^{\prime }=xcos\theta -ysin\theta \\ {y}^{\prime }=xsin\theta +ycos\theta \end{gathered}$$
写成齐次坐标的形式：
$$\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}cos\theta & -sin\theta & 1\\ sin\theta & cos\theta & 1\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$

3. 2D坐标系旋转平移总结

已知 红点在蓝色坐标系的位置P(xb, yb)，也知道蓝色坐标系相较于黑色坐标系 旋转的角度θ, 其中 蓝色坐标系的原点在黑色坐标系中的位置为(δx, δy).

求解： 红点在黑色坐标系同的位置(xa, ya)？

以向量的方式推导：

​ P点在A系下的坐标可以用向量$\overrightarrow{O_{a}P}$表示：
$$\overrightarrow{O_{a}P}=\overrightarrow{O_a{O}_b}+\overrightarrow{O_{b}P}$$
​ 其中$\overrightarrow{O_a{O}_b}=({\delta }_x,{\delta }_y)$，那么我们该如何表示向量$\overrightarrow{O_{b}P}$的坐标值呢？

​ 在此处有个误区，大家可能会觉得$\overrightarrow{O_{b}P}=(x_b,y_b)$。其实不是这样的，(xb,yb)坐标表示的是点P在B坐标系中的位置，视角是站在B坐标系上的，此时我们的视角应该是在A坐标系，或者是和A坐标系平行的。

所以，我们应该将B坐标系进行旋转，保证和A坐标系平行的，如下图所示。

根据前面旋转部分的推导，我们可以得到:
$$\overrightarrow{O_{b}P}=\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\left[\begin{array}{c}{x}_b\\ y_b\end{array}\right]$$
则根据公式（14），
$$\overrightarrow{O_{a}P}=\left[\begin{array}{c}{x}_a\\ y_a\end{array}\right]=\left[\begin{array}{c}{\delta }_x\\ {\delta }_y\end{array}\right]+\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\left[\begin{array}{c}{x}_b\\ y_b\end{array}\right]=\left[\begin{array}{c}cos\theta \ast x_b+{\delta }_x-sin\theta \ast y_b\\ sin\theta \ast x_b+{\delta }_y+cos\theta \ast y_b\end{array}\right]$$
写成齐次坐标的形式：
$$\left[\begin{array}{c}{x}_a\\ y_a\\ 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]\left[\begin{array}{c}{x}_b\\ y_b\\ 1\end{array}\right]$$

$T$表示transform，变换的意思。

${}_B^{A}T$表示的是 由B坐标系变换为A坐标系的意思。

对于2D坐标系的旋转和平移大致上我们可以得到以下：
$$\begin{gathered} P_a{=}_B^{A}T\cdot P_b \\ {P}_b{=}_B^A{T}^{-1}\cdot P_a \end{gathered}$$

注意：先平移还是先旋转的问题

• 上述转换为局部坐标系->全局坐标系下的转换，此时P点的原点为局部坐标系的原点，此时无论是先旋转局部坐标系，还是先平移局部坐标系都是一样的；
• 而当进行全局坐标系到局部坐标系的转换时，此时P点的原点为全局坐标系下的原点，当先旋转全局坐标系时，局部坐标系原点的位置也会随着旋转而变化，变化的值为$R\ast ({\delta }_a,{\delta }_b)$，即此时的转换方式为$R\ast P+R\ast ({\delta }_a,{\delta }_b)$；当先平移坐标系时，此时的转换方式为$R\ast (P+({\delta }_a,{\delta }_b))$。

2.3 3D坐标系转换

3D坐标系转换，仅仅是添加了一维z-轴，所使用的基础公式和2D并无差异。

1. 3D坐标系平移

同样已知 红色点在蓝色坐标系的位置为(x1,y1,z1)

蓝色坐标系的原点在黑色坐标系中的位置为(δx,δy,δz)

求解： 红色点在黑色坐标系中的位置点(x2,y2,z2)?

根据向量的加法有：
$$\overrightarrow{O_{2}P}=\overrightarrow{O_2{O}_1}+\overrightarrow{O_{1}P}=({\delta }_x,{\delta }_y,{\delta }_z)+(x_1,y_1,z_1)$$
同样，用齐次坐标表示：
$$\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& {\delta }_x\\ 0& 1& 0& {\delta }_y\\ 0& 0& 1& {\delta }_z\end{array}\right]\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]$$

2. 3D坐标系旋转

1. 绕z轴旋转（同2D旋转）：
   yaw：偏航角。 是沿世界坐标系的Z轴旋转的角度。0表示面向世界坐标系的X轴正向。

   绕z-轴旋转，z坐标保持不变
   $$\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]=\left[\begin{array}{ccc}cos\theta & -sin\theta & 0\\ sin\theta & cos\theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]$$

2. 绕y轴旋转：
   pitch：俯仰角。 就是抬头或低头，和大地水平面的夹角。换句话说，是沿自身坐标系（X轴向前的坐标系）的Y轴旋转的角度。
   绕y-轴旋转，z坐标保持不变

$$\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]=\left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right]\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]$$

你是不是也发现了，这个公式好像和2D旋转的不大一样？别急，手动画一下旋转图像你就会明白的～

还记得前面介绍，旋转角的正负吗？忘记的同学可以看一下本篇文章的第一部分坐标系, 旋转的正方向为X->Y->Z->X，而此时绕y轴旋转的角度theta，为 旋转的负方向 ，所以此时应在原旋转矩阵的基础上取逆矩阵即可～，这也是为什么绕Y轴旋转的公式不一样的原因！！！

3. 绕X轴旋转：
   roll：翻滚角。 就是左倾或右倾，和大地水平面的夹角。换句话说，是沿自身坐标系（X轴向前的坐标系）的X轴旋转的角度。
   x坐标保持不变
   $$\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\theta & sin\theta \\ 0& -sin\theta & cos\theta \end{array}\right]\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]$$

4. 3D坐标系的旋转：
   $$R_{zyx}=R_x(\alpha )R_y(\beta )R_z(\gamma )$$
   将上述三个矩阵依次相乘：
   

​ 上述矩阵看成：
$$\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]$$
​

如果我想通过旋转矩阵反求旋转角度，该如何做呢？观察旋转矩阵各个项之间的关系。

三角函数中，如果要求解一个角度值，可以通过:
$$\theta =arctan(sin\theta ,cos\theta )$$

求绕x-轴旋转的角度：
$$\alpha =arctan(R_{21},R_{11})$$
求绕y-轴旋转的角度：

​ 求解绕y轴旋转有点麻烦，已知公式（24），则：
$$\beta =arctan(-R_{31},cos\beta )$$
​ 那么$cos\beta$怎么求呢？我们继续观察$R_{11}$和$R_{21}$, 他们分别是$cos(\alpha )\cdot cos(\beta )$ 和 $sin(\alpha )\cdot cos(\beta )$

​ 尝试使用$\sqrt{R_{11}^2+R_{21}^2}$,化简后得到：
$$cos\beta =\sqrt{R_{11}^2+R_{21}^2}$$
​ 则：
$$\beta =arctan(-R_{31},\sqrt{R_{11}^2+R_{21}^2})$$
求绕z-轴的旋转角度：
$$\gamma =arctan(R_{32},R_{33})$$