楼主: zkcaptain
收起左侧

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

[复制链接]

0

主题

5

帖子

15

积分

实习生

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

我的情况和你的一样,静止的时候ROLL和PITCH都是稳定的,唯独YAW会一直的变大或者变小。你解决没?
21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0

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会一直自增下去!困惑?
21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0
     

0

主题

12

帖子

36

积分

实习生

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

本帖子中包含更多资源

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

x
21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0
     

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. }
复制代码

21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0
     

0

主题

12

帖子

36

积分

实习生

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

楼主,是不是因为你的YAW加入了磁力计的周期性校准所以才会长时间不会增加或减小?我使用的是IMU不加入磁力计数值做计算的,因为觉得校准会比较麻烦。希望楼主解释下。
还有楼主,为什么我晃动板子的时候数据跳跃很大,等静止下来角度才慢慢恢复正确,你的也是这样么?
代码我粘在后面的回复里了,希望楼主有时间的话帮我看下,小弟感激不尽!!
21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0
     

0

主题

14

帖子

52

积分

初级技术员

发表于 2016-4-26 10:08 | 显示全部楼层 |返回版面
学习了,楼主真厉害,正在研究四轴飞行器,很有帮助
21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0
     

6

主题

100

帖子

307

积分

资深技术员

发表于 2016-10-23 18:21 | 显示全部楼层 |返回版面
看着不错,测试一下
21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0
     

0

主题

2

帖子

6

积分

实习生

发表于 2016-11-12 13:59 | 显示全部楼层 |返回版面
你好我想请教一下,程序里这个Accel_Zout_Offset,为什么要设置成600啊。就是加计的Z轴输出为什么要加600呢~谢谢
21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0
     

0

主题

2

帖子

6

积分

实习生

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

楼主代码里面,这个地方是不是应该没有负号啊
21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0
     

0

主题

10

帖子

30

积分

实习生

发表于 2017-2-13 17:33 | 显示全部楼层 |返回版面
赞赞赞
21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0

24

主题

169

帖子

507

积分

高级技术员

发表于 2017-5-3 15:28 | 显示全部楼层 |返回版面
orphanping 发表于 2016-2-22 16:18
我的情况和你的一样,静止的时候ROLL和PITCH都是稳定的,唯独YAW会一直的变大或者变小。你解决没? ...

我的情况是:载体动作很大,姿态角数据变化却很小,不明白啥原因
21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0

0

主题

1

帖子

3

积分

实习生

发表于 2017-5-12 18:52 | 显示全部楼层 |返回版面
老哥吊,我是真的服
21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0
     

11

主题

134

帖子

417

积分

资深技术员

发表于 2017-5-24 11:00 | 显示全部楼层 |返回版面
楼主真乃牛人
21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0
     

0

主题

10

帖子

30

积分

实习生

发表于 2017-5-24 15:56 | 显示全部楼层 |返回版面
多谢楼主,学习了
21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0

0

主题

1

帖子

3

积分

实习生

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

本帖子中包含更多资源

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

x
21ic公开课,21ic网友共同的学习圈子!学单片机、嵌入式、模拟、电源……就看这里 http://open.21ic.com 移步更多21ic独家微课:http://open.21ic.com/list/3/0/0
您需要登录后才可以回帖 登录 | 注册

本版积分规则

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