目錄
1. 引言
2. 四元數轉旋轉矩陣
3. 已知旋轉矩陣求四元數
3.1 先求w
3.2 先求x
3.3 先求y
3.4 先求z
4. 四元數與旋轉矩陣轉換的C++實作
4. 總結
1. 引言 上一篇文章我們主要介紹了歐拉角與旋轉矩陣之間的關系,這篇文章介紹旋轉矩陣和四元數之間的關系。關于四元數的定義和工作原理這裡就不詳細介紹了,給大家推薦一篇文章Understanding Quaternions以及它的中文翻譯版了解四元數。我認為這篇文章寫得非常透徹,相信不管是誰再去寫關于四元數原理的文章都無出其右了。
2. 四元數轉旋轉矩陣 四元數對空間中的點進行變換有固定的表達形式
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 。我們這裡隻讨論歸一化的四元數。設一個四元數
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 表達如下:
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 這個四元數描述的就是繞軸
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 旋轉
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 。空間中一個點
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 在慣性系下的坐标為
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,在用四元數進行操作之前我們首先要把它變成純四元數的形式即
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,運用四元數對這個點進行旋轉,得到一個新的點
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,如下式:
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 是以在已知四元數時,旋轉矩陣的計算公式如下:
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 3. 已知旋轉矩陣求四元數 關于旋轉矩陣轉四元數有一個問題曾經困擾了我很久,就是關于開方的問題,大家都了解開方其實是有正負兩個解的,為什麼忽略一個解呢?不隻是在這裡,在很多其他地方,特别是後面會講到的機器人逆解問題,也有類似的情況出現。為了避免大家踩坑,我們先把這個坑填上。先給一個命題:四元數和它的相反四元數描述相同的旋轉。這是一個真命題,簡單證明一下,我們知道四元數對點進行變換的公式如下:
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 對于歸一化四元數
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,它的逆是
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 (其實對于歸一化四元數,它的逆就是它的共轭),那麼四元數
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 的逆呢?很顯然是
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,如果讓四元數
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 作用于同一個點
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 呢:
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 負負得正,是以
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 和
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 作用效果是一樣的。
接下來回歸正題,旋轉矩陣怎麼求四元數呢,我們還是要從前面得到的旋轉矩陣出發來進行推導,在這個過程中不要忘記我們讨論旋轉對應的四元數始終是歸一化的,即元素平方和為1。
需要注意的是,我們求解四元數有四種途徑,從數學上講他們的行為是一樣的,除了在表達式奇異的位置(所謂表達式奇異在這裡是指分母為0),但是從數值分析的角度上考慮,當分母太小時,計算結果對舍入誤差非常敏感,是以需要盡量減小舍入誤差的影響,是以才會根據實際情況選擇一種解法來求四元數。接下來我們通過以下旋轉矩陣求四元數:
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 3.1 先求w 我們詳細講一下先求
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,後面的方法類似,把
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 對角線元素相加
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 (注意
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 代表
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 的第i行第j列的元素),前面也提到對于歸一化四元數它的元素平方和為1,即
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,是以
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,是以怎麼求
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 呢?很簡單:
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 有了
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,其他元素就比較容易了,請觀察
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 和
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,兩個元素作差再除以
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 就得到了
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,同理
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 和
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 作差再除以
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 可以得到
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 與
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 作差再除以
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 可以得到
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,最終計算結果如下:
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 接下來踩一個坑,求
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 時有一個開方運算,為什麼不取負值?你可能也看到了,如果
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 取負值,相應的
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 的求解分母都有
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,他們也會取負值,是以得到的四元數就是我們在第三節開頭提到的負四元數,它和我們公式得到的四元數描述相同的旋轉,是以我們取
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 為正的一組解,後面三種情況也相同。
3.2 先求x 在3.1中如果我們求出的
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 過小,為了避免解的不穩定,需要采用其他的求解方法,我們可以從對角線元素中先求
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 或者
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 。具體先求哪一個呢?我們當然希望先求大的那一個,因為你會看到,通過對角線元素求出的那個值始終是作為另外三個等式的分母,這個值越大越不容易受到舍入誤差的影響。
具體的比較方法就是比較三個對角線元素哪個大,如果
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 最大,說明
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 相對來說絕對值最大,為什麼這麼說呢?大概提一下:
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 這樣很明顯可以看出當
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 最大時說明
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 絕對值最大,當
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 最大時說明
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 絕對值最大,當
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 最大時說明
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 絕對值最大。
當你發現
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 最大時,我們就先求
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,根據歸一化四元數元素平方和為1,利用對角線元素我們就可以求解
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,如下:
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 後面的求解就類似了,找到矩陣主對角線對稱的元素,作和或者作差然後除以
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 即可。最終計算結果如下:
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 3.3 先求y 當你發現
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 最大時,我們就先求
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,思路與3.1一樣,結果如下:
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 3.4 先求z 當你發現
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 最大時,我們就先求
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 ,思路與3.1一樣,結果如下:
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 4. 四元數與旋轉矩陣轉換的C++實作 代碼是算法的載體,我始終覺得不寫爛代碼是算法工程師的基本素養,接下來我們把旋轉矩陣與四元數的變換關系用C++實作一遍。
先說從四元數轉旋轉矩陣,這個過程是很直接的,我們直接把
11. 機器人正運動學---姿态描述之四元數1. 引言2. 四元數轉旋轉矩陣3. 已知旋轉矩陣求四元數4. 四元數與旋轉矩陣轉換的C++實作4. 總結 對應的旋轉矩陣寫入即可。頭檔案中的描述如下:
/**
* @brief This class is used to represent rotation in 3d space.
* */
class Rotation {
public:
Rotation();
/**
* @brief Construct
* @param[in] Xx \ref data[0]
* @param[in] Yx \ref data[1]
* @param[in] Zx \ref data[2]
* @param[in] Xy \ref data[3]
* @param[in] Yy \ref data[4]
* @param[in] Zy \ref data[5]
* @param[in] Xz \ref data[6]
* @param[in] Yz \ref data[7]
* @param[in] Zz \ref data[8]
*/
Rotation(double Xx, double Yx, double Zx, double Xy, double Yy, double Zy,
double Xz, double Yz, double Zz);
/**
* @brief Convert quaternion to Rotation object.
* q=[w,(x,y,z)], w is the scalar, (x,y,z)is the direction vector.
* @param[in] x X of the quaternion
* @param[in] y Y of the quaternion
* @param[in] z Z of the quaternion
* @param[in] w W of the quaternion
*/
static Rotation quaternion(double x, double y, double z, double w);
/**
* @brief Get quaternion from the Rotation.
* @param[in] x Reference to quaternion x
* @param[in] y Reference to quaternion y
* @param[in] z Reference to quaternion z
* @param[in] w Reference to quaternion w
*/
void getQuaternion(double &x, double &y, double &z, double &w) const;
/**
* @brief data of the rotation matrix
* @note the rotation matrix is defined as:
* [data[0], data[1], data[2];
* data[3], data[4], data[5];
* data[6], data[7], data[8]]
* */
double data[9];
};
在頭檔案中我們定義了一個數組來儲存旋轉矩陣的9個元素。四元數轉旋轉矩陣的代碼如下:
Rotation Rotation::quaternion(double x, double y, double z, double w) {
Rotation r;
r.data[0] = 1 - 2 * y * y - 2 * z * z;
r.data[1] = 2 * (x * y - w * z);
r.data[2] = 2 * (x * z + w * y);
r.data[3] = 2 * (x * y + w * z);
r.data[4] = 1 - 2 * x * x - 2 * z * z;
r.data[5] = 2 * (y * z - w * x);
r.data[6] = 2 * (x * z - w * y);
r.data[7] = 2 * (y * z + w * x);
r.data[8] = 1 - 2 * x * x - 2 * y * y;
return r;
}
旋轉矩陣轉四元數的代碼如下:
void Rotation::getQuaternion(double &x, double &y, double &z, double &w) const {
double epsilon = 1E-12;
double r11 = data[0];
double r12 = data[1];
double r13 = data[2];
double r21 = data[3];
double r22 = data[4];
double r23 = data[5];
double r31 = data[6];
double r32 = data[7];
double r33 = data[8];
w = sqrt(1.0f + r11 + r22 + r33) / 2.0f;
if (fabs(w) > epsilon) {
// compute w first.
x = (r32 - r23) / (4 * w);
y = (r13 - r31) / (4 * w);
z = (r21 - r12) / (4 * w);
} else {
if (r11 >= r22 && r11 >= r33) {
// compute x first
x = sqrt(1.0f + r11 - r22 - r33) / 2.0f;
w = (r32 - r23) / (4 * x);
y = (r21 + r12) / (4 * x);
z = (r31 + r13) / (4 * x);
} else if (r22 >= r11 && r22 >= r33) {
// compute y first
y = sqrt(1.0f - r11 + r22 - r33) / 2.0f;
w = (r13 - r31) / (4 * y);
x = (r21 + r12) / (4 * y);
z = (r23 + r32) / (4 * y);
} else {
// compute z first
z = sqrt(1.0f - r11 - r22 + r33) / 2.0f;
w = (r21 - r12) / (4 * z);
x = (r13 + r31) / (4 * z);
y = (r23 + r32) / (4 * z);
}
}
}
原理講清楚之後,代碼其實十分簡單,就是把公式套用一遍,這裡就不過多介紹了,旋轉矩陣與四元數的轉換相關C++源代碼我已經上傳到github: https://github.com/hitgavin/rosws/tree/master/src/frames,感興趣可以參考一下。
4. 總結 這篇文章主要介紹了旋轉矩陣與四元數之間的關系,下一篇文章我們将介紹姿态描述的另一種方法:軸角/旋轉向量。由于個人能力有限,所述内容難免存在疏漏,歡迎指出,歡迎讨論。