四元数的历史
推荐观看3blue1brown的视频和交互操作动画,链接:
四元数的可视化
交互操作动画
待续。。。
计算坐标系与描述坐标系。
采用四元数进行计算的好处(姿态解算):
- 无奇点,方程不会退化;
- 比方向余弦法需解算9个联立微分方程相比,仅需要4个联立微分方程;
- 四元数微分方程的约束条件只有1个,比方向余弦法中的6个要少得多。
四元数的运算法则
定义1: 称量
q
q
q
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
(1)
q = q_0 + q_1 i + q_2 j + q_3 k \tag{1}
q=q0+q1i+q2j+q3k(1)
为四元数,其中
q
0
q_0
q0、
q
1
q_1
q1、
q
2
q_2
q2、
q
3
q_3
q3为任意实数。
q
0
q_0
q0称为四元数的标部,
q
1
i
+
q
2
j
+
q
3
k
q_1 i+q_2 j+q_3 k
q1i+q2j+q3k称为四元数的矢部,并记作
q
\bold{q}
q。因此
q
q
q也可以记作:
q
=
q
0
+
q
(2)
q = q_0 + \boldsymbol{q} \tag{2}
q=q0+q(2)
定义2:
i
i
i、
j
j
j、
k
k
k满足如下运算法则:
{
i
2
=
j
2
=
k
2
=
−
1
j
k
=
−
k
j
=
i
k
i
=
−
i
k
=
j
i
j
=
−
j
i
=
k
(3)
\left \{ \begin{aligned} i^2 &= j^2 = k^2 = -1 \\ jk &= -kj = i \\ ki &= -ik = j \\ ij &= -ji = k \end{aligned} \right. \tag{3}
⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧i2jkkiij=j2=k2=−1=−kj=i=−ik=j=−ji=k(3)
定义3: 称
q
∗
q^*
q∗为
q
q
q的共轭四元数:
q
∗
=
q
0
−
q
1
i
−
q
2
j
−
q
3
k
=
q
0
−
q
(4)
q^* = q_0 - q_1 i - q_2 j - q_3 k = q_0 - \boldsymbol{q}\tag{4}
q∗=q0−q1i−q2j−q3k=q0−q(4)
定义4: 设
p
p
p、
q
q
q为任意两个四元数:
p
=
p
0
+
p
1
i
+
p
2
j
+
p
3
k
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
p = p_0 + p_1 i + p_2 j + p_3 k \\ q = q_0 + q_1 i + q_2 j + q_3 k
p=p0+p1i+p2j+p3kq=q0+q1i+q2j+q3k
则
p
p
p与
q
q
q的相加定义为:
p
+
q
=
(
p
0
+
q
0
)
+
(
p
1
+
q
1
)
i
+
(
p
2
+
q
2
)
j
+
(
p
3
+
q
3
)
k
(5)
\begin{aligned} p + q &= (p_0 + q_0) + (p_1 + q_1)i \\ &+ (p_2 + q_2)j + (p_3 + q_3)k \tag{5} \end{aligned}
p+q=(p0+q0)+(p1+q1)i+(p2+q2)j+(p3+q3)k(5)
称
p
+
q
p + q
p+q 为四元数
p
p
p与
q
q
q之和。
定义5: 设
p
p
p、
q
q
q为任意两个四元数,则
p
p
p与
q
q
q的相乘定义为:
p
q
=
(
p
0
+
p
1
i
+
p
2
j
+
p
3
k
)
(
q
0
+
q
1
i
+
q
2
j
+
q
3
k
)
=
p
0
q
0
−
(
p
1
q
1
+
p
2
q
2
+
p
3
q
3
)
+
(
p
1
i
+
p
2
j
+
p
3
k
)
q
0
+
p
0
(
q
1
i
+
q
2
j
+
q
3
k
)
+
[
(
p
2
p
3
−
p
3
q
2
)
i
+
(
p
3
q
1
−
p
1
q
3
)
j
+
(
p
1
q
2
−
p
2
q
1
)
k
]
(6)
\begin{aligned} pq &= ( p_0 + p_1 i + p_2 j + p_3 k)(q_0 + q_1 i + q_2 j + q_3 k) \\ &= p_0 q_0 - (p_1 q_1 + p_2 q_2 + p_3 q_3) + (p_1 i + p_2 j + p_3 k) q_0 \\ &+ p_0 (q_1 i + q_2 j + q_3 k) + [(p_2 p_3 - p_3 q_2)i \\ &+ (p_3 q_1 - p_1 q_3)j + (p_1 q_2 - p_2 q_1)k] \end{aligned} \tag{6}
pq=(p0+p1i+p2j+p3k)(q0+q1i+q2j+q3k)=p0q0−(p1q1+p2q2+p3q3)+(p1i+p2j+p3k)q0+p0(q1i+q2j+q3k)+[(p2p3−p3q2)i+(p3q1−p1q3)j+(p1q2−p2q1)k](6)
称
p
q
pq
pq为四元数
p
p
p与
q
q
q的积。
定义6:
定义7:
定义8:
四元数的对数运算
一般情况下,如无特殊说明,我们所指的四元数就是单位四元数。
单位四元数
q
=
[
η
,
ξ
]
q = [\eta, \boldsymbol{\xi}]
q=[η,ξ]的对数定义为:
ln
q
=
[
0
,
arccos
η
2
1
−
η
2
ξ
]
\ln q = \left [ 0, \frac{\arccos \eta}{2 \sqrt{1 - \eta^2}} \boldsymbol{\xi} \right]
lnq=[0,21−η2arccosηξ]
单位四元数表示旋转,一个绕单位轴
n
\boldsymbol{n}
n,转过角度
ϕ
\phi
ϕ的坐标系转动,可以表示为:
q
=
[
cos
(
ϕ
2
)
,
sin
(
ϕ
2
)
n
]
q = \left [ \cos \left( \frac{\phi}{2} \right), \sin \left( \frac{\phi}{2} \right) \boldsymbol{n} \right]
q=[cos(2ϕ),sin(2ϕ)n]
对上式所示的使用欧拉轴和角表示的单位四元数,其等价形式为:
ln
q
=
[
0
,
ϕ
2
n
]
\ln q = \left [ 0, \frac{\phi}{2} \boldsymbol{n} \right]
lnq=[0,2ϕn]
简写为
ln
q
=
ϕ
2
n
\ln q = \frac{\phi}{2} \boldsymbol{n}
lnq=2ϕn
定理1: 已知随时间变化的四元数
q
(
t
)
q(t)
q(t)和角速度
ω
(
t
)
\boldsymbol \omega (t)
ω(t),有如下等式成立:
1
2
d
d
t
(
Γ
T
Γ
)
=
Γ
T
ω
\frac{1}{2} \frac{\text{d}}{\text{d} t} (\Gamma^T \Gamma) = \Gamma^T \boldsymbol \omega
21dtd(ΓTΓ)=ΓTω
其中, Γ = 2 ln q \Gamma = 2 \ln q Γ=2lnq
四元数表示旋转
aaa
四元数运动学方程
欧拉运动学方程是用欧拉角与欧拉角速率表示刚体的角速度。刚体的角速度同样可以用四元数及其导数进行表示,称为四元数运动学方程。
姿态动力学方程
I x ω ˙ x + ( I z − I y ) ω y ω z = L x I y ω ˙ y + ( I x − I z ) ω x ω z = L y I z ω ˙ z + ( I y − I x ) ω x ω y = L z \begin{array}{c} I_x \dot{\omega}_x + (I_z - I_y) \omega_y \omega_z = L_x \\ I_y \dot{\omega}_y + (I_x - I_z) \omega_x \omega_z = L_y \\ I_z \dot{\omega}_z + (I_y - I_x) \omega_x \omega_y = L_z \end{array} Ixω˙x+(Iz−Iy)ωyωz=LxIyω˙y+(Ix−Iz)ωxωz=LyIzω˙z+(Iy−Ix)ωxωy=Lz
四元数运动学方程
q ˙ = 1 2 q ω \dot{q} = \frac{1}{2} q \boldsymbol{\omega} q˙=21qω
用四元数表示的欧拉角
{
ϕ
=
ϕ
(
q
0
,
q
1
,
q
2
,
q
3
)
ψ
=
ψ
(
q
0
,
q
1
,
q
2
,
q
3
)
θ
=
θ
(
q
0
,
q
1
,
q
2
,
q
3
)
(xx)
\left \{ \begin{array} {c} \phi = \phi(q_0,q_1, q_2, q_3) \\ \psi = \psi(q_0,q_1, q_2, q_3) \\ \theta = \theta(q_0,q_1, q_2, q_3) \\ \end{array} \right. \tag{xx}
⎩⎨⎧ϕ=ϕ(q0,q1,q2,q3)ψ=ψ(q0,q1,q2,q3)θ=θ(q0,q1,q2,q3)(xx)
利用上式可消除姿态动力学方程右端的欧拉角,以及四元数方程求解后再变换为欧拉角。
用欧拉角表示的四元数
{
q
0
=
q
0
(
ϕ
,
ψ
,
θ
)
q
1
=
q
1
(
ϕ
,
ψ
,
θ
)
q
2
=
q
2
(
ϕ
,
ψ
,
θ
)
q
3
=
q
3
(
ϕ
,
ψ
,
θ
)
q
0
2
+
q
1
2
+
q
2
2
+
q
3
2
=
1
\left \{ \begin{array} {l} q_0 = q_0(\phi, \psi, \theta) \\ q_1 = q_1(\phi, \psi, \theta) \\ q_2 = q_2(\phi, \psi, \theta) \\ q_3 = q_3(\phi, \psi, \theta) \\ q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1 \end{array} \right.
⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧q0=q0(ϕ,ψ,θ)q1=q1(ϕ,ψ,θ)q2=q2(ϕ,ψ,θ)q3=q3(ϕ,ψ,θ)q02+q12+q22+q32=1
利用上式可把欧拉角表示的运动初始条件变换为四元数表示的运动初始条件。
应用上述方程可以把姿态动力学问题变换为以
(
ω
x
,
ω
y
,
ω
z
,
q
0
,
q
1
,
q
2
,
q
3
)
T
(\omega_x, \omega_y, \omega_z, q_0, q_1, q_2, q_3)^T
(ωx,ωy,ωz,q0,q1,q2,q3)T为状态的动力学方程。
导航计算图
关于万向节死锁(Gimbal Lock)
万向死节锁是由于动态欧拉角导致的。物体绕世界坐标系的轴所旋转的角度就是静态欧拉角;物体绕自身坐标系的轴所旋转的角度就是动态欧拉角。世界坐标系就是我们常说的惯性系。
描述的时候采用的是世界坐标系,旋转的时候用的是物体坐标系,所以导致认知上的错误。