<求助>姿态融合算法搞不定了

zksniper

来来回回折腾了2个星期了，姿态融合算法一直搞不定，求出来的欧拉角怎么都不对，求高手解惑。
先说明一下：我用的传感器是mpu9150，定义的板子的坐标是：x轴指向北，y轴指向东，z轴指向地。陀螺仪、加速度计和磁力计的轴向经过旋转都保持这个朝向。

问题：我用的是AHRSupdate这个程序，参考自：http://www.amobbs.com/thread-5492189-1-1.html，但是算出来的欧拉角只有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.        
   
2. Pitch = asin(-2*q1*q3 + 2*q0*q2) * 57.3; //俯仰角，绕y轴转动         
   
3. Roll  = atan2(2*q2*q3 + 2*q0*q1,-2*q1*q1 - 2*q2*q2 + 1) * 57.3; //滚动角，绕x轴转动
   
4. Yaw   = atan2(2*q1*q2 + 2*q0*q3,-2*q2*q2 - 2*q3*q3 + 1) * 57.3;  //偏航角，绕z轴转动

复制代码

一直都找不到问题到底出在哪，隐隐约约感觉是我定义的坐标轴方向不能套用上面的公式来计算？可是为什么Roll的数据是对的呢？

john800422

初始角度 init_Roll, init_Pitch, init_Yaw 是正確的嗎?

zksniper

john800422 发表于 2013-8-16 15:56
初始角度 init_Roll, init_Pitch, init_Yaw 是正確的嗎?

恩，是的，单位是弧度，举个例子：当我把板子的y轴指向地时，Roll=1.5xxxxxx，乘以57.3的话，大概也是正90度左右。
Pitch和Yaw我也都通过串口打印出来测过，都没问题

当我将板子放平，且x轴指向正北方时，初始化的q0=0.99xx   q1=-0.002xxx   q2=0.003xxx   q3=0.01xxxx

zksniper

我觉得是不是我刚开始转换加速度计和陀螺仪的轴向时出了错？
为了让MPU9150的加速度计、陀螺仪和磁力计的轴向保持一致，程序里我保持磁力计的轴向不变，然后将加速度计和陀螺仪的轴向转成和磁力计的一样。
为了方便观看，我截个图，下图是mpu9150 datasheet里原始的加速度计、陀螺仪和磁力计的轴向：


其中改变陀螺仪轴向我用的是官方自带的程序：
先定义转轴矩阵：

1. 
   
2. static signed char gyro_orientation[9] = { 0, 1, 0,
   
3.                                            1, 0, 0,
   
4.                                            0, 0, -1};

复制代码

再调用函数：dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))
这部分应该没问题，不过要说明一点的就是，这个函数里好像写了也可以改变加速度计的轴向，可实际测出来发现，只能改变陀螺仪的轴向。

然后我自己手动改变加速度计的轴向：

1. 
   
2. ax=(float)-accel[1];             
   
3. ay=(float)-accel[0];
   
4. az=(float)accel[2];

复制代码

这个，我个人觉得不是那么靠谱，贴出来方便大家找出错误原因，上面这个代码就是将加速度计y轴的输出放到ax，x轴输出放到ay，加负号是为了当ax或ay指向地时，输出为正，z轴输出保持不变，z轴指向地时为正。

磁力计部分轴向保持不变：

1. 
   
2. mx =(float)mag[0];                                               
   
3. my =(float)mag[1];
   
4. mz =(float)mag[2];

复制代码

john800422

AHRSupdate
用的是有含電子羅盤的嗎?
可以先試試沒有電子羅盤的

zksniper

john800422 发表于 2013-8-16 16:52
AHRSupdate
用的是有含電子羅盤的嗎?
可以先試試沒有電子羅盤的

恩，AHRSupdata是含电子罗盘的。
IMUupdata是不含电子罗盘的，我之前试过，好像也不行，我现在再试试，稍后给出结果。

john800422

zksniper 发表于 2013-8-16 17:12
恩，AHRSupdata是含电子罗盘的。
IMUupdata是不含电子罗盘的，我之前试过，好像也不行，我现在再试试，稍 ...

沒有仔細看過四元數+電子羅盤的算法
但我覺得可能是因為電子羅盤沒校正的原因

導致
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);
計算出來有問題

john800422

zksniper 发表于 2013-8-16 16:49
我觉得是不是我刚开始转换加速度计和陀螺仪的轴向时出了错？
为了让MPU9150的加速度计、陀螺仪和磁力计的轴 ...

軸向的部分
沒用到電子羅盤的話是沒有問題的

若要用電子羅盤的話
建議改電子羅盤就好

zksniper

john800422 发表于 2013-8-16 17:25
沒有仔細看過四元數+電子羅盤的算法
但我覺得可能是因為電子羅盤沒校正的原因

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

我的电子罗盘没有校准，不校准的话确实会差很多吗？应该不会导致Yaw和Pitch的数据都不对，而只有Roll的数据对这种现象吧。
改天校准一下试试，下周一给出测试结果和校准方法，以便大家参考和指出错误。

PS：我用过你的互补滤波补偿Yaw的方法，就是用芯片自带DMP读出四元数，转化成欧拉角，然后用电子罗盘对Yaw进行互补滤波，不过貌似我公式写的不太对，Yaw还是不正确，也没太深入研究这条路，想先用AHRSupdate这个算法做出来，用来理解四元数和欧拉角。

john800422

zksniper 发表于 2013-8-16 18:10
刚刚用不带电子罗盘的IMUupdate程序试了一下，结果如下：
1. Pitch和Roll数据都对，只不过Pitch到不了90 ...

1. Pitch和Roll数据都对，只不过Pitch到不了90度，最多也就正负80度，这个在我的程序里应该算正常。
---> 加速度計和陀螺儀有校正過嗎?

2. Yaw完全不受控制，它表示自己很随意。。。。。想是多少就是多少，看了下规律，是在-180到+180之间快速递减或递增，不像别人四轴飘的那么慢。
---> 大部分四軸上都是直接用PID的D控制器的( Yaw = Kd*Gyro_Z ), 所以不會飄的很嚴重

3. 我的电子罗盘没有校准，不校准的话确实会差很多吗？
---> 電子羅盤沒校準影響大不大要看環境的磁場干擾大不大, 若沒有干擾, 不校正誤差不會很大, 但通常都會有影響

4. 应该不会导致Yaw和Pitch的数据都不对，而只有Roll的数据对这种现象吧
---> IMUupdate 的方法並沒有用到電子羅盤, 所以若理論正確的話, Pitch , Roll 的誤差來源應該為加速度計或陀螺儀

zksniper

john800422 发表于 2013-8-16 18:24
1. Pitch和Roll数据都对，只不过Pitch到不了90度，最多也就正负80度，这个在我的程序里应该算正常。
---> ...

我试了一下对电子罗盘进行校准，发现校准前和校准后差别不大，问题依旧存在，如下：
对电子罗盘校准后，采用AHRSupdate算法，算出欧拉角还是只有Roll是对的；Pitch无论怎么样都不对；而Yaw，有一个奇怪的现象，在顶楼我也提到了，就是当Roll=+90度时，Yaw的输出是对的，稳定性也很好，达到预期效果。

Roll=+90度时的轴向为：x指向北，y指地，z指西。这时如果绕y轴转动，Yaw就是对的，这个问题很奇怪，没想明白为什么。

john800422

zksniper 发表于 2013-8-19 14:01
我试了一下对电子罗盘进行校准，发现校准前和校准后差别不大，问题依旧存在，如下：
对电子罗盘校准后， ...

先確定IMUupdate正確無誤
把Pitch與Roll解決

再用AHRSupdate
來看Yaw

不要甚麼事都想要一次完成
大部分這樣的想法只會讓你浪費更多時間

最後還是問
"加速度計和陀螺儀有校正過嗎?"

zksniper

john800422 发表于 2013-8-19 23:06
先確定IMUupdate正確無誤
把Pitch與Roll解決

IMUupdate我试过了，Pitch和Roll都没问题，这个我在9楼给你回复了

对于加速度计和陀螺仪的校准，我用的是mpu9150内部自带的校准程序，只不过没有求平均，不知道会不会对Yaw和Pitch有足够大的影响，我观察过加速度计和陀螺仪的数据，用来进行姿态融合应该没问题。

我现在一直在想是不是电子罗盘的轴向定义不符合AHRSupdata的设定？不过对应于不同轴向和旋转顺序的公式我都反复检查过，也没发现错误，计算出来的Yaw和Pitch，数据不对这个说法好像不太准确，准确来说应该是在正负之间来回的震荡，不知道什么原因。

还有就是Ki、Kp和HalfT这三个参数了，这三个参数我也反复调过，影响是有的，但没有达到让Yaw和Pitch正确的地步。

john800422

zksniper 发表于 2013-8-20 10:53
IMUupdate我试过了，Pitch和Roll都没问题，这个我在9楼给你回复了

对于加速度计和陀螺仪的校准，我用的 ...

1. 我观察过加速度计和陀螺仪的数据，用来进行姿态融合应该没问题
---> 如何觀察的? 陀螺儀的話, 靜止時應該在0或在0處上下震盪, 加速度計的加速度值在90度與-90度應該互為反號且為1g, 0度時應為0g

2. 我现在一直在想是不是电子罗盘的轴向定义不符合AHRSupdata的设定？不过对应于不同轴向和旋转顺序的公式我都反复检查过，也没发现错误，
    计算出来的Yaw和Pitch，数据不对这个说法好像不太准确，准确来说应该是在正负之间来回的震荡，不知道什么原因
---> AHRSupdate的算法我並沒有仔細理解過, 原因是

1. 
   
2. ex = (ay*vz - az*vy) + (my*wz - mz*wy);
   
3. ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
   
4. ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
   

复制代码

.
      電子羅盤的誤差會影響姿態角
      假設做在飛行器上, 飛過一顆磁鐵, 影響大的話可能就準備墬機了 (( 不一定為真
      若要使用 AHRSupdate , 先把

1. 
   
2. hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
   
3. hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
   
4. hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         
   
5. bx = sqrt((hx*hx) + (hy*hy));
   
6. bz = hz;
   
7. 
   
8. wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
   
9. wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
   
10. wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
    

复制代码

.
      原理搞清楚, 再繼續測試

3. 还有就是Ki、Kp和HalfT这三个参数了，这三个参数我也反复调过，影响是有的，但没有达到让Yaw和Pitch正确的地步。
---> HalfT是不應該調的, HalfT是取樣的週期, 若是算法有問題, 正確不了也是正常的

zksniper

john800422 发表于 2013-8-20 11:55
1. 我观察过加速度计和陀螺仪的数据，用来进行姿态融合应该没问题
---> 如何觀察的? 陀螺儀的話, 靜止時 ...

非常感谢，第2条对我很有启发

hht594

学习中……

zksniper

AHRSupdate这个算法我是没办法了，搞不了，我都开始怀疑算法是有问题的，希望哪位用过的高手给个代码研究研究，最后我还是采用了IMUupdate+楼上黄脸猫的互补滤波求YAW，结果还算满意，姿态融合就先这样吧，先往下走，完事了再回过头来研究。稍后放上代码供比我还新的新手参考。

再次感谢楼上黄脸猫，先给出求YAW的互补滤波算法：

1.        
   
2. Pitch = asin(-2 * q1 * q3 + 2 * q0 * q2); //俯仰角，绕y轴转动         
   
3. Roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1); //滚动角，绕x轴转动
   
4. //0.9和0.1是修正系数，其中5.73=0.1*57.3，乘以57.3是为了将弧度转化为角度
   
5. 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)));
   
6. Pitch = Pitch * 57.3;
   
7. Roll = Roll * 57.3
   

复制代码

tree666

IMUupdata和AHRSupdate算法和坐标系定义没有关系？

记得原文Z轴是向上的？

还是只要满足右手定则就行？

zksniper

tree666 发表于 2013-9-30 00:07
IMUupdata和AHRSupdate算法和坐标系定义没有关系？

记得原文Z轴是向上的？

z轴没必要向上，轴向的定义可以随意，只要你的公式是根据你轴向的定义推导出来的就行，大部分人为了省事，都是按照那两个常见的轴向来定义自己的轴向，只需要直接套公式就行了

电气工程化动自

babyhua

收藏一下

onev

LZ 我也用AHRS 但奇怪的是在开始的一段时间内 大概半分钟内吧 姿态会论七八糟的飘 过完了这阵子就好了  而且我拿iPhone指南针试过 我的yaw角与它没偏差  至少肉眼看不出来  但开始的那段飘不知道为什么  LZ有遇到过吗

zksniper

onev 发表于 2014-5-11 17:29
LZ 我也用AHRS 但奇怪的是在开始的一段时间内 大概半分钟内吧 姿态会论七八糟的飘 过完了这阵子就好了  而 ...

查看最开始的那半分钟，你传感器数据是否正确。尤其是磁力计。

zjzly

为什么要先定义x轴，y轴的指向呢？只要磁力计和陀螺仪，加速度计的坐标一致就可以了吧？

zksniper

这里的指向是指参考系