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

2万 · 2014-3-31 23:49

能起飞了么

只看该作者 · 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是什么

只看该作者 · 2014-4-1 08:57

smileage 发表于 2014-3-31 14:09
再有两个问题请教：楼主在使用磁力计的时候，在init_quaternion和while里面使用了两种方式：
我想问下：whi ...

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

只看该作者 · 2014-4-1 08:59

smileage 发表于 2014-3-31 14:15
楼主在AHRS方式里没有使用run_self_test();，是不是也应该使用这个函数？
我在使用run_self_test()的时候遇 ...

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

只看该作者 · 2014-4-1 09:03

smileage 发表于 2014-3-31 14:15
楼主在AHRS方式里没有使用run_self_test();，是不是也应该使用这个函数？
我在使用run_self_test()的时候遇 ...

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

只看该作者 · 2014-4-1 09:05

wangdezhi 发表于 2014-3-31 23:49
能起飞了么

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

只看该作者 · 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，并没有转化成角度

只看该作者 · 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轴不校准的话，倾斜的时候变动很大。

只看该作者 · 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也是很准确，你试试吧，看乘和不乘有什么区别。

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

只看该作者 · 2014-4-1 10:32

smileage 发表于 2014-4-1 09:38
我来谈谈自己校准磁力计的方法吧。
X\Y轴的校准：平面旋转一周，取mx_max,mx_min,my_max,my_min，根据实 ...

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

只看该作者 · 2014-4-1 13:27

MPU9150， 9轴sensor，不错，顶

只看该作者 · 2014-4-1 14:42

楼主，你在“第三部分：如何确定自己的轴向”截图部分出自哪个pdf？我想看一下推导过程。
ZYX方式我能在网上找到对应的公式，但是ZXY方式的变化我就弄不明白了。
还有初始化四元数和最后结算的欧拉角公式前的符号，正或负快搞晕了。

只看该作者 · 2014-4-1 15:55

smileage 发表于 2014-4-1 14:42
楼主，你在“第三部分：如何确定自己的轴向”截图部分出自哪个pdf？我想看一下推导过程。
ZYX方式我能在网 ...

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

我记得我代码里，zyx和zxy的方式都有

只看该作者 · 2014-4-1 16:26

zkcaptain 发表于 2014-4-1 10:32
这些方法我现在不用了，到时候更新代码的时候再一起放上来。
灵敏度和偏置都要校准，并且要空间上校准 ...

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

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

只看该作者 · 2014-4-1 16:44

smileage 发表于 2014-4-1 16:26
楼主是准备用空间多点的方法求球心坐标吗？能否先简单谈谈

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

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

只看该作者 · 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) ，这个方法我还不是很理解，但是用起来好像更简单。

只看该作者 · 2014-4-2 11:01

smileage 发表于 2014-4-2 10:36
按照我的理解，空间绕八字的方法不是通过找最大值最小值的方法来实现的。因为要在三维空间中找到最大值和 ...

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

只看该作者 · 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字法，我的观点是通过普通的样本点（不是最大值和最小值）来找圆心（偏置坐标），这个和楼主的想法有出入。

只看该作者 · 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字的绕动方法”和“平面法拓展到空间”的校准公式，降低运算量，并保证一定的精度。不过我很有兴趣试试你的求圆心公式法，不知可否将具体推导公式共享一下。

只看该作者 · 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进行测试，谢谢。

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

技术新星奖章

沉静之湖泊

无冕之王奖章

常驻人口

技术奇才奖章

缘定三生

涓涓之细流

以坛为家

技术高手奖章

精华达人奖章

湍急之河流