感謝匿名的開源分享，感謝群友的熱心幫助。
說什么四元數(shù)完全解析其實都是前輩們的解析，小弟真心是一個搬磚的，搬得不好希望大神們給以批評和指正，在此謝過了。因為本人是小菜鳥一枚，對，最菜的那種菜鳥······所以對四元數(shù)求解姿態(tài)角這么一個在大神眼里簡單的算法，小弟我還是費了很大勁才稍微理解了那么一點點，小弟搬磚整理時也是基于小弟的理解和智商的，有些太基礎(chǔ)，有些可能錯了，大牛們發(fā)現(xiàn)了再罵過我后希望能夠給與指正哈。
好，廢話到此為止，開始說主體。四元數(shù)和姿態(tài)角怎么說呢？先得給和我一樣的小菜鳥們理一理思路，小鳥我在此畫了一個“思維導(dǎo)圖”（我承認(rèn)我畫的丑），四元數(shù)解算姿態(tài)首先分為兩部分理解：第一部分先理解什么是四元數(shù)，四元數(shù)與姿態(tài)角間的關(guān)系；第二部分要理解怎么由慣性單元測出的加速度和角速度求出四元數(shù)，再由四元數(shù)求出歐拉角。
圖1 渣渣思維導(dǎo)圖
在講解什么是四元數(shù)時，小弟的思維是順著說的，先由四元數(shù)的定義說起，說到四元數(shù)與姿態(tài)角間的關(guān)系。但在講解姿態(tài)解算時，小弟的思維是逆向的，就是反推回來的，從歐拉角一步步反推回到慣性器件的測量數(shù)據(jù)，這樣逆向說是因為便于理解，因為實際在工程應(yīng)用時和理論推導(dǎo)有很大差別。
實際應(yīng)用時正確的求解順序應(yīng)該為圖1中序號順序，即1->2->3->…….
但在筆者講解姿態(tài)求解時思路是如圖2的。
圖2 逆向講解思路
大家在看四元數(shù)時最好結(jié)合著代碼一塊看，小弟看的是匿名四軸的代碼，感覺寫的非常好也非常清晰，粘出來大家一塊觀摩。紅色部分是核心代碼，總共分為八個步驟，和圖1中的八個步驟是一一對應(yīng)的。講解介紹時也是和代碼對比起來講解的。代碼可以去匿名官網(wǎng)上下載，都是開源的，不是小弟的，所以小弟不方便加在附件中。
//四元數(shù)更新姿態(tài) #define Kp 2.0f //加速度權(quán)重，越大則向加速度測量值收斂越快 #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.對誤差進(jìn)行積分 V_error_I += V_error * Ki; //5.互補濾波，姿態(tài)誤差補償?shù)浇撬俣壬�，修正角速度積分漂移 Gyro += V_error * Kp + V_error_I; //6.一階龍格庫塔法更新四元數(shù) Q.Runge_Kutta_1st(Gyro, deltaT); //7.四元數(shù)歸一化 Q.normalize(); //8.四元數(shù)轉(zhuǎn)歐拉角 Q.to_euler(&angle.x, &angle.y, &angle.z); } |
好的，下面搬磚開始！。。。。。。。。嘿咻嘿咻�。。�！
關(guān)于四元數(shù)的定義摘自秦永元的《慣性導(dǎo)航》，里面有非常好的講解，大家可以直接看緒論和第九章就可以。下面我粘貼了部分原文，粘貼的比較多比較詳細(xì)，應(yīng)為本人比較笨還愛較真，所以按本人的風(fēng)格就要詳盡一點，大牛們都可以自動忽略。
四元數(shù)定義、表達(dá)方式及運算方法——摘自《慣性導(dǎo)航》-秦永元P289-292          |
好，關(guān)于四元數(shù)定義就搬這么多，其他的大家去附件下載《慣性導(dǎo)航》的pdf自己看吧。
下面開始搬四元數(shù)與姿態(tài)解算關(guān)系的。。。。。。嘿咻嘿咻~~~~
從上面我們知道了四元數(shù)的定義，可這四個數(shù)和我們要求的三個姿態(tài)角有什么關(guān)系呢？下面是詳細(xì)的推導(dǎo)，同樣摘自《慣性導(dǎo)航》-秦永元P292-297。
四元數(shù)與姿態(tài)陣間的關(guān)系——摘自《慣性導(dǎo)航》-秦永元P292-297            |
呃，粘了這么多其實就是為了想知道推導(dǎo)過程小伙伴好理解，真正有用的就是（9.2.34）這個公式�！@個公式把四元數(shù)轉(zhuǎn)換成了方向余弦矩陣中的幾個元素，再用這幾個元素轉(zhuǎn)換為歐拉角。就求解除了姿態(tài)！
先從四元數(shù)q0~q3轉(zhuǎn)成方向余弦矩陣：
再從方向余弦矩陣轉(zhuǎn)換為歐拉角：
好的，公式原理都講清楚了，下面來看一下匿名的代碼：
//四元數(shù)轉(zhuǎn)歐拉角，這里四元數(shù)是q1~q4 和公式里q0~q3相對應(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()來處理pitch接近90/-90時的奇點 *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)); } } |
對比余弦矩陣轉(zhuǎn)換為歐拉角的公式很容易理解了吧，注意一下，紅色是匿名原版的代碼，和公式還是有出入的，自己細(xì)心觀察一下吧。被注釋了的黑色代碼是我根據(jù)上面的公式寫的。但黑色的求解出來的歐拉角反映出來的姿態(tài)是不對的，具體表現(xiàn)為俯仰（pitch）和橫滾（roll）是相反的，為啥根據(jù)公式寫的是不對的？群里的小伙伴給與了我熱心的解答。
這個錯誤主要是由于方向余弦矩陣的旋轉(zhuǎn)順序不一樣，也就是公式（9.2.39）不一樣，這是由于旋轉(zhuǎn)的先后順序不同引起的，具體大家參考《慣性導(dǎo)航》緒論來看就能明白，因為這一點小弟還有點混亂，就點到這為止。
以上就是四元數(shù)求解歐拉角的方法。通過公式可以看到，要求歐拉角需要求得四元數(shù)的方向余弦矩陣；要求得四元數(shù)方向余弦矩陣，需要求得四元數(shù)q0~q3，那么如何求得q0~q3？接下來詳細(xì)介紹。
三、四元數(shù)微分及龍格庫塔求Q0~Q3
首先我們先來看一下在程序里如何求解的q0~q3：
//一階龍格庫塔法更新四元數(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; } |
這就是一階龍格庫塔法求解q的微分方程，傳入?yún)?shù)只需要這個周期的角速度g.x、g.y、g.z和周期時間deltaT。下面一張是從某位大神的貼吧上盜的圖，描繪的是一階龍格庫塔的計算式。
相信很多人和我一樣，單看上圖很難理解其中的意思和其由來，于是我又找了很多帖子，感謝前人做出的貢獻(xiàn)，小弟在這里再次整理大神的四元數(shù)微分方程推導(dǎo)公式，便于大家理解。摘自附件中《推導(dǎo)_四元數(shù).pdf》
雖然在下也不是很懂，不過粘出來還是能起到理解的作用，這樣大家就不會覺得這是憑空變出來的，本人數(shù)學(xué)功底薄弱，沒有對推導(dǎo)進(jìn)行過驗證，如果有不對的地方歡迎指正。
接著使用一階龍格庫塔（Runge-Kutta）發(fā)求出q0~q3，這一點很多人不知道一階龍格庫塔怎么推導(dǎo)的，下面也是這位網(wǎng)友的推導(dǎo)，大家參考著理解吧。
 |
 |
是由捷聯(lián)陀螺的輸出（對機械轉(zhuǎn)子陀螺必須經(jīng)過誤差補償，將在下面介紹）。這部分看似很簡單，但是也有讓筆者難以理解的地方，希望后人能補充修正進(jìn)行更好的講解。有了上一步的龍格庫塔方程，我們現(xiàn)在需要的就是角速度的測量值。
在四軸上安裝陀螺儀，可以測量四軸傾斜的角速度，由于陀螺儀輸出的是四軸的角速度，不會受到四軸振動影響。因此該信號中噪聲很小。四軸的角度又是通過對角速度積分而得，這可進(jìn)一步平滑信號，從而使得角度信號更加穩(wěn)定。因此四軸控制所需要的角度和角速度可以使用陀螺儀所得到的信號。由于從陀螺儀的角速度獲得角度信息，需要經(jīng)過積分運算。如果角速度信號存在微小的偏差，經(jīng)過積分運算之后，變化形成積累誤差。這個誤差會隨著時間延長逐步增加，最終導(dǎo)致電路飽和，無法形成正確的角度信號。
如何消除這個累積誤差呢？可以通過上面的加速度傳感器獲得的角度信息對此進(jìn)行校正。利用加速度計所獲得的角度信息 θg 與陀螺儀積分后的角度θ 進(jìn)行比較，將比較的誤差信號經(jīng)過比例Tg 放大之后與陀螺儀輸出的角速度信號疊加之后再進(jìn)行積分。對于加速度計給定的角度θg ，經(jīng)過比例、積分環(huán)節(jié)之后產(chǎn)生的角度θ必然最終等于θg 。由于加速度計獲得的角度信息不會存在積累誤差，所以最終將輸出角度θ中的積累誤差消除了。加速度計所產(chǎn)生的角度信息θg 中會疊加很強的有四軸運動加速度噪聲信號。為了避免該信號對于角度θ 的影響，因此比例系數(shù) Tg 應(yīng)該非常小。這樣，加速度的噪聲信號經(jīng)過比例、積分后，在輸出角度信息中就會非常小了。由于存在積分環(huán)節(jié)，所以無論比例Tg多么小，最終輸出角度θ必然與加速度計測量的角度θg相等，只是這個調(diào)節(jié)過程會隨著Tg 的減小而延長。
#define Kp 2.0f //加速度權(quán)重，越大則向加速度測量值收斂越快 #define Ki 0.001f //誤差積分增益 //1.重力加速度歸一化 acc.normalize(); //2.提取四元數(shù)的等效余弦矩陣中的重力分量 Q.vector_gravity(V_gravity); //3.向量叉積得出姿態(tài)誤差 V_error = acc % V_gravity; //4.對誤差進(jìn)行積分 V_error_I += V_error * Ki; //5.互補濾波，姿態(tài)誤差補償?shù)浇撬俣壬�，修正角速度積分漂移 Gyro += V_error * Kp + V_error_I; |
// 返回該四元數(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); } |
 |
acc是機體坐標(biāo)參照系上，加速度計測出來的重力向量，也就是實際測出來的重力向量。 acc是測量得到的重力向量，V_gravity是陀螺積分后的姿態(tài)來推算出的重力向量，它們都是機體坐標(biāo)參照系上的重力向量。 那它們之間的誤差向量，就是陀螺積分后的姿態(tài)和加計測出來的姿態(tài)之間的誤差。 向量間的誤差，可以用向量叉積（也叫向量外積、叉乘）來表示，V_error就是兩個重力向量的叉積。 這個叉積向量仍舊是位于機體坐標(biāo)系上的，而陀螺積分誤差也是在機體坐標(biāo)系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。（你可以自己拿東西想象一下）由于陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現(xiàn)在對機體坐標(biāo)系的糾正。 |
這里誤差沒說清楚，不是指向量差。這個叉積誤差是指將帶有誤差的加計向量轉(zhuǎn)動到與重力向量重合，需要繞什么軸，轉(zhuǎn)多少角度。逆向推理一下，這個叉積在機體三軸上的投影，就是加計和重力之間的角度誤差。也就是說，如果陀螺按這個叉積誤差的軸，轉(zhuǎn)動叉積誤差的角度（也就是轉(zhuǎn)動三軸投影的角度）那就能把加計和重力向量的誤差消除掉。（具體可看向量叉積的定義）如果完全按叉積誤差轉(zhuǎn)過去，那就是完全信任加計。如果一點也不轉(zhuǎn)，那就是完全信任陀螺。那么把這個叉積的三軸乘以x%，加到陀螺的積分角度上去，就是這個x%互補系數(shù)的互補算法了。 |
//3.向量叉積得出姿態(tài)誤差 V_error = acc % V_gravity; |
V_error_I += V_error * Ki; |
Gyro += V_error * Kp + V_error_I; |
Q.normalize(); |
 |
四元數(shù)解算姿態(tài)完全解析及資料匯總.docx
(2.36 MB, 下載次數(shù): 77)
| 歡迎光臨 (http://www.zg4o1577.cn/bbs/) | Powered by Discuz! X3.1 |