楼主: zkcaptain
收起左侧

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

[复制链接]
     

36

主题

266

帖子

1547

积分

助理工程师

发表于 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];

楼主,按照右手定理,磁力计本来就符合的,你的程序里Z轴做了反向,是不是这里有问题?还是我弄错了右手定理。

本帖子中包含更多资源

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

x
http://www.iseeic.com 电子芯片网
     

64

主题

255

帖子

340

积分

禁止发言

 楼主| 发表于 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];

坐标轴要保证一致

0

主题

26

帖子

78

积分

初级技术员

发表于 2014-3-10 09:41 | 显示全部楼层 |返回版面
楼主,你是否做过mpu9150的位移融合呢,即从一个点移动到另一个点的位移。以芯片的静止为计算开始和结束。请问楼主是否做过?没有做过的话,是否能建议我有哪些资料参考。
     

36

主题

266

帖子

1547

积分

助理工程师

发表于 2014-3-10 10:34 | 显示全部楼层 |返回版面
zkcaptain 发表于 2014-3-10 09:06
坐标轴要保证一致

我还是没理解,能详细说说吗?

磁力计的x\y本来就和加速度的不一样,z轴向下刚好符合右手定理啊
http://www.iseeic.com 电子芯片网
     

64

主题

255

帖子

340

积分

禁止发言

 楼主| 发表于 2014-3-10 12:40 | 显示全部楼层 |返回版面
smileage 发表于 2014-3-10 10:34
我还是没理解,能详细说说吗?

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

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

64

主题

255

帖子

340

积分

禁止发言

 楼主| 发表于 2014-3-10 12:48 | 显示全部楼层 |返回版面
baoliguo 发表于 2014-3-10 09:41
楼主,你是否做过mpu9150的位移融合呢,即从一个点移动到另一个点的位移。以芯片的静止为计算开始和结束。 ...

不了解这方面的
     

36

主题

266

帖子

1547

积分

助理工程师

发表于 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;  就好了
这个是为什么,按道理说我和楼主的设计没有不一样的地方啊?
http://www.iseeic.com 电子芯片网
     

64

主题

255

帖子

340

积分

禁止发言

 楼主| 发表于 2014-3-11 09:03 | 显示全部楼层 |返回版面
smileage 发表于 2014-3-10 13:18
抱歉,原来没有注意到x/y已经置换了,现在有新的问题想请教,

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

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

36

主题

266

帖子

1547

积分

助理工程师

发表于 2014-3-11 15:35 | 显示全部楼层 |返回版面
zkcaptain 发表于 2014-3-11 09:03
旋转的正方向也是自己定的,前面加负号就可以改变,你和我的定义肯定不一样 ...

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

因为四元数和欧拉角第一次接触到,还没有仔细研究
http://www.iseeic.com 电子芯片网
     

64

主题

255

帖子

340

积分

禁止发言

 楼主| 发表于 2014-3-12 09:06 | 显示全部楼层 |返回版面
smileage 发表于 2014-3-11 15:35
再请教下,如果Z轴不进行校准,那么得出来的Yaw是不是不准确,而R和P不受影响。

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

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

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

0

主题

26

帖子

78

积分

初级技术员

发表于 2014-3-17 08:53 | 显示全部楼层 |返回版面
烈火狂龙 发表于 2013-9-28 13:58
MARK!前面的路还很长,楼主加油!

烈火兄,能免费送套四轴不?
     

36

主题

266

帖子

1547

积分

助理工程师

发表于 2014-3-17 14:34 | 显示全部楼层 |返回版面
楼主,我现在使用AHRS方式,在实际使用过程中有过冲现象:
比如Yaw从200度变到220度,角度会先变到230度再慢慢下下来,这个问题是什么原因,如何解决。
http://www.iseeic.com 电子芯片网
     

64

主题

255

帖子

340

积分

禁止发言

 楼主| 发表于 2014-3-18 09:12 | 显示全部楼层 |返回版面
smileage 发表于 2014-3-17 14:34
楼主,我现在使用AHRS方式,在实际使用过程中有过冲现象:
比如Yaw从200度变到220度,角度会先变到230度再 ...

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

0

主题

2

帖子

6

积分

实习生

发表于 2014-3-19 19:51 | 显示全部楼层 |返回版面
楼主太给力啦
     

36

主题

266

帖子

1547

积分

助理工程师

发表于 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
http://www.iseeic.com 电子芯片网
     

64

主题

255

帖子

340

积分

禁止发言

 楼主| 发表于 2014-3-31 09:25 | 显示全部楼层 |返回版面
smileage 发表于 2014-3-29 15:01
楼主,我使用了你的代码,发现IMU方式算出的Yaw的稳定性很差,而AHRS的稳定性较好(1度以内变化)。
这是什 ...

IMU方式,你用什么求的yaw?
     

36

主题

266

帖子

1547

积分

助理工程师

发表于 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:我做的是航向仪,需要三维补偿。
http://www.iseeic.com 电子芯片网
     

36

主题

266

帖子

1547

积分

助理工程师

发表于 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

http://www.iseeic.com 电子芯片网
     

36

主题

266

帖子

1547

积分

助理工程师

发表于 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) 三者都是开启的。
http://www.iseeic.com 电子芯片网

200

主题

3879

帖子

1万

积分

资深工程师

发表于 2014-3-31 23:48 | 显示全部楼层 |返回版面
好高端呢
您需要登录后才可以回帖 登录 | 注册

本版积分规则

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