自动驾驶仿真：角雷达坐标系转换详解

---

一、 为什么要转换角雷达坐标？

• 我们在做角雷达传感器仿真的时候，使用场景软件搭建角雷达模型会发现角雷达有独立的坐标系，它与车辆坐标系是不一致的。而实际车辆需要用到的目标数据一般都是参照车辆坐标系输出，为了确保仿真角雷达输出的目标数据与实际雷达一致。我们需要对角雷达输出的数据进行适配，即目标值的旋转和平移，以获取到参考车辆坐标系输出的数据。

---

注：本文主要介绍的是角雷达安装绕Z轴旋转的转换方法，但会提供绕X轴、Y轴旋转的公式。

二、角雷达坐标系和车辆坐标系

1、角雷达的安装角度

1）如下图所示θ角为角雷达的安装角度（我们的角雷达只绕Z轴旋转），另外需要注意每个车厂对于安装角度的定义不太一致，我们在套用公式的时候需要注意对安装角度的转换。

2、角雷达的坐标系

1）如上图角雷达的安装角度所示，橙色的线相当于角雷达的背板，垂直于背板就是角雷达X+（上）的方向，平行于背板就是Y+（左），如下图所示：

3、车辆坐标系

1）车头方向是Y+，垂直于车头就是X+，如下图所示：

我们要做的就是将角雷达坐标旋转至车辆坐标，但不是真正意义的坐标系旋转，而是角雷达数据的目标值相对于车辆坐标系输出。

三、 坐标系转换思路

1、坐标系绕Z轴旋转

1）使用右前角雷达作为案例进行转换（先旋转再平移），而且为了让公式推导看起来更简单，讲解时先让原点重合一下：

2）下图示，障碍物在右前角雷达的坐标系的坐标为（x1，y1），而目标物在自车坐标系的坐标则为（x0，y0），右角雷达坐标系与自车坐标系的夹角为α，注意α并非右角雷达的安装角度θ。

2）对坐标系做辅助线处理。

由上图得X0（自车坐标系旋转后）公式：
$$x_0=x_{1}cos\alpha -y_{1}sin\alpha$$
由上图得Y0（自车坐标系旋转后）公式：
$$y_0=x_{1}sin\alpha +y_{1}cos\alpha$$

注意：
1、以上两条公式是旋转后得原始公式，如果α的参照一致则可以直接代入。
2、由于我的角雷达只是绕Z轴旋转，所以 Z0 = Z1。

并且由于我的α不是实际的车辆安装角度θ，因此：
$$x_0=x_{1}cos（90°-\theta ）-y_{1}sin（90°-\theta ）$$
$$y_0=x_{1}sin（90°-\theta ）+y_{1}cos（90°-\theta ）$$

2、坐标系平移

平移只是简单得坐标点平移，假设右角雷达的安装位置为（X2，Y2）：
由上图得X0（自车坐标系旋转平移后）公式：
$$x_0=x_{1}cos（90°-\theta ）-y_{1}sin（90°-\theta ）+x_2$$
由上图得Y0（自车坐标系旋转平移后）公式：
$$y_0=x_{1}sin（90°-\theta ）+y_{1}cos（90°-\theta ）+y_2$$

3、其他目标值转换

由于速度加速度等参数是不存其他关系，仅存在三角关系，则它们的关系，例：
$$v_0=v_{1}cos(90°-\theta )。$$

四、 其余角雷达的转换公式

已知条件：Z0 = Z1，并且由于角雷达安装在车辆坐标系的位置不同，X2和Y2的方向需要注意（X2,Y2为角雷达在车辆坐标系下的安装位置）

1、左前角雷达转换公式

1）注意左前雷达的X2为负值，Y2为正值：
$$x_0=x_{1}cos（90°+\theta ）-y_{1}sin（90°+\theta ）+x_2$$

$$y_0=x_{1}sin（90°+\theta ）+y_{1}cos（90°+\theta ）+y_2$$

2、左后角雷达转换公式

1）注意左后角雷达的X2为负值，Y2为正值：
$$x_0=x_{1}cos（270°-\theta ）-y_{1}sin（270°-\theta ）+x_2$$

$$y_0=x_{1}sin（270°-\theta ）+y_{1}cos（270°-\theta ）+y_2$$

3、右后角雷达转换公式

1）注意右后角雷达的X2为正值，Y2为正值：
$$x_0=x_{1}cos（270°+\theta ）-y_{1}sin（270°+\theta ）+x_2$$

$$y_0=x_{1}sin（270°+\theta ）+y_{1}cos（270°+\theta ）+y_2$$

五、XY轴旋转拓展

1、旋转公式至矩阵转换

绕Z轴旋转的原始等式：

$$\begin{array}{c}{x}_0=x_{1}cos\alpha -y_{1}sin\alpha \\ y_0=x_{1}sin\alpha +y_{1}cos\alpha \\ z_0=z_1\end{array}$$

将上述等式转换成矩阵，即可得到

$$\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]=\left[\begin{array}{ccc}cos\alpha & -sin\alpha & 0\\ sin\alpha & cos\alpha & 0\\ 0& 0& 1\end{array}\right]\ast \left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]=RZ\ast \left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]$$

把角雷达探测到的值和上述RZ矩阵进行矩阵乘法运算，就完成绕Z轴的旋转。这个绕Z轴旋转的矩阵称为RZ。

2、绕X轴Y轴旋转的矩阵关系式

另外只要继续完成了绕X轴和Y轴的旋转（矩阵分别为RX和RY），就是正真意义的完成转换，是否需要绕这两轴旋转的要根据具体情况选择。

RX矩阵：
$$RX=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\alpha & sin\alpha \\ 0& -sin\alpha & cos\alpha \end{array}\right]$$
RY矩阵：
$$RY=\left[\begin{array}{ccc}cos\alpha & 0& -sin\alpha \\ 0& 1& 0\\ sin\alpha & 0& cos\alpha \end{array}\right]$$
RZ矩阵：
$$RZ=\left[\begin{array}{ccc}cos\alpha & -sin\alpha & 0\\ sin\alpha & cos\alpha & 0\\ 0& 0& 1\end{array}\right]$$

绕XYZ轴旋转的坐标转换公式：

$$\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]=RX\ast RY\ast RZ\ast \left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]$$

最后加上平移矩阵：
$$\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]=RX\ast RY\ast RZ\ast \left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]+Txyz$$

---

综上，完成角雷达的坐标系转换