MPU6050姿态解算2-欧拉角&旋转矩阵
MPU6050姿态解算2-欧拉角&旋转矩阵
注：本篇中的一些图采用横线放置，若观看不方便，可点击文章末尾的阅读原文跳转到网页版
1
IMU姿态解算
IMU，即惯性测量单元，一般包含三轴陀螺仪与三轴加速度计。之前的文章MPU6050姿态解算方式1-DMP已将对MPU6050这款IMU作了简单的介绍，并通过其内部的DMP处理单元直接得到姿态解算的四元数结果。本篇将通过软件解算的方式，利用欧拉角与旋转矩阵来对陀螺仪与加速度计的原始数据进行姿态求解，并将两种姿态进行互补融合，最终得到IMU的实时姿态。
本篇的姿态解算选用的旋转顺序为ZYX，即IMU坐标系初始时刻与大地坐标系重合，然后依次绕自己的Z、Y、X轴进行旋转，这里先自定义一下每次的旋转名称和符号：
- 绕IMU的Z轴旋转：航向角yaw， 转动 y 角度
- 绕IMU的Y轴旋转：俯仰角pitch，转动 p 角度
- 绕IMU的X轴旋转：横滚角row， 转动 r 角度
三次旋转的示意图如下：
另外，横滚roll，俯仰pitch，偏航yaw的实际含义如下图：
2
旋转矩阵
旋转矩阵的知识请先参阅3维旋转矩阵推导与助记与3维旋转矩阵推导与助记-补充篇，这里只列出本篇需要用到的3个旋转矩阵，注意这3个旋转矩阵是坐标变换的旋转矩阵。
3
欧拉角旋转
欧拉角旋转的知识请先参阅欧拉角旋转，这里需要说明的是，本篇需要用到的绕着自己运动的轴，以ZYX顺序旋转。
4
加速度计解算姿态角
加速度计测量的是其感受到的加速度，在静止的时候，其本身是没有加速运动的，但因为重力加速度的作用，根据相对运动理论，其感受的加速度与重力加速度正好相反，即读到的数据是竖直向上的。加速度计的英文简写为acc，下面用首字母a代表加速度计数据。
加速度利用静止时刻感受到重力加速度，计算姿态：
- 当加速度计水平放置，即Z轴竖直向上时，Z轴可以读到1g的数值（g为重力加速度），X轴和Y轴两个方向读到0，可以记作（0，0，g）。
- 当加速度计旋转一定的姿态时，重力加速度会在加速度的3个轴上产生相应的分量，其本质是大地坐标系下的（0，0，g）在新的加速度计自身坐标系下的坐标，加速度计读到的3个值就是（0，0，g）向量的新坐标。
姿态的旋转选用ZYX顺序的3次旋转方式，则上述描述可表示为：
解这个方程，可以得到roll和pitch角（由于绕Z旋转时，感受到的重力加速度是不变的，因此加速度计无法计算yaw角）
3次旋转过程的分解过程如下图：
5
陀螺仪解算姿态角
陀螺仪测量的绕3个轴转动的角速度，因此，对角速度积分，可以得到角度。陀螺仪的英文简写为gyro，下面用首字母g代表陀螺仪数据。
如下图，IMU在第n个时刻的姿态角度为r、p、y，其含义为IMU坐标系从初始位置，经过绕Z旋转y角度，绕Y旋转p角度，绕X旋转r角度，得到了最终的姿态，此时需要计算下一个时刻(n+1)的姿态。设n+1时刻的姿态角为r+Δr、p+Δp、y+Δy，该姿态也是经历了3次旋转。要想计算n+1时刻的姿态，只要在n时刻姿态的基础上，加上对应的姿态角度变化量即可。姿态角度的变化量可以通过角速度与采用时间周期积分即可。
这里红框中dr/dt等角速度实际是假想的角速度，用于姿态更新，姿态更新是以大地坐标系为参考，而陀螺仪在第n个状态读出的角速度是以它自己所在的坐标系为参考，需要将读到的gyro陀螺数据经过变换，才能用于计算更新第n+1次的姿态。
那dr/dt等角速度该怎样理解呢？看下面这个图，还是将其分解为3次旋转：
首先来看dy/dt，它是3次旋转过程中绕Z轴的yaw角的角速度，3次旋转首先就是绕着Z轴旋转，Z轴方向的单位向量可表示为[0 0 1]T，T表示向量转置，因此[0 0 dy/dt]T表示在图中状态①的坐标中绕Z的角速度。由于之后该坐标系还要经历绕Y和绕X的两次旋转，因此这里[0 0 dy/dt]T角速度在经历两次旋转后，在最终的坐标系(状态③)中的坐标也要经历两次变换。图中的[gx_Z gy_Z gz_Z]T表示3次旋转过程中绕Z轴的yaw角的角速度在最终姿态中的等效转动角速度，实际就是状态①坐标系中绕Z轴的角速度在状态③坐标系中的新的坐标。
同理，dp/dt还需要经历1次旋转变换，而dr/dt不需要经历旋转。
将dy/dt，dp/dt，dr/dt三者都变换到状态③坐标系中的新的坐标相加，实际就是状态③时刻陀螺仪自己读到的gyro数据。
所以，从dr/dt等角速度到陀螺仪读到的角速度gx等的转换关系推导过程如下：
进一步，再把这里的状态③理解为状态n，则根据状态n时刻读到的陀螺仪数据，反解dy/dt等角速度数据，即可更新得到状态n+1的姿态。反解就是求逆矩阵，即：
6
姿态融合
由上面的分析可知，加速度计在静止时刻，根据感受到的重力加速度，可以计算出roll和pitch角，并且角度计算只与当前姿态有关。而陀螺仪是对时间间隔内的角速度积分，得到每一次的角度变换量，累加到上一次的姿态角上，得到新的姿态角，陀螺仪可以计算roll、pitch、yaw三个角。
实际上，加速度仅在静止时刻可以得到较准确的姿态，而陀螺仪仅对转动时的姿态变化敏感，且陀螺仪若本身存在误差，则经过连续的时间积分，误差会不断增大。因此，需要结合两者计算的姿态，进行互补融合。当然，这里只能对roll和pitch融合，因为加速度计没有得到yaw。
K为比例系数，需要根据实际来调整，如选用0.4。
7
MATLAB公式推导
上面的一些推导计算过程，可用MATLAB来辅助计算，防止手工计算出错：
先定义3个旋转矩阵Y
% 旋转顺序：Z,Y,X（从大地坐标系到IMU坐标系）
% 定义一些符号 r=row p=pitch y=yaw
syms r p y
% 3个旋转矩阵（坐标系旋转,注意负号的位置！）
M_x = [ 1, 0, 0;
0, cos(r), sin(r);
0, -sin(r), cos(r)];
M_y = [cos(p), 0, -sin(p);
0, 1, 0;
sin(p), 0, cos(p)];
M_z = [ cos(y), sin(y), 0;
-sin(y), cos(y), 0;
0, 0, 1];推导陀螺仪的变换矩阵
%% 推导陀螺仪的变换矩阵
%定义一些符号 drdt dpdt dydt 指的是分别对 roll pitch yaw求导，也就是角速度
syms drdt dpdt dydt
% 绕x轴转动 row 的角速度
dr_t = [drdt;
0;
0];
% 绕y轴转动 pitch 的角速度
dp_t = [ 0;
dpdt;
0];
% 绕z轴转动 yaw 的角速度
dy_t = [ 0;
0;
dydt];
% [矩阵X*矩阵Y*yaw角速度(绕Z)] + [矩阵X*pitch角速度(绕Y)] + [roll角速度(绕X)]
% IMU_gyro实际就是IMU测得的3个陀螺仪数据
IMU_gyro = M_x*M_y*dy_t + M_x*dp_t + dr_t;
fprintf('M_x*M_y*dy_t + M_x*dp_t + dr_t=\r\n')
disp(IMU_gyro)
% roll pitch yaw角速度组成的列向量，这个实际是要求的大地坐标系的3个角速度
rpy_t = [drdt;
dpdt;
dydt];
%手动分解IMU_gyro为矩阵M_gyro与列向量rpy_t相乘的形式
%根据IMU_gyro写出M_gyro，该矩阵将大地坐标系的角速度转换为IMU坐标系
M_gyro = [ 1, 0, -sin(p);
0, cos(r),cos(p)*sin(r);
0,-sin(r),cos(p)*cos(r)];
%验证一下
if M_gyro * rpy_t==IMU_gyro
fprintf('M_gyro is true\r\n');
else
fprintf('M_gyro is false\r\n');
end
fprintf('M_gyro=\r\n')
disp(M_gyro)
% 对M_gyro求逆矩阵，用于将IMU坐标系的陀螺仪角速度值转换到大地坐标系
M_gyro_inv = inv(M_gyro);
fprintf('M_gyro_inv=\r\n')
disp(M_gyro_inv)
% matlab求解的逆矩阵需要在再手工化简
M_gyro_inv_ =[ 1, (sin(p)*sin(r))/cos(p), (cos(r)*sin(p))/cos(p);
0, cos(r), -sin(r);
0, sin(r)/cos(p), cos(r)/cos(p)];
% 验证一下，M_gyro_inv_ * M_gyro_inv应该是单位矩阵
fprintf('M_gyro_inv_ * M_gyro=\r\n')
disp(M_gyro_inv_ * M_gyro)
fprintf('M_gyro_inv_ =\r\n')
disp(M_gyro_inv_)运行后的输出结果：
M_x*M_y*dy_t + M_x*dp_t + dr_t=
drdt - dydt*sin(p)
dpdt*cos(r) + dydt*cos(p)*sin(r)
dydt*cos(p)*cos(r) - dpdt*sin(r)
M_gyro is true
M_gyro=
[ 1, 0, -sin(p)]
[ 0, cos(r), cos(p)*sin(r)]
[ 0, -sin(r), cos(p)*cos(r)]
M_gyro_inv=
[ 1, (sin(p)*sin(r))/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2), (cos(r)*sin(p))/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2)]
[ 0, cos(r)/(cos(r)^2 + sin(r)^2), -sin(r)/(cos(r)^2 + sin(r)^2)]
[ 0, sin(r)/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2), cos(r)/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2)]
M_gyro_inv_ * M_gyro=
[ 1, 0, cos(r)^2*sin(p) - sin(p) + sin(p)*sin(r)^2]
[ 0, cos(r)^2 + sin(r)^2, 0]
[ 0, 0, cos(r)^2 + sin(r)^2]
M_gyro_inv_ =
[ 1, (sin(p)*sin(r))/cos(p), (cos(r)*sin(p))/cos(p)]
[ 0, cos(r), -sin(r)]
[ 0, sin(r)/cos(p), cos(r)/cos(p)]程序先计算出了M_x*M_y*dy_t + M_x*dp_t + dr_t，然后手工分解为M_gyro与rpy_t相乘的形式，最后求得M_gyro的逆矩阵M_gyro_inv_，即得到用于将IMU坐标系的陀螺仪角速度值转换到大地坐标系的旋转矩阵。
推导加速度计的变换矩阵
%% 推导加速度计的变换矩阵
M_acc=M_x*M_y*M_z;
fprintf('M_acc=\r\n')
disp(M_acc)
%重力向量
syms g
acc = [0;
0;
g];
IMU_acc=M_acc*acc;
fprintf('IMU_acc=\r\n')
disp(IMU_acc)运行后的输出结果：
M_acc=
[ cos(p)*cos(y), cos(p)*sin(y), -sin(p)]
[ cos(y)*sin(p)*sin(r) - cos(r)*sin(y), cos(r)*cos(y) + sin(p)*sin(r)*sin(y), cos(p)*sin(r)]
[ sin(r)*sin(y) + cos(r)*cos(y)*sin(p), cos(r)*sin(p)*sin(y) - cos(y)*sin(r), cos(p)*cos(r)]
IMU_acc=
-g*sin(p)
g*cos(p)*sin(r)
g*cos(p)*cos(r)计算得到的IMU_acc即是加速度计感受到的重力加速度向量在IMU最终姿态所在坐标系中的坐标。