[问答]

四元数解算的姿态不稳定

问答对人有帮助，内容完整，我也想知道答案 0 使用四元数解算姿态时遇到这两个问题：1.不加入磁力计时，初始时解算出的姿态角挺稳定的，都是0，不过偏航角会漂移，后来加了磁力计，数据倒是不飘了，就是初始时解算出来的姿态角会衰减振荡，经过十几秒之后才能稳定（放置的初始偏航角很小时，振荡时间很短，但是放置存在一定偏航角时，振荡时间长），想问下怎么解决这个振荡问题；2.加入磁力计，当放置的偏航角比较小的时候，解算出来滚转俯仰基本为0，正常，当放置偏航角为正负160这么大时，计算的滚转和俯仰角大约10左右，应该是0的呀；有人说是由于把磁力计融入到了roll和pitch,下面是我使用的解算过程，大家帮忙看下问题出在哪里： CPU_INT08U Att_Solution (float angle_output,float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { float norm; float hx, hy, hz, bx, bz; float wx, wy, wz; float vx, vy, vz; float ex, ey, ez; float q0q0 = q0q0; float q0q1 = q0q1; float q0q2 = q0q2; float q0q3 = q0q3; float q1q1 = q1q1; float q1q2 = q1q2; float q1q3 = q1q3; float q2q2 = q2q2; float q2q3 = q2q3; float q3q3 = q3q3; / 对加速度在体轴系下归一化 / norm = sqrt(axax + ayay + azaz); if(norm == 0) return 0; ax = ax / norm; ay = ay / norm; az = az / norm; norm = sqrt(mxmx + mymy + mzmz); if(norm == 0) return 0; mx = mx / norm; my = my / norm; mz = mz / norm; / compute reference direction of magnetic field / hx = 2mx(0.5f-q2q2-q3q3)+2my(q1q2-q0q3)+2mz(q1q3+q0q2); hy = 2mx(q2q2+q3q3)+2my(0.5f-q1q1-q3q3)+2mz(q2q3-q0q1); hz = 2mx(q1q3-q0q2)+2my(q2q3+q0q1)+2mz(0.5f-q1q1-q2q2); bx = sqrt((hxhx)+(hyhy)); bz = hz; / 将地理坐标系下重力加速度旋转到体轴系下 / vx = 2(q1q3 - q0q2); vy = 2(q0q1 + q2q3); vz = q0q0 - q1q1 - q2q2 + q3q3 ; wx = 2bx(0.5f-q2q2-q3q3)+2bz(q1q3-q0q2); wy = 2bx(q1q2-q0q3)+2bz(q0q1+q2q3); wz = 2bx(q0q2+q1q3)+2bz(0.5f-q1q1-q2q2); / 叉乘求取误差分量 / ex = (ayvz - azvy) + (mywz - mzwy); ey = (azvx - axvz) + (mzwx - mxwz); ez = (axvy - ayvx) + (mxwy - mywx); // ex = (ayvz - azvy) ; // ey = (azvx - axvz) ; // ez = (axvy - ayvx) ; / 对误差分量进行积分 / exInt = exInt + ex Ki; eyInt = eyInt + ey * Ki; ezInt = ezInt + ez * Ki; /* 将误差弥补给角速度 (误差来源于旋转矩阵，最终来源于角速度)/ gx = gx + Kpex + exInt; gy = gy + Kpey + eyInt; gz = gz + Kpez + ezInt; /* 一阶龙格库塔法更新四元数 / q0 = q0 + (-q1gx - q2gy - q3gz)0.002f/2.0f; q1 = q1 + (q0gx + q2gz - q3gy)0.002f/2.0f; q2 = q2 + (q0gy - q1gz + q3gx)0.002f/2.0f; q3 = q3 + (q0gz + q1gy - q2gx)0.002f/2.0f; / 四元数归一化 / norm = sqrt(q0q0 + q1q1 + q2q2 + q3q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; / 四元数转化为欧拉角 / angle_output[0] = atan2(2.0f (q2q3 + q0q1), q0q0-q1q1-q2q2+q3q3) * 57.3f; angle_output[1] = asin(2.0f * (q0q2 - q1q3)) * 57.3f; angle_output[2] = atan2(2.0f * (q1q2 + q0q3), q0q0+q1q1-q2q2-q3q3) * 57.3f; return 1; } 0
2019-7-5 04:35:55　　评论淘帖0 邀请回答您可以邀请以下用户，快速回答问题 × wanqing192 该类别下有 6 个回答。邀请回答 raylue 该类别下有 5 个回答。邀请回答 xiezhe272 该类别下有 5 个回答。邀请回答 wyerwwr 该类别下有 5 个回答。邀请回答 zsjjx 该类别下有 5 个回答。邀请回答 mashiquan123 该类别下有 4 个回答。邀请回答 60user160 该类别下有 4 个回答。邀请回答 tunkanxi8082 该类别下有 4 个回答。邀请回答 moxlanlan 该类别下有 4 个回答。邀请回答 ickey_feng 该类别下有 4 个回答。邀请回答报纸弟弟麦花该类别下有 4 个回答。邀请回答帅帅的欧尼酱该类别下有 4 个回答。邀请回答红粉小Q猪该类别下有 4 个回答。邀请回答 hjfjsdgfjdsf 该类别下有 4 个回答。邀请回答 dfgsgdg 该类别下有 4 个回答。邀请回答泰坦之心该类别下有 4 个回答。邀请回答 zhushanyu262 该类别下有 4 个回答。邀请回答 xujun621 该类别下有 4 个回答。邀请回答 zweipcb 该类别下有 4 个回答。邀请回答小幸运多幸运该类别下有 4 个回答。邀请回答举报卢鉴冰相关推荐 • 请问四轴解算的卡尔曼滤波、互补滤波、四元数都是什么？ 3052 • 请问四轴的姿态解算算法有哪些？ 1158 • mpu6050解算姿态问题 6242 • 请问姿态解算用什么方法处理更好？ 2591 • 请问四元数在无人机姿态控制中应用在哪一个环节？ 2284 • 请问四轴姿态解算误差修正的原理是什么？ 1269 • 请问MPU6050的软件解算姿态和DMP解算姿态各自的优缺点是什么？ 4249 • 求教有关四元数求解姿态问题！ 4086 • 为什么MPU6050数据融合四元数解算得到的俯仰角和滚转角比实际值小两倍多？ 3898 • 四元数求解姿态输出值很小 1149 1个回答