MPU6050 姿态融合.docVIP

MPU6050 姿态融合首先要明确，MPU6050 是一款姿态传感器，使用它就是为了得到待测物体（如四轴、平衡小车） x、y、z 轴的倾角（俯仰角 Pitch、滚转角 Roll、偏航角 Yaw） 。我们通过 I2C 读取到 MPU6050 的六个数据（三轴加速度 AD 值、三轴角速度 AD 值）经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。 主要介绍三种姿态融合算法：四元数法 、一阶互补算法和卡尔曼滤波算法。  一、四元数法 关于四元数的一些概念和计算就不写上来了，我也不懂。我能告诉你的是：通过下面的算法，可以把六个数据转化成四元数（q0、q1、q2、q3），然后四元数转化成欧拉角（P、R、Y 角）。  虽然 MPU6050 自带的 DMP库可以直接输出四元数，减轻 STM32 的运算负担， 这里在此没有使用，因为我是用 STM32 的硬件 I2C 读取 MPU6050 数据的（/forum.ph ... 4page=1#pid3625735），DMP库需要对 I2C 函数进行修改，如 DMP 库中的 I2C 写：i2c_write(st.hw-addr, st.reg-pwr_mgmt_1, 1, (data[0]))；有4个输入变量，而 STM32 硬件 I2C 的 I2C 写为：void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8 pBuffer, u8 writeAddr)，只有 3 个输入量（这之间的差异好像是由于 MPU6050 的 DMP 库是针对 MSP430?单片机写的），所以必须进行修改，但是改固件库是一件很痛苦的事，你们应该都懂。当然，如果你用模拟 I2C 的话，是容易实现的，网上的 DMP 移植几乎都是基于模拟 I2C 的。 复制代码  #includemath.h #include stm32f10x.h // // 变量定义  #define Kp 100.0f? ?? ?? ?? ?? ?? ?? ?? ?// 比例增益支配率收敛到加速度计/磁强计 #define Ki 0.002f? ?? ?? ?? ?? ? // 积分增益支配率的陀螺仪偏见的衔接 #define halfT 0.001f? ?? ?? ?? ?? ? // 采样周期的一半  float q0 = 1, q1 = 0, q2 = 0, q3 = 0;? ?? ?? ? // 四元数的元素，代表估计方向 float exInt = 0, eyInt = 0, ezInt = 0;? ?? ???// 按比例缩小积分误差  float Yaw,Pitch,Roll;??//偏航角，俯仰角，翻滚角  void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { ? ?? ???float norm; ? ?? ???float vx, vy, vz; ? ?? ???float ex, ey, ez;??  ? ?? ???// 测量正常化 ? ?? ???norm = sqrt(ax*ax + ay*ay + az*az);? ?? ? ? ?? ???ax = ax / norm;? ?? ?? ?? ?? ?? ? //单位化 ? ?? ???ay = ay / norm; ? ?? ???az = az / norm;? ?? ?  ? ?? ???// 估计方向的重力 ? ?? ???vx = 2*(q1*q3 - q0*q2); ? ?? ???vy = 2*(q0*q1 + q2*q3); ? ?? ???vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;  ? ?? ???// 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和 ? ?? ???ex = (ay*vz - az*vy); ? ?? ???ey = (az*vx - ax*vz); ? ?? ???ez = (ax*vy - ay*vx);  ? ?? ???// 积分误差比例积分增益 ? ?? ???exInt = exInt + ex*Ki; ? ?? ???eyInt = eyInt + ey*Ki; ? ?? ???ezInt = ezInt + ez*Ki;  ? ?? ???// 调整后的陀螺仪测量 ? ?? ???gx = gx + Kp*ex + exInt; ? ?? ???gy = gy + Kp*ey + eyInt; ? ?? ???gz = gz + Kp*ez + ezInt;  ? ?? ???// 整合四元数率和正常化 ?