|
感謝匿名的開(kāi)源分享，感謝群友的熱心幫助。 說(shuō)什么四元數(shù)完全解析其實(shí)都是前輩們的解析，小弟真心是一個(gè)搬磚的，搬得不好希望大神們給以批評(píng)和指正，在此謝過(guò)了。因?yàn)楸救耸切〔锁B(niǎo)一枚，對(duì)，最菜的那種菜鳥(niǎo)······所以對(duì)四元數(shù)求解姿態(tài)角這么一個(gè)在大神眼里簡(jiǎn)單的算法，小弟我還是費(fèi)了很大勁才稍微理解了那么一點(diǎn)點(diǎn)，小弟搬磚整理時(shí)也是基于小弟的理解和智商的，有些太基礎(chǔ)，有些可能錯(cuò)了，大牛們發(fā)現(xiàn)了再罵過(guò)我后希望能夠給與指正哈。 好，廢話到此為止，開(kāi)始說(shuō)主體。四元數(shù)和姿態(tài)角怎么說(shuō)呢？先得給和我一樣的小菜鳥(niǎo)們理一理思路，小鳥(niǎo)我在此畫(huà)了一個(gè)“思維導(dǎo)圖”（我承認(rèn)我畫(huà)的丑），四元數(shù)解算姿態(tài)首先分為兩部分理解：第一部分先理解什么是四元數(shù)，四元數(shù)與姿態(tài)角間的關(guān)系；第二部分要理解怎么由慣性單元測(cè)出的加速度和角速度求出四元數(shù)，再由四元數(shù)求出歐拉角。 
圖1 渣渣思維導(dǎo)圖 在講解什么是四元數(shù)時(shí)，小弟的思維是順著說(shuō)的，先由四元數(shù)的定義說(shuō)起，說(shuō)到四元數(shù)與姿態(tài)角間的關(guān)系。但在講解姿態(tài)解算時(shí)，小弟的思維是逆向的，就是反推回來(lái)的，從歐拉角一步步反推回到慣性器件的測(cè)量數(shù)據(jù)，這樣逆向說(shuō)是因?yàn)楸阌诶斫�，因�(yàn)閷?shí)際在工程應(yīng)用時(shí)和理論推導(dǎo)有很大差別。 實(shí)際應(yīng)用時(shí)正確的求解順序應(yīng)該為圖1中序號(hào)順序，即1->2->3->……. 但在筆者講解姿態(tài)求解時(shí)思路是如圖2的。 
圖2 逆向講解思路 大家在看四元數(shù)時(shí)最好結(jié)合著代碼一塊看，小弟看的是匿名四軸的代碼，感覺(jué)寫(xiě)的非常好也非常清晰，粘出來(lái)大家一塊觀摩。紅色部分是核心代碼，總共分為八個(gè)步驟，和圖1中的八個(gè)步驟是一一對(duì)應(yīng)的。講解介紹時(shí)也是和代碼對(duì)比起來(lái)講解的。代碼可以去匿名官網(wǎng)上下載，都是開(kāi)源的，不是小弟的，所以小弟不方便加在附件中。 //四元數(shù)更新姿態(tài) #define Kp 2.0f //加速度權(quán)重，越大則向加速度測(cè)量值收斂越快 #define Ki 0.001f //誤差積分增益 void ANO_IMU::Quaternion_CF(Vector3f gyro,Vector3f acc, float deltaT) { Vector3f V_gravity, V_error, V_error_I; //1.重力加速度歸一化 acc.normalize(); //2.提取四元數(shù)的等效余弦矩陣中的重力分量 Q.vector_gravity(V_gravity); //3.向量叉積得出姿態(tài)誤差 V_error = acc % V_gravity; //4.對(duì)誤差進(jìn)行積分 V_error_I += V_error * Ki; //5.互補(bǔ)濾波，姿態(tài)誤差補(bǔ)償?shù)浇撬俣壬�，修正角速度積分漂移 Gyro += V_error * Kp + V_error_I; //6.一階龍格庫(kù)塔法更新四元數(shù) Q.Runge_Kutta_1st(Gyro, deltaT); //7.四元數(shù)歸一化 Q.normalize(); //8.四元數(shù)轉(zhuǎn)歐拉角 Q.to_euler(&angle.x, &angle.y, &angle.z); } |
好的，下面搬磚開(kāi)始！。。。。。。。。嘿咻嘿咻�。。。�
一． 什么是四元數(shù)？關(guān)于四元數(shù)的定義摘自秦永元的《慣性導(dǎo)航》，里面有非常好的講解，大家可以直接看緒論和第九章就可以。下面我粘貼了部分原文，粘貼的比較多比較詳細(xì)，應(yīng)為本人比較笨還愛(ài)較真，所以按本人的風(fēng)格就要詳盡一點(diǎn)，大牛們都可以自動(dòng)忽略。 四元數(shù)定義、表達(dá)方式及運(yùn)算方法——摘自《慣性導(dǎo)航》-秦永元P289-292
|
好，關(guān)于四元數(shù)定義就搬這么多，其他的大家去附件下載《慣性導(dǎo)航》的pdf自己看吧。 下面開(kāi)始搬四元數(shù)與姿態(tài)解算關(guān)系的。。。。。。嘿咻嘿咻~~~~
二、四元數(shù)與姿態(tài)陣間的關(guān)系從上面我們知道了四元數(shù)的定義，可這四個(gè)數(shù)和我們要求的三個(gè)姿態(tài)角有什么關(guān)系呢？下面是詳細(xì)的推導(dǎo)，同樣摘自《慣性導(dǎo)航》-秦永元P292-297。 四元數(shù)與姿態(tài)陣間的關(guān)系——摘自《慣性導(dǎo)航》-秦永元P292-297
|
呃，粘了這么多其實(shí)就是為了想知道推導(dǎo)過(guò)程小伙伴好理解，真正有用的就是（9.2.34）這個(gè)公式�！@個(gè)公式把四元數(shù)轉(zhuǎn)換成了方向余弦矩陣中的幾個(gè)元素，再用這幾個(gè)元素轉(zhuǎn)換為歐拉角。就求解除了姿態(tài)！ 
先從四元數(shù)q0~q3轉(zhuǎn)成方向余弦矩陣： 
再?gòu)姆较蛴嘞揖仃囖D(zhuǎn)換為歐拉角： 
好的，公式原理都講清楚了，下面來(lái)看一下匿名的代碼： //四元數(shù)轉(zhuǎn)歐拉角，這里四元數(shù)是q1~q4 和公式里q0~q3相對(duì)應(yīng)。 void Quaternion::to_euler(float *roll, float *pitch, float *yaw) { if (roll) { *roll = degrees(atan2f(2.0f*(q1*q2 + q3*q4),1 - 2.0f*(q2*q2 + q3*q3))); //*roll = degrees(atan2f(-2.0f*(q2*q4 - q1*q3),1 - 2.0f*(q2*q2 + q3*q3))); } if (pitch) { // 使用safe_asin()來(lái)處理pitch接近90/-90時(shí)的奇點(diǎn) *pitch = degrees(safe_asin(2.0f*(q1*q3 - q2*q4))); //*pitch = degrees(safe_asin(2.0f*(q3*q4 + q1*q2))); } if (yaw) { *yaw = degrees(atan2f(2.0f*(q2*q3 - q1*q4), 2.0f*(q1*q1 + q2*q2) - 1)); //*yaw = degrees(atan2f(2.0f*(q2*q3 - q1*q4), 2.0f*(q1*q1 + q3*q3) - 1)); } } |
對(duì)比余弦矩陣轉(zhuǎn)換為歐拉角的公式很容易理解了吧，注意一下，紅色是匿名原版的代碼，和公式還是有出入的，自己細(xì)心觀察一下吧。被注釋了的黑色代碼是我根據(jù)上面的公式寫(xiě)的。但黑色的求解出來(lái)的歐拉角反映出來(lái)的姿態(tài)是不對(duì)的，具體表現(xiàn)為俯仰（pitch）和橫滾（roll）是相反的，為啥根據(jù)公式寫(xiě)的是不對(duì)的？群里的小伙伴給與了我熱心的解答。 這個(gè)錯(cuò)誤主要是由于方向余弦矩陣的旋轉(zhuǎn)順序不一樣，也就是公式（9.2.39）不一樣，這是由于旋轉(zhuǎn)的先后順序不同引起的，具體大家參考《慣性導(dǎo)航》緒論來(lái)看就能明白，因?yàn)檫@一點(diǎn)小弟還有點(diǎn)混亂，就點(diǎn)到這為止。 
以上就是四元數(shù)求解歐拉角的方法。通過(guò)公式可以看到，要求歐拉角需要求得四元數(shù)的方向余弦矩陣；要求得四元數(shù)方向余弦矩陣，需要求得四元數(shù)q0~q3，那么如何求得q0~q3？接下來(lái)詳細(xì)介紹。 三、四元數(shù)微分及龍格庫(kù)塔求Q0~Q3首先我們先來(lái)看一下在程序里如何求解的q0~q3： //一階龍格庫(kù)塔法更新四元數(shù) void Quaternion::Runge_Kutta_1st(Vector3f &g, float deltaT) { q1 += 0.5 * (-q2*g.x - q3*g.y - q4*g.z)* deltaT; q2 += 0.5 * (q1*g.x + q3*g.z - q4*g.y)* deltaT; q3 += 0.5 * (q1*g.y - q2*g.z + q4*g.x)* deltaT; q4 += 0.5 * (q1*g.z + q2*g.y - q3*g.x)* deltaT; } |
這就是一階龍格庫(kù)塔法求解q的微分方程，傳入?yún)?shù)只需要這個(gè)周期的角速度g.x、g.y、g.z和周期時(shí)間deltaT。下面一張是從某位大神的貼吧上盜的圖，描繪的是一階龍格庫(kù)塔的計(jì)算式。 
相信很多人和我一樣，單看上圖很難理解其中的意思和其由來(lái)，于是我又找了很多帖子，感謝前人做出的貢獻(xiàn)，小弟在這里再次整理大神的四元數(shù)微分方程推導(dǎo)公式，便于大家理解。摘自附件中《推導(dǎo)_四元數(shù).pdf》 雖然在下也不是很懂，不過(guò)粘出來(lái)還是能起到理解的作用，這樣大家就不會(huì)覺(jué)得這是憑空變出來(lái)的，本人數(shù)學(xué)功底薄弱，沒(méi)有對(duì)推導(dǎo)進(jìn)行過(guò)驗(yàn)證，如果有不對(duì)的地方歡迎指正。 接著使用一階龍格庫(kù)塔（Runge-Kutta）發(fā)求出q0~q3，這一點(diǎn)很多人不知道一階龍格庫(kù)塔怎么推導(dǎo)的，下面也是這位網(wǎng)友的推導(dǎo)，大家參考著理解吧。 這里的角速度 是由捷聯(lián)陀螺的輸出（對(duì)機(jī)械轉(zhuǎn)子陀螺必須經(jīng)過(guò)誤差補(bǔ)償，將在下面介紹）。 對(duì)比著匿名四軸的代碼看一看（g.x、g.y、g.z是捷聯(lián)陀螺的輸出），代碼的意思就比較清楚了。在往上一步步推，我們就要求陀螺輸出了，并且還要對(duì)數(shù)據(jù)進(jìn)行互補(bǔ)濾波處理。 四、慣性單元測(cè)量值融合這部分看似很簡(jiǎn)單，但是也有讓筆者難以理解的地方，希望后人能補(bǔ)充修正進(jìn)行更好的講解。有了上一步的龍格庫(kù)塔方程，我們現(xiàn)在需要的就是角速度的測(cè)量值。 在四軸上安裝陀螺儀，可以測(cè)量四軸傾斜的角速度，由于陀螺儀輸出的是四軸的角速度，不會(huì)受到四軸振動(dòng)影響。因此該信號(hào)中噪聲很小。四軸的角度又是通過(guò)對(duì)角速度積分而得，這可進(jìn)一步平滑信號(hào)，從而使得角度信號(hào)更加穩(wěn)定。因此四軸控制所需要的角度和角速度可以使用陀螺儀所得到的信號(hào)。由于從陀螺儀的角速度獲得角度信息，需要經(jīng)過(guò)積分運(yùn)算。如果角速度信號(hào)存在微小的偏差，經(jīng)過(guò)積分運(yùn)算之后，變化形成積累誤差。這個(gè)誤差會(huì)隨著時(shí)間延長(zhǎng)逐步增加，最終導(dǎo)致電路飽和，無(wú)法形成正確的角度信號(hào)。 如何消除這個(gè)累積誤差呢？可以通過(guò)上面的加速度傳感器獲得的角度信息對(duì)此進(jìn)行校正。利用加速度計(jì)所獲得的角度信息 θg 與陀螺儀積分后的角度θ 進(jìn)行比較，將比較的誤差信號(hào)經(jīng)過(guò)比例Tg 放大之后與陀螺儀輸出的角速度信號(hào)疊加之后再進(jìn)行積分。對(duì)于加速度計(jì)給定的角度θg ，經(jīng)過(guò)比例、積分環(huán)節(jié)之后產(chǎn)生的角度θ必然最終等于θg 。由于加速度計(jì)獲得的角度信息不會(huì)存在積累誤差，所以最終將輸出角度θ中的積累誤差消除了。加速度計(jì)所產(chǎn)生的角度信息θg 中會(huì)疊加很強(qiáng)的有四軸運(yùn)動(dòng)加速度噪聲信號(hào)。為了避免該信號(hào)對(duì)于角度θ 的影響，因此比例系數(shù) Tg 應(yīng)該非常小。這樣，加速度的噪聲信號(hào)經(jīng)過(guò)比例、積分后，在輸出角度信息中就會(huì)非常小了。由于存在積分環(huán)節(jié)，所以無(wú)論比例Tg多么小，最終輸出角度θ必然與加速度計(jì)測(cè)量的角度θg相等，只是這個(gè)調(diào)節(jié)過(guò)程會(huì)隨著Tg 的減小而延長(zhǎng)。 先把這個(gè)過(guò)程的代碼粘出來(lái)，看著代碼一步步理解： #define Kp 2.0f //加速度權(quán)重，越大則向加速度測(cè)量值收斂越快 #define Ki 0.001f //誤差積分增益 //1.重力加速度歸一化 acc.normalize(); //2.提取四元數(shù)的等效余弦矩陣中的重力分量 Q.vector_gravity(V_gravity); //3.向量叉積得出姿態(tài)誤差 V_error = acc % V_gravity; //4.對(duì)誤差進(jìn)行積分 V_error_I += V_error * Ki; //5.互補(bǔ)濾波，姿態(tài)誤差補(bǔ)償?shù)浇撬俣壬�，修正角速度積分漂移 Gyro += V_error * Kp + V_error_I; |
1.重力加速度歸一化：加速度計(jì)數(shù)據(jù)歸一化，把加速度計(jì)的三維向量轉(zhuǎn)換為單位向量， 因?yàn)槭菃挝皇噶康絽⒖夹缘耐队埃砸鸭铀俣扔?jì)數(shù)據(jù)單位化，其實(shí)歸一化改變的只是這三個(gè)向量的長(zhǎng)度，也就是只改變了相同的倍數(shù)，方向并沒(méi)有改變，也是為了與單位四元數(shù)對(duì)應(yīng)。 2.提取四元數(shù)的等效余弦矩陣中的重力分量： // 返回該四元數(shù)的等效余弦矩陣中的重力分量 void Quaternion::vector_gravity(Vector3f &v) { v.x = 2*(q2*q4 - q1*q3); v.y = 2*(q1*q2 + q3*q4); v.z = 1-2*(q2*q2 + q3*q3); } |
將當(dāng)前姿態(tài)的重力在三個(gè)軸上的分量分離出來(lái),把四元數(shù)換算成方向余弦中的第三行的三個(gè)元素，根據(jù)余弦矩陣和歐拉角的定義，就是地理坐標(biāo)系(參考坐標(biāo)系)的Z軸的重力向量。當(dāng)我讀完這句話腦子挺懵的，鬧不明白啊，于是又找到了下面的資料，可以進(jìn)行解釋了。 別忘了這是個(gè)正交矩陣哦！這樣就知道代碼怎么來(lái)的了吧？好繼續(xù)。 3.向量叉積得出姿態(tài)誤差: 哎呀，又來(lái)棘手問(wèn)題了，這個(gè)我也不太明白怎么講啊，還是把大神的講解粘過(guò)來(lái)吧，大家看看是不是這么回事： acc是機(jī)體坐標(biāo)參照系上，加速度計(jì)測(cè)出來(lái)的重力向量，也就是實(shí)際測(cè)出來(lái)的重力向量。 acc是測(cè)量得到的重力向量，V_gravity是陀螺積分后的姿態(tài)來(lái)推算出的重力向量，它們都是機(jī)體坐標(biāo)參照系上的重力向量。 那它們之間的誤差向量，就是陀螺積分后的姿態(tài)和加計(jì)測(cè)出來(lái)的姿態(tài)之間的誤差。 向量間的誤差，可以用向量叉積（也叫向量外積、叉乘）來(lái)表示，V_error就是兩個(gè)重力向量的叉積。 這個(gè)叉積向量仍舊是位于機(jī)體坐標(biāo)系上的，而陀螺積分誤差也是在機(jī)體坐標(biāo)系，而且叉積的大小與陀螺積分誤差成正比，正好拿來(lái)糾正陀螺。（你可以自己拿東西想象一下）由于陀螺是對(duì)機(jī)體直接積分，所以對(duì)陀螺的糾正量會(huì)直接體現(xiàn)在對(duì)機(jī)體坐標(biāo)系的糾正。 |
看了上面的話，小弟一直對(duì)這個(gè)誤差向量感到莫名其妙，后來(lái)又找到大神的一下一段話： 這里誤差沒(méi)說(shuō)清楚，不是指向量差。這個(gè)叉積誤差是指將帶有誤差的加計(jì)向量轉(zhuǎn)動(dòng)到與重力向量重合，需要繞什么軸，轉(zhuǎn)多少角度。逆向推理一下，這個(gè)叉積在機(jī)體三軸上的投影，就是加計(jì)和重力之間的角度誤差。也就是說(shuō)，如果陀螺按這個(gè)叉積誤差的軸，轉(zhuǎn)動(dòng)叉積誤差的角度（也就是轉(zhuǎn)動(dòng)三軸投影的角度）那就能把加計(jì)和重力向量的誤差消除掉。（具體可看向量叉積的定義）如果完全按叉積誤差轉(zhuǎn)過(guò)去，那就是完全信任加計(jì)。如果一點(diǎn)也不轉(zhuǎn)，那就是完全信任陀螺。那么把這個(gè)叉積的三軸乘以x%，加到陀螺的積分角度上去，就是這個(gè)x%互補(bǔ)系數(shù)的互補(bǔ)算法了。 |
這個(gè)看了好像終于理解點(diǎn)了，再看下代碼： //3.向量叉積得出姿態(tài)誤差 V_error = acc % V_gravity; |
這里“ % ”已經(jīng)重定義為叉乘的算法了。 4.對(duì)誤差進(jìn)行積分： 積分求誤差,關(guān)于當(dāng)前姿態(tài)分離出的重力分量,與當(dāng)前加速度計(jì)測(cè)得的重力分量的差值進(jìn)行積分消除誤差 V_error_I += V_error * Ki; |
5.互補(bǔ)濾波，姿態(tài)誤差補(bǔ)償?shù)浇撬俣壬�，修正角速度積分漂移 系數(shù)不停地被陀螺積分更新，也不停地被誤差修正，它和公式所代表的姿態(tài)也在不斷更新。 將積分誤差反饋到陀螺儀上，修正陀螺儀的值。將該誤差V_error輸入 PI 控制器后與本次姿態(tài)更新周期中陀螺儀測(cè)得的角速度相加，最終得到一個(gè)修正的角速度值,將其輸入四元數(shù)微分方程，更新四元數(shù)。 Gyro += V_error * Kp + V_error_I; |
Gyro就是得到的修正角速度值，可以用于求解四元數(shù)q0~q3了。 到這里回顧一下八個(gè)步驟還漏了一個(gè)第七步： 7.四元數(shù)歸一化： 規(guī)范化四元數(shù)作用： 1.表征旋轉(zhuǎn)的四元數(shù)應(yīng)該是規(guī)范化的四元數(shù)，但是由于計(jì)算誤差等因素，計(jì)算過(guò)程中四元數(shù)會(huì)逐漸失去規(guī)范化特性，因此必須對(duì)四元數(shù)做規(guī)范化處理。 2.意義在于單位化四元數(shù)在空間旋轉(zhuǎn)時(shí)是不會(huì)拉伸的,僅有旋轉(zhuǎn)角度.這類似與線性代數(shù)里面的正交變換。 3.由于誤差的引入，使得計(jì)算的變換四元數(shù)的模不再等于1，變換四元數(shù)失去規(guī)范性，因此再次更新四元數(shù)。 計(jì)算歐拉角時(shí)候必須要對(duì)四元數(shù)歸一化處理。
呃，關(guān)于四元數(shù)求解姿態(tài)的磚好像搬完了。為什么要用四元數(shù)法求解姿態(tài)呢？再搬一點(diǎn)關(guān)于歐拉角法和旋轉(zhuǎn)矢量法的介紹的。
搬磚搬得好累啊，不過(guò)搬得差不多了，感覺(jué)挺亂的？呃，主要是由于比較多吧，那我再串一遍？拉倒吧，你看得都累，我寫(xiě)著不累？沒(méi)鬧明白再自己串一遍吧，相信第二遍就能明白了。 哎~對(duì)于我這樣的渣渣而言也就能理解到這一步了，這也是我好幾天的心血整理了一下，也許有和我一樣的菜鳥(niǎo)呢，對(duì)他們也許能有點(diǎn)幫助，做得不好希望大神們能耐心給與指正，而不是嗤之以鼻，或者噴我一頓就走。。。畢竟整理了兩天呢（我還以為一中午就能搞定呢）。渣渣的學(xué)習(xí)之路也是挺不容易的，因?yàn)榛A(chǔ)渣渣，學(xué)校渣渣，所以難以得到有效地幫助和指導(dǎo)，有時(shí)在群里尋求幫助，無(wú)聊的群友會(huì)告訴你看書(shū)去，呵呵。。。我也知道看書(shū)啊。。。哪怕你能告訴我我的問(wèn)題在那本書(shū)的那部分能有相似吧？一句看書(shū)去，上網(wǎng)查啊，等于沒(méi)回答，如果一直這樣自己看下去可能半年也解決不了，因?yàn)樵膶W(xué)習(xí)環(huán)境是有局限性的。 不過(guò)好在有更多很熱心的群友能提供給我?guī)椭�，把他們收集的好貼發(fā)給我，或者干脆手寫(xiě)一個(gè)公式推導(dǎo)，一個(gè)電路圖，然后拍照發(fā)給我，還有的幫我下載照片，分類命名給我，艾瑪！熱淚盈眶啊有木有�。。≡俅胃兄x這些熱心幫助我的小伙伴@奇點(diǎn)，@杜掌柜，@廉價(jià)物品，@忘記名字的小伙伴······ 下面附上被我搬磚的幾個(gè)好貼，謝謝大神們的樂(lè)于分享： | 
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
