对于用欧式距离不便衡量的距离,如两个方向的距离,我们需要定义一种Counterpart,以方便统计分析。我们假设读者对于李群(Lie group)和李代数(Lie algebra)有一定了解。读者也可以参考此文。本文的符号系统均尽可能按照参考文献1、2。
李群、李代数与旋转运动
这里仅简单总结下李群上的旋转变换等基础知识,更详细的内容请参考此文。
定义域为李群的旋转矩阵 R ∈ S O ( 3 ) R\in SO(3) R∈SO(3) 满足 R ⊤ R = I R^{\top}R = I R⊤R=I 且 det R = 1 \det R = 1 detR=1。旋转矩阵可用来表示一个方向、坐标系的变换以及一个旋转过程。下文中,我们用 R a b R_{ab} Rab 表示在坐标系 a a a下,坐标系 b b b的方向。
如下图,定义一个角速度为 ω = ω ^ θ ˙ \omega = \hat{\omega}\dot{\theta} ω=ω^θ˙ 其中 ω ^ \hat{\omega} ω^ 为单位长度的向量,此处即旋转轴, θ ˙ \dot{\theta} θ˙ 为单位时间的旋转角度。
那么,我们可以一次计算出空间中绕该轴以该角速度旋转的某一点的速度为 x ^ ˙ = ω × x ^ \dot{\hat{x}} = \omega \times \hat{x} x^˙=ω×x^ 到目前我们还未指定坐标系,下述内容中,我们定义固定坐标系 s s s和刚体(随动)坐标系 b b b。
首先讨论坐标系 s s s下的旋转问题。在该坐标系下,令 R ( t ) ≜ R s b ( t ) R(t) \triangleq R_{sb}(t) R(t)≜Rsb(t) 表示某一时刻刚体坐标系在固定坐标系下的方向,显然此时
R = [ x ^ , y ^ , z ^ ] R = [\hat{x}, \hat{y}, \hat{z}] R=[x^,y^,z^] 那么 R ˙ = [ x ^ ˙ , y ^ ˙ , z ^ ˙ ] = [ ω s × x ^ , ω s × y ^ , ω s × z ^ ] = ω s × R ≜ ω s ∧ R \dot{R} = [\dot{\hat{x}}, \dot{\hat{y}}, \dot{\hat{z}}] = [ \omega_{s} \times \hat{x}, \omega_{s} \times \hat{y}, \omega_{s} \times \hat{z}] = \omega_{s} \times R \triangleq \omega_{s}^{\wedge} R R˙=[x^˙,y^˙,z^˙]=[ωs×x^,ωs×y^,ωs×z^]=ωs×R≜ωs∧R 其中 ω s ∧ \omega_{s}^{\wedge} ωs∧ 是一个 3 × 3 3\times 3 3×3 的反对称(skew-symmetric)矩阵,即 ω s ∧ = − ( ω s ∧ ) ⊤ \omega_{s}^{\wedge} = -(\omega_{s}^{\wedge})^{\top} ωs∧=−(ωs∧)⊤, ω s ∧ ∈ s o ( 3 ) \omega_{s}^{\wedge} \in so(3) ωs∧∈so(3) 被称为李代数。
ω ∧ = [ 0 − ω 3 ω 2 ω 3 0 − ω 1 − ω 2 ω 1 0 ] \omega^{\wedge} = \begin{bmatrix} 0 & -\omega_{3} & \omega_{2} \\ \omega_{3} & 0 & -\omega_{1} \\ -\omega_{2} & \omega_{1} & 0 \end{bmatrix} ω∧=⎣⎡0ω3−ω2−ω30ω1ω2−ω10⎦⎤
ω s ∧ \omega_{s}^{\wedge} ωs∧ 有一个有用的特性是 R ω s ∧ R ⊤ = ( R ω s ) ∧ R\omega_{s}^{\wedge}R^{\top} = (R\omega_{s})^{\wedge} Rωs∧R⊤=(Rωs)∧
通过上述公式,我们易知 ω s ∧ = R ˙ R − 1 \omega_{s}^{\wedge} = \dot{R}R^{-1} ωs∧=R˙R−1
令 ω b \omega_{b} ωb 表示刚体坐标系下的角速度,下面我们讨论其与 ω s \omega_{s} ωs 的转换关系。定义固定坐标系下刚体坐标系的方向为 R s b R_{sb} Rsb 那么,
ω s = R s b ω b ω b = R s b − 1 ω s = R ⊤ ω s ω b ∧ = ( R ω s ) ∧ = R ⊤ ω s ∧ R = R ⊤ R ˙ R ⊤ R = R ⊤ R ˙ = R − 1 R ˙ \begin{aligned}\omega_{s} = R_{sb}\omega_{b} \\ \omega_{b} = R_{sb}^{-1}\omega_{s} = R^{\top}\omega_{s} \\ \omega_{b}^{\wedge} = (R\omega_{s})^{\wedge} = R^{\top}\omega_{s}^{\wedge}R = R^{\top}\dot{R}R^{\top}R = R^{\top}\dot{R} = R^{-1}\dot{R} \end{aligned} ωs=Rsbωbωb=Rsb−1ωs=R⊤ωsωb∧=(Rωs)∧=R⊤ωs∧R=R⊤R˙R⊤R=R⊤R˙=R−1R˙ 注意 ω b \omega_{b} ωb 并不是相对于运动的刚体坐标系的角速度,而是相对于假设静止的刚体坐标系的角速度。同时需要注意的是, ω s \omega_{s} ωs 与刚体坐标系的选取无关, ω b \omega_{b} ωb 与固定坐标系的选取无关。尽管 R R R 为刚体坐标系在固定坐标系下的表示。
任意坐标系 { d } \{d\} { d}下的角速度可以在另一个坐标系 { c } \{c\} { c}下表示,只要我们指导坐标系之间的方向变换关系: ω c = R c d ω d \omega_{c} = R_{cd}\omega_{d} ωc=Rcdωd
考虑上图,给定旋转轴 ω ^ \hat{\omega} ω^ ,以及一个点 p p p,现将其从 p ( 0 ) p(0) p(0) 位置运动到 p ( θ ) p(\theta) p(θ) 位置。假设旋转过程中速度为单位速度 1 r a d / s 1 rad/s 1rad/s,那么该运动路径上任意一个点为 p ( t ) , t ∈ [ 0 , θ ] p(t),t\in[0,\theta] p(t),t∈[0,θ]。此时,如上图,计算其任意时刻速度为: p ˙ = ω ^ × p = ω ^ ∧ p \dot{p} = \hat{\omega} \times p = \hat{\omega}^{\wedge}p p˙=ω^×p=ω^∧p 结合其初始条件 p ( 0 ) p(0) p(0),我们可知
p ( t ) = e ω ^ ∧ t p ( 0 ) p(t) = e^{\hat{\omega}^{\wedge}t}p(0) p(t)=eω^∧tp(0) 注意上述分析都是在单位速度假设下的。同样的,因为我们可以替换 t t t 和 θ \theta θ ,所以, p ( θ ) = e ω ^ ∧ θ p ( 0 ) p(\theta) = e^{\hat{\omega}^{\wedge}\theta}p(0) p(θ)=eω^∧θp(0)
若将 e ω ^ ∧ θ e^{\hat{\omega}^{\wedge}\theta} eω^∧θ 进行Taylor展开,我们还可以得到 R o t ( ω ^ , θ ) = e ω ^ ∧ θ