陀螺仪和加速度计的读数和四元数怎么建立关系
Posted
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了陀螺仪和加速度计的读数和四元数怎么建立关系相关的知识，希望对你有一定的参考价值。
四元数与欧拉角之间是有公式互相转换的，而欧拉角指的是三个角：俯仰角，翻滚角，转向角，这三个都是角度，与角度有关的只有陀螺仪，而且必须是三轴陀螺仪，或者是三个陀螺仪。至于加速度计，不觉得跟四元数有什么关系。 参考技术A 四元数:一种姿态表示方法，包含四个元素，[q0,q1,q2,q3]。其中物理意义是，q0钢体旋转的角度二分之一的余弦，[q1,q2,q3]刚体的旋转轴。具体请参见其它资料陀螺仪：一种传感器，测量旋转角速度
加速度计：一种传感器，测量重力矢量（虽然它可以测量加速度，但在姿态测量中，它是为了获取重力的方向）
其它：刚体的状态需要6个参数来完全确定，分别是位置和旋转（姿态）的3个独立正交的物理量，所以测量姿态需要3个轴的陀螺仪。
陀螺仪测量的是角速度，它与姿态存在的是微分关系，可以使用一阶龙格库塔法更新四元数，即姿态。但是微分方程是有通解和特解问题的，即使用陀螺仪积分四元数有初始状态C的问题。此外离散积分（求和）还有统计上的问题，设陀螺仪的测量方差为0.001，那么两次采样后的方差将是0.002。所以只要陀螺仪存在噪声，由它积分后得到的姿态的方差一定是随时间发散的，如何避免解决呢？
方法是通过对姿态进行直接测量（这也是多传感器组合测量物理量的基本方法），确定初始状态，同时由于直接测量的统计期望的方差是极小的，用它来稳定陀螺仪的积分带来的方差增长（方法由互补滤波，卡尔曼滤波等，我也只会这两种，第一种简单快速有效，第二种精确理想）。
问题：一个确定的姿态需要两个不平行的向量，只测量了重力矢量，能对抗陀螺仪的积分方差吗？
答案是，不能！那么结果是什么呢？整个姿态解算的结果（陀螺仪+加速度计）会随着时间发生偏航角的漂移，即姿态永远是可以知道水平面在哪里，但是不知道自己的正前方在哪里，会围绕重力方向发生旋转。怎么解决？
引入磁力计或者rtk，获得另外一个水平的向量来完全的确定姿态。
不足之处请指正，四川金瑞麒，运动控制，祝好！
龙格库塔（Runge-Kutta）法求四元数微分方程
文章目录
一、背景知识
陀螺仪是一种测量角运动的装置，在导航、运动检测、姿态检测等方面有着非常广泛的应用。在惯性测量单元（IMU）中，陀螺仪起主要作用，加速度计往往用来辅助，通过滤波算法和融合算法可以由IMU输出的数据得到物体的姿态。事实上，如果陀螺仪的性能足够好，就可以只通过对陀螺仪得到准确的姿态了。这篇文章要解决的问题就是如何用陀螺仪的数据得到物体的姿态。
1. 坐标系
陀螺仪输出的是角速度，那么对角速度积分是不是就能得到物体在空间中的姿态呢？事实上并非如此，在这里需要理解三个坐标系：世界坐标系、载体坐标系和惯性坐标系。
世界坐标系又称作宇宙坐标系或者全局坐标系，是其他坐标系的参考系，我们可以在世界坐标系下去描述其他坐标系或者物体的位置和姿态。
载体坐标系，又称作机体坐标系、物体坐标系等，顾名思义，这个坐标系是物体上的坐标系，会跟随物体的运动而运动。以手机为例，一般会在手机上建立如下所示的右手坐标系，这个坐标系就是一个载体坐标系。陀螺仪安装在手机上，如果忽略芯片本身和安装带来的非正交误差的话，可以认为陀螺仪的坐标系和手机的坐标系是同一个坐标系。所以，陀螺仪输出的角速度实际上是手机载体坐标系的角速度，直接对陀螺仪角速度积分得到的不是我们从世界坐标系看到的手机的姿态角度。需要将陀螺仪角速度转换到惯性坐标系，然后再积分才能得到物体的姿态。
惯性坐标系是为了简化世界坐标系和载体坐标系的转化而产生的。两个直角坐标系的关系包括旋转和位移。惯性坐标系的原点和载体坐标系的原点是同一个，所以通过旋转关系，两个坐标系就能够重合。而惯性坐标系通过位移就能够和世界坐标系重合。
2. 四元数
表示一个物体的姿态有很多种方法，最主流的方法有欧拉角、旋转矩阵、轴角，以及四元数。这些表示方法各有优缺点，但综合来说，四元数是一种非常适合在工程上表示姿态的方法。四元数可以理解成复数的扩展，它有一个实部和三个虚部，表示成 q = w + x ∗ i + y ∗ j + z ∗ k q=w+x\\asti+y\\astj+z\\astk q=w+x∗i+y∗j+z∗k，其中 i ∗ i = − 1 , j ∗ j = − 1 , k ∗ k = − 1 , i ∗ j ∗ k = − 1 i\\asti=-1,j\\astj=-1,k\\astk=-1,i\\astj\\astk=-1 i∗i=−1,j∗j=−1,k∗k=−1,i∗j∗k=−1。四元数有自己一套运算法则和性质，后面在其他文章具体阐述，这里介绍几个重要的点。
四元数的矩阵形式
类似复数，四元数也有模长或者称为2-范数，即
∥
q
∥
=
w
2
+
x
2
+
y
2
+
z
2
\\beginVmatrixq\\endVmatrix=\\sqrt w^2+x^2+y^2+z^2
∥∥q∥∥=w2+x2+y2+z2，如果
∥
q
∥
=
1
\\beginVmatrixq\\endVmatrix=1
∥∥q∥∥=1，则称这个四元数位单位四元数，单位四元数有如下性质：
q
−
1
=
q
∗
q^-1=q^\\ast
q−1=q∗。 以上是关于陀螺仪和加速度计的读数和四元数怎么建立关系的主要内容，如果未能解决你的问题，请参考以下文章 简单VR照片 使用陀螺仪姿态角（RollPitchYaw ）四元数
假设两个四元数相乘
q
1
q
2
q_1q_2
q1q2，其中
q
1
=
a
+
b
∗
i
+
c
∗
j
+
d
∗
k
q_1=a+b\\asti+c\\astj+d\\astk
q1=a+b∗i+c∗j+d∗k，
q
2
=
e
+
f
∗
i
+
g
∗
j
+
h
∗
k
q_2=e+f\\asti+g\\astj+h\\astk
q2=e+f∗i+g∗j+h∗k，则
q
1
∗
q
2
=
a
e
+
a
f
∗
i
+
a
g
∗
j
+
a
h
∗
k
+
b
e
∗
i
−
b
f
+
b
g
∗
k
−
b
h
∗
j
+
c
e
∗
j
−
c
f
∗
k
−
c
g
+
c
h
∗
i
+
d
e
k
+
d
f
∗
j
−
d
g
∗
i
−
d
h
=
(
a
e
−
b
f
−
c
g
−
d
h
)
+
(
b
e
+
a
f
−
d
g
+
c
h
)
∗
i
(
c
e
+
d
f
+
a
g
−
b
h
)
∗
j
(
d
e
−
c
f
+
b
g
+
a
h
)
∗
k
\\beginaligned q_1 * q_2 = & ae + af * i + ag * j + ah * k + \\\\ & be * i - bf + bg * k - bh * j + \\\\ & ce * j - cf * k - cg + ch * i + \\\\ & dek + df * j - dg * i - dh \\\\ =& (ae - bf - cg - dh) + \\\\ & (be + af - dg + ch) * i \\\\ & (ce + df + ag - bh) * j \\\\ & (de - cf + bg + ah) * k \\endaligned
q1∗q2==ae+af∗i+ag∗j+ah∗k+be∗i−bf+bg∗k−bh∗j+ce∗j−cf∗k−cg+ch∗i+dek+df∗j−dg∗i−dh(ae−bf−cg−dh)+(be+af−dg+ch)∗i(ce+df+ag−bh)∗j(de−cf+bg+ah)∗k
上面的结果可以写成一个方阵和一个列矩阵相乘：
q
1
∗
q
2
=
∣
a
−
b
−
c
−
d
b
a
−
d
c
c
d
a
−
b
d
−
c
b
a
∣
∣
e
f
g
h
∣
q_1 * q_2 = \\beginvmatrixa&-b&-c&-d\\\\b&a&-d&c\\\\c&d&a&-b\\\\d&-c&b&a\\endvmatrix \\beginvmatrixe\\\\f\\\\g\\\\h\\endvmatrix
q1∗q2=∣∣∣∣∣∣∣∣abcd−bad−c−c−dab−dc−b