刚体运动学姿态推导——弄懂欧拉角、四元数、旋转矩阵(一)姿态表示
最近做项目用到了刚体姿态方面的知识,乱七八糟东西一堆,尤其用到速度表示的微分运动学,特别容易弄混,因此写个帖子方便复习和查找 。主要是记录机器人方向用到的知识。
旋转矩阵
对于空间中的两组坐标系[
x
1
x_1
x1,
y
1
y_1
y1,
z
1
z_1
z1],[
x
0
x_0
x0,
y
0
y_0
y0,
z
0
z_0
z0]我们可以用一个旋转矩阵来描述二者之间的姿态变化:
R
1
0
=
(
x
1
⋅
x
0
y
1
⋅
x
0
z
1
⋅
x
0
x
1
⋅
y
0
y
1
⋅
y
0
z
1
⋅
y
0
x
1
⋅
z
0
y
1
⋅
z
0
z
1
⋅
z
0
)
R_{1}^{0}=\left(\begin{array}{lll} x_{1} \cdot x_{0} & y_{1} \cdot x_{0} & z_{1} \cdot x_{0} \\ x_{1} \cdot y_{0} & y_{1} \cdot y_{0} & z_{1} \cdot y_{0} \\ x_{1} \cdot z_{0} & y_{1} \cdot z_{0} & z_{1} \cdot z_{0} \end{array}\right)
R10=⎝⎛x1⋅x0x1⋅y0x1⋅z0y1⋅x0y1⋅y0y1⋅z0z1⋅x0z1⋅y0z1⋅z0⎠⎞
他的意思是,坐标系0经过
R
1
0
R_{1}^{0}
R10的旋转,姿态会和坐标系1相同。也就是说
R
1
0
R_{1}^{0}
R10表示了从0到1的旋转。上标是出发系,下标是目标系。
R
1
0
R_{1}^{0}
R10的具体数值由出发系的转置点乘目标系得到,即
R
1
0
=
[
x
0
y
0
z
0
]
T
⋅
[
x
1
y
1
z
1
]
R_{1}^{0}=[x_{0} y_{0}z_{0}]^T \cdot [x_{1} y_{1}z_{1}]
R10=[x0y0z0]T⋅[x1y1z1]
当然,旋转矩阵除了表示坐标系之间的变换,还有其他的作用,首先,可以表示同一个点在不同系下坐标的关系。设点
p
p
p在0系下的坐标是
p
0
p^0
p0,在1系下的坐标是
p
1
p^1
p1,则:
p
0
=
R
1
0
p
1
p^{0}=R_{1}^{0}p^{1}
p0=R10p1
此外,它还有第三层含义,即表示向量在同一个坐标系中的旋转。设有一个向量
v
0
v_{0}
v0,经过一个旋转
R
1
0
R_{1}^{0}
R10后变为向量
v
1
v_{1}
v1,则:
v
1
=
R
0
1
v
0
v^{1}=R_{0}^{1}v^{0}
v1=R01v0
因此,R其实有三种含义:
- 表示点在不同坐标系下的转换
- 表示坐标系之间的旋转
- 表示向量之间的旋转 。
使用时,要分清楚究竟是哪一层意思,从哪一系到哪一系,就能避免混淆。
欧拉角
旋转矩阵用9个元素表示一个三维的运动(旋转),显然是包含约束的。这样使用非常不方便。因此提出了一种只用三个元素的表示方法,欧拉角。欧拉角表示刚体按顺序绕三个轴依次旋转,这三个轴有很多选择,既可以选则固定轴,又可以选择旋转后的轴。因此,广义上的欧拉角有非常多种,Z-X-Z,X-Y-Z等等。在机器人学中,常用的是两种:
- RPY角:刚体绕固定系的X轴转γ角,再绕固定系的Y轴旋转β角,最后绕固定系的Z轴旋转α角,三次都是绕固定系。
B A R X Y Z ( γ , β , α ) = R Z ( α ) R Y ( β ) R X ( γ ) = [ c α c β c α s β s γ − s α c γ c α s β c γ + s α s ν s α s β s α s β s γ + c α c γ s α s β c γ − c α s γ − s β c β s γ c β c γ ] { }_{B}^{A} R_{X Y Z}(\gamma, \beta, \alpha)=R_{Z}(\alpha) R_{Y}(\beta) R_{X}(\gamma)=\left[\begin{array}{ccc} c \alpha c \beta & c \alpha s \beta s \gamma-s \alpha c \gamma & c \alpha s \beta c \gamma+s \alpha s \nu \\ s \alpha s \beta & s \alpha s \beta s \gamma+c \alpha c \gamma & s \alpha s \beta c \gamma-c \alpha s \gamma \\ -s \beta & c \beta s \gamma & c \beta c \gamma \end{array}\right] BARXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)=⎣⎡cαcβsαsβ−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsνsαsβcγ−cαsγcβcγ⎦⎤
因为是绕固定系,所以每次是左乘。这种方法测出的三个角为roll(绕x轴),pitch(绕y轴),yaw(绕z轴)。 - ZYX角,刚体绕绕自身的Z轴旋转α,然后绕自身的Y轴旋转β ,最后绕自身的X轴旋转γ,后两次旋转是绕旋转后的系。
A B R Z / Y ′ X ′ ( γ , β , α ) = R Z ( α ) R Y ( β ) R X ( γ ) = [ c α c β c α s β s γ − s α c γ c α s β c γ + s α s γ s α s β s α s β s γ + c α c γ s α s β c γ − c α s γ − s β c β s γ c β c γ ] A_{B} R_{Z / Y \prime X \prime}(\gamma, \beta, \alpha)=R_{Z}(\alpha) R_{Y}(\beta) R_{X}(\gamma)=\left[\begin{array}{ccc} c \alpha c \beta & c \alpha s \beta s \gamma-s \alpha c \gamma & c \alpha s \beta c \gamma+s \alpha s \gamma \\ s \alpha s \beta & s \alpha s \beta s \gamma+c \alpha c \gamma & s \alpha s \beta c \gamma-c \alpha s \gamma \\ -s \beta & c \beta s \gamma & c \beta c \gamma \end{array}\right] ABRZ/Y′X′(γ,β,α)=RZ(α)RY(β)RX(γ)=⎣⎡cαcβsαsβ−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγ−cαsγcβcγ⎦⎤
我们发现,这两种方法是等价的。也就是说,只要绕对应轴转动的角度相同,虽然顺序不同,但转动的结果也是相同的。因此,之后对这两种方法不再区分 。
欧拉角有一个很重要的问题,那就是万向节死锁。当绕y轴旋转90度时,我们发现,之前绕x轴转和之后绕z轴转,转动方向是一致的。也就是说,这两个运动其实是在一个自由度上。那么势必有些姿态就无法达到,也就无法用欧拉角表示了。因此,我们提出了四元数的方法。
四元数
四元素用四个数来表示旋转。即
Q
=
{
η
,
ε
}
\mathrm{Q}=\{\eta, \varepsilon\}
Q={η,ε}
η
=
cos
θ
2
ε
x
=
sin
θ
2
r
x
ε
y
=
sin
θ
2
r
y
ε
z
=
sin
θ
2
r
z
\begin{aligned} \eta &=\cos \frac{\theta}{2} \\ \varepsilon_{x} &=\sin \frac{\theta}{2} r_{x}\\ \varepsilon_{y} &=\sin \frac{\theta}{2} r_{y}\\ \varepsilon_{z} &=\sin \frac{\theta}{2} r_{z} \end{aligned}
ηεxεyεz=cos2θ=sin2θrx=sin2θry=sin2θrz
四元数不存在奇异点,但比较抽象,不容易理解。
旋转矩阵与欧拉角的转换
欧拉角转旋转矩阵
欧拉角其实就是三次独立的旋转,每次旋转的矩阵为:
R
x
=
[
1
0
0
0
cos
θ
−
sin
θ
0
sin
θ
cos
θ
]
R
y
=
[
cos
ϕ
0
sin
ϕ
0
1
0
−
sin
ϕ
0
cos
ϕ
]
R
z
=
[
cos
ψ
−
sin
ψ
0
sin
ψ
cos
ψ
0
0
0
1
]
R_{x}=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta \end{array}\right]\\ R_{y}=\left[\begin{array}{ccc} \cos \phi & 0 & \sin \phi \\ 0 & 1 & 0 \\ -\sin \phi & 0 & \cos \phi \end{array}\right]\\ R_{z}=\left[\begin{array}{ccc} \cos \psi & -\sin \psi & 0 \\ \sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end{array}\right]
Rx=⎣⎡1000cosθsinθ0−sinθcosθ⎦⎤Ry=⎣⎡cosϕ0−sinϕ010sinϕ0cosϕ⎦⎤Rz=⎣⎡cosψsinψ0−sinψcosψ0001⎦⎤
得到:
R
=
R
z
(
ϕ
)
R
y
(
θ
)
R
x
(
ψ
)
=
[
cos
θ
cos
ϕ
sin
ψ
sin
θ
cos
ϕ
−
cos
ψ
sin
θ
cos
ψ
sin
θ
cos
ϕ
+
sin
ψ
sin
ϕ
cos
θ
sin
ϕ
sin
ψ
sin
θ
sin
ϕ
+
cos
ψ
cos
ϕ
cos
ψ
sin
θ
sin
ϕ
−
sin
ψ
cos
θ
−
sin
θ
sin
ψ
cos
θ
cos
ψ
cos
θ
]
R=R_{z}(\phi) R_{y}(\theta) R_{x}(\psi)=\left[\begin{array}{ccc} \cos \theta \cos \phi & \sin \psi \sin \theta \cos \phi-\cos \psi \sin \theta & \cos \psi \sin \theta \cos \phi+\sin \psi \sin \phi \\ \cos \theta \sin \phi & \sin \psi \sin \theta \sin \phi+\cos \psi \cos \phi & \cos \psi \sin \theta \sin \phi-\sin \psi \cos \theta \\ -\sin \theta & \sin \psi \cos \theta & \cos \psi \cos \theta \end{array}\right]
R=Rz(ϕ)Ry(θ)Rx(ψ)=⎣⎡cosθcosϕcosθsinϕ−sinθsinψsinθcosϕ−cosψsinθsinψsinθsinϕ+cosψcosϕsinψcosθcosψsinθcosϕ+sinψsinϕcosψsinθsinϕ−sinψcosθcosψcosθ⎦⎤
旋转矩阵转欧拉角
做一些简单的代数运算即可得到:
θ
x
=
atan
2
(
r
32
,
r
33
)
θ
y
=
atan
2
(
−
r
31
,
r
32
2
+
r
33
2
)
θ
z
=
atan
2
(
r
21
,
r
11
)
\begin{array}{c} \theta_{x}=\operatorname{atan} 2\left(r_{32}, r_{33}\right) \\ \theta_{y}=\operatorname{atan} 2\left(-r_{31}, \sqrt{r_{32}^{2}+r_{33}^{2}}\right) \\ \theta_{z}=\operatorname{atan} 2\left(r_{21}, r_{11}\right) \end{array}
θx=atan2(r32,r33)θy=atan2(−r31,r322+r332)θz=atan2(r21,r11)
旋转矩阵与四元数的转换
四元数转旋转矩阵
推导过程比较复杂,首先,利用罗德里格旋转公式获得从角轴到旋转矩阵的转换,对于单位向量
(
x
,
y
,
z
)
(x,y,z)
(x,y,z)旋转
θ
\theta
θ角后得到的旋转矩阵:
R
=
I
+
ω
^
sin
(
θ
)
+
ω
^
2
(
1
−
cos
(
θ
)
)
R=I+\widehat{\omega} \sin (\theta)+\widehat{\omega}^{2}(1-\cos (\theta))
R=I+ω
sin(θ)+ω
2(1−cos(θ))
R
=
[
cos
θ
+
x
2
(
1
−
cos
θ
)
x
y
(
1
−
cos
θ
)
y
sin
θ
x
y
(
1
−
cos
θ
)
cos
θ
+
y
2
(
1
−
cos
θ
)
−
x
sin
θ
−
y
sin
θ
x
sin
θ
cos
θ
]
R=\left[\begin{array}{ccc} \cos \theta+x^{2}(1-\cos \theta) & x y(1-\cos \theta) & y \sin \theta \\ x y(1-\cos \theta) & \cos \theta+y^{2}(1-\cos \theta) & -x \sin \theta \\ -y \sin \theta & x \sin \theta & \cos \theta \end{array}\right]
R=⎣⎡cosθ+x2(1−cosθ)xy(1−cosθ)−ysinθxy(1−cosθ)cosθ+y2(1−cosθ)xsinθysinθ−xsinθcosθ⎦⎤
再从角轴转化为四元数:
[
1
−
2
y
2
−
2
z
2
2
x
y
+
2
w
z
2
x
z
−
2
w
y
2
x
y
−
2
w
z
1
−
2
x
2
−
2
z
2
2
y
z
+
2
w
x
2
x
z
+
2
w
y
2
y
z
−
2
w
x
1
−
2
x
2
−
2
y
2
]
\left[\begin{array}{ccc} 1-2 y^{2}-2 z^{2} & 2 x y+2 w z & 2 x z-2 w y \\ 2 x y-2 w z & 1-2 x^{2}-2 z^{2} & 2 y z+2 w x \\ 2 x z+2 w y & 2 y z-2 w x & 1-2 x^{2}-2 y^{2} \end{array}\right]
⎣⎡1−2y2−2z22xy−2wz2xz+2wy2xy+2wz1−2x2−2z22yz−2wx2xz−2wy2yz+2wx1−2x2−2y2⎦⎤
旋转矩阵转四元数
旋转矩阵转四元数需要用到上面的结论:
m
12
+
m
21
=
(
2
x
y
+
2
w
z
)
+
(
2
x
y
−
2
w
z
)
=
4
x
y
m
12
−
m
21
=
(
2
x
y
+
2
w
z
)
−
(
2
x
y
−
2
w
z
)
=
4
w
z
m
31
+
m
13
=
(
2
x
z
+
2
w
y
)
+
(
2
x
z
−
2
w
y
)
=
4
x
z
m
31
−
m
13
=
(
2
x
z
+
2
w
y
)
−
(
2
x
z
−
2
w
y
)
=
4
w
y
m
23
+
m
32
=
(
2
y
z
+
2
w
x
)
+
(
2
y
z
−
2
w
x
)
=
4
y
z
m
23
−
m
32
=
(
2
y
z
+
2
w
x
)
−
(
2
y
z
−
2
w
x
)
=
4
w
x
\begin{array}{l} m_{12}+m_{21}=(2 x y+2 w z)+(2 x y-2 w z)=4 x y \\ m_{12}-m_{21}=(2 x y+2 w z)-(2 x y-2 w z)=4 w z \\ m_{31}+m_{13}=(2 x z+2 w y)+(2 x z-2 w y)=4 x z \\ m_{31}-m_{13}=(2 x z+2 w y)-(2 x z-2 w y)=4 w y \\ m_{23}+m_{32}=(2 y z+2 w x)+(2 y z-2 w x)=4 y z \\ m_{23}-m_{32}=(2 y z+2 w x)-(2 y z-2 w x)=4 w x \end{array}
m12+m21=(2xy+2wz)+(2xy−2wz)=4xym12−m21=(2xy+2wz)−(2xy−2wz)=4wzm31+m13=(2xz+2wy)+(2xz−2wy)=4xzm31−m13=(2xz+2wy)−(2xz−2wy)=4wym23+m32=(2yz+2wx)+(2yz−2wx)=4yzm23−m32=(2yz+2wx)−(2yz−2wx)=4wx
于是:
w
=
m
11
+
m
22
+
m
33
+
1
2
w=\frac{\sqrt{m_{11}+m_{22}+m_{33}+1}}{2}
w=2m11+m22+m33+1
x
=
m
11
−
m
22
−
m
33
+
1
2
x=\frac{\sqrt{m_{11}-m_{22}-m_{33}+1}}{2}
x=2m11−m22−m33+1
y
=
−
m
11
+
m
22
−
m
33
+
1
2
y=\frac{\sqrt{-m_{11}+m_{22}-m_{33}+1}}{2}
y=2−m11+m22−m33+1
z
=
−
m
11
−
m
22
+
m
33
+
1
2
z=\frac{\sqrt{-m_{11}-m_{22}+m_{33}+1}}{2}
z=2−m11−m22+m33+1
欧拉角与四元数之间的转换
欧拉角转四元数
欧拉角相当于三次绕定轴的旋转,每次选择都可以方便地用四元数进行转换,因此得到以下结果:
q
=
[
cos
γ
2
0
0
sin
γ
2
]
[
cos
β
2
0
sin
β
2
0
]
[
cos
α
2
sin
α
2
0
0
]
=
[
cos
α
2
cos
β
2
cos
γ
2
+
sin
α
2
sin
β
2
sin
γ
2
sin
α
2
cos
β
2
cos
γ
2
−
cos
α
2
sin
β
2
sin
γ
2
cos
α
2
sin
β
2
cos
γ
2
+
sin
α
2
cos
β
2
sin
γ
2
cos
α
2
cos
β
2
sin
γ
2
−
sin
α
2
sin
β
2
cos
γ
2
]
q=\left[\begin{array}{c} \cos \frac{\gamma}{2} \\ 0 \\ 0 \\ \sin \frac{\gamma}{2} \end{array}\right]\left[\begin{array}{c} \cos \frac{\beta}{2} \\ 0 \\ \sin \frac{\beta}{2} \\ 0 \end{array}\right]\left[\begin{array}{c} \cos \frac{\alpha}{2} \\ \sin \frac{\alpha}{2} \\ 0 \\ 0 \end{array}\right]=\left[\begin{array}{c} \cos \frac{\alpha}{2} \cos \frac{\beta}{2} \cos \frac{\gamma}{2}+\sin \frac{\alpha}{2} \sin \frac{\beta}{2} \sin \frac{\gamma}{2} \\ \sin \frac{\alpha}{2} \cos \frac{\beta}{2} \cos \frac{\gamma}{2}-\cos \frac{\alpha}{2} \sin \frac{\beta}{2} \sin \frac{\gamma}{2} \\ \cos \frac{\alpha}{2} \sin \frac{\beta}{2} \cos \frac{\gamma}{2}+\sin \frac{\alpha}{2} \cos \frac{\beta}{2} \sin \frac{\gamma}{2} \\ \cos \frac{\alpha}{2} \cos \frac{\beta}{2} \sin \frac{\gamma}{2}-\sin \frac{\alpha}{2} \sin \frac{\beta}{2} \cos \frac{\gamma}{2} \end{array}\right]
q=⎣⎢⎢⎡cos2γ00sin2γ⎦⎥⎥⎤⎣⎢⎢⎡cos2β0sin2β0⎦⎥⎥⎤⎣⎢⎢⎡cos2αsin2α00⎦⎥⎥⎤=⎣⎢⎢⎡cos2αcos2βcos2γ+sin2αsin2βsin2γsin2αcos2βcos2γ−cos2αsin2βsin2γcos2αsin2βcos2γ+sin2αcos2βsin2γcos2αcos2βsin2γ−sin2αsin2βcos2γ⎦⎥⎥⎤
四元数转欧拉角
求得上述方程的逆解,即可求得欧拉角:
[
α
β
γ
]
=
[
atan
2
(
2
(
q
0
q
1
+
q
2
q
3
)
,
1
−
2
(
q
1
2
+
q
2
2
)
)
arcsin
(
2
(
q
0
q
2
−
q
1
q
3
)
)
atan
2
(
2
(
q
0
q
3
+
q
1
q
2
)
,
1
−
2
(
q
2
2
+
q
3
2
)
)
]
\left[\begin{array}{c} \alpha \\ \beta \\ \gamma \end{array}\right]=\left[\begin{array}{c} \operatorname{atan} 2\left(2\left(q_{0} q_{1}+q_{2} q_{3}\right), 1-2\left(q_{1}^{2}+q_{2}^{2}\right)\right) \\ \arcsin \left(2\left(q_{0} q_{2}-q_{1} q_{3}\right)\right) \\ \operatorname{atan} 2\left(2\left(q_{0} q_{3}+q_{1} q_{2}\right), 1-2\left(q_{2}^{2}+q_{3}^{2}\right)\right) \end{array}\right]
⎣⎡αβγ⎦⎤=⎣⎡atan2(2(q0q1+q2q3),1−2(q12+q22))arcsin(2(q0q2−q1q3))atan2(2(q0q3+q1q2),1−2(q22+q32))⎦⎤