1坐标系的定义
1.1车身坐标系定义如下:
1.2传感器坐标系定义如下:
将车身坐标系定义为 O 2 X Y Z O_2XYZ O2XYZ,传感器的坐标系定义为 O 3 X Y Z O_3XYZ O3XYZ,绕 Z Z Z轴旋转的角度为 ψ \psi ψ,其中逆时针方向为正。
2坐标系旋转
绕
Z
Z
Z轴逆时针旋转的旋转矩阵定义为
R
=
[
cos
ψ
−
sin
ψ
0
sin
ψ
cos
ψ
0
0
0
1
]
R = \left[ \begin{matrix} \cos\psi & -\sin\psi & 0\\ \sin\psi&\cos\psi&0\\0&0&1 \end{matrix} \right]
R=⎣⎡cosψsinψ0−sinψcosψ0001⎦⎤
绕
Z
Z
Z轴顺时针旋转的旋转矩阵定义为
R
=
[
cos
ψ
sin
ψ
0
−
sin
ψ
cos
ψ
0
0
0
1
]
R = \left[ \begin{matrix} \cos\psi & \sin\psi & 0\\-\sin\psi&\cos\psi&0\\0&0&1 \end{matrix} \right]
R=⎣⎡cosψ−sinψ0sinψcosψ0001⎦⎤
那么
O
3
X
Y
Z
O_3XYZ
O3XYZ与
O
2
X
Y
Z
O_2XYZ
O2XYZ坐标系之间的转换可以表示为
[
x
2
y
2
z
2
]
=
[
cos
ψ
−
sin
ψ
0
sin
ψ
cos
ψ
0
0
0
1
]
[
x
3
y
3
z
3
]
\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]
⎣⎡x2y2z2⎦⎤=⎣⎡cosψsinψ0−sinψcosψ0001⎦⎤⎣⎡x3y3z3⎦⎤
考虑到车体与传感器之间只存在绕
Z
Z
Z轴的旋转,故坐标系旋可以只考虑
O
X
Y
OXY
OXY平面内的旋转变换。假设按照下图方式旋转:
变换关系可以简化表示如下:
[
x
2
y
2
]
=
[
cos
ψ
−
sin
ψ
sin
ψ
cos
ψ
]
[
x
3
y
3
]
\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]
[x2y2]=[cosψsinψ−sinψcosψ][x3y3]
3坐标系平移
假设传感器相对车身坐标位置为
(
x
2
1
,
y
2
1
)
(x^1_2,y^1_2)
(x21,y21) 。所以最终传感器测量的目标位置在车体坐标系下的点表示如下:
[
x
2
y
2
]
=
[
cos
ψ
−
sin
ψ
sin
ψ
cos
ψ
]
[
x
3
y
3
]
+
[
x
2
1
y
2
1
]
\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]
[x2y2]=[cosψsinψ−sinψcosψ][x3y3]+[x21y21]