关于姿态融合算法的问题,算出的欧拉角为什么不对

2019-07-14 22:32发布

来来回回折腾了很久,一直没搞定姿态融合算法,算出来的欧拉角怎么都不对。

说明:我用的传感器是mpu9150,定义的板子的坐标是:x轴指向北,y轴指向东,z轴指向地。陀螺仪、加速度计和磁力计的轴向经过旋转都保持这个朝向。

问题:我用的是AHRSupdate这个程序,但是算出来的欧拉角只有Roll是对的,Yaw和Pitch都不对,其中,当我把Roll旋转+90度时,Yaw的数据是对的,但是Pitch的怎么都不对,举个栗子:板子水平放置的时候Pitch保持为0,板子x轴倾斜+60度左右并保持此姿态的时候,Pitch只是在正负20之间来回震荡几次就又回到0。

我程序如下:
先初始化四元数,其中init_ax/init_ay/init_az的单位是g,比如z轴和重力方向重合时的读数是1;init_mx/init_my/init_mz的单位就是磁力计输出的单位,未经过任何改变:
  1.   
  2. init_Roll  = atan2(init_ay, init_az);
  3. init_Pitch = -asin(init_ax);              //init_Pitch = asin(ay / 1);      
  4. init_Yaw   = -atan2(init_my*cos(init_Roll) + init_mx*sin(init_Roll)*sin(init_Pitch) - init_mz*sin(init_Roll)*cos(init_Pitch),
  5.                      init_mx*cos(init_Pitch) + init_mz*sin(init_Pitch));                       //atan2(my, mx);
  6. q0 = cos(0.5*init_Roll)*cos(0.5*init_Pitch)*cos(0.5*init_Yaw) + sin(0.5*init_Roll)*sin(0.5*init_Pitch)*sin(0.5*init_Yaw);  //w
  7. q1 = sin(0.5*init_Roll)*cos(0.5*init_Pitch)*cos(0.5*init_Yaw) - cos(0.5*init_Roll)*sin(0.5*init_Pitch)*sin(0.5*init_Yaw);  //x   
  8. q2 = cos(0.5*init_Roll)*sin(0.5*init_Pitch)*cos(0.5*init_Yaw) + sin(0.5*init_Roll)*cos(0.5*init_Pitch)*sin(0.5*init_Yaw);  //y   
  9. q3 = cos(0.5*init_Roll)*cos(0.5*init_Pitch)*sin(0.5*init_Yaw) - sin(0.5*init_Roll)*sin(0.5*init_Pitch)*cos(0.5*init_Yaw);  //z
复制代码然后循环调用AHRSupdate()更新四元数,并用下面的公式结算欧拉角:
  1. Pitch = asin(-2*q1*q3 + 2*q0*q2) * 57.3; //俯仰角,绕y轴转动         
  2. Roll  = atan2(2*q2*q3 + 2*q0*q1,-2*q1*q1 - 2*q2*q2 + 1) * 57.3; //滚动角,绕x轴转动
  3. Yaw   = atan2(2*q1*q2 + 2*q0*q3,-2*q2*q2 - 2*q3*q3 + 1) * 57.3;  //偏航角,绕z轴转动
复制代码一直都找不到问题到底出在哪,感觉可能是我定义的坐标轴方向不能套用上面的公式来计算?可是为什么Roll的数据是对的呢?
友情提示: 此问题已得到解决,问题已经关闭,关闭后问题禁止继续编辑,回答。
该问题目前已经被作者或者管理员关闭, 无法添加新回复
8条回答
远不及你
1楼-- · 2019-07-15 09:10
我想想看,AHRSupdate的话
用的是含有电子罗盘的版本吗?
有么有试过没有电子罗盘的?
人走了
2楼-- · 2019-07-15 11:25
远不及你 发表于 2015-5-22 14:58
我想想看,AHRSupdate的话
用的是含有电子罗盘的版本吗?
有么有试过没有电子罗盘的?

AHRSupdata是含电子罗盘的。
IMUupdata是不含电子罗盘的,我之前试过,好像也不行
无人岛
3楼-- · 2019-07-15 15:01
看程序感觉好像是电子罗盘没有校准的原因
人走了
4楼-- · 2019-07-15 19:06
无人岛 发表于 2015-5-22 16:42
看程序感觉好像是电子罗盘没有校准的原因

用不带电子罗盘的IMUupdate程序试了一下,结果如下:
1. Pitch和Roll数据都对,只不过Pitch到不了90度,最多也就正负80度,这个在我的程序里应该算正常。
2. Yaw完全不受控制,它表示自己很随意。。。。。想是多少就是多少,看了下规律,是在-180到+180之间快速递减或递增,不像别人四轴飘的那么慢。

我的电子罗盘没有校准,不校准的话确实会差很多吗?应该不会导致Yaw和Pitch的数据都不对,而只有Roll的数据对这种现象吧。
改天校准一下试试,下周一给出测试结果和校准方法,以便大家参考和指出错误。
无人岛
5楼-- · 2019-07-15 20:34
 精彩回答 2  元偷偷看……
欧洲王
6楼-- · 2019-07-15 22:50
算法有这么简单吗

一周热门 更多>