四元数代数基础
四元数的定义
四元数通常被定义为
q ˉ = q 1 i + q 2 j + q 3 k + q 4 \bar{q} = q_{1}\mathbf{i} + q_{2}\mathbf{j} + q_{3}\mathbf{k} + q_{4} qˉ=q1i+q2j+q3k+q4
其中 i , j \mathbf{i}, \mathbf{j} i,j 和 k \mathbf{k} k 为虚数单位, 并且满足
i 2 = − 1 , j 2 = − 1 , k 2 = − 1 − i j = j i = k − j k = k j = i − k i = i k = j \mathbf{i}^{2} = -1,\; \mathbf{j}^{2} = -1,\; \mathbf{k}^{2} = -1 \\ -\mathbf{i}\mathbf{j} = \mathbf{j}\mathbf{i} = \mathbf{k}\\ -\mathbf{j}\mathbf{k} = \mathbf{k}\mathbf{j} = \mathbf{i}\\ -\mathbf{k}\mathbf{i} = \mathbf{i}\mathbf{k} = \mathbf{j} i2=−1,j2=−1,k2=−1−ij=ji=k−jk=kj=i−ki=ik=j
q
4
q_{4}
q4 为四元数的实部(或称为标量部分, scalar part),
q
1
i
+
q
2
j
+
q
3
k
q_{1}\mathbf{i} + q_{2}\mathbf{j} + q_{3}\mathbf{k}
q1i+q2j+q3k 为四元数的虚部(或向量部分, vector part). 四元数因此可以表示成列向量
q
ˉ
=
[
q
q
4
]
=
[
q
1
q
2
q
3
q
4
]
T
\bar{q} = \begin{bmatrix} \mathbf{q} \\ q_{4} \end{bmatrix} = \begin{bmatrix} q_{1} & q_{2} & q_{3} & q_{4} \end{bmatrix}^{\mathrm{T}}
qˉ=[qq4]=[q1q2q3q4]T
如果
q
\mathbf{q}
q 与
q
4
q_{4}
q4 满足
q
=
[
k
x
sin
(
θ
/
2
)
k
y
sin
(
θ
/
2
)
k
z
sin
(
θ
/
2
)
]
=
k
^
sin
(
θ
/
2
)
,
q
4
=
cos
(
θ
/
2
)
\mathbf{q} = \begin{bmatrix} k_{x}\sin{(\theta / 2)} \\ k_{y}\sin{(\theta / 2)} \\ k_{z}\sin{(\theta / 2)} \end{bmatrix} = \hat{\mathbf{k}}\sin{(\theta / 2)}, \quad q_{4} = \cos{(\theta / 2)}
q=
kxsin(θ/2)kysin(θ/2)kzsin(θ/2)
=k^sin(θ/2),q4=cos(θ/2)
则 q ˉ \bar{q} qˉ 被称为 “旋转的四元数(quaternion of rotation)”. 上述记号中, k ^ \hat{\mathbf{k}} k^ 描述了沿着旋转轴的单位向量, θ \theta θ 表示旋转的角度.
旋转四元数为单位四元数, 满足
∣
q
ˉ
∣
=
q
ˉ
T
q
ˉ
=
∣
q
∣
2
+
q
4
2
=
1
|\bar{q}| = \sqrt{\bar{q}^{\mathrm{T}}\bar{q}} = \sqrt{{|\mathbf{q}|}^{2} + q_{4}^{2}} = 1
∣qˉ∣=qˉTqˉ=∣q∣2+q42=1
此后, 我们将使用"四元数"这个属于来表示旋转的四元数.
四元数 q ˉ \bar{q} qˉ 和四元数 − q ˉ -\bar{q} −qˉ 描述了最终坐标系位置相同两个的旋转, 即轴角表示法不是唯一的. 唯一的区别是旋转的方向, 具有正实部 q 4 q_{4} q4 的四元数描述了最短的旋转.
四元数相乘
常用等式
外积反对称矩阵的性质
反交换性
加法分配性
标量乘
平行向量的外积
拉格朗日公式(Lagrange’s Formula)
雅克比(Jacobi)等式
旋转
用四元数表示的向量外积
⌊ ω × ⌋ \lfloor \bm\omega\times \rfloor ⌊ω×⌋ 的幂
矩阵 Ω \bm\Omega Ω 的性质
矩阵 Ξ \bm\Xi Ξ 的性质
四元数与旋转矩阵的关系
四元数的时间导数
考虑本地坐标系(local coordinate frame)
{
L
}
\{L\}
{L} 正在相对于全局参考系(global reference frame)
{
G
}
\{G\}
{G} 运动, 我们可以计算对应旋转的变化率, 或称为导数. 我们根据导数的定义进行计算
q
ˉ
˙
G
L
(
t
)
(
t
)
=
lim
Δ
t
→
0
1
Δ
t
(
q
ˉ
G
L
(
t
+
Δ
t
)
−
q
ˉ
G
L
(
t
)
)
\.{\bar{q}}_{G}^{L(t)}(t) = \lim_{\Delta t \to 0}{\dfrac{1}{\Delta t}\left( \bar{q}_{G}^{L(t+\Delta t)} - \bar{q}_{G}^{L(t)} \right)}
qˉ˙GL(t)(t)=Δt→0limΔt1(qˉGL(t+Δt)−qˉGL(t))
四元数
q
ˉ
G
L
(
t
+
Δ
t
)
\bar{q}_{G}^{L(t+\Delta t)}
qˉGL(t+Δt) 可以表示为两个四元数的积
q
ˉ
G
L
(
t
+
Δ
t
)
=
q
ˉ
L
(
t
)
L
(
t
+
Δ
t
)
⊗
q
ˉ
G
L
(
t
)
\bar{q}_{G}^{L(t+\Delta t)} = \bar{q}_{L(t)}^{L(t+\Delta t)}\otimes \bar{q}_{G}^{L(t)}
qˉGL(t+Δt)=qˉL(t)L(t+Δt)⊗qˉGL(t)
其中
q
ˉ
L
(
t
)
L
(
t
+
Δ
t
)
=
[
k
^
sin
(
θ
/
2
)
cos
(
θ
/
2
)
]
\bar{q}_{L(t)}^{L(t+\Delta t)} = \begin{bmatrix} \hat{\mathbf{k}}\sin{(\theta / 2)} \\ \cos{(\theta / 2)} \end{bmatrix}
qˉL(t)L(t+Δt)=[k^sin(θ/2)cos(θ/2)]
注意到四元数
q
ˉ
L
(
t
)
L
(
t
+
Δ
t
)
\bar{q}_{L(t)}^{L(t+\Delta t)}
qˉL(t)L(t+Δt) 表示了从坐标系
{
L
(
t
)
}
\{L(t)\}
{L(t)} 到坐标系
{
L
(
t
+
Δ
t
)
}
\{L(t+\Delta t)\}
{L(t+Δt)} 的旋转, 用在坐标系
{
L
(
t
+
Δ
t
)
}
\{L(t+\Delta t)\}
{L(t+Δt)}下的单位向量
k
^
\hat{\mathbf{k}}
k^ 表示旋转轴, 用
θ
\theta
θ 表示旋转角度.
在极限的计算中, 当 Δ t → 0 \Delta t \to 0 Δt→0, 旋转角将趋于无穷小, 所以可以用一阶泰勒展开(Taylor expansion) 来近似 sin \sin sin 与 cos \cos cos 函数
q ˉ L ( t ) L ( t + Δ t ) = [ k ^ sin ( θ / 2 ) cos ( θ / 2 ) ] ≈ [ k ^ ⋅ θ / 2 1 ] = [ 1 2 ⋅ δ θ 1 ] \bar{q}_{L(t)}^{L(t+\Delta t)} = \begin{bmatrix} \hat{\mathbf{k}}\sin{(\theta / 2)} \\ \cos{(\theta / 2)} \end{bmatrix} \approx \begin{bmatrix} \hat{\mathbf{k}}\cdot\theta/2 \\ 1 \end{bmatrix} = \begin{bmatrix} \frac{1}{2} \cdot \delta\bm{\theta} \\ 1 \end{bmatrix} qˉL(t)L(t+Δt)=[k^sin(θ/2)cos(θ/2)]≈[k^⋅θ/21]=[21⋅δθ1]
δ
θ
\delta\bm{\theta}
δθ 表示了从坐标系
{
L
(
t
)
}
\{L(t)\}
{L(t)} 到坐标系
{
L
(
t
+
Δ
t
)
}
\{L(t+\Delta t)\}
{L(t+Δt)} 旋转的轴角向量, 除以时间微元
Δ
t
\Delta t
Δt, 并计算极限即得到了角速度
ω
=
lim
Δ
t
→
0
δ
θ
Δ
t
\bm{\omega} = \lim_{\Delta t \to 0}{\dfrac{\delta\bm\theta}{\Delta t}}
ω=Δt→0limΔtδθ
于是四元数的导数可以推导为
q
ˉ
˙
G
L
(
t
)
(
t
)
=
lim
Δ
t
→
0
1
Δ
t
(
q
ˉ
G
L
(
t
+
Δ
t
)
−
q
ˉ
G
L
(
t
)
)
=
lim
Δ
t
→
0
1
Δ
t
(
q
ˉ
L
(
t
)
L
(
t
+
Δ
t
)
⊗
q
ˉ
G
L
(
t
)
−
q
ˉ
G
L
(
t
)
)
≈
lim
Δ
t
→
0
1
Δ
t
(
[
1
2
⋅
δ
θ
1
]
−
[
0
1
]
)
⊗
q
ˉ
G
L
(
t
)
=
1
2
[
ω
0
]
⊗
q
ˉ
G
L
(
t
)
=
1
2
[
−
⌊
ω
×
⌋
ω
−
ω
T
0
]
q
ˉ
G
L
(
t
)
=
1
2
Ω
(
ω
)
q
ˉ
G
L
(
t
)
=
1
2
Ξ
(
q
ˉ
G
L
(
t
)
)
ω
\begin{aligned} \.{\bar{q}}_{G}^{L(t)}(t) =& \lim_{\Delta t \to 0}{\dfrac{1}{\Delta t}\left( \bar{q}_{G}^{L(t+\Delta t)} - \bar{q}_{G}^{L(t)} \right)} \\ =& \lim_{\Delta t \to 0}{\dfrac{1}{\Delta t}\left( \bar{q}_{L(t)}^{L(t+\Delta t)}\otimes \bar{q}_{G}^{L(t)} - \bar{q}_{G}^{L(t)} \right)} \\ \approx& \lim_{\Delta t \to 0}{\dfrac{1}{\Delta t}\left( \begin{bmatrix} \frac{1}{2} \cdot \delta\bm{\theta} \\ 1 \end{bmatrix} - \begin{bmatrix} \bm{0} \\ 1 \end{bmatrix} \right)\otimes\bar{q}_{G}^{L(t)}} \\ =& \dfrac{1}{2}\begin{bmatrix} \bm{\omega} \\ 0 \end{bmatrix} \otimes \bar{q}_{G}^{L(t)} \\ =& \dfrac{1}{2} \begin{bmatrix} -\lfloor\bm{\omega\times}\rfloor & \bm{\omega}\\ -\bm{\omega}^{\rm{T}} & 0 \end{bmatrix} \bar{q}_{G}^{L(t)} \\ =& \dfrac{1}{2}\bm{\Omega}(\bm{\omega})\bar{q}_{G}^{L(t)} \\ =& \dfrac{1}{2}\bm\Xi\left(\bar{q}_{G}^{L(t)}\right)\bm\omega \end{aligned}
qˉ˙GL(t)(t)==≈====Δt→0limΔt1(qˉGL(t+Δt)−qˉGL(t))Δt→0limΔt1(qˉL(t)L(t+Δt)⊗qˉGL(t)−qˉGL(t))Δt→0limΔt1([21⋅δθ1]−[01])⊗qˉGL(t)21[ω0]⊗qˉGL(t)21[−⌊ω×⌋−ωTω0]qˉGL(t)21Ω(ω)qˉGL(t)21Ξ(qˉGL(t))ω
其中
Ω
(
ω
)
=
[
0
ω
z
−
ω
y
ω
x
−
ω
z
0
ω
x
ω
y
ω
y
−
ω
x
0
ω
z
−
ω
x
−
ω
y
−
ω
z
0
]
\bm\Omega(\bm\omega) = \begin{bmatrix} 0 & \omega_{z} & -\omega_{y} & \omega_{x} \\ -\omega_{z} & 0 & \omega_{x} & \omega_{y} \\ \omega_{y} & -\omega_{x} & 0 & \omega_{z} \\ -\omega_{x} & -\omega_{y} & -\omega_{z} & 0 \\ \end{bmatrix} \\
Ω(ω)=
0−ωzωy−ωxωz0−ωx−ωy−ωyωx0−ωzωxωyωz0
并且
Ξ
(
q
ˉ
G
L
(
t
)
)
=
[
q
4
−
q
3
q
2
q
3
q
4
−
q
1
−
q
2
q
1
q
4
−
q
1
−
q
2
−
q
3
]
\bm\Xi\left(\bar{q}_{G}^{L(t)}\right) = \begin{bmatrix} q_{4} & -q_{3} & q_{2} \\ q_{3} & q_{4} & -q_{1} \\ -q_{2} & q_{1} & q_{4} \\ -q_{1} & -q_{2} & -q_{3} \end{bmatrix}
Ξ(qˉGL(t))=
q4q3−q2−q1−q3q4q1−q2q2−q1q4−q3
并且请注意, 旋转速率
ω
=
ω
L
(
t
)
\bm\omega = \bm\omega^{L(t)}
ω=ωL(t) 表示在本地坐标系中, 并非惯性坐标系(全局参考系)中.
参考文献
[1] R. O. Allen and D. H. Chang. Performance Testing of the Systron Donner Quartz Gyro (QRS11-100-420); Sn’s 3332, 3347 and 3544. Technical Report ENGINEERING MEMORANDUM EM #343-1297, JPL, 1993.
[2] W. G. Breckenridge. Quaternions - Proposed Standard Conventions. Technical Report INTEROFFICE MEMORANDUM IOM 343-79-1199, JPL, 1999.
[3] E. J. Lefferts, F. L. Markley, and M. D. Shuster. Kalman filtering for spacecraft attitude estimation. Journal of Guidance, Control, and Dynamics, 5(5):417–429, Sept.-Oct. 1982.
[4] P. S. Maybeck. Stochastic Models, Estimation and Control, volume 1. Academic Press, New York, 1979.
[5] M. D. Shuster. A survey of attitude representations. Journal of the Astronautical Sciences, 41(4):439–517, October–December 1993.
[6] D. Simon. Optimal State Estimation, Kalman, H∞, and Nonlinear Approaches. John Wiley & Sons, Inc., Hoboken, NJ, 2006.
[7] N. Trawny. Sun sensor model. Technical Report 2005-001, University of Minnesota, Dept. of Comp. Sci. & Eng., Jan. 2005.
[8] J. R. Wertz, editor. Spacecraft Attitude Determination and Control. Kluwer Academic, Dordrecht; Boston, 1978