xuyulei008
发表于 2013-12-9 22:05:52
楼主好人
绝版彬仔
发表于 2013-12-9 23:10:33
赞一个............
a457738242
发表于 2013-12-18 09:41:10
楼主 我的三个欧拉角在初始化之后就一直乱跳 Yaw是因为我没用磁力计补偿但是 pitch 和roll也乱跳就算静止也是这样 能指导下么
zksniper
发表于 2013-12-18 09:56:19
a457738242 发表于 2013-12-18 09:41
楼主 我的三个欧拉角在初始化之后就一直乱跳 Yaw是因为我没用磁力计补偿但是 pitch 和roll也乱跳就算静 ...
如果是代码问题,那你需要把你的代码贴出来,如果是理论问题,那你需要详细说明情况。
a457738242
发表于 2013-12-18 11:04:21
zksniper 发表于 2013-12-18 09:56
如果是代码问题,那你需要把你的代码贴出来,如果是理论问题,那你需要详细说明情况。 ...
我刚刚找到原因了我直接把陀螺仪的值用度/秒带到四元数更新公式里了 改成弧度就好多了 不过想请问楼主 磁力计如何校准呢?
zksniper
发表于 2013-12-18 11:34:27
本帖最后由 zksniper 于 2013-12-18 11:40 编辑
a457738242 发表于 2013-12-18 11:04
我刚刚找到原因了我直接把陀螺仪的值用度/秒带到四元数更新公式里了 改成弧度就好多了 不过想请问楼主...
比较好的办法是做椭球拟合,可以找john800422请教,要求不高的可以参考我帖子里的方法,大四轴我没试过电机对磁力计到底有多少干扰,到时候再说,不上电机校准磁力计没啥用,唯一的作用就是看看数对不对。
真要费劲去校准的话,那么电机不同转速下磁场强度也不同,磁力计受到的干扰也不同,光靠一次静态校准是不够的。需要收集不同电机转速时的磁场情况,做类似于全温度补偿,甚至要引进补偿算法,反正我没见到过这种动态校准补偿磁力计的。
a457738242
发表于 2013-12-18 13:13:14
zksniper 发表于 2013-12-18 11:34
比较好的办法是做椭球拟合,可以找john800422请教,要求不高的可以参考我帖子里的方法,大四轴我没试过电 ...
不是很理解 初始化时候 的Yaw计算 init_Yaw=atan2(init_mx*cos(init_Roll) + init_my*sin(init_Roll)*sin(init_Pitch) + init_mz*sin(init_Roll)*cos(init_Pitch),
init_my*cos(init_Pitch) - init_mz*sin(init_Pitch));//类似于atan2(my, mx),其中的init_Roll和init_Pitch是弧度
楼主能解释下么
bygreencn
发表于 2013-12-18 14:13:10
讲得很详细
zksniper
发表于 2013-12-18 15:28:57
a457738242 发表于 2013-12-18 13:13
不是很理解 初始化时候 的Yaw计算 init_Yaw=atan2(init_mx*cos(init_Roll) + init_my*sin(init_R ...
这个是利用加速度计和磁力计来初始化的Yaw。
一般求yaw如果你打算让磁力计x轴指向正北为0度,那么就用yaw=atan2(my/mx)来求,但是当你的传感器不是处于绝对水平状态时,用这个公式求出来的初始化yaw就不对,也就是说这个公式只是用于传感器处于水平状态,没有向上和向下的倾角的时候。
那么用什么公式能够保证不管你传感器处在什么样的位置,都能够正确的初始化yaw呢?这时候就需要利用加速度计和磁力计配合算出实际的my和mx了,具体公式就是先利用加速度计求出静止时的roll和pitch,然后将(mx、my)与(roll、pitch)结合算出带有倾角的(mx' my'),最后将(mx' my')带入公式yaw=atan2(my/mx)就行了,具体公式参考我帖子里上传的pdf
a457738242
发表于 2013-12-18 19:42:25
zksniper 发表于 2013-12-18 15:28
这个是利用加速度计和磁力计来初始化的Yaw。
一般求yaw如果你打算让磁力计x轴指向正北为0度,那么就用ya ...
好的 谢谢楼主
rcdny123
发表于 2013-12-18 20:03:23
备份一下,下次用到。
a457738242
发表于 2013-12-18 21:09:15
zksniper 发表于 2013-12-18 11:34
比较好的办法是做椭球拟合,可以找john800422请教,要求不高的可以参考我帖子里的方法,大四轴我没试过电 ...
楼主 我参考了你的IMU 算法 但是 发现Yaw的输出几乎不变化 不明白为什么..按理说 直接对Yaw用陀螺仪进行积分应该也会有明显变化吧
a457738242
发表于 2013-12-18 21:28:56
找到问题了 犯了个小错误 嘿嘿
brentcao
发表于 2013-12-19 00:03:53
楼主辛苦了
a457738242
发表于 2013-12-25 19:37:02
楼主 Yaw 从180度到-180度变化的时候会先从180到0再到-180的问题解决了么?
zksniper
发表于 2013-12-26 09:14:32
a457738242 发表于 2013-12-25 19:37
楼主 Yaw 从180度到-180度变化的时候会先从180到0再到-180的问题解决了么?
见95楼·········
a457738242
发表于 2013-12-26 10:03:41
这个问题 我昨天晚上解决了 没有用AHRSUpdate的方法 对IMU稍作改进即可但是现在碰到的问题是 通过磁力计和陀螺仪互补滤波计算出来的Yaw 角度一大就不准了 已经将磁场分解了 还是这样
zksniper
发表于 2013-12-26 10:13:42
本帖最后由 zksniper 于 2013-12-26 10:16 编辑
a457738242 发表于 2013-12-26 10:03
这个问题 我昨天晚上解决了 没有用AHRSUpdate的方法 对IMU稍作改进即可但是现在碰到的问题是 通过磁力计 ...
分解没用,我试过,总有一个象限里会出现同样的错误,至今也没找到解决方法
回头你搞定了,别忘了告诉我怎么做的,我学习学习。
a457738242
发表于 2013-12-26 10:16:59
zksniper 发表于 2013-12-26 10:13
分解没用,我试过,总有一个象限里会出现同样的错误,至今也没找到解决方法
回头你搞定了,别忘了告诉我 ...
嗯不过 在小角度范围内还是相对比较准的 可能对四轴来说勉强能用
roothoo
发表于 2013-12-26 22:11:02
LZ写得很好,我有些问题想请教LZ
请问这段代码的原理是什么?
=Q**Q*
是将磁力计测得的机体坐标轴下的值转换成大地坐标系下的
bx by bz是什么意义?
为何b=?
wx,wy,wz为何这样计算呢?
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
chushibinsaobao
发表于 2013-12-29 11:14:43
感谢楼主分享,谢谢
robotkid
发表于 2013-12-29 11:25:49
好淫,站一个
victor9079
发表于 2013-12-29 20:41:47
正需要,先收藏下,有空慢慢看
zksniper
发表于 2013-12-30 10:21:14
本帖最后由 zksniper 于 2013-12-30 10:25 编辑
roothoo 发表于 2013-12-26 22:11
LZ写得很好,我有些问题想请教LZ
请问这段代码的原理是什么?
=Q**Q*
是将磁力计测得的机体坐标轴下的值转换成大地坐标系下的
bx by bz是什么意义?
为何b=?
wx,wy,wz为何这样计算呢?
bx by bz是地理坐标系下的磁场矢量的参考值,理论上地球磁场的分量只有由南向北和指向地心,没有东西向的磁场分量,因此如果你规定bx指向正北,那么只有bx和bz有值,by=0,bx可由地理坐标系下测量值hx和hy计算得到(直角三角形矢量合成),而bz就等于hz,都是垂直向下的。
wx wy wz是机体坐标系下的参考值,是由地理坐标系下的参考值bx by bz经过四元数矩阵旋转过来的,然后通过机体wx wy wz(参考值)和机体mx my mz(测量值)求叉积得到误差量用来修正陀螺仪,如下:
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
AI谁谁
发表于 2014-1-5 10:08:09
mark 一遍没全懂
xl1736
发表于 2014-1-10 22:21:05
非常好,顶起来,顺便收藏了!
yinian
发表于 2014-1-18 10:58:09
嗯, 非常好,谢谢啊
tianyiran02
发表于 2014-1-20 05:43:57
太感动了。先留名,放假了就开始干!!!!
兄弟会十戒之首
发表于 2014-1-27 18:58:00
请接收我全身心的供奉,我现在搞6050,真的是毫无头绪。 拜读了,收下了!
兄弟会十戒之首
发表于 2014-1-27 19:31:46
一遍看完了,保存成图片,下下来接着看,还有很多不明白!
也无风雨赵
发表于 2014-2-19 15:17:18
不错,收藏了以后容易找!
木君之上
发表于 2014-2-19 19:29:28
讲的真的太好了,对于初学的人是很好的资料,特别是那些数据的处理,真的非常感谢
木君之上
发表于 2014-2-19 19:44:08
zksniper 发表于 2013-9-5 10:24
我用的就是DMP,带自校准,DMP读取的accel和gyro的数据我也看过,gyro的数据没问题,accel是有一个小的偏 ...
我也是,用了DMP和自检后,放十几分钟也不会飘,一直在0.1左右,挺好的,就是速度慢了些,才200Hz,
木君之上
发表于 2014-2-19 19:52:40
zksniper 发表于 2013-9-10 17:24
内部的DMP得出的四元数不太好用,转化为欧拉角以后会有下面这个问题:
板子的角度稍微变化一下,DMP输出 ...
我的也有这个问题,不知道是上位机反应慢还是怎么,我晃动的时候数据跟着变,但是总是慢半拍,等我静下来的时候数据基本要过好几秒才慢慢回到0,(飘逸不严重,一般能回到0.1以下),等哪天再试一下,
木君之上
发表于 2014-2-19 22:30:20
楼主,不好意思还想请教一点,大家现在的姿态控制都用的是PID,这样的话就需要用到陀螺仪和加速度计的读数,如果使用了DMP的话,直接读出四元数,还得换算回去变成陀螺仪和加速度计的值,感觉好别扭,好像DMP也可以不用四元数,也可以读取陀螺仪的数值,但是不知道使用DMP读出来的陀螺仪的数值和不使用DMP的有没有区别,如果没区别的话那就太失败了。不知道有没有直接利用四元数进行姿态控制的,或者说利用四元数换算出来的陀螺仪读数比直接读的要好?我记不清了在哪儿看见过谁说过四元数加扩展卡尔曼滤波这个说法,不知道是什么,也不知道是不是也会回到陀螺仪读数上面去然后用PID去控制,说的有点多了。。。
zksniper
发表于 2014-2-20 09:22:19
木君之上 发表于 2014-2-19 22:30
楼主,不好意思还想请教一点,大家现在的姿态控制都用的是PID,这样的话就需要用到陀螺仪和加速度计的读数 ...
1.你可以试试用欧拉角(pitch,roll,yaw)来进行PID控制,先将四元数转化为欧拉角,然后再PID。这是我见过的最多的版本,你说的用加速度计和陀螺仪的数值来PID的我还真没见过,有一种方法是直接对陀螺仪的角速度进行微分得到角度,不过这也是对角度的PID,我还没见过直接对传感器裸数据进行PID的,或许是你理解有误?难道你说的“陀螺仪和加速度计的读数”是指欧拉角?
2.经过DMP出来的陀螺仪数值是校准后的,没经过DMP的是未校准的,当然经过DMP出来的陀螺仪数值好了,不过这种校准也可以自己通过代码来做。
3.通过DMP可以输出欧拉角、四元数或者/加速度计和陀螺仪的数值/。
4.有人提过直接用四元数进行PID,这样可以避免欧拉角的奇点,不过我也不懂,也不知道能不能做成。
木君之上
发表于 2014-2-20 09:41:03
zksniper 发表于 2014-2-20 09:22
1.你可以试试用欧拉角(pitch,roll,yaw)来进行PID控制,先将四元数转化为欧拉角,然后再PID。这是我见过的 ...
1、见笑了,我理解错了{:lol:} ,谢谢指点!
2、懂了。
3、关于DMP的输出,还有点小疑问
那个dmp_read_fifo函数的sensors参数
/* sensors can contain a combination of the following flags:
*\n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO这个是单个陀螺仪轴的
*\n INV_XYZ_GYRO 这个是输出陀螺仪三个轴的信息
*\n INV_XYZ_ACCEL 这个是输出加速度计三个轴的信息
*\n INV_WXYZ_QUAT 这个是输出融合后的四元数矩阵
楼主提到的输出欧拉角,请问还有别的函数吗,还是指的是利用四元数转换得到的欧拉角
4、我也再去看看,如果有新的信息贴出来大家交流
zksniper
发表于 2014-2-20 13:51:43
木君之上 发表于 2014-2-20 09:41
1、见笑了,我理解错了 ,谢谢指点!
2、懂了。
dmp_read_fifo输出不了欧拉角,看错了,欧拉角是自己从四元数转过来的,
*\n INV_X_GYRO, INV_Y_GYRO, INV_Z_GYRO
*\n INV_XYZ_GYRO
*\n INV_XYZ_ACCEL
*\n INV_WXYZ_QUAT
可输出的也就这些了,没别的了
吉祥瑞
发表于 2014-2-21 20:44:21
看起来很不错 谢谢楼主分享
jinchenaquarius
发表于 2014-2-25 19:56:28
楼主研究的很彻底 这个东西想了好久了今天才开始搞
Daniel_Lau
发表于 2014-2-27 10:41:48
zksniper 发表于 2013-12-2 10:47
更新-采用AHRSUpdate算法进行姿态融合:
这段时间一直没有太多时间搞这玩意,不过苦恼多日的AHRSUpdate算法 ...
楼主,试了试你更新后的AHRSUpdate算法,但没成功,姿态参数全都不正确,我也不知道问题出在了哪里。。。
具体表现是:MEMS放在正北时,YAW的值为60多。运动MEMS,PITCH和ROLL数据全都不对。很是奇怪。用DMP解算出来的姿态就没有问题。
Daniel_Lau
发表于 2014-2-27 10:57:43
zksniper 发表于 2014-2-20 13:51
dmp_read_fifo输出不了欧拉角,看错了,欧拉角是自己从四元数转过来的,
*\n INV_X_GYRO, INV_Y_GYRO ...
我是用CM4内核的MCU做的,按理说更新速度非常快,但实际上速度还没有DMP快,一直很疑惑这个AHRS算法,望楼主解答一下~
zksniper
发表于 2014-2-27 12:25:09
Daniel_Lau 发表于 2014-2-27 10:57
我是用CM4内核的MCU做的,按理说更新速度非常快,但实际上速度还没有DMP快,一直很疑惑这个AHRS算法,望 ...
算法没问题,也肯定能比DMP更快,关键在于其他代码如何写,欧拉角错误应该是你代码不对,对比下定义的轴向以及正方向是否对了。
智波景严
发表于 2014-2-27 18:03:07
楼主醍醐灌顶啊!!!!!!!!!!!!!!!!!!
gyx07048153
发表于 2014-2-27 20:00:15
感谢LZ,mark慢慢看
yat
发表于 2014-2-27 22:03:36
mark【分享心得】9DOF姿态融合 四元数 欧拉角转换 有代码有内涵
smartchenlang
发表于 2014-3-4 20:19:06
这是我看到关于MPU9150最好的帖子,是初学者的启蒙教程,楼主仁义博爱,送上最伟大的敬意!
silence2455
发表于 2014-3-12 11:41:40
楼主大神,数据处理我参考了你的代码,用示波器测了一下,AHRSUpdate这个函数运行了12ms,死循环一共运行84ms,这样的话更新频率达不到要求啊,请问楼主是什么情况?灰常感谢
silence2455
发表于 2014-3-12 11:47:51
我用的是S12单片机,I/O口模拟IIC,会不会是因为这个导致读取数据太慢呢?
zksniper
发表于 2014-3-13 09:22:51
silence2455 发表于 2014-3-12 11:47
我用的是S12单片机,I/O口模拟IIC,会不会是因为这个导致读取数据太慢呢?
AHRSUpdate这个函数运行了12ms
这个是不可能的,本身算法的运算时间,跟你的I2C没任何关系,只跟你的MCU工作频率和运算效率有关,不过再次也不会12ms,我测过AHRSupdate这个函数的时间,F103的芯片,108mhz,只用0.1ms。
MCU跟传感器通讯所用的时间就看你模拟I2C的程序怎么写了,这倒是影响很大
silence2455
发表于 2014-3-13 19:19:39
zksniper 发表于 2014-3-13 09:22
这个是不可能的,本身算法的运算时间,跟你的I2C没任何关系,只跟你的MCU工作频率和运算效率有关,不过 ...
灰 常 感 谢 楼主耐心回复,我忘记设置倍频了,把总线频率设为64MHZ后运行AHRSUpdate这个函数的时间只有2ms了,但是死循环一共花了10ms,我们说的更新频率应该是把死循环里的时间都算在内吧?那么100HZ还是有点低啊!还想请问楼主是怎样对陀螺仪和磁力计做简单校准的?是自检吗?
zksniper
发表于 2014-3-14 09:17:06
silence2455 发表于 2014-3-13 19:19
灰 常 感 谢 楼主耐心回复,我忘记设置倍频了,把总线频率设为64MHZ后运行AHRSUpdate这个函数的时间只有2 ...
代码问题,不要死等磁力计,读出9150的磁力计数据,至少要2.3ms,
还有,你不要把printf的时间也算上,这玩意最占时间。
lorinis
发表于 2014-3-14 21:08:55
真是好东西,慢慢学习中,非常非常感谢楼主
fengdingkusong
发表于 2014-3-14 21:13:34
最近猛料不少
silence2455
发表于 2014-3-18 22:35:32
zksniper 发表于 2014-3-14 09:17
代码问题,不要死等磁力计,读出9150的磁力计数据,至少要2.3ms,
还有,你不要把printf的时间也算上, ...
楼主大神,你的AHRSupdate函数和原版的有些不一样,请问你改进的版本借鉴了哪些资料呢?你对陀螺仪和加速度计的原始数据是做了零点校准吗?对磁力计的处理我有些不懂。。我现在的航向角有些不正常,虽然数据都很稳定不会漂,范围也是对的,但是旋转模块的时候航向角变化不均匀,简单地说就是在某个位置模块转过90度的时候角度变化只有60度左右,在另一个位置角度变化会有100多度,不知道你有没有遇到这个问题。。恳请赐教!
zksniper
发表于 2014-3-19 09:47:37
silence2455 发表于 2014-3-18 22:35
楼主大神,你的AHRSupdate函数和原版的有些不一样,请问你改进的版本借鉴了哪些资料呢?你对陀螺仪和加速 ...
1.AHRSupdate的那部分自己改的,那部分意思就是将磁力计的坐标轴朝向改成我需要的朝向
2.陀螺仪和加速度计我只做了零点校准
3.磁力计你看到的代码只对xy轴进行了校准,通过gain和offset来将椭圆修正到圆。现在我把z轴也加入了校准,可以把椭球修正到圆球,不过代码还没放上来,弄完了再说
4.你yaw变化的情况可能是磁力计xy轴没校准的原因
silence2455
发表于 2014-3-24 21:56:08
zksniper 发表于 2014-3-19 09:47
1.AHRSupdate的那部分自己改的,那部分意思就是将磁力计的坐标轴朝向改成我需要的朝向
2.陀螺仪和加速度 ...
楼主大神,我航向角变化不均匀的问题还没解决。想请问你一下,读磁力计的原始数据要切换到Single measurement mode吧,然后读Sensitivity adjustment data for each axis要切换到Fuse ROM access mode模式吧?我看了数据手册上的那个公式(Hadj=H*((ASA-128)/256+1)),但是不太懂为什么这么做,能帮忙解释一下吗?(我用的是S12单片机,不好用,没有用官方的库)
生来孤独
发表于 2014-3-24 22:49:14
感谢楼主分享,最近正在处理数据
zksniper
发表于 2014-3-25 09:24:13
silence2455 发表于 2014-3-24 21:56
楼主大神,我航向角变化不均匀的问题还没解决。想请问你一下,读磁力计的原始数据要切换到Single measure ...
灵敏度调整只需要在初始化的时候做一次就行了,Hadj=H*((ASA-128)/256+1)自己化简一下:Hadj=H*((ASA+128)/256),移位比除法快,换成代码就是H*((ASA+128)>>8),结果就是mag=(mag*(ASA+128))>>8
mag的机制就是每读取一次MXL到MZH,mag都会从single measure转到power down mode,所以每次读取完数据,要执行一次single measure,然后在下一次读取数据之前,先读取一下ST1这个寄存器,看数据是否准备好,if没准备好就不读取,准备好了就读取,这样可以大幅度提高姿态解算的频率,没必要死等mag数据,毕竟mag本身没那么高的output rate,也不需要很快的得到它,基本保证10ms读一次mag就够了
代码顺序给你:
i2cread(Compass_Addr, Compass_ST1, 1, tmp+6);
if(tmp == 1)
{
i2cread(Compass_Addr, Compass_HXL, 6, tmp);//读取compass
mag = (((signed short int)tmp) << 8) | tmp;
mag = (((signed short int)tmp) << 8) | tmp;
mag = (((signed short int)tmp) << 8) | tmp;
mag = ((long)mag * mag_sens_adj_val) >> 8;//灵敏度调整,mag_sens_adj_val=ASA+128
mag = ((long)mag * mag_sens_adj_val) >> 8;
mag = ((long)mag * mag_sens_adj_val) >> 8;
//修正mag
init_mx =(float)mag * MXgain + MXoffset; //转换坐标轴
init_my =(float)mag * MYgain + MYoffset;
init_mz =(float)-mag * MZgain + MZoffset;
i2cwrite(Compass_Addr, Compass_CNTL, 1, data_write+6); //开启compass:single measurement mode
}
最近姿态解算和PID都完成了,稍后会放上完整代码。。。
silence2455
发表于 2014-3-25 15:53:15
多谢楼主耐心解答,期待楼主新代码,给32个赞!
电气工程化动自
发表于 2014-3-26 09:01:51
感谢楼主啊,我的也快有成效啦!
silence2455
发表于 2014-3-26 20:09:46
zksniper 发表于 2014-3-25 09:24
灵敏度调整只需要在初始化的时候做一次就行了,Hadj=H*((ASA-128)/256+1)自己化简一下:Hadj=H*((ASA+128 ...
楼主大神,求助啊!
1.我开始用的是AHRSupdate这个算法,但是Yaw的变化不均匀,而且我水平转动的时候在某一个点Pitch会先变到30度左右再回。
2.我现在用IMUupdate这个算法,我的想法是这样的:融合陀螺仪和加计得到的四元数和Yaw,然后拿这个Yaw和磁力计解算出的Yaw做一个互补得到稳定的Yaw。Yaw = -(0.985 * (yaw+gz*2*halfT*57.3) + 57.3*0.015* 磁力计得出的Yaw。
现在Yaw大概5秒钟会有一度的变化,其余都正常。但是在这个算法中,磁力计就是只在输出的时候修正一下吗(除了初始化用了一下)?
3.对于2中括号里的yaw,如果我取的是初始化的yaw,那么后来得到的Yaw稳定在一个很小的范围,怎么都不会有大的变化。如果取的是四元数得到的yaw,那么除了几秒钟漂一度,其余正常。这个漂移不能忍受啊!!请问该怎么解决?正确的思路应该是怎样的呢?
恳请楼主大神赐教啊!!!
silence2455
发表于 2014-3-26 21:03:54
a457738242 发表于 2013-12-18 21:28
找到问题了 犯了个小错误 嘿嘿
请问你的问题出在哪里???
firstzys
发表于 2014-3-26 21:55:10
标记一下,DMP简单但姿态解算部分没有得到学习,自己直接利用陀螺仪值计算用计算不出。。。
ele-madman
发表于 2014-3-27 10:41:49
zksniper 发表于 2013-12-2 10:47
更新-采用AHRSUpdate算法进行姿态融合:
这段时间一直没有太多时间搞这玩意,不过苦恼多日的AHRSUpdate算法 ...
楼主,采用你的AHRSUpdate算法,yaw正常,pitch 或 roll向一个方向偏转一个角度,这个角先增大再减小,最后再增大稳定,这是什么原因呢
zksniper
发表于 2014-3-27 13:32:51
silence2455 发表于 2014-3-26 20:09
楼主大神,求助啊!
1.我开始用的是AHRSupdate这个算法,但是Yaw的变化不均匀,而且我水平转动的时候在某 ...
你仔细看下我给的代码就明白了,这几个问题的答案,代码里都体现了。
zksniper
发表于 2014-3-27 13:33:42
ele-madman 发表于 2014-3-27 10:41
楼主,采用你的AHRSUpdate算法,yaw正常,pitch 或 roll向一个方向偏转一个角度,这个角先增大再减小, ...
调下kp和ki
ele-madman
发表于 2014-3-27 14:00:42
zksniper 发表于 2014-3-27 13:33
调下kp和ki
好的,感谢楼主,但我改了改没多大效果,我得出的角度要放大十倍貌似才正确,这是正常的么
zksniper
发表于 2014-3-27 17:08:49
ele-madman 发表于 2014-3-27 14:00
好的,感谢楼主,但我改了改没多大效果,我得出的角度要放大十倍貌似才正确,这是正常的么 ...
你角度转弧度 弧度转角度的时候系数或者单位弄错了吧,要么你代码里有单位的缩放。
silence2455
发表于 2014-3-27 18:22:38
ele-madman 发表于 2014-3-27 14:00
好的,感谢楼主,但我改了改没多大效果,我得出的角度要放大十倍貌似才正确,这是正常的么 ...
兄台,我的 Yaw不正常,你的Yaw很正常吗?请问一下你读出的磁力计灵敏度校准值是多少?我在三种模式下都读到170,171,188,按理说应该是在fuse rom mode下才能读到吧,我怕是我读错了
ele-madman
发表于 2014-3-28 09:39:59
zksniper 发表于 2014-3-27 17:08
你角度转弧度 弧度转角度的时候系数或者单位弄错了吧,要么你代码里有单位的缩放。 ...
暂时还没找到原因,不过不影响,静止时也不会漂移,再次感谢楼主,这是我见过最好的帖子了{:smile:}
ele-madman
发表于 2014-3-28 09:44:28
silence2455 发表于 2014-3-27 18:22
兄台,我的 Yaw不正常,你的Yaw很正常吗?请问一下你读出的磁力计灵敏度校准值是多少?我在三种模式下都 ...
你用X,Y 轴的值带入这个角度转换公式 angle= atan2((double)y,(double)x) * (180 / 3.14159265) + 180;最后得出的是0-360的角度值,这样就可以判断值是否正确了
silence2455
发表于 2014-3-28 10:55:32
ele-madman 发表于 2014-3-28 09:44
你用X,Y 轴的值带入这个角度转换公式 angle= atan2((double)y,(double)x) * (180 / 3.14159265) + 180; ...
兄台,我按你说的比较了一下,在XY平面上这两个值差别很小,几乎相等。这是不是可以说明磁力计的数据没有问题?
我用上位机观察姿态,感觉转动的时候Yaw的变化有有一个回调,而且在XY平面转动Pitch在某个点会有30度左右的变化,恳请赐教!
silence2455
发表于 2014-3-28 11:23:16
zksniper 发表于 2014-3-27 13:32
你仔细看下我给的代码就明白了,这几个问题的答案,代码里都体现了。
楼主大哥,请问你一下,如果我测出了准确的Pitch和Roll,是不是可以通过Yaw=atan2(mx*cos(Pitch) + my*sin(Pitch)*sin(Roll) - mz*sin(Pitch)*cos(Roll),my*cos(Roll) + mz*sin(Roll))* 57.3这个公式用磁力计算出Yaw??
但是我这样算出的Yaw不正常,我转动的时候Yaw的变化是大幅度地跳着前进,请问这是为什么? 所以我用IMUupdate这个算法的时候做互补滤波不能完全消除Yaw的漂移。
但是我用Yaw=atan2(init_my,init_mx)这个公式时,在XY平面内转动,Yaw各种正常。
求教啊大神,陷入困境了。
zksniper
发表于 2014-3-28 12:53:28
silence2455 发表于 2014-3-28 11:23
楼主大哥,请问你一下,如果我测出了准确的Pitch和Roll,是不是可以通过Yaw=atan2(mx*cos(Pitch) + my*sin ...
你要知道,你给我看的公式,如果没有一个前提条件,我是看不出来对错的,而这个前提条件就是你的传感器轴向和旋转的正方向
不过从你的思路来说,你想用互补滤波求yaw,那你就的明白什么是互补滤波,粗俗点讲,首先你要有互补,互补是两个东西的事,可是你这俩公式里都没体现出互补,说明公式的思路不对,你第一个公式静止时可以用,动态时不能用,第二个公式只能在平面里用,也不适合。因此你好好琢磨一下别人代码里互补滤波如何用的,最好能找出几个不同的情况比较一下就清楚了。
我用的互补滤波公式:Yaw = 0.9 * (Yaw + init_gz*2*halfT) + 5.73 * atan2(init_my*cos(Roll) + init_mx*sin(Roll)*sin(Pitch) - init_mz*sin(Roll)*cos(Pitch),init_mx*cos(Pitch) - init_mz*sin(Pitch))
等号右边的yaw第一次执行时是从初始化四元数中得来的yaw,相当于给一个初始值,会在将来被等号左边的yaw代替。
init_gz是陀螺仪z轴的输出,乘以2倍的halfT,得到的是陀螺仪在一个四元数更新周期中转过的角度。
Yaw + init_gz*2*halfT就是陀螺仪得到的当前yaw相对于地球坐标系转过的角度;
atan2(init_my*cos(Roll) + init_mx*sin(Roll)*sin(Pitch) - init_mz*sin(Roll)*cos(Pitch),init_mx*cos(Pitch) - init_mz*sin(Pitch))结合了磁力计的输出和四元数更新出来的Pitch和Roll,算出来空间上yaw相对于地球坐标系转过的角度;
这两者求互补滤波,融合了短期内陀螺仪的准确和长期内磁力计的准确,其他解释我代码里有
不过前面我也说过,这种办法会出现一种情况,在yaw=0或者yaw=180度附近转动,yaw会先绕一个大圈然后才定到正确的角度上,至今我也没有解决,不过准确度和稳定度还是可以的
姿态更新算法这个东西,每个人定义都不完全一样,导致代码写的都不一样,只参考一个人很困难,关键在于了解四元数和欧拉角的基础定义,之前我就是,用IMU+互补滤波吧,会出现yaw绕圈后才正确的问题,用AHRS吧,yaw一直都改不对,有时候pitch和roll也给改乱了,后来才知道连AHRSupdata这个代码都不是拿来就能用的,还得根据自己的轴向定义去修改。碰巧定义一样,才能拿来就用。
所以我给你的公式你只能参考,然后根据自己的情况去改,有时候改改正负号就好了,有时候要大改,这个我也没法具体说,除非你跟我的定义都一样
ele-madman
发表于 2014-3-28 14:00:34
silence2455 发表于 2014-3-28 10:55
兄台,我按你说的比较了一下,在XY平面上这两个值差别很小,几乎相等。这是不是可以说明磁力计的数据没有 ...
就像楼主大哥说的那样,这种情况就是你定义的轴向不一样
ele-madman
发表于 2014-3-28 14:06:35
zksniper 发表于 2014-3-28 12:53
你要知道,你给我看的公式,如果没有一个前提条件,我是看不出来对错的,而这个前提条件就是你的传感器轴 ...
楼主大哥,我的四轴飞得还不是很稳,调PID已经没有多大改善了,我想加入滤波器是不是会好一些,能不能用卡尔曼把欧拉角和角速度融合一下应该会好一点,这样可行不?
zksniper
发表于 2014-3-28 14:47:54
ele-madman 发表于 2014-3-28 14:06
楼主大哥,我的四轴飞得还不是很稳,调PID已经没有多大改善了,我想加入滤波器是不是会好一些,能不能用 ...
我也不知道,还没到那一步
ele-madman
发表于 2014-3-29 02:28:07
zksniper 发表于 2014-3-28 14:47
我也不知道,还没到那一步
还有个问题,就是roll转动的时候,pitch先跟着增大或减小,然后roll稳定,pitch回到平衡点,这是什么情况啊
skyxjh
发表于 2014-3-29 18:12:40
讲得很好,学习了
silence2455
发表于 2014-3-31 17:02:31
zksniper 发表于 2014-3-28 12:53
你要知道,你给我看的公式,如果没有一个前提条件,我是看不出来对错的,而这个前提条件就是你的传感器轴 ...
楼主大哥,实在不好意思,还是想麻烦你一下{:handshake:}
你在用IMUupdate这个算法时 是通过陀螺仪和磁力计做互补滤波得到Yaw的,但是这里磁力计的数据更新较慢,导致求得的倾角和磁力计数据不同步,用atan2(my*cos(Pitch) + mx*sin(Pitch)*sin(Roll) - mz*sin(Pitch)*cos(Roll), mx*cos(Roll) + mz*sin(Roll))这个公式是不行的,也就是你说的只在静态时有效吧? 所以即使做互补滤波Yaw也会乱跳。不过在初始化的时候我用这个公式求出的Yaw是没有问题的。
我现在用AHRSupdate这个算法,观察了好几天,确定轴向没问题,将磁力计的轴向旋转到和陀螺仪加速度计一致了。初始化得到的角度也很正确,注释掉关于磁力计的部分得到的另外两个倾角也很正确。
可是当水平旋转到0附近时,pitch和roll会有很大变化,我现在严重怀疑是磁力计的校准出现了问题,导致叉积(my*wz - mz*wy),(mz*wx - mx*wz),(mx*wy - my*wx)在某处有较大变化,使四元数有较大变化,从而pitch和roll也会有很大变化。
我磁力计的校准是参考楼主的方法。(实验室在9楼,会不会也有一点影响?)
还望楼主大哥帮忙解答,是在是感激不尽!!!
zksniper
发表于 2014-4-1 09:25:46
silence2455 发表于 2014-3-31 17:02
楼主大哥,实在不好意思,还是想麻烦你一下
你在用IMUupdate这个算法时 是通过陀螺仪和 ...
你在用IMUupdate这个算法时 是通过陀螺仪和磁力计做互补滤波得到Yaw的,但是这里磁力计的数据更新较慢,导致求得的倾角和磁力计数据不同步,用atan2(my*cos(Pitch) + mx*sin(Pitch)*sin(Roll) - mz*sin(Pitch)*cos(Roll), mx*cos(Roll) + mz*sin(Roll))这个公式是不行的,也就是你说的只在静态时有效吧? 所以即使做互补滤波Yaw也会乱跳。不过在初始化的时候我用这个公式求出的Yaw是没有问题的。
磁力计的output rate是慢,但是这是因为磁力计没必要那么高的更新速率,10ms一次足够了,“不同步”指的是用上一次的数据和这一次的数据进行融合,而磁力计的数据虽然更新比陀螺仪慢,但是用的还是同一次的数据来融合的,不存在不同步的现象,最多就是姿态更新频率慢了而已,我IMU的求yaw公式都是经过我测试没问题才会放上来,不管是静止还是运动,精度、稳定度和实时度都很好,基本都是零点1度的浮动。
我现在用AHRSupdate这个算法,观察了好几天,确定轴向没问题,将磁力计的轴向旋转到和陀螺仪加速度计一致了。初始化得到的角度也很正确,注释掉关于磁力计的部分得到的另外两个倾角也很正确。
可是当水平旋转到0附近时,pitch和roll会有很大变化,我现在严重怀疑是磁力计的校准出现了问题,导致叉积(my*wz - mz*wy),(mz*wx - mx*wz),(mx*wy - my*wx)在某处有较大变化,使四元数有较大变化,从而pitch和roll也会有很大变化。
你AHRSupdata()里的代码修改了吗?不修改是不能用的。
silence2455
发表于 2014-4-1 13:02:32
zksniper 发表于 2014-4-1 09:25
磁力计的output rate是慢,但是这是因为磁力计没必要那么高的更新速率,10ms一次足够了,“不同步”指 ...
楼主大哥,AHRSupdate里面涉及磁力计的就这些吧!定义Y轴指向正北,磁力计的轴向旋转到和陀螺仪一致了,这里应该没错吧?
如果这里没错的话,我觉得很可能是磁力计校准的问题啊!还请明示,感激不尽!!
zksniper
发表于 2014-4-1 13:29:57
silence2455 发表于 2014-4-1 13:02
楼主大哥,AHRSupdate里面涉及磁力计的就这些吧!定义Y轴指向正北,磁力计的轴向旋转到和陀螺仪一致了, ...
这段没问题,你试试磁力计不校准。
再好好检查下代码,看哪有问题,有些c语言上的错误很容易被忽略,比如定义的数据类型是否匹配,强制转换是否可行等,参数传递时是否是自己想要的参数。
silence2455
发表于 2014-4-1 15:24:08
zksniper 发表于 2014-4-1 13:29
这段没问题,你试试磁力计不校准。
再好好检查下代码,看哪有问题,有些c语言上的错误很容易被忽略,比 ...
楼主大哥,如果你也认为这段代码没问题,那么AHRSupdate就没问题了。而且初始化得到的欧拉角和后来解算出的角度也是几乎一样的(静止时),所以初始化也应该没有问题。
根据我的现象(角度基本正常,但Yaw变化不均匀,水平转动时在0度附近某点,Pitch和Roll会有很大变化。)来看,只能是叉积(my*wz - mz*wy)在某点有个突变,
导致四元数有很大变化,才会使Pitch和Roll会有很大变化(当我注释掉磁力计做的叉积后,Pitch和Roll都是正常的)。
磁力计校准的那部分注释掉几乎没有任何改变。
变量类型,强制转换我都看了好久,有一点一直很疑惑。磁力计的数据是以13位的补码存储在寄存器里,也就是前4位都是标志位,
我定义了uchar型的H,L分别取高八位和低八位,然后m=((int)H<<8)+L。这样感觉不对吧,这样好像只把第一位当标志位了,但是修改后角度更不正常。
还请楼主大哥帮忙解答,非常感谢!!
zksniper
发表于 2014-4-1 15:48:58
silence2455 发表于 2014-4-1 15:24
楼主大哥,如果你也认为这段代码没问题,那么AHRSupdate就没问题了。而且初始化得到的欧拉角和后来解算出 ...
试试这个:
signed short int mag;
unsigned char H, L;
mag = (((signed short int)H)<<8) | L;
注意mag的高位和低位别弄错了,对照你自己的磁力计看一下。
silence2455
发表于 2014-4-1 16:20:57
zksniper 发表于 2014-4-1 15:48
试试这个:
signed short int mag;
unsigned char H, L;
楼主大哥,你是让我把int型换成signed short int型试试是吧?我用的是16位的单片机,应该是没区别的。
我看了你的程序,读取磁力计的数据用的是官方的库吧,没有什么特别的调整。但是数据手册上说,陀螺仪的数据是以16位的补码存储的,
而磁力计是13位补码,所以个人感觉读取数据可能会不同。
楼主大哥能不能把磁力计校准的部分粘贴处理,我试一下,反正我们用的都是9150,也就是环境的差别吧!
感激不尽{:handshake:}
zksniper
发表于 2014-4-1 16:48:37
本帖最后由 zksniper 于 2014-4-1 16:50 编辑
silence2455 发表于 2014-4-1 16:20
楼主大哥,你是让我把int型换成signed short int型试试是吧?我用的是16位的单片机,应该是没区别的。
我 ...
你磁力计不校准的话,读出来的mx my mz各是多少?我怀疑你的问题压根就不是校准的问题,即便不校准,也不会出现突然在某一点上的角度变化,最多欧拉角不准确
你磁力计进行过灵敏度调整了吗?调整以后的mx my mz是多少?
silence2455
发表于 2014-4-1 17:05:34
zksniper 发表于 2014-4-1 16:48
你磁力计不校准的话,读出来的mx my mz各是多少?我怀疑你的问题压根就不是校准的问题,即便不校准,也不 ...
楼主大哥说的有道理。我的灵敏度校准值是170,171,184;我指向正北时磁力计原始数据是15,55,-29;指向正南是-121,51,-25;大概在+-200以内。
X轴和Z轴负数居多,y轴经常是正的,请问是不是有点不正常?
silence2455
发表于 2014-4-1 17:15:27
zksniper 发表于 2014-4-1 16:48
你磁力计不校准的话,读出来的mx my mz各是多少?我怀疑你的问题压根就不是校准的问题,即便不校准,也不 ...
楼主大哥,这个是我读磁力计数据的代码?麻烦你看看有没有什么错误,拜谢!!(地址确定没错)
zksniper
发表于 2014-4-1 17:21:02
silence2455 发表于 2014-4-1 17:05
楼主大哥说的有道理。我的灵敏度校准值是170,171,184;我指向正北时磁力计原始数据是15,55,-29;指向正 ...
我指向正北时磁力计原始数据是15,55,-29;指向正南是-121,51,-25
看你这数据,-29和-25对应的应该是z轴,那么如果你指向正北或正南:mx和my其中一个应该等于0左右。水平面上垂直于磁场方向的磁场强度差不多是0。
可是15 55, -12151 没一个是0的,还有15和-121差这么多?最起码绝对值相差不会那么大才对,你确定你得到的磁力计数据的过程没问题?
silence2455
发表于 2014-4-1 23:57:26
zksniper 发表于 2014-4-1 17:21
看你这数据,-29和-25对应的应该是z轴,那么如果你指向正北或正南:mx和my其中一个应该等于0左右。水平 ...
楼主大哥,我又仔细地研究了一下读取数据的过程,我发现在m=((int)H<<8)+L这个过程中补码会自动转换成原码,数据处理是没错的,我严格按照官方的库里读磁力计数据的部分写的。
现在问题是我的H,也就是高八位,要么是0要么是255,所以我的磁力计原始数据不超过256,这是不是不正常?
按理说磁力计量程不是-4095--4095吗,怎么得到的数据这么小?
不知道楼主大哥是不是这种情况?难道我的9150模块有问题?
产品ID肯定是可以读出来的(0x48)
恳请赐教
silence2455
发表于 2014-4-2 19:37:34
zksniper 发表于 2014-4-1 17:21
看你这数据,-29和-25对应的应该是z轴,那么如果你指向正北或正南:mx和my其中一个应该等于0左右。水平 ...
请问楼主大哥,你磁力计的原始数据大概是怎样的?
这个是毕设,很急啊,还请帮忙啊!!!
silence2455
发表于 2014-4-3 16:37:56
zksniper 发表于 2014-4-1 17:21
看你这数据,-29和-25对应的应该是z轴,那么如果你指向正北或正南:mx和my其中一个应该等于0左右。水平 ...
请问楼主大哥,磁力计原始数据完全不符合x*x+y*y+z*z恒定这个规律,是不是说明数据出错了?
hfjydq
发表于 2014-4-8 09:16:58
想请教下楼主,我现在用的是MPU6050的DMP,读出四元数,然后计算出姿态角,现在roll和pitch经过里面的加速度计校正,都很准确,但yaw加速度计是不能校正的,所以我准备外扩了一个HMC5883L磁力计模块,现在模块买回来了,但不知道是直接接在MPU6050的外扩IIC接口上合适,还是和MPU6050接在同一个IIC接口然后一起接到MCU上合适?请指点,还有楼主知道怎么用磁力计校正yaw吗?MPU6050的DMP我还想继续使用,MCU我用的是STM32,请指点!!!万分感谢
zksniper
发表于 2014-4-8 09:30:18
silence2455 发表于 2014-4-1 23:57
楼主大哥,我又仔细地研究了一下读取数据的过程,我发现在m=((int)H
你仔细看看mpu9150的datasheet吧,上面有对磁力计做说明,经过单位转换和灵敏度调整以后,磁力计的范围就不是-4095~+4095了,你要知道你磁力计的单位是什么,然后转换成高斯,看看是多少高斯,正常地球磁场根据各个地方不同,范围大概是0.3-0.6高斯。
zksniper
发表于 2014-4-8 09:31:00
silence2455 发表于 2014-4-3 16:37
请问楼主大哥,磁力计原始数据完全不符合x*x+y*y+z*z恒定这个规律,是不是说明数据出错了? ...
没听说过这个规律,我都是看数据符合不符合现实
zksniper
发表于 2014-4-8 09:34:27
hfjydq 发表于 2014-4-8 09:16
想请教下楼主,我现在用的是MPU6050的DMP,读出四元数,然后计算出姿态角,现在roll和pitch经过里面的加速 ...
磁力计接哪个口都行,哪方便接哪,正常来讲,直接接MCU的I2C会快很多,因为不需要再去操作mpu6050的寄存器了。
用磁力计校正yaw见帖子
hfjydq
发表于 2014-4-8 09:40:54
zksniper 发表于 2014-4-8 09:34
磁力计接哪个口都行,哪方便接哪,正常来讲,直接接MCU的I2C会快很多,因为不需要再去操作mpu6050的寄存 ...
那我就直接接STM32了,是和MPU6050接一个IIC接口好呢?还是再用STM32另外模拟一个IIC接口?先把硬件连接搞清楚,然后再弄程序,楼主技术大牛,而且是好人,膜拜
zksniper
发表于 2014-4-8 09:47:35
hfjydq 发表于 2014-4-8 09:40
那我就直接接STM32了,是和MPU6050接一个IIC接口好呢?还是再用STM32另外模拟一个IIC接口?先把硬件连接 ...
接哪个I2C都没什么影响