<求助>姿态融合算法搞不定了
本帖最后由 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]