?

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的實際含義如下圖：

?

?

?

旋轉矩陣的知識請先參閱3維旋轉矩陣推導與助記與3維旋轉矩陣推導與助記-補充篇，這里只列出本篇需要用到的3個旋轉矩陣，注意這3個旋轉矩陣是坐標變換的旋轉矩陣。

?

?

歐拉角旋轉的知識請先參閱歐拉角旋轉，這里需要說明的是，本篇需要用到的繞著自己運動的軸，以ZYX順序旋轉。

加速度計測量的是其感受到的加速度，在靜止的時候，其本身是沒有加速運動的，但因為重力加速度的作用，根據相對運動理論，其感受的加速度與重力加速度正好相反，即讀到的數據是豎直向上的。加速度計的英文簡寫為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次旋轉過程的分解過程如下圖：

?

?

?

陀螺儀測量的繞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的姿態。反解就是求逆矩陣，即：

?

?

由上面的分析可知，加速度計在靜止時刻，根據感受到的重力加速度，可以計算出roll和pitch角，并且角度計算只與當前姿態有關。而陀螺儀是對時間間隔內的角速度積分，得到每一次的角度變換量，累加到上一次的姿態角上，得到新的姿態角，陀螺儀可以計算roll、pitch、yaw三個角。

實際上，加速度僅在靜止時刻可以得到較準確的姿態，而陀螺儀僅對轉動時的姿態變化敏感，且陀螺儀若本身存在誤差，則經過連續的時間積分，誤差會不斷增大。因此，需要結合兩者計算的姿態，進行互補融合。當然，這里只能對roll和pitch融合，因為加速度計沒有得到yaw。

?

?

K為比例系數，需要根據實際來調整，如選用0.4。

?

上面的一些推導計算過程，可用MATLAB來輔助計算，防止手工計算出錯：

先定義3個旋轉矩陣Y

% 旋轉順序：Z,Y,X（從大地坐標系到IMU坐標系）% 定義一些符號 r=row p=pitch y=yawsyms 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=
')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
');else    fprintf('M_gyro is false
');endfprintf('M_gyro=
')disp(M_gyro)
% 對M_gyro求逆矩陣，用于將IMU坐標系的陀螺儀角速度值轉換到大地坐標系M_gyro_inv = inv(M_gyro);fprintf('M_gyro_inv=
')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=
')disp(M_gyro_inv_ * M_gyro)fprintf('M_gyro_inv_ =
')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=
')disp(M_acc)
%重力向量syms gacc = [0;       0;       g];IMU_acc=M_acc*acc;fprintf('IMU_acc=
')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)

審核編輯 ：李倩