四元數(shù)解算姿態(tài)完全解析及匯總

1、從上面我們知道了四的定義，可這四個數(shù)和我們要求的三個姿態(tài)角有什么關(guān)系呢？下面是詳細的推導(dǎo)，同樣摘自慣性導(dǎo)航-秦P292-297。四與姿態(tài)陣間的關(guān)系摘自慣性導(dǎo)航-秦P292-297二、四與姿態(tài)陣間的關(guān)系呃，粘了這么多其實就是為了想知道推導(dǎo)過程小伙伴好理解，真正有用的就是（9.2.34）這個公式。這個公式把四歐拉角。就求解除了姿態(tài)！轉(zhuǎn)換成了方向余弦矩陣中的幾個元素，再用這幾個元素轉(zhuǎn)換為先從四q0q3 轉(zhuǎn)成方向余弦矩陣：x𝑏𝑞2+𝑞2 𝑞2 𝑞22(𝑞1𝑞2 𝑞0w

2、902;3)2(𝑞1𝑞3 + 𝑞0𝑞2)𝑥0123𝑦𝑏 = 2(𝑞1𝑞2 + 𝑞0𝑞3)𝑛𝑞2𝑞2 + 𝑞2 𝑞22(𝑞2𝑞3 𝑞0𝑞1) 𝑦𝑛0123𝑧𝑏𝑧𝑛𝑞2&#

3、119902;2 𝑞2 + 𝑞22(𝑞1𝑞3 𝑞0𝑞2)2(𝑞2𝑞3 + 𝑞0𝑞1)0123再從方向余弦矩陣轉(zhuǎn)換為歐拉角：𝜃 = sin1（ 𝑇32） 俯仰角𝑝𝑖𝑡𝑐𝜃 = tan1 （ 𝑇31 ） 橫滾角𝑟𝑜𝑙𝑙𝑇33ҵ

4、79; = tan1 （ 𝑇12 ） 航向角𝑦𝑎𝑤𝑇22好的，公式原理都講清楚了，下面來看一下的代碼：對比余弦矩陣轉(zhuǎn)換為歐拉角的公式很容易理解了吧，注意一下，紅色是原版的代碼，和公式還是有出入的，細心觀察一下吧。被注釋了的黑色代碼是我根據(jù)上面的公式寫的。但黑色的求解出來的歐拉角反映出來的姿態(tài)是不對的，具體表現(xiàn)為俯仰（pitch）和橫滾（roll）是相反的，為啥根據(jù)公式寫的是不對的？群里的小伙伴給與了我熱心的解答。這個錯誤主要是由于方向余弦矩陣的旋轉(zhuǎn)順序不一樣，也就是公式（9.2.39）不一樣，這是由于旋轉(zhuǎn)的先后順序

5、不同引起的，具體大家參考慣性導(dǎo)航緒論來看就能明白，因為這一點小弟還有點，就點到這為止。以上就是四求解歐拉角的方法。通過公式可以看到，要求歐拉角需要求得四的方向余弦矩陣；要求得四下來詳細介紹。方向余弦矩陣，需要求得四q0q3，那么如何求得 q0q3？接/四轉(zhuǎn)歐拉角，這里四是q1q4 和公式里 q0q3 相對應(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