Arthals' ink

Back

四元数#

TIP

强烈推荐参考 Krasjet / Quaternion 以获得直观且详细的性质证明推导。

小小的吐槽:王老师上节课明明刚说四元数不重要不要求掌握,结果这节课花了绝大部分时间来推导 hhh。

定义#

四元数是复数的推广,表示为:

q=w+xi+yj+zkq = w + xi + yj + zk

其中:

  • ww 是实数部分;
  • x,y,zx, y, z 是虚数部分;

i,j,ki, j, k 是虚数单位,满足以下关系:

i2=j2=k2=ijk=1i^2 = j^2 = k^2 = ijk = -1

反交换性质:

ij=k=ji,jk=i=kj,ki=j=ikij = k = -ji, \quad jk = i = -kj, \quad ki = j = -ik

向量形式

q=(w,v),v=(x,y,z)q = (w, \bold{v}), \quad \bold{v} = (x, y, z)

运算性质#

乘法:对于两个四元数 q1=(w1,v1)q_1 = (w_1, \bold{v}_1)q2=(w2,v2)q_2 = (w_2, \bold{v}_2),其乘法定义为:

q1q2=(w1w2v1v2,w1v2+w2v1+v1×v2)=(w1w2v1v2,w1v2+w2v1+v1×v2)\begin{aligned} q_1 q_2 &= (w_1 w_2 - \bold{v}_1^{\top} \bold{v}_2, \, w_1 \bold{v}_2 + w_2 \bold{v}_1 + \bold{v}_1 \times \bold{v}_2) \\ &= (w_1 w_2 - \bold{v}_1 \cdot \bold{v}_2, \, w_1 \bold{v}_2 + w_2 \bold{v}_1 + \bold{v}_1 \times \bold{v}_2) \end{aligned}

这被称为 Graßmann 积。

注意:四元数的乘法 不可交换,即 q1q2q2q1q_1 q_2 \neq q_2 q_1

共轭

q=(w,v)q^* = (w, -\bold{v})

模长

q2=w2+vv=qq=qq\|q\|^2 = w^2 + \bold{v}^{\top} \bold{v} = qq^* = q^*q

q1=qq2q^{-1} = \frac{q^*}{\|q\|^2}

这是模长的直接推论。

几何意义与应用#

单位四元数:若四元数的模长为 11,即 q=1\|q\| = 1,则称其为单位四元数。单位四元数可表示三维空间中的旋转。其还具有性质 q1=qq^{-1} = q^*

纯四元数:若四元数的实部为 00,即 q=(0,v)q = (0, \bold{v}),则称其为纯四元数。纯四元数可以表示三维空间中的向量。

旋转表示:任何一个旋转,都可以表示为绕某个单位向量 ω^\hat{\omega} 旋转 θ\theta 角度。

那么,对应的四元数可以表示为:

q=[cosθ2,sinθ2ω^]q = \left[\cos\frac{\theta}{2}, \sin\frac{\theta}{2} \hat{\omega}\right]

注意,旋转到四元数存在“双重覆盖”关系,我们可以很容易地发现:

q=[cosθ2,sinθ2ω^]q=[cosθ2,sinθ2ω^]=[cos(πθ2),sin(πθ2)(ω^)]\begin{aligned} q &= \left[\cos\frac{\theta}{2}, \sin\frac{\theta}{2} \hat{\omega}\right] \\ -q &= \left[-\cos\frac{\theta}{2}, -\sin\frac{\theta}{2}\hat{\omega}\right] \\ &= \left[\cos(\pi - \frac{\theta}{2}), \sin(\pi - \frac{\theta}{2}) (-\hat{\omega})\right] \end{aligned}

是等价的(q-q 意味着同一旋转轴但是翻转正方向,然后旋转 2πθ2\pi - \theta)。

double_coverage

相应地,从四元数恢复轴角表示:

θ=2arccos(w),ω^={vsin(θ/2),θ00,θ=0\theta = 2 \arccos(w), \quad \hat{\omega} = \begin{cases} \frac{\bold{v}}{\sin(\theta/2)}, & \theta \neq 0 \\ 0, & \theta = 0 \end{cases}

其中,ω\omega 是单位四元数的实部,四元数的一种常见表示就是 (w,x,y,z)(w,x,y,z)

四元数运算#

向量旋转:任意向量 v\mathbf{v} 沿着以单位向量定义的旋转轴 u\mathbf{u} 旋转 θ\theta 度得到 v\mathbf{v}',那么:

令向量 v\mathbf{v} 的四元数形式 v=[0,v]v = [0, \mathbf{v}],旋转四元数 q=[cos(θ2),sin(θ2)u]q = \left[\cos\left(\frac{\theta}{2}\right), \sin\left(\frac{\theta}{2}\right)\mathbf{u}\right]

则旋转后的向量 v\mathbf{v}' 可表示为:

v=qvq=qvq1\mathbf{v}' = qv q^* = qv q^{-1}

如果是给定四元数 qq 旋转向量 v\mathbf{v} ,那么设 q=[w,r]q = [w, \mathbf{r}] 是单位四元数(即 w2+r2=1w^2 + \|\mathbf{r}\|^2 = 1),向量 v\mathbf{v} 的四元数形式为 v=[0,v]v = [0, \mathbf{v}]

则:

qvq=[w,r][0,v][w,r]=[rv,wv+r×v][w,r]=[0,(12r2)v+2(rv)r+2w(r×v)]\begin{aligned} qvq^* &= [w, \mathbf{r}][0, \mathbf{v}][w, -\mathbf{r}] \\ &= [ - \mathbf{r} \cdot \mathbf{v}, w\mathbf{v} + \mathbf{r} \times \mathbf{v} ][w, -\mathbf{r}] \\ &= [0, (1-2\|\mathbf{r}\|^2)\mathbf{v} + 2(\mathbf{r} \cdot \mathbf{v})\mathbf{r} + 2w(\mathbf{r} \times \mathbf{v})] \end{aligned}

最后一个等式的展开计算如下

实部:

=(rv)w(wv+r×v)(r)=w(rv)+w(vr)+(r×v)r=0\begin{aligned} &= (- \mathbf{r} \cdot \mathbf{v})w - (w\mathbf{v} + \mathbf{r} \times \mathbf{v}) \cdot (-\mathbf{r}) \\ &= -w (\mathbf{r} \cdot \mathbf{v}) + w (\mathbf{v} \cdot \mathbf{r}) + (\mathbf{r} \times \mathbf{v}) \cdot \mathbf{r} \\ &= 0 \quad \end{aligned}

虚部:

=(rv)(r)+w(wv+r×v)+(wv+r×v)×(r)=(rv)r+w2v+w(r×v)w(v×r)(r×v)×r=(rv)r+w2v+2w(r×v)[(rr)v(vr)r]=(12r2)v+2(rv)r+2w(r×v)\begin{aligned} &= (- \mathbf{r} \cdot \mathbf{v})(-\mathbf{r}) + w (w\mathbf{v} + \mathbf{r} \times \mathbf{v}) + (w\mathbf{v} + \mathbf{r} \times \mathbf{v}) \times (-\mathbf{r}) \\ &= (\mathbf{r} \cdot \mathbf{v})\mathbf{r} + w^2 \mathbf{v} + w (\mathbf{r} \times \mathbf{v}) - w (\mathbf{v} \times \mathbf{r}) - (\mathbf{r} \times \mathbf{v}) \times \mathbf{r} \\ &= (\mathbf{r} \cdot \mathbf{v})\mathbf{r} + w^2 \mathbf{v} + 2w (\mathbf{r} \times \mathbf{v}) - \big[ (\mathbf{r} \cdot \mathbf{r})\mathbf{v} - (\mathbf{v} \cdot \mathbf{r})\mathbf{r} \big] \\ &= (1 - 2\|\mathbf{r}\|^2)\mathbf{v} + 2(\mathbf{r} \cdot \mathbf{v})\mathbf{r} + 2w (\mathbf{r} \times \mathbf{v}) \end{aligned}

其中利用了叉乘展开式:

a×b×c=(ac)b(ab)ca \times b \times c = (a \cdot c)b - (a \cdot b)c

以及单位四元数约束条件 w2+r2=1w^2 + \|\mathbf{r}\|^2 = 1,将 w2=1r2w^2 = 1 - \|\mathbf{r}\|^2 代入后合并同类项。

旋转组合:两个旋转 q1q_1q2q_2 的组合等价于四元数的乘法:

q2(q1xq1)q2=(q2q1)x(q1q2) q_2 (q_1 x q_1^*) q_2^* = (q_2 q_1) x (q_1^* q_2^*)

虽然四元数不满足交换律,但其满足结合律(可以证明四元数存在对应的四维矩阵,所以矩阵的性质也是四元数的性质)。

注意:

  • 四元数的旋转表示具有 33 个自由度(四个参数加一个单位模长约束)。
  • 几何上,单位四元数可以看作 44 维球面 S3S^3 的壳。

四元数的距离#

这部分证明参见 Krasjet / Quaternion 第 4 节・四元数插值(第 37 页)。

在单位三维球面 S3S^3 上,或两个四元数 (q1,q2)(q_1, q_2) 之间的角度:

p,q=arccos(pq)\langle p, q \rangle = \arccos(p \cdot q)

对应旋转之间的距离:

dist(p,q)=2arccos(pq)\text{dist}(p, q) = 2 \arccos(|p \cdot q|)

或等价地:

dist(p,q)=2min{p,q,p,q}\text{dist}(p, q) = 2 \min \{\langle p, q \rangle, \langle p, -q \rangle\}

这里需要在两个值之间取最小值的原因也可以参见 Krasjet / Quaternion 第 5.4 节・双倍覆盖带来的问题(第 46 页)。

可以证明,两个旋转 (R1,R2)(R_1, R_2) 的距离与其对应四元数 q(R1)q(R_1)q(R2)q(R_2) 在球面上的距离成线性关系(前者是后者的两倍)。

unit_circle_and_rotation_diagram

四元数插值#

这部分证明参见 Krasjet / Quaternion 第 5 节・四元数插值(第 41 页)。

线性插值(Linear Interpolation, Lerp)#

q(t)=(1t)q1+tq2q(t) = (1-t)q_1 + tq_2

lerp

归一化线性插值(Normalized Linear Interpolation, Nlerp)#

q(t)=(1t)q1+tq2(1t)q1+tq2q(t) = \frac{(1-t)q_1 + tq_2}{\|(1-t)q_1 + tq_2\|}

nlerp

省流:就是除个模长,让他恢复为单位四元数。

球面线性插值(Spherical Linear Interpolation, Slerp)#

以上两种插值都有问题,他们实际上是线性切分了弦长,而不是弧长,这会导致在转动的时候的角速度不均匀:

nlerp

所以,我们要引入新的插值方式,这就是球面线性插值(Spherical Linear Interpolation, Slerp):

q(t)=sin((1t)θ)sin(θ)q1+sin(tθ)sin(θ)q2q(t) = \frac{\sin((1-t)\theta)}{\sin(\theta)} q_1 + \frac{\sin(t\theta)}{\sin(\theta)} q_2

其中 θ\thetaq1q_1q2q_2 之间的夹角,θ=arccos(q1q2)\theta = \arccos(q_1 \cdot q_2)

slerp

证明的一个方法在 Krasjet / Quaternion 第 5.3 节・球面线性插值(第 43 页)。

不过老师的 Slide 上有另一种更简单的利用三角函数性质的证明方法:

vector_geometry_angle_diagram

α+β=ψv(t)=w0v0+w1v1sinαw1=sinβw0=sin(πψ)1=sinψw0=sinβsinψw1=sinαsinψψ=cos1(v0v1)\begin{aligned} \alpha+\beta&=\psi\\ \mathbf{v}(t)&=w_0\mathbf{v}_0+w_1\mathbf{v}_1\\ \frac{\sin\alpha}{w_1}&=\frac{\sin\beta}{w_0}=\frac{\sin(\pi-\psi)}1=\sin\psi\\ w_{0}&=\frac{\sin\beta}{\sin\psi}\\ w_{1}&=\frac{\sin\alpha}{\sin\psi}\\ \psi&=\cos^{-1}(\mathbf{v}_0\cdot\mathbf{v}_1) \end{aligned}

第三个式子利用了三角形的性质:

Asinα=Bsinβ=Csinγ\frac{A}{\sin\alpha}=\frac{B}{\sin\beta}=\frac{C}{\sin\gamma}

球面均匀采样#

考虑我们如何随机采样一个旋转。

引理:在 SO(3)\mathbb{SO}(3) 中均匀采样旋转矩阵等价于从单位四元数的集合 S(3)\mathbb{S}(3) 中均匀采样。

原因:两个旋转之间的距离与对应的四元数在单位球面上的距离成线性关系。

那么,如何均匀采样 S(3)\mathbb{S}(3) 呢?

方法:从四维标准正态分布 N(0,I4×4)\mathcal{N}(0, I_{4 \times 4}) 中随机采样一个变量,并将其归一化,从而得到(直接解释为)单位四元数。

原因:由于标准正态分布是各向同性的(即在所有方向上均匀分布),所以采样得到的单位四元数在 S(3)\mathbb{S}(3) 中也是均匀分布的。

随后,采样得到的单位四元数也就可以转换为对应的旋转矩阵(如果需要)。

有趣的事实#

对于神经网络来讲,最好的旋转表示方法是 9 个数的旋转矩阵。因为其他的表示方法均可能出现对于输入的微小扰动,即一个小的旋转,出现一个跳变,而只有最初最冗余的 R3×3\mathbb{R}^{3\times3} 旋转矩阵保证其必然是连续的( 即连续性 ),而这对于神经网络是很好的性质。

运动规划#

形式化表述#

配置空间 (Configuration Space)#

定义:配置空间(Configuration spcae,C-space)是 Rn\mathbb{R}^n 的一个子集,包含系统的所有可能状态(即状态空间)。

  • CC:配置空间,表示所有可能状态的集合。
  • CfreeCC_{\text{free}} \subseteq C自由空间,包含所有有效状态(无碰撞)。
  • CobsCC_{\text{obs}} \subseteq C障碍空间,表示有障碍的无效状态。
  • CfreeCobs=CC_{\text{free}} \cup C_{\text{obs}} = C
  • CfreeCobs=C_{\text{free}} \cap C_{\text{obs}} = \varnothing

问题定义#

configuration_space_pathfinding

给定:

  • 自由空间 CfreeC_{\text{free}}
  • 起始状态 qstartCfreeq_{\text{start}} \in C_{\text{free}}
  • 目标状态 qgoalCfreeq_{\text{goal}} \in C_{\text{free}}

目标:计算一系列动作,使机器人从 qstartq_{\text{start}} 移动到 qgoalq_{\text{goal}}

注意,这里的符号 qq 不是四元数(quaternion)的意思,其是配置空间中的一个点,即状态。

例如,对于一个机械臂,其配置空间可能是 Rn\mathbb{R}^n,那么 qq 就是关节的角度组合之一 (θ1,θ2,,θn)(\theta_1, \theta_2, \dots, \theta_n)

挑战#

  1. 避免障碍物:确保路径始终在 CfreeC_{\text{free}} 内。
  2. 长规划时间:路径可能较长,需要优化。
  3. 高维空间:配置空间维度可能很高(例如多关节机器人)。
机器人学 II
https://arthals.ink/blog/robotics-ii
Author Arthals
Published at March 5, 2025
What do you think?
  • Like
    0
    Like
Comments
  • Latest
  • Oldest
  • Hottest
Powered by Waline v3.5.1