3D姿态估计-间接卡尔曼滤波

Indirect Kalman Filter for 3D Attitude Estimation

翻译一篇2005的论文。

1 四元数代数中的元素

1.1 四元数定义

四元数通常定义为

q⃗ =q4+q1i+q2j+q3k

其中 i，j，k是纯虚数，满足以下条件

i2=−1,j2=−1,k2=−1−ij=ji=k,−jk=kj=i,−ki=ik=j

数字 q4 是一个实数或者说四元数的标量部分， q1i+q2j+q3k 是虚部或者说是四元数的矢量部分，寺院数量可以被写为4维的列矩阵，

q⃗ =[qq4]=[q1q2q3q4]T

如果数 q 和 q4 满足

q=⎡⎣⎢kxsin(θ/2)kysin(θ/2)kzsin(θ/2)⎤⎦⎥=k⃗ sin(θ/2),q4=cos(θ/2)

那么元素 q1,...,q4 叫做“旋转四元数”或者“欧拉对称参数”，在这个等式中， k⃗  是轴向的单位四元数， θ 是旋转角度，旋转四元数是一个单位四元数，满足

|q⃗ |=q⃗ Tq⃗ −−−√=|q|2+q24−−−−−−−−√=1

因此我们用术语四元数来表示旋转四元数。

1.2 四元数乘法

四元数乘法定义为

q̄ ⊗p̄ =(q4+q1i+q2j+q3k)(p4+p1i+p2j+p3k)=q4p4−q1p1−q2p2−q3p3+(q4p1+q1p4−q2p3+q3p2)i+(q4p2+q2p4−q3p1+q1p3)j+(q4p3+q3p4−q1p2+q2p1)k=⎡⎣⎢⎢⎢⎢q4p1+q3p2−q2p3+q1p4−q3p1+q4p2+q1p3+q2p4q2p1−q1p2+q4p3+q3p4−q1p1−q2p2−q2p3+q4p4⎤⎦⎥⎥⎥⎥

四元数乘法可以另写成矩阵形式，为此，我们先介绍矩阵叉乘符号，使用反对称运算

⌊q×⌋=⎡⎣⎢0q3−q2−q30q1q2−q10⎤⎦⎥

叉乘可以被写成

q×p=∣∣∣∣iq1p1jq2p2kq3p3∣∣∣∣=⎡⎣⎢q2p3−q3p2q3p1−q1p3q1p2−q2p1⎤⎦⎥=⎡⎣⎢0q3−q2−q30q1q2−q10⎤⎦⎥⎡⎣⎢p1p2p3⎤⎦⎥=⌊q×⌋p

四元数的乘法可以被重写成

q̄ ⊗p̄ =L(q̄ )p̄ =⎡⎣⎢⎢⎢⎢q4−q3q2−q1q3q4−q1−q2−q2q1q4−q3q1q2q3q4⎤⎦⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢p1p2p3p4⎤⎦⎥⎥⎥⎥=[q4I3×3−⌊q×⌋−qTqq4][pp4]=[q4p+p4q−q×pp4q4−qTp]=[p4q+q4p+⌊p×⌋qp4q4−pTq]=[p4I3×3+⌊p×⌋−pTpp4][qq4]=⎡⎣⎢⎢⎢⎢p4p3−p2−p1−p3p4p1−p2p2−p1p4−p3p1p2p3p4⎤⎦⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢q1q2q3q4⎤⎦⎥⎥⎥⎥=R(p̄ )q̄ 

L 和 R 的属性如下：

L(q̄ −1)=LT(q̄ )R(p̄ −1)=RT(p̄ )L=[Ψ(q̄ )q̄ ]R=[Ξ(p̄ )p̄ ]

其中Ψ和Ξ定义为

Ψ=[q4I3×3−⌊q×⌋−qT]Ξ=[p4I3×3−⌊p×⌋−pT]

如果两个旋转， CA and CB 是相关的，例如 CACX=CXCB (手眼标定),那么相关的矩阵 L(q̄ A)−R(q̄ B) 是反对称矩阵，秩为2，特别， q̄ A4=q̄ B4 , ||qA||=||qB|| 。
四元数有一个关于乘法中性的元素，定义为

q̄ 0=[0001]Tq̄ ⊗q̄ 0=q̄ 0⊗q̄ =q̄ 

旋转的逆描述为逆或者负数共扼四元数，表示为

q̄ −1=[−qq4]=[−ksin(θ/2)cos(θ/2)]=[ksin(−θ/2)cos(−θ/2)]

q̄ ⊗q̄ −1=q̄ −1⊗q̄ =q̄ 0(q̄ ⊗p̄ )−1=p̄ −1⊗q̄ −1

1.3 有用的性质

1.3.1 叉乘反对称矩阵 ⌊ω×⌋ 的属性

反交換律

⌊ω×⌋=−⌊ω×⌋T

⌊a×⌋b=−⌊b×⌋a⟸aT⌊b×⌋=−bT⌊a×⌋

加法分配律

⌊a×⌋+⌊b×⌋=⌊a+b×⌋

标量乘法

c⋅⌊ω×⌋=⌊c⋅ω×⌋

平行矢量叉乘

ω×(c⋅ω)=c⋅⌊ω×⌋ω=−c⋅(ωT⌊ω×⌋)T=03×1

拉格朗日公式

⌊a×⌋⌊b×⌋=baT−(aTb)I3×3⟸a×(b×c)=b(aTc)−c(aTb)

⌊a×⌋⌊b×⌋+abT=⌊b×⌋⌊a×⌋+baT⌊(a×b)×⌋=baT−abT

雅可比恒等式

⌊a×⌋⌊b×⌋c+⌊b×⌋⌊c×⌋a+⌊c×⌋⌊a×⌋b=0

旋转

⌊Ca×⌋=C⌊a×⌋CTC(a×b)=(Ca)×(Cb)

四元数表示的向量叉乘，如果我们定义四元数

ā=[a0],b̄ =[b0],c̄ =[c0]=[a×b0]

我们可以得到：

c̄ =12(b̄ ⊗ā+ā⊗b̄ −1)=12((L(b)+RT(b))ā=12([−2⌊b×⌋000][a0])

⌊ω×⌋ 的次方

⌊ω×⌋2=ωωT−|ω|2⋅I3×3⌊ω×⌋3=−|ω|2⋅⌊ω×⌋⌊ω×⌋4=−|ω|2⋅⌊ω×⌋2⌊ω×⌋5=|ω|4⋅⌊ω×⌋

以此类推。

1.3.2 Ω矩阵的性质

矩阵Ω在一个向量和四元数相乘时出现，他用于例如四元数微分，具有如下属性：

Ω(ω)=⎡⎣⎢⎢⎢⎢0−ωzωy−ωxωz0−ωx−ωy−ωyωx0ωzωxωyωz0⎤⎦⎥⎥⎥⎥=[−⌊ω×⌋−ωTω0]

Ω(ω)2=[⌊ω×⌋2−ωωT−ωT⌊ω×⌋−⌊ω×⌋ω−ωTω]=[−|ω|2⋅I3×3−01×303×1−|ω|2]=−|ω|2⋅I4×4

Ω(ω)3=−|ω|2⋅Ω(ω)

Ω(ω)4=−|ω|4⋅I4×4

Ω(ω)5=|ω|4⋅Ω(ω)

Ω(ω)6=−|ω|6⋅I4×4

以此类推。

1.3.3 Ξ矩阵的性质

Ξ(q̄)矩阵在一个向量与四元数相乘时出现。Ξ(q̄) 和 Ω(a)的关系，与乘法矩阵 L(q̄ ) 和 R(p̄ ) 的关系相同。

Ξ(q̄ )=⎡⎣⎢⎢⎢⎢q4q3−q2−q1−q3q4q1−q2q2−q1q4−q3⎤⎦⎥⎥⎥⎥=[q4⋅I3×3+⌊q×⌋−qT]

ΞT(q̄ )=⎡⎣⎢q4−q3q2q3q4−q1−q2q1q4−q1−q2−q3⎤⎦⎥

可以看出

ΞT(q̄ )Ξ(q̄ )=I3×3Ξ(q̄ )ΞT(q̄ )=I4×4−q̄ q̄ TΞT(q̄ )q̄ =03×1

Ξ 和 Ω的关系

Ω(a)q̄ =Ξ(q̄ )a

1.4 四元数和旋转矩阵之间的关系

给定一个向量P，我们定义对应的四元数为

p̄ =[p0]

我们将利用在不同坐标系中表示的向量之间的两个关系

pLp=C(q̄ )LGC(q̄ )pGp

q̄ =̄ LḠ q 和 CLGC(q̄ ) 是3×3的旋转矩阵，是(全局)坐标系{G}相对于(局部)坐标系{L}。
也可以将一个向量从一个坐标系转移到另一个坐标系通过将四元数分别左乘和右乘旋转四元数和它的逆。

pLp=qLGq̄ ⊗p̄ Gp̄ ⊗qLGq−1̄ =[(2q24−1)I3×3−2q4⌊q×⌋+2qqT0][PGP0]

由此我们得到四元数和他对应的旋转矩阵之间的关系。

CLGC(q̄ )=(2q4−1)I3×3−2q4⌊q×⌋+2qqT

也可以写成

CLGC(q̄ )=ΞT(q̄ )Ψ(q̄ )

我们可以通过三个四元数相乘得到相似的形式，尽管这没有什么物理意义

q̄ ⊗p̄ ⊗q̄ −1=L(q̄ )RT(q̄ )p̄ =[C(q̄ )001][pp4]=[C(q̄ )pp4]

如果只有一个很小的旋转 δq̄  ,我们可以用小的角度近似值来简化上面的表达式。我们可以描述一个小角度四元数

δq̄ =[δqδq4]=[ksin(δθ/2)cos(δθ/2)]≈[δθ/21]

我们得到对应的旋转矩阵表达式

CLGC(δq̄ )≈I3×3−⌊δθ×⌋

旋转矩阵也可以用轴角表示

CLGC=cos(θ)⋅I3×3−sin(θ)⌊k×⌋+(1−cos(θ))kkT=(2cos2(θ/2)−1)⋅I3×3−2cos(θ/2)sin(θ/2)⌊k×⌋+2sin2(θ/2)kkT

我们用四元数表示旋转矩阵

CLGC(q̄ )=⎡⎣⎢⎢q21−q22−q23+q242(q1q2−q3q4)2(q1q3+q2q4)2(q1q2+q3q4)−q21+q22−q23+q242(q2q3−q1q4)2(q1q3−q2q4)2(q2q3+q1q4)−q21−q22+q23+q24⎤⎦⎥⎥=⎡⎣⎢⎢2q21+2q24−12(q1q2−q3q4)2(q1q3+q2q4)2(q1q2+q3q4)2q22+2q24−12(q2q3−q1q4)2(q1q3−q2q4)2(q2q3+q1q4)2q23+2q24−1⎤⎦⎥⎥=⎡⎣⎢⎢1−2q22−2q23−12(q1q2−q3q4)2(q1q3+q2q4)2(q1q2+q3q4)1−2q21−2q232(q2q3−q1q4)2(q1q3−q2q4)2(q2q3+q1q4)1−2q21−2q22⎤⎦⎥⎥

旋转矩阵还有另一种表现形式

CLGC(q̄ )=I3×3−2q4⌊q×⌋+2⌊q×⌋2

注意，由于四元组乘法的约定，两个旋转矩阵的乘积将对应于相同顺序的两个四元的乘积，因此

CL1L2C(qL1L2q̄ )⋅CL2GC(qL2Gq̄ )=CL1GC(qL1L2q̄ ⊗qL2Gq̄ )

最终，我们可以解释 qL2L1q̄ =[ksin(θ/2)cos(θ/2)] 表示L2坐标系到L1坐标系的旋转，旋转轴K在L1中表示，这也可以表示为指数形式

CL1L2C(q̄ )=exp(−⌊k×⌋θ)

1.5 四元数时间微分

当本地坐标系L相对全局坐标系G移动，我们可以计算改变率或者对应的四元数微分来描述他们的关系，我们通过计算差分式的极限

qL(t)Gq̄ ′(t)=lim∆t→01∆t(qL(t+∆t)Gq̄ −qL(t)Gq̄ )

四元数 qL(t+∆t)Gq̄  可以表示为两个寺院书叉乘

qL(t+∆t)Gq̄ =qL(t+∆t)tq̄ ⊗qL(t)Gq̄ 

其中

qL(t+∆t)L(t)q̄ =[ksin(θ/2)cos(θ/2)]

注意，四元数 qL(t+∆t)L(t)q̄  表示L(t)坐标系到L(t+∆t)坐标系的旋转，旋转角度为θ，旋转轴K是L(t+∆t)坐标系下的表示。

在极限条件下，∆t → 0,旋转角度将会非常小，我们可以近似sin和cos函数用他们一阶泰勒展开式。

qL(t+∆t)L(t)q̄ =[ksin(θ/2)cos(θ/2)]=[kθ/21]=⎡⎣12⋅δθ1⎤⎦

向量δθ与旋转轴同向，大小为旋转角度，将这个矢量除以∆t，极限时，得到旋转速度

ω=lim∆t→0δθ∆t

现在我们准备推出四元数的导数

qL(t)Gq̄ ′(t)=lim∆t→01∆t(qL(t+∆t)Gq̄ −qL(t)Gq̄ )≈lim∆t→01∆t(⎡⎣12⋅δθ1⎤⎦−[01])⊗qL(t)Gq̄ =12[ω1])⊗qL(t)Gq̄ =12[−⌊ω×⌋−ωTω0]qL(t)Gq̄ =12Ω(ω)qL(t)Gq̄ =12Ξ(qL(t)Gq̄ )ω