欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

互补滤波法补充

程序员文章站 2024-03-25 22:06:46
...

四旋翼姿态解算——互补滤波法补充(融合磁力计)

2017年03月01日 16:18:06 阅读数:6614 标签: 算法数据飞控数学 更多

个人分类: STM32四旋翼无人机

版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/hongbin_xu/article/details/59110226

转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 
文章链接:http://blog.csdn.net/hongbin_xu/article/details/59110226 或 http://hongbin96.com/119

上一次的博文中(点我打开)介绍了互补滤波法的算法流程和程序实现,但是仅仅只是融合了三轴加速度计和三轴陀螺仪的数据解算出姿态。

由于机体水平时,加速度计无法测量绕 Z 轴的旋转量,即偏航角yaw,并且磁力计也同样无法测得z轴的旋转量。

所以,我们考虑使用加速度计和磁力计同时对陀螺仪进行校正。这次介绍如何融合磁力计的数据进行姿态解算。 
在介绍算法之前,想再提一点自己的感想。很多新手入门姿态解算算法时,最先接触的就是这个Mahony的互补滤波算法,也相对简单一些。有的人觉得数学推导很麻烦,也不愿意花时间研究代码,懵懵懂懂地把程序复制粘贴过去就随便填了几个参数,然后就去看现象。其实大家都知道这种方法是不对的吧,没出问题还好,一旦出了错就不知所措了。很多时候,飞控出了问题,不一定是控制的问题,有时候问题出在姿态解算上(“血与泪”的教训,我当初就是在这个地方被坑了很久)。得到了姿态角后,先看看波形,看看数据怎么样,滤波器有没有用。(不想自己写程序和调试的,请无视这些。) 
这里再推荐一个很实用的工具:匿名上位机。功能很强大,可以自己写程序上传数据,在上位机上看波形,一切OK之后再进行下一步的工作。 
这是匿名上位机的界面的截图,可以到匿名科创的官网上下载: 
互补滤波法补充

好了,开始介绍算法部分吧! 
还是按照这张框图得流程来介绍: 
互补滤波法补充
首先,假设磁力计测量得到的三轴磁场数据为互补滤波法补充 。 
为方便运算,像此前一样,将数据归一化之后再进行运算。得到载体坐标系b下的标准磁场数据互补滤波法补充,其中互补滤波法补充。为表示简便,将归一化之后的数据还是表示为互补滤波法补充。 
将这组载体坐标系b下测得的数据互补滤波法补充旋转到导航坐标系n下,得到互补滤波法补充。 
坐标系的旋转相当于乘以一个旋转矩阵互补滤波法补充 。 
使用以前推导得到的结论,其为四元数表示的旋转矩阵互补滤波法补充: 
互补滤波法补充 
得到: 
互补滤波法补充 
假定理想情况下,让导航坐标系n中x轴指向正北方向,并设这个新的导航坐标系下磁力计数据为互补滤波法补充。那么根据高中物理知识分析可以知道,Z轴方向分量(即重力方向分量)相等,水平方向分量相等。 则容易得到: 
互补滤波法补充 
若取y轴指向正北方向,则: 
互补滤波法补充 
取x轴或y轴朝向正北,按照自己情况定义。我在程序中取的是x轴,所以后面使用x轴朝正北方向的结果来推导。 
将磁力线在导航坐标系n中的理想输出互补滤波法补充 再次旋转到载体坐标系b中,得到在b系中的理想输出: 
互补滤波法补充 
将理想输出互补滤波法补充和原始输出互补滤波法补充 作叉积得到误差e(w),将其作为补偿项送给陀螺仪进行校正: 
互补滤波法补充 
表示成矩阵形式: 
互补滤波法补充 
这部分只是用来修正YAW轴的,还要加上加速度的校正补偿项(前面的博文中已经推导过),再送给陀螺仪: 
互补滤波法补充 
后面的就与前面博文中介绍的步骤一样了,这里不做赘述。

终于可以上代码了:

////////////////////////////////////////////////////////////////////////////////
#define Kp 10.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.008f                          // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.001f                   // half the sample period采样周期的一半

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
  float norm;
  float hx, hy, hz, bx, bz;
  float wx, wy, wz;
  float vx, vy, vz;
  float ex, ey, ez;

  // 先把这些用得到的值算好
  float q0q0 = q0*q0;
  float q0q1 = q0*q1;
  float q0q2 = q0*q2;
  float q0q3 = q0*q3;
  float q1q1 = q1*q1;
  float q1q2 = q1*q2;
  float q1q3 = q1*q3;
  float q2q2 = q2*q2;
  float q2q3 = q2*q3;
  float q3q3 = q3*q3;

    if(ax*ay*az==0)
        return;

    if(mx*my*mz == 0)
        return;

  norm = sqrt(ax*ax + ay*ay + az*az);       //acc数据归一化
  ax = ax / norm;
  ay = ay / norm;
  az = az / norm;

  norm = sqrt(mx*mx + my*my + mz*mz);       //mag数据归一化
  mx = mx / norm;
  my = my / norm;
  mz = mz / norm;

//  mx = 0;
//  my = 0;
//  mz = 0;

hx = 2 * mx * (0.5 - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2);  
hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5 - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1);  
hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 -q2q2);          
bx = sqrt((hx*hx) + (hy*hy));  
bz = hz;

  // estimated direction of gravity and flux (v and w)              估计重力方向和流量/变迁
  vx = 2*(q1q3 - q0q2);                                             //四元素中xyz的表示
  vy = 2*(q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3 ;

    wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2);  
  wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);  
  wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2); 

  // error is sum of cross product between reference direction of fields and direction measured by sensors
//  ex = (ay*vz - az*vy) ;                                               //向量外积在相减得到差分就是误差
//  ey = (az*vx - ax*vz) ;
//  ez = (ax*vy - ay*vx) ;

    ex = (ay*vz - az*vy) + (my*wz - mz*wy);  
    ey = (az*vx - ax*vz) + (mz*wx - mx*wz);  
    ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

  exInt = exInt + ex * Ki;                                //对误差进行积分
  eyInt = eyInt + ey * Ki;
  ezInt = ezInt + ez * Ki;

  // adjusted gyroscope measurements
  gx = gx + Kp*ex + exInt;                                              //将误差PI后补偿到陀螺仪,即补偿零点漂移
  gy = gy + Kp*ey + eyInt;
  gz = gz + Kp*ez + ezInt;                                          //这里的gz由于没有观测者进行矫正会产生漂移,表现出来的就是积分自增或自减

  // integrate quaternion rate and normalise                           //四元素的微分方程
  q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

  // normalise quaternion
  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;

  Q_ANGLE.z = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
  Q_ANGLE.y  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
  Q_ANGLE.x = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97

照着前面推导的步骤,再跟着程序走一遍,是不是清晰了许多?

推荐一些比较好的资料的链接: 
陀螺仪、加速度计 、磁力计的介绍 
http://www.360doc.com/content/13/1217/19/235269_337950743.shtml 
磁力计工作原理介绍 
http://blog.sina.com.cn/s/blog_402c071e0102v8ig.html