楼主: zkcaptain
收起左侧

[小应用]GD32F103+MPU9150 四轴飞行器第一步:9DOF姿态融合

[复制链接]

0

主题

5

帖子

15

积分

实习生

发表于 2016-2-22 16:18 | 显示全部楼层 |返回版面
545483362 发表于 2016-1-28 10:53
楼主你的四轴放着不动的话偏航角YAW会不会逐渐变大或变小,我的就出现了这个情况,其他两个角还好 ...

我的情况和你的一样,静止的时候ROLL和PITCH都是稳定的,唯独YAW会一直的变大或者变小。你解决没?

0

主题

5

帖子

15

积分

实习生

发表于 2016-2-22 17:35 | 显示全部楼层 |返回版面
楼主,为什么我的MPU9150在静止不动的情况下,yaw会一直增大?请问您有类似的情况吗?
Yaw = 50.00 Pitch = -0.73 roll = -0.73
Yaw = 50.00 Pitch = -0.73 roll = -0.73
Yaw = 50.01 Pitch = -0.73 roll = -0.73
Yaw = 50.02 Pitch = -0.74 roll = -0.74
Yaw = 50.03 Pitch = -0.74 roll = -0.74
Yaw = 50.04 Pitch = -0.73 roll = -0.73
Yaw = 50.05 Pitch = -0.73 roll = -0.73
Yaw = 50.05 Pitch = -0.73 roll = -0.73
Yaw = 50.06 Pitch = -0.73 roll = -0.73
Yaw = 50.07 Pitch = -0.73 roll = -0.73
Yaw = 50.08 Pitch = -0.74 roll = -0.74
Yaw = 50.09 Pitch = -0.74 roll = -0.74
Yaw = 50.10 Pitch = -0.74 roll = -0.74
Yaw = 50.11 Pitch = -0.75 roll = -0.75
Yaw = 50.11 Pitch = -0.75 roll = -0.75
Yaw = 50.12 Pitch = -0.75 roll = -0.75
Yaw = 50.13 Pitch = -0.75 roll = -0.75
Yaw = 50.14 Pitch = -0.75 roll = -0.75
Yaw = 50.15 Pitch = -0.75 roll = -0.75
Yaw = 50.15 Pitch = -0.75 roll = -0.75
Yaw = 50.16 Pitch = -0.75 roll = -0.75
Yaw = 50.17 Pitch = -0.75 roll = -0.75
Yaw = 50.18 Pitch = -0.74 roll = -0.74
Yaw = 50.19 Pitch = -0.74 roll = -0.74
Yaw = 50.20 Pitch = -0.75 roll = -0.75
Yaw = 50.20 Pitch = -0.74 roll = -0.74
Yaw = 50.21 Pitch = -0.74 roll = -0.74
Yaw = 50.22 Pitch = -0.74 roll = -0.74
Yaw = 50.23 Pitch = -0.74 roll = -0.74
Yaw = 50.23 Pitch = -0.74 roll = -0.74
Yaw = 50.24 Pitch = -0.74 roll = -0.74
Yaw = 50.25 Pitch = -0.74 roll = -0.74
Yaw = 50.26 Pitch = -0.74 roll = -0.74
Yaw = 50.27 Pitch = -0.73 roll = -0.73
Yaw = 50.27 Pitch = -0.73 roll = -0.73
Yaw = 50.28 Pitch = -0.73 roll = -0.73
Yaw = 50.29 Pitch = -0.73 roll = -0.73
Yaw = 50.30 Pitch = -0.73 roll = -0.73
Yaw = 50.31 Pitch = -0.73 roll = -0.73
Yaw = 50.32 Pitch = -0.73 roll = -0.73
Yaw = 50.33 Pitch = -0.72 roll = -0.72
Yaw = 50.34 Pitch = -0.72 roll = -0.72
Yaw = 50.34 Pitch = -0.72 roll = -0.72
Yaw = 50.35 Pitch = -0.72 roll = -0.72
Yaw = 50.36 Pitch = -0.72 roll = -0.72
Yaw = 50.37 Pitch = -0.73 roll = -0.73
Yaw会一直自增下去!困惑?
     

0

主题

12

帖子

36

积分

实习生

发表于 2016-2-23 20:27 | 显示全部楼层 |返回版面
楼主,我用了IMU为什么飘得那么厉害?我的源数据就在漂,你有用到滤波吗?下图中忽略前面的H和后面的T。前三列是加速度计的读数,后三列是陀螺仪的读数,抖动好厉害啊

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册 手机登录

x
     

0

主题

12

帖子

36

积分

实习生

发表于 2016-2-23 21:15 | 显示全部楼层 |返回版面
本帖最后由 huxiaoyingying 于 2016-2-23 21:17 编辑
orphanping 发表于 2016-2-22 17:35
楼主,为什么我的MPU9150在静止不动的情况下,yaw会一直增大?请问您有类似的情况吗?
Yaw会一直自增下去! ...

我的也是跟你一样!请问你找到什么原因了吗?

求楼主大神解答一下跪拜!!!


  1. const float g_kp=2.0f;//加速度权重,越大则向加速度测量值收敛越快
  2. const float g_ki=0.005f;//误差积分增益
  3. const float g_period=0.04f;//单位秒
  4. float q0=1,q1=0,q2=0,q3=0;
  5. float exInt=0,eyInt=0,ezInt=0;//scaled integral error
  6. void CCCOMDealDlg::Quaternion_CF(FLOAT_SENSORDATA stSensor, float *roll, float *pitch, float *yaw)
  7. {
  8.         float norm;
  9.         float vx, vy, vz;
  10.         float ex, ey, ez;
  11.         float ax,ay,az;
  12.         float gx,gy,gz;

  13.         ax = stSensor.fAx;//加速度
  14.         ay = stSensor.fAy;
  15.         az = stSensor.fAz;
  16.         gx = stSensor.fTx;//角速度
  17.         gy = stSensor.fTy;
  18.         gz = stSensor.fTz;

  19.         // normalise the measurements(加计的三维向量转成单位向量)
  20.         norm = sqrt(ax*ax + ay*ay + az*az);
  21.         ax = ax / norm;
  22.         ay = ay / norm;
  23.         az = az / norm;

  24.         // estimated direction of gravity(提取四元数的等效余弦矩阵中的重力分量)
  25.         vx = 2*(q1*q3 - q0*q2);
  26.         vy = 2*(q0*q1 + q2*q3);
  27.         vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;


  28.         // error is sum of cross product between reference direction of field and direction measured by sensor
  29.         // 向量叉积得出姿态误差
  30.         ex = (ay*vz - az*vy);
  31.         ey = (az*vx - ax*vz);
  32.         ez = (ax*vy - ay*vx);
  33.         
  34.         // integral error scaled integral gain
  35.         // 对误差进行积分
  36.         exInt = exInt + ex*g_ki;
  37.         eyInt = eyInt + ey*g_ki;
  38.         ezInt = ezInt + ez*g_ki;

  39.         // adjusted gyroscope measurements
  40.         // 互补滤波,姿态误差补偿到角速度上,修正角速度积分漂移
  41.         gx = gx + g_kp*ex + exInt;
  42.         gy = gy + g_kp*ey + eyInt;
  43.         gz = gz + g_kp*ez + ezInt;

  44.         // integrate quaternion rate and normalise
  45.         // 一阶龙格库塔法更新四元数
  46.         q0 = q0 + (-q1*gx - q2*gy - q3*gz)*g_period/2;
  47.         q1 = q1 + (q0*gx + q2*gz - q3*gy)*g_period/2;
  48.         q2 = q2 + (q0*gy - q1*gz + q3*gx)*g_period/2;
  49.         q3 = q3 + (q0*gz + q1*gy - q2*gx)*g_period/2;

  50.         // normalise quaternion
  51.         // 四元数归一化
  52.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  53.         q0 = q0 / norm;
  54.         q1 = q1 / norm;
  55.         q2 = q2 / norm;
  56.         q3 = q3 / norm;

  57.         //TRACE("%.2f %.2f %.2f %.2f\n", q0, q1, q2, q3);

  58.         float radian;
  59.         //四元数转欧拉角
  60.     if(roll)
  61.         {
  62.                 //radian = atan2f(2.0f*(q0*q1 + q2*q3), 1 - 2.0f*(q1*q1 + q2*q2));
  63.                 radian = atan2f(-2.0f*(q1*q3 - q0*q2),1 - 2.0f*(q1*q1 + q2*q2));
  64.         *roll = radian*180/3.14f;
  65.     }
  66.     if(pitch)//俯仰角
  67.         {
  68.         // 使用safe_asin()来处理pitch接近90/-90时的奇点
  69.                 //radian = asin(2.0f*(q0*q2 - q1*q3));
  70.                 radian = asin(2.0f*(q2*q3 + q0*q1));
  71.         *pitch = radian*180/3.14f;
  72.     }
  73.     if(yaw)//航向角
  74.         {
  75.                 //radian = atan2f(2.0f*(q1*q2 - q0*q3), 2.0f*(q0*q0 + q1*q1) - 1);
  76.                 radian = atan2f(2.0f*(q1*q2 - q0*q3),  1- 2.0f*(q1*q1 + q3*q3));
  77.                 *yaw = radian*180/3.14f;
  78.     }
  79.         //TRACE("%.2f\t%.2f\t%.2f\n", *roll, *pitch, *yaw);
  80. }
复制代码

     

0

主题

12

帖子

36

积分

实习生

发表于 2016-2-23 21:27 | 显示全部楼层 |返回版面
zkcaptain 发表于 2016-1-29 16:26
不会,代码请对比参考另外一个帖子里的

楼主,是不是因为你的YAW加入了磁力计的周期性校准所以才会长时间不会增加或减小?我使用的是IMU不加入磁力计数值做计算的,因为觉得校准会比较麻烦。希望楼主解释下。
还有楼主,为什么我晃动板子的时候数据跳跃很大,等静止下来角度才慢慢恢复正确,你的也是这样么?
代码我粘在后面的回复里了,希望楼主有时间的话帮我看下,小弟感激不尽!!
     

0

主题

14

帖子

52

积分

初级技术员

发表于 2016-4-26 10:08 | 显示全部楼层 |返回版面
学习了,楼主真厉害,正在研究四轴飞行器,很有帮助
     

6

主题

117

帖子

358

积分

资深技术员

发表于 2016-10-23 18:21 | 显示全部楼层 |返回版面
看着不错,测试一下
     

0

主题

2

帖子

6

积分

实习生

发表于 2016-11-12 13:59 | 显示全部楼层 |返回版面
你好我想请教一下,程序里这个Accel_Zout_Offset,为什么要设置成600啊。就是加计的Z轴输出为什么要加600呢~谢谢
     

0

主题

2

帖子

6

积分

实习生

发表于 2016-11-12 16:30 | 显示全部楼层 |返回版面
//陀螺仪y轴为前进方向   
                init_Roll = -atan2(init_ax, init_az);    //算出的单位是弧度,如需要观察则应乘以57.3转化为角度

楼主代码里面,这个地方是不是应该没有负号啊
     

0

主题

14

帖子

42

积分

实习生

发表于 2017-2-13 17:33 | 显示全部楼层 |返回版面
赞赞赞

26

主题

184

帖子

552

积分

高级技术员

发表于 2017-5-3 15:28 | 显示全部楼层 |返回版面
本帖最后由 无为之益 于 2017-7-31 18:06 编辑

非常好的帖子,感谢楼主

0

主题

1

帖子

3

积分

实习生

发表于 2017-5-12 18:52 | 显示全部楼层 |返回版面
老哥吊,我是真的服
     

11

主题

135

帖子

420

积分

资深技术员

发表于 2017-5-24 11:00 | 显示全部楼层 |返回版面
楼主真乃牛人
     

0

主题

14

帖子

42

积分

实习生

发表于 2017-5-24 15:56 | 显示全部楼层 |返回版面
多谢楼主,学习了
     

0

主题

2

帖子

6

积分

实习生

发表于 2017-5-30 01:40 | 显示全部楼层 |返回版面
楼主你好,参考移植了一下您的程序,但是发现,每次在水平上初始化后的yaw角值总是不为0,换一个方向运行yaw角也不同,但旋转时数值能变化且稳定,希望楼主能帮忙解答,谢谢

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册 手机登录

x

26

主题

184

帖子

552

积分

高级技术员

发表于 2017-7-28 10:22 | 显示全部楼层 |返回版面
功德无量
     

4

主题

12

帖子

30

积分

实习生

发表于 2017-9-8 14:20 | 显示全部楼层 |返回版面
能不能求个楼主的QQ啊。交流一下,我也是电机控制工程师

24

主题

1420

帖子

4261

积分

中级工程师

发表于 2017-9-8 21:28 | 显示全部楼层 |返回版面
MPU9150(INVENSENSE公司的,单芯片内集成了加速度计、陀螺仪和磁力计,并且内置DMP用于姿态融合,不过只融合了加速度计和陀螺仪,具有自校准功能,价格比MPU6050贵很多。。

26

主题

184

帖子

552

积分

高级技术员

发表于 2017-9-14 17:40 | 显示全部楼层 |返回版面
lilyhei 发表于 2014-11-12 10:41
楼主,我用你的AHRS的代码其他一切都正常,只是Yaw角在变化后会回到260这个值上面,求解决方案 ...

我也遇到机体作yaw转动后 停下来,yaw会回到一个 固定值上,不明原因。其他的都好

26

主题

184

帖子

552

积分

高级技术员

发表于 2017-9-14 17:50 | 显示全部楼层 |返回版面
21ic怎么禁止楼主发言呢?这么好的帖子。
只好请教大家;exInt = exInt + ex*Ki * halfT;                           //乘以采样周期的一半(单位:s)
      eyInt = eyInt + ey*Ki * halfT;
      ezInt = ezInt + ez*Ki * halfT;
网上好些同样的 融合算法,没有*half,即
exInt = exInt + ex*Ki ;                         
      eyInt = eyInt + ey*Ki;
      ezInt = ezInt + ez*Ki ;
到底哪一个正确呢?我两种情况都试了,效果差不多。不明白这个 公式如何 推导来的,有明白的大神吗?能否给小白讲讲呢?谢谢
您需要登录后才可以回帖 登录 | 注册 手机登录

本版积分规则

分享 快速回复 返回顶部 返回列表