四元数乘法

首先来看一下四元数的乘法：
对两个四元数q和p：
q=w1+x1i+y1j+z1kp=w2+x2i+y2j+z2k\begin{array}{l} {q=w_{1}+x_{1} i+y_{1} j+z_{1} k} \\ {p=w_{2}+x_{2} i+y_{2} j+z_{2} k} \end{array}q=w1+x1i+y1j+z1kp=w2+x2i+y2j+z2k
他们的乘法可以写成：
qp=+(x1∗w2+w1∗x2+z1∗y2−z1∗y2)i+(y1∗w2+w1∗y2+z1∗x2−x1∗z2)j+(z1∗w2+w1∗z2+x1∗y2−x2∗y1)k+(w1∗w2−x1∗x2−y1∗y2−z1∗z2)\begin{aligned} q p= +\left(x_{1} * w_{2}+w_{1} * x_{2}+z_{1} * y_{2}-z_{1} * y_{2}\right) i\\ +\left(y_{1} * w_{2}+w_{1} * y_{2}+z_{1} * x_{2}-x_{1} * z_{2}\right) j\\ +\left(z_{1} * w_{2}+w_{1} * z_{2}+x_{1} * y_{2}-x_{2} * y_{1}\right) k\\ +\left(w_{1} * w_{2}-x_{1} * x_{2}-y_{1} * y_{2}-z_{1} * z_{2}\right)\\ \end{aligned}qp=+(x1∗w2+w1∗x2+z1∗y2−z1∗y2)i+(y1∗w2+w1∗y2+z1∗x2−x1∗z2)j+(z1∗w2+w1∗z2+x1∗y2−x2∗y1)k+(w1∗w2−x1∗x2−y1∗y2−z1∗z2)

当四元数实部为0，即：
q=0+x1i+y1j+z1kp=0+x2i+y2j+z2k\begin{array}{l} {q=0+x_{1} i+y_{1} j+z_{1} k} \\ {p=0+x_{2} i+y_{2} j+z_{2} k} \end{array}q=0+x1i+y1j+z1kp=0+x2i+y2j+z2k
他们的乘法可以写成：
q×p=+(z1∗y2−z1∗y2)i+(z1∗x2−x1∗z2)j+(x1∗y2−x2∗y1)k+(−x1∗x2−y1∗y2−z1∗z2)\begin{aligned} q ×p= +\left(z_{1} * y_{2}-z_{1} * y_{2}\right) i\\ +\left(z_{1} * x_{2}-x_{1} * z_{2}\right) j\\ +\left(x_{1} * y_{2}-x_{2} * y_{1}\right) k\\ +\left(-x_{1} * x_{2}-y_{1} * y_{2}-z_{1} * z_{2}\right)\\ \end{aligned}q×p=+(z1∗y2−z1∗y2)i+(z1∗x2−x1∗z2)j+(x1∗y2−x2∗y1)k+(−x1∗x2−y1∗y2−z1∗z2)

四元数的时间导数

再看一下四元数的时间导数：
dq(t)dt=lim⁡t→0q(t+δt)−q(t)δt=lim⁡t→0δq⊗q(t)−q(t)δt=lim⁡t→0(δq−1)⊗q(t)δt=lim⁡t→0[0jθ2]⊗q(t)δt=12[0ω]⊗q\begin{aligned} \frac{d \mathbf{q}(t)}{d t} &=\lim _{t \rightarrow 0} \frac{\mathbf{q}(t+\delta t)-\mathbf{q}(t)}{\delta t} \\ &=\lim _{t \rightarrow 0} \frac{\delta \mathbf{q} \otimes \mathbf{q}(t)-\mathbf{q}(t)}{\delta t} \\ &=\lim _{t \rightarrow 0} \frac{(\delta \mathbf{q}-1) \otimes \mathbf{q}(t)}{\delta t}\\ &=\lim _{t \rightarrow 0} \frac{\left[\begin{array}{l} {0} \\ {\frac{j \theta}{2}} \end{array}\right] \otimes \mathbf{q}(t)}{\delta t}=\frac{1}{2}\left[\begin{array}{l} {0} \\ {\omega} \end{array}\right] \otimes \mathbf{q} \end{aligned} dtdq(t)=t→0limδtq(t+δt)−q(t)=t→0limδtδq⊗q(t)−q(t)=t→0limδt(δq−1)⊗q(t)=t→0limδt[02jθ]⊗q(t)=21[0ω]⊗q

实际上就是把三维角速度用四元数的方式表示，Ω与四元数相乘，实际上就是四元数之间的乘法。

VINS-MONO里的相关公式

看下VINS-MONO里面和四元数相关的函数：
qbk+1w=qbkw⊗∫t∈[tk,tk+1]12Ω(ω^t−bwt−nw)qtbkdt\mathbf{q}_{b_{k+1}}^{w}=\mathbf{q}_{b_{k}}^{w} \otimes \int_{t \in\left[t_{k}, t_{k+1}\right]} \frac{1}{2} \mathbf{\Omega}\left(\hat{\omega}_{t}-\mathbf{b}_{w_{t}}-\mathbf{n}_{w}\right) \mathbf{q}_{t}^{b_{k}} d tqbk+1w=qbkw⊗∫t∈[tk,tk+1]21Ω(ω^t−bwt−nw)qtbkdt

Ω(ω)=[−⌊ω⌋×ω−ωT0],⌊ω⌋×=[0−ωzωyωz0−ωx−ωyωx0]\boldsymbol{\Omega}(\boldsymbol{\omega})=\left[\begin{array}{cc} {-\lfloor\boldsymbol{\omega}\rfloor_{\times}} & {\boldsymbol{\omega}} \\ {-\boldsymbol{\omega}^{T}} & {0} \end{array}\right],\lfloor\boldsymbol{\omega}\rfloor_{\times}=\left[\begin{array}{ccc} {0} & {-\omega_{z}} & {\omega_{y}} \\ {\omega_{z}} & {0} & {-\omega_{x}} \\ {-\omega_{y}} & {\omega_{x}} & {0} \end{array}\right]Ω(ω)=[−⌊ω⌋×−ωTω0],⌊ω⌋×=⎣⎡0ωz−ωy−ωz0ωxωy−ωx0⎦⎤

把Ω完整地写出来之后：
Ω(ω)=[0ωz−ωyωx−ωz0ωxωyωy−ωx0ωz−ωx−ωy−ωz0]\Omega(\omega)=\left[\begin{array}{cccc} {0} & {\omega_{z}} & {-\omega_{y}} &{\omega_x}\\ {-\omega_{z}} & {0} & {\omega_{x}} &{\omega_y}\\ {\omega_{y}} & {-\omega_{x}} & {0} &{\omega_z} \\ {{-\omega_x}} & {-\omega_{y}} &{-\omega_{z}} &0 \end{array}\right]Ω(ω)=⎣⎢⎢⎡0−ωzωy−ωxωz0−ωx−ωy−ωyωx0−ωzωxωyωz0⎦⎥⎥⎤

我们发现Ω和形式为[kx,ky,kz,0][ k_x,k_y,k_z,0][kx,ky,kz,0]的四元数相乘：
Ω(ω)=[ωz∗ky−ωy∗kz−ωz∗kx+ωx∗kzωy∗kx−ωx∗ky−ωx∗kx−ωy∗ky−ωz∗kz]\Omega(\omega)=\left[\begin{array}{cccc} {\omega_{z}}*k_y - {\omega_{y}}*k_z \\ {-\omega_{z}*k_x} + {\omega_{x}}*k_z \\ {\omega_{y}*k_x - \omega_{x}*k_y} \\ {-\omega_{x}*k_x} -{\omega_{y}*k_y}-{\omega_{z}*k_z} \end{array}\right]Ω(ω)=⎣⎢⎢⎡ωz∗ky−ωy∗kz−ωz∗kx+ωx∗kzωy∗kx−ωx∗ky−ωx∗kx−ωy∗ky−ωz∗kz⎦⎥⎥⎤

这是不是和四元数乘法很像？于是我们就知道Ω和四元数乘法其实就是一样的东西。

12[0ω]⊗q=12Ω(ω)q\frac{1}{2}\left[\begin{array}{l} {0} \\ {\omega} \end{array}\right] \otimes \mathbf{q} = \frac{1}{2}\Omega(\omega)q21[0ω]⊗q=21Ω(ω)q
用四元数表示的角速度与bkb_kbk时刻的四元数相乘，表示当前时刻各个方向的角速度，然后在kkk到k+1k+1k+1间隔求积分，得到k+1k+1k+1时刻相对于kkk时刻的旋转。

参考：
https://www.cnblogs.com/youzx/p/6387740.html
https://mp.weixin.qq.com/s/FDMDleH8wszsD9HkBXwAnQ

VIO中的角速度和四元数相关推荐

在matlab和python if中的旋转变换（四元数、欧拉角、旋转矩阵)等
在matlab和python tf中的旋转变换(四元数.欧拉角.旋转矩阵等) 目录 1. 基本的认识 2. 变换矩阵 3. 欧拉角 4. 四元数 5. matlab中相互转换 6. python tf ...
数学中的一朵“奇葩”——四元数
代数学是数学中最古老的的学科之一,在之后相当长的一段时期内,代数学都曾成为数学的中心.但中世纪过后,传统的代数学开始沉寂,陷入解方程的泥淖中,而自微积分被发明之后,数学迎来分析学的黄金时代,进而代数学 ...
自己写的调用Onboard-SDK-master中大疆API控制无人机210飞行并SDK中记录飞行高度、四元数的程序
一.程序的功能: 本程序实现的功能是:以大地为坐标系,融合数据的海拔(单位:米)为高度,实现210无人机的位移控制,同时以50hz频率来记录飞行中的信息(高度和四元数). 注:本程序是我自己写的,过程 ...
VIO中IMU选型与数据处理
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨蘅芜君@知乎来源丨https://zhuanlan.zhihu.com/p/433625159 ...
ros中有关欧拉角和四元数互转的python写法
1.欧拉角转四元数如:pos = Pose() q = tf.transformations.quaternion_from_euler(0, 0, point.z) pos.orientation ...
从零开始学习VIO笔记 --- 第一讲：基础知识（四元数，李代数）
从零开始学习VIO笔记 --- 第一讲:基础知识(四元数,李代数) 一. 向量的内积与外积二. 旋转与平移 2.1 旋转表示 --- 旋转矩阵R 2.2 平移向量 2.3 变换矩阵T与齐次坐标 2. ...
刚体运动中的坐标变换-旋转矩阵、旋转向量、欧拉角及四元数
坐标变换及其方法 1.转化关系图 2 换算关系 3.1 旋转矩阵换算至其他 3.2 四元数换算至其他 3.3 旋转向量转换至旋转矩阵与四元数 3.3 欧拉角转换到旋转矩阵和四元数 3 坐标变换 4 坐 ...
四元数微分求角速度【离散】
在姿态解算中,我们一般会利用角速度w和当前的四元数q(k)预测下一时刻的四元数q(k+1),但是有时候我们需要根据四元数反过来求角速度. 四元数的微分令 q(t) 是一个单位四元数函数,ω(t) 是 ...
Vivado Logic Analyzer中VIO核的使用
ChipScope有两种使用的方式:cdc和IP Core.由于VIO的IP只能通过IP的方式来使用,所以IP的方式ChipScope中有重要的意义. 同样,Vivado中的VIO也只能通过IP的方式 ...

VIO中的角速度和四元数

VIO的角速度和四元数

四元数乘法

四元数的时间导数

VINS-MONO里的相关公式

VIO中的角速度和四元数相关推荐

最新文章

热门文章