四元数的小理解

无人的回忆 · 发表于 2014-8-11 21:53:20

本帖最后由无人的回忆于 2014-8-11 21:56 编辑

最近研究了好几天的四元数，不知不觉间也有了自己的一些理解，其实觉得做四轴，不应该直接照搬程序改参数然后PID的（比较明显的例子就是卡尔曼的程序），或者说直接使用MPU6050内部的DMP（说实话内部这个东西算出来的也不是特别的好），而是应该去研究一下程序本身应该是要告诉我们什么东西，解释一下，本人也是菜鸟一名，没有参加TI，所以不为比赛；也没有工作什么，不为金钱什么的，帖子发了，供参加TI的同学们看看，也让自己总结一下，找点儿灵感~
第一次玩四轴的时候也看匿名的程序，第一次知道四元数这个东西，当时就不太清楚说这个四元数是什么意思....通过查找资料慢慢明白说：四元数其实类比于我们的一元N次方程中的元，就是未知数或者说变量的意思。那么说到这里，四元数其实就是四个变量组成的一个数，具体公式为下（l,m,n是瞬时轴的向量，θ为绕瞬时轴的角度）

那么既然是一直在变得量，且跟角度有关，所以我们在定义四元数的时候要定义为全局或者静态变量，以便不停的更新，四元数有一个很重要的特性就是四个数的平方相加为1，即标准化，也就是匿名程序在最后求解角度之前的几条指令的作用。

从匿名的程序上可以清晰看出来，vx，vy，vz的取值刚好为第三行的1 2 3列元素，那么变换的话就是把飞机上的三维坐标X0，Y0，Z0通过一次旋转转到与原Z轴重合，因为后面还有四元数的更新方程，所以这次的旋转也就是相对于原先位置的一次旋转，所以即使后面转换出姿态角，也是一个连续的量，而不是离散的

无人的回忆 · 发表于 2014-8-11 21:56:54

   说过了vx，vy，vz，就是后面的ex，ey，ez了，这里其实就是两个向量的外积，且是用向量的方法表示的，它反映的就是看我算出来的表示vx，vy，vz向量与加速度计算出来的姿态误差，因为在这里vx，vy，vz是单位向量的三轴分量，ax，ay，az也进行了归一化，所以根据向量积的公式就知道这里算出来的就是一个角度，这儿也是为什么加速度计的值要进行滤波的原因，因为这里对他是要信任的。那为什么不直接使用加速度计的滤波值作运算算出姿态呢？其实我也不是很理解这块，既然信任却又不使用，不过加速度计算出来的姿态确实是抖动而且短期不稳定的，可能就是因为这个吧.....

   后面就是一个很简单的PI运算了（当然你也可以用PID，只不过没有太大的必要啦），为什么会有I运算呢？因为加速度计的数据是长期稳定的，所以在长时间的时候要是让我们算出来的数据与加速度计算出来的数据吻合的，所以要有I运算，是不是觉得要是加速度计是稳定的，短期不抖动的就很美好呢？可现实就是这么骨干.....

   最后就是传说中的四元数微分方程了，其实他也没有太不好理解（当然不涉及复杂的理论情况下....），其实就是
上面的用Q表示的四元数方程对时间进行微分，之后经过一系列复杂的运算之后变成了

也就是我们看到的程序里的样子，其中Wx，Wy，Wz都是角速度，而角速度的单位是弧度每秒，所以就有了传递参数时候的参数转换过程，而对于方程最后乘了一个halfT，即采样周期的一半，其实是使用了毕卡算法的一阶近似解

其中Q(tk)是上一时刻算出的四元数，ΔΘ为一个采样周期内的角增量（近似于角速度乘以一个采样周期的时间：W*T），因为微分方程中并没有将角速度除以2，所以就在时间单位上进行除2处理，变成了现在程序的样子了。
   至于最后的角度求解，那就是固定的公示了，没有什么好说的。
   总之在理解整个姿态解算的过程中确实遇到了很多不懂的地方，所以也学到了许多，觉得大学生做东西就应该这样，遇到不会的、不懂的，积极解决，投入时间，绝对是有回报的，写完才觉得自己有些地方理解的也不是很到位，还希望有大神能够指点一番。
   最后补充一句，纯属个人理解，不喜勿喷哈~