600字范文 > 四元数罗德里格斯公式欧拉角旋转矩阵推导和资料

四元数罗德里格斯公式欧拉角旋转矩阵推导和资料

时间：2023-03-31 00:57:03

写在前面

1、本文内容

四元数、罗德里格斯公式、欧拉角、旋转矩阵推导和资料

2、转载请注明出处：

/qq_41102371/article/details/126002167

资料

四元数

Understanding Quaternions 中文翻译《理解四元数》 /understanding-quaternions/

http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf

几种三维空间旋转的表达方式转换

三维旋转：欧拉角、四元数、旋转矩阵、轴角之间的转换 /p/45404840

彻底搞懂“旋转矩阵/欧拉角/四元数”，让你体会三维旋转之美 /weixin_45590473/article/details/122884112

展示欧拉角与四元数动态变换关系的网站

https://quaternions.online/

罗德里格斯公式

/wiki/Rodrigues’_rotation_formula

罗德里格斯公式（Rodrigues Formula） /weixin_40215443/article/details/123950141

二维xy坐标旋转 /qq_41102371/article/details/116245483#t4

推导

先放这，有空来推一遍

几种表达方式

K K K是 k k k的叉乘矩阵，即 K = k ∧ \mathbf{K}=\mathbf{k}^{\land} K=k∧,( k \mathbf{k} k的叉乘矩阵也可表示为 k × \mathbf{k}_{\times} k×)

1、

R = I + ( 1 − cos ⁡ θ ) K 2 + ( sin ⁡ θ ) K = I + ( 1 − cos ⁡ θ ) [ k ∧ ] 2 + ( sin ⁡ θ ) k ∧ \begin{aligned} \mathbf{R}&=\mathbf{I}+(1-\cos\theta)\mathbf{K}^2+(\sin\theta)\mathbf{K}\\ &=\mathbf{I}+(1-\cos\theta)[\mathbf{k}^{\land}]^2+(\sin\theta)\mathbf{k}^{\land}\\ \end{aligned} R=I+(1−cosθ)K2+(sinθ)K=I+(1−cosθ)[k∧]2+(sinθ)k∧

2、

由 K 2 = k ∧ k ∧ = k k T − I \mathbf{K}^2=\mathbf{k}^{\land}\mathbf{k}^{\land}=\mathbf{k}\mathbf{k}^T-\mathbf{I} K2=k∧k∧=kkT−I

可得

R = I + ( 1 − cos ⁡ θ ) ( k k T − I ) + ( sin ⁡ θ ) k ∧ = cos ⁡ θ I + ( 1 − cos ⁡ θ ) k k T + ( sin ⁡ θ ) k ∧ \begin{aligned} \mathbf{R}&=\mathbf{I}+(1-\cos\theta)(\mathbf{k}\mathbf{k}^T-\mathbf{I})+(\sin\theta)\mathbf{k}^{\land}\\ &=\cos\theta\mathbf{I}+(1-\cos\theta)\mathbf{k}\mathbf{k}^T+(\sin\theta)\mathbf{k}^{\land} \end{aligned} R=I+(1−cosθ)(kkT−I)+(sinθ)k∧=cosθI+(1−cosθ)kkT+(sinθ)k∧

3、

高博的14讲中推导了指数映射

R = e x p ( θ k ∧ ) = cos ⁡ θ I + ( 1 − cos ⁡ θ ) k k T + ( sin ⁡ θ ) k ∧ \mathbf{R}=exp(\theta \mathbf{k}^{\land})=\cos\theta\mathbf{I}+(1-\cos\theta)\mathbf{k}\mathbf{k}^T+(\sin\theta)\mathbf{k}^{\land} R=exp(θk∧)=cosθI+(1−cosθ)kkT+(sinθ)k∧

四元数转旋转矩阵

根据http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf，四元数转旋转矩阵为：

G L C ( q ˉ ) = ( 2 q 4 2 − 1 ) I 3 × 3 − 2 q 4 [ q × ] + 2 q q T = ( 2 q 4 2 − 1 ) I 3 × 3 − 2 q 4 [ q × ] + 2 [ q × ] 2 + 2 ∣ q ∣ 2 ⋅ I 3 × 3 = ( 2 q 4 2 + 2 ∣ q ∣ 2 − 1 ) I 3 × 3 − 2 q 4 [ q × ] + 2 [ q × ] 2 = ( 2 q 4 2 + 2 ( q 1 2 + q 2 2 + q 3 2 ) − 1 ) I 3 × 3 − 2 q 4 [ q × ] + 2 [ q × ] 2 = ( 2 − 1 ) I 3 × 3 − 2 q 4 [ q × ] + 2 [ q × ] 2 = I 3 × 3 − 2 q 4 [ q × ] + 2 [ q × ] 2 \begin{aligned} {}_G^L\mathbf{C}(\bar{q})&=(2q_4^2-1)\mathbf{I}_{3\times3}-2q_4[\mathbf{q}_{\times}]+2\mathbf{q}\mathbf{q}^T\\ &=(2q_4^2-1)\mathbf{I}_{3\times3}-2q_4[\mathbf{q}_{\times}]+2[\mathbf{q}_{\times}]^2+2|\mathbf{q}|^2\cdot \mathbf{I}_{3\times3}\\ &=(2q_4^2+2|\mathbf{q}|^2-1)\mathbf{I}_{3\times3}-2q_4[\mathbf{q}_{\times}]+2[\mathbf{q}_{\times}]^2\\ &=(2q_4^2+2(q_1^2+q_2^2+q_3^2)-1)\mathbf{I}_{3\times3}-2q_4[\mathbf{q}_{\times}]+2[\mathbf{q}_{\times}]^2\\ &=(2-1)\mathbf{I}_{3\times3}-2q_4[\mathbf{q}_{\times}]+2[\mathbf{q}_{\times}]^2\\ &=\mathbf{I}_{3\times3}-2q_4[\mathbf{q}_{\times}]+2[\mathbf{q}_{\times}]^2 \end{aligned} GLC(qˉ)=(2q42−1)I3×3−2q4[q×]+2qqT=(2q42−1)I3×3−2q4[q×]+2[q×]2+2∣q∣2⋅I3×3=(2q42+2∣q∣2−1)I3×3−2q4[q×]+2[q×]2=(2q42+2(q12+q22+q32)−1)I3×3−2q4[q×]+2[q×]2=(2−1)I3×3−2q4[q×]+2[q×]2=I3×3−2q4[q×]+2[q×]2

其中 q ˉ = [ q q 4 ] = [ q 1 , q 2 , q 3 , q 4 ] T \bar{q}=\begin{bmatrix}\mathbf{q}\\q_4\end{bmatrix}=[q_1,q_2,q_3,q_4]^T qˉ=[qq4]=[q1,q2,q3,q4]T， q = [ q 1 , q 2 , q 3 ] T \mathbf{q}=[q_1,q_2,q_3]^T q=[q1,q2,q3]T， q 1 2 + q 2 2 + q 3 2 + q 4 2 = 1 q_1^2+q_2^2+q_3^2+q_4^2=1 q12+q22+q32+q42=1

则

G L ⁣ R = [ 1 − 2 q 2 2 − 2 q 3 2 2 ( q 1 q 2 + q 3 q 4 ) 2 ( q 1 q 3 − q 2 q 4 ) 2 ( q 1 q 2 − q 3 q 4 ) 1 − 2 q 1 2 − 2 q 3 2 2 ( q 2 q 3 + q 1 q 4 ) 2 ( q 1 q 3 + q 2 q 4 ) 2 ( q 2 q 3 − q 1 q 4 ) 1 − 2 q 1 2 − 2 q 2 2 ] {}_G^L\!R= \begin{bmatrix} 1-2q_2^2-2q_3^2& 2(q_1q_2+q_3q_4) & 2(q_1q_3-q_2q_4) \\ 2(q_1q_2-q_3q_4) & 1-2q_1^2-2q_3^2 & 2(q_2q_3+q_1q_4) \\ 2(q_1q_3+q_2q_4)& 2(q_2q_3-q_1q_4) & 1-2q_1^2-2q_2^2 \end{bmatrix} GLR=⎣ ⎡1−2q22−2q322(q1q2−q3q4)2(q1q3+q2q4)2(q1q2+q3q4)1−2q12−2q322(q2q3−q1q4)2(q1q3−q2q4)2(q2q3+q1q4)1−2q12−2q22⎦ ⎤

即将世界坐标系下的点 P G P^G PG旋转到局部坐标系下的点 P L P^L PL

P L = G L ⁣ R P G P^L={}_G^L\!R P^G PL=GLRPG

而

三维旋转：欧拉角、四元数、旋转矩阵、轴角之间的转换 /p/45404840中是将局部坐标系下的点 P L P^L PL转换到世界坐标系下的 P G P^G PG：

L G ⁣ R = [ 1 − 2 q 2 2 − 2 q 3 2 2 ( q 1 q 2 − q 3 q 4 ) 2 ( q 1 q 3 + q 2 q 4 ) 2 ( q 1 q 2 + q 3 q 4 ) 1 − 2 q 1 2 − 2 q 3 2 2 ( q 2 q 3 − q 1 q 4 ) 2 ( q 1 q 3 − q 2 q 4 ) 2 ( q 2 q 3 + q 1 q 4 ) 1 − 2 q 1 2 − 2 q 2 2 ] {}_L^G\!R= \begin{bmatrix} 1-2q_2^2-2q_3^2& 2(q_1q_2-q_3q_4) & 2(q_1q_3+q_2q_4) \\ 2(q_1q_2+q_3q_4) & 1-2q_1^2-2q_3^2 & 2(q_2q_3-q_1q_4) \\ 2(q_1q_3-q_2q_4)& 2(q_2q_3+q_1q_4) & 1-2q_1^2-2q_2^2 \end{bmatrix} LGR=⎣ ⎡1−2q22−2q322(q1q2+q3q4)2(q1q3−q2q4)2(q1q2−q3q4)1−2q12−2q322(q2q3+q1q4)2(q1q3+q2q4)2(q2q3−q1q4)1−2q12−2q22⎦ ⎤

P G = L G ⁣ R P L P^G={}_L^G\!R P^L PG=LGRPL

这正好满足：

G L ⁣ R = L G ⁣ R T ( G L ⁣ R = L G ⁣ R − 1 ) {}_G^L\!R ={{}_L^G\!R}^T({}_G^L\!R ={{}_L^G\!R}^{-1}) GLR=LGRT(GLR=LGR−1)

近似

根据http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf原文当 δ q ˉ \delta\bar{q} δqˉ非常小时

以下为个人理解的推导

G L C ( q ˉ ) = ( 2 q 4 2 − 1 ) I 3 × 3 − 2 q 4 [ q × ] + 2 q q T ≈ ( 2 × 1 2 − 1 ) I 3 × 3 − 2 × 1 [ 1 2 δ θ × ] + 2 × 0 3 × 3 = I 3 × 3 − [ δ θ × ] \begin{aligned} {}_G^L\mathbf{C}(\bar{q})&=(2q_4^2-1)\mathbf{I}_{3\times3}-2q_4[\mathbf{q}_{\times}]+2qq^T\\ &\approx (2\times1^2-1)\mathbf{I}_{3\times3}-2\times 1[\frac{1}{2}\delta \mathbf{\theta}_{\times}]+2\times0_{3\times3}\\ &=\mathbf{I}_{3\times3}-[\delta \mathbf{\theta}_{\times}] \end{aligned} GLC(qˉ)=(2q42−1)I3×3−2q4[q×]+2qqT≈(2×12−1)I3×3−2×1[21δθ×]+2×03×3=I3×3−[δθ×]

其中 q q T ≈ 0 3 × 3 qq^T\approx0_{3\times3} qqT≈03×3

四元数与轴角

还是参考http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf，

q ˉ = [ q q 4 ] = [ q 1 q 2 q 3 q 4 ] = [ k x sin ⁡ ( θ / 2 ) k y sin ⁡ ( θ / 2 ) k z sin ⁡ ( θ / 2 ) cos ⁡ ( θ / 2 ) ] = [ k ^ sin ⁡ ( θ / 2 ) cos ⁡ ( θ / 2 ) ] , k ^ = [ k x , k y , k z ] T , q 4 = cos ⁡ ( θ / 2 ) \bar{q}=\begin{bmatrix}\mathbf{q}\\q_4\end{bmatrix}=\begin{bmatrix}q_1\\q_2\\q_3\\q_4\end{bmatrix}=\begin{bmatrix}k_x\sin(\theta/2)\\k_y\sin(\theta/2)\\k_z\sin(\theta/2)\\\cos(\theta/2)\end{bmatrix}=\begin{bmatrix}\hat{\mathbf{k}}\sin(\theta/2)\\\cos(\theta/2)\end{bmatrix},\hat{\mathbf{k}}=[k_x,k_y,k_z]^T,q_4=\cos(\theta/2) qˉ=[qq4]=⎣ ⎡q1q2q3q4⎦ ⎤=⎣ ⎡kxsin(θ/2)kysin(θ/2)kzsin(θ/2)cos(θ/2)⎦ ⎤=[k^sin(θ/2)cos(θ/2)],k^=[kx,ky,kz]T,q4=cos(θ/2)

转换代码

欧拉角转旋转矩阵

#include <Eigen/Core>#include <Eigen/Dense>#include <iostream>#define PI 3.1415926int main(int argc, char* argv[]){std::cout<<PI<<std::endl;if(argc<4){std::cout<<"please input a 3x1 vector,for example:\neuler2rt 45 30 60"<<std::endl;return 0; }Eigen::Vector3d eulerAngle(atof(argv[1]),atof(argv[2]),atof(argv[3]));std::cout<<"eulerAngle:\nx: "<<eulerAngle[0]<<" y: "<<eulerAngle[1]<<" z: "<<eulerAngle[2]<<std::endl;eulerAngle=eulerAngle/180*PI;Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle[0],Eigen::Vector3d::UnitX()));Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle[1],Eigen::Vector3d::UnitY()));Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle[2],Eigen::Vector3d::UnitZ()));rotation_matrix=rollAngle*pitchAngle*yawAngle;std::cout<<"rotation_matrix:\n"<<rotation_matrix<<std::endl;Eigen::Vector3d eulerAngle2=rotation_matrix.eulerAngles(0,1,2);std::cout<<"eulerAngle:\nx: "<<eulerAngle2[0]/PI*180<<" y: "<<eulerAngle2[1]/PI*180<<" z: "<<eulerAngle2[2]/PI*180<<std::endl;return 0;}

使用

erluer2rt 30 45 60

旋转矩阵转欧拉角和四元数

#include <Eigen/Core>#include <Eigen/Dense>#include <iostream>#include <fstream>#define PI 3.1415926int main(int argc, char* argv[]){if(argc<2){std::cout<<"please input a file including a 3x3 matrix"<<std::endl;return 0; }std::string filename;filename=argv[1]; std::ifstream fin;fin.open(filename,std::ios::in);if(!fin.is_open()){std::cout<<"read file "<<filename<<" failed."<<std::endl;return 0;}Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();for (std::size_t i = 0; i < 3; ++i) {for (std::size_t j = 0; j < 3; ++j) {double value;fin >> value;// std::cout<<value<<" ";rotation_matrix(i, j) = value;}}std::cout<<std::endl;fin.close();std::cout<<"rotation_matrix:\n"<<rotation_matrix<<std::endl;Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(0,1,2);// std::cout<<eulerAngle<<std::endl;std::cout<<"eulerAngle:\nx: "<<eulerAngle[0]/PI*180<<" y: "<<eulerAngle[1]/PI*180<<" z: "<<eulerAngle[2]/PI*180<<std::endl;//转四元数Eigen::Quaterniond rotation_q(rotation_matrix);std::cout<<"rotation_q:\nx: "<<rotation_q.x()<<" y:"<<rotation_q.y()<<" z:"<<rotation_q.z()<<" w:"<<rotation_q.w()<<" "<<std::endl;return 0;}

使用

rt2euler r_3x3.txt

其中r_3x3.txt格式如下

1 0 00 1 00 0 1