四元数微分形式的推导
关于四元数表示旋转的一些知识
1.四元数表示向量旋转
定义
p
v
=
[
p
x
p
y
p
z
]
T
p_v=\left[\begin{array}{ccc}{p_{x}} & {p_{y}} & {p_{z}}\end{array}\right]^T
pv=[pxpypz]T为三维空间中的一点,将其转换成纯四元数形式即:
p
=
[
0
p
v
]
T
p=\left[\begin{array}{cc}0 & {p_{v}}\end{array}\right]^T
p=[0pv]T,令
q
=
[
c
o
s
(
θ
2
)
v
s
i
n
(
θ
2
)
]
T
q=\left[\begin{array}{cc}{cos(\frac{\theta}{2})} & {v sin(\frac{\theta}{2})}\end{array}\right]^T
q=[cos(2θ)vsin(2θ)]T为单位四元数,则有
p
′
=
q
⋅
p
⋅
q
−
1
=
[
0
p
v
′
]
T
p^{\prime} =q \cdot p \cdot q^{-1}=\left[\begin{array}{cc}0 & {p^{\prime}_{v}}\end{array}\right]^T
p′=q⋅p⋅q−1=[0pv′]T其中
p
v
′
=
[
p
x
′
p
y
′
p
z
′
]
T
p^{\prime}_{v}=\left[\begin{array}{ccc}{p^{\prime}_{x}} & {p^{\prime}_{y}} & {p^{\prime}_{z}}\end{array}\right]^T
pv′=[px′py′pz′]T表示
p
v
p_v
pv绕旋转轴
v
v
v旋转
θ
\theta
θ角度后得到的新向量在原三维空间中的坐标表示。
2.四元数表示坐标系旋转
定义向量
v
0
v_0
v0在
o
x
y
z
oxyz
oxyz坐标系中的表示为
v
1
=
[
v
x
v
y
v
z
]
T
v_1=\left[\begin{array}{ccc}{v_{x}} & {v_{y}} & {v_{z}}\end{array}\right]^T
v1=[vxvyvz]T,令坐标系
o
x
y
z
oxyz
oxyz绕单位旋转轴
v
v
v旋转
θ
\theta
θ角度,得到新坐标系
o
′
x
′
y
′
z
′
o^{\prime}x^{\prime}y^{\prime}z^{\prime}
o′x′y′z′,此时
v
0
v_0
v0在新坐标系中的坐标表示为
v
1
′
=
[
v
x
′
v
y
′
v
z
′
]
T
v^{\prime}_1=\left[\begin{array}{ccc}{v^{\prime}_{x}} & {v^{\prime}_{y}} & {v^{\prime}_{z}}\end{array}\right]^T
v1′=[vx′vy′vz′]T。那么,向量
v
0
v_0
v0在两个坐标系之间的坐标转换关系为:
[
0
v
1
′
]
T
=
q
−
1
⋅
[
0
v
1
]
T
⋅
q
\left[\begin{array}{cc}0 & {v^{\prime}_{1}}\end{array}\right]^T=q^{-1} \cdot \left[\begin{array}{cc}0 & {v_{1}}\end{array}\right]^T \cdot q
[0v1′]T=q−1⋅[0v1]T⋅q
其中
q
=
[
c
o
s
(
θ
2
)
v
s
i
n
(
θ
2
)
]
T
q=\left[\begin{array}{cc}{cos(\frac{\theta}{2})} & {v sin(\frac{\theta}{2})}\end{array}\right]^T
q=[cos(2θ)vsin(2θ)]T
值得注意的是,上述两种四元数表示旋转的方式得到的四元数旋转关系是不同的,这是因为1.表示向量在同一个坐标系中的旋转,而2.中旋转的是坐标系。
以1.为例,先做
q
1
q_1
q1旋转,再做
q
2
q_2
q2旋转之后向量
x
x
x在新坐标系中的表示为
x
′
x^{\prime}
x′,则有:
x
t
e
m
p
=
q
1
⋅
x
⋅
q
1
−
1
x_{temp} = q_1 \cdot x \cdot {q_1}^{-1}
xtemp=q1⋅x⋅q1−1
x
′
=
q
2
⋅
x
t
e
m
p
⋅
q
2
−
1
x^{\prime} = q_2 \cdot x_{temp} \cdot {q_2}^{-1}
x′=q2⋅xtemp⋅q2−1
x
′
=
q
2
q
1
⋅
x
t
e
m
p
⋅
q
2
q
1
−
1
x^{\prime} = {q_2 q_1} \cdot x_{temp} \cdot {q_2 q_1}^{-1}
x′=q2q1⋅xtemp⋅q2q1−1
四元数 q 2 q 1 q_2 q_1 q2q1表示了连续两次的旋转。
以2.为例,先做
q
1
q_1
q1旋转,再做
q
2
q_2
q2旋转之后向量
x
x
x在新坐标系中的表示为
x
′
x^{\prime}
x′,则有:
x
t
e
m
p
=
q
1
−
1
⋅
x
⋅
q
1
x_{temp} = q_1^{-1} \cdot x \cdot {q_1}
xtemp=q1−1⋅x⋅q1
x
′
=
q
2
−
1
⋅
x
t
e
m
p
⋅
q
2
x^{\prime} = q_2^{-1} \cdot x_{temp} \cdot {q_2}
x′=q2−1⋅xtemp⋅q2
x
′
=
q
1
q
2
−
1
⋅
x
t
e
m
p
⋅
q
1
q
2
x^{\prime} = {q_1 q_2}^{-1} \cdot x_{temp} \cdot {q_1 q_2}
x′=q1q2−1⋅xtemp⋅q1q2
四元数 q 1 q 2 q_1 q_2 q1q2表示了连续两次的旋转。
四元数微分形式推导
以无人机的姿态表示为例子,我们定义单位四元数 q ( t ) q(t) q(t)来表示从无人机的地理系E到机体系B的旋转关系。在 t + Δ t t+\Delta t t+Δt时刻,旋转可表示为 q ( t + Δ t ) q(t+\Delta t) q(t+Δt);即在 Δ t \Delta t Δt过程中,机体坐标系又经过了一个微小旋转,这个微小旋转的瞬时旋转角速度为 w w w;接着对瞬时旋转轴做单位化处理 w ^ = w / ∣ ∣ w ∣ ∣ \hat{w}=w/||w|| w^=w/∣∣w∣∣在 Δ t \Delta t Δt时刻转过的角度为 Δ θ = Δ t ∣ ∣ w ∣ ∣ \Delta \theta =\Delta t||w|| Δθ=Δt∣∣w∣∣则这次的微小旋转可由如下形式的单位四元数表示:
Δ q = cos Δ θ 2 + sin Δ θ 2 w ^ = cos ∣ ∣ w ∣ ∣ 2 Δ t + sin ∣ ∣ w ∣ ∣ 2 Δ t w ^ \Delta q=\cos \frac{\Delta \theta}{2}+\sin \frac{\Delta \theta}{2} \hat{w} = \cos \frac{||w||}{2} \Delta t + \sin \frac{||w||}{2} \Delta t \hat{w} Δq=cos2Δθ+sin2Δθw^=cos2∣∣w∣∣Δt+sin2∣∣w∣∣Δtw^
假定地理系到机体系的旋转四元数为
q
e
b
=
[
q
0
q
1
q
2
q
3
]
T
q^b_e =\left[\begin{array}{cccc}q_0 & q_1 & q_2 & q_3 \end{array}\right]^T
qeb=[q0q1q2q3]T;
则根据上述2.有
[
0
e
r
]
T
=
(
q
b
e
)
−
1
[
0
b
r
]
T
q
b
e
\left[\begin{array}{cc}0 & ^er \end{array}\right]^T=(q^e_b)^{-1} \left[\begin{array}{cc}0 & ^br \end{array}\right]^T q^e_b \\
[0er]T=(qbe)−1[0br]Tqbe
即(注意:此处转化为与1.相同的表示形式,但两者的物理意义不同):
[
0
e
r
]
T
=
(
q
e
b
)
[
0
b
r
]
T
(
q
e
b
)
−
1
\left[\begin{array}{cc}0 & ^er \end{array}\right]^T=(q^b_e) \left[\begin{array}{cc}0 & ^br \end{array}\right]^T (q^b_e)^{-1}
[0er]T=(qeb)[0br]T(qeb)−1
那么根据上面的推导,连续两次的旋转可以表示为: q ( t + Δ t ) = Δ q ⋅ q q(t+\Delta t)=\Delta q \cdot q q(t+Δt)=Δq⋅q;则有:
q ( t + Δ t ) − q ( t ) = ( cos ∣ ∣ w ∣ ∣ 2 Δ t + sin ∣ ∣ w ∣ ∣ 2 Δ t w ^ ) q ( t ) − q ( t ) = − 2 sin 2 ∣ ∣ w ∣ ∣ 2 Δ t q ( t ) + w ^ sin ∣ ∣ w ∣ ∣ 2 Δ t q ( t ) q(t+\Delta t)-q(t)=(\cos \frac{||w||}{2} \Delta t + \sin \frac{||w||}{2} \Delta t \hat{w})q(t)-q(t) \\ =-2{\sin ^2 \frac{||w||}{2}} \Delta t q(t) + \hat{w} \sin \frac{||w||}{2} \Delta t q(t) q(t+Δt)−q(t)=(cos2∣∣w∣∣Δt+sin2∣∣w∣∣Δtw^)q(t)−q(t)=−2sin22∣∣w∣∣Δtq(t)+w^sin2∣∣w∣∣Δtq(t)
略去高阶项可得:
q
(
t
+
Δ
t
)
−
q
(
t
)
=
w
^
sin
∣
∣
w
∣
∣
2
Δ
t
q
(
t
)
q(t+\Delta t)-q(t)= \hat{w} \sin \frac{||w||}{2} \Delta t q(t)
q(t+Δt)−q(t)=w^sin2∣∣w∣∣Δtq(t)
q
(
t
)
˙
=
lim
Δ
t
→
0
q
(
t
+
Δ
t
)
−
q
(
t
)
Δ
t
=
lim
Δ
t
→
0
w
^
sin
∣
∣
w
∣
∣
2
Δ
t
⋅
q
(
t
)
=
1
2
w
^
⋅
∥
w
∥
⋅
q
(
t
)
=
1
2
⋅
w
⋅
q
(
t
)
\dot{q(t)}=\lim _{\Delta t \rightarrow 0} \frac{q(t+\Delta t)-q(t)}{\Delta t} =\lim _{\Delta t \rightarrow 0} \frac{\hat{w} \sin \frac{||w||}{2}}{\Delta t}\cdot q(t) \\ =\frac{1}{2} \hat{w} \cdot\|w\| \cdot q(t) \\ = \frac{1}{2} \cdot w \cdot q(t)
q(t)˙=Δt→0limΔtq(t+Δt)−q(t)=Δt→0limΔtw^sin2∣∣w∣∣⋅q(t)=21w^⋅∥w∥⋅q(t)=21⋅w⋅q(t)
注意:此处的 ⋅ \cdot ⋅表示四元数乘法; w w w为角速度的纯四元数表示, w = [ 0 w x w y w z ] T w=\left[\begin{array}{cccc}{0} & {w_{x}} & {w_{y}} & {w_{z}}\end{array}\right]^T w=[0wxwywz]T;
由于实际工程中我们都是通过固连在机体上的陀螺仪等传感器来获知机体角速度
w
b
w^b
wb它与地理坐标系下的角速度表示有如下关系
w
=
q
(
t
)
w
b
q
(
t
)
−
1
w=q(t)w^bq(t)^{-1}
w=q(t)wbq(t)−1带入上式即可得到姿态解算过程中常用的四元数的微分形式
q
˙
(
t
)
=
1
2
q
(
t
)
w
b
\dot q(t)=\frac {1} {2}q(t)w^b
q˙(t)=21q(t)wb
可以看出,通过一次四元数乘法运算便可得到四元数的微分。
上式可以写成如下的矩阵形式:
[ q ˙ 0 q ˙ 1 q ˙ 2 q ˙ 3 ] = 1 2 [ q 0 − q 1 − q 2 − q 3 q 1 q 0 − q 3 q 2 q 2 q 3 q 0 − q 1 q 3 − q 2 q 1 q 0 ] [ 0 ω x b ω y b ω z b ] \left[\begin{array}{c}{\dot{q}_{0}} \\ {\dot{q}_{1}} \\ {\dot{q}_{2}} \\ {\dot{q}_{3}}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{q_{0}} & {-q_{1}} & {-q_{2}} & {-q_{3}} \\ {q_{1}} & {q_{0}} & {-q_{3}} & {q_{2}} \\ {q_{2}} & {q_{3}} & {q_{0}} & {-q_{1}} \\ {q_{3}} & {-q_{2}} & {q_{1}} & {q_{0}}\end{array}\right]\left[\begin{array}{c}{0} \\ {\omega_{x}^{b}} \\ {\omega_{y}^{b}} \\ {\omega_{z}^{b}}\end{array}\right] ⎣⎢⎢⎡q˙0q˙1q˙2q˙3⎦⎥⎥⎤=21⎣⎢⎢⎡q0q1q2q3−q1q0q3−q2−q2−q3q0q1−q3q2−q1q0⎦⎥⎥⎤⎣⎢⎢⎡0ωxbωybωzb⎦⎥⎥⎤
或者:
[ q ˙ 0 q ˙ 1 q ˙ 2 q ˙ 3 ] = 1 2 [ 0 − ω x b − ω y b − ω z b ω x b 0 ω z b − ω y b ω y b − ω z b 0 ω x b ω z b ω y b − ω x b 0 ] [ q 0 q 1 q 2 q 3 ] \left[\begin{array}{l}{\dot{q}_{0}} \\ {\dot{q}_{1}} \\ {\dot{q}_{2}} \\ {\dot{q}_{3}}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{0} & {-\omega_{x}^{b}} & {-\omega_{y}^{b}} & {-\omega_{z}^{b}} \\ {\omega_{x}^{b}} & {0} & {\omega_{z}^{b}} & {-\omega_{y}^{b}} \\ {\omega_{y}^{b}} & {-\omega_{z}^{b}} & {0} & {\omega_{x}^{b}} \\ {\omega_{z}^{b}} & {\omega_{y}^{b}} & {-\omega_{x}^{b}} & {0}\end{array}\right]\left[\begin{array}{l}{q_{0}} \\ {q_{1}} \\ {q_{2}} \\ {q_{3}}\end{array}\right] ⎣⎢⎢⎡q˙0q˙1q˙2q˙3⎦⎥⎥⎤=21⎣⎢⎢⎡0ωxbωybωzb−ωxb0−ωzbωyb−ωybωzb0−ωxb−ωzb−ωybωxb0⎦⎥⎥⎤⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤
四元数与旋转矩阵的关系
我们知道,从地理坐标系到机体坐标系的旋转矩阵可以有如下形式表示:(一般均为zyx顺序旋转)
R e b = R X ⋅ R Y ⋅ R z = [ 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 θ ] R^b_e=R_{X} \cdot R_{Y} \cdot R_{z}=\left[ \begin{array}{cc}{\cos \theta \cos \psi} & {\cos \theta \sin \psi} & {-\sin \theta} \\ {\sin \phi \sin \theta \cos \psi-\cos \phi \sin \psi} & {\sin \phi \sin \theta \sin \psi+\cos \phi \cos \psi} & {\sin \phi \cos \theta} \\ {\cos \phi \sin \theta \cos \psi+\sin \phi \sin \psi} & {\cos \phi \sin \theta \sin \psi-\sin \phi \cos \psi} & {\cos \phi \cos \theta}\end{array}\right] Reb=RX⋅RY⋅Rz=⎣⎡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
b
e
=
[
1
−
2
q
2
2
−
2
q
3
2
2
q
1
q
2
−
2
q
0
q
3
2
q
1
q
3
+
2
q
0
q
2
2
q
1
q
2
+
2
q
0
q
3
1
−
2
q
1
2
−
2
q
3
2
2
q
2
q
3
−
2
q
0
q
1
2
q
1
q
3
−
2
q
0
q
2
2
q
2
q
3
+
2
q
0
q
1
1
−
2
q
1
2
−
2
q
2
2
]
R^e_b=\left[ \begin{array}{lll}{1-2 q_{2}^{2}-2 q_{3}^{2}} & {2 q_{1} q_{2}-2 q_{0} q_{3}} & {2 q_{1} q_{3}+2 q_{0} q_{2}} \\ {2 q_{1} q_{2}+2 q_{0} q_{3}} & {1-2 q_{1}^{2}-2 q_{3}^{2}} & {2 q_{2} q_{3}-2 q_{0} q_{1}} \\ {2 q_{1} q_{3}-2 q_{0} q_{2}} & {2 q_{2} q_{3}+2 q_{0} q_{1}} & {1-2 q_{1}^{2}-2 q_{2}^{2}}\end{array}\right]
Rbe=⎣⎡1−2q22−2q322q1q2+2q0q32q1q3−2q0q22q1q2−2q0q31−2q12−2q322q2q3+2q0q12q1q3+2q0q22q2q3−2q0q11−2q12−2q22⎦⎤
则有
R
b
e
=
R
e
b
T
R^e_b={R^b_e} ^T
Rbe=RebT再根据对应项相等,即可求出由四元数表示的欧拉角
ϕ = arctan 2 ( 2 ( q 2 q 3 + q 0 q 1 ) , ( 1 − 2 ( q 1 2 + q 2 2 ) ) ) \phi=\arctan 2\left(2\left(q_{2} q_{3}+q_{0} q_{1}\right),\left(1-2\left(q_{1}^{2}+q_{2}^{2}\right)\right)\right) ϕ=arctan2(2(q2q3+q0q1),(1−2(q12+q22)))
θ = − arcsin ( 2 ( q 1 q 3 − q 0 q 2 ) ) \theta=-\arcsin \left(2\left(q_{1} q_{3}-q_{0} q_{2}\right)\right) θ=−arcsin(2(q1q3−q0q2))
ψ = arctan 2 ( 2 ( q 1 q 2 + q 0 q 3 ) , ( 1 − 2 ( q 2 2 + q 3 2 ) ) ) \psi=\arctan 2\left(2\left(q_{1} q_{2}+q_{0} q_{3}\right),\left(1-2\left(q_{2}^{2}+q_{3}^{2}\right)\right)\right) ψ=arctan2(2(q1q2+q0q3),(1−2(q22+q32)))
同样的,由欧拉角表示的四元数如下:
q = = [ cos ϕ 2 cos θ 2 cos ψ 2 + sin ϕ 2 sin θ 2 sin ψ 2 sin ψ 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{\phi }{2} \cos \frac{\theta }{2} \cos \frac{\psi}{2}+\sin \frac{\phi }{2} \sin \frac{\theta }{2} \sin \frac{\psi}{2}} \\ {\sin \frac{\psi}{2} \cos \frac{\psi}{2}-\cos \frac{\phi }{2} \sin \frac{\theta }{2} \sin \frac{\psi}{2}} \\ {\cos \frac{\phi }{2} \sin \frac{\theta }{2} \cos \frac{\psi}{2}+\sin \frac{\phi }{2} \cos \frac{\theta }{2} \sin \frac{\psi}{2}} \\ {\cos \frac{\phi }{2} \cos \frac{\theta }{2} \sin \frac{\psi}{2}-\sin \frac{\phi }{2} \sin \frac{\theta }{2} \cos \frac{\psi}{2}}\end{array}\right] q==⎣⎢⎢⎡cos2ϕcos2θcos2ψ+sin2ϕsin2θsin2ψsin2ψcos2ψ−cos2ϕsin2θsin2ψcos2ϕsin2θcos2ψ+sin2ϕcos2θsin2ψcos2ϕcos2θsin2ψ−sin2ϕsin2θcos2ψ⎦⎥⎥⎤
上式中 ϕ , θ , ψ \phi ,\theta , \psi ϕ,θ,ψ分别表示绕机体三轴转动的欧拉角;
欧拉角表示法的弊端
欧拉角速率与机体角速度之间的关系如下式所示:
[ ϕ ˙ θ ˙ ψ ˙ ] = [ 1 sin ϕ tan θ cos ϕ tan θ 0 cos ϕ − sin ϕ 0 sin ϕ / cos θ cos ϕ / cos θ ] [ p q r ] \left[\begin{array}{c}{\dot{\phi}} \\ {\dot{\theta}} \\ {\dot{\psi}}\end{array}\right]=\left[\begin{array}{ccc}{1} & {\sin \phi \tan \theta} & {\cos \phi \tan \theta} \\ {0} & {\cos \phi} & {-\sin \phi} \\ {0} & {\sin \phi / \cos \theta} & {\cos \phi / \cos \theta}\end{array}\right]\left[\begin{array}{l}{\boldsymbol{p}} \\ {\boldsymbol{q}} \\ {\boldsymbol{r}}\end{array}\right] ⎣⎡ϕ˙θ˙ψ˙⎦⎤=⎣⎡100sinϕtanθcosϕsinϕ/cosθcosϕtanθ−sinϕcosϕ/cosθ⎦⎤⎣⎡pqr⎦⎤
可以看到,当 θ = 9 0 。 \theta =90^。 θ=90。这时0出现在分母上,也就是我们所说的解不存在的情况,此时无法实现有效控制。这就是常说的“万向节锁死”;这也是不用旋转矩阵来进行姿态递推的原因。