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

[复制链接]
楼主: zkcaptain
手机看帖
扫描二维码
随时随地手机跟帖
wangdezhi| | 2014-3-31 23:49 | 显示全部楼层
能起飞了么

使用特权

评论回复
zkcaptain|  楼主 | 2014-4-1 08:53 | 显示全部楼层
smileage 发表于 2014-3-31 14:03
楼主,你在IMU方式的公式里,gz*2*halfT应该再乘以57.3。
我测试的方法是,单使用Yaw = Yaw + gz*2*hal ...
楼主,你在IMU方式的公式里,gz*2*halfT应该再乘以57.3。
我测试的方法是,单使用Yaw = Yaw + gz*2*halfT*57.3,或者单使用Yaw=Mag_Yaw;

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


我IMU公式里的求yaw,gz*2*halfT不用再乘以57.3,gz*2*halfT的单位就是角度,不是弧度,公式我都自己测试过,用过的,没问题。

你单使用Yaw = Yaw + gz*2*halfT*57.3有漂移,是因为陀螺仪长时间积分有误差积累,所以才会漂移,不是没校准好的问题,这个方法也不可取,短时间可用,长时间误差太大

单使用Yaw=Mag_Yaw,你的Mag_Yaw是什么

使用特权

评论回复
zkcaptain|  楼主 | 2014-4-1 08:57 | 显示全部楼层
smileage 发表于 2014-3-31 14:09
再有两个问题请教:楼主在使用磁力计的时候,在init_quaternion和while里面使用了两种方式:
我想问下:whi ...

是的,不过现在我正在用的方法更快,只是每10ms左右更新一次磁力计的数据,这个频率足够了,磁力计本身的output rate也才几百hz而已,没必要死等磁力计的数据,基本上现在我姿态更新速率可以到500hz,过段时间再开一贴,更新下代码

使用特权

评论回复
zkcaptain|  楼主 | 2014-4-1 08:59 | 显示全部楼层
smileage 发表于 2014-3-31 14:15
楼主在AHRS方式里没有使用run_self_test();,是不是也应该使用这个函数?
我在使用run_self_test()的时候遇 ...

这个自检我没用,看代码只有用dmp的时候,这个自检才有用,我现在都是自己校准传感器。

使用特权

评论回复
zkcaptain|  楼主 | 2014-4-1 09:03 | 显示全部楼层
smileage 发表于 2014-3-31 14:15
楼主在AHRS方式里没有使用run_self_test();,是不是也应该使用这个函数?
我在使用run_self_test()的时候遇 ...

run_self_test()需要传感器位置放正,z轴朝上并静止。

使用特权

评论回复
zkcaptain|  楼主 | 2014-4-1 09:05 | 显示全部楼层
wangdezhi 发表于 2014-3-31 23:49
能起飞了么

正在调参数,尽量保证能飞的稳定一些

使用特权

评论回复
smileage| | 2014-4-1 09:22 | 显示全部楼层
zkcaptain 发表于 2014-4-1 08:53
我IMU公式里的求yaw,gz*2*halfT不用再乘以57.3,gz*2*halfT的单位就是角度,不是弧度,公式我都自己测 ...
init_gx=(float)(gyro[0] / (Gyro_500_Scale_Factor * 57.3));                       //单位转化成:弧度/s
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)));


比如在GD32F103CB IMU这个工程里面,init_gx在进入函数IMUupdate()之前的单位是弧度/s,在后面的公式中直接乘以s,并没有转化成角度

使用特权

评论回复
smileage| | 2014-4-1 09:38 | 显示全部楼层
zkcaptain 发表于 2014-4-1 09:05
正在调参数,尽量保证能飞的稳定一些

我来谈谈自己校准磁力计的方法吧。
X\Y轴的校准:平面旋转一周,取mx_max,mx_min,my_max,my_min,根据实测(mx_max-mx_min)和(my_max-my_min)基本是相等的,所以我觉得灵敏度可以不用修正,不如直接相信出厂时候的一致性,毕竟即使是自己的校准过程也是有误差的。这样我们需要保存的就是(mx_max+mx_min)/2,(my_max+my_min)/2这两个偏置值offset。
Z轴的校准:Z轴在竖直或者竖直倒立的时候有最大值和最小值,我们同样可以测得(mz_max+mz_min)/2,同理默认Z轴的灵敏度和X/Y轴是相等的。
后面的处理只要init_mx =(float)mag[1]-offset_x;init_my =(float)mag[0]-offset_y;init_mz =(float)-mag[2]-offset_z;即可。
根据实测,这样的校准方式在倾斜的时候效果还是不错的。不知道楼主的校准方法怎么样,如果Z轴不校准的话,倾斜的时候变动很大。

使用特权

评论回复
zkcaptain|  楼主 | 2014-4-1 10:29 | 显示全部楼层
本帖最后由 zkcaptain 于 2014-4-1 10:55 编辑
smileage 发表于 2014-4-1 09:22
比如在GD32F103CB IMU这个工程里面,init_gx在进入函数IMUupdate()之前的单位是弧度/s,在后面的公式 ...

看样子是需要乘以57.3,我也忘了当时是怎么考虑的了,当时没乘以57.3,测得yaw也是很准确,你试试吧,看乘和不乘有什么区别。

忘了说了,如果可以的话,把测试结果发上来吧。

使用特权

评论回复
zkcaptain|  楼主 | 2014-4-1 10:32 | 显示全部楼层
smileage 发表于 2014-4-1 09:38
我来谈谈自己校准磁力计的方法吧。
X\Y轴的校准:平面旋转一周,取mx_max,mx_min,my_max,my_min,根据实 ...

这些方法我现在不用了,到时候更新代码的时候再一起放上来。
灵敏度和偏置都要校准,并且要空间上校准

使用特权

评论回复
mybao| | 2014-4-1 13:27 | 显示全部楼层
MPU9150, 9轴sensor,不错,顶  

使用特权

评论回复
smileage| | 2014-4-1 14:42 | 显示全部楼层
楼主,你在“第三部分:如何确定自己的轴向”截图部分出自哪个pdf?我想看一下推导过程。
ZYX方式我能在网上找到对应的公式,但是ZXY方式的变化我就弄不明白了。
还有初始化四元数和最后结算的欧拉角公式前的符号,正或负快搞晕了。

使用特权

评论回复
zkcaptain|  楼主 | 2014-4-1 15:55 | 显示全部楼层
smileage 发表于 2014-4-1 14:42
楼主,你在“第三部分:如何确定自己的轴向”截图部分出自哪个pdf?我想看一下推导过程。
ZYX方式我能在网 ...


AppNote - 9-Axis MotionFusion and Calibration Algorithms.pdf (126.48 KB)

使用特权

评论回复
smileage| | 2014-4-1 16:26 | 显示全部楼层
zkcaptain 发表于 2014-4-1 10:32
这些方法我现在不用了,到时候更新代码的时候再一起放上来。
灵敏度和偏置都要校准,并且要空间上校准 ...

楼主是准备用空间多点的方法求球心坐标吗?能否先简单谈谈

我现在用的是平面校准的方法,参考电子罗盘校准方法
http://bbs.hiapk.com/thread-874711-1-1.html

使用特权

评论回复
zkcaptain|  楼主 | 2014-4-1 16:44 | 显示全部楼层
smileage 发表于 2014-4-1 16:26
楼主是准备用空间多点的方法求球心坐标吗?能否先简单谈谈

我现在用的是平面校准的方法,参考电子罗盘校 ...

空间绕8字,找到xyz在空间上的最大最小值,另灵敏度最大的轴的gain=1,求得xyz的gain和offset。
校准方法写成c语言了,不通过PC干预。

使用特权

评论回复
smileage| | 2014-4-2 10:36 | 显示全部楼层
zkcaptain 发表于 2014-4-1 16:44
空间绕8字,找到xyz在空间上的最大最小值,另灵敏度最大的轴的gain=1,求得xyz的gain和offset。
校准方法 ...

按照我的理解,空间绕八字的方法不是通过找最大值最小值的方法来实现的。因为要在三维空间中找到最大值和最小值,绕一个八字是不可能找到的,除非遍历整个三维空间。

空间绕八字的方法应该是通过足够的样本点求出圆心O(γx, γy, γz), 相当于解方程求圆心。在这个过程中,灵敏度首先是默认一致的,所以空间绕八字校准的只是偏置。我看了9-Axis MotionFusion and Calibration Algorithms.pdf,里面讲到的也只有偏置值,没有灵敏度。

另外,pdf中还讲到Compass bias using gyros (ML_MAG_BIAS_FROM_GYRO) ,这个方法我还不是很理解,但是用起来好像更简单。

使用特权

评论回复
zkcaptain|  楼主 | 2014-4-2 11:01 | 显示全部楼层
smileage 发表于 2014-4-2 10:36
按照我的理解,空间绕八字的方法不是通过找最大值最小值的方法来实现的。因为要在三维空间中找到最大值和 ...

空间8字法不是只绕一个8字。。。。。其目的是让每一个轴向尽可能的扫过8个象限,并且每个象限尽可能多的扫描,这时得到的max和min才有意义,也可以通过这个max和min得到gain和offset,这样扫出来的是个椭球,而不是圆球,所以不存在圆心一说,我也没看到要求灵敏度首先要一致,在用任何方法校准之前如何先保证灵敏度一致?
例如加速度计的校准,也是先取+1g和-1g下的数据,然后才得出gain和offset

使用特权

评论回复
smileage| | 2014-4-2 13:56 | 显示全部楼层
空间遍历找最大值和最小值是最基本的方法,但是应用有难度。因为在空间中,最大值和最小值一般只出现在X/Y/Z轴各自与地球磁力线重合的情况下,那么即使通过反复8字也可能无法得到正确的最大值和最小值,这会造成计算的gain和offset有偏差。最大值最小值一般用在平面校准法,比如在X/Y和Y/Z平面各自缓慢旋转一周,就能得到mx_max,mx_min,my_max,my_min,mz_max,mz_min,从而求得各自的gain和offset,因为平面是固定的,所以旋转校准的目的性很强。

对于gain,我的想法是校准还不如不校准。我测试过PNI和honeywell的磁传感器(分立元件),还有ST的集成传感器LSM303DLH,应该说分立元件的灵敏度很差,是必须校准的;但是集成传感器如LSM303DLH、AK8975的灵敏度一致性已经相当好了。拿AK8975来说,我在X/Y平面认真地旋转一周,(mx_max-mx_min)(my_max-my_min)两者的值基本相等。另外,我觉得gain不好校准的原因是,根据地球磁场强度50-60uT的大小,(mx_max-mx_min)的值在200-300之间变化,这时如果校准得到的数据即使只有2-3个值的偏差,对gain的影响都很大,况且磁力计读出数据本来就有跳动。相比于自己校准gain,我更相信芯片的一致性。

对于空间8字法,我的观点是通过普通的样本点(不是最大值和最小值)来找圆心(偏置坐标),这个和楼主的想法有出入。

使用特权

评论回复
zkcaptain|  楼主 | 2014-4-2 15:44 | 显示全部楼层
本帖最后由 zkcaptain 于 2014-4-2 15:59 编辑
smileage 发表于 2014-4-2 13:56
空间遍历找最大值和最小值是最基本的方法,但是应用有难度。因为在空间中,最大值和最小值一般只出现在X/Y/ ...

你所说的空间8字难,其实不难,关键在于是否采到足够的样本点而已,我可以拿着四轴全方位的去转,总能找到足够的样本点,真要严格去校准磁力计,就不会用这种大众方法,而且不但要校准,还要有算法去对磁力计进行补偿,以应对可预知的外界磁场的干扰。对于我的四轴来说,只是这个方法足够用了而已。而不是说这个方法最优,这个方法用于水平和空间都可以。说用于水平面校准,其实也是忽略了垂直方向的磁场强度而已,况且地球磁场在地平面也并不是完全垂直于地面的,当你水平面旋转时,肯定会有来自垂直方向磁场的分量进来,这时你得出的gain和offset肯定比空间校准的误差要大,因为你是忽略了垂直方向的磁场。因此只能说,在简陋的校准环境下,空间校准肯定比只有水平面校准强。

你的gain可以不用校准,可是我这边的情况是,我的一个轴Magmax和Magmin之间绝对值之和能比另一个轴多6个uT,不校准gain的话,角度误差肯定很大。同一个型号的芯片,他们的gain和offset都不会完全一样,有的误差甚至大到离谱,如何去相信?只校准offset的话,某些情况能用,某些情况就不好用。如果你觉得你的芯片不用校准gain就已经很好了,那最省事了,可是我用的这几颗芯片需要去校准gain。

其实你说的“通过普通的样本点(不是最大值和最小值)来找圆心”,在校准之前这个“圆心”并不是真正的圆心,而是椭圆心,因为你不能保证磁力计3个轴的gain是一致的,在你校准以后,你才能保证3个轴的gain误差最小,这个时候的圆心才是圆心。实际上这个方法已经包含了gain的校准.

我的方法严格来说,其实是空间8字的变种,因为我的方法是采用“空间8字的绕动方法”和“平面法拓展到空间”的校准公式,降低运算量,并保证一定的精度。不过我很有兴趣试试你的求圆心公式法,不知可否将具体推导公式共享一下。

使用特权

评论回复
smileage| | 2014-4-2 18:56 | 显示全部楼层
本帖最后由 smileage 于 2014-4-2 19:01 编辑
zkcaptain 发表于 2014-4-2 15:44
你所说的空间8字难,其实不难,关键在于是否采到足够的样本点而已,我可以拿着四轴全方位的去转,总能找到 ...

zkcaptain,目前我自己采用的是平面校准法(209楼),包含两个平面的校准,分别是XY平面和YZ平面,这其实已经完成了三维空间校准,包括三轴的gain和offset。但是由于我在做的产品不能在YZ平面进行旋转,所以目前我也正在寻找更好的校准方法,据我所知,honeywell的demo板和iphone的三维电子罗盘都是采用求圆心公式法。

关于gain的校准,gain是由芯片的出厂特性决定的,而offset还和使用环境相关。你测得的结果是“一个轴Magmax和Magmin之间绝对值之和能比另一个轴多6个uT”,这个的确和我的测试结果不同。你的测试结果对我很有参考价值,我会对更多mpu9150的gain进行测试,谢谢。

使用特权

评论回复
发新帖 我要提问
您需要登录后才可以回帖 登录 | 注册

本版积分规则