【分享心得】9DOF姿态融合四元数欧拉角转换有代码有内涵

zksniper · 发表于 2013-8-30 09:34:08

写这个帖子的目的是分享一下我的经验，没有太多的理论，一切从实际操作出发，写出我对四元数和欧拉角的理解，供比我还新的新手参考，如有错误，请指正，避免误导新人。

我用的算法是Madgwick写的AHRSUpdate和IMUUpdate，简单有效，其中AHRSUpdate是融合了陀螺仪、加速度计和磁力计，而IMUUpdate只融合了陀螺仪和加速度计，但对于做四轴飞行器来说，IMUUpdate也够用了，不过我没发现有用AHRSUpdate的，并且我的一大半的时间都是花在AHRSUpdate这个算法上，直到现在也没用这个算法解算出来正确的四元数和欧拉角，什么方法都试过了，导致我现在都开始怀疑这个算法的正确性，希望用过的高手来指点迷径。

做四轴飞行器也是为了好玩，目前我只完成了第一步：姿态融合。接下来才是更重要的，比如上各种什么机架、电调、电机、螺旋桨之类的，然后写各种PID控制代码，系统整合以后还要调试各种参数，抗干扰，使系统稳定，最后还可能要加各种应用器件。

我现在用的算法：先用IMUUpdate算出Pitch和Roll，然后再结合互补滤波求出Yaw，其中求Yaw的公式参考自john800422。

第一部分：硬件
1.传感器：MPU9150（INVENSENSE公司的，单芯片内集成了加速度计、陀螺仪和磁力计，并且内置DMP用于姿态融合，不过只融合了加速度计和陀螺仪，具有自校准功能，价格比MPU6050贵很多，但是省PCB面积，省事，轴向重合度高。实际上就是把MPU6050和磁力计AK8975放在同一个芯片里，程序还是使用MPU6050的驱动）；
2.MCU：GD32F103CB（Gigadevice公司的，支持国产，与ST同型号的MCU 直接兼容，性价比更高，外接8M晶振，晶振远离传感器，避免干扰磁力计）
3.电源芯片：TLV70233DBVR（TI的LDO，输入2-6V，输出3.3V，只需要外接2个X7R无极性陶瓷电容）
4.串口：MAX3232（方便调试）
5.USB供电

上图：
图1：PCB的3D效果图，实物图就不传了，太丑，跟别人没法比，测试版，先追求调通得出姿态角：

第二部分：软件
1.使用keil，uvision4.1.0，工具链：RealView MDK-ARM Version4.12；
2.库文件：ST官方库STM32F10x_StdPeriph_Driver version V3.3.0；
3.驱动：官方的MPU6050驱动inv_mpu.c和inv_mpu_dmp_motion_driver.c；

先看几个图，然后再说坐标轴的设定和算法部分。

上图：
图1：系统初始化，顺序从上到下依次是：初始化MPU、设置需要使用哪些传感器、设置陀螺仪测量范围（我设的是正负500度/s）、设置加速度计测量范围（我设的是正负4g）、配置fifo、设置采样率、装载DMP、设置陀螺仪轴向(比较重要)、使能DMP的一些玩意儿、设置DMP的FIFO、自校准陀螺仪和加速度计、开启DMP、开始姿态融合，见下图：

图2：由四元数求出的最终姿态角，其中Yaw为航向角，表示机头偏离正北方多少度，范围-180到+180；Pitch为俯仰角，表示机头正方向与水平线的夹角，范围-90到+90；Roll为翻滚角，表示机翼与水平线的夹角，范围：-180到+180。下图为机身水平，且机头正北偏西37度左右的数据：

图3：下图为机翼水平，机头指向正北，且机头向下25度的数据：

图4：下图为机头指向正北，保持水平，且机翼的右翼向下倾斜23度的数据：

图5：对于Yaw来说，代码还不够完善，当机头从南偏东170度到正南，再到南偏西170度时，Yaw会从-180先到0，然后再到+180，如图中红圈部分的数据，而不是我们想象中的-180直接到+180，表现到实际当中就是，当机头转到正南方向时，会先回转到北，再转到南，这个情况要想办法解决。如下图：

图6：看下欧拉角的奇异点，在奇异点处一个转动状态对应无穷多组自由度值，当物体转到这些奇异点附近，便没法求解。图中当Pitch为+90度时，机体的姿态便没法控制，Roll的轴向发生了变化。如下图：

第三部分：如何确定自己的轴向
首先，轴向的定义跟初始化四元数和最后结算的欧拉角有关，跟IMUUpdate四元数更新算法无关，换句话说，不管你的轴向如何定义，IMUUpdate随便用，但是初始化四元数的公式和最后结算欧拉角的公式要做适当的改变，这个后面算法中有说。加速度计也好，陀螺仪也好，磁力计也好，他们的轴向都要满足右手定理，如下图：

再附上一段注释用于解释如何定义合理的轴向，以及如何正确旋转传感器的轴向，解释这么多其实就是说定义好的轴向要满足右手定理，如下图：

下图，旋转前是[x y z]，旋转后就是[-y x z]：

下图是如何确定旋转角度的正方向，用右手握住坐标轴，拇指指向轴向的正方向，四个指头弯曲的方向就是旋转角度的正方向，在初始化四元数时，计算出的欧拉角的正方向也要满足这个条件：

我的程序使用的轴向如下图所示，未作任何改变：

第四部分：算法
第一步是校准，加速度计和陀螺仪我用的是MPU9150内部自校准，磁力计的校准采用如下方法：

第二步是初始化四元数，常见的轴向定义是绕x轴旋转是Roll，绕y轴旋转是Pitch，绕z轴旋转是Yaw，我的程序也是这样定义的，详细说明见程序，这里举个另外一种轴向定义来对初始化四元数进行说明，方便比较。

下面我们来定义绕x轴旋转是Pitch，绕y轴旋转是Roll，绕z轴旋转是Yaw，轴向的正方向如上图一样，不变。
先对加速度计和磁力计的数据进行处理，得到init_xx来供我们使用如下图：

然后通过公式计算出初始的Roll、Pitch、Yaw，注意加负号保证旋转角度的正方向，如下图：

其中Yaw的正方向未必对，可以自己去验证下，具体参考公式见附件-ST电子罗盘计算Yaw：

然后由上面的初始化欧拉角求出初始化四元数，这时要注意旋转顺序的不同，公式也不同，大部分旋转顺序是Z-Y-X，我的程序里也用的这个顺序，在这里我们按Z-X-Y的顺序来旋转，并得出求四元数的公式以做比较，其旋转矩阵:
q=qyaw*qpitch*qroll=
(cos(0.5*Yaw)+ksin(0.5*Yaw)) * (cos(0.5*Pitch)+isin(0.5* Pitch)) * (cos(0.5*Roll)+jsin(0.5* Roll))
得出初始化四元数计算公式如下图所示：

其中i，j，k之间相乘的顺序不能随意变，在前的先计算，在后的后计算相乘的公式如下图：

至此初始化四元数完成。

第三步就是使用IMUUpdate算法了，用完以后再根据公式计算出欧拉角，此公式跟旋转顺序和旋转使用的轴向有关，我们的旋转顺序是Z-X-Y，且绕Z是Yaw，绕X是Pitch，绕Y是Roll，推到过程如下图：
首先得出3个方向余旋矩阵：
下图绕Z轴Yaw：

下图绕X轴pitch：

下图绕Y轴Roll：

然后按照我们的Z-X-Y顺序求得C=Croll * Cpitch * Cyaw，如下图：

将上图的方向余旋矩阵C与下图的四元数姿态矩阵做对比，即可求出欧拉角，注意上图的方向余旋矩阵C是随着我们对坐标轴的定义变化而变化的，而下图的四元数姿态矩阵是固定的：

最后一步就是求出欧拉角，公式如下图：

至此，算法完成，参考如下：
//本程序的读写mpu9150部分是在http://www.amobbs.com/thread-5538389-1-1.html，作者_spetrel的基础上修改而来；
//本程序Yaw的互补滤波出自http://www.amobbs.com/thread-5542278-1-1.html，作者john800422；
//本程序采用的gyro accel融合算法出自作者Madgwick；
在此感谢以上作者的无私分享和帮助！

最后，附上我的代码和AHRS IMU的原始代码：

∨ZERO∧ · 发表于 2013-8-30 10:20:00

讲得很详细，差不多懂了。还要再看看！

sunliezhi · 发表于 2013-8-30 10:26:33

一直在收集这方面的资料，但还未真正开始做。

zksniper · 发表于 2013-8-30 10:39:32

本帖最后由 zksniper 于 2013-8-30 10:40 编辑

其实论坛里讲算法的帖子不少了，但牵涉到实际的东西，都很零碎，一部分还是炫耀贴，对新手没啥帮助，就想写这么个帖子把细节都写出来，尤其是四元数和欧拉角直接的乱七八糟的关系，轴向该如何旋转，懂也好，不懂也好，大家一起讨论讨论。自己每说一点对别人来说都是一个启发。

john800422 · 发表于 2013-8-31 00:11:34

本帖最后由 john800422 于 2013-8-31 00:19 编辑

不錯
支持分享經驗與心得

有時間的話可以試試用橢圓擬合或是橢圓球擬合做電子羅盤的校正

另外我覺得四軸最重要的就三大部分：濾波、姿態、平衡
完成姿態和濾波其實就完成一半了

夏日么么茶 · 发表于 2013-8-31 08:22:55

不错，好好看看先

sedulity11 · 发表于 2013-8-31 08:38:34

不错，:-)

huhulixin · 发表于 2013-8-31 11:22:48

好资料！！！

gaxula123 · 发表于 2013-8-31 15:18:16

好东西！！

linhui102 · 发表于 2013-8-31 19:52:53

知道是好东西，但是还有好多看不懂耶

gaxula123 · 发表于 2013-8-31 21:19:05

不错思路很清晰

想飞的四轴 · 发表于 2013-9-2 08:43:46

楼主我的也写到四元数了但是不知道得出角度后该怎么滤波了？我看到网上有采用卡尔曼滤波直接求解角度值得，我现在是用四元数求得，但是然后该怎么滤波呢？下面是我求解到的地方，接下来该怎么办呀？

seanwood · 发表于 2013-9-2 10:05:49

多轴用9150没太大意义。
加计和陀螺需要靠近重心/气动中心，以减少在多轴晃动时的有害加速度。而机架中心不论金属件、电机、动力供电线都比较密集，容易干扰电子罗盘。
多轴的电子罗盘都和GPS集成在一起，使用非金属支撑杆架高，以获得干净的地磁环境。

zksniper · 发表于 2013-9-2 17:34:14

john800422 发表于 2013-8-31 00:11
不錯
支持分享經驗與心得
有時間的話可以試試用橢圓擬合或是橢圓球擬合做電子羅盤的校正

恩，为了先得出正确的数据，我都没仔细去校准，接下来就好好搞一搞。

zksniper · 发表于 2013-9-2 17:39:52

想飞的四轴发表于 2013-9-2 08:43
楼主我的也写到四元数了但是不知道得出角度后该怎么滤波了？我看到网上有采用卡尔曼滤波直接求解角度值 ...

咱俩进度差不多，我也刚刚得出角度，我的想法是：先不需要滤波，先把算出的角度或四元数直接拿来做控制，飞一飞看看什么效果，反正不会爆炸又，如果效果不理想再考虑是什么原因造成的，然后挨个解决，真需要滤波了再去做呗。

从算出角度开始，往后的其实我也不是很清楚。慢慢看论文里的帖子学着。

zksniper · 发表于 2013-9-2 17:48:00

本帖最后由 zksniper 于 2013-9-2 17:50 编辑

seanwood 发表于 2013-9-2 10:05
多轴用9150没太大意义。
加计和陀螺需要靠近重心/气动中心，以减少在多轴晃动时的有害加速度。而机架中心不 ...

这个当初做板子确实没有考虑到。

就先拿这个板子当个测试板来用吧，下次懂的多了直接一步到位。

顺便问一句：为啥我用AHRSupdate这个算法融合磁力计就算不出来正确的欧拉角呢？公式应该都是没错的啊
具体的帖子在这里：http://www.amobbs.com/thread-5546997-1-4.html

huhulixin · 发表于 2013-9-3 00:28:03

好资料啊~~~

codwest · 发表于 2013-9-4 20:34:50

楼主，厉害

monroeking · 发表于 2013-9-4 23:19:04

正在学习中。。。

_yibei_ · 发表于 2013-9-4 23:37:21

请问楼主Kp和Ki这两个参数对四元数有什么影响，应该怎么调？我调出来角度有漂移，，

zksniper · 发表于 2013-9-5 09:20:19

本帖最后由 zksniper 于 2013-9-5 09:35 编辑

_yibei_ 发表于 2013-9-4 23:37
请问楼主Kp和Ki这两个参数对四元数有什么影响，应该怎么调？我调出来角度有漂移，， ...

哪个角度有漂移？如何漂移法？

这两个参数也没太好的调法，一般kp我是在1-5之内来回调，太大了感觉角度会超调然后再回来，太小了感觉角度变化的慢；kp定下来之后再调ki，
ki的话，我觉得0.001-0.5都可以，感觉上没什么影响，你可以在这个范围试一试，每隔0.1或0.3调1次。
以上都是个人经验，不懂理论，不过我觉得你角度的漂移跟kp和ki关系不大
一般情况下不飞起来的时候，使用原代码中默认的kp=2，ki=0.005就行了，我代码调来调去最后也用的这个默认值，感觉角度不管是反应也好，精确度也好，都还可以

_yibei_ · 发表于 2013-9-5 10:09:44

zksniper 发表于 2013-9-5 09:20
哪个角度有漂移？如何漂移法？

这两个参数也没太好的调法，一般kp我是在1-5之内来回调，太大了感觉角度 ...

请问你是不是用了6050的DMP功能？我逛论坛看见很多人读取6050原始的数据都会有漂移，从而导致四元数融合发生漂移，你估计会不会是这种可能？

zksniper · 发表于 2013-9-5 10:24:47

本帖最后由 zksniper 于 2013-9-5 10:29 编辑

_yibei_ 发表于 2013-9-5 10:09
请问你是不是用了6050的DMP功能？我逛论坛看见很多人读取6050原始的数据都会有漂移，从而导致四元数融合 ...

我用的就是DMP，带自校准，DMP读取的accel和gyro的数据我也看过，gyro的数据没问题，accel是有一个小的偏差的，我取accel的测量范围是正负4g，用DMP读出来的accel的偏差大概在几十LSB左右，不知道这个偏差是不是你所说的漂移。如果你是想要消除这个偏差的话，对accel进行校准和求平均就可以了。这个偏差对求四元数没有太大的影响，顶多就是输出的角度会有一些偏差，但不至于漂移，如果你得出的四元数在静止情况下会随时间发生变化，那可能是算法的问题。不知道你用的什么算法？也是IMUupdate吗？

如果你想说的漂移是数据会随时间缓慢的变化的话，我这边的情况是：DMP读出的数据不会随时间变化。

最好能详细描述一下你的漂移的状况。是如何漂移的

_yibei_ · 发表于 2013-9-5 11:03:48

嗯，我也用的是IMUupdate这个函数，感觉应该不是四元数的问题，我遇到的问题是刚开始角度还比较稳定，但是当转动角度，或是过个几分钟，角度就会大范围的移动，我没有用DMP，现在正调呢，但也遇到了问题，至今没有出来效果。你的DMP程序能给看一下吗？就几天的时间，作比赛做个飞控真心有难度，，

zksniper · 发表于 2013-9-5 11:08:12

_yibei_ 发表于 2013-9-5 11:03
嗯，我也用的是IMUupdate这个函数，感觉应该不是四元数的问题，我遇到的问题是刚开始角度还比较稳定，但是 ...

我的代码帖子里有，在楼主位，代码里用的就是DMP。

如果你的accel和gyro数据没问题，那就是你算初始化四元数的公式或者最后结算欧拉角的公式有问题了，可以先好好看下这个帖子。

_yibei_ · 发表于 2013-9-5 12:01:41

zksniper 发表于 2013-9-5 11:08
我的代码帖子里有，在楼主位，代码里用的就是DMP。

如果你的accel和gyro数据没问题，那就是你算初始化四 ...

好的，我用你的程序试了下，前面都没有问题，但是到那个自校准函数 run_self_test();就过不去了，打印出来bias has not been modified ......请问这个是什么问题啊？真的谢谢了，做比赛伤不起啊，，

pipi516 · 发表于 2013-9-5 12:08:45

不错！支持下！

zksniper · 发表于 2013-9-5 12:15:13

本帖最后由 zksniper 于 2013-9-5 12:17 编辑

_yibei_ 发表于 2013-9-5 12:01
好的，我用你的程序试了下，前面都没有问题，但是到那个自校准函数 run_self_test();就过不去了，打印出 ...

一：在程序执行到这里如果你移动了板子，就会无法校准
二：你需要修改一下static void run_self_test(void)这个函数下的以下代码：
result = mpu_run_self_test(gyro, accel);
if (result == 0x7 )
其中result的值要根据你的传感器进行修改。参考datasheet，并仔细看函数mpu_run_self_test(gyro, accel)，debug一下，看mpu_run_self_test(gyro, accel)的返回值是多少，result就是多少。

我是为了调试方便，只要出错就会进入死循环，看哪里出错了。

实在着急可以先将自校准注销掉，看看后面的结果如何。

_yibei_ · 发表于 2013-9-5 13:31:48

zksniper 发表于 2013-9-5 12:15
一：在程序执行到这里如果你移动了板子，就会无法校准
二：你需要修改一下static void run_self_test(voi ...

嗯，搞出来了，改成0就可以了，真心感谢。谢谢了，，

zksniper · 发表于 2013-9-5 13:54:43

本帖最后由 zksniper 于 2013-9-5 13:56 编辑

_yibei_ 发表于 2013-9-5 13:31
嗯，搞出来了，改成0就可以了，真心感谢。谢谢了，，

其实mpu_run_self_test我也没看的太明白，不过个人觉得result最后等于0是不对的，当初我调的时候result也等于0过，那时候好像是我焊接有问题，我的mpu9150没有焊好；

如果你的传感器是mpu6050或者mpu9150，且只用gyro和accel的话，result应该=3才对；

如果你的传感器是mpu9150，且gyro、accel和mag都用了的话，result应该=7；

不过你还是先看下后面的结果吧，这个自校准可以回头再看。

cshp138 · 发表于 2013-9-5 18:20:29

LZ不是用DMP输出四元数得到欧拉角吗，怎么又用AHRSUpdata来搞四元数？还是你用了两个程序来对比效果？

liang_work · 发表于 2013-9-5 19:15:37

好贴，记下，顶上。

_yibei_ · 发表于 2013-9-5 19:21:22

zksniper 发表于 2013-9-5 13:54
其实mpu_run_self_test我也没看的太明白，不过个人觉得result最后等于0是不对的，当初我调的时候result也 ...

嗯，我用的是6050模块，所以电路应该没有问题。输出的角度我看了下，静止的时候角度非常稳定，但是当抖动过大的时候角度就会飘的有点大，还有就是角度恢复的有点慢，这个是什么原因？

kxm2008 · 发表于 2013-9-5 19:38:48

本帖最后由 kxm2008 于 2013-9-5 19:46 编辑

楼主大爱，初学者的福音，谢谢

zksniper · 发表于 2013-9-6 13:20:49

cshp138 发表于 2013-9-5 18:20
LZ不是用DMP输出四元数得到欧拉角吗，怎么又用AHRSUpdata来搞四元数？还是你用了两个程序来对比效果？ ...

我程序用的是IMUupdate来融合的姿态，AHRSupdate加上磁力计的我没搞出来。

我程序里的DMP只是用来读取出经过自校准的accel和gyro的数据而已。

当然我程序里被我注释掉的部分也有用DMP读取四元数并转化为欧拉角的代码。

zksniper · 发表于 2013-9-6 13:25:50

本帖最后由 zksniper 于 2013-9-6 13:27 编辑

_yibei_ 发表于 2013-9-5 19:21
嗯，我用的是6050模块，所以电路应该没有问题。输出的角度我看了下，静止的时候角度非常稳定，但是当抖动 ...

先调一下Kp和Ki这两个参数，看有没有效果。

不行的话，还有halfT这个参数，要用Timer来计时，比如你一次四元数更新用了T秒，那么halfT=T/2，其实这个halfT可以固定在0.002秒左右，效果会好，即便是用Timer也没那么准确。

我对halfT这个参数理解的不是很好，只是觉得把它调到能让我的数据比较好就行了。

_yibei_ · 发表于 2013-9-7 02:57:57

zksniper 发表于 2013-9-6 13:25
先调一下Kp和Ki这两个参数，看有没有效果。

不行的话，还有halfT这个参数，要用Timer来计时，比如你一次 ...

嗯，还有一个问题，当我的传感器静止下来，每次过个几十秒，角度都会有一个2到3度的跳变，这个是什么原因啊，应该怎么解决？我试着调了下Kp和Ki，都消除不了这个角度跳变，，

sedulity11 · 发表于 2013-9-7 09:48:33

mark下来，电脑上细看

陈敏锐 · 发表于 2013-9-7 16:07:12

好贴，果断收藏

SZHjy · 发表于 2013-9-7 23:29:31

多谢楼主

twitter · 发表于 2013-9-7 23:43:25

john800422 发表于 2013-8-31 00:11
不錯
支持分享經驗與心得
有時間的話可以試試用橢圓擬合或是橢圓球擬合做電子羅盤的校正

有个问题请教一下，不同地点的重力加速度和地磁应该是不同的吧，是不是意味着9轴在不同的地方使用前都得先校准一下，才能准确测量？

john800422 · 发表于 2013-9-8 00:46:03

twitter 发表于 2013-9-7 23:43
有个问题请教一下，不同地点的重力加速度和地磁应该是不同的吧，是不是意味着9轴在不同的地方使用前都得 ...

是的
若是要準確測量的話
最好使用前都先校正一下

若是不要求精確的話
相同地區&相同高度, 重力差別並不大, 加速度計重新校正的需求就不是這麼大
磁場的話, 除了地區, 還要看環境, 環境影響大的話, 就需要對羅盤重新做校正了

twitter · 发表于 2013-9-8 12:59:23

john800422 发表于 2013-9-8 00:46
是的
若是要準確測量的話
最好使用前都先校正一下

还有个问题麻烦一下，我移植的Pansenti的MPU9150Lib，校正量程的算法是最大最小除2的方式，静止状态，AK8975计算出的Yaw角度值是在一个6度范围内震荡的，这是合理的情形么？

jib943 · 发表于 2013-9-9 05:14:55

好帖顶起来让其他人看到

zksniper · 发表于 2013-9-9 11:20:47

_yibei_ 发表于 2013-9-7 02:57
嗯，还有一个问题，当我的传感器静止下来，每次过个几十秒，角度都会有一个2到3度的跳变，这个是什么原因 ...

具体哪个角度会随时间跳变呢？是3个角度都会随时间有2-3度的跳变？

你的加速度计、陀螺仪都经过校准了吗？

如果你的result=0x0的话，那么你的加速度计和陀螺仪应该是没经过DMP内部自校准的，你可以看看你的加速度计和陀螺仪的原始数据，与理论值是否有偏差，

我觉得你说的这个偏差有可能是校准的问题

zksniper · 发表于 2013-9-9 11:23:09

twitter 发表于 2013-9-8 12:59
还有个问题麻烦一下，我移植的Pansenti的MPU9150Lib，校正量程的算法是最大最小除2的方式，静止状态，AK8 ...

你的yaw是怎么求的呢？如果你加入了磁力计，并且磁力计没有受到太大干扰的情况下，在6度范围内震荡是有问题的。

zksniper · 发表于 2013-9-9 11:43:58

正常情况下，静止状态下的数据如下：

下图是刚上电时候的数据：

下图是上电半个小时以后的数据：

可以看出正常的静止状态下，数据的波动范围是不超过1度的，也不会有漂移。

如果静止状态下的结果和想象中的有差别的话，那应该是算法和校准的问题。

其实只要算法和校准做好了，静止状态下大家都差不多，上了电机以后才会出现一些差异化的问题。

snowblow · 发表于 2013-9-9 13:01:47

支持分享

twitter · 发表于 2013-9-9 17:53:09

本帖最后由 twitter 于 2013-9-9 18:03 编辑

zksniper 发表于 2013-9-9 11:23
你的yaw是怎么求的呢？如果你加入了磁力计，并且磁力计没有受到太大干扰的情况下，在6度范围内震荡是有问 ...

9150的磁力计mpu_get_compass_reg返回的原始数值跳动范围的话，X轴、Y轴、Z轴都在8以内。水平放在桌面上。

我这里测出的数值最大最小值范围是：

calData->magMaxX = 181;
calData->magMinX = -153;
calData->magMaxY = 171;
calData->magMinY = -155;
calData->magMaxZ = 60;
calData->magMinZ = -271;

融合算法是Pansenti 的 MPU9150Lib里移植的，没改动。

https://github.com/Pansenti/MPU9150Lib/tree/master/libraries/MPU9150Lib

_yibei_ · 发表于 2013-9-9 18:21:12

zksniper 发表于 2013-9-9 11:20
具体哪个角度会随时间跳变呢？是3个角度都会随时间有2-3度的跳变？

你的加速度计、陀螺仪都经过校准了吗 ...

嗯，真的非常感谢，我也估计是没经过DMP内部自校准的，不过没关系，我们已经比完赛，有的是时间给我慢慢的查找问题，，

li873719015 · 发表于 2013-9-9 19:00:11

好帖需顶

ding_ding0304 · 发表于 2013-9-9 22:46:08

谢谢楼主，虽然还看不太懂，马上要做四轴肯定会用到

zksniper · 发表于 2013-9-10 09:23:50

twitter 发表于 2013-9-9 17:53
9150的磁力计mpu_get_compass_reg返回的原始数值跳动范围的话，X轴、Y轴、Z轴都在8以内。水平放在桌面上 ...

首先你的磁力计最好校准一下，最起码，x和y的最大值和最小值的绝对值要相等或者差不多大小，不过我觉的这个不是你问题的根本，你可以先对磁力计校准一下看看。

yaw在6度以内震荡的话，应该是算法的问题。或许对于yaw的融合算法，有些地方需要你去尝试着调整。

lqjesse · 发表于 2013-9-10 16:37:08

楼主，请问用内部的DMP和IMUupdate结果有什么区别？

zksniper · 发表于 2013-9-10 17:24:17

本帖最后由 zksniper 于 2013-9-10 17:27 编辑

lqjesse 发表于 2013-9-10 16:37
楼主，请问用内部的DMP和IMUupdate结果有什么区别？

内部的DMP得出的四元数不太好用，转化为欧拉角以后会有下面这个问题：
板子的角度稍微变化一下，DMP输出的角度就会发生很大变化，然后再慢慢的回到对应板子的角度。
举个栗子：
当我的板子Roll倾斜30度时，DMP输出的Roll角度可能会达到80度，甚至150度，然后再慢慢回到30度，达到稳定。。

IMUupdate可以通过调节kp和ki来改善这个问题，DMP的我不知道该如何改善。

用mpu6050或mpu9150的可以自己实际测一下，也有可能我这种情况是个个例。
之前不是有人发过一个帖子吗？说DMP也挺好用的，能很好的跟踪并反应角度的变化，我自己实际测出来的情况就是不好用。

zerok · 发表于 2013-9-17 16:52:23

zksniper 发表于 2013-9-5 13:54
其实mpu_run_self_test我也没看的太明白，不过个人觉得result最后等于0是不对的，当初我调的时候result也 ...

hi，lz，我看motion driver里说，self test返回值为0时，表示自测试成功呢。

zerok · 发表于 2013-9-17 16:57:06

zksniper 发表于 2013-9-9 11:43
正常情况下，静止状态下的数据如下：

我把lz的程序移植到32上面，pitch和roll结果也不错，但是yaw方向的角度基本不会变，我只是改了lz对单片机的一些驱动，算法上没动，不知道是我哪里给弄错了。

zksniper · 发表于 2013-9-17 17:30:54

zerok 发表于 2013-9-17 16:52
hi，lz，我看motion driver里说，self test返回值为0时，表示自测试成功呢。

但是我进入mpu_run_self_test，看代码的流程结果就是result=0x07，并且官方的Embedded_MotionDriver_5.1代码里给出的result也是0x07，如下：

zksniper · 发表于 2013-9-17 17:32:35

zerok 发表于 2013-9-17 16:57
我把lz的程序移植到32上面，pitch和roll结果也不错，但是yaw方向的角度基本不会变，我只是改了lz对单片机 ...

你用的传感器型号是什么？带磁力计吗？我程序里是融合了磁力计的

landmuto · 发表于 2013-9-17 21:16:09

mark！！！！

zerok · 发表于 2013-9-21 14:11:43

zksniper 发表于 2013-9-17 17:32
你用的传感器型号是什么？带磁力计吗？我程序里是融合了磁力计的

我用的9050，没内部磁力计，打算外接一个，但是没找到太多的参考资料呢，还在摸索中、、、

zksniper · 发表于 2013-9-22 09:15:15

zerok 发表于 2013-9-21 14:11
我用的9050，没内部磁力计，打算外接一个，但是没找到太多的参考资料呢，还在摸索中、、、 ...

没接磁力计的话，用imuupdate算法，yaw不会变化很正常。我不用磁力计测的yaw角也是这样。你可以试试DMP自带的读出四元数的代码。

panzhengsheng · 发表于 2013-9-22 19:25:39

感谢楼主无私解答问题

panzhengsheng · 发表于 2013-9-22 20:02:20

你好,请问你用MPU6050时,有用到INT脚吗?谢谢!

zksniper · 发表于 2013-9-24 17:15:08

panzhengsheng 发表于 2013-9-22 20:02
你好,请问你用MPU6050时,有用到INT脚吗?谢谢!

没有～～～～～～

richards · 发表于 2013-9-24 17:30:16

值得一看谢谢

zerok · 发表于 2013-9-24 17:52:28

zksniper 发表于 2013-9-22 09:15
没接磁力计的话，用imuupdate算法，yaw不会变化很正常。我不用磁力计测的yaw角也是这样。你可以试试DMP ...

lz，请教个问题哈，程序里面的IIC器件地址是在什么地方定义的呢？

zksniper · 发表于 2013-9-25 08:55:00

zerok 发表于 2013-9-24 17:52
lz，请教个问题哈，程序里面的IIC器件地址是在什么地方定义的呢？

inv_mpu.c下第525行，

const struct hw_s hw={
0x68, //addr
1024, //max_fifo
118, //num_reg
340, //temp_sens
-521, //temp_offset
256 //bank_size
};

复制代码

panzhengsheng · 发表于 2013-9-26 23:27:36

zksniper 发表于 2013-9-24 17:15
没有～～～～～～

好的,谢谢!

kmani · 发表于 2013-9-28 11:52:28

楼主大好人啊，先顶再看。

szcszc55 · 发表于 2013-9-28 19:54:14

好东西，顶一下！！

tree666 · 发表于 2013-9-30 00:13:48

不错！感谢楼主！

想跟楼主确定下是不是IMUupdate和AHRSupdate算法和坐标轴定义无关，只要是右手定则的都行？

比如我的定义是北东地（NED），也可以直接使用？需要注意什么呢？

zksniper · 发表于 2013-9-30 09:21:03

本帖最后由 zksniper 于 2013-9-30 09:31 编辑

tree666 发表于 2013-9-30 00:13
不错！感谢楼主！

想跟楼主确定下是不是IMUupdate和AHRSupdate算法和坐标轴定义无关，只要是右手定则的都 ...

是的，满足右手定则就行，如果你要确定绕哪个轴是pitch，哪个轴是roll，还需要再接着推导一下初始化四元数的公式

方便的话，尽量让你的所有传感器的xyz轴都和北东地一致，如果不一致，那在同一个轴向的相反方向也行，比如陀螺仪的x轴指向北，那么加速度计或磁力计的x轴也可以指向南，然后修改一下后面的公式，我记得一般只是修改正负号。

具体推到你可以好好看看这个帖子琢磨一下，个人建议先试试IMUupdate，因为我觉得加入磁力计后AHRSupdate有问题，不管我怎么算AHRSupdate都得不出我想要的结果。

JESTER9 · 发表于 2013-9-30 09:21:53

多谢分享

zerok · 发表于 2013-10-7 09:40:32

zksniper 发表于 2013-9-25 08:55
inv_mpu.c下第525行，

谢谢！

haphard · 发表于 2013-10-7 11:57:04

四轴，好资料

liuziang66 · 发表于 2013-10-8 22:05:40

谢谢楼主看了楼主的讲解又多明白了一点

cpholr1 · 发表于 2013-10-9 12:39:53

留下脚印

JESTER9 · 发表于 2013-10-11 11:05:16

好贴，记下，顶上。

janeanndavid · 发表于 2013-10-12 22:32:24

mark........

cshp138 · 发表于 2013-10-15 10:52:54

想问问LZ用DMP自校正输出accgyro来作IMUupdate的话，更新速率不会更不上吗，DMP最多200HZ而且你还要进一步融合处理6轴数据。

zksniper · 发表于 2013-10-16 10:27:52

本帖最后由 zksniper 于 2013-10-16 10:29 编辑

cshp138 发表于 2013-10-15 10:52
想问问LZ用DMP自校正输出accgyro来作IMUupdate的话，更新速率不会更不上吗，DMP最多200HZ而且你还要进一步 ...

DMP是由MPU来处理的，MCU只是来读取它，姿态更新速率跟你的代码和MCU工作频率有很大关系，比如你代码里用了大量的浮点运算、开方、三角函数或延迟等，这些都会显著降低你的姿态更新速率。举个例子：磁力计HMC5883L，output rate是160Hz，如果我们用它来做姿态融合，那岂不是我们的更新速率只能低于160Hz了？而且更新速率也不是说一定要追求越快越好，适用就行了。

cshp138 · 发表于 2013-10-16 11:41:51

zksniper 发表于 2013-10-16 10:27
DMP是由MPU来处理的，MCU只是来读取它，姿态更新速率跟你的代码和MCU工作频率有很大关系，比如你代码里用 ...

多谢回复，主要是目前通过DMP输出的ACCGYRO来作IMUdate，得到效果有明显滞后的现象，调KP,KI也不行。难道LZ不会这样？

Forever · 发表于 2013-10-16 12:43:19

好东西。收藏了！

zksniper · 发表于 2013-10-16 12:46:54

cshp138 发表于 2013-10-16 11:41
多谢回复，主要是目前通过DMP输出的ACCGYRO来作IMUdate，得到效果有明显滞后的现象，调KP,KI也不行。难道 ...

用的也是IMUupdata这个算法？halfT怎么定义的？

cshp138 · 发表于 2013-10-16 15:10:45

zksniper 发表于 2013-10-16 12:46
用的也是IMUupdata这个算法？halfT怎么定义的？

对啊，用系统时间计算运算姿态周期的一半

王小才 · 发表于 2013-10-16 16:39:13

好贴，感谢

zksniper · 发表于 2013-10-16 16:39:54

cshp138 发表于 2013-10-16 15:10
对啊，用系统时间计算运算姿态周期的一半

#define Kp 2.0f
#define Ki 0.005f
这两个我用的默认的值，没问题，你可以多调一调这两个数。

kmani · 发表于 2013-10-21 13:49:03

楼主，太感谢你了，好帖子。

yue621 · 发表于 2013-11-18 22:41:36

支持楼主~~！我也一直想研究但是还没有开始，。。。。。。

ipaner · 发表于 2013-11-21 10:03:05

john800422 发表于 2013-8-31 00:11
不錯
支持分享經驗與心得
有時間的話可以試試用橢圓擬合或是橢圓球擬合做電子羅盤的校正

太对了，准确的姿态信息可以说是四轴控制的核心。至于控制算法和调参，从工作量的角度还占不到一半

287355586 · 发表于 2013-11-30 15:29:00

牛逼牛逼

wenziheni · 发表于 2013-11-30 20:19:58

好东西收下了

shenfeipascal · 发表于 2013-11-30 23:58:46

学习了，多谢，资料先收下了

zksniper · 发表于 2013-12-2 10:47:04

本帖最后由 zksniper 于 2013-12-2 14:53 编辑

更新-采用AHRSUpdate算法进行姿态融合：
这段时间一直没有太多时间搞这玩意，不过苦恼多日的AHRSUpdate算法问题终于解决了，借鉴了billhsu对AHRS算法的改进，该算法直接融合9轴数据，包括加速度计、陀螺仪和磁力计，其中0°<yaw<360°, -90°<pitch<+90°, -180°<roll<180°，并且我定义：Yaw北偏西为正，pitch往上为正，roll“右翼”下沉为正。传感器坐标轴：绕y旋转是roll，绕x旋转为pitch，绕z旋转为yaw。代码里只有姿态更新，更新频率350-400Hz，这是跑72Mhz频率下的数据，跑108M时，姿态更新频率在400-500Hz之间。

说下改进：
1.由于IMUupdate算法只融合了加速度计和陀螺仪的数据，因此我用了互补滤波来融合磁力计以修正偏航角，但是这种互补滤波有个小问题，就是假如定义偏航角的范围是0-360度，那么当机头大概从北偏西1度转到北偏东364度时，机头会经过0度(360度)这个点，那么这时，yaw不会直接1-0-364这么变化，而是会被逆向积分从1-20-180-270-364这么转一圈，这是个不好的现象，实验了半天也没有解决，而AHRSupdate很好的解决了这个问题。
2.放弃了DMP输出，DMP输出会有部分数据丢失现象，输出频率只有200Hz，虽然也够用，但还是太低了。我采用I2C直接读取raw数据，做了简单的陀螺仪零偏校准，加上了加速度计的平滑滤波，一下步准备加入磁力计的椭球拟合校准。
3.考虑到电机的转动频率是100Hz以上，因此我采用加速度计的低通滤波(DLPF)为98Hz，可以有效滤掉电机振动带来的影响，结合平滑滤波效果应该不错。

说下AHRSUpdate算法的缺点：
由于把磁力计的数据融合到了pitch和roll里面，那么当磁力计突然收到强磁场干扰时，会导致四轴失控。亲测过拿手机等物品靠近飞控板时，pitch、roll和yaw的数据有明显变化，变化程度跟物品具有的磁场强度成正比，不过应该是有算法可以滤掉这些固定的外部干扰的。
而IMUupdate只把磁力计融合进了yaw，那么即便有强磁场干扰也不会造成全方位的失控。

按照惯例，上代码，代码里有详细注释，此代码跑的是108Mhz，想用72M可以自己在system_stm32f10x.c下修改：

monroeking · 发表于 2013-12-3 16:58:29

zksniper 发表于 2013-12-2 10:47
更新-采用AHRSUpdate算法进行姿态融合：
这段时间一直没有太多时间搞这玩意，不过苦恼多日的AHRSUpdate算法 ...

楼主的这篇贴子看了好几遍，想通了很多，这次更新很给力！顶起来！

clcicoccad · 发表于 2013-12-5 23:52:40

讲得太好了，好好学习学习

ZMHCISS · 发表于 2013-12-6 12:46:51

楼主厉害，谢谢提供

soniclidi · 发表于 2013-12-6 18:43:11

标记，留着以后看

yixin1851 · 发表于 2013-12-9 11:51:48

看了一遍，不是很懂

【分享心得】9DOF姿态融合四元数欧拉角转换有代码有内涵

本帖子中包含更多资源

阿莫论坛20周年了！感谢大家的支持与爱护！！

本帖子中包含更多资源

本帖子中包含更多资源

本帖子中包含更多资源

本帖子中包含更多资源

本帖子中包含更多资源

本帖子中包含更多资源

【分享心得】9DOF姿态融合 四元数 欧拉角转换 有代码有内涵

本帖子中包含更多资源

阿莫论坛20周年了！感谢大家的支持与爱护！！

本帖子中包含更多资源

本帖子中包含更多资源

本帖子中包含更多资源

本帖子中包含更多资源

本帖子中包含更多资源

本帖子中包含更多资源

【分享心得】9DOF姿态融合四元数欧拉角转换有代码有内涵