DEMO
Created 2021.06.05 by Cong Yu; Last modified: 2021.06.05-v1.0.2
Contact: windmillyucong@163.com
Copyleft! 2022 Cong Yu. Some rights reserved.
Quaternion
refitem: https://stanford.edu/class/ee267/lectures/lecture10.pdf
约定
- robot坐标系 front x, left y, up z
- 欧拉角顺序 zyx ypr yaw pitch roll
- $\Psi$ 方向或偏航 (heading or yaw)
- $\theta$ 升降或俯仰 (elevation or pitch)
- $\phi$ 倾斜或横滚 (bank or roll)
- n系 导航坐标系
- b系 机体坐标系
1. 基本概念
基本形式
\[q = q_w + i q_x + j q_y + k q_z\]where:
- $q_w$是实部
- $q_x,q_y,q_z$是虚部
- i,j,k有如下特性:
单位四元数
用于三维刚体运动时,必须使用单位四元数,约束其模长为1
\(\|q\| = \sqrt {q_w^2 + q_x^2+ q_y^2+ q_z^2} = 1\)
纯四元数
纯四元数 , 有 $q_w = 0$
2. 基本运算
Quaternion Algebra
四元数代数 (sync/0.books/Robotics/Course on SLAM-excerpt-Quaternion algebra.pdf)
refitem: https://stanford.edu/class/ee267/lectures/lecture10.pdf p20
1. 四元数乘法
四元数乘法是四元数运算中最基本的运算之一。对于两个四元数 $q_1 = [w_1, \mathbf{v}_1]$ 和 $q_2 = [w_2, \mathbf{v}_2]$,其乘法定义为:
\[q_1 \otimes q_2 = [w_1w_2 - \mathbf{v}_1 \cdot \mathbf{v}_2, w_1\mathbf{v}_2 + w_2\mathbf{v}_1 + \mathbf{v}_1 \times \mathbf{v}_2]\]其中:
- $\cdot$ 表示向量点积
- $\times$ 表示向量叉积
分量形式
对于四元数 $q_1 = w_1 + ix_1 + jy_1 + kz_1$ 和 $q_2 = w_2 + ix_2 + jy_2 + kz_2$,其乘法结果可以表示为:
\[\begin{aligned} q_1 \otimes q_2 = &(w_1w_2 - x_1x_2 - y_1y_2 - z_1z_2) \\ &+ i(w_1x_2 + x_1w_2 + y_1z_2 - z_1y_2) \\ &+ j(w_1y_2 - x_1z_2 + y_1w_2 + z_1x_2) \\ &+ k(w_1z_2 + x_1y_2 - y_1x_2 + z_1w_2) \end{aligned}\]矩阵形式
四元数乘法也可以用矩阵形式表示。对于四元数 $q = [w, x, y, z]$,定义左乘矩阵 $L(q)$ 和右乘矩阵 $R(q)$:
\[L(q) = \begin{bmatrix} w & -x & -y & -z \\ x & w & -z & y \\ y & z & w & -x \\ z & -y & x & w \end{bmatrix}, \quad R(q) = \begin{bmatrix} w & -x & -y & -z \\ x & w & z & -y \\ y & -z & w & x \\ z & y & -x & w \end{bmatrix}\]则四元数乘法可以表示为:
\[q_1 \otimes q_2 = L(q_1)q_2 = R(q_2)q_1\]重要性质
- 不满足交换律:$q_1 \otimes q_2 \neq q_2 \otimes q_1$
- 满足结合律:$(q_1 \otimes q_2) \otimes q_3 = q_1 \otimes (q_2 \otimes q_3)$
- 满足分配律:$q_1 \otimes (q_2 + q_3) = q_1 \otimes q_2 + q_1 \otimes q_3$
- $[1, 0, 0, 0]$ 是乘法的单位元
- 对于单位四元数,其逆等于其共轭:$q^{-1} = q^*$
- 对于旋转四元数,必须保持单位长度:$|q| = \sqrt{w^2 + x^2 + y^2 + z^2} = 1$
几何意义
四元数乘法在旋转中的应用:
- 连续旋转可以通过四元数乘法组合
- 对于点 $p$ 的旋转:$p_1 = q \otimes p_0 \otimes q^{-1}$
- 两个旋转的组合:$q_{total} = q_2 \otimes q_1$
注意说明:后文的公式中如果没有特意说明, $q_1q_2$ 和 $q_1 \cdot q_2$ 等这些形式的写法都指的是四元数乘法 $q_1 \otimes q_2$
2. 四元数求逆
对于四元数 $q = q_w + i q_x + j q_y + k q_z$,其逆定义为:
\[q^{-1} = \frac {q_w - i q_x - j q_y - k q_z} {\|q\|²}\]其中 $|q|² = q_w² + q_x² + q_y² + q_z²$ 是四元数的平方范数。
对于单位四元数($|q| = 1$),逆运算简化为:$q⁻¹ = q_w - i q_x - j q_y - k q_z$
重要性质:
- 对于单位四元数,$q^{-1} = q^*$(共轭)
- $qq⁻¹ = q⁻¹q = \mathbf 1$(其中$\mathbf 1$是单位四元数$[1,0,0,0]$)
- $(q_1q_2)⁻¹ = q_2^{-1}q_1^{-1}$(由于非交换性,顺序很重要)
- 对于纯四元数,有$q_w = 0$,逆就是取负值
- 对于表示旋转的单位四元数,逆表示相反的旋转
3. 表达姿态与旋转
必须使用单位四元数
表达姿态
- 四元数表达姿态不是唯一的。对于同一个旋转/姿态,存在两个四元数表示:q 和 -q。
- 轴角转单位四元数:
axis-angle to quaternion (need normalized axis $v$) \(\begin{align} q_w &= cos(\theta/2) \\ q_x &= v_x sin(\theta/2) \\ q_y &= v_y sin(\theta/2) \\ q_z &= v_z sin(\theta/2) \\ q &= q_w + i q_x + j q_y + k q_z \end{align}\)
\[q = [\cos(\theta / 2), \mathbf v \sin(\theta/2)]\]姿态间的旋转
姿态q1到q2的旋转动作有
\[\begin{align} \\q_2 &= q_1^2 q_1 \\q_1^2 &= q_2 q_1^{-1} \end{align}\]点的旋转
运算时,需要将三维点$p = [p_x,p_y,p_z]^T$写为纯四元数形式$p := [0, p_x,p_y,p_z]^T$
对于一个三维点 $p_0$,有一单位四元数表达的旋转操作$q$,作用于$p_0$的结果 \(p_t = q p_0 q^{-1}\)
逆旋转
旋转的逆旋转:四元数的逆表示相反的旋转动作
\[\begin{gather} qq^{-1} = q^{-1}q = \mathbf 1, \\ \mathbf 1 = [1,0,0,0] \end{gather}\]点的逆旋转
\[\begin{align} \\p_t = q p_0 q^{-1} \\p_0 = q^{-1}p_tq \end{align}\]多次旋转
旋转叠加:左乘,先做q_1旋转再做q_2旋转,有总旋转
\[q_a = q_2q_1\]点的旋转叠加
\[p_t = q_2q_1p_0q_1^{-1}q_2^{-1} = q_a p_0 q_a^{-1}\]4. 四元数微分
四元数微分描述了四元数随时间的变化率。对于一个单位四元数 $q(t)$,其微分 $\dot{q}(t)$ 表示四元数在时间 $t$ 处的瞬时变化率。
4.1 从轴角表示推导
考虑一个旋转四元数 $q(t)$,它可以表示为轴角形式:
\[q(t) = \begin{bmatrix} \cos\frac{\theta(t)}{2} \\ \mathbf{u}(t)\sin\frac{\theta(t)}{2} \end{bmatrix}\]其中 $\mathbf{u}(t)$ 是单位旋转轴,$\theta(t)$ 是旋转角度。
当时间变化一个微小量 $\Delta t$ 时,四元数变为:
\[q(t + \Delta t) = \begin{bmatrix} \cos\frac{\theta(t + \Delta t)}{2} \\ \mathbf{u}(t + \Delta t)\sin\frac{\theta(t + \Delta t)}{2} \end{bmatrix}\]4.2 角速度与四元数微分的关系
定义角速度 $\boldsymbol{\omega}(t)$ 为:
\[\boldsymbol{\omega}(t) = \lim_{\Delta t \to 0} \frac{\Delta \theta}{\Delta t} \mathbf{u}(t)\]其中 $\Delta \theta$ 是在 $\Delta t$ 时间内的旋转角度。
- 角度增量:$\Delta \theta = |\boldsymbol{\omega}|\Delta t$
- 旋转轴保持不变近似:$\mathbf{u}(t + \Delta t) \approx \mathbf{u}(t)$
- 旋转轴与角速度的关系:$\omega = |\omega| \mathbf u(t)$
4.3 四元数微分的推导
四元数的微分可以表示为:
\[\begin{equation} \begin{split} \dot{q}(t) &= \lim_{\Delta t \to 0} \frac{q(t + \Delta t) - q(t)}{\Delta t} \end{split} \end{equation}\]让我们逐步推导:
- 首先,对于小角度旋转,我们有以下近似:
- 小角度近似
- $\cos\frac{\Delta \theta}{2} \approx 1$
- $\sin\frac{\Delta \theta}{2} \approx \frac{\Delta \theta}{2}$
- 旋转轴不变近似
- $\mathbf{u}(t + \Delta t) \approx \mathbf{u}(t)$
- 小角度近似
-
应用旋转轴不变近似,将 $q(t + \Delta t)$ 展开: \(\begin{split} q(t + \Delta t) &\approx \begin{bmatrix} \cos\frac{\theta(t) + \Delta \theta}{2} \\ \mathbf{u}(t)\sin\frac{\theta(t) + \Delta \theta}{2} \end{bmatrix} \\ &= \begin{bmatrix} \cos\frac{\theta(t)}{2}\cos\frac{\Delta \theta}{2} - \sin\frac{\theta(t)}{2}\sin\frac{\Delta \theta}{2} \\ \mathbf{u}(t)(\sin\frac{\theta(t)}{2}\cos\frac{\Delta \theta}{2} + \cos\frac{\theta(t)}{2}\sin\frac{\Delta \theta}{2}) \end{bmatrix} \end{split}\)
-
应用小角度近似: \(\begin{split} q(t + \Delta t) &\approx \begin{bmatrix} \cos\frac{\theta(t)}{2} - \sin\frac{\theta(t)}{2}\frac{\Delta \theta}{2} \\ \mathbf{u}(t)(\sin\frac{\theta(t)}{2} + \cos\frac{\theta(t)}{2}\frac{\Delta \theta}{2}) \end{bmatrix} \\ &= q(t) + \frac{\Delta \theta}{2} \begin{bmatrix} -\sin\frac{\theta(t)}{2} \\ \mathbf{u}(t)\cos\frac{\theta(t)}{2} \end{bmatrix} \end{split}\)
-
代入微分定义: \(\begin{split} \dot{q}(t) &\equiv \lim_{\Delta t \to 0} \frac{q(t + \Delta t) - q(t)}{\Delta t} \\ &= \lim_{\Delta t \to 0} \frac{1}{\Delta t} \frac{\Delta \theta}{2} \begin{bmatrix} -\sin\frac{\theta(t)}{2} \\ \mathbf{u}(t)\cos\frac{\theta(t)}{2} \end{bmatrix} \\ &= \frac{\|\boldsymbol{\omega}\|}{2} \begin{bmatrix} -\sin\frac{\theta(t)}{2} \\ \mathbf{u}(t)\cos\frac{\theta(t)}{2} \end{bmatrix} \end{split}\)
-
注意到 $\boldsymbol{\omega} = |\boldsymbol{\omega}|\mathbf{u}(t)$,我们可以将上式重写为: \(\dot{q}(t) = \frac{1}{2} \begin{bmatrix} 0 \\ \boldsymbol{\omega}(t) \end{bmatrix} \begin{bmatrix} \cos\frac{\theta(t)}{2} \\ \mathbf{u}(t)\sin\frac{\theta(t)}{2} \end{bmatrix}\)
- 这正好是四元数乘法的形式,其中:
- $\begin{bmatrix} 0 \ \boldsymbol{\omega}(t) \end{bmatrix}$ 是角速度的纯四元数表示
- $\begin{bmatrix} \cos\frac{\theta(t)}{2} \ \mathbf{u}(t)\sin\frac{\theta(t)}{2} \end{bmatrix}$ 就是 $q(t)$
-
因此,最终得到四元数微分方程: \(\dot{q}(t) = \frac{1}{2} \begin{bmatrix} 0 \\ \boldsymbol{\omega}(t) \end{bmatrix} \otimes q(t)\)
- 注意:这里的角速度 $\boldsymbol{\omega}(t)$ 是在全局坐标系(世界坐标系)下表示的。在实际应用中,我们通常通过IMU等传感器获得的是物体坐标系下的角速度 $\boldsymbol{\omega}_b$。两者之间的关系为: \(\boldsymbol{\omega}_b = q^* \otimes \boldsymbol{\omega} \otimes q\) 或 \(\boldsymbol{\omega} = q \otimes \boldsymbol{\omega}_b \otimes q^*\)
因此,在实际应用中,我们通常使用物体坐标系下的角速度,此时四元数微分方程变为: \(\dot{q}(t) = \frac{1}{2} q(t) \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega}_b(t) \end{bmatrix}\)
4.4 矩阵形式表示
为了便于计算,可以将四元数微分表示为矩阵形式:
\[\dot{q}(t) = \frac{1}{2} \Omega(\boldsymbol{\omega}(t)) q(t)\]其中 $\Omega(\boldsymbol{\omega})$ 是一个 4×4 矩阵:
\[\Omega(\boldsymbol{\omega}) = \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix}\]4.5 推导思路2
refitem:
- Quaternion kinematics for the error-state Kalman filter https://arxiv.org/pdf/1711.02508.pdf p45 经典教材
- https://blog.csdn.net/qq_39554681/article/details/88909564
- https://blog.csdn.net/u013236946/article/details/72831380
- http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf
- https://blog.csdn.net/weixin_37835423/article/details/109452148
- https://zhuanlan.zhihu.com/p/254888810
- https://stanford.edu/class/ee267/lectures/lecture10.pdf p26
有
\(\begin{align} &q(t): &t时刻姿态四元数 \\ &q(t+\Delta t): &t+\Delta时刻姿态四元数 \\ &w: &瞬时角速度(global \ frame) \end{align}\) \(q_\Delta = q(\Delta t \|\omega\|, \frac{\omega}{\|\omega\|})\) 有 \(q(t+\Delta t) = q_\Delta q(t) \tag2\) 所以 \(\begin{equation} \begin{split} \dot{q}(t) =&\lim_{\Delta{t} \to 0}\frac{q(t+\Delta{t})-q(t)}{\Delta{t}}\\ =&\hat{\boldsymbol\omega}\lim_{\Delta{t} \to 0}\frac{\sin(\Vert{\boldsymbol\omega}\Vert\Delta{t}/2)}{\Delta{t}}q(t)\\ =&\hat{\boldsymbol\omega}\frac{d}{dt}\sin(\frac{\Vert{\boldsymbol\omega}\Vert t}{2})|_{t=0}q(t)\\ =&\hat{\boldsymbol\omega}\frac{\Vert{\boldsymbol\omega}\Vert}{2}q(t)\\ =&\frac{1}{2}\boldsymbol\omega(t)q(t) \end{split} \end{equation}\)
在实际中,能得到的角速度其实是相对于物体坐标系下的角速度 $ω’$(例如通过IMU获得)。世界坐标系下的角速度 ω 不动,物体坐标系旋转,则角速度 ω 在物体坐标系下的表示为 $ω′=q^ωq$。 将 代入 $ω=qω’q^$ (14),得到: \(\dot{q}=\frac{1}{2}{q}\boldsymbol\omega'\)
若 $q’$ 已知,(14)两边同时右乘 q∗,可以求得角速度:
\[ω =2 \dot q q^*\]根据(14),四元数的二阶导为: \(\begin{equation} \begin{split} \ddot{q}=&\frac{1}{2}(\dot{\boldsymbol\omega} q+\boldsymbol\omega\dot{q})\\ =&\frac{1}{2}\dot{\boldsymbol\omega} q + \frac{1}{4}\boldsymbol\omega\boldsymbol\omega{q}\\ =&(-\frac{1}{4}\Vert{\boldsymbol\omega}\Vert^2+\frac{1}{2}\dot{\boldsymbol\omega})q \end{split} \end{equation}\)
当 qq 的一阶导和二阶导都是已知时,由上式两边同时右乘 q∗q∗ 可以求出角加速度: \(\begin{equation} \begin{split} \dot{\boldsymbol\omega}=&2\ddot{q}q^*-\boldsymbol\omega\dot{q}q^*\\ =&2\ddot{q}q^*-2\dot{q}q^*\dot{q}q^*\\ =&2(\ddot{q}q^*-(\dot{q}q^*)^2) \end{split} \end{equation}\)
微分方程(14)可以用数值积分求解。令 hh 为时间步长, qk*qk 和 **ωkωk 是 *k**hkh 时刻的四元数和角速度。则在下一时刻的四元数近似为: \(q_{k+1}=q_k+\frac{1}{2}h\boldsymbol\omega_k{q_k}\)
由于增量计算, q**k+1qk+1不一定是单位四元数,所以需要规范化为单位四元数: \(q_{k+1}\leftarrow\frac{q_{k+1}}{\Vert{q_{k+1}}\Vert}\)
4.6 推导思路3
- VIO笔记Learning_vio L1 p27
当旋转一段微小的时间,即$\theta \rightarrow 0$ 趋于0时,可以得到
\(\Delta q = \begin{bmatrix} \text{cos} \frac {\delta \theta} 2 \\ \mathbf{u} \text{sin}\frac{\delta\theta}2 \end{bmatrix} \approx \begin{bmatrix} 1 \\ \mathbf{u} \frac{\delta\theta}2 \end{bmatrix} = \begin{bmatrix} 1 \\ \frac 1 2 \mathbf {\delta\theta} \end{bmatrix}\) 其中 $\mathbf{\delta\theta}$ 的方向为旋转轴,模长为旋转角度 ????最后一个等号???以及这句话是怎么表达出来的???
对时间求导
角速度 \(\omega = \lim_{\Delta{t} \to 0} \frac {\delta \theta}{ \Delta t}\)
四元数微分
5. 四元数表达下的陀螺仪积分
Gyro Integration with Quaternions
在实际应用中,我们通常使用数值积分方法来求解四元数微分方程。最常用的是四阶龙格-库塔法(RK4):
具体数理详见 2022-05-05-Runge-Kutta
\[\begin{aligned} k_1 &= \frac{1}{2}q_k \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega}_{b,k} \end{bmatrix} \\ k_2 &= \frac{1}{2}(q_k + \frac{h}{2}k_1) \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega}_{b,k+\frac{1}{2}} \end{bmatrix} \\ k_3 &= \frac{1}{2}(q_k + \frac{h}{2}k_2) \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega}_{b,k+\frac{1}{2}} \end{bmatrix} \\ k_4 &= \frac{1}{2}(q_k + hk_3) \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega}_{b,k+1} \end{bmatrix} \\ q_{k+1} &= q_k + \frac{h}{6}(k_1 + 2k_2 + 2k_3 + k_4) \end{aligned}\]其中:
- $h$ 是时间步长
- $\boldsymbol{\omega}_{b,k}$ 是 $t_k$ 时刻物体坐标系下的角速度测量值
- $\boldsymbol{\omega}_{b,k+\frac{1}{2}}$ 是 $t_k + \frac{h}{2}$ 时刻的角速度(通常通过线性插值得到)
- $\boldsymbol{\omega}{b,k+1}$ 是 $t{k+1}$ 时刻的角速度
注意:由于数值积分可能破坏四元数的单位约束,每次积分后需要进行归一化:
\[q_{k+1} \leftarrow \frac{q_{k+1}}{\|q_{k+1}\|}\]在惯性导航系统中,更新姿态过程:
- 从陀螺仪获取角速度测量值 $\boldsymbol{\omega}_b$(物体坐标系下)
- 使用四元数微分方程更新姿态四元数: \(\dot{q}(t) = \frac{1}{2} q(t) \otimes \begin{bmatrix} 0 \\ \boldsymbol{\omega}_b(t) \end{bmatrix}\)
- 对更新后的四元数进行归一化
- 重复以上步骤
这种方法的优点是:
- 计算效率高
- 避免了欧拉角表示中的万向节锁问题
- 直接使用物体坐标系下的角速度测量值,无需坐标转换
6. 四元数插值
在机器人运动规划中,经常需要在两个姿态之间进行平滑插值。四元数插值主要有两种方法:
6.1. 线性插值 (LERP)
对于两个四元数 $q_0$ 和 $q_1$,线性插值定义为:
\[q_{lerp}(t) = \frac{(1-t)q_0 + tq_1}{\|(1-t)q_0 + tq_1\|}\]其中 $t \in [0,1]$ 是插值参数。
特点:
- 计算简单
- 插值路径不是最短路径
- 插值速度不均匀
6.2. 球面线性插值 (SLERP)
球面线性插值能够保证插值路径是最短路径,且速度均匀:
\[q_{slerp}(t) = q_0 (q_0^{-1}q_1)^t\]或者等价的表达式:
\[q_{slerp}(t) = \frac{\sin((1-t)\theta)}{\sin\theta}q_0 + \frac{\sin(t\theta)}{\sin\theta}q_1\]其中 $\theta$ 是 $q_0$ 和 $q_1$ 之间的夹角。
特点:
- 插值路径是最短路径
- 角速度恒定
- 计算相对复杂