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

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/zghforever/article/details/82844308

1坐标系的定义

1.1车身坐标系定义如下：

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

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

2坐标系旋转

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

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

变换关系可以简化表示如下：
$\left[ \begin{matrix} x_2\\y_2 \end{matrix} \right]= \left[ \begin{matrix} \cos\psi & -\sin\psi\\\sin\psi&\cos\psi \end{matrix} \right] \left[ \begin{matrix} x_3\\y_3 \end{matrix} \right]$

3坐标系平移

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