Chinaunix首页 | 论坛 | 博客
  • 博客访问: 83072
  • 博文数量: 9
  • 博客积分: 187
  • 博客等级: 入伍新兵
  • 技术积分: 121
  • 用 户 组: 普通用户
  • 注册时间: 2011-03-06 22:38
个人简介

网络设备制造商硬件打杂店小二

文章分类

全部博文(9)

文章存档

2016年(2)

2012年(1)

2011年(6)

我的朋友

分类: 嵌入式

2016-11-20 19:50:41

  四旋翼和云台都离不开加速计和陀螺仪。加速计提供实时的重力加速度方向,进而可以计算出机体姿态角度。陀螺仪提供实时的旋转角度,通过积分也能换算成机体姿态角度。加速计对机体振动非常敏感,稍有扰动,加速度方向就会偏移(外力与重力的合力),但是长期运行时,经过平滑滤波算法之后的加速计数据就比较可信。相比而言,陀螺仪则对外部扰动不敏感,反而是长时间积分后会出现角度漂移。因此实际应用中,都是将二者采集的数据进行融合,相互取长补短。

  下面这段代码是网上比较常见的6轴融合算法,原作者只添加了几句简单的英文注释,几处关键段落阅读起来非常费解。而网上的大多数引用页面对这段代码也都没有做进一步的分析,我怀疑大家也都是一知半解,拿来就用,用得顺手便没有再去深究。这个周末我仔细研究了一下,对各关键公式进行了理论推导,供大家参考。三个关键段落的推导过程详见后文Note1,Note2,Note3。

点击(此处)折叠或打开

  1. void IMUupdate() {
  2.         float norm;
  3.         float vx, vy, vz;
  4.         float ex, ey, ez;
  5.         float ax,ay,az,gx,gy,gz;
  6.         float q0,q1,q2,q3;
  7.         float q0temp,q1temp,q2temp,q3temp;
  8.        
  9.         ax=Data.pSensor->accel[0];
  10.         ay=Data.pSensor->accel[1];
  11.         az=Data.pSensor->accel[2];
  12.         
  13.         gx=Data.pSensor->gyro[0];
  14.         gy=Data.pSensor->gyro[1];
  15.         gz=Data.pSensor->gyro[2];
  16.         
  17.         q0=Data.q[0];
  18.         q1=Data.q[1];
  19.         q2=Data.q[2];
  20.         q3=Data.q[3];
  21.         
  22.         // normalise the measurements
  23.         norm = sqrt(ax*ax + ay*ay + az*az);
  24.         ax = ax / norm;
  25.         ay = ay / norm;
  26.         az = az / norm;

  27.         //*Note1
  28.         // estimated direction of gravity
  29.         vx = 2*(q1*q3 - q0*q2);
  30.         vy = 2*(q0*q1 + q2*q3);
  31.         vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

  32.         //*Note2
  33.         // error is sum of cross product between reference direction of field and direction measured by sensor
  34.         ex = (ay*vz - az*vy);
  35.         ey = (az*vx - ax*vz);
  36.         ez = (ax*vy - ay*vx);

  37.         // integral error scaled integral gain
  38.         exInt = exInt + ex*Ki;
  39.         eyInt = eyInt + ey*Ki;
  40.         ezInt = ezInt + ez*Ki;

  41.         // adjusted gyroscope measurements, PID
  42.         gx = gx + Kp*ex + exInt;
  43.         gy = gy + Kp*ey + eyInt;
  44.         gz = gz + Kp*ez + ezInt;

  45.         q0temp=q0;
  46.         q1temp=q1;
  47.         q2temp=q2;
  48.         q3temp=q3;

  49.         //*Note3
  50.         // integrate quaternion rate and normalise
  51.         q0 = q0temp + (-q1temp *gx - q2temp *gy - q3temp *gz)*halfT;
  52.         q1 = q1temp + (q0temp *gx + q2temp *gz - q3temp *gy)*halfT;
  53.         q2 = q2temp + (q0temp *gy - q1temp *gz + q3temp *gx)*halfT;
  54.         q3 = q3temp + (q0temp *gz + q1temp *gy - q2temp *gx)*halfT;
  55.        
  56.         // normalise quaternion
  57.         norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  58.         q0 = q0 * norm;
  59.         q1 = q1 * norm;
  60.         q2 = q2 * norm;
  61.         q3 = q3 * norm;
  62.         
  63.         Data.q[0]=q0;
  64.         Data.q[1]=q1;
  65.         Data.q[2]=q2;
  66.         Data.q[3]=q3;
  67. }

*Note1



*Note2


 

*Note3






阅读(5869) | 评论(0) | 转发(0) |
给主人留下些什么吧!~~