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

只看该作者 · 2014-3-10 09:06

smileage 发表于 2014-3-9 11:23
init_mx =(float)mag[1]-8;
init_my =(float)1.046632*mag[0]-1.569948;
init_mz =(float)-mag[2];

坐标轴要保证一致

只看该作者 · 2014-3-10 09:41

楼主，你是否做过mpu9150的位移融合呢，即从一个点移动到另一个点的位移。以芯片的静止为计算开始和结束。请问楼主是否做过？没有做过的话，是否能建议我有哪些资料参考。

只看该作者 · 2014-3-10 10:34

zkcaptain 发表于 2014-3-10 09:06
坐标轴要保证一致

我还是没理解，能详细说说吗？

磁力计的x\y本来就和加速度的不一样，z轴向下刚好符合右手定理啊

只看该作者 · 2014-3-10 12:40

smileage 发表于 2014-3-10 10:34
我还是没理解，能详细说说吗？

磁力计的x\y本来就和加速度的不一样，z轴向下刚好符合右手定理啊 ...

正因为磁力计的x/y/z朝向和加速度计的不一样，才要转变磁力计的x/y/z的朝向，让他俩一样

只看该作者 · 2014-3-10 12:48

baoliguo 发表于 2014-3-10 09:41
楼主，你是否做过mpu9150的位移融合呢，即从一个点移动到另一个点的位移。以芯片的静止为计算开始和结束。 ...

不了解这方面的

只看该作者 · 2014-3-10 13:18

本帖最后由 smileage 于 2014-3-10 13:39 编辑

抱歉，原来没有注意到x/y已经置换了，现在有新的问题想请教，

我把磁力计的x/y置换，z轴反向后，Yaw的0度方向测得是图中的+X方向，但是旋转方向反了，本来顺时针0-90-180-270-0的，现在变成0-270-180-90-0了。

Yaw = -atan2(2*q1*q2 - 2*q0*q3, -2 * q1 * q1 - 2 * q3 * q3 + 1) * 57.3;  //偏航角，绕z轴转动
if(Yaw < 0 ){Yaw = Yaw + 360;}
if(Yaw > 360 ){Yaw = Yaw - 360;}
Pitch = asin(2*q2*q3 + 2*q0*q1) * 57.3; //俯仰角，绕x轴转动
Roll  = -atan2(-2*q0*q2 + 2*q1*q3, -2 * q1 * q1 - 2 * q2* q2 + 1) * 57.3; //滚动角，绕y轴转动

于是，我将Yaw改成Yaw = atan2(2*q1*q2 - 2*q0*q3, -2 * q1 * q1 - 2 * q3 * q3 + 1) * 57.3; 就好了
这个是为什么，按道理说我和楼主的设计没有不一样的地方啊？

只看该作者 · 2014-3-11 09:03

smileage 发表于 2014-3-10 13:18
抱歉，原来没有注意到x/y已经置换了，现在有新的问题想请教，

我把磁力计的x/y置换，z轴反向后，Yaw的0度 ...

旋转的正方向也是自己定的，前面加负号就可以改变，你和我的定义肯定不一样

只看该作者 · 2014-3-11 15:35

zkcaptain 发表于 2014-3-11 09:03
旋转的正方向也是自己定的，前面加负号就可以改变，你和我的定义肯定不一样 ...

再请教下，如果Z轴不进行校准，那么得出来的Yaw是不是不准确，而R和P不受影响。

因为四元数和欧拉角第一次接触到，还没有仔细研究

只看该作者 · 2014-3-12 09:06

smileage 发表于 2014-3-11 15:35
再请教下，如果Z轴不进行校准，那么得出来的Yaw是不是不准确，而R和P不受影响。

因为四元数和欧拉角第一 ...

磁力计如果只融合到yaw的话，肯定对roll和pitch没影响

理论上来说，磁力计xyz都要校准，只校准xy只能保证在水平面磁力计的准确性，空间上不能保证

只看该作者 · 2014-3-17 08:53

烈火狂龙发表于 2013-9-28 13:58
MARK！前面的路还很长，楼主加油！

烈火兄，能免费送套四轴不？

只看该作者 · 2014-3-17 14:34

楼主，我现在使用AHRS方式，在实际使用过程中有过冲现象：
比如Yaw从200度变到220度，角度会先变到230度再慢慢下下来，这个问题是什么原因，如何解决。

只看该作者 · 2014-3-18 09:12

smileage 发表于 2014-3-17 14:34
楼主，我现在使用AHRS方式，在实际使用过程中有过冲现象：
比如Yaw从200度变到220度，角度会先变到230度再 ...

kp、ki调一下，halfT用定时器计时，不要固定

只看该作者 · 2014-3-19 19:51

楼主太给力啦

只看该作者 · 2014-3-29 15:01

本帖最后由 smileage 于 2014-3-29 15:29 编辑

楼主，我使用了你的代码，发现IMU方式算出的Yaw的稳定性很差，而AHRS的稳定性较好（1度以内变化）。
这是什么原因造成的呢？

下面是IMU方式出来的数据，跳动大
221.5
223.7
224.4
226.5
224.4
225.7
219.1
222.4
224.3

AHRS方式，数据稳定
161.4
161.6
161.6
161.6
161.4
161.4
161.2
161.3
161.3
161.4
161.5
161.5
161.5
161.7
161.8
161.7
161.7
161.8
161.8
161.8
161.8
162.1
161.9
161.8

我看了mx\my\mz的原数据，的确跳动是挺大的，这也解释了IMU方式为什么跳动大。
但是为什么AHRS没有出现这个问题呢？这个想不通 mx=-28.000000, my=-98.000000, mz=-114.000000
mx=-28.000000, my=-96.000000, mz=-114.000000
mx=-26.000000, my=-99.000000, mz=-111.000000
mx=-38.000000, my=-97.000000, mz=-109.000000
mx=-38.000000, my=-93.000000, mz=-106.000000
mx=-34.000000, my=-100.000000, mz=-110.000000
mx=-38.000000, my=-97.000000, mz=-111.000000
mx=-29.000000, my=-99.000000, mz=-116.000000
mx=-25.000000, my=-96.000000, mz=-117.000000
mx=-32.000000, my=-94.000000, mz=-107.000000
mx=-29.000000, my=-95.000000, mz=-109.000000
mx=-32.000000, my=-96.000000, mz=-117.000000
mx=-32.000000, my=-96.000000, mz=-112.000000
mx=-31.000000, my=-97.000000, mz=-109.000000
mx=-25.000000, my=-96.000000, mz=-119.000000
mx=-37.000000, my=-98.000000, mz=-107.000000
mx=-32.000000, my=-91.000000, mz=-117.000000
mx=-33.000000, my=-97.000000, mz=-109.000000
mx=-34.000000, my=-91.000000, mz=-107.000000
mx=-30.000000, my=-94.000000, mz=-112.000000
mx=-29.000000, my=-99.000000, mz=-109.000000
mx=-29.000000, my=-99.000000, mz=-113.000000
mx=-32.000000, my=-98.000000, mz=-105.000000
mx=-29.000000, my=-95.000000, mz=-111.000000
mx=-29.000000, my=-93.000000, mz=-109.000000
mx=-31.000000, my=-102.000000, mz=-116.000000
mx=-31.000000, my=-95.000000, mz=-109.000000
mx=-29.000000, my=-99.000000, mz=-111.000000
mx=-30.000000, my=-98.000000, mz=-110.000000
mx=-34.000000, my=-100.000000, mz=-110.000000
mx=-31.000000, my=-99.000000, mz=-109.000000
mx=-31.000000, my=-102.000000, mz=-111.000000
mx=-33.000000, my=-97.000000, mz=-109.000000

只看该作者 · 2014-3-31 09:25

smileage 发表于 2014-3-29 15:01
楼主，我使用了你的代码，发现IMU方式算出的Yaw的稳定性很差，而AHRS的稳定性较好（1度以内变化）。
这是什 ...

IMU方式，你用什么求的yaw？

只看该作者 · 2014-3-31 14:03

zkcaptain 发表于 2014-3-31 09:25
IMU方式，你用什么求的yaw？

Yaw = -(0.9 * (-Yaw + init_gz*2*halfT) + 5.73 * atan2(init_mx*cos(Roll) + init_my*sin(Roll)*sin(Pitch) + init_mz*sin(Roll)*cos(Pitch), init_my*cos(Pitch) - init_mz*sin(Pitch)));

Yaw = 0.92 * (Yaw + gz*2*halfT) + 0.08 * Mag_Yaw;

楼主，你在IMU方式的公式里，gz*2*halfT应该再乘以57.3。
我测试的方法是，单使用Yaw = Yaw + gz*2*halfT*57.3，或者单使用Yaw=Mag_Yaw；

前者，静止的时候，角度慢慢往上漂，一直漂，我觉得应该是没校准好；
后者，数据跳动就像上贴那样，4-5度之间跳动，我觉得这个可以用平均值处理。

因为时间有限，我现在主要把精力放在AHRS上面了，有空再测试IMU的方式。
PS:我做的是航向仪，需要三维补偿。

只看该作者 · 2014-3-31 14:09

本帖最后由 smileage 于 2014-3-31 14:35 编辑

再有两个问题请教：楼主在使用磁力计的时候，在init_quaternion和while里面使用了两种方式：
我想问下：while里面的方式是为了获得更高频率吗？
另外，我这边测得的结果是mpu_set_bypass(0);不用磁力计的数据也能得到更新。

      mpu_set_bypass(1);                   //开启bypass，必须有这句代码
      mpu_get_compass_reg(mag, ×tamp);  //读取compass数据
      //进行x y轴的校准，未对z轴进行校准，参考MEMSense的校准方法
      init_mx =(float)mag[1]-offset_x;
      init_my =(float)mag[0]-offset_y;
      init_mz =(float)-mag[2]-offset_z;

      mpu_set_bypass(0);

            data_write[6]=0x00;
            data_write[7]=0x02;
      i2cwrite(MPU9150_Addr, User_Ctrl, 1, data_write+6);       //关闭MPU9150的I2C_MASTER模式，必须要有这句
            Delay(24000);          //这俩句之间的延迟至少24000
            i2cwrite(MPU9150_Addr, Bypass_Enable_Cfg, 1, data_write+7);       //开启bypass，必须有这句代码，综合这两句就是开启Bypass though

      mpu_get_compass_reg(mag, ×tamp);  //读取compass数据
      //进行x y轴的校准，未对z轴进行校准，参考ST的校准方法
      init_mx =(float)mag[1]-offset_x;
      init_my =(float)mag[0]-offset_y;
      init_mz =(float)-mag[2]-offset_z;

            data_write[6]=0x20;
            data_write[7]=0x00;
            i2cwrite(MPU9150_Addr, User_Ctrl, 1, data_write+6); //开启MPU9150的I2C_MASTER模式，必须要有这句
            Delay(24000);                //这俩句之间的延迟至少24000
            i2cwrite(MPU9150_Addr, Bypass_Enable_Cfg, 1, data_write+7);//关闭bypass，必须有这句代码，综合这两句就是关闭Bypass though

只看该作者 · 2014-3-31 14:15

楼主在AHRS方式里没有使用run_self_test();，是不是也应该使用这个函数？
我在使用run_self_test()的时候遇到楼上一个同学遇到的问题，返回值是0x4，也就是说gyro和accel没有成功，这个没想明白，用的就是楼主的原版程序，
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS) 三者都是开启的。