[问答]

四元素法解算6050姿态角，求得的欧拉角需要一定时间才能稳定，求解

3933

问答对人有帮助，内容完整，我也想知道答案0 四元素法解算6050姿态角，因四元素不断更新，即便静止状态，但只要一直调用该函数，四元素也将不断更新，导致求得的欧拉角也不断更新，需要一定时间稳定，求助大神！！代码如下： float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向 float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差 float exDif=0,eyDif=0,ezDif=0; float Yaw,Pitch,Roll; //偏航角，俯仰角，翻滚角 #define Kp 2.0f // 比例增益支配率收敛到加速度计/磁强计 #define Ki 0.001f // 积分增益支配率的陀螺仪偏见的衔接 #define Kd 0.001f #define halfT 0.005f //四元素求欧拉角 void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez; static float last_ex,last_ey,last_ez; // 测量正常化 norm = sqrt(axax + ayay + azaz); ax = ax / norm; //单位化 ay = ay / norm; az = az / norm; // 估计方向的重力 vx = 2(q1q3 - q0q2); vy = 2(q0q1 + q2q3); vz = q0q0 - q1q1 - q2q2 + q3q3; // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和 ex = (ayvz - azvy); ey = (azvx - axvz); ez = (axvy - ayvx); // 积分误差比例积分增益 exInt = exInt + exKi; eyInt = eyInt + eyKi; ezInt = ezInt + ezKi; exDif = ex-last_ex; eyDif = ey-last_ey; ezDif = ez-last_ez; last_ex = ex; last_ey = ey; last_ez = ez; // 调整后的陀螺仪测量 gx = gx + Kpex + exInt + KdexDif; gy = gy + Kpey + eyInt + KdeyDif; gz = gz + Kpez + ezInt + KdezDif; // 整合四元数率和正常化 q0 = q0 + (-q1gx - q2gy - q3gz)halfT; q1 = q1 + (q0gx + q2gz - q3gy)halfT; q2 = q2 + (q0gy - q1gz + q3gx)halfT; q3 = q3 + (q0gz + q1gy - q2gx)halfT; // 正常化四元 norm = sqrt(q0q0 + q1q1 + q2q2 + q3q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,转换为度数 Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv Yaw = atan2(2(q1q2 + q0q3),q0q0+q1q1-q2q2-q3q3) 57.3; //Yaw,转换为度数 } 0 已退回20积分
2018-9-20 21:39:41　　评论淘帖0 邀请回答您可以邀请以下用户，快速回答问题 × 谢新貌该类别下有 43 个回答。邀请回答新星之火12138 该类别下有 39 个回答。邀请回答 911119qianhao 该类别下有 35 个回答。邀请回答 hgimtk 该类别下有 32 个回答。邀请回答 wang21cj 该类别下有 31 个回答。邀请回答独孤辉二该类别下有 27 个回答。邀请回答 wuli北该类别下有 25 个回答，其中被选为最佳答案 1 次。邀请回答 horayte 该类别下有 24 个回答，其中被选为最佳答案 1 次。邀请回答 butterflydw 该类别下有 24 个回答。邀请回答 hy381 该类别下有 24 个回答。邀请回答周鹏生该类别下有 24 个回答。邀请回答修罗君该类别下有 24 个回答，其中被选为最佳答案 1 次。邀请回答牛牛爱吃草该类别下有 24 个回答。邀请回答大洼球王该类别下有 23 个回答。邀请回答蔡甸一角该类别下有 23 个回答。邀请回答 32949523596693 该类别下有 23 个回答。邀请回答 hisysteeoke 该类别下有 23 个回答。邀请回答名士流该类别下有 23 个回答。邀请回答洒下墨色该类别下有 22 个回答。邀请回答 a732538 该类别下有 22 个回答。邀请回答举报 MrXcq 相关推荐 • 请问谁有mpu6050姿态解算的欧拉角？1449 • 为什么四元素解算出roll和pitch数值一直变化？1355 • 为什么MPU6050数据融合四元数解算得到的俯仰角和滚转角比实际值小两倍多？3772 • MPU6050姿态解算的原理是什么1634 • 请问四元数在无人机姿态控制中应用在哪一个环节？2202 • mpu6050解算姿态问题6151 •四元数解算的姿态不稳定4158 • 请问MPU6050的软件解算姿态和DMP解算姿态各自的优缺点是什么？4119 • 请问四轴姿态解算误差修正的原理是什么？1206 • 请问6050能直接读出欧拉角为什么很多飞控代码还要用角速度去算?2823 1个回答

答案对人有帮助，有参考价值0 请教到大神了，比例增益Kp的值设的太小了，PI调节器响应太慢，目前Kp设置为100就好了

2018-9-21 11:02:54 评论举报 MrXcq