四元数求导

10 Nov 2020

目前看到的最好的文档

https://krasjet.github.io/quaternion/quaternion.pdf

四元数具有一个实部和三个虚部，常用一个标量和一个向量来表示：

\[q=q_{0}+q_{1} i+q_{2} j+q_{3} k=[s, v]^{T}, \quad s=q_{0} \in R, \quad v=\left[q_{1}, q_{2}, q_{3}\right]^{T} \in R^{3}\]

四元数的乘法有：

\[p \otimes q=\left[\begin{array}{c} s_{p} s_{q}-v_{p}^{T} v_{q} \\ s_{p} v_{q}+s_{q} v_{p}+v_{p} \times v_{q} \end{array}\right]=[p]_{L} q=[q]_{R} p\]

其中：

\[[p]_{L}=\left[\begin{array}{cc} s_{p} & -v_{p}^{T} \\ v_{p} & s_{p} I+v_{p}^{\wedge} \end{array}\right]=s_{p} I+\left[\begin{array}{cc} 0 & -v_{p}^{T} \\ v_{p} & v_{p}^{\wedge} \end{array}\right] \\ \quad[p]_{R}=\left[\begin{array}{cc} s_{q} & -v_{q}^{T} \\ v_{q} & s_{q} I-v_{q}^{\wedge} \end{array}\right]=s_{q} I+\left[\begin{array}{cc} 0 & -v_{q}^{T} \\ v_{q} & -v_{q}^{\wedge} \end{array}\right]\]

注意如果写成虚部在前实部在后的四元数形式，左乘和右乘矩阵的符号有所变化。

同时有性质:

\[[p]_{L}[q]_{R}=[q]_{R}[p]_{L}\]

单位四元数有：

\[q^T q = 1\]

同时，为了用四元数表示三维空间中点的旋转，首先将三维空间中的点表示为一个虚四元数

\[p=[0, x, y, z]^{T}=[0, v]^{T}\]

用四元数表示旋转：

\[q=\left[\cos \frac{\theta}{2}, n \sin \frac{\theta}{2}\right], \quad p^{\prime}=q p q^{-1}\]

结果仍为纯虚四元数，虚部表示旋转后的三维坐标。

对于用任何方式表示旋转时，对一个旋转再进行旋转有左乘和右乘两种形式，分别代表不同的意义。

首先定义四元数的导数有

\[\dot{q} \triangleq \lim _{\Delta t \rightarrow 0} \frac{q(t+\Delta t)-q(t)}{\Delta t}=\lim _{\Delta \theta \rightarrow 0} \frac{q \otimes \Delta q_{L}-q}{\Delta \theta}=\lim _{\Delta \theta \rightarrow 0} \frac{\Delta q_{G} \otimes q-q}{\Delta \theta}\]

其中 △q_L(Local) 表示局部坐标系的角度扰动，而 △q_G(Global) 表示全局坐标系的角度扰动，我们实际测量的角速度往往是在物体坐标系下的角速度 w（例如VIO中通过 IMU 获得），故常采用四元数右乘形式。

设初始旋转为 q = [s, v]^T 发生了(单位)轴角为 w \theta 的旋转（对应四元数 △q），那么 △q 可表示为

\[\Delta \mathbf{q}=\left[\cos \frac{\theta}{2}, \boldsymbol{\omega} \sin \frac{\theta}{2}\right]^{\mathrm{T}}\]

那么有

\[\begin{aligned} \lim _{\theta \rightarrow 0} \frac{\mathbf{q} \otimes \Delta \mathbf{q}-\mathbf{q}}{\theta} &=\lim _{\theta \rightarrow 0} \frac{\left[s \cos \frac{\theta}{2}-\mathbf{v}^{T} \boldsymbol{\omega} \sin \frac{\theta}{2}, s \boldsymbol{\omega} \sin \frac{\theta}{2}+\cos \frac{\theta}{2} \mathbf{v}+\mathbf{v} \times \boldsymbol{\omega} \sin \frac{\theta}{2}\right]^{\mathrm{T}}-\mathbf{q}}{\theta} \\ &=\lim _{\theta \rightarrow 0} \frac{\left[s\left(\cos \frac{\theta}{2}-1\right)-\mathbf{v}^{T} \boldsymbol{\omega} \sin \frac{\theta}{2}, s \boldsymbol{\omega} \sin \frac{\theta}{2}+\left(\cos \frac{\theta}{2}-1\right) \mathbf{v}+\mathbf{v} \times \boldsymbol{\omega} \sin \frac{\theta}{2}\right]^{\mathrm{T}}}{\theta} \\ &=\left[-\frac{1}{2} \mathbf{v}^{T} \boldsymbol{\omega}, \frac{1}{2} s \boldsymbol{\omega}+\frac{1}{2} \mathbf{v} \times \boldsymbol{\omega}\right]^{\mathrm{T}} \\ &=\mathbf{q} \otimes\left[0, \frac{1}{2} \boldsymbol{\omega}\right]^{\mathrm{T}} \\ &=\frac{1}{2} \mathbf{q} \otimes \left[0, \boldsymbol{\omega}\right]^{\mathrm{T}} \end{aligned}\]

因此，若局部的体坐标系测出的角速度为 w_L， 那么 q 的导数为

\[\begin{aligned} \dot{q} &= \frac{1}{2} \mathbf{q} \otimes \left[0, \boldsymbol{\omega_{L}}\right]^{\mathrm{T}} \\ &= \frac{1}{2} \Omega\left(w_{L}\right) q \end{aligned}\]

其中

\[\Omega(w_L) \triangleq[w_L]_{R}=\left[\begin{array}{ll} 0 & -w_{L}^{T} \\ w_{L} & -w_{L}^{\wedge} \end{array}\right]\]

上述关于四元数的求导内容来自手写 VIO 教程，实际上我很怀疑其中的数学推导，在求导时，w 到底表示的旋转角围绕的单位轴，还是角速度？（即单位轴乘上一个表示角速度大小的标量），查找资料后，换了一种推导方式：

以无人机的姿态为例，定义单位四元数 q(t) 来表示从无人机的地理系 E 到机体系 B 的旋转关系，在 t + △t 时刻，旋转可以表示为 q(t+△t)，即在 △t 过程中，机体坐标系又经过了一个微小旋转，这个微小旋转的瞬时旋转角速度为 w，接着为了得到瞬间旋转轴，做单位化处理:

\[\hat{w}=w /\|w\|\]

在 △t 时刻转过的角度大小为

\[\Delta \theta=\Delta t\|w\|\]

那么此次微小的旋转可由下面的四元数表示

\[\Delta q= \left[ \cos \frac{\Delta \theta}{2}, \sin \frac{\Delta \theta}{2} \hat{w}\right]=\left[\cos \frac{\|w\| \Delta t}{2}, \sin \frac{\|w\| \Delta t}{2} \hat{w} \right]\]

所以

\[\begin{aligned} q(t+△t) &= q \otimes \Delta q \\ &= \left[ s \cos \frac{\|w\| \Delta t}{2} - \sin \frac{\|w\| \Delta t}{2} v^{T} \hat{w}, s \sin \frac{\|w\| \Delta t}{2} \hat{w} + \cos \frac{\|w\| \Delta t}{2} v + \sin \frac{\|w\| \Delta t}{2} v × \hat{w} \right] \end{aligned}\]

那么有

\[q \otimes \Delta q - q = \\ \left[ \left(\cos \frac{\|w\| \Delta t}{2} - 1\right)s - \sin \frac{\|w\| \Delta t}{2} v^{T} \hat{w}, s \sin \frac{\|w\| \Delta t}{2} \hat{w} + \left(\cos \frac{\|w\| \Delta t}{2} - 1\right) v + \sin \frac{\|w\| \Delta t}{2} v × \hat{w} \right]\]

所以

\[\begin{aligned} \lim _{\Delta \theta \rightarrow 0} \frac{q \otimes \Delta q - q}{\Delta \theta} &= \lim_{\Delta t \rightarrow 0} \frac{\left[ \left(\cos \frac{\|w\| \Delta t}{2} - 1\right)s - \sin \frac{\|w\| \Delta t}{2} v^{T} \hat{w}, s \sin \frac{\|w\| \Delta t}{2} \hat{w} + \left(\cos \frac{\|w\| \Delta t}{2} - 1\right) v + \sin \frac{\|w\| \Delta t}{2} v × \hat{w} \right]}{\Delta t} \\ &= \left[-\frac{1}{2} v^{T} \|w\| \hat{w}, \frac{1}{2} s \|w\|\hat{w}+\frac{1}{2} v \times \|w\| \hat{w}\right]^{\mathrm{T}} \\ &= q \otimes [0, \frac{1}{2}w]^{\mathrm{T}} \\ &= \frac{1}{2} q \otimes [0, w]^{\mathrm{T}} \end{aligned}\]

其中 w 表示角速度，即单位轴乘上一个表示角速度大小的标量。