1.概述
做MPC控制的时候,需要把 map 坐标系下的相关坐标点转换到 car 坐标系中,由于是只考虑 xy 平面,所以计算比较简单。做完之后,考虑到三维空间的坐标系转换还是不太懂,主要是ROS系统中的tf变换,于是摸了两天鱼。今天详细的记录下来,旋转矩阵、四元素、欧拉角以及各个之间的转换关系。
阅读了好几篇大神的文章,具体也找不到连接了,在此顶礼膜拜,如有相似的地方,那就相似吧,毕竟原理都一样。
本文采用的为右手坐标系,以车身为参考的话,就是车头朝向为x轴正向,垂直于x轴且指向车身左侧为y轴正向,z轴垂直于xy平面,且向上为正方向,原点为车辆质心,此时车辆绕z轴旋转角为yaw角 ,y轴旋转为 pitch角 , x轴旋转为 roll角。
这里需要注意的点,如果是右手坐标系的话,逆时针旋转了角度 θ \theta θ的话,对应的旋转角为 − θ -\theta −θ,顺时针的话旋转角为 θ \theta θ;左手坐标系与之相反。
2.原理
2.1旋转矩阵
对于两个三维点
p
1
(
x
1
,
y
1
,
z
1
)
p1(x1,y1,z1)
p1(x1,y1,z1),
p
2
(
x
2
,
y
2
,
z
2
)
p2(x2,y2,z2)
p2(x2,y2,z2),由点
p
1
p1
p1 经过旋转矩阵
R
R
R 旋转到
p
2
p2
p2,则有:
R
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
R=\begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix}
R=
r11r21r31r12r22r32r13r23r33
[
x
2
y
2
z
2
]
=
R
∗
[
x
1
y
1
z
1
]
\begin{bmatrix} x_2 \\ y_2 \\ z_2 \end{bmatrix} = R*\begin{bmatrix} x_1 \\ y_1 \\ z_1 \end{bmatrix}
x2y2z2
=R∗
x1y1z1
注意:旋转矩阵为正交矩阵
R
∗
R
T
=
E
R*R^T=E
R∗RT=E。
绕x轴旋转角度
θ
\theta
θ
R
x
(
θ
)
=
[
1
0
0
0
c
o
s
(
θ
)
−
s
i
n
(
θ
)
0
s
i
n
(
θ
)
c
o
s
(
θ
)
]
R_x(\theta)=\begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(\theta) & -sin(\theta) \\ 0 & sin(\theta) & cos(\theta) \end{bmatrix}
Rx(θ)=
1000cos(θ)sin(θ)0−sin(θ)cos(θ)
绕y轴旋转角度
θ
\theta
θ
R
y
(
θ
)
=
[
c
o
s
(
θ
)
0
s
i
n
(
θ
)
0
1
0
−
s
i
n
(
θ
)
0
c
o
s
(
θ
)
]
R_y(\theta)=\begin{bmatrix} cos(\theta) & 0 & sin(\theta) \\ 0 & 1 & 0 \\ -sin(\theta) & 0 & cos(\theta) \end{bmatrix}
Ry(θ)=
cos(θ)0−sin(θ)010sin(θ)0cos(θ)
绕z轴旋转角度
θ
\theta
θ
R
z
(
θ
)
=
[
c
o
s
(
θ
)
−
s
i
n
(
θ
)
0
s
i
n
(
θ
)
c
o
s
(
θ
)
0
0
0
1
]
R_z(\theta)=\begin{bmatrix} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1 \end{bmatrix}
Rz(θ)=
cos(θ)sin(θ)0−sin(θ)cos(θ)0001
任意一个坐标系都可以通过依次绕着三个坐标轴旋转到指定的朝向,这三个角度就是欧拉角。
2.2 欧拉角
还是以汽车为例子,车身本身有一个车身坐标系 car ,同时,车辆定位以及路点所采用的是 map 坐标系,车辆的运动相对于这两个坐标系的意义是不同的,这里不考虑车辆的移动变化,仅仅考虑旋转:
- 不论车辆如何运动,坐标系 car 始终是和车辆的运动保持一致的,所以车辆的运动对于 car 坐标系来说,始终保持静止车辆坐标对于car坐标系不变,car 坐标系会随着车辆的运动而不停的变化,因为坐标系不是静止的,此时的欧拉角称为动态欧拉角
- 另一种是车辆的运动相对于 map 坐标系,车辆本身的坐标对于坐标系 map 不停的发生变化,但是坐标系 map 却是保持不动的,此时的欧拉角称为静态欧拉角
使用动态欧拉角的时候会发生万向锁现象,静态欧拉角则不会。万向锁的现象,简单来说,就是坐标系绕某一轴旋转90°后,再换另外一个轴旋转的时候,会失去一个自由度,如下图所示:
实际使用中,首先需要选定一个参考系做为参考系(参考系是静止不动的,在做相关的数据处理的时候,其他坐标系的朝向一般都是向参考系转换的),如上图所示,wiki找的图,这里选取XYZ坐标系做为参考系,xyz坐标系为刚体本身的坐标系,会随着刚体的运动而运动。直线N为xy平面与XY平面的交线, α \alpha α为x轴与交线N的夹角, β \beta β为z轴与Z轴的夹角, γ \gamma γ为交线与X轴的夹角。
每当使用欧拉角时,必须明确的指出夹角的顺序,并指定其参考轴。即使相同的欧拉角,按照不同的旋转顺序,最终得到的结果是不一样的,这个必须要注意。
2.2.1 由欧拉角求旋转矩阵
设三个轴
x
,
y
,
z
x,y,z
x,y,z的欧拉角分别为
θ
x
,
θ
y
,
θ
z
\theta_x,\theta_y,\theta_z
θx,θy,θz,对应的正弦值、余弦值简写为
s
x
,
c
x
,
s
y
,
c
y
,
s
z
,
c
z
s_x,c_x,s_y,c_y,s_z,c_z
sx,cx,sy,cy,sz,cz,且旋转的顺序为
y
a
w
,
p
i
t
c
h
.
r
o
l
l
yaw,pitch.roll
yaw,pitch.roll,因此旋转矩阵为
R
(
θ
z
,
θ
y
,
θ
x
)
=
R
z
(
θ
z
)
∗
R
y
(
θ
y
)
∗
R
x
(
θ
x
)
=
[
c
y
c
z
c
z
s
x
s
y
−
c
x
s
z
s
x
s
z
+
c
x
c
z
s
y
c
y
s
z
c
x
c
z
+
s
x
s
y
s
z
c
x
s
y
s
z
−
c
z
s
x
−
s
y
c
y
s
x
c
x
c
y
]
R(\theta_z,\theta_y,\theta_x)=R_z(\theta_z)*R_y(\theta_y)*R_x(\theta_x)=\begin{bmatrix} c_yc_z & c_zs_xs_y-c_xs_z & s_xs_z+c_xc_zs_y \\ c_ys_z & c_xc_z+s_xs_ys_z & c_xs_ys_z-c_zs_x \\ -s_y & c_ys_x & c_xc_y \end{bmatrix}
R(θz,θy,θx)=Rz(θz)∗Ry(θy)∗Rx(θx)=
cyczcysz−syczsxsy−cxszcxcz+sxsyszcysxsxsz+cxczsycxsysz−czsxcxcy
2.2.2 由旋转矩阵求欧拉角
R
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
=
[
c
y
c
z
c
z
s
x
s
y
−
c
x
s
z
s
x
s
z
+
c
x
c
z
s
y
c
y
s
z
c
x
c
z
+
s
x
s
y
s
z
c
x
s
y
s
z
−
c
z
s
x
−
s
y
c
y
s
x
c
x
c
y
]
R=\begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix}=\begin{bmatrix} c_yc_z & c_zs_xs_y-c_xs_z & s_xs_z+c_xc_zs_y \\ c_ys_z & c_xc_z+s_xs_ys_z & c_xs_ys_z-c_zs_x \\ -s_y & c_ys_x & c_xc_y \end{bmatrix}
R=
r11r21r31r12r22r32r13r23r33
=
cyczcysz−syczsxsy−cxszcxcz+sxsyszcysxsxsz+cxczsycxsysz−czsxcxcy
求解方程,可以有
θ
x
=
a
t
a
n
r
32
r
33
\theta_x=atan \frac{r_{32}}{r_{33}}
θx=atanr33r32
θ
y
=
a
t
a
n
−
r
31
r
32
2
+
r
33
2
\theta_y=atan \frac{-r_{31}}{\sqrt{r_{32}^2+r_{33}^2}}
θy=atanr322+r332−r31
θ
z
=
a
t
a
n
r
21
r
11
\theta_z=atan \frac{r_{21}}{r_{11}}
θz=atanr11r21
从上面的公式,也可以看出万向锁的问题,比如
r
11
,
r
33
r_{11},r_{33}
r11,r33等于0的时候,公式就无法使用了,所以在由旋转矩阵向欧拉角求解的时候,要注意看对应的矩阵元素是不是为0。
2.3 四元素(Quaternion)
学过CAD的都知道,三维控制可以按照一定的方式投影到二维,那么同理,四维空间也可以通过一样的方式投影到三维。然后,四元素就神奇的产生了。
通常情况下,都会采用单位四元素
q
=
ω
+
x
i
⇀
+
y
j
⇀
+
z
k
⇀
=
[
s
,
V
]
q=\omega+x\overrightharpoon{i}+y\overrightharpoon{j}+z \overrightharpoon{k}=[s,V]
q=ω+xi+yj+zk=[s,V]来表示旋转,其中
∥
q
∥
=
x
2
+
y
2
+
z
2
+
ω
2
=
1
\lVert q \rVert=x^2+y^2+z^2+\omega^2=1
∥q∥=x2+y2+z2+ω2=1
具体的四元素相关运算法则,这里就不详细描述了。
2.3.1 由四元素求解旋转矩阵
R ( q ) = [ 1 − 2 y 2 − 2 z 2 2 x y − 2 z w 2 x z + 2 y w 2 x y + 2 z w 1 − 2 x 2 − 2 z 2 2 y z − 2 x w 2 x z − 2 y w 2 y z + 2 x w 1 − 2 x 2 − 2 y 2 ] R(q)=\begin{bmatrix} 1-2y^2-2z^2 & 2xy-2zw & 2xz+2yw \\ 2xy+2zw & 1-2x^2-2z^2 & 2yz-2xw \\ 2xz-2yw & 2yz+2xw & 1-2x^2-2y^2 \end{bmatrix} R(q)= 1−2y2−2z22xy+2zw2xz−2yw2xy−2zw1−2x2−2z22yz+2xw2xz+2yw2yz−2xw1−2x2−2y2
2.3.2 由旋转矩阵求解四元素
- 情况1
ω = 1 + r 11 + r 22 + r 33 2 \omega=\frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2} ω=21+r11+r22+r33
x = r 32 − r 23 4 ω x=\frac{r_{32}-r_{23}}{4\omega} x=4ωr32−r23
y = r 13 − r 31 4 ω y=\frac{r_{13}-r_{31}}{4\omega} y=4ωr13−r31
z = r 21 − r 12 4 ω z=\frac{r_{21}-r_{12}}{4\omega} z=4ωr21−r12
其中,需要满足 ω ≠ 0 , 1 + r 11 + r 22 + r 33 > 0 \omega\not=0,1+r_{11}+r_{22}+r_{33}>0 ω=0,1+r11+r22+r33>0,即 1 + t r ( R ) > 0 1+tr(R)>0 1+tr(R)>0 - 情况2
如果 ω \omega ω趋近于0, t r ( R ) tr(R) tr(R)趋近于 -1,则求解四元素的过程为
– 情况 2.1 如果 m a x { r 11 , r 22 , r 33 } = r 11 max\begin{Bmatrix}{r_{11},r_{22},r_{33}}\end{Bmatrix}=r_{11} max{r11,r22,r33}=r11
t = 1 + r 11 − r 22 − r 33 t=\sqrt{1+r_{11}-r_{22}-r_{33}} t=1+r11−r22−r33
ω = r 32 − r 23 t \omega=\frac{r_{32}-r_{23}}{t} ω=tr32−r23
x = t 4 x=\frac{t}{4} x=4t
y = r 13 + r 31 t y=\frac{r_{13}+r_{31}}{t} y=tr13+r31
z = r 12 + r 21 t z=\frac{r_{12}+r_{21}}{t} z=tr12+r21
– 情况 2.2 如果 m a x { r 11 , r 22 , r 33 } = r 22 max\begin{Bmatrix}{r_{11},r_{22},r_{33}}\end{Bmatrix}=r_{22} max{r11,r22,r33}=r22
t = 1 − r 11 + r 22 − r 33 t=\sqrt{1-r_{11}+r_{22}-r_{33}} t=1−r11+r22−r33
ω = r 13 − r 31 t \omega=\frac{r_{13}-r_{31}}{t} ω=tr13−r31
x = r 12 + r 21 t x=\frac{r_{12}+r_{21}}{t} x=tr12+r21
y = t 4 y=\frac{t}{4} y=4t
z = r 32 + r 23 t z=\frac{r_{32}+r_{23}}{t} z=tr32+r23
– 情况 2.3 如果 m a x { r 11 , r 22 , r 33 } = r 33 max\begin{Bmatrix}{r_{11},r_{22},r_{33}}\end{Bmatrix}=r_{33} max{r11,r22,r33}=r33
t = 1 − r 11 − r 22 + r 33 t=\sqrt{1-r_{11}-r_{22}+r_{33}} t=1−r11−r22+r33
ω = r 21 − r 12 t \omega=\frac{r_{21}-r_{12}}{t} ω=tr21−r12
x = r 13 + r 31 t x=\frac{r_{13}+r_{31}}{t} x=tr13+r31
y = r 23 − r 32 t y=\frac{r_{23}-r_{32}}{t} y=tr23−r32
z = t 4 z=\frac{t}{4} z=4t
2.4 轴-角(Axis-Angle)
最后在补充一下,轴-角的概念,主要是自己用的不多,然后就不是很熟悉,这里简单记录一下吧!
轴-角(Axis-Angle)顾名思义就可以知道,就是某一坐标系通过围绕某一向量旋转一定角度,从而达到参考系的角度。设定等效旋转的方向向量为
K
⇀
=
[
k
x
,
k
y
,
k
z
]
T
\overrightharpoon{K}=\begin{bmatrix}{k_x,k_y,k_z}\end{bmatrix}^T
K=[kx,ky,kz]T,等效旋转角为
θ
\theta
θ,则可以求出对应的四元素:
x
=
k
x
∗
s
i
n
θ
2
x=k_x*sin\frac{\theta}{2}
x=kx∗sin2θ
y
=
k
y
∗
s
i
n
θ
2
y=k_y*sin\frac{\theta}{2}
y=ky∗sin2θ
z
=
k
z
∗
s
i
n
θ
2
z=k_z*sin\frac{\theta}{2}
z=kz∗sin2θ
ω
=
c
o
s
θ
2
\omega=cos\frac{\theta}{2}
ω=cos2θ
且有
x
2
+
y
2
+
z
2
+
ω
2
=
1
x^2+y^2+z^2+\omega^2=1
x2+y2+z2+ω2=1
3 坐标系的平移
设定坐标点
A
(
x
1
,
y
1
,
z
1
)
A (x_1,y_1,z_1)
A(x1,y1,z1)和坐标点
B
(
x
2
,
y
2
,
z
2
)
B(x_2,y_2,z_2)
B(x2,y2,z2),原先两个点都是以参考系为坐标系的,现在将坐标系原点移动到
B
B
B点(不考虑坐标系的旋转),则点
A
A
A在以点
B
B
B为坐标系原点的新坐标系中的坐标为
(
x
1
−
x
2
,
y
1
−
y
2
,
z
1
−
z
2
)
(x_1-x_2,y_1-y_2,z_1-z_2)
(x1−x2,y1−y2,z1−z2)。
坐标系的旋转加平移,这样就可以把空间中的任意坐标系重新投影到另一个坐标系中。
这里记录一下踩到的坑,以点 B B B为圆心绕着某个轴逆时针旋转相应的角度,角度为正值,顺时针为负值。但是对于点 A A A来说,角度的正负取值确实相反的。举个栗子说一下,就是以点 B B B为原点的坐标系,绕 z z z轴逆时针旋转了角度 θ \theta θ,对于原点 B B B来说,旋转的角度是正值,但是对于点 A A A来看,坐标系就是按照负角度进行旋转的了,这里尤其需要理解,不然很容易出错,不敢确定的话,就自己找个简单的数据,计算出结果看看,这样最保险!!!
至于不同的表示方式之间的转换方式,自己动手推导一下就行,这里实在是码不动了,公式太难了。。。。。