?

IMU，即慣性測量單元，一般包含三軸陀螺儀與三軸加速度計。之前的文章MPU6050姿態(tài)解算方式1-DMP已將對MPU6050這款I(lǐng)MU作了簡單的介紹，并通過其內(nèi)部的DMP處理單元直接得到姿態(tài)解算的四元數(shù)結(jié)果。本篇將通過軟件解算的方式，利用歐拉角與旋轉(zhuǎn)矩陣來對陀螺儀與加速度計的原始數(shù)據(jù)進行姿態(tài)求解，并將兩種姿態(tài)進行互補融合，最終得到IMU的實時姿態(tài)。

本篇的姿態(tài)解算選用的旋轉(zhuǎn)順序為ZYX，即IMU坐標系初始時刻與大地坐標系重合，然后依次繞自己的Z、Y、X軸進行旋轉(zhuǎn)，這里先自定義一下每次的旋轉(zhuǎn)名稱和符號：

• 繞IMU的Z軸旋轉(zhuǎn)：航向角yaw， ?轉(zhuǎn)動 y 角度

• 繞IMU的Y軸旋轉(zhuǎn)：俯仰角pitch，轉(zhuǎn)動 p 角度

• 繞IMU的X軸旋轉(zhuǎn)：橫滾角row， ? 轉(zhuǎn)動 r 角度

三次旋轉(zhuǎn)的示意圖如下：

?

?

另外，橫滾roll，俯仰pitch，偏航y(tǒng)aw的實際含義如下圖：

?

?

?

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

?

?

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

加速度計測量的是其感受到的加速度，在靜止的時候，其本身是沒有加速運動的，但因為重力加速度的作用，根據(jù)相對運動理論，其感受的加速度與重力加速度正好相反，即讀到的數(shù)據(jù)是豎直向上的。加速度計的英文簡寫為acc，下面用首字母a代表加速度計數(shù)據(jù)。

加速度利用靜止時刻感受到重力加速度，計算姿態(tài)：

• 當加速度計水平放置，即Z軸豎直向上時，Z軸可以讀到1g的數(shù)值（g為重力加速度），X軸和Y軸兩個方向讀到0，可以記作（0，0，g）。

• 當加速度計旋轉(zhuǎn)一定的姿態(tài)時，重力加速度會在加速度的3個軸上產(chǎn)生相應(yīng)的分量，其本質(zhì)是大地坐標系下的（0，0，g）在新的加速度計自身坐標系下的坐標，加速度計讀到的3個值就是（0，0，g）向量的新坐標。

姿態(tài)的旋轉(zhuǎn)選用ZYX順序的3次旋轉(zhuǎn)方式，則上述描述可表示為：

?

?

解這個方程，可以得到roll和pitch角（由于繞Z旋轉(zhuǎn)時，感受到的重力加速度是不變的，因此加速度計無法計算yaw角）

?

?

3次旋轉(zhuǎn)過程的分解過程如下圖：

?

?

?

陀螺儀測量的繞3個軸轉(zhuǎn)動的角速度，因此，對角速度積分，可以得到角度。陀螺儀的英文簡寫為gyro，下面用首字母g代表陀螺儀數(shù)據(jù)。

如下圖，IMU在第n個時刻的姿態(tài)角度為r、p、y，其含義為IMU坐標系從初始位置，經(jīng)過繞Z旋轉(zhuǎn)y角度，繞Y旋轉(zhuǎn)p角度，繞X旋轉(zhuǎn)r角度，得到了最終的姿態(tài)，此時需要計算下一個時刻(n+1)的姿態(tài)。設(shè)n+1時刻的姿態(tài)角為r+Δr、p+Δp、y+Δy，該姿態(tài)也是經(jīng)歷了3次旋轉(zhuǎn)。要想計算n+1時刻的姿態(tài)，只要在n時刻姿態(tài)的基礎(chǔ)上，加上對應(yīng)的姿態(tài)角度變化量即可。姿態(tài)角度的變化量可以通過角速度與采用時間周期積分即可。

?

?

這里紅框中dr/dt等角速度實際是假想的角速度，用于姿態(tài)更新，姿態(tài)更新是以大地坐標系為參考，而陀螺儀在第n個狀態(tài)讀出的角速度是以它自己所在的坐標系為參考，需要將讀到的gyro陀螺數(shù)據(jù)經(jīng)過變換，才能用于計算更新第n+1次的姿態(tài)。

那dr/dt等角速度該怎樣理解呢？看下面這個圖，還是將其分解為3次旋轉(zhuǎn)：

?

?

首先來看dy/dt，它是3次旋轉(zhuǎn)過程中繞Z軸的yaw角的角速度，3次旋轉(zhuǎn)首先就是繞著Z軸旋轉(zhuǎn)，Z軸方向的單位向量可表示為[0 0 1]T，T表示向量轉(zhuǎn)置，因此[0 0 dy/dt]T表示在圖中狀態(tài)①的坐標中繞Z的角速度。由于之后該坐標系還要經(jīng)歷繞Y和繞X的兩次旋轉(zhuǎn)，因此這里[0 0 dy/dt]T角速度在經(jīng)歷兩次旋轉(zhuǎn)后，在最終的坐標系(狀態(tài)③)中的坐標也要經(jīng)歷兩次變換。圖中的[gx_Z gy_Z gz_Z]T表示3次旋轉(zhuǎn)過程中繞Z軸的yaw角的角速度在最終姿態(tài)中的等效轉(zhuǎn)動角速度，實際就是狀態(tài)①坐標系中繞Z軸的角速度在狀態(tài)③坐標系中的新的坐標。

同理，dp/dt還需要經(jīng)歷1次旋轉(zhuǎn)變換，而dr/dt不需要經(jīng)歷旋轉(zhuǎn)。

將dy/dt，dp/dt，dr/dt三者都變換到狀態(tài)③坐標系中的新的坐標相加，實際就是狀態(tài)③時刻陀螺儀自己讀到的gyro數(shù)據(jù)。

所以，從dr/dt等角速度到陀螺儀讀到的角速度gx等的轉(zhuǎn)換關(guān)系推導(dǎo)過程如下：

?

?

進一步，再把這里的狀態(tài)③理解為狀態(tài)n，則根據(jù)狀態(tài)n時刻讀到的陀螺儀數(shù)據(jù)，反解dy/dt等角速度數(shù)據(jù)，即可更新得到狀態(tài)n+1的姿態(tài)。反解就是求逆矩陣，即：

?

?

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

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

?

?

K為比例系數(shù)，需要根據(jù)實際來調(diào)整，如選用0.4。

?

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

先定義3個旋轉(zhuǎn)矩陣Y

% 旋轉(zhuǎn)順序：Z,Y,X（從大地坐標系到IMU坐標系）% 定義一些符號 r=row p=pitch y=yawsyms r p y
% 3個旋轉(zhuǎn)矩陣（坐標系旋轉(zhuǎn),注意負號的位置！）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];

推導(dǎo)陀螺儀的變換矩陣

%% 推導(dǎo)陀螺儀的變換矩陣%定義一些符號 drdt dpdt dydt 指的是分別對 roll pitch yaw求導(dǎo)，也就是角速度syms drdt dpdt dydt
% 繞x軸轉(zhuǎn)動 row 的角速度dr_t = [drdt;           0;           0];      % 繞y軸轉(zhuǎn)動 pitch 的角速度dp_t = [   0;        dpdt;           0];      % 繞z軸轉(zhuǎn)動 yaw 的角速度dy_t = [   0;           0;        dydt];
% [矩陣X*矩陣Y*yaw角速度(繞Z)] + [矩陣X*pitch角速度(繞Y)] + [roll角速度(繞X)]% IMU_gyro實際就是IMU測得的3個陀螺儀數(shù)據(jù)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相乘的形式%根據(jù)IMU_gyro寫出M_gyro，該矩陣將大地坐標系的角速度轉(zhuǎn)換為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坐標系的陀螺儀角速度值轉(zhuǎn)換到大地坐標系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應(yīng)該是單位矩陣fprintf('M_gyro_inv_ * M_gyro=
')disp(M_gyro_inv_ * M_gyro)fprintf('M_gyro_inv_ =
')disp(M_gyro_inv_)

運行后的輸出結(jié)果：

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坐標系的陀螺儀角速度值轉(zhuǎn)換到大地坐標系的旋轉(zhuǎn)矩陣。

推導(dǎo)加速度計的變換矩陣

%% 推導(dǎo)加速度計的變換矩陣
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)

運行后的輸出結(jié)果：

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)

審核編輯 ：李倩