?

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

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

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

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

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

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

?

?

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

?

?

?

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

?

?

歐拉角旋轉(zhuǎn)的知識(shí)請(qǐng)先參閱歐拉角旋轉(zhuǎn)，這里需要說(shuō)明的是，本篇需要用到的繞著自己運(yùn)動(dòng)的軸，以ZYX順序旋轉(zhuǎn)。

加速度計(jì)測(cè)量的是其感受到的加速度，在靜止的時(shí)候，其本身是沒(méi)有加速運(yùn)動(dòng)的，但因?yàn)橹亓铀俣鹊淖饔?，根?jù)相對(duì)運(yùn)動(dòng)理論，其感受的加速度與重力加速度正好相反，即讀到的數(shù)據(jù)是豎直向上的。加速度計(jì)的英文簡(jiǎn)寫為acc，下面用首字母a代表加速度計(jì)數(shù)據(jù)。

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

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

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

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

?

?

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

?

?

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

?

?

?

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

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

?

?

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

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

?

?

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

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

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

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

?

?

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

?

?

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

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

?

?

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

?

上面的一些推導(dǎo)計(jì)算過(guò)程，可用MATLAB來(lái)輔助計(jì)算，防止手工計(jì)算出錯(cuò)：

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

% 旋轉(zhuǎn)順序：Z,Y,X（從大地坐標(biāo)系到IMU坐標(biāo)系）% 定義一些符號(hào) r=row p=pitch y=yawsyms r p y
% 3個(gè)旋轉(zhuǎn)矩陣（坐標(biāo)系旋轉(zhuǎn),注意負(fù)號(hào)的位置?。?/span>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)陀螺儀的變換矩陣%定義一些符號(hào) drdt dpdt dydt 指的是分別對(duì) roll pitch yaw求導(dǎo)，也就是角速度syms drdt dpdt dydt
% 繞x軸轉(zhuǎn)動(dòng) row 的角速度dr_t = [drdt;           0;           0];      % 繞y軸轉(zhuǎn)動(dòng) pitch 的角速度dp_t = [   0;        dpdt;           0];      % 繞z軸轉(zhuǎn)動(dòng) yaw 的角速度dy_t = [   0;           0;        dydt];
% [矩陣X*矩陣Y*yaw角速度(繞Z)] + [矩陣X*pitch角速度(繞Y)] + [roll角速度(繞X)]% IMU_gyro實(shí)際就是IMU測(cè)得的3個(gè)陀螺儀數(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角速度組成的列向量，這個(gè)實(shí)際是要求的大地坐標(biāo)系的3個(gè)角速度rpy_t = [drdt;         dpdt;         dydt];
%手動(dòng)分解IMU_gyro為矩陣M_gyro與列向量rpy_t相乘的形式%根據(jù)IMU_gyro寫出M_gyro，該矩陣將大地坐標(biāo)系的角速度轉(zhuǎn)換為IMU坐標(biāo)系M_gyro = [ 1,     0,       -sin(p);           0, cos(r),cos(p)*sin(r);           0,-sin(r),cos(p)*cos(r)];%驗(yàn)證一下if M_gyro * rpy_t==IMU_gyro    fprintf('M_gyro is true
');else    fprintf('M_gyro is false
');endfprintf('M_gyro=
')disp(M_gyro)
% 對(duì)M_gyro求逆矩陣，用于將IMU坐標(biāo)系的陀螺儀角速度值轉(zhuǎn)換到大地坐標(biāo)系M_gyro_inv = inv(M_gyro);fprintf('M_gyro_inv=
')disp(M_gyro_inv)  % matlab求解的逆矩陣需要在再手工化簡(jiǎn)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)]; % 驗(yàn)證一下，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_)

運(yùn)行后的輸出結(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)]

程序先計(jì)算出了M_x*M_y*dy_t + M_x*dp_t + dr_t，然后手工分解為M_gyro與rpy_t相乘的形式，最后求得M_gyro的逆矩陣M_gyro_inv_，即得到用于將IMU坐標(biāo)系的陀螺儀角速度值轉(zhuǎn)換到大地坐標(biāo)系的旋轉(zhuǎn)矩陣。

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

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

運(yùn)行后的輸出結(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)

審核編輯 ：李倩