IMU姿態(tài)解算

IMU，即慣性測(cè)量單元，一般包含三軸陀螺儀與三軸加速度計(jì)。之前的文章。。。已將對(duì)MPU6050這款I(lǐng)MU作了簡(jiǎn)單的介紹，并通過其內(nèi)部的DMP處理單元直接得到姿態(tài)解算的四元數(shù)結(jié)果。本篇將通過軟件解算的方式，利用歐拉角與旋轉(zhuǎn)矩陣來對(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 id="s0sssss00s" class="md-pair-s " style="">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)矩陣

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

歐拉角旋轉(zhuǎn)

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

加速度計(jì)解算姿態(tài)角

加速度計(jì)測(cè)量的是其感受到的加速度，在靜止的時(shí)候，其本身是沒有加速運(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)過程的分解過程如下圖：

陀螺儀解算姿態(tài)角

陀螺儀測(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)過繞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)角度的變化量可以通過角速度與采用時(shí)間周期積分即可。

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

那dr/dt等角速度該怎樣理解呢？看下面這個(gè)圖，還是將其分解為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)①的坐標(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)過程中繞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)過程如下：

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

姿態(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)過連續(xù)的時(shí)間積分，誤差會(huì)不斷增大。因此，需要結(jié)合兩者計(jì)算的姿態(tài)，進(jìn)行互補(bǔ)融合。當(dāng)然，這里只能對(duì)roll和pitch融合，因?yàn)榧铀俣扔?jì)沒有得到y(tǒng)aw。

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

MATLAB公式推導(dǎo)

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

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

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

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

程序先計(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ì)的變換矩陣

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