姿態(tài)解算篇B

前言

本篇博文主要是以mahony的算法為基礎(chǔ)理解姿態(tài)解算的過程，主要參考的論文就是William Premerlani and Paul Bizard的關(guān)于DCM的一篇經(jīng)典論文《Direction Cosine Matrix IMU_Theory》，一定要搞透這篇論文，沒看過它都不敢稱自己研究過飛控算法；然后接下來還有madgwick和mahony的論文需要研究。
?
然后就是結(jié)合國內(nèi)的一本很不錯的書籍《捷聯(lián)慣性導(dǎo)航技術(shù)》，不需要細(xì)看，只需要了解其中關(guān)于姿態(tài)解算部分的即可。

基本知識

陀螺儀和加速計（特性分析）

1）陀螺儀

Gyro sensitivity、 operating range、offset、drift、calibrationandsaturation must be taken into account in the implementation of DCM。

靈敏度

測量角速度，具有高動態(tài)特性，它是一個間接測量角度的器件其中一個關(guān)鍵參數(shù)就是gyro sensitivity（其單位是millivolts per degree persecond，把轉(zhuǎn)速轉(zhuǎn)換到電壓值），測量范圍越小氣靈敏度越好。也就是說測量的是角度的導(dǎo)數(shù)，即角速度，要將角速度對時間積分才能得到角度。陀螺儀就是內(nèi)部有一個陀螺，它的軸由于陀螺效應(yīng)始終與初始方向平行，這樣就可以通過與初始方向的偏差計算出旋轉(zhuǎn)方向和角度。

偏移

偏移就是在陀螺沒有轉(zhuǎn)動的時候卻又輸出，這個輸出量的大小和供電電壓以及溫度有關(guān)，該偏移可以在陀螺儀上電時通過一小段時間的測量來修正。

漂移

它是由于在時間的積累下偏移和噪聲相互影響的結(jié)果，例如有一個偏置（offset）0.1dps加在上面，于是測量出來是0.1dps，積分一秒之后，得到的角度是0.1度，1分鐘之后是6度，還能忍受，一小時之后是360度，轉(zhuǎn)了一圈，也就是說，陀螺儀在短時間內(nèi)有很大的參考價值。

白噪聲

電信號的測量中，一定會帶有白噪聲，陀螺儀數(shù)據(jù)的測量也不例外。所以獲得的陀螺儀數(shù)據(jù)中也會帶有白噪聲，而且這種白噪聲會隨著積分而累加。

積分誤差

對陀螺儀角速度的積分是離散的，長時間的積分會出現(xiàn)漂移的情況。所以要考慮積分誤差的問題。

溢出

就是轉(zhuǎn)速超過了其測量的最大轉(zhuǎn)速范圍。關(guān)于這個問題的解決辦法，在DCM IMU:Theory中給出來三種解決辦法，不寫了。

2）加速度計

加速度計的低頻特性好，可以測量低速的靜態(tài)加速度。在無人機(jī)上就是對重力加速度g的測量和分析。當(dāng)把加速度計拿在手上隨意轉(zhuǎn)動時，看的是重力加速度在三個軸上的分量值。加速度計在自由落體時，其輸出為0。為什么會這樣呢？這里涉及到加速度計的設(shè)計原理：加速度計測量加速度是通過比力來測量(比力方程，秦永元的書中有介紹)，而不是通過加速度。加速度計僅僅測量的是重力加速度,而重力加速度與剛才所說的R坐標(biāo)系（EarthFrame）是固連的,通過這種關(guān)系,可以得到加速度計所在平面與地面的角度關(guān)系。

加速度計僅僅測量的是重力加速度，3軸加速度計輸出重力加速度在加速度計所在機(jī)體坐標(biāo)系3個軸上的分量大小。重力加速度的方向和大小是固定的。通過這種關(guān)系，可以得到加速度計所在平面與地面的角度關(guān)系。加速度計若是繞著重力加速度的軸轉(zhuǎn)動，則測量值不會改變，也就是說加速度計無法感知這種水平旋轉(zhuǎn)。

Accelerometersare used for roll-pitch drift correction because they have zero drift

有關(guān)陀螺儀和加速度計和關(guān)系，姿態(tài)解算融合的原理，再把下面這個比喻放到這里一遍。

機(jī)體好似一條船，姿態(tài)就是航向（船頭的方位），重力是燈塔，陀螺（角速度積分）是舵手，加速度計是瞭望手。舵手負(fù)責(zé)估計和把穩(wěn)航向，他相信自己，本來船向北開的，就一定會一直往北開，覺得轉(zhuǎn)了90度彎，那就會往東開。當(dāng)然如果舵手很牛逼，也許能估計很準(zhǔn)確，維持很長時間。不過只信任舵手，肯定會迷路，所以一般都有瞭望手來觀察誤差。

瞭望手根據(jù)地圖燈塔方位和船的當(dāng)前航向，算出燈塔理論上應(yīng)該在船的X方位。然而看到實際燈塔在船的Y方位，那肯定船的當(dāng)前航向有偏差了，偏差就是ERR=X-Y。舵手收到瞭望手給的ERR報告，覺得可靠，那就聽個90%ERR，覺得天氣不好、地圖誤差大，那就聽個10%ERR，根據(jù)這個來糾正估算航向。

3）磁傳感器

可以測量磁場，在沒有其他磁場的情況下，僅僅測量的是地球的磁場，而地磁也是和R坐標(biāo)系固連的，通過這種關(guān)系，可以得到平面A和地平面的關(guān)系。(平面A：和磁場方向垂直的平面)，同樣的，若是沿著磁場方向的軸旋轉(zhuǎn)，測量值不會改變，無法感知這種旋轉(zhuǎn)。

綜合考慮，加速度計和磁傳感器都是極易受外部干擾的傳感器，都只能得到2維的角度關(guān)系，但是測量值隨時間的變化相對較小，結(jié)合加速度計和磁傳感器可以得到3維的角度關(guān)系。陀螺儀可以積分得到三維的角度關(guān)系，動態(tài)性能好，受外部干擾小，但測量值隨時間變化比較大。可以看出，它們優(yōu)缺點互補(bǔ)，結(jié)合起來才能有好的效果。

4）關(guān)于數(shù)據(jù)融合

現(xiàn)在有了三個傳感器，都能在一定程度上測量角度關(guān)系，但是究竟相信誰？根據(jù)剛才的分析，應(yīng)該是在短時間內(nèi)更加相信陀螺儀，隔三差五的問問加速度計和磁傳感器，角度飄了多少了？有一點必須非常明確，陀螺儀才是主角，加速度計和磁傳感器僅僅是跑龍?zhí)椎摹Ｆ鋵嵓铀俣扔嫙o法對航向角進(jìn)行修正，修正航向角需要磁力計。

參考crazypony的分析：http://www.crazepony.com/wiki/mpu6050.html和《DCM IMU:Theory》

代碼算法分析

首先就是基于mahony算法的AHRS姿態(tài)解算的一套源碼，理論基礎(chǔ)是DCM IMU:Theory，很有參考價值。先自己分析一下，看看每一行具體是做什么的，是如何實現(xiàn)姿態(tài)解算和修正的。然后會給出相應(yīng)的分析

/* *AHRS */ // AHRS.c // Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion // compensation algorithms from my filter [Madgwick] which eliminatesthe need for a reference // direction of flux (bx bz) to be predefined and limits the effect ofmagnetic distortions to yaw // axis only. // User must define 'halfT' as the (sample period / 2), and the filtergains 'Kp' and 'Ki'. // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elementsrepresenting the estimated // orientation. See my report foran overview of the use of quaternions in this application. // User must call 'AHRSupdate()' every sample period and parsecalibrated gyroscope ('gx', 'gy', 'gz'), // accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz')data. Gyroscope units are // radians/second, accelerometer and magnetometer units are irrelevantas the vector is normalised. #include "stm32f10x.h" #include "AHRS.h" #include "Positioning.h" #include <math.h> #include <stdio.h> /* Private define------------------------------------------------------------*/ #define Kp 2.0f // proportional gain governs rate of convergence toaccelerometer/magnetometer #define Ki 0.005f // integral gain governs rate of convergenceof gyroscope biases #define halfT 0.0025f // half the sample period #define ACCEL_1G 1000 //theacceleration of gravity is: 1000 mg /* Private variables---------------------------------------------------------*/ static float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing theestimated orientation static float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error /* Public variables----------------------------------------------------------*/ EulerAngle_Type EulerAngle; //unit: radian u8InitEulerAngle_Finished = 0; float Magnetoresistor_mGauss_X = 0, Magnetoresistor_mGauss_Y = 0,Magnetoresistor_mGauss_Z = 0;//unit: milli-Gauss float Accelerate_mg_X, Accelerate_mg_Y, Accelerate_mg_Z;//unit: mg float AngularRate_dps_X, AngularRate_dps_Y, AngularRate_dps_Z;//unit:dps: degree per second int16_t Magnetoresistor_X, Magnetoresistor_Y, Magnetoresistor_Z; uint16_t Accelerate_X = 0, Accelerate_Y = 0, Accelerate_Z = 0; uint16_t AngularRate_X = 0, AngularRate_Y = 0, AngularRate_Z = 0; u8 Quaternion_Calibration_ok = 0; /* Private macro-------------------------------------------------------------*/ /* Private typedef-----------------------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/ /****************************************************************************** *Function Name : AHRSupdate *Description : None *Input : None *Output : None *Return : None ******************************************************************************* void AHRSupdate(float gx, float gy, float gz, float ax, float ay, floataz, float mx, float my, float mz) { float norm; float hx, hy, hz, bx, bz; float vx, vy, vz, wx, wy, wz; float ex, ey, ez; // auxiliary variables to reduce number of repeated operations float q0q0 = q0*q0; float q0q1 = q0*q1; float q0q2 = q0*q2; float q0q3 = q0*q3; float q1q1 = q1*q1; float q1q2 = q1*q2; float q1q3 = q1*q3; float q2q2 = q2*q2; float q2q3 = q2*q3; float q3q3 = q3*q3; // normalise the measurements 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; // compute reference direction of magnetic field hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2); hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1); hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 -q2q2); bx = sqrt((hx*hx) + (hy*hy)); bz = hz; // estimated direction of gravity and magnetic field (v and w) vx = 2*(q1q3 - q0q2); vy = 2*(q0q1 + q2q3); vz = q0q0 - q1q1 - q2q2 + q3q3; wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2); wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3); wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2); // error is sum ofcross product between reference direction of fields and directionmeasured by sensors 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); // integral error scaled integral gain exInt = exInt + ex*Ki* (1.0f / sampleFreq); eyInt = eyInt + ey*Ki* (1.0f / sampleFreq); ezInt = ezInt + ez*Ki* (1.0f / sampleFreq); // adjusted gyroscope measurements gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; // integrate quaternion rate and normalize 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; // normalise quaternion norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; }

上述算法的源碼分析先給一個簡要的代碼注釋分析：

//陀螺儀、加速度計、磁力計數(shù)據(jù)融合 void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { float norm; float hx, hy, hz, bx, bz; float vx, vy, vz, wx, wy, wz; //v*當(dāng)前姿態(tài)計算得來的重力在三軸上的分量 float ex, ey, ez; // auxiliary variables to reduce number of repeated operations float q0q0 = q0*q0; float q0q1 = q0*q1; float q0q2 = q0*q2; float q0q3 = q0*q3; float q1q1 = q1*q1; float q1q2 = q1*q2; float q1q3 = q1*q3; float q2q2 = q2*q2; float q2q3 = q2*q3; float q3q3 = q3*q3; // normalise the measurements 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; // compute reference direction of magnetic field hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2); hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1); hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2); bx = sqrt((hx*hx) + (hy*hy)); bz = hz; // estimated direction of gravity and magnetic field (v and w) //參考坐標(biāo)n系轉(zhuǎn)化到載體坐標(biāo)b系的用四元數(shù)表示的方向余弦矩陣第三列即是。 //處理后的重力分量 vx = 2*(q1q3 - q0q2); vy = 2*(q0q1 + q2q3); vz = q0q0 - q1q1 - q2q2 + q3q3; //處理后的mag，反向使用DCM得到 wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2); wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3); wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2); // error is sum of cross product between reference direction of fields and direction measured by sensors 體現(xiàn)在加速計補(bǔ)償和磁力計補(bǔ)償，因為僅僅依靠加速計補(bǔ)償沒法修正Z軸的變差，所以還需要通過磁力計來修正Z軸。（公式28）。《四元數(shù)解算姿態(tài)完全解析及資料匯總》的作者把這部分理解錯了，不是什么叉積的差，而叉積的計算就是這樣的。計算方法是公式10。 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); // integral error scaled integral gain exInt = exInt + ex*Ki* (1.0f / sampleFreq); eyInt = eyInt + ey*Ki* (1.0f / sampleFreq); ezInt = ezInt + ez*Ki* (1.0f / sampleFreq); // adjusted gyroscope measurements //將誤差PI后補(bǔ)償?shù)酵勇輧x，即補(bǔ)償零點漂移。通過調(diào)節(jié)Kp、Ki兩個參數(shù)，可以控制加速度計修正陀螺儀積分姿態(tài)的速度。（公式16和公式29） gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; // integrate quaternion rate and normalize //一階龍格庫塔法更新四元數(shù) 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; // normalise quaternion norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; }

3、在深入一點

1）無人機(jī)控制中，主要就是姿態(tài)解算和姿態(tài)控制部分，該部分主要介紹姿態(tài)解算，下面是來一張比較好理解的框圖。

由上面兩幅圖很容易理解整個控制和姿態(tài)解算了吧，在第四部分也介紹了關(guān)于陀螺儀、加速計、磁力計它們的作用，所以不再單獨介紹了。不理解的往上翻翻再看看吧。

2）在姿態(tài)解算過程中，到底用什么表示無人機(jī)的姿態(tài)呢？姿態(tài)表示的方法有很多種，比如歐拉角、四元數(shù)、DCM，各有的各的優(yōu)勢，比較常用的就是四元數(shù)，方便計算。上面的姿態(tài)解算算法也是基于四元數(shù)的。下面介紹一個它們?nèi)齻€的優(yōu)缺點。

姿態(tài)解算方法的比較：

算法	優(yōu)點	缺點	PS
歐拉角法（3參數(shù)）	1、? 通過歐拉角微分方程直接解算出pitch、roll、yaw 2、? 概念直觀，易于理解 3、? 解算結(jié)果永遠(yuǎn)是正交的，無需再次正交化處理	1、? 方程中有三角函數(shù)的運算，接超越函數(shù)有一定的困難 2、? 當(dāng)俯仰角接近90°時出現(xiàn)退化現(xiàn)象	1、? 適應(yīng)于水平姿態(tài)角變化不大的情況 2、? 不適用于全姿態(tài)運載體
方向余弦法（9參數(shù)）	1、? 對姿態(tài)矩陣微分方程的求解，避免了歐拉角法中出現(xiàn)的退化現(xiàn)象 2、? 可以全姿態(tài)運行	1、參數(shù)量過多，計算量大，實時性不好	很少在工程中使用
四元數(shù)法（4參數(shù)）	1、? 直接求解四元數(shù)微分方程 2、? 只需要求解四個參數(shù)，計算量小 3、算法簡單，易于操作	漂移比較嚴(yán)重	可以在實現(xiàn)過程中修正漂移，應(yīng)用比較廣泛

3）廢話不多說，進(jìn)入正題。上述算法主要就是利用加速度計和磁力計修正陀螺儀的誤差，算法是使用了PI反饋控制器實現(xiàn)反饋修正的。如下圖：

首先，東北天坐標(biāo)系是n系（地理坐標(biāo)系，參考坐標(biāo)系），載體坐標(biāo)系是b系，就是我們飛行器的坐標(biāo)系。對于四元數(shù)法的姿態(tài)解算，需要求取的就是四元數(shù)的值；方向余弦矩陣（用于表示n系和b系的相對關(guān)系）中的元素本來應(yīng)該是三角函數(shù)，這里由于使用四元數(shù)法，所以矩陣中的元素就成了四元數(shù)。所以姿態(tài)解算的任務(wù)就是求解由四元數(shù)構(gòu)成的方向余弦矩陣nCb（nCb表示從b系到n的轉(zhuǎn)換矩陣，同理，bCn表示從n系到b的轉(zhuǎn)換矩陣，它們的關(guān)系是轉(zhuǎn)置）。

顯然，上述矩陣是有誤差存在的。對于一個確定的向量n，用不同的坐標(biāo)系表示時，他們所表示的大小和方向一定是相同的。但是由于這兩個坐標(biāo)系的轉(zhuǎn)換矩陣存在誤差，那么當(dāng)一個向量經(jīng)過這么一個有誤差存在的旋轉(zhuǎn)矩陣變換后，在另一個坐標(biāo)系中肯定和理論值是有偏差的，我們通過這個偏差來修正這個旋轉(zhuǎn)矩陣。這個旋轉(zhuǎn)矩陣的元素是四元數(shù)，這就是說修正的就是四元數(shù)，于是乎姿態(tài)就這樣被修正了，這才是姿態(tài)解算的原理。姿態(tài)解算求的是四元數(shù)，是通過修正旋轉(zhuǎn)矩陣中的四元數(shù)來達(dá)到姿態(tài)解算的目的，而不要以為通過加速度計和地磁計來修正姿態(tài)，加速度計和地磁計只是測量工具和載體，通過這兩個器件表征旋轉(zhuǎn)矩陣的誤差存在，然后通過算法修正誤差，修正四元數(shù)，修正姿態(tài)。

加速度計修正pitch_roll

加速度計可以修正pitch_roll，但是我們必須要考慮離心加速度（centrifugalacceleration），離心加速度就等于旋轉(zhuǎn)率向量（即gyro vector）和速度向量的叉積（沒有原因，平均得來的并且還相當(dāng)準(zhǔn)確，速度可以用GPS獲取）。我們假設(shè)飛機(jī)方向和X軸平行。

在機(jī)體上測得的重力的為：

Pitch_roll的旋轉(zhuǎn)修正向量是由DCM的Z行與歸一化以后的重力參考向量的叉積。

在n系中，加速度計輸出為，經(jīng)過bCn（用四元數(shù)表示的轉(zhuǎn)換矩陣）轉(zhuǎn)換之后到b系中的值為；在b系中，加速度計的測量值為，現(xiàn)在和均表示在b系中的豎直向下的向量，由此，我們來做向量積（叉積），得到誤差，利用這個誤差來修正bCn矩陣，于是四元數(shù)就在這樣一個過程中被修正了。但是，由于加速度計無法感知z軸上的旋轉(zhuǎn)運動，所以還需要用地磁計來進(jìn)一步補(bǔ)償。

PS：公式不好加，只能直接切圖了~~~

加速度計在靜止時測量的是重力加速度，是有大小和方向的；同理，地磁計同樣測量的是地球磁場的大小和方向，只不過這個方向不再是豎直向下，而是與x軸（或者y軸）呈一個角度，與z軸呈一個角度。記作，假設(shè)x軸對準(zhǔn)北邊，所以by=0，即。倘若知道bx和bz的精確值，那么就可以采用和加速度計一樣的修正方法來修正。只不過在加速度計中，在n系中的參考向量是，變成了地磁計的。

如果我們知道bx和bz的精確值，那么就可以擺脫掉加速度計的補(bǔ)償，直接用地磁計和陀螺儀進(jìn)行姿態(tài)解算，但是你看過誰只用陀螺儀和地磁計進(jìn)行姿態(tài)解算嗎？沒有，因為沒人會去測量當(dāng)?shù)氐牡卮艌鱿鄬τ跂|北天坐標(biāo)的夾角，也就是bx和bz（插曲：關(guān)于這個bx和bz的理解：可以對比重力加速度的理解，就像vx vy vz似的，因為在每一處的歸一化以后的重力加速度都是0 0 1然后旋轉(zhuǎn)到機(jī)體坐標(biāo)系，而地球每一處的地磁大小都不一樣的，不能像重力加速度那樣直接旋轉(zhuǎn)得到了，只能用磁力計測量到的數(shù)據(jù)去強(qiáng)制擬合。）。

那么現(xiàn)在怎么辦？前面已經(jīng)講了，姿態(tài)解算就是求解旋轉(zhuǎn)矩陣，這個矩陣的作用就是將b系和n正確的轉(zhuǎn)化直到重合。現(xiàn)在我們假設(shè)nCb旋轉(zhuǎn)矩陣是經(jīng)過加速度計校正后的矩陣，當(dāng)某個確定的向量（b系中）經(jīng)過這個矩陣旋轉(zhuǎn)之后（到n系），這兩個坐標(biāo)系在XOY平面上重合（參考DCM IMU:Theory的Drift cancellation部分），只是在z軸旋轉(zhuǎn)上會存在一個偏航角的誤差。

下圖表示的是經(jīng)過nCb旋轉(zhuǎn)之后的b系和n系的相對關(guān)系。可以明顯發(fā)現(xiàn)加速度計可以把b系通過四元數(shù)法從任意角度拉到與n系水平的位置上，此時只剩下一個偏航角誤差。這也是為什么加速度計無法修正偏航的原因。

此方式在數(shù)學(xué)上沒有任何問題，但是由于地磁傳感器極易受到各種干擾，而此算法又將地磁傳感器所指示的方向過度的融入到了姿態(tài)當(dāng)中，導(dǎo)致實際使用中數(shù)據(jù)會非常不穩(wěn)定。怪不得crazypony的算法中沒有使用磁力計修正YAW，這應(yīng)該就是原因所在吧。

ardupilot源碼分析

就理解了一個通過gyro_vector更新DCM的算法，這個應(yīng)該是在renormalization直接就需要了解，可以前期沒理解到底是干嘛的，現(xiàn)在補(bǔ)上；關(guān)于renormalization的算法可以參考上一篇博文。

姿態(tài)解算過程中不僅需要修正陀螺儀的各種errors，還需要實時的更新的DCM。上周一直沒能理解的一個問題，在AP_AHRS_DCM.cpp中的matrix_update(delta_t)，就是實時更新DCM矩陣的，源碼如下，這一部分研究了很久，需要的基礎(chǔ)知識比較多。先上源碼

// update the DCM matrix using only the gyros Void AP_AHRS_DCM::matrix_update(float _G_Dt) { _omega.zero(); // average across first two healthy gyros. This reduces noise on // systems with more than one gyro. We don't use the 3rd gyro // unless another is unhealthy as 3rd gyro on PH2 has a lot more // noise uint8_t healthy_count = 0; Vector3f delta_angle; for (uint8_t i=0; i<_ins.get_gyro_count(); i++) { if (_ins.get_gyro_health(i) && healthy_count < 2) { Vector3f dangle; if (_ins.get_delta_angle(i, dangle)) { healthy_count++; delta_angle += dangle; } } } if (healthy_count > 1) { delta_angle /= healthy_count; } if (_G_Dt > 0) { _omega = delta_angle / _G_Dt; _omega += _omega_I; _dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _G_Dt); } }

首先就是通過陀螺儀獲取兩次gyro_vector，然后取平均以降低誤差；然后就是比較專業(yè)的算法實現(xiàn)_dcm_matrix.rotate((_omega +_omega_P + _omega_yaw_P) * _G_Dt)了。進(jìn)入到matrix3.cpp中有rotate()函數(shù)，該函數(shù)就是實現(xiàn)DCM更新的算法實現(xiàn)，算法主要是用陀螺儀的輸出值與DCM矩陣的乘積再加回到DCM中去，處理過程中使用了離散化的概念，即dcm（k+1）=dcm（k）+增量，因為公式里有求導(dǎo)，必須離散化后才能計算機(jī)處理，感謝青龍的指導(dǎo)。

// apply an additional rotation from a body frame gyro vector // to a rotation matrix. template <typename T> void Matrix3<T>::rotate(const Vector3<T> &g) { Matrix3<T> temp_matrix; temp_matrix.a.x = a.y * g.z - a.z * g.y; temp_matrix.a.y = a.z * g.x - a.x * g.z; temp_matrix.a.z = a.x * g.y - a.y * g.x; temp_matrix.b.x = b.y * g.z - b.z * g.y; temp_matrix.b.y = b.z * g.x - b.x * g.z; temp_matrix.b.z = b.x * g.y - b.y * g.x; temp_matrix.c.x = c.y * g.z - c.z * g.y; temp_matrix.c.y = c.z * g.x - c.x * g.z; temp_matrix.c.z = c.x * g.y - c.y * g.x; (*this) += temp_matrix;//增加累加到原始數(shù)據(jù)上 }

公式太多，沒辦法還是上圖吧~~~

最后來一張神人做的圖。（右鍵查看圖，放大看）

算法的理解過程比較艱難，千萬不能閉門造車，要多多交流，思想的碰撞才能產(chǎn)生出火花，多謝各位群友的無私幫助。通過上述的分析以后，對整個姿態(tài)解算過程有了整體的框架理解，接下來會結(jié)合上面算法的分析去分析ardupilot中關(guān)于姿態(tài)解算的源碼，應(yīng)該會理解的快一點了吧，希望如此

總結(jié)

以上是生活随笔為你收集整理的Pixhawk代码分析-姿态解算篇B的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。