NXP传感器融合笔记03(四元数)

NXP传感器融合笔记03(四元数)

这个笔记覆盖视频教程的第三讲和第四讲。全部关于四元数。四元数非常重要,我们真正程序里做姿态运算不会用欧拉角,很少用旋转矩阵,基本都是用四元数



四元数(Quaternion)概念

知道了轴角对,那么四元数的概念就非常好引入: {\displaystyle q=\left(\cos {\tfrac {\theta }{2}},{\boldsymbol {\omega }}\sin {\tfrac {\theta }{2}}\right)} 其中 \theta 是轴角对中的角,而 \omega 就是轴角对中的轴向量,前面的 cos 叫做标量部分叫做 w ,后面的 sin 叫做矢量部分,叫做 x \ y \ z . 有的表示方法把标量放到最前面,有的放到最后面,要注意区分. 所以四元数一般写作 \begin{bmatrix} w&x&y&z\end{bmatrix} 或者 \begin{bmatrix} x&y&z&w\end{bmatrix} (wxyz 的情况居多) 。


四元数可以看做一个标量加上三个复数所组成的一个矢量表示的一组数:表示方法:

\mathbf{q}=\left[\begin{array}{l}{q_{w}} \\ {\mathbf{q}_{v}}\end{array}\right] (标量在前,也有的把标量放在后面的,看paper时候注意甄别!!!)

或者写作: \mathrm{q}=q_{0}+i q_{1}+j q_{2}+k q_{3}

满足:

  • i^{2}=j^{2}=k^{2}=i j k=-1
  • i=j k=-k j
  • j=k i=-i k
  • k=i j=-j i

四元数有几个重要性质:(四元数的性质和旋转矩阵(正交矩阵)一模一样)

  • 四元数就像旋转矩阵一样,AB != BA,不满足结合律
  • 单位四元数可以表示姿态,和旋转矩阵一一对应
  • 单位四元数的共轭和取逆一样的结果,就是把矢量部分取负,非常像旋转矩阵(正交矩阵)

四元数的几个重要运算

  1. 归一化:就是就是单位化: q^{-1}=q^{\star} /|q|^{2} 只要单位四元数才能表示姿态!
  2. 共轭: q^{\star}=q_{0}-q=q_{0}-i q_{1}-j q_{2}-k q_{3} : 标量不变,矢量部分取逆,单位共轭四元数表示原单位四元数的反方向旋转
  3. 四元数相乘(插积): \mathrm{pq}=\mathrm{p}_{0} \mathrm{q}_{0}-\mathrm{p} \cdot \mathrm{q}+\mathrm{p}_{0} \mathrm{q}+\mathrm{q}_{0} \mathrm{p}+\mathrm{p} \times \mathrm{q} (四元数相乘不满足交换律),写成矩阵形式: \mathbf{p} \otimes \mathbf{q}=\left[\begin{array}{l}{p_{w} q_{w}-p_{x} q_{x}-p_{y} q_{y}-p_{z} q_{z}} \\ {p_{w} q_{x}+p_{x} q_{w}+p_{y} q_{z}-p_{z} q_{y}} \\ {p_{w} q_{y}-p_{x} q_{z}+p_{y} q_{w}+p_{z} q_{x}} \\ {p_{w} q_{z}+p_{x} q_{y}-p_{y} q_{x}+p_{z} q_{w}}\end{array}\right]
  4. 点积: \mathbf{p} \cdot \mathbf{q}=p_{0} q_{0}+p_{1} q_{1}+p_{2} q_{2}+p_{3} q_{3}=|\mathbf{p} \| \mathbf{q} |\cos \varphi 和三维向量点积差不多。其中这个角度指的是3D空间中两个旋转所转过的角度的一半


以后如不另行说明,说道四元数,一般都指单位四元数

四元数旋转向量的几何图景

一个向量如何通过四元数旋转呢?

设有向量 v=\left(0, v_{x}, v_{y}, v_{z}\right) ,四元数q那么

将v通过q转到w的公式为: L_{q}(v)=w=q v q^{*} ,看到了吧,就是两个四元数相乘哎!几何意义可以用轴角对解释,绕着q所对应的轴角对的轴,旋转 2\theta °角

左右两幅图其实都是一个意思,四元数旋转向量的几何直观解释

利用四元数进行连续旋转

连续旋转如何用四元数来表示呢:两个旋转p,q: L_{p}(u)=p u p^{\star} \text { and } L_{q}(v)=q v q^{*}

实际上可以用 w=L_{q p}(u) 来执行连续旋转,也就是说,先组合成一个新四元数,然后再转向量,如下图

两个连续的旋转四元数可以组合成一个新的四元数,表示之前两种旋转的叠加

四元数和旋转矢量(Roation Vector)的互相转化

根据四元数和旋转矢量的定义:设旋转矢量单位化的轴为 \widehat{\boldsymbol{n}} ,转过角度为 \eta

则有 四元数和旋转矢量的关系

q=q_{0}+q_{1} i+q_{2} j+q_{3} k=\cos \left(\frac{\eta}{2}\right)+\widehat{n} \sin \left(\frac{\eta}{2}\right)

旋转矢量转四元数:

q_{0}=\cos \left(\frac{\eta}{2}\right) \Rightarrow \eta=2 \cos ^{-1}\left(q_{0}\right)

\begin{array}{l}{q_{1}=\sin \left(\frac{\eta}{2}\right) n_{x} \Rightarrow n_{x}=\frac{q_{1}}{\sin \left(\frac{\eta}{2}\right)}} \\ {q_{2}=\sin \left(\frac{\eta}{2}\right) n_{y} \Rightarrow n_{y}=\frac{q_{2}}{\sin \left(\frac{\eta}{2}\right)}} \\ {q_{3}=\sin \left(\frac{\eta}{2}\right) n_{z} \Rightarrow n_{z}=\frac{q_{3}}{\sin \left(\frac{\eta}{2}\right)}}\end{array}

四元数转旋转矢量:

这个简单,按定义来就好: q=\cos \left(\frac{\eta}{2}\right)+\widehat{n} \sin \left(\frac{\eta}{2}\right)

四元数微积分

先说结论,然后推导: \dot{q}(t)=(1 / 2) q(t) \omega(t),其中 \omega(t)=\left\{0, \omega_{x}, \omega_{y}, \omega_{z}\right\}

推导:

定义:使用极限的方式定义四元数微分: \begin{aligned} \frac{d q(t)}{d t} &=\dot{q}(t)=\lim _{\delta t \rightarrow 0}\left\{\frac{q(t+\delta t)-q(t)}{\delta t}\right\} \\ & \Rightarrow q(t+d t)=q(t)+d t \dot{q}(t) \end{aligned}

正向传播: q(t+\delta t)=q(t) \delta q(t)

其中: \delta q=\left\{q_{0}, \delta q_{1}, \delta q_{2}, \delta q_{3}\right\}=\left\{q_{0}, \sin \left(\frac{\omega_{x} \delta t}{2}\right), \sin \left(\frac{\omega_{y} \delta t}{2}\right), \sin \left(\frac{\omega_{z} \delta t}{2}\right)\right\} ,其中q0接近于1

那么有: d q=\left\{q_{0}, d q_{1}, d q_{2}, d q_{3}\right\}=\left\{1,\left(\frac{\omega_{x} d t}{2}\right),\left(\frac{\omega_{y} d t}{2}\right),\left(\frac{\omega_{z} d t}{2}\right)\right\}

进而有: q(t+d t)=q(t) d q=q(t)\left\{1,\left(\frac{\omega_{x} d t}{2}\right),\left(\frac{\omega_{y} d t}{2}\right),\left(\frac{\omega_{z} d t}{2}\right)\right\}

结合极限定义,有: q(t)+d t \dot{q}(t)=q(t)\left\{1,\left(\frac{\omega_{x} d t}{2}\right),\left(\frac{\omega_{y} d t}{2}\right),\left(\frac{\omega_{z} d t}{2}\right)\right\}

\left(\frac{d t}{2}\right) 提出来,整理得: \begin{array}{c}{q(t)+d t \dot{q}(t)=q(t)+\left(\frac{d t}{2}\right) q(t)\left\{0, \omega_{x}, \omega_{y}, \omega_{z}\right\}} \\ {\Rightarrow \dot{q}(t)=\left(\frac{1}{2}\right) q(t) \omega(t)}\end{array}

四元数积分:(由微分公式而来)

q_{k}=q_{k-1} \Delta q\left(\omega_{k} \delta t\right)

\Delta q=\left\{\cos \left(\frac{\left|\omega_{k}\right| \delta t}{2}\right),\left(\frac{\omega_{k}}{\left|\omega_{k}\right|}\right) \sin \left(\frac{\left|\omega_{k}\right| \delta t}{2}\right)\right\}

四元数与旋转矩阵的相互转化

将四元数转化为旋转矩阵的公式为:

C_{b}^{n} = \left|\begin{array}{ccc} q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2} & 2\left(q_{1} q_{2}-q_{0} q_{3}\right) & 2\left(q_{1} q_{3}+q_{0} q_{2}\right) \\ 2\left(q_{1} q_{2}+q_{0} q_{3}\right) & q_{0}^{2}-q_{1}^{2}+q_{2}^{2}-q_{3}^{2} & 2\left(q_{2} q_{3}-q_{0} q_{1}\right) \\ 2\left(q_{1} q_{3}-q_{0} q_{2}\right) & 2\left(q_{2} q_{3}+q_{0} q_{1}\right) & q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2} \end{array}\right|

其中 qn->b的四元数旋转. 但是:四元数的记法似乎不像矩阵那么统一:对于n->b对应的四元数:

  • 国内(秦老师,严老师)记法为 Q_{b}^{n} (从上往下,和方向余弦阵表示方法相反)
  • 国外(Paul D.Groves GNSS与惯性及多传感器组合导航系统原理): Q_{n}^{b} (和方向余弦阵表示方法相同)

这个问题困扰了我N多年,如果用上面这种想法解释的话,各种国内外文献都可以解释的通。咱们就这么理解吧!

matllab实战

先介绍一个验证网站,在线计算3D数学旋转,拿着他做对照吧(注意四元数的格式):3D Rotation Converter

练习来自:Paul D.Groves GNSS与惯性及多传感器组合导航系统原理

练习1

已知N系到B系的欧拉角:

求 Cn2b:

Note: 这里要注意一下,书写 \psi _{nb} 的时候有的作者表示n->b系的旋转,有的作者表示b->n系的旋转,这个问题行业没有统一,需要特别注意。在我的笔记里,为了避免这个问题,我全部写成Cb2n(表示b系 到 n系的旋转) 和 Cn2b(表示n系到b系的旋转), ... (多个2 代表"to", 意思就明白多了)

\mathbf{C}_{n}^{b}, 根据公式: C_{n}^{b}=\left(\begin{array}{ccc}{\cos \theta \cos \psi} & {\cos \theta \sin \psi} & {-\sin \theta} \\ {\cos \psi \sin \theta \sin \phi-\cos \phi \sin \psi} & {\cos \phi \cos \psi+\sin \theta \sin \phi \sin \psi} & {\cos \theta \sin \phi} \\ {\cos \phi \cos \psi \sin \theta+\sin \phi \sin \psi} & {\cos \phi \sin \theta \sin \psi-\cos \psi \sin \phi} & {\cos \theta \cos \phi}\end{array}\right)

可得:

roll = deg2rad(-30);
pitch = deg2rad(30);
yaw = deg2rad(45);
eul = [roll pitch yaw]';

Cn2b = ch_eul2m(eul);

输出:

ans =

    0.6124    0.6124   -0.5000
   -0.7891    0.4356   -0.4330
   -0.0474    0.6597    0.7500


练习2

已知 \mathbf{C}_{b}^{n}=\left[\begin{array}{ccc}{0.612372} & {-0.78915} & {-0.04737} \\ {0.612372} & {0.435596} & {0.65974} \\ {-0.5} & {-0.43301} & {0.75}\end{array}\right]

Q_{n2b} (注意,对于严龚敏的书,: Q_{n2b} 写做 Q_{b}^{n} . 对于Groves的书: Q_{n2b} 写做 Q_{n}^{b}

Cb2n = [0.612372 -0.78915 -0.04737;  0.612372 0.435596 0.65974 ; -0.5 -0.43301 0.75];
Qn2b = ch_m2q(Cb2n);
Qn2b

输出:

ans =

    0.8364
   -0.3266
    0.1353
    0.4189

练习3

已知

请将矩阵转为欧拉角和四元数

Cb2n = [0.612372 -0.78915 -0.04737; 0.612372 0.435596 0.65974; -0.5 -0.43301 0.75];
eul = ch_m2eul(Cb2n');
eul


Qn2b = ch_m2q(Cb2n);
Qn2b

输出

eul =

   -0.5236
    0.5236
    0.7854


Qn2b =

    0.8364
   -0.3266
    0.1353
    0.4189

练习4

已知

请将四元数转成矩阵和欧拉角

Qn2b = [0.836356 -0.32664 0.135299 0.418937]';
Cb2n = ch_q2m(Qn2b);
Cb2n

eul = ch_q2eul(Qn2b);
rad2deg(eul)

输出:

Cb2n =

    0.6124   -0.7891   -0.0474
    0.6124    0.4356    0.6597
   -0.5000   -0.4330    0.7500


ans =

  -29.9999
   30.0000
   45.0000

参考

编辑于 05-25

文章被以下专栏收录