文章目录
一、背景知识
陀螺仪是一种测量角运动的装置，在导航、运动检测、姿态检测等方面有着非常广泛的应用。在惯性测量单元（IMU）中，陀螺仪起主要作用，加速度计往往用来辅助，通过滤波算法和融合算法可以由IMU输出的数据得到物体的姿态。事实上，如果陀螺仪的性能足够好，就可以只通过对陀螺仪得到准确的姿态了。这篇文章要解决的问题就是如何用陀螺仪的数据得到物体的姿态。
1. 坐标系
陀螺仪输出的是角速度，那么对角速度积分是不是就能得到物体在空间中的姿态呢？事实上并非如此，在这里需要理解三个坐标系：世界坐标系、载体坐标系和惯性坐标系。
世界坐标系又称作宇宙坐标系或者全局坐标系，是其他坐标系的参考系，我们可以在世界坐标系下去描述其他坐标系或者物体的位置和姿态。
载体坐标系，又称作机体坐标系、物体坐标系等，顾名思义，这个坐标系是物体上的坐标系，会跟随物体的运动而运动。以手机为例，一般会在手机上建立如下所示的右手坐标系，这个坐标系就是一个载体坐标系。陀螺仪安装在手机上，如果忽略芯片本身和安装带来的非正交误差的话，可以认为陀螺仪的坐标系和手机的坐标系是同一个坐标系。所以，陀螺仪输出的角速度实际上是手机载体坐标系的角速度，直接对陀螺仪角速度积分得到的不是我们从世界坐标系看到的手机的姿态角度。需要将陀螺仪角速度转换到惯性坐标系，然后再积分才能得到物体的姿态。
惯性坐标系是为了简化世界坐标系和载体坐标系的转化而产生的。两个直角坐标系的关系包括旋转和位移。惯性坐标系的原点和载体坐标系的原点是同一个，所以通过旋转关系，两个坐标系就能够重合。而惯性坐标系通过位移就能够和世界坐标系重合。
2. 四元数
表示一个物体的姿态有很多种方法，最主流的方法有欧拉角、旋转矩阵、轴角，以及四元数。这些表示方法各有优缺点，但综合来说，四元数是一种非常适合在工程上表示姿态的方法。四元数可以理解成复数的扩展，它有一个实部和三个虚部，表示成 q = w + x ∗ i + y ∗ j + z ∗ k q=w+x\ast{i}+y\ast{j}+z\ast{k} q=w+x∗i+y∗j+z∗k，其中 i ∗ i = − 1 , j ∗ j = − 1 , k ∗ k = − 1 , i ∗ j ∗ k = − 1 i\ast{i}=-1,j\ast{j}=-1,k\ast{k}=-1,i\ast{j}\ast{k}=-1 i∗i=−1,j∗j=−1,k∗k=−1,i∗j∗k=−1。四元数有自己一套运算法则和性质，后面在其他文章具体阐述，这里介绍几个重要的点。
四元数的矩阵形式
类似复数，四元数也有模长或者称为2-范数，即 ∥ q ∥ = w 2 + x 2 + y 2 + z 2 \begin{Vmatrix}q\end{Vmatrix}=\sqrt {w^2+x^2+y^2+z^2} ∥∥q∥∥=w2+x2+y2+z2，如果 ∥ q ∥ = 1 \begin{Vmatrix}q\end{Vmatrix}=1 ∥∥q∥∥=1，则称这个四元数位单位四元数，单位四元数有如下性质： q − 1 = q ∗ q^{-1}=q^{\ast} q−1=q∗。
假设两个四元数相乘 q 1 q 2 q_1q_2 q1q2，其中 q 1 = a + b ∗ i + c ∗ j + d ∗ k q_1=a+b\ast{i}+c\ast{j}+d\ast{k} q1=a+b∗i+c∗j+d∗k， q 2 = e + f ∗ i + g ∗ j + h ∗ k q_2=e+f\ast{i}+g\ast{j}+h\ast{k} 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 \begin{aligned} 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 \end{aligned} 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 = \begin{vmatrix}a&-b&-c&-d\\b&a&-d&c\\c&d&a&-b\\d&-c&b&a\end{vmatrix} \begin{vmatrix}e\\f\\g\\h\end{vmatrix} q1∗q2=∣∣∣∣∣∣∣∣abcd−bad−c−c−dab−dc−ba∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣efgh∣∣∣∣∣∣∣∣
如果矩阵 q 2 = 1 + 0 ∗ i + 0 ∗ j + 0 ∗ k q_2 = 1 + 0 * i + 0 * j + 0 * k q2=1+0∗i+0∗j+0∗k，则 q 1 ∗ q 2 = q 1 q_1 * q_2 = q_1 q1∗q2=q1
四元数与旋转的关系
四元数可以表示三维空间中物体的旋转或姿态，从 q = w + x ∗ i + y ∗ j + z ∗ k q = w + x * i + y * j + z * k q=w+x∗i+y∗j+z∗k很难想象出来，但是如果换成 q = [ cos θ 2 , sin θ 2 ∗ n ^ ] q = [\cos{\frac{\theta}{2}}, \sin{\frac{\theta}{2}} * \hat{n}] q=[cos2θ,sin2θ∗n^]就好理解多了，这个四元数表示一个绕轴 n ^ \hat{n} n^旋转 θ \theta θ度的旋转动作。对一个向量（或者坐标系）进行旋转可以表示为 v ′ = q ∗ v q v^{'}=q^{\ast}vq v′=q∗vq。
二、数学模型
为了用陀螺仪数据求出物体的姿态，我们需要将物理原理抽象成数学模型，然后用数学方法求解。假设惯性坐标系位 E E E，载体坐标系位 b b b，陀螺仪输出的角速度记作 ω E b b \omega_{Eb}^{b} ωEbb，物体在惯性坐标系的角速度记作 ω E b E \omega_{Eb}^{E} ωEbE，通过坐标系旋转 q q q将载体坐标系下的角速度转换为惯性坐标系下的角速度，即 q ∗ ω → E b b q = ω → E b E q^{\ast} \overrightarrow\omega_{Eb}^{b} q = \overrightarrow\omega_{Eb}^{E} q∗ωEbbq=ωEbE。只要求出 q q q就能知道物体相对的姿态变化情况。
1. 四元数微分方程
令 Q = [ cos θ 2 , n ^ sin θ 2 ] Q = [\cos{\frac{\theta}{2}}, \hat{n}\sin{\frac{\theta}{2}}] Q=[cos2θ,n^sin2θ]，则 Q Q Q对时间 t t t的微分方程为：
Q ˙ = − 1 2 sin θ 2 d θ d t + d n ^ d t sin θ 2 + 1 2 n ^ cos θ 2 d θ d t = 1 2 d θ d t ( cos θ 2 n ^ − sin θ 2 ) , because d n ^ d t = 0 = 1 2 n ^ d θ d t ( cos θ 2 + n ^ sin θ 2 ) , because n ^ ∗ n ^ = − 1 = 1 2 n ^ ω → E b E ( cos θ 2 + n ^ sin θ 2 ) = 1 2 n ^ ω → E b E Q = 1 2 n ^ Q ω → E b b , because ω → E b b = Q ∗ ω → E b E Q , Q ∗ Q = 1 = 1 2 ( q 0 + q 1 i + q 2 j + q 3 k ) ( 0 + ω x i + ω y i + ω z k ) \begin{aligned} \dot{Q} & = - \frac{1}{2}\sin{\frac{\theta}{2}} \frac{
{\rm d}\theta}{
{\rm d}t} + \frac{
{\rm d}\hat{n}}{
{\rm d}t} \sin{\frac{\theta}{2}} + \frac{1}{2}\hat{n}\cos{\frac{\theta}{2}}\frac{
{\rm d}\theta}{
{\rm d}t} \\ & = \frac{1}{2}\frac{
{\rm d}\theta}{
{\rm d}t}(\cos{\frac{\theta}{2}}\hat{n}-\sin{\frac{\theta}{2}}),& \text{because }\frac{
{\rm d}\hat{n}}{
{\rm d}t}=0 \\ & = \frac{1}{2}\hat{n}\frac{
{\rm d}\theta}{
{\rm d}t}(\cos{\frac{\theta}{2}}+\hat{n}\sin{\frac{\theta}{2}}), &\text{because }\hat{n}\ast\hat{n}=-1 \\ & = \frac{1}{2}\hat{n}\overrightarrow\omega_{Eb}^{E}(\cos{\frac{\theta}{2}}+\hat{n}\sin{\frac{\theta}{2}}) \\ & = \frac{1}{2}\hat{n}\overrightarrow\omega_{Eb}^{E}Q \\ & = \frac{1}{2}\hat{n}Q\overrightarrow\omega_{Eb}^{b}, & \text{because }\overrightarrow\omega_{Eb}^{b}=Q^{\ast}\overrightarrow\omega_{Eb}^{E}Q,Q^{\ast}Q=1 \\ & = \frac{1}{2}(q_0+q_1i+q_2j+q_3k)(0+\omega_xi+\omega_yi+\omega_zk) \end{aligned} Q˙=−21sin2θdtdθ+dtdn^sin2θ+21n^cos2θdtdθ=21dtdθ(cos2θn^−sin2θ),=21n^dtdθ(cos2θ+n^sin2θ),=21n^ωEbE(cos2θ+n^sin
扫一扫
7074
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
