传感器坐标系与车身坐标系的转换关系

1坐标系的定义

1.1车身坐标系定义如下：

1.2传感器坐标系定义如下：

将车身坐标系定义为$O_{2}XYZ$,传感器的坐标系定义为$O_{3}XYZ$，绕$Z$轴旋转的角度为$\psi$,其中逆时针方向为正。

2坐标系旋转

绕$Z$轴逆时针旋转的旋转矩阵定义为
$$R=\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]$$
绕$Z$轴顺时针旋转的旋转矩阵定义为
$$R=\left[\begin{array}{ccc}\cos \psi & \sin \psi & 0\\ -\sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]$$

那么$O_{3}XYZ$与$O_{2}XYZ$坐标系之间的转换可以表示为
$$\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]=\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_3\\ y_3\\ z_3\end{array}\right]$$
考虑到车体与传感器之间只存在绕$Z$轴的旋转，故坐标系旋可以只考虑$OXY$平面内的旋转变换。假设按照下图方式旋转：

变换关系可以简化表示如下：
$$\left[\begin{array}{c}{x}_2\\ y_2\end{array}\right]=\left[\begin{array}{cc}\cos \psi & -\sin \psi \\ \sin \psi & \cos \psi \end{array}\right]\left[\begin{array}{c}{x}_3\\ y_3\end{array}\right]$$

3坐标系平移

假设传感器相对车身坐标位置为 $(x_2^1,y_2^1)$ 。所以最终传感器测量的目标位置在车体坐标系下的点表示如下：
$$\left[\begin{array}{c}{x}_2\\ y_2\end{array}\right]=\left[\begin{array}{cc}\cos \psi & -\sin \psi \\ \sin \psi & \cos \psi \end{array}\right]\left[\begin{array}{c}{x}_3\\ y_3\end{array}\right]+\left[\begin{array}{c}{x}_2^1\\ y_2^1\end{array}\right]$$