如图所示，将 waypoint 坐标从主车坐标系转换到世界坐标系。
[
x
′
y
′
z
′
]
=
[
x
y
z
]
[
c
o
s
θ
s
i
n
θ
0
−
s
i
n
θ
c
o
s
θ
0
0
0
1
]
\begin{bmatrix} x'&y'&z' \end{bmatrix}=\begin{bmatrix} x&y&z \end{bmatrix}\begin{bmatrix} \mathrm{cos}\ \theta &\mathrm{sin}\ \theta &0 \\ \mathrm{-sin}\ \theta &\mathrm{cos}\ \theta &0 \\ 0&0&1 \end{bmatrix}
[x′y′z′]=[xyz]
cos θ−sin θ0sin θcos θ0001
其中
θ
\theta
θ使车辆的朝角（heading）。
同理，如果将世界坐标系转换到主车坐标系，公式如下：
[
x
y
z
]
=
[
x
′
y
′
z
′
]
[
c
o
s
θ
−
s
i
n
θ
0
s
i
n
θ
c
o
s
θ
0
0
0
1
]
\begin{bmatrix} x&y&z \end{bmatrix}=\begin{bmatrix} x'&y'&z' \end{bmatrix}\begin{bmatrix} \mathrm{cos}\ \theta &\mathrm{-sin}\ \theta &0 \\ \mathrm{sin}\ \theta &\mathrm{cos}\ \theta &0 \\ 0&0&1 \end{bmatrix}
[xyz]=[x′y′z′]
cos θsin θ0−sin θcos θ0001
12-25
4051
4051
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
