轉(zhuǎn)載請(qǐng)注明出處：http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/
文章鏈接：http://blog.csdn.net/hongbin_xu/article/details/60964592 或 http://hongbin96.com/122

在前面的博文中介紹了使用互補(bǔ)濾波法進(jìn)行姿態(tài)解算的原理以及程序，點(diǎn)擊打開鏈接 。
在互補(bǔ)濾波法中，我們使用加速度計(jì)來(lái)補(bǔ)償陀螺儀，將兩者的數(shù)據(jù)融合解算出四元數(shù)下的姿態(tài)。
在梯度下降法中，依然是通過(guò)加速度計(jì)的數(shù)據(jù)來(lái)與陀螺儀的數(shù)據(jù)進(jìn)行融合，而不同點(diǎn)是：互補(bǔ)濾波法中，先求出載體坐標(biāo)系b下對(duì)加速度做叉積求出誤差，補(bǔ)償給陀螺儀以校正誤差；梯度下降法中，通過(guò)加速度計(jì)的數(shù)據(jù)求出一組四元數(shù) ，然后通過(guò)四元數(shù)的微分方程，并使用陀螺儀得到的數(shù)據(jù)求解出另外一組四元數(shù) 。因?yàn)樵诟咚龠\(yùn)動(dòng)狀態(tài)下，陀螺儀數(shù)據(jù)更可靠；而在低速運(yùn)動(dòng)狀態(tài)下，加速度計(jì)數(shù)據(jù)更可靠。所以兩組四元數(shù)分別乘以權(quán)重再相加，就得到了期望的輸出結(jié)果。

大概地介紹了下之后，再講講梯度：
在向量微積分中，標(biāo)量場(chǎng)的梯度是一個(gè)向量場(chǎng)。標(biāo)量場(chǎng)中某一點(diǎn)上的梯度指向標(biāo)量場(chǎng)增長(zhǎng)最快的方向，梯度的長(zhǎng)度是這個(gè)最大的變化率。
在單變量的實(shí)值函數(shù)的情況，梯度只是導(dǎo)數(shù)，或者，對(duì)于一個(gè)線性函數(shù)，也就是線的斜率。（摘自百度百科：點(diǎn)擊打開鏈接）
我的理解是：一個(gè)多元函數(shù)在某點(diǎn)上的梯度就是在該點(diǎn)上最大的變化率，其方向就是函數(shù)值增大的最陡的方向。
舉個(gè)例子來(lái)看看：（圖畫的比較難看，請(qǐng)見(jiàn)諒）

如上圖所示，紅線表示的是一個(gè)標(biāo)量函數(shù)，黑線是圖中標(biāo)注點(diǎn)上關(guān)于該函數(shù)的切線。很容易得到，在該點(diǎn)上的梯度與切線方向相同。
如果我們想要求得函數(shù)的極小值，首先選取函數(shù)曲線上的任意一個(gè)點(diǎn)，那么，沿著梯度的反方向走一個(gè)比較小的步長(zhǎng) ，得到一個(gè)新的點(diǎn)。再在這個(gè)新的點(diǎn)上重復(fù)之前的步驟，最后求解出極小值。
接著給出梯度下降法的迭代公式：

式中： ， 表示運(yùn)算前后自變量x的值； 表示梯度下降的步長(zhǎng)； 表示函數(shù)在自變量x等于 時(shí)，函數(shù)的梯度；負(fù)號(hào)表示沿著梯度的負(fù)方向逼近極點(diǎn)。

梯度下降法就是，在某一點(diǎn)增加最快的方向的反方向（下降最快）一直走，當(dāng)走到某一個(gè)極值點(diǎn)（極小值）時(shí)，這個(gè)點(diǎn)就是最優(yōu)解。

接下來(lái)開始使用梯度下降法求解姿態(tài)的算法介紹：
假設(shè)一個(gè)誤差函數(shù) ，我們希望其滿足： ，即誤差為0。接著，我們要求解這個(gè)方程，得到x的值。此時(shí)x值對(duì)應(yīng)最優(yōu)解。
使用梯度下降法求解，首先我們就需要先求出它的導(dǎo)數(shù) 。
將x換成四元數(shù)。函數(shù)也相應(yīng)變化，其導(dǎo)數(shù)變?yōu)槿缦滦问?#xff1a;

因?yàn)槲覀兦蠼獾淖藨B(tài)是三維姿態(tài)，而上式的因變量只對(duì)應(yīng)了一維的情況。所以將因變量推廣到xyz軸下，變成一個(gè)多元向量函數(shù)：

求它的導(dǎo)數(shù)：

（也叫作雅克比矩陣）

準(zhǔn)備工作完成了，接下來(lái)結(jié)合算法流程圖來(lái)推導(dǎo)算法：

取定導(dǎo)航坐標(biāo)系n中標(biāo)準(zhǔn)重力加速度g，定義為 ，從導(dǎo)航坐標(biāo)系n轉(zhuǎn)換到載體坐標(biāo)系b下，乘以旋轉(zhuǎn)矩陣 。
這里直接給出旋轉(zhuǎn)矩陣 ，推導(dǎo)步驟在前面的博文中，不再贅述。

上式代入，推導(dǎo)出：

將 與歸一化之后的加速度計(jì)數(shù)據(jù) 作差。
得到誤差函數(shù)：

當(dāng)這個(gè)誤差函數(shù)為0（即最小值）時(shí)，我們認(rèn)為旋轉(zhuǎn)矩陣沒(méi)有誤差，姿態(tài)是精確的。
那么，接下來(lái)的任務(wù)就是使用前面介紹的梯度下降法相關(guān)公式來(lái)求解了。

用前面闡述的方法求這個(gè)多元向量函數(shù)的導(dǎo)數(shù)（即它的雅克比矩陣）：

代入公式求出梯度：

由于線性代數(shù)中的矩陣乘法法則知：兩個(gè)矩陣相乘行數(shù)與列數(shù)應(yīng)當(dāng)相同。所以將雅克比矩陣進(jìn)行轉(zhuǎn)置再與原函數(shù)相乘，得到梯度。
將得到的梯度值再套入公式 ，累加，最后會(huì)趨近于最小值0，而收斂速度取決于步長(zhǎng) ：

通過(guò)離散疊加替代積分可以求解出一組四元數(shù) ，將其乘以權(quán)重與陀螺儀數(shù)據(jù)通過(guò)四元數(shù)微分方程求解得到的另一組四元數(shù) （這部分在前面的其他博文中已經(jīng)推導(dǎo)過(guò)）。（注：這一部分只是簡(jiǎn)單處理了一下，與框圖中的不同）
最終的公式：

為最終輸出的四元數(shù)；
為權(quán)重；
為加速度計(jì)的數(shù)據(jù)通過(guò)梯度下降法得到的四元數(shù)；
為陀螺儀的數(shù)據(jù)通過(guò)四元數(shù)微分方程求解得到的四元數(shù)。

就介紹到這了，后面這段Madgwick的代碼網(wǎng)上也有很多地方能下載到。其中用到的理論和公式在前面的介紹中也大致過(guò)了一遍，主要使用的也就是這個(gè)公式：

步長(zhǎng) 和權(quán)重在程序中僅僅是簡(jiǎn)單處理了一下，直接賦予了一個(gè)常量。
然而實(shí)際中權(quán)重與運(yùn)動(dòng)速度有關(guān)：

高速運(yùn)動(dòng)下，相信陀螺儀更多一些，所以權(quán)重要小一點(diǎn)；
低速運(yùn)動(dòng)下，相信加速度計(jì)更多一些，所以權(quán)重要大一點(diǎn)。
若要取得最佳的權(quán)重，應(yīng)當(dāng)是 與的收斂速度相等時(shí)的 值。

步長(zhǎng) 也與物體運(yùn)動(dòng)的角速度、采樣時(shí)間等相關(guān)。

關(guān)于如何取值，取多大合適，這個(gè)我也不清楚。

好了，不說(shuō)廢話，上代碼：

//===================================================================================================== // MadgwickAHRS.c //===================================================================================================== // // Implementation of Madgwick's IMU and AHRS algorithms. // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms // // Date Author Notes // 29/09/2011 SOH Madgwick Initial release // 02/10/2011 SOH Madgwick Optimised for reduced CPU load // 19/02/2012 SOH Madgwick Magnetometer measurement is normalised // //=====================================================================================================//--------------------------------------------------------------------------------------------------- // Header files#include "MadgwickAHRS.h" #include <math.h>//--------------------------------------------------------------------------------------------------- // Definitions#define sampleFreq 512.0f // sample frequency in Hz #define betaDef 0.1f // 2 * proportional gain//--------------------------------------------------------------------------------------------------- // Variable definitionsvolatile float beta = betaDef; // 2 * proportional gain (Kp) volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame//--------------------------------------------------------------------------------------------------- // Function declarationsfloat invSqrt(float x); //--------------------------------------------------------------------------------------------------- // IMU algorithm updatevoid MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {float recipNorm;float s0, s1, s2, s3;float qDot1, qDot2, qDot3, qDot4;float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;// Rate of change of quaternion from gyroscopeqDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {// Normalise accelerometer measurementrecipNorm = invSqrt(ax * ax + ay * ay + az * az);ax *= recipNorm;ay *= recipNorm;az *= recipNorm; // Auxiliary variables to avoid repeated arithmetic_2q0 = 2.0f * q0;_2q1 = 2.0f * q1;_2q2 = 2.0f * q2;_2q3 = 2.0f * q3;_4q0 = 4.0f * q0;_4q1 = 4.0f * q1;_4q2 = 4.0f * q2;_8q1 = 8.0f * q1;_8q2 = 8.0f * q2;q0q0 = q0 * q0;q1q1 = q1 * q1;q2q2 = q2 * q2;q3q3 = q3 * q3;// Gradient decent algorithm corrective steps0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitudes0 *= recipNorm;s1 *= recipNorm;s2 *= recipNorm;s3 *= recipNorm;// Apply feedback stepqDot1 -= beta * s0;qDot2 -= beta * s1;qDot3 -= beta * s2;qDot4 -= beta * s3;}// Integrate rate of change of quaternion to yield quaternionq0 += qDot1 * (1.0f / sampleFreq);q1 += qDot2 * (1.0f / sampleFreq);q2 += qDot3 * (1.0f / sampleFreq);q3 += qDot4 * (1.0f / sampleFreq);// Normalise quaternionrecipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);q0 *= recipNorm;q1 *= recipNorm;q2 *= recipNorm;q3 *= recipNorm; }//--------------------------------------------------------------------------------------------------- // Fast inverse square-root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_rootfloat invSqrt(float x) {float halfx = 0.5f * x;float y = x;long i = *(long*)&y;i = 0x5f3759df - (i>>1);y = *(float*)&i;y = y * (1.5f - (halfx * y * y));return y; }

以上僅是個(gè)人見(jiàn)解，希望能對(duì)更多人的學(xué)習(xí)提供幫助。
╮(～▽～)╭

我的其他博文的鏈接：
四旋翼姿態(tài)解算——基礎(chǔ)理論及推導(dǎo) ：
http://blog.csdn.net/hongbin_xu/article/details/55667899
四旋翼姿態(tài)解算——互補(bǔ)濾波算法及理論推導(dǎo)：
http://blog.csdn.net/hongbin_xu/article/details/56846490
四旋翼姿態(tài)解算——互補(bǔ)濾波法補(bǔ)充（融合磁力計(jì)）：
http://blog.csdn.net/hongbin_xu/article/details/59110226

總結(jié)

以上是生活随笔為你收集整理的四旋翼姿态解算——梯度下降法理论推导的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

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