概述

LIS2MDL 包含三軸磁力計。
lsm6ds3trc包含三軸陀螺儀與三軸加速度計。
姿態(tài)有多種數(shù)學表示方式，常見的是四元數(shù)，歐拉角，矩陣和軸角。他們各自有其自身的優(yōu)點，在不同的領域使用不同的表示方式。在四軸飛行器中使用到了四元數(shù)和歐拉角。

姿態(tài)解算選用的旋轉(zhuǎn)順序為ZYX，即IMU坐標系初始時刻與大地坐標系重合，然后依次繞自己的Z、Y、X軸進行旋轉(zhuǎn)：

繞IMU的Z軸旋轉(zhuǎn)：航向角yaw

繞IMU的Y軸旋轉(zhuǎn)：俯仰角pitch

繞IMU的X軸旋轉(zhuǎn)：橫滾角row

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

由于需要解析姿態(tài)角，故將陀螺儀速度修改快一點。

視頻教學

https://www.bilibili.com/video/BV13j411W7F5/

樣品申請

https://www.wjx.top/vm/OhcKxJk.aspx#

完整代碼下載

https://download.csdn.net/download/qq_24312945/88639256

使用硬件

歐拉角

橫滾角φ：機體繞OBXB轉(zhuǎn)動，軸Y'B與平面OBXBYB構成的夾角。

俯仰角θ：機體繞OBYB轉(zhuǎn)動，軸Z'B與平面OBYBZB構成的夾角。

偏航角ψ：機體繞OBZB轉(zhuǎn)動，軸X'B與平面OBXBZB構成的夾角。

將姿態(tài)角從機體坐標系轉(zhuǎn)換到慣性坐標系中是為了便于分析無人機狀態(tài)，反映無人機在慣性坐標系下的姿態(tài)運動狀態(tài)，利用齊次線性變換可實現(xiàn)坐標系的轉(zhuǎn)換，旋轉(zhuǎn)矩陣就是在線性變化中產(chǎn)生的，用REB表示慣性坐標系{E}到機體坐標系{B}的變換。

例如，繞OBXB旋轉(zhuǎn)必角，此時兩個坐標系存在必的角度差，不再重合。點(x, y, z)的轉(zhuǎn)換方程為:

可提取轉(zhuǎn)換矩陣：

同理，繞口OBYB旋轉(zhuǎn)θ角得:

而繞OBZB旋轉(zhuǎn)ψ角得:

不同旋轉(zhuǎn)順序有不同的旋轉(zhuǎn)矩陣，按照偏航，俯仰，橫滾的順序，即分別繞X-Y-Z旋轉(zhuǎn)，就可計算出旋轉(zhuǎn)矩陣REB，REB等于依次旋轉(zhuǎn)所得的矩陣連乘，且順序為從右向左排列。

萬向節(jié)死鎖

當俯仰角θ=±Π/2時，橫滾運動與偏航運動的旋轉(zhuǎn)軸重合，出現(xiàn)萬向節(jié)死鎖現(xiàn)象，在空間失去了一個自由度。如式所示，φ或ψ的變化具有相同的效果，因此不再具有唯一性啊。

四元數(shù)法

本文選擇的是四元數(shù)法進行姿態(tài)解算。無人機姿態(tài)解算方法主要有四種，它們各自的優(yōu)缺點如下圖所示。歐拉角法不能用于計算飛行器的全姿態(tài)角，且難以實時計算而不易于工程應用。方向余弦法不會出現(xiàn)“奇點”現(xiàn)象，但計算量大，效率低。四元數(shù)法避免了復雜的三角函數(shù)運算，變?yōu)榍蠼饩€性微分方程，算法簡單易操作，且不存在角度奇異性問題，可以更好的線性化系統(tǒng)，是一種更實用的工程方法。

四元數(shù)的概念誕生在1843年的愛爾蘭，是數(shù)學家哈密頓研究空間幾何時提出。在如今的導航技術領域，四元數(shù)的優(yōu)勢逐漸被發(fā)現(xiàn)，得到了研究者們的廣泛關注，并逐漸應用在姿態(tài)解算領域。

四元數(shù)是由四個元構成的數(shù)Q（q0,q1,q2,q3） = q0 + q1i + q2j + q3k;其中，q0,q1,q2,q3是實數(shù)，i,j,k既是互相正交的單位向量，又是虛單位根號-1。四元數(shù)即可看作四維空間中的一個向量，又可以看做一個超復數(shù)。對于后續(xù)有一個重要的變化需要記?。?/p>

Q=q0 + q1i + q2j + q3k

可視為一個超復數(shù)，Q 的共軛復數(shù)記為

Q'=q0 - q1i - q2j - q3k

Q°稱為Q的共軛四元數(shù)。

同時，有

ij=k，jk=i，ki=j，ji=-k，kj=-i，ik=-j

i2 = j2 = k2 =ijk=-1

其中，i、j、k是相互正交的單位向量，其幾何意義可理解為分別繞三個坐標軸的旋轉(zhuǎn)，q0、q1、q2、q3為常數(shù)，有

通過四元數(shù)進行歐拉角求解，可以減少芯片運算負擔，提高運算速度。

一個矢量V相對于坐標系OXYZ固定：V = xi + yj + zk；坐標系OXYZ轉(zhuǎn)動了Q得到一個新坐標系OX’Y’Z’:V = x’i’ + y’j‘ + z’k’；設四元數(shù)Ve、Ve‘

Ve = xi + yj + zk;

Ve’ = x’i + y’j + z’k;

則Ve’ = Q* Ve * Q'；

設Q = q0 + q1i + q2j + q3k;則Q' = q0 - q1i - q2j - q3k;

則Ve’ = Q* Ve * Q'=(q0 + q1i + q2j + q3k) * (0+xi + yj + zk) + (q0 - q1i - q2j - q3k)

可以算出

x’=(q0 ^2+q1 ^2-q2 ^2-q3 ^2)x+2(q1q2+ q1q3)y+2(q1q3-q0q2)z

y’ = 2(q1q2-q0q3)x+(q0 ^2-q1 ^2+q2 ^2-q3 ^2)y+2(q2q3+q0q1)z

z’ = 2(q1q3+q0q2)x+2(q2q3-q0q1)y+(q0 ^2-q1 ^2-q2 ^2+q3 ^2)z

結合 可以反推

Pitch  = asin(2 * q2 * q3 + 2 * q0* q1)* 57.3; // pitch ,轉(zhuǎn)換為度數(shù)
        Roll = atan2(-2 * q1 * q3 + 2 * q0 * q2, q0*q0-q1*q1-q2*q2+q3*q3)* 57.3; // rollv
        Yaw = atan2(2*(q1*q2 - q0*q3),q0*q0-q1*q1+q2*q2-q3*q3) * 57.3;   //偏移太大，

將加速度的三維向量轉(zhuǎn)為單位向量

// 測量正常化
        norm = sqrt(ax*ax + ay*ay + az*az);      
        ax = ax / norm;                   //單位化
        ay = ay / norm;
        az = az / norm;

世界坐標系重力分向量是通過方向旋轉(zhuǎn)矩陣的最后一列的三個元素乘上加速度就可以算出機體坐標系中的重力向量。

// 估計方向的重力
        vx = 2*(q1*q3 - q0*q2);//由下向上方向的加速度在加速度計X分量 
        vy = 2*(q0*q1 + q2*q3);//由下向上方向的加速度在加速度計X分量 
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;//由下向上方向的加速度在加速度計Z分量

將磁力計的三維向量轉(zhuǎn)為單位向量。

norm = sqrt(mx*mx + my*my + mz*mz);         
        mx = mx / norm;
        my = my / norm;
        mz = mz / norm;

計算地磁計在理論地磁坐標系下的機體上三個軸的分量：

● hx, hy, hz 分別表示地磁計測量值在機體坐標系下的x、y、z軸分量。

● 這一步通過方向余弦矩陣（DCM）將地磁計測量值從地磁坐標系轉(zhuǎn)換到機體坐標系。

// 這里計算得到的是地磁計在理論地磁坐標系下的機體上三個軸的分量
        hx = 2*mx*(0.5 - q2*q2 - q3*q3) + 2*my*(q1*q2 - q0*q3) + 2*mz*(q1*q3 + q0*q2);
        hy = 2*mx*(q1*q2 + q0*q3) + 2*my*(0.5 - q1*q1 - q3*q3) + 2*mz*(q2*q3 - q0*q1);
        hz = 2*mx*(q1*q3 - q0*q2) + 2*my*(q2*q3 + q0*q1) + 2*mz*(0.5 - q1*q1 - q2*q2);

計算磁北方向與當前航向角的夾角：

● bx 表示地磁計測量值與磁北方向的夾角，也即當前航向角。

● bx 的計算使用了地磁計在機體坐標系下的x和z軸分量，通過計算角度來表示航向角。

//bx計算的是當前航向角和磁北的夾角，也就是北天東坐標下的航向角
                //當羅盤水平旋轉(zhuǎn)的時候，航向角在0-360之間變化
                bx = sqrt((hx*hx) + (hy*hy));
        bz = hz;

將地磁計在機體坐標系下的磁向量轉(zhuǎn)換到機體坐標系：

● 類似之前的步驟，使用方向余弦矩陣（DCM）將地磁計測量值從機體坐標系轉(zhuǎn)換到地磁坐標系。

● 得到角速度分量 wx, wy, wz，這些值在姿態(tài)融合中用于更新四元數(shù)。

//地磁計在n系下磁向量轉(zhuǎn)換到b系下，反向使用DCM得到
        wx = 2*bx*(0.5 - q2*q2 - q3*q3) + 2*bz*(q1*q3 - q0*q2);
        wy = 2*bx*(q1*q2 - q0*q3) + 2*bz*(q0*q1 + q2*q3);
        wz = 2*bx*(q0*q2 + q1*q3) + 2*bz*(0.5 - q1*q1 - q2*q2);

這些步驟的目的是將地磁計測量值轉(zhuǎn)換到機體坐標系中，并結合加速度計的測量值估計機體坐標系中的方向和重力向量。這樣的處理是為了在姿態(tài)融合算法中使用這些數(shù)據(jù)，以更準確地估計設備的方向。請確保在使用這段代碼時，四元數(shù)和傳感器數(shù)據(jù)的值都已經(jīng)被正確更新。

姿態(tài)解算

雙環(huán)PI控制器

陀螺儀能夠迅速響應設備的旋轉(zhuǎn)，在短時間內(nèi)誤差較小且可靠。然而，因為溫度漂移、零漂移和積分誤差會隨時間累積，陀螺儀的長時間精度受到影響。在靜止狀態(tài)下，加速度計的漂移很小，其傾角求解過程中不存在積分誤差，但在飛行過程中，加速度計受到發(fā)動機和機架振動以及轉(zhuǎn)動和運動加速度的干擾。磁羅盤測量的地磁向量在特定地理范圍內(nèi)可視為不變，但磁羅盤易受硬磁場和軟磁場干擾。

因此，若系統(tǒng)外環(huán)采用九軸姿態(tài)傳感器（包括三軸加速度計、三軸磁羅盤和三軸陀螺儀）進行數(shù)據(jù)融合，磁羅盤易受干擾可能導致融合后的數(shù)據(jù)仍有較大誤差。為此，在內(nèi)環(huán)使用六軸姿態(tài)傳感器（包括三軸加速度計和三軸陀螺儀）進行數(shù)據(jù)融合，對融合后的傳感器姿態(tài)偏差進行二次修正，以提高整體精度。

外環(huán)九軸姿態(tài)傳感器數(shù)據(jù)融合，記在飛行器機體坐標系下an=[ax ay az]T和mn=[mx my mz]T分別為加速度計和磁羅盤實際測量得到的重力向量和地磁向量。

記vn=[vx vy vz]T和wn=[mx my mz]T是將地理坐標系下重力向量kb=[0 0 1g]T和地磁向量nb=[nx 0 nz]T(不考慮地理磁偏角因素，將機頭固定向北)通過四元數(shù)坐標換算成機體坐標系下的重力向量和地磁向量。向量之間的誤差為坐標軸的旋轉(zhuǎn)誤差，可以用向量的叉積en=[ex ey ez]T表示，如下所示。

由于我的LSM6DS3TR-C為六軸，不帶三軸陀螺儀，故代碼如下。

//這個叉積向量仍舊是位于機體坐標系上的，而陀螺積分誤差也是在機體坐標系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。
//（你可以自己拿東西想象一下）由于陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現(xiàn)在對機體坐標系的糾正。

        ex = (ay*vz - az*vy);
        ey = (az*vx - ax*vz);
        ez = (ax*vy - ay*vx);

添加磁力計之后代碼如下所示。

//這個叉積向量仍舊是位于機體坐標系上的，而陀螺積分誤差也是在機體坐標系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。
//（你可以自己拿東西想象一下）由于陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現(xiàn)在對機體坐標系的糾正。

//                ex = (ay*vz - az*vy);
//        ey = (az*vx - ax*vz);
//        ez = (ax*vy - ay*vx);
        ex = (ay*vz - az*vy) + (my*wz - mz*wy);
        ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
        ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

由于陀螺儀是對機體直接積分，所以，陀螺儀的誤差可以體現(xiàn)為機體坐標的誤差。因此修正坐標軸的誤差可以達到修正陀螺儀誤差的目的，從而將加速度計和磁羅盤進行修正陀螺儀，實現(xiàn)了九軸的數(shù)據(jù)融合。即如果陀螺儀按照叉積誤差的軸，轉(zhuǎn)動叉積誤差的角度，就可以消除機體坐標上實際測量的重力向量和地磁向量和坐標換算后的重力向量和地磁向量之間的誤差。

PI調(diào)節(jié)器的比例部分用于迅速糾正陀螺儀誤差，積分部分用于消除穩(wěn)態(tài)偏差。PI調(diào)節(jié)器的比例系數(shù)和積分系數(shù)自己去修正。陀螺儀經(jīng)過外環(huán)PI控制器修正姿態(tài)誤差后輸出值為了gn =[gx gy gz]T

// 積分誤差比例積分增益,計算陀螺儀測量的重力向量與估計方向的重力向量之間的誤差。
//        exInt = exInt + ex*Ki;
//        eyInt = eyInt + ey*Ki;
//        ezInt = ezInt + ez*Ki;
    /* PI */
    if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
    {                
        exInt = exInt + ex*Ki*halfT;
        eyInt = eyInt + ey*Ki*halfT;
        ezInt = ezInt + ez*Ki*halfT;                

        // 調(diào)整后的陀螺儀測量,使用叉積誤差來進行比例-積分（PI）修正陀螺儀的零偏。將修正量乘以比例增益Kp，并加上之前計算的積分誤差exInt、eyInt和ezInt。
//        gx = gx + Kp*ex + exInt;
//        gy = gy + Kp*ey + eyInt;
//        gz = gz + Kp*ez + ezInt;

        gx = gx + Kp*ex + exInt;
        gy = gy + Kp*ey + eyInt;
        gz = gz + Kp*ez + ezInt; 
 }

內(nèi)環(huán)的六軸姿態(tài)傳感器數(shù)據(jù)融合是將地理坐標系下的重力場向量與加速度計在機體坐標系下采集到的重力向量進行叉乘，求出兩者向量誤差。并通過PI控制器修正向量誤差，從而達到修正外環(huán)九軸數(shù)據(jù)融合后的陀螺儀的偏差的目的。在每個姿態(tài)解算周期讀取出機體坐標系下雙環(huán)PI控制后的陀螺儀的角速率

整合四元數(shù)率和正?；?根據(jù)陀螺儀的測量值和比例-積分修正值，對四元數(shù)進行更新。

// 整合四元數(shù)率和正?；?根據(jù)陀螺儀的測量值和比例-積分修正值，對四元數(shù)進行更新。根據(jù)微分方程的離散化形式，將四元數(shù)的每個分量加上相應的微分項乘以采樣周期的一半（halfT）。
        q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
        q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
        q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
        q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  

        // 正?；脑獢?shù)
        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 / norm;
        q1 = q1 / norm;
        q2 = q2 / norm;
        q3 = q3 / norm;

偏航角

六軸傳感器（包括三軸加速度計和三軸陀螺儀）可以用于估算設備在空間中的姿態(tài)，包括俯仰角（Pitch）、橫滾角（Roll）和偏航角（Yaw）。然而，六軸傳感器僅依賴陀螺儀和加速度計數(shù)據(jù)，可能無法準確測量偏航角（Yaw），原因如下：

無磁場參考：六軸傳感器缺少磁羅盤，沒有固定的參考方向。因此，在長時間內(nèi)，陀螺儀的積分誤差可能導致偏航角估計漂移。 陀螺儀誤差累積：陀螺儀測量的是角速度，要得到偏航角，需要將角速度積分。由于陀螺儀存在零漂、噪聲和溫度漂移等誤差，這些誤差在積分過程中會累積，使得偏航角估計產(chǎn)生較大的漂移。

雖然六軸傳感器可能無法準確測量偏航角，但可以通過將其與磁羅盤（三軸磁場傳感器）結合，形成九軸傳感器（包括三軸加速度計、三軸磁羅盤和三軸陀螺儀），以提高偏航角估計的準確性。九軸傳感器融合了磁場信息，為偏航角提供了一個穩(wěn)定的參考方向，有助于減小陀螺儀誤差對偏航角估計的影響。

陀螺儀解析代碼

//加速度單位g，陀螺儀rad/s
void IMUupdate1(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{

        float norm;
        float vx, vy, vz;
        float ex, ey, ez;  

        float hx, hy, hz, bx, bz;    
          float wx, wy, wz;
                float  halfT;
                now_update  = HAL_GetTick(); //ms
                halfT       = ((float)(now_update - last_update) / 2000.0f);
                last_update = now_update;    
        // 測量正?；?把加計的三維向量轉(zhuǎn)成單位向量。
        norm = sqrt(ax*ax + ay*ay + az*az);      
        ax = ax / norm;                   //單位化
        ay = ay / norm;
        az = az / norm; 

        norm = sqrt(mx*mx + my*my + mz*mz);         
        mx = mx / norm;
        my = my / norm;
        mz = mz / norm; 
        // 這里計算得到的是地磁計在理論地磁坐標系下的機體上三個軸的分量
        hx = 2*mx*(0.5 - q2*q2 - q3*q3) + 2*my*(q1*q2 - q0*q3) + 2*mz*(q1*q3 + q0*q2);
        hy = 2*mx*(q1*q2 + q0*q3) + 2*my*(0.5 - q1*q1 - q3*q3) + 2*mz*(q2*q3 - q0*q1);
        hz = 2*mx*(q1*q3 - q0*q2) + 2*my*(q2*q3 + q0*q1) + 2*mz*(0.5 - q1*q1 - q2*q2);         


                //bx計算的是當前航向角和磁北的夾角，也就是北天東坐標下的航向角
                //當羅盤水平旋轉(zhuǎn)的時候，航向角在0-360之間變化
                bx = sqrt((hx*hx) + (hy*hy));
        bz = hz;   

        //地磁計在n系下磁向量轉(zhuǎn)換到b系下，反向使用DCM得到
        wx = 2*bx*(0.5 - q2*q2 - q3*q3) + 2*bz*(q1*q3 - q0*q2);
        wy = 2*bx*(q1*q2 - q0*q3) + 2*bz*(q0*q1 + q2*q3);
        wz = 2*bx*(q0*q2 + q1*q3) + 2*bz*(0.5 - q1*q1 - q2*q2);      

        // 估計方向的重力,世界坐標系重力分向量是通過方向旋轉(zhuǎn)矩陣的最后一列的三個元素乘上加速度就可以算出機體坐標系中的重力向量。
        vx = 2*(q1*q3 - q0*q2);//由下向上方向的加速度在加速度計X分量 
        vy = 2*(q0*q1 + q2*q3);//由下向上方向的加速度在加速度計X分量 
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;//由下向上方向的加速度在加速度計Z分量


//這個叉積向量仍舊是位于機體坐標系上的，而陀螺積分誤差也是在機體坐標系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。
//（你可以自己拿東西想象一下）由于陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現(xiàn)在對機體坐標系的糾正。

//                ex = (ay*vz - az*vy);
//        ey = (az*vx - ax*vz);
//        ez = (ax*vy - ay*vx);
        ex = (ay*vz - az*vy) + (my*wz - mz*wy);
        ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
        ez = (ax*vy - ay*vx) + (mx*wy - my*wx);




        // 積分誤差比例積分增益,計算陀螺儀測量的重力向量與估計方向的重力向量之間的誤差。
//        exInt = exInt + ex*Ki;
//        eyInt = eyInt + ey*Ki;
//        ezInt = ezInt + ez*Ki;
    /* PI */
    if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
    {                
        exInt = exInt + ex*Ki*halfT;
        eyInt = eyInt + ey*Ki*halfT;
        ezInt = ezInt + ez*Ki*halfT;                

        // 調(diào)整后的陀螺儀測量,使用叉積誤差來進行比例-積分（PI）修正陀螺儀的零偏。將修正量乘以比例增益Kp，并加上之前計算的積分誤差exInt、eyInt和ezInt。
//        gx = gx + Kp*ex + exInt;
//        gy = gy + Kp*ey + eyInt;
//        gz = gz + Kp*ez + ezInt;

        gx = gx + Kp*ex + exInt;
        gy = gy + Kp*ey + eyInt;
        gz = gz + Kp*ez + ezInt; 
 }
        // 整合四元數(shù)率和正?；?根據(jù)陀螺儀的測量值和比例-積分修正值，對四元數(shù)進行更新。根據(jù)微分方程的離散化形式，將四元數(shù)的每個分量加上相應的微分項乘以采樣周期的一半（halfT）。
        q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
        q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
        q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
        q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  

        // 正?；脑獢?shù)
        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 / norm;
        q1 = q1 / norm;
        q2 = q2 / norm;
        q3 = q3 / norm;

//        Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,轉(zhuǎn)換為度數(shù)
//        Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv
//        Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;   //偏移太大，等我找一個好用的

        Pitch  = asin(2 * q2 * q3 + 2 * q0* q1)* 57.3; // pitch ,轉(zhuǎn)換為度數(shù)
        Roll = atan2(-2 * q1 * q3 + 2 * q0 * q2, q0*q0-q1*q1-q2*q2+q3*q3)* 57.3; // rollv
        Yaw = atan2(2*(q1*q2 - q0*q3),q0*q0-q1*q1+q2*q2-q3*q3) * 57.3;   //偏移太大，等我找一個好用的



}

上報匿名助手能正常進行解析。

審核編輯：湯梓紅