CW32L012解算MPU6050姿态数据

姿态解算

我们仅仅获取了MPU6050的三轴加速度和角速度，要想得到姿态角，需要利用读取的数据进行姿态解算。

一、欧拉角

欧拉角是一种用于描述三维刚体相对参考坐标系（通常为水平面的直角坐标系）姿态的参数，通过依次绕三个互相垂直的坐标轴（对应姿态检测中常用的翻滚角 Roll、俯仰角 Pitch、偏航角 Yaw，分别绕 X、Y、Z 轴旋转）的旋转角度来表征设备的倾斜、转向状态。

如下图所示：

横滚角（Roll）：绕运动坐标系的Y轴旋转

偏航角（Yaw）：绕运动坐标系的Z轴旋转

俯仰角（Roll）：绕运动坐标系的X轴旋转

二、姿态解算

2.1利用加速度计解算姿态（仅能解算Roll和Pitch）

欧拉角与旋转矩阵来对陀螺仪与加速度计的原始数据进行姿态求解，并将两种姿态进行互补融合，最终得到IMU的实时姿态。

2.1.1位姿矩阵：

位姿矩阵是用于在三维笛卡尔参考坐标系中，统一描述刚体的位置（平移状态）与姿态 的 4×4 阶齐次变换矩阵，其前 3×3 的子矩阵为旋转矩阵，对应由欧拉角等姿态参数转换而来的刚体旋转信息，用来表征设备相对参考坐标系的倾斜、转向姿态（即 MPU6050 检测的 Roll、Pitch、Yaw 对应的姿态状态），矩阵最后一列的前 3 个元素为平移向量，用来表征设备在参考坐标系中的三维位置坐标，最后一行固定为 [0,0,0,1] 以满足齐次坐标的运算规范，它可以将刚体的旋转与平移变换整合为一次矩阵运算，在嵌入式姿态检测与空间定位的场景中，能简化不同坐标系间的姿态、位置转换计算，常作为姿态解算、多传感器融合的基础数学工具。

当加速度计水平放置，即Z轴竖直向上时，Z轴可以读到1g的数值（g为重力加速度），X轴和Y轴两个方向读到0，初始位姿可以记作：

本篇的姿态解算选用的旋转顺序为ZYX，即IMU坐标系初始时刻与大地坐标系重合，然后依次绕自己的Z、Y、X轴进行旋转，这里先自定义一下每次的旋转名称和符号：

绕IMU的Z轴旋转：航向角yaw， 转动y角度

绕IMU的Y轴旋转：俯仰角pitch，转动p角度

绕IMU的X轴旋转：横滚角row， 转动r角度

当IMU绕Z轴旋转y度，再绕Y轴旋转P度，再绕X轴旋转r度，其终止位姿可以表示为：

根据机器人运动学：对于一个传感器的末端姿态，我们可以将其看作分别绕z、y，x轴旋转得到，其中正向运动学的绕各轴的旋转矩阵如下：

现在我们已知了初始位姿，终止位姿、绕三轴的旋转矩阵，对其进行机器人逆运动学的求解，可求得绕三个轴的旋转角度：

因为MPU6050是绕运动坐标系旋转，所以对应的矩阵变换是左乘：

解这个方程，可以得到roll和pitch角（由于绕Z旋转时，感受到的重力加速度是不变的，因此加速度计无法计算****yaw角）

当IMU绕Z轴旋转y度，再绕Y轴旋转P度，再绕X轴旋转r度，其终止位姿可以表示为：

根据机器人运动学：对于一个传感器的末端姿态，我们可以将其看作分别绕z、y，x轴旋转得到，其中正向运动学的绕各轴的旋转矩阵如下：

现在我们已知了初始位姿，终止位姿、绕三轴的旋转矩阵，对其进行机器人逆运动学的求解，可求得绕三个轴的旋转角度：

因为MPU6050是绕运动坐标系旋转，所以对应的矩阵变换是左乘：

解这个方程，可以得到roll和pitch角（由于绕Z旋转时，感受到的重力加速度是不变的，因此加速度计无法计算yaw角）

解这个方程，可以得到roll和pitch角（由于绕Z旋转时，感受到的重力加速度是不变的，因此加速度计无法计算yaw角）

对应C语言代码：

//（atan2返回弧度，需转成度） Pitch角公式：atan2(AX, sqrt(AY2 + AZ2)) × (180/π)
        accAnglePitch = atan2(ax, sqrt(ay*ay + az*az)) * (180.0f / M_PI);
        //  （若算roll角：atan2(AY, sqrt(AX2 + AZ2)) × (180/π)）
        accAngleRoll = atan2(ay, sqrt(ax*ax + az*az)) * (180.0f / M_PI);

2.2利用陀螺仪解算姿态

我们知道陀螺仪输出的是角速度，我们对其进行积分，可以得到角度：

yaw角解算对应代码：

float gyroRateZ = -((gz / GYRO_SENSITIVITY) - gyroBiasZ);  // 扣除零偏后的角速度后取反
// 步骤3：角速度积分计算Yaw角（核心：角度 = 角速度 × 时间）
// 复用IMU_Process_Kalman中已计算的dt（采样时间，单位s）
 Cacl_yawAngle += gyroRateZ * dt;

pitch和roll角对应解算公式：

pitch和roll角对应解算代码（此代码在卡尔曼滤波器中，根据传入参数pitch和roll来决定解算什么角）：

//此代码在卡尔曼滤波器中，根据传入参数pitch和roll来决定解算什么角
float KalmanFilter(float newAngle, float newRate, float dt,
                   float *kalmanAngle, float *kalmanBias, float kalmanP[2][2]) 
float rate = newRate - *kalmanBias;  // 减去偏置             
*kalmanAngle += dt * rate;           // 积分得到角度变化