四元数旋转运算过程

本博客通过运算验证旋转后的四元数仍是一个虚四元数,即实部分量为0,虚部三分量仍表示一个空间点。

  • 使用四元数 p=(0,x,y,z) p = ( 0 , x , y , z ) 表示空间点 (x,y,z) ( x , y , z ) ,即 p=(0,v⃗ ) p = ( 0 , v → ) ,实部为0,为虚四元数,
  • 使用单位向量 n⃗ =(n1,n2,n3) n → = ( n 1 , n 2 , n 3 ) 表示旋转轴。
  • 使用 θ θ 表示旋转角度。

  • 使用四元数 q=(cosθ2,n⃗ sinθ2) q = ( cos ⁡ θ 2 , n → sin ⁡ θ 2 ) 描述这一旋转,易验证 q q 为单位四元数满足 ||q||=1 | | q | | = 1

四元数的共轭定义如下:
p=(s,v⃗ ) p ∗ = ( s , − v → )
即对原四元数虚部取反得到其共轭
四元数求逆满足公式:
p1=q||p||2 p − 1 = q ∗ | | p | | 2

由于 q q 是单位四元数,因此求逆时,分母为1,所以:
q1=(cosθ2,n⃗ sinθ2) q − 1 = ( cos ⁡ θ 2 , − n → ⋅ sin ⁡ θ 2 )

旋转后坐标满足 p1=qpq1 p 1 = q p q − 1 1,本文通过四元数逐项相乘来分析 p1 p 1 实部结果

任给四元数 qa=(sa,va) q a = ( s a , v a ) qb=(sb,vb) q b = ( s b , v b ) ,四元数乘法法则如下:
qaqb=(sasbvTavb,savb+sbva+va×vb) q a q b = ( s a s b − v a T v b , s a v b + s b v a + v a × v b )

先求解 r=qp r = q p
r=qp=(cosθ20(n1v1+n2v2+n3v3)sinθ2(v⃗ cosθ2+(n⃗ sinθ2)×v⃗ )T)T r = q p = ( cos ⁡ θ 2 ⋅ 0 − ( n 1 v 1 + n 2 v 2 + n 3 v 3 ) ⋅ sin ⁡ θ 2 ( v → ⋅ cos ⁡ θ 2 + ( n → ⋅ sin ⁡ θ 2 ) × v → ) T ) T

其中, ((n⃗ sinθ2)×v⃗ )T=i⃗ n1sinθ2v1j⃗ n2sinθ2v2k⃗ n3sinθ2v3=sinθ2[n2v3v2n3,n3v1v3n1,n1v2v1n2] ( ( n → ⋅ sin ⁡ θ 2 ) × v → ) T = [ i → j → k → n 1 ⋅ sin ⁡ θ 2 n 2 ⋅ sin ⁡ θ 2 n 3 ⋅ sin ⁡ θ 2 v 1 v 2 v 3 ] = sin ⁡ θ 2 ⋅ [ n 2 v 3 − v 2 n 3 , n 3 v 1 − v 3 n 1 , n 1 v 2 − v 1 n 2 ]
所以, r=(n1v1+n2v2+n3v3)sinθ2cosθ2v1+sinθ2(n2v3v2n3)cosθ2v2+sinθ2(n3v1v3n1)cosθ2v3+sinθ2(n1v2v1n2)T r = ( − ( n 1 v 1 + n 2 v 2 + n 3 v 3 ) ⋅ sin ⁡ θ 2 cos ⁡ θ 2 ⋅ v 1 + sin ⁡ θ 2 ⋅ ( n 2 v 3 − v 2 n 3 ) cos ⁡ θ 2 ⋅ v 2 + sin ⁡ θ 2 ⋅ ( n 3 v 1 − v 3 n 1 ) cos ⁡ θ 2 ⋅ v 3 + sin ⁡ θ 2 ⋅ ( n 1 v 2 − v 1 n 2 ) ) T

再求解 q1=rq1=(s1,v1) q 1 = r q − 1 = ( s 1 , v 1 → ) 即可,我们只需验证 s1 s 1 是否为0即可。继续使用四元数乘法法则:
s1=((n1v1+n2v2+n3v3)sinθ2)cosθ2+(cosθ2v1+sinθ2(n2v3v2n3))n1sinθ2+(cosθ2v2+sinθ2(n3v1v3n1))n2sinθ2+(cosθ2v3+sinθ2(n1v2v1n2))n3sinθ2 s 1 = ( − ( n 1 v 1 + n 2 v 2 + n 3 v 3 ) ⋅ sin ⁡ θ 2 ) ⋅ cos ⁡ θ 2 + ( cos ⁡ θ 2 ⋅ v 1 + sin ⁡ θ 2 ⋅ ( n 2 v 3 − v 2 n 3 ) ) ⋅ n 1 ⋅ sin ⁡ θ 2 + ( cos ⁡ θ 2 ⋅ v 2 + sin ⁡ θ 2 ⋅ ( n 3 v 1 − v 3 n 1 ) ) ⋅ n 2 ⋅ sin ⁡ θ 2 + ( cos ⁡ θ 2 ⋅ v 3 + sin ⁡ θ 2 ⋅ ( n 1 v 2 − v 1 n 2 ) ) ⋅ n 3 ⋅ sin ⁡ θ 2
抵消化简后得到:
s1=(sinθ2)2n1n1v1n2n2v2n3n3v3 s 1 = ( sin ⁡ θ 2 ) 2 ⋅ | n 1 n 2 n 3 n 1 n 2 n 3 v 1 v 2 v 3 |
显然,此行列式为0,即旋转后的四元数 p1 p 1 实部为0,其虚部仍表示一个空间坐标,证毕。


  1. 高翔《视觉SLAM十四讲》 ↩

你可能感兴趣的:(SLAM相关)