参考:《Quaternion kinematics for error-state kalman filter》、《Indirect Kalman Filter for 3D Attitude Estimation》、《SLAM十四讲》
2019/2/21更新:
1)如何利用四元数计算angle distance
文章目录
旋转向量( the rotation vector
or the angle-axis vector
)和欧拉角可以使用3个量描述旋转,虽没有DoF的冗余,但是存在奇异性,就是我们常说的万向节死锁(Gimbal Lock
),简单说就是,当两个旋转轴重合时,绕这两个轴的旋转效果是一样的,所以表征机体旋转的欧拉角就不再是唯一的了. 所以出于这种考虑,就出现了旋转矩阵和四元数,四元数其实就是扩展的复数,关于复数表示旋转的理解,阮老师的博客 虚数的意义 讲得很通俗易懂,所以四元数使用最少的量同时保证旋转的紧凑性和非奇异性.
因为3D空间的旋转具有3个自由度,如果使用旋转矩阵的9个量描述3个DoF,那么就需要满足正交矩阵并且行列式为1,同样,使用四元数表示旋转应当满足单位四元数.
一、四元数基本运算
关于四元数的基本概念、加减法、乘法点乘数乘的区别、共轭与求逆、求模运算就不在这提了,《SLAM十四讲》已经写得很清楚了.
1.1 四元数的乘法
p
⊗
q
=
[
p
w
q
w
−
p
x
q
x
−
p
y
q
y
−
p
z
q
z
p
w
q
x
+
p
x
q
w
+
p
y
q
z
−
p
z
q
y
p
w
q
y
−
p
x
q
z
+
p
y
q
w
+
p
z
q
x
p
w
q
z
+
p
x
q
y
−
p
y
q
x
+
p
z
q
w
]
=
[
p
w
q
w
−
p
v
⊤
q
v
p
w
q
v
+
q
w
p
v
+
p
v
×
q
v
]
(1)
\mathbf { p } \otimes \mathbf { q } = \left[ \begin{array} { c } { p _ { w } q _ { w } - p _ { x } q _ { x } - p _ { y } q _ { y } - p _ { z } q _ { z } } \\ { p _ { w } q _ { x } + p _ { x } q _ { w } + p _ { y } q _ { z } - p _ { z } q _ { y } } \\ { p _ { w } q _ { y } - p _ { x } q _ { z } + p _ { y } q _ { w } + p _ { z } q _ { x } } \\ { p _ { w } q _ { z } + p _ { x } q _ { y } - p _ { y } q _ { x } + p _ { z } q _ { w } } \end{array} \right] = \left[ \begin{array} { c } { p _ { w } q _ { w } - \mathbf { p } _ { v } ^ { \top } \mathbf { q } _ { v } } \\ { p _ { w } \mathbf { q } _ { v } + q _ { w } \mathbf { p } _ { v } + \mathbf { p } _ { v } \times \mathbf { q } _ { v } } \end{array} \right] \tag{1}
p⊗q=⎣⎢⎢⎡pwqw−pxqx−pyqy−pzqzpwqx+pxqw+pyqz−pzqypwqy−pxqz+pyqw+pzqxpwqz+pxqy−pyqx+pzqw⎦⎥⎥⎤=[pwqw−pv⊤qvpwqv+qwpv+pv×qv](1)
可以看到乘积里面含有叉乘运算,叉乘不满足交换律,所以四元数的乘法不满足交换律.
进一步将四元数乘法转换成普通的矩阵与向量之间的乘法,可得:
q
1
⊗
q
2
=
[
q
1
]
L
q
2
,
q
1
⊗
q
2
=
[
q
2
]
R
q
1
\mathbf { q } _ { 1 } \otimes \mathbf { q } _ { 2 } = \left[ \mathbf { q } _ { 1 } \right] _ { L } \mathbf { q } _ { 2 } \quad , \quad \mathbf { q } _ { 1 } \otimes \mathbf { q } _ { 2 } = \left[ \mathbf { q } _ { 2 } \right] _ { R } \mathbf { q } _ { 1 }
q1⊗q2=[q1]Lq2,q1⊗q2=[q2]Rq1
[ q ] L = [ q w − q x − q y − q z q x q w − q z q y q y q z q w − q x q z − q y q x q w ] , [ q ] R = [ q w − q x − q y − q z q x q w q z − q y q y − q z q w q x q z q y − q x q w ] [ \mathbf { q } ] _ { L } = \left[ \begin{array} { c c c c } { q _ { w } } & { - q _ { x } } & { - q _ { y } } & { - q _ { z } } \\ { q _ { x } } & { q _ { w } } & { - q _ { z } } & { q _ { y } } \\ { q _ { y } } & { q _ { z } } & { q _ { w } } & { - q _ { x } } \\ { q _ { z } } & { - q _ { y } } & { q _ { x } } & { q _ { w } } \end{array} \right] , \quad [ \mathbf { q } ] _ { R } = \left[ \begin{array} { c c c c } { q _ { w } } & { - q _ { x } } & { - q _ { y } } & { - q _ { z } } \\ { q _ { x } } & { q _ { w } } & { q _ { z } } & { - q _ { y } } \\ { q _ { y } } & { - q _ { z } } & { q _ { w } } & { q _ { x } } \\ { q _ { z } } & { q _ { y } } & { - q _ { x } } & { q _ { w } } \end{array} \right] [q]L=⎣⎢⎢⎡qwqxqyqz−qxqwqz−qy−qy−qzqwqx−qzqy−qxqw⎦⎥⎥⎤,[q]R=⎣⎢⎢⎡qwqxqyqz−qxqw−qzqy−qyqzqw−qx−qz−qyqxqw⎦⎥⎥⎤
[
q
]
L
=
q
w
I
+
[
0
−
q
v
⊤
q
v
[
q
v
]
×
]
,
[
q
]
R
=
q
w
I
+
[
0
−
q
v
⊤
q
v
−
[
q
v
]
×
]
[ \mathbf { q } ] _ { L } = q _ { w } \mathbf { I } + \left[ \begin{array} { c c } { 0 } & { - \mathbf { q } _ { v } ^ { \top } } \\ { \mathbf { q } _ { v } } & { \left[ \mathbf { q } _ { v } \right] _ { \times } } \end{array} \right] , \quad [ \mathbf { q } ] _ { R } = q _ { w } \mathbf { I } + \left[ \begin{array} { c c } { 0 } & { - \mathbf { q } _ { v } ^ { \top } } \\ { \mathbf { q } _ { v } } & { - \left[ \mathbf { q } _ { v } \right] _ { \times } } \end{array} \right]
[q]L=qwI+[0qv−qv⊤[qv]×],[q]R=qwI+[0qv−qv⊤−[qv]×]
由下式
q
⊗
x
⊗
p
=
(
q
⊗
x
)
⊗
p
=
[
p
]
R
[
q
]
L
x
=
q
⊗
(
x
⊗
p
)
=
[
q
]
L
[
p
]
R
x
\begin{aligned} \mathbf { q } \otimes \mathbf { x } \otimes \mathbf { p } & = ( \mathbf { q } \otimes \mathbf { x } ) \otimes \mathbf { p } = [ \mathbf { p } ] _ { R } [ \mathbf { q } ] _ { L } \mathbf { x } \\ & = \mathbf { q } \otimes ( \mathbf { x } \otimes \mathbf { p } ) = [ \mathbf { q } ] _ { L } [ \mathbf { p } ] _ { R } \mathbf { x } \end{aligned}
q⊗x⊗p=(q⊗x)⊗p=[p]R[q]Lx=q⊗(x⊗p)=[q]L[p]Rx
可得
[
p
]
R
[
q
]
L
=
[
q
]
L
[
p
]
R
[ \mathbf { p } ] _ { R } [ \mathbf { q } ] _ { L } = [ \mathbf { q } ] _ { L } [ \mathbf { p } ] _ { R }
[p]R[q]L=[q]L[p]R
1.2 纯虚四元数的乘法
两个纯虚四元数相乘的结果不再是纯虚四元数了
p
v
⊗
q
v
=
−
p
v
⊤
q
v
+
p
v
×
q
v
=
[
−
p
v
⊤
q
v
p
v
×
q
v
]
\mathbf { p } _ { v } \otimes \mathbf { q } _ { v } = - \mathbf { p } _ { v } ^ { \top } \mathbf { q } _ { v } + \mathbf { p } _ { v } \times \mathbf { q } _ { v } = \left[ \begin{array} { c } { - \mathbf { p } _ { v } ^ { \top } \mathbf { q } _ { v } } \\ { \mathbf { p } _ { v } \times \mathbf { q } _ { v } } \end{array} \right]
pv⊗qv=−pv⊤qv+pv×qv=[−pv⊤qvpv×qv]
1.3 纯虚四元数的指数
纯虚四元数为 v = v x i + v y j + v z k = u θ = [ 0 u θ ] \mathbf v = v_xi + v_yj + v_zk= \mathbf { u } \theta = \left[ \begin{array} { l } { 0 } \\ { \mathbf { u } \theta } \end{array} \right] v=vxi+vyj+vzk=uθ=[0uθ],其中 θ = ∥ v ∥ ∈ R \theta = \| \mathbf { v } \| \in \mathbb { R } θ=∥v∥∈R, u \mathbf { u } u 为单位向量(后面会证明,这里的旋转其实是绕单位向量 u \mathbf { u } u 对目标物体进行角度 2 θ 2 \theta 2θ 的旋转,注意 θ \theta θ 前面的这个 2 2 2 系数).
利用泰勒展开,纯虚四元数 v \boldsymbol v v 的指数 e v e^{\boldsymbol v} ev 构成一个新的四元数:
e v = e u θ = cos θ + u sin θ = [ cos θ u sin θ ] e ^ { \mathrm { v } } = e ^ { \mathbf { u } \theta } = \cos \theta + \mathbf { u } \sin \theta = \left[ \begin{array} { c } { \cos \theta } \\ { \mathbf { u } \sin \theta } \end{array} \right] ev=euθ=cosθ+usinθ=[cosθusinθ]
上面形式不就是我们当初学复数时遇到的欧拉公式吗,
e
i
θ
=
cos
θ
+
i
sin
θ
e ^ { i \theta } = \cos \theta + i \sin \theta
eiθ=cosθ+isinθ
注意到,
∥
e
v
∥
2
=
cos
2
θ
+
sin
2
θ
=
1
\left\| e ^ { \mathrm { v } } \right\| ^ { 2 } = \cos ^ { 2 } \theta + \sin ^ { 2 } \theta = 1
∥ev∥2=cos2θ+sin2θ=1,即纯虚四元数的指数是一个单位四元数. 这也说明了使用四元数表示旋转为什么需要是单位四元数了吧.
关于这个纯虚四元数的指数映射含义为:从纯虚四元数空间映射到以单位四元数表示的旋转空间(an application from the space of pure quaternions to the space of rotations represented by unit quaternions).
1.4 单位四元数的对数
上面公式的逆运算也成立,即,如果四元数
q
\mathbf { q }
q 是单位四元数,那么它的对数将是一个纯虚四元数
log
q
=
log
(
cos
θ
+
u
sin
θ
)
=
log
(
e
u
θ
)
=
u
θ
=
[
0
u
θ
]
\log \mathbf { q } = \log ( \cos \theta + \mathbf { u } \sin \theta ) = \log \left( e ^ { \mathbf { u } \theta } \right) = \mathbf { u } \theta = \left[ \begin{array} { c } { 0 } \\ { \mathbf { u } \theta } \end{array} \right]
logq=log(cosθ+usinθ)=log(euθ)=uθ=[0uθ]
二、旋转矩阵和四元数导数
2.1 旋转矩阵的导数
因为
R
⊤
R
=
I
\mathbf { R } ^ { \top } \mathbf { R } = \mathbf I
R⊤R=I,所以其导数必然是0,即,
d
d
t
(
R
⊤
R
)
=
R
˙
⊤
R
+
R
⊤
R
˙
=
0
\frac { d } { d t } \left( \mathbf { R } ^ { \top } \mathbf { R } \right) = \dot { \mathbf { R } } ^ { \top } \mathbf { R } + \mathbf { R } ^ { \top } \dot { \mathbf { R } } = 0
dtd(R⊤R)=R˙⊤R+R⊤R˙=0
R
⊤
R
˙
=
−
(
R
⊤
R
˙
)
⊤
\mathbf { R } ^ { \top } \dot { \mathbf { R } } = - \left( \mathbf { R } ^ { \top } \dot { \mathbf { R } } \right) ^ { \top }
R⊤R˙=−(R⊤R˙)⊤
由此可知
R
⊤
R
\mathbf { R } ^ { \top } \mathbf { R }
R⊤R 是反对称矩阵(skew-symmetric
),
3
×
3
3 \times 3
3×3 维的反对称矩阵即是李代数
s
o
(
3
)
\mathfrak { s o } ( 3 )
so(3) (只有3DoF),使用
ω
∈
R
3
↔
[
ω
]
×
∈
s
o
(
3
)
\omega \in \mathbb { R } ^ { 3 } \leftrightarrow [ \omega ] _ {\times } \in \mathfrak { s o } ( 3 )
ω∈R3↔[ω]×∈so(3) 表示,当然因为
ω
\omega
ω 与
[
ω
]
×
[ \omega ] _ { \times }
[ω]× 只差了反对称符号,所以二者都可以表示为李代数,不做区分.
[
ω
]
×
≜
[
0
−
ω
z
ω
y
ω
z
0
−
ω
x
−
ω
y
ω
x
0
]
[ \omega ] _ { \times } \triangleq \left[ \begin{array} { c c c } { 0 } & { - \omega _ { z } } & { \omega _ { y } } \\ { \omega _ { z } } & { 0 } & { - \omega _ { x } } \\ { - \omega _ { y } } & { \omega _ { x } } & { 0 } \end{array} \right]
[ω]×≜⎣⎡0ωz−ωy−ωz0ωxωy−ωx0⎦⎤
而后就可以得到,旋转矩阵的导数形式:
R
˙
=
R
[
ω
]
×
\dot { \mathbf { R } } = \mathbf { R } [ \omega ] _ { \times }
R˙=R[ω]×
这里的
ω
=
(
ω
x
,
ω
y
,
ω
z
)
\omega =\left( \omega _ { x } , \omega _ { y } , \omega _ { z } \right)
ω=(ωx,ωy,ωz) 亦称为角速度,对应IMU测量得到的三个轴上的角速度(IMU坐标系). 既然旋转矩阵能写成上面这种形式,那么旋转矩阵一定是指数函数,所以假定
ω
\omega
ω 是常数,那么根据角度=速度*时间
,得到下面的时域积分形式:
R
(
t
)
=
R
(
0
)
e
[
ω
]
×
t
=
R
(
0
)
e
[
ω
t
]
×
\mathbf { R } ( t ) = \mathbf { R } ( 0 ) e ^ { [ \omega ] _ { \times } t } = \mathbf { R } ( 0 ) e ^ { [ \omega t ] _ { \times } }
R(t)=R(0)e[ω]×t=R(0)e[ωt]×
稍作变换,即得到旋转矩阵如下
R
=
R
(
0
)
⊤
R
(
t
)
=
e
[
ω
t
]
×
=
e
[
ϕ
]
×
\mathbf { R } = \mathbf { R } ( 0 ) ^ { \top } \mathbf { R } ( t ) =e ^ { [ \omega t ] _ { \times } } = e ^ { [ \phi ] _ { \times } }
R=R(0)⊤R(t)=e[ωt]×=e[ϕ]×
这就是我们熟悉的李代数到李群的转换:
exp
:
s
o
(
3
)
→
S
O
(
3
)
;
[
ϕ
]
×
↦
exp
(
[
ϕ
]
×
)
=
e
[
ϕ
]
×
\exp : \mathfrak { s o } ( 3 ) \rightarrow S O ( 3 ) ; [ \phi ] _ { \times } \mapsto \exp \left( [ \phi ] _ { \times } \right) = e ^ { [ \phi ] _ {\times} }
exp:so(3)→SO(3);[ϕ]×↦exp([ϕ]×)=e[ϕ]×
2.2 四元数的导数
因为
q
∗
⊗
q
=
1
\mathbf { q } ^ { * } \otimes \mathbf { q }=1
q∗⊗q=1,所以其导数也等于0,旋转矩阵求导也是这么做的. 如下:
d
(
q
∗
⊗
q
)
d
t
=
q
˙
∗
⊗
q
+
q
∗
⊗
q
˙
=
0
\frac { d \left( \mathbf { q } ^ { * } \otimes \mathbf { q } \right) } { d t } = \dot { \mathbf { q } } ^ { * } \otimes \mathbf { q } + \mathbf { q } ^ { * } \otimes \dot { \mathbf { q } } = 0
dtd(q∗⊗q)=q˙∗⊗q+q∗⊗q˙=0
q ∗ ⊗ q ˙ = − ( q ˙ ∗ ⊗ q ) = − ( q ∗ ⊗ q ˙ ) ∗ \mathbf { q } ^ { * } \otimes \dot { \mathbf { q } } = - \left( \dot { \mathbf { q } } ^ { * } \otimes \mathbf { q } \right) = - \left( \mathbf { q } ^ { * } \otimes \dot { \mathbf { q } } \right) ^ { * } q∗⊗q˙=−(q˙∗⊗q)=−(q∗⊗q˙)∗
由上式可以看出,
q
∗
⊗
q
˙
\mathbf { q } ^ { * } \otimes \dot { \mathbf { q } }
q∗⊗q˙ 是纯虚四元数,因为只有纯虚四元数的共轭的相反数等于其本身,所以,进一步将其写成:
q
∗
⊗
q
˙
=
Ω
=
[
0
Ω
]
∈
H
p
\mathbf { q } ^ { * } \otimes \dot { \mathbf { q } } = \mathbf { \Omega } = \left[ \begin{array} { l } { 0 } \\ { \mathbf { \Omega } } \end{array} \right] \in \mathbb { H } _ { p }
q∗⊗q˙=Ω=[0Ω]∈Hp
移项即得四元数的导数,但是这里的
Ω
\Omega
Ω 不再是角速度了,而是速度
ω
\omega
ω 的一半,即
Ω
=
ω
/
2
\Omega = \omega / 2
Ω=ω/2.
q
˙
=
q
⊗
Ω
=
1
2
q
⊗
ω
\dot { \mathbf { q } } = \mathbf { q } \otimes \Omega= \frac { 1 } { 2 } \mathbf { q } \otimes \mathbf { \omega }
q˙=q⊗Ω=21q⊗ω
还有另外一种求四元数导数的方法,有兴趣参考《Indirect Kalman Filter for 3D Attitude Estimation》中的公式87.
假定
Ω
\Omega
Ω 是常量,那么就可以得到下式:
q
(
t
)
=
q
(
0
)
⊗
e
Ω
t
\mathbf { q } ( t ) = \mathbf { q } ( 0 ) \otimes e ^ { \mathbf { \Omega } t }
q(t)=q(0)⊗eΩt
移项即可得到
q
=
e
V
=
e
Ω
t
=
e
ω
t
/
2
\mathbf { q } = e ^ { \mathbf { V } }=e ^ { \mathbf { \Omega } t }=e ^ { \mathbf { \omega } t / 2}
q=eV=eΩt=eωt/2
旋转向量的指数映射到四元数空间的操作如下:
Exp
:
R
3
→
S
3
;
ϕ
↦
Exp
(
ϕ
)
=
e
ϕ
/
2
\operatorname { Exp } : \mathbf { R } ^ { 3 } \rightarrow S ^ { 3 } ; \phi \mapsto \operatorname { Exp } ( \boldsymbol { \phi } ) = e ^ { \phi / 2 }
Exp:R3→S3;ϕ↦Exp(ϕ)=eϕ/2
三、证明 θ = ϕ / 2 \theta = \phi /2 θ=ϕ/2
四元数
q
\mathbf q
q 在4D空间与实单位四元数
q
1
=
[
1
,
0
,
0
,
0
]
\mathbf { q } _ { 1 } = [ 1,0,0,0 ]
q1=[1,0,0,0] 之间的旋转角度
θ
\theta
θ 等于其在3D空间对目标物体所做旋转角度
ϕ
\phi
ϕ 的一半,即
θ
=
ϕ
/
2
\theta = \phi /2
θ=ϕ/2.
下面的证明分为两部分进行:
1) 3D vector rotation formula
如上图,当绕轴
u
\mathbf { u }
u 对向量
x
\mathbf { x }
x 旋转
ϕ
\mathbf { \phi }
ϕ 角度后(
ξ
=
u
ϕ
\boldsymbol {\xi} = \mathbf { u }\mathbf {\phi}
ξ=uϕ 即为旋转向量,也是李代数
s
o
(
3
)
\mathfrak { s o } ( 3 )
so(3)),得到旋转后的向量
x
′
\mathbf {x}^{ \prime }
x′,如下:
x
′
=
x
∥
+
x
⊥
cos
ϕ
+
(
u
×
x
)
sin
ϕ
(3-1)
\mathbf { x } ^ { \prime } = \mathbf { x } _ { \| } + \mathbf { x } _ { \perp } \cos \phi + ( \mathbf { u } \times \mathbf { x } ) \sin \phi \tag{3-1}
x′=x∥+x⊥cosϕ+(u×x)sinϕ(3-1)
这个公式就叫做三维向量旋转公式(3D vector rotation formula
)
2) The rotation action
四元数
q
\mathbf { q }
q 对向量
x
\mathbf { x }
x 进行旋转得到
x
′
=
q
⊗
x
⊗
q
∗
\mathbf { x } ^ { \prime } = \mathbf { q } \otimes \mathbf { x } \otimes \mathbf { q } ^ { * }
x′=q⊗x⊗q∗,这种乘法叫做(the sandwich product
),因为将向量
x
\mathbf { x }
x 写进了四元数的乘法当中,所以也需要将
x
\mathbf { x }
x 转换到四元数空间(向量空间转换到四元数空间),即,
x
=
x
i
+
y
j
+
z
k
=
[
0
x
]
∈
H
p
(3-2)
\mathbf { x } = x i + y j + z k = \left[ \begin{array} { l } { 0 } \\ { \mathbf { x } } \end{array} \right] \in \mathbb { H } _ { p } \tag{3-2}
x=xi+yj+zk=[0x]∈Hp(3-2)
假设所作的旋转轴为
u
\mathbf u
u,旋转角度为
ϕ
\mathbf \phi
ϕ,则四元数
q
\mathbf { q }
q 可以先写成
q
=
Exp
(
u
ϕ
)
=
exp
(
u
ϕ
/
2
)
=
cos
ϕ
2
+
u
sin
ϕ
2
\mathbf { q } = \operatorname{Exp}(\mathbf { u } \mathbf { \phi } ) = \operatorname{exp}(\mathbf { u } \mathbf { \phi } /2) = \cos \frac { \phi } { 2 } + \mathbf { u } \sin \frac { \phi } { 2 }
q=Exp(uϕ)=exp(uϕ/2)=cos2ϕ+usin2ϕ,对旋转后的结果进行展开,如下:
x
′
=
q
⊗
x
⊗
q
∗
=
(
cos
ϕ
2
+
u
sin
ϕ
2
)
⊗
(
0
+
x
)
⊗
(
cos
ϕ
2
−
u
sin
ϕ
2
)
.
.
.
.
.
.
=
x
⊥
cos
ϕ
+
(
u
×
x
)
sin
ϕ
+
x
∥
(3-3)
\begin{aligned} \mathbf { x } ^ { \prime } & = \mathbf { q } \otimes \mathbf { x } \otimes \mathbf { q } ^ { * } \\ & = \left( \cos \frac { \phi } { 2 } + \mathbf { u } \sin \frac { \phi } { 2 } \right) \otimes ( 0 + \mathbf { x } ) \otimes \left( \cos \frac { \phi } { 2 } - \mathbf { u } \sin \frac { \phi } { 2 } \right) \\ & ...... \\ & = \mathbf { x } _ { \perp } \cos \phi + ( \mathbf { u } \times \mathbf { x } ) \sin \phi + \mathbf { x } _ { \| } \end{aligned} \tag{3-3}
x′=q⊗x⊗q∗=(cos2ϕ+usin2ϕ)⊗(0+x)⊗(cos2ϕ−usin2ϕ)......=x⊥cosϕ+(u×x)sinϕ+x∥(3-3)
对比公式(3-1),二者一模一样,这就证明了四元数对目标物体旋转了
ϕ
\phi
ϕ 角度,这个四元数在其4D空间相对实单位四元数旋转了
ϕ
/
2
\phi /2
ϕ/2 角度. 这时候祭出这张图再合适不过了.
四、计算 angle distance
1) 计算普通单位四元数与实单位四元数之间的角位移
因为实单位四元数的向量部分(即旋转向量的旋转轴)为0,所以其方向可以是任意方向,所以我们假定其与这个普通单位四元数旋转轴相同,那么它俩之间的角位移就是旋转向量对应的角度
ϕ
\phi
ϕ,那么计算一个普通单位四元数与实单位四元数之间的角位移:
Cannot read property 'type' of undefined
θ \theta θ 是这个四元数在其4D空间相对实单位四元数旋转角度,转换成对目标物体旋转的角度为 ϕ = 2 θ \phi =2 \theta ϕ=2θ,显然 ϕ \phi ϕ 就是我们要的angle distance,即旋转向量对应的角度
2) 计算两个普通单位四元数之间的角位移
为了计算
p
\mathbf { p }
p 与
q
\mathbf { q }
q 之间的角位移,一种直观的想法就是计算
p
⊗
q
−
1
\mathbf { p } \otimes \mathbf { q^{-1} }
p⊗q−1 与 实单位四元数
q
1
\mathbf q_1
q1 之间的角位移
p
⊗
q
−
1
=
[
p
w
q
w
+
p
v
⊤
q
v
−
p
w
q
v
+
q
w
p
v
−
p
v
×
q
v
]
\mathbf { p } \otimes \mathbf { q^{-1} } = \left[ \begin{array} { c } { p _ { w } q _ { w } + \mathbf { p } _ { v } ^ { \top } \mathbf { q } _ { v } } \\ { -p _ { w } \mathbf { q } _ { v } + q _ { w } \mathbf { p } _ { v } - \mathbf { p } _ { v } \times \mathbf { q } _ { v } } \end{array} \right]
p⊗q−1=[pwqw+pv⊤qv−pwqv+qwpv−pv×qv]
根据上面推导可知:
cos
θ
=
q
1
⊤
p
⊗
q
−
1
=
p
w
q
w
+
p
v
⊤
q
v
\cos \theta = \mathbf { q } _ { 1 } ^ { \top }\mathbf { p } \otimes \mathbf { q^{-1} }= p _ { w } q _ { w } + \mathbf { p } _ { v } ^ { \top } \mathbf { q } _ { v }
cosθ=q1⊤p⊗q−1=pwqw+pv⊤qv
而 p w q w + p v ⊤ q v p _ { w } q _ { w } + \mathbf { p } _ { v } ^ { \top } \mathbf { q } _ { v } pwqw+pv⊤qv 不就是 p ⊤ q \mathbf { p } ^ { \top } \mathbf { q } p⊤q 的结果嘛!
这部分的代码实现可以参考Eigen Geometry模块下的angularDistance()
函数
<完>
@leatherwang