飞控陀螺仪，磁力计，加速计，四元数姿态结算

最新推荐文章于 2024-08-02 01:36:54 发布
最新推荐文章于 2024-08-02 01:36:54 发布
阅读量4.4k 收藏 38
点赞数 1
原文链接：https://blog.csdn.net/qq_41422043/article/details/83955983
版权

MPU6050主要包含陀螺仪和加速度计。陀螺仪主要测量角速度，即可以测出某一时间段物体转过的角度。加速度计测量的是物体的加速度，重力加速度即物体受重力作用的情况下具有的加速度，物体静止时，加速度计测出来的加速度等于重力加速度g，约等于9.8米每平方秒，重力加速度总是竖直向下，通过计算重力加速度在X,Y的分量，则可以计算出物体相对于水平面的倾斜角度。

我们先来看看如何用欧拉角描述旋转(坐标变换)：

绕Z轴旋转即滚转角，绕Y轴旋转即偏航角，绕X旋转即俯仰角。

这里需要注意绕X,Y,Z轴方向旋转的先后顺序不一样，得出余弦矩阵的顺序（也就是公式）也不一样。并且在一组旋转里X,Y,Z不可交换(Euler旋转)，所以有先后顺序之分（程序中）。旋转变换有12种表示方式，分别为：XYZ、XZY、XYX、XZX、YXZ、YZX、YXY、YZY、ZXY、ZYX、ZXZ、ZYZ。这里还有一点需要注意，那就是"Gimbal Lock"。图中（ZYX，以下均为此顺序）的绕Z旋转会导致YX轴变化，但是Y影响X不会影响Z，X不会影响ZY。因此，如果旋转Y±90°，那么Z轴方向不变，但是会改变X轴方向，导致YX轴同向。此时保持Y值不变，那么改变Z或者X的效果相同。

Gimbal Lock总结就是，其源自Euler旋转原理，此原理旋转变量不可交换，所以有先后之分，所以可以改变后两个轴而第一轴方向不变，所以产生轴共线，即Gimbal Lock。

上图中得出旋转后的欧拉角公式，但是无人机的姿态结算中不能用欧拉角公式计算，一方面是因为欧拉角微分方程中包含了大量的三角运算，这给实时结算带来了一定的困难。而且当俯仰角为90度时方程式会出现神奇的“Gimbal Lock”。所以欧拉角方法只适合水平姿态变化不大的情况，而不适用于飞行器的姿态确定。

下面对四元数姿态进行结算：

  1. 重力加速度归一化。即将加速度计的三维向量（ax,ay,az）转换为单位向量，因为是单位矢量到参考性的投影，所以要把加速度计数据单位化。归一化只是改变这三个向量的长度，方向不改变，也就是只改变了相同的倍数，只是为了与单位四元数对应。ax,ay,az是机体坐标参照系上，加速度测出来的重力向量。
  2. 四元数换成方向余弦中的第三行的三个元素。将当前姿态重力在三个轴的分量分离出来，把四元数换算成方向余弦中的第三行的三个元素(vx,vy,vz)。惯性测量器件测量的都是关于b系的值。vx,vy,vz是陀螺仪的值积分后的姿态来推算出的重力向量。
  3. 向量叉积得出姿态误差。将ax,ay,az和vx,vy,vz对应的进行向量叉积（向量外积、叉乘）。分别得出ex,ey,ez。这个叉积向量仍然是位于机体坐标系，并和积分误差成正比，正好矫正陀螺。
  4. 对误差进行积分。将ax,ay,az和vx,vy,vz的误差进行积分消除误差。
  5. 进行滤波。陀螺仪测的值不断的进行更新，相应的积分误差也不断的修正，最后将积分误差反馈给陀螺仪，修正陀螺仪的值。
  6. 四元数微分方程。通过一阶龙格库塔法更新四元数。
  7. 四元数归一化。对四元数的单位化，单位化的四元数可以表示一个完整的旋转，只有单位四元数才可以表示旋转，这就是四元数表示旋转的约束条件。
  8. 四元数转欧拉角。

程序如下：


 
 
  1. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     float Kp = 
     
     0.4f; 
     
     // 比例增益
    
    
    
   
   
  2. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     float Ki = 
     
     0.001f; 
     
     // 积分增益
    
    
    
   
   
  3. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     float exInt = 
     
     0.f; 
     
     // 重力在X轴上的分量
    
    
    
   
   
  4. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     float eyInt = 
     
     0.0f;
    
    
    
   
   
  5. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     float	ezInt = 
     
     0.0f;
    
    
    
   
   
  6. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
     
    
    
    
   
   
  7. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     //四元数
    
    
    
   
   
  8. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     static 
     
     float q0 = 
     
     1.0f;
    
    
    
   
   
  9. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     static 
     
     float q1 = 
     
     0.0f;
    
    
    
   
   
  10. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     static 
     
     float q2 = 
     
     0.0f;
    
    
    
   
   
  11. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     static 
     
     float q3 = 
     
     0.0f;
    
    
    
   
   
  12. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
     
    
    
    
   
   
  13. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     float Rot_matrix[
     
     3][
     
     3];
    
    
    
   
   
  14. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     Vector3f angle1;
    
    
    
   
   
  15. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  16. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     void	IMU_Updata(Vector3f acc,Vector3f gyro,Attitude *state,float dt)
    
    
    
   
   
  17. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     {
    
    
    
   
   
  18. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  19. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     float norm;
    
    
    
   
   
  20. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     float ex,ey,ez;
    
    
    
   
   
  21. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     float halfT	= dt/
     
     2.0f;
    
    
    
   
   
  22. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     //重力加速度在X,Y,Z方向的分量
    
    
    
   
   
  23. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     static 
     
     float verxZ,veryZ,verzZ;
    
    
    
   
   
  24. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         angle1.x = 
     
     0.0f;
    
    
    
   
   
  25. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         angle1.y = 
     
     0.0f;
    
    
    
   
   
  26. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     float q0q0,q0q1,q0q2,q0q3,q1q1,q1q2,q1q3,q2q2,q2q3,q3q3;
    
    
    
   
   
  27. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  28. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     //1、获取原始值
    
    
    
   
   
  29. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     //角速度转弧速度
    
    
    
   
   
  30. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         gyro.x = gyro.x	* DEG2RAD;
    
    
    
   
   
  31. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         gyro.y = gyro.y	* DEG2RAD;
    
    
    
   
   
  32. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         gyro.z = gyro.z	* DEG2RAD;
    
    
    
   
   
  33. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  34. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     //2、加速度计量化
    
    
    
   
   
  35. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     if((acc.x!=
     
     0.0f)||(acc.y!=
     
     0.0f)||(acc.z!=
     
     0.0f))
    
    
    
   
   
  36. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         {
    
    
    
   
   
  37. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	norm =	sqrtf3(acc.x , acc.y , acc.z);
    
    
    
   
   
  38. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	acc.x = acc.x /norm ;
    
    
    
   
   
  39. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	acc.y = acc.y /norm ;
    
    
    
   
   
  40. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	acc.z = acc.z /norm ;
    
    
    
   
   
  41. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    		
    
    
    
   
   
  42. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    		
    
    
    
   
   
  43. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     //3、叉乘
    
    
    
   
   
  44. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	ex = (acc.y * verzZ) - (veryZ * acc.z);
    
    
    
   
   
  45. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	ey = (acc.x * verzZ) - (verxZ * acc.z);
    
    
    
   
   
  46. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	ez = (acc.x * veryZ) - (verxZ * acc.y);
    
    
    
   
   
  47. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    		
    
    
    
   
   
  48. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     //4、积分误差滤波
    
    
    
   
   
  49. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	gyro.x += ex	* Kp + exInt;
    
    
    
   
   
  50. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	gyro.y += ey	* Kp + eyInt;
    
    
    
   
   
  51. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	gyro.z += ez	* Kp + ezInt;	
    
    
    
   
   
  52. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    		
    
    
    
   
   
  53. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     }
    
    
    
   
   
  54. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  55. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     float q0_last =q0;
    
    
    
   
   
  56. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     float q1_last =q1;
    
    
    
   
   
  57. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     float q2_last =q2;
    
    
    
   
   
  58. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     float q3_last =q3;
    
    
    
   
   
  59. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  60. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     //解四元微分方程
    
    
    
   
   
  61. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q0 += (-q1_last*gyro.x - q2_last * gyro.y - q3_last *gyro.z)*halfT;
    
    
    
   
   
  62. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q1 += (	q0_last*gyro.x + q3_last * gyro.y - q2_last *gyro.z)*halfT;
    
    
    
   
   
  63. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q2 += (-q3_last*gyro.x + q0_last * gyro.y + q1_last *gyro.z)*halfT;
    
    
    
   
   
  64. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q3 += (	q2_last*gyro.x - q1_last * gyro.y + q0_last *gyro.z)*halfT;
    
    
    
   
   
  65. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
     
    
    
    
   
   
  66. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         norm = sqrtf4(q0,q1,q2,q3);
    
    
    
   
   
  67. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q0 = q0/norm;
    
    
    
   
   
  68. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q1 = q1/norm;
    
    
    
   
   
  69. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q2 = q2/norm;
    
    
    
   
   
  70. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q3 = q3/norm;
    
    
    
   
   
  71. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  72. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  73. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q0q0 = q0 * q0;
    
    
    
   
   
  74. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q0q1 = q0 * q1;
    
    
    
   
   
  75. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q0q2 = q0 * q2;
    
    
    
   
   
  76. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q0q3 = q0 * q3;
    
    
    
   
   
  77. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q1q1 = q1 * q1;
    
    
    
   
   
  78. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q1q2 = q1 * q2;
    
    
    
   
   
  79. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q1q3 = q1 * q3;
    
    
    
   
   
  80. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q2q2 = q2 * q2;
    
    
    
   
   
  81. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q2q3 = q2 * q3;
    
    
    
   
   
  82. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q3q3 = q3 * q3;
    
    
    
   
   
  83. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  84. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         verxZ = 
     
     2.0f * (q1q3 - q0q2); 
    
    
    
   
   
  85. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         veryZ = 
     
     2.0f * (q2q3 + q0q1);
    
    
    
   
   
  86. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         verzZ = q0q0 - q1q1 - q2q2 + q3q3;
    
    
    
   
   
  87. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  88. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     0][
     
     0] = q0q0 + q1q1 - q2q2 - q3q3;
    
    
    
   
   
  89. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     0][
     
     1] = 
     
     2.0f * (q1q2 - q0q3);
    
    
    
   
   
  90. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     0][
     
     2] = 
     
     2.0f * (q1q3 + q0q2);
    
    
    
   
   
  91. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     1][
     
     0] = 
     
     2.0f * (q1q2 + q0q3);
    
    
    
   
   
  92. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     1][
     
     1] = q0q0 - q1q1 + q2q2 - q3q3;
    
    
    
   
   
  93. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     1][
     
     2] = 
     
     2.0f * (q2q3 - q0q1);
    
    
    
   
   
  94. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     2][
     
     0] = 
     
     2.0f * (q1q3 - q0q2);
    
    
    
   
   
  95. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     2][
     
     1] = 
     
     2.0f * (q2q3 + q0q1);
    
    
    
   
   
  96. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     2][
     
     2] = q0q0 - q1q1 - q2q2 + q3q3;
    
    
    
   
   
  97. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  98. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     //四元数转换为欧拉角输出
    
    
    
   
   
  99. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         state->pitch = 
     
     asin(Rot_matrix[
     
     1][
     
     2]);
    
    
    
   
   
  100. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         state->roll	 = 
     
     atan2(-Rot_matrix[
     
     0][
     
     2],Rot_matrix[
     
     2][
     
     2]);
    
    
    
   
   
  101. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         state->yaw   = 
     
     atan2(-Rot_matrix[
     
     1][
     
     0], Rot_matrix[
     
     1][
     
     1]);
    
    
    
   
   
  102. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  103. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     }

运算宏定义如下：


 
 
  1. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     #define MM2M 			0.001f
    
    
    
   
   
  2. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     #define DEG2RAD 	0.017453292519943295769236907684886f	//角度转弧度
    
    
    
   
   
  3. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     #define RAD2DEG 	57.295779513082320876798154814105f	//弧度转角度
    
    
    
   
   
  4. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  5. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     #define sq(a)		((a) * (a))	//平方、
    
    
    
   
   
  6. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     #define	sqrtf4(a,b,c,d)	__sqrtf(sq(a) + sq(b) + sq(c) + sq(d))
    
    
    
   
   
  7. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     #define sqrtf3(a,b,c)	__sqrtf(sq(a) + sq(b) + sq(c))
    
    
    
   
   
  8. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     #define	sqrtf2(a,b)	__sqrtf(sq(a) + sq(b))

在加速度计矫正的基础上+磁力计：
在三维空间中，根据重力加速度，加速度为我们提供一个水平位置的参考，但是无法获得方向的参考，这时就需要磁力计，它能给人们提供一个正北方向的绝对参考。

在上述姿态结算的基础上进行磁力计矫正：

  1. 把磁力计的数据进行归一化处理（进行量化）。
  2. 根据当前四元数姿态值估算各重力分量vx,vy,vz，再根据vx,vy,vz预估磁场的方向wx,wy,wz。
  3. 再根据wx,wy,wz对磁力计测出的值进行误差矫正。
  4. 把加速度计和磁力计修正后的陀螺仪数据整合到四元数中。
  5. 最后进行角度运算。

 
 
  1. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     float KpDef = 
     
     0.5f;
    
    
    
   
   
  2. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     float KiDef	=	
     
     0.025f;
    
    
    
   
   
  3. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
     
    
    
    
   
   
  4. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     float q0 = 
     
     1.0f,q1 = 
     
     0.0f,q2 = 
     
     0.0f,q3 = 
     
     0.0f;
    
    
    
   
   
  5. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
     
    
    
    
   
   
  6. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     float integralFBx = 
     
     0.0f,integralFBy = 
     
     0.0f,integralFBz = 
     
     0.0f;
    
    
    
   
   
  7. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
     
    
    
    
   
   
  8. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     static 
     
     float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
    
    
    
   
   
  9. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
     
    
    
    
   
   
  10. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     static 
     
     float Rot_matrix[
     
     3][
     
     3];	
     
     //方向余弦矩阵
    
    
    
   
   
  11. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
     
    
    
    
   
   
  12. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     //使用加速度传感器测量重力向量，使用磁力计测量方向
    
    
    
   
   
  13. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     void AHRS_Uptate(Vector3f acc,Vector3f gyro,Vector3i mag,Attitude *state,float dt)
    
    
    
   
   
  14. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     {
    
    
    
   
   
  15. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
     
    
    
    
   
   
  16. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q0q0 = q0 * q0;
    
    
    
   
   
  17. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q0q1 = q0 * q1;
    
    
    
   
   
  18. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q0q2 = q0 * q2;
    
    
    
   
   
  19. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q0q3 = q0 * q3;
    
    
    
   
   
  20. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q1q1 = q1 * q1;
    
    
    
   
   
  21. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q1q2 = q1 * q2;
    
    
    
   
   
  22. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q1q3 = q1 * q3;
    
    
    
   
   
  23. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q2q2 = q2 * q2;
    
    
    
   
   
  24. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q2q3 = q2 * q3;
    
    
    
   
   
  25. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         q3q3 = q3 * q3;
    
    
    
   
   
  26. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     //大地坐标系(e)  --> 机体坐标系(b)
    
    
    
   
   
  27. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     0][
     
     0] = q0q0 + q1q1 - q2q2 - q3q3;
    
    
    
   
   
  28. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     0][
     
     1] = 
     
     2.f * (q1q2 + q0q3);
    
    
    
   
   
  29. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     0][
     
     2] = 
     
     2.f * (q1q3 - q0q2);
    
    
    
   
   
  30. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     1][
     
     0] = 
     
     2.f * (q1q2 - q0q3);
    
    
    
   
   
  31. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     1][
     
     1] = q0q0 - q1q1 + q2q2 - q3q3;
    
    
    
   
   
  32. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     1][
     
     2] = 
     
     2.f * (q2q3 + q0q1);
    
    
    
   
   
  33. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     2][
     
     0] = 
     
     2.f * (q1q3 + q0q2);
    
    
    
   
   
  34. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     2][
     
     1] = 
     
     2.f * (q2q3 - q0q1);
    
    
    
   
   
  35. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         Rot_matrix[
     
     2][
     
     2] = q0q0 - q1q1 - q2q2 + q3q3;
    
    
    
   
   
  36. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  37. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     float ex = 
     
     0.0f, ey = 
     
     0.0f, ez = 
     
     0.0f;
    
    
    
   
   
  38. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     float norm;
    
    
    
   
   
  39. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  40. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
        
     
     if((mag.x != 
     
     0) || (mag.y != 
     
     0) || (mag.z !=
     
     0))
    
    
    
   
   
  41. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         {
    
    
    
   
   
  42. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     //磁力计量化
    
    
    
   
   
  43. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	norm = sqrtf3(mag.x,mag.y,mag.z);
    
    
    
   
   
  44. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	mag.x /= norm;
    
    
    
   
   
  45. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	mag.y /= norm;
    
    
    
   
   
  46. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	mag.z /= norm;
    
    
    
   
   
  47. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    		
    
    
    
   
   
  48. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     //地球磁场的参考方向
    
    
    
   
   
  49. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     float hx = Rot_matrix[
     
     0][
     
     0] * mag.x + Rot_matrix[
     
     1][
     
     0] * mag.y + Rot_matrix[
     
     2][
     
     0] * mag.z ;
     
     //计算出x轴的方向
    
    
    
   
   
  50. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     float hy = Rot_matrix[
     
     0][
     
     1] * mag.x + Rot_matrix[
     
     1][
     
     1] * mag.y + Rot_matrix[
     
     2][
     
     1]	* mag.z	;
     
     //计算出Y轴的方向
    
    
    
   
   
  51. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     float by = sqrtf2(hx,hy);	
     
     //预估磁场的方向
    
    
    
   
   
  52. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     float bz = Rot_matrix[
     
     0][
     
     2] * mag.x + Rot_matrix[
     
     1][
     
     2] * mag.y + Rot_matrix[
     
     2][
     
     2] * mag.z	;
     
     //计算出Z轴方向
    
    
    
   
   
  53. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    		
    
    
    
   
   
  54. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     //估计磁场的方向
    
    
    
   
   
  55. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     float wx = Rot_matrix[
     
     0][
     
     1] *	by + Rot_matrix[
     
     0][
     
     2] * bz;
    
    
    
   
   
  56. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     float wy = Rot_matrix[
     
     1][
     
     1]	* by + Rot_matrix[
     
     1][
     
     2] * bz;
    
    
    
   
   
  57. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     float wz = Rot_matrix[
     
     2][
     
     1]	* by + Rot_matrix[
     
     2][
     
     2]	*	bz;
    
    
    
   
   
  58. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    		
    
    
    
   
   
  59. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     //误差是估计方向和场矢量测量方向的乘积
    
    
    
   
   
  60. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	ex = mag.y * wz - mag.z * wy;
    
    
    
   
   
  61. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	ey = mag.z * wx - mag.x * wz;
    
    
    
   
   
  62. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	ez = mag.x * wz - mag.z * wx;
    
    
    
   
   
  63. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    		
    
    
    
   
   
  64. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     if((acc.x !=
     
     0)||(acc.y !=
     
     0)||(acc.z !=
     
     0))
    
    
    
   
   
  65. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	{
    
    
    
   
   
  66. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	 
     
     //加速度值量化
    
    
    
   
   
  67. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	norm = sqrtf3(acc.x,acc.y,acc.z);
    
    
    
   
   
  68. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	acc.x /= norm;
    
    
    
   
   
  69. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	acc.y	/= norm;
    
    
    
   
   
  70. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	acc.z	/= norm;
    
    
    
   
   
  71. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    			
    
    
    
   
   
  72. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     //根据当前四元数的姿态值估算出各重力分量Vx,Vy,Vz
    
    
    
   
   
  73. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     float vx = Rot_matrix[
     
     0][
     
     2];
    
    
    
   
   
  74. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     float vy = Rot_matrix[
     
     1][
     
     2];
    
    
    
   
   
  75. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     float vz = Rot_matrix[
     
     2][
     
     2];
    
    
    
   
   
  76. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    			
    
    
    
   
   
  77. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     //使用叉积来计算重力误差
    
    
    
   
   
  78. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	ex += acc.y * vz - acc.z * vy;
    
    
    
   
   
  79. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	ey += acc.z * vx - acc.x * vz;
    
    
    
   
   
  80. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	ez += acc.x * vy - acc.y * vx;
    
    
    
   
   
  81. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	}
    
    
    
   
   
  82. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    		
    
    
    
   
   
  83. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
            
     
     //只有当从加速度计或磁力计收集有效数据时才应用反馈
    
    
    
   
   
  84. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     if(ex != 
     
     0.0f && ey != 
     
     0.0f && ez !=
     
     0.0f)
    
    
    
   
   
  85. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	{
    
    
    
   
   
  86. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     //把上述计算得到的重力和磁力差进行积分运算
    
    
    
   
   
  87. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     if(KiDef > 
     
     0.0f)
    
    
    
   
   
  88. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	{
    
    
    
   
   
  89. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	    integralFBx += KiDef * ex * dt;
    
    
    
   
   
  90. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	    integralFBy += KiDef * ey *	dt;
    
    
    
   
   
  91. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	    integralFBz += KiDef * ez *	dt;
    
    
    
   
   
  92. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    				
    
    
    
   
   
  93. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	    gyro.x += integralFBx;
    
    
    
   
   
  94. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	    gyro.y += integralFBy;
    
    
    
   
   
  95. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	    gyro.z += integralFBz;
    
    
    
   
   
  96. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	}
    
    
    
   
   
  97. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     else
    
    
    
   
   
  98. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	{
    
    
    
   
   
  99. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	    integralFBx = 
     
     0.0f;
    
    
    
   
   
  100. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	    integralFBy = 
     
     0.0f;
    
    
    
   
   
  101. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	    integralFBz = 
     
     0.0f;
    
    
    
   
   
  102. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	}
    
    
    
   
   
  103. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     //把上述计算得到的重力差和磁力差进行比例运算
    
    
    
   
   
  104. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    			
    
    
    
   
   
  105. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	    gyro.x += KpDef * ex;
    
    
    
   
   
  106. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	    gyro.y += KpDef * ey;
    
    
    
   
   
  107. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	    gyro.z += KpDef *	ez;
    
    
    
   
   
  108. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	}
    
    
    
   
   
  109. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     float dq0 = 
     
     0.5f*(-q1 * gyro.x - q2 * gyro.y - q3 * gyro.z);
    
    
    
   
   
  110. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     float dq1 = 
     
     0.5f*( q0 * gyro.x + q2 * gyro.z - q3 * gyro.y);
    
    
    
   
   
  111. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     float dq2 = 
     
     0.5f*( q0 * gyro.y - q1 * gyro.z + q3 * gyro.x);
    
    
    
   
   
  112. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     float dq3 = 
     
     0.5f*( q0 * gyro.z + q1 * gyro.y - q2 * gyro.x); 
    
    
    
   
   
  113. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  114. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	q0 += dt * dq0;
    
    
    
   
   
  115. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	q1 += dt * dq1;
    
    
    
   
   
  116. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	q2 += dt * dq2;
    
    
    
   
   
  117. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	q3 += dt * dq3;
    
    
    
   
   
  118. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    		
    
    
    
   
   
  119. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	norm = sqrtf4(q0, q1, q2, q3);
    
    
    
   
   
  120. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	q0 /= norm;
    
    
    
   
   
  121. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	q1 /= norm;
    
    
    
   
   
  122. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	q2 /= norm;
    
    
    
   
   
  123. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	q3 /= norm;		
    
    
    
   
   
  124. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    		
    
    
    
   
   
  125. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	Rot_matrix[
     
     0][
     
     0] = q0q0 + q1q1 - q2q2 - q3q3;
    
    
    
   
   
  126. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	Rot_matrix[
     
     0][
     
     1] = 
     
     2.f * (q1q2 + q0q3);
    
    
    
   
   
  127. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	Rot_matrix[
     
     0][
     
     2] = 
     
     2.f * (q1q3 - q0q2);
    
    
    
   
   
  128. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	Rot_matrix[
     
     1][
     
     0] = 
     
     2.f * (q1q2 - q0q3);
    
    
    
   
   
  129. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	Rot_matrix[
     
     1][
     
     1] = q0q0 - q1q1 + q2q2 - q3q3;
    
    
    
   
   
  130. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	Rot_matrix[
     
     1][
     
     2] = 
     
     2.f * (q2q3 + q0q1);
    
    
    
   
   
  131. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	Rot_matrix[
     
     2][
     
     0] = 
     
     2.f * (q1q3 + q0q2);
    
    
    
   
   
  132. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	Rot_matrix[
     
     2][
     
     1] = 
     
     2.f * (q2q3 - q0q1);
    
    
    
   
   
  133. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	Rot_matrix[
     
     2][
     
     2] = q0q0 - q1q1 - q2q2 + q3q3;
    
    
    
   
   
  134. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    		
    
    
    
   
   
  135. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
     
     //四元数转换为欧拉角输出
    
    
    
   
   
  136. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	state->pitch = 
     
     asin(Rot_matrix[
     
     1][
     
     2]);
    
    
    
   
   
  137. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	state->roll	 = 
     
     atan2(-Rot_matrix[
     
     0][
     
     2],Rot_matrix[
     
     2][
     
     2]);
    
    
    
   
   
  138. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     	state->yaw   = 
     
     atan2(-Rot_matrix[
     
     1][
     
     0], Rot_matrix[
     
     1][
     
     1]);	
    
    
    
   
   
  139. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
         }
    
    
    
   
   
  140. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     }

运算宏定义如下：


 
 
  1. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     #define MM2M 		0.001f
    
    
    
   
   
  2. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     #define DEG2RAD 	0.017453292519943295769236907684886f	// 角度转弧度
    
    
    
   
   
  3. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     #define RAD2DEG 	57.295779513082320876798154814105f	// 弧度转角度 
    
    
    
   
   
  4. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    	
    
    
    
   
   
  5. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     #define sq(a)		((a) * (a))	//平方、
    
    
    
   
   
  6. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     #define	sqrtf4(a,b,c,d)	__sqrtf(sq(a) + sq(b) + sq(c) + sq(d))
    
    
    
   
   
  7. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     #define sqrtf3(a,b,c)	__sqrtf(sq(a) + sq(b) + sq(c))
    
    
    
   
   
  8. 
   
   
    
    
    
    
   
   
    
   
   
    
    
    
    
     
     #define	sqrtf2(a,b)	__sqrtf(sq(a) + sq(b))

以上仅为本人的理解，仅供参考，如有细节上的错误希望大神指点指点！

吃好甜
关注
无人机姿态解算：四元数及其与欧拉角的转换
w1050321758的博客
09-28 9818
四元数在无人机姿态解算中的应用与实现 **引言：**获得无人机飞行时的飞行姿态对于无人机稳定控制来说至关重要。无人机主要通过传感器数据融合来进行状态估计，常用于无人机的传感器包括：MPU（包含了三轴加速度计和三轴陀螺仪，9250还包括磁力计），气压计，超声波传感器，光流计等。通过传感器数据融合算法可以得到无人机的最优状态估计值，包括无人机的姿态角，俯仰pitch、横滚、偏航角等。通过传感器的数据来求得无人机姿态角的信息的过程就是姿态解算，本文详细推导了利用四元数与四阶龙格库塔法求解四元数微分方程得到姿态角，
惯性导航姿态结算
06-16
为了得到准确的惯性器件原始输出信号，介绍了MEMS加速度计、陀螺仪、 磁力计的工作原理，并针对加速度计原始数据中含脉冲噪声干扰的特点，对其原始数据 进行了中值滤波处理，针对陀螺仪原始数据中含高频噪声的特点，对其原始数据进行了 均值滤波处理，针对磁力计原始数据中含随机噪声比较多的问题，对其原始数据进行了 Butterworth滤波处理。实验表明，对传感器数据进行预处理达到了预想的去噪效果，为 后续进一步处理打好了基础。
姿态解算学习笔记
riverandlove的博客
08-02 173
近些天使用四元素姿态解算时发现对姿态解算理解不够深入，学习较为困难。故做此篇帮助小白入门姿态解算。由不同处理方式，分成不同种类姿态解算。只有欧拉角方法存在万向节锁。
stm32 MPU6050四元数姿态结算程序
12-28
STM32F103B + MPU6050+四元数解算，采用IO模拟IIC接口读取数据，
三轴加速度计解算姿态（四元数）
Hugh_W的博客
03-30 3323
三轴加速度计解算载体姿态（四元数形式）
飞控学习笔记-姿态角解算（MPU6050 加速度计加陀螺仪）
热门推荐
yunddun的博客
09-12 4万+
全球首例整合性6轴运动处理组件 3轴陀螺仪＋3轴加速度计 IIC接口，可用于扩展外部传感器 数据格式：旋转矩阵、四元数(quaternion)、欧拉角格式 体积小 无人机传感器 MPU6050:3轴陀螺仪＋3轴加速度（姿态） HMC5883:3轴数字罗盘（精确方向控制） MS5611:气压计（高度计） Pitch:俯仰角（抬头低头） Roll:滚转角（翻身） Yaw：偏航角（转弯） 三个角度组合，可唯一确定一个姿态 ...
三轴陀螺仪解算姿态（四元数）
Hugh_W的博客
03-30 4475
三轴陀螺仪解算姿态（四元数形式）
三轴陀螺仪的四元数解算方法——详细分析
m0_68116022的博客
06-18 2536
为了解决欧拉角在解算角度时出现的“万向锁”现象，本文分析四元数在三轴陀螺仪解算方面的应用。
如何利用陀螺仪和加速度计的数据在四轴飞行器中实现基本的姿态稳定？请结合四元数和欧拉角的转换。
最新发布
11-01
此外，文档还会介绍如何利用磁力计数据进行校准以提高姿态估计的准确性，虽然在当前环境中磁力计数据是无效的，但这是在其他情况下可能需要考虑的重要因素。 参考资源链接：[正点原子飞控算法解析：角速度与姿态...
TI杯参赛者福利—四轴飞控板姿态解算（原理图、源代码开源）-电路方案
04-22
3、加速度计和陀螺仪采用MPU6050； 4、磁力计采用HMC5883L； 5、添加了四个LED灯，两个按键和一个蜂鸣器； 6、集成了CP2102 USB转TTL芯片； 7、支持BSL/SBW/JTAG下载，BSL用串口下载就行，但是不能仿真~ 代码说明: 1...
在STM32飞控系统中，如何利用四元数算法来提高姿态解算的准确性和效率？请结合《STM32飞控DIY教程：四元数算法详解》进行详细说明。
10-30
在实际应用中，四元数算法需要结合加速度计、陀螺仪和磁力计等多种传感器数据。STM32微控制器具备处理这些传感器数据的能力，但需要通过合适的算法来融合它们。《STM32飞控DIY教程：四元数算法详解》提供了一个完整...
基于四元数解算陀螺仪姿态角算法的实现
06-18
基于四元数的姿态解算方法能够有效结合陀螺以及加速 度计的误差特性， 将运动场以及重力加速度两个互不相干的 物理矢量进行互补融合。主要利用陀螺仪测量的角速度作为 四元数的更新， 以重力加速度作为四元数的观测， 通过8 位 微处理器实时解算姿态角。 基于四元数的解算方法， 利用叉乘有效地把三轴陀螺以 及三轴加速度计的数据进行融合， 使得测量的俯仰角、横滚 角逼近真角度， 经过试验验证了该算法的有效性， 且计算量 少， 在姿态控制领域有这良好的应用前景。
STM32飞控DIY教程：四元数算法详解
飞控系统是飞行器的核心部分，负责收集各种传感器数据（如陀螺仪、加速度计、磁力计等），通过算法计算控制指令，驱动飞行器的姿态调整和动力控制。飞控系统对于保证飞行器稳定飞行和执行复杂的飞行任务至关重要。 ...
Python处理加速度和陀螺仪数据计算姿态角
Forter_J的博客
12-27 1万+
通过x、y、z加速度和陀螺仪计算姿态角（欧拉角） #coding:utf-8 import math #IMU算法更新 Kp = 100 #比例增益控制加速度计/磁强计的收敛速度 Ki = 0.002 #积分增益控制陀螺偏差的收敛速度 halfT = 0.001 #采样周期的一半 #传感器框架相对于辅助框架的四元数(初始化四元数的值) q0 = 1 q1 = 0 q2 = 0 q3 = ...
MPU6050姿态结算（采用硬件I2C实现数据融合算法）
wearlee的博客
04-16 5323
姿态角（Euler角）pitch yaw roll 飞行器的姿态角并不是指哪个角度，是三个角度的统称。 它们是：俯仰、滚转、偏航。你可以想象是飞机围绕XYZ三个轴分别转动形成的夹角。 地面坐标系（earth-surface inertial reference frame）Sg--------OXgYgZg   ①在地面上选一点Og ②使Xg轴在水平面内并指向某一方向 ③Zg轴垂
【飞控入门基础】 四元数
qq_37451250的博客
08-01 1764
四元数 四元数（Quaternions）是由威廉·卢云·哈密顿在1843年爱尔兰发现的数学概念。四元数的乘法不符合交换律。如果把四元数的集合考虑成多维实数空间的话，四元数就代表着一个四维空间，相对于复数就是二维空间。四元数也称为超复数。四元数形式为： 其中a为实部，b , c , d为虚部，i , j , k为虚数单位。 i , j , k三者的关系为： 1.向量的叉乘 2...
加速度计和陀螺仪指南——数学模型和基本算法
工大机器人工作室
12-17 3837
本帖转自 http://www.geek-workshop.com/thread-1695-1-1.html 本帖翻译自 IMU（加速度计和陀螺仪设备）在嵌入式应用中使用的指南。 这篇文章主要介绍加速度计和陀螺仪的数学模型和基本算法，以及如何融合这两者，侧重算法、思想的讨论.介绍 本指南旨在向兴趣者介绍惯性 MEMS（微机电系统）传感器，特别是加速度计和陀螺仪以及其他整合 IMU（惯性测量...
陀螺仪及四元数解算
zb313982521的博客
01-10 1179
陀螺仪 四元数
四轴飞行器Mahony姿态结算
爱吃炸鸡的小猪's blog
03-06 1432
首先我们要搞明白姿态结算在飞控中的位置： 简而言之就是将采集的数据通过算法转换为可以供姿态控制使用的欧拉角，较为精准的数据 转换思路： 在计算的时候我们一般将欧拉角（当然也可以叫做方向角）转换成四元数来计算：飞行的姿态可以用欧拉角描述的方向余弦矩阵来表示，因为欧拉角存在万向节“死锁”的问题，所以不适合全姿态飞行器，飞行的姿态也可以用四元数描述的方向余弦矩阵来表示。两者的方向余弦矩阵数据一一对应。所以我们将四元数作为中间量进行运算，最后的结果转换为欧拉角，这样就可以解决“死锁......
评论 2
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值