<求助>姿态融合算法搞不定了
本帖最后由 zksniper 于 2013-8-16 16:30 编辑来来回回折腾了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的单位就是磁力计输出的单位，未经过任何改变：
init_Roll= atan2(init_ay, init_az);
init_Pitch = -asin(init_ax); //init_Pitch = asin(ay / 1);
init_Yaw = -atan2(init_my*cos(init_Roll) + init_mx*sin(init_Roll)*sin(init_Pitch) - init_mz*sin(init_Roll)*cos(init_Pitch),
init_mx*cos(init_Pitch) + init_mz*sin(init_Pitch)); //atan2(my, mx);
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
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
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
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()更新四元数，并用下面的公式结算欧拉角：
Pitch = asin(-2*q1*q3 + 2*q0*q2) * 57.3; //俯仰角，绕y轴转动
Roll= atan2(2*q2*q3 + 2*q0*q1,-2*q1*q1 - 2*q2*q2 + 1) * 57.3; //滚动角，绕x轴转动
Yaw = atan2(2*q1*q2 + 2*q0*q3,-2*q2*q2 - 2*q3*q3 + 1) * 57.3;//偏航角，绕z轴转动一直都找不到问题到底出在哪，隐隐约约感觉是我定义的坐标轴方向不能套用上面的公式来计算？可是为什么Roll的数据是对的呢？ 初始角度 init_Roll, init_Pitch, init_Yaw 是正確的嗎? 本帖最后由 zksniper 于 2013-8-16 16:22 编辑
john800422 发表于 2013-8-16 15:56 static/image/common/back.gif
初始角度 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 于 2013-8-16 16:52 编辑
我觉得是不是我刚开始转换加速度计和陀螺仪的轴向时出了错？
为了让MPU9150的加速度计、陀螺仪和磁力计的轴向保持一致，程序里我保持磁力计的轴向不变，然后将加速度计和陀螺仪的轴向转成和磁力计的一样。
为了方便观看，我截个图，下图是mpu9150 datasheet里原始的加速度计、陀螺仪和磁力计的轴向：
其中改变陀螺仪轴向我用的是官方自带的程序：
先定义转轴矩阵：
static signed char gyro_orientation = { 0, 1, 0,
1, 0, 0,
0, 0, -1};再调用函数：dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))
这部分应该没问题，不过要说明一点的就是，这个函数里好像写了也可以改变加速度计的轴向，可实际测出来发现，只能改变陀螺仪的轴向。
然后我自己手动改变加速度计的轴向：
ax=(float)-accel;
ay=(float)-accel;
az=(float)accel;这个，我个人觉得不是那么靠谱，贴出来方便大家找出错误原因，上面这个代码就是将加速度计y轴的输出放到ax，x轴输出放到ay，加负号是为了当ax或ay指向地时，输出为正，z轴输出保持不变，z轴指向地时为正。
磁力计部分轴向保持不变：
mx =(float)mag;
my =(float)mag;
mz =(float)mag; AHRSupdate
用的是有含電子羅盤的嗎?
可以先試試沒有電子羅盤的 john800422 发表于 2013-8-16 16:52 static/image/common/back.gif
AHRSupdate
用的是有含電子羅盤的嗎?
可以先試試沒有電子羅盤的
恩，AHRSupdata是含电子罗盘的。
IMUupdata是不含电子罗盘的，我之前试过，好像也不行，我现在再试试，稍后给出结果。 zksniper 发表于 2013-8-16 17:12 static/image/common/back.gif
恩，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);
計算出來有問題 zksniper 发表于 2013-8-16 16:49 static/image/common/back.gif
我觉得是不是我刚开始转换加速度计和陀螺仪的轴向时出了错？
为了让MPU9150的加速度计、陀螺仪和磁力计的轴 ...
軸向的部分
沒用到電子羅盤的話是沒有問題的
若要用電子羅盤的話
建議改電子羅盤就好
john800422 发表于 2013-8-16 17:25 static/image/common/back.gif
沒有仔細看過四元數+電子羅盤的算法
但我覺得可能是因為電子羅盤沒校正的原因
刚刚用不带电子罗盘的IMUupdate程序试了一下，结果如下：
1. Pitch和Roll数据都对，只不过Pitch到不了90度，最多也就正负80度，这个在我的程序里应该算正常。
2. Yaw完全不受控制，它表示自己很随意。。。。。想是多少就是多少，看了下规律，是在-180到+180之间快速递减或递增，不像别人四轴飘的那么慢。
我的电子罗盘没有校准，不校准的话确实会差很多吗？应该不会导致Yaw和Pitch的数据都不对，而只有Roll的数据对这种现象吧。
改天校准一下试试，下周一给出测试结果和校准方法，以便大家参考和指出错误。
PS：我用过你的互补滤波补偿Yaw的方法，就是用芯片自带DMP读出四元数，转化成欧拉角，然后用电子罗盘对Yaw进行互补滤波，不过貌似我公式写的不太对，Yaw还是不正确，也没太深入研究这条路，想先用AHRSupdate这个算法做出来，用来理解四元数和欧拉角。
本帖最后由 john800422 于 2013-8-16 18:26 编辑
zksniper 发表于 2013-8-16 18:10 static/image/common/back.gif
刚刚用不带电子罗盘的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 于 2013-8-19 14:13 编辑
john800422 发表于 2013-8-16 18:24 static/image/common/back.gif
1. Pitch和Roll数据都对，只不过Pitch到不了90度，最多也就正负80度，这个在我的程序里应该算正常。
---> ...
我试了一下对电子罗盘进行校准，发现校准前和校准后差别不大，问题依旧存在，如下：
对电子罗盘校准后，采用AHRSupdate算法，算出欧拉角还是只有Roll是对的；Pitch无论怎么样都不对；而Yaw，有一个奇怪的现象，在顶楼我也提到了，就是当Roll=+90度时，Yaw的输出是对的，稳定性也很好，达到预期效果。
Roll=+90度时的轴向为：x指向北，y指地，z指西。这时如果绕y轴转动，Yaw就是对的，这个问题很奇怪，没想明白为什么。 zksniper 发表于 2013-8-19 14:01 static/image/common/back.gif
我试了一下对电子罗盘进行校准，发现校准前和校准后差别不大，问题依旧存在，如下：
对电子罗盘校准后， ...
先確定IMUupdate正確無誤
把Pitch與Roll解決
再用AHRSupdate
來看Yaw
不要甚麼事都想要一次完成
大部分這樣的想法只會讓你浪費更多時間
最後還是問
"加速度計和陀螺儀有校正過嗎?" john800422 发表于 2013-8-19 23:06 static/image/common/back.gif
先確定IMUupdate正確無誤
把Pitch與Roll解決
IMUupdate我试过了，Pitch和Roll都没问题，这个我在9楼给你回复了
对于加速度计和陀螺仪的校准，我用的是mpu9150内部自带的校准程序，只不过没有求平均，不知道会不会对Yaw和Pitch有足够大的影响，我观察过加速度计和陀螺仪的数据，用来进行姿态融合应该没问题。
我现在一直在想是不是电子罗盘的轴向定义不符合AHRSupdata的设定？不过对应于不同轴向和旋转顺序的公式我都反复检查过，也没发现错误，计算出来的Yaw和Pitch，数据不对这个说法好像不太准确，准确来说应该是在正负之间来回的震荡，不知道什么原因。
还有就是Ki、Kp和HalfT这三个参数了，这三个参数我也反复调过，影响是有的，但没有达到让Yaw和Pitch正确的地步。 本帖最后由 john800422 于 2013-8-20 11:57 编辑
zksniper 发表于 2013-8-20 10:53 static/image/common/back.gif
IMUupdate我试过了，Pitch和Roll都没问题，这个我在9楼给你回复了
对于加速度计和陀螺仪的校准，我用的 ...
1. 我观察过加速度计和陀螺仪的数据，用来进行姿态融合应该没问题
---> 如何觀察的? 陀螺儀的話, 靜止時應該在0或在0處上下震盪, 加速度計的加速度值在90度與-90度應該互為反號且為1g, 0度時應為0g
2. 我现在一直在想是不是电子罗盘的轴向定义不符合AHRSupdata的设定？不过对应于不同轴向和旋转顺序的公式我都反复检查过，也没发现错误，
计算出来的Yaw和Pitch，数据不对这个说法好像不太准确，准确来说应该是在正负之间来回的震荡，不知道什么原因
---> AHRSupdate的算法我並沒有仔細理解過, 原因是
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);
.
電子羅盤的誤差會影響姿態角
假設做在飛行器上, 飛過一顆磁鐵, 影響大的話可能就準備墬機了 (( 不一定為真
若要使用 AHRSupdate , 先把
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);
.
原理搞清楚, 再繼續測試
3. 还有就是Ki、Kp和HalfT这三个参数了，这三个参数我也反复调过，影响是有的，但没有达到让Yaw和Pitch正确的地步。
---> HalfT是不應該調的, HalfT是取樣的週期, 若是算法有問題, 正確不了也是正常的 john800422 发表于 2013-8-20 11:55 static/image/common/back.gif
1. 我观察过加速度计和陀螺仪的数据，用来进行姿态融合应该没问题
---> 如何觀察的? 陀螺儀的話, 靜止時 ...
非常感谢，第2条对我很有启发 学习中…… AHRSupdate这个算法我是没办法了，搞不了，我都开始怀疑算法是有问题的，希望哪位用过的高手给个代码研究研究，最后我还是采用了IMUupdate+楼上黄脸猫的互补滤波求YAW，结果还算满意，姿态融合就先这样吧，先往下走，完事了再回过头来研究。稍后放上代码供比我还新的新手参考。
再次感谢楼上黄脸猫，先给出求YAW的互补滤波算法：
Pitch = asin(-2 * q1 * q3 + 2 * q0 * q2); //俯仰角，绕y轴转动
Roll= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1); //滚动角，绕x轴转动
//0.9和0.1是修正系数，其中5.73=0.1*57.3，乘以57.3是为了将弧度转化为角度
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)));
Pitch = Pitch * 57.3;
Roll = Roll * 57.3
IMUupdata和AHRSupdate算法和坐标系定义没有关系？
记得原文Z轴是向上的？
还是只要满足右手定则就行？ tree666 发表于 2013-9-30 00:07 static/image/common/back.gif
IMUupdata和AHRSupdate算法和坐标系定义没有关系？
记得原文Z轴是向上的？
z轴没必要向上，轴向的定义可以随意，只要你的公式是根据你轴向的定义推导出来的就行，大部分人为了省事，都是按照那两个常见的轴向来定义自己的轴向，只需要直接套公式就行了 {:smile:}{:smile:}{:smile:}{:smile:}{:smile:}{:smile:}{:smile:} 收藏一下
LZ 我也用AHRS 但奇怪的是在开始的一段时间内 大概半分钟内吧 姿态会论七八糟的飘 过完了这阵子就好了而且我拿iPhone指南针试过 我的yaw角与它没偏差至少肉眼看不出来但开始的那段飘不知道为什么LZ有遇到过吗 onev 发表于 2014-5-11 17:29
LZ 我也用AHRS 但奇怪的是在开始的一段时间内 大概半分钟内吧 姿态会论七八糟的飘 过完了这阵子就好了而 ...
查看最开始的那半分钟，你传感器数据是否正确。尤其是磁力计。 为什么要先定义x轴，y轴的指向呢？只要磁力计和陀螺仪，加速度计的坐标一致就可以了吧？ 这里的指向是指参考系
页:
[1]