刚体的旋转

╰+哭是因爲堅強的太久メ 2021-12-18 16:57 296阅读 0赞

### 文章目录 ###

*   *  Eigen中的旋转操作
     *   *  旋转矩阵
         *  四元数
         *  欧拉角
         *  角轴
         *  变换矩阵
     *  各种旋转之间的转化
     *   *  四元数和角轴
         *  四元数和旋转矩阵
         *  四元数和欧拉角
         *  角轴和旋转矩阵
         *  角轴和欧拉角
         *  欧拉角和旋转矩阵

## Eigen中的旋转操作 ##

### 旋转矩阵 ###

*  构造

Eigen::Matrix3d rotation_matrix;
    rotation_matrix<< 0.707107,  -0.707107,  0,
             		  0.707107,   0.707107,  0,
             		  0,          0,         1;

--------------------

### 四元数 ###

*  构造

//直接构造
    Eigen::Quaterniond(1,0,0,0);
    
    // 从旋转矩阵
    Eigen::Matrix3d R; 
          R<< 0.707107,  -0.707107,  0,
              0.707107,   0.707107,  0,
              0,          0,         1;
    
    Eigen::Quaterniond q(R);
    
    
    // 从角轴
    Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1)); //绕z轴旋转45度
    Eigen::Quaterniond q(rotation_vector);
    
    //从数组, 数组的顺序应该是[x y z w]
    double array[4]= { 1,2,3,4};
    Eigen::Quaterniond q1(array); //相当于直接构造　q1(4,1,2,3);

*  读取系数

// 1. 
    q.coeffs()  // x, y, z, w 的顺序
    q.vec()　　　// x, y, z 虚数部分
    q.x();
    q.y();
    q.z();
    q.w();

*  其他常用操作

1.  归一化　`normalize()` :直接改原始的q，`normalized() const` ：返回一个归一化的q，原始不改变
2.  返回旋转矩阵`toRotationMatrix`
3.  取模 `norm()`
4.  模长的平方`squaredNorm()`
5.  取共轭`conjugate()`
6.  取逆 `inverse()`
7.  点乘`dot`

--------------------

### 欧拉角 ###

*  注意事项：

1.  常用的一种欧拉角顺序 yaw-pitch-roll, 所对应的坐标轴：Z, Y, X
2.  我们如果定义欧拉角：`Eigen::Vector3d　euler_angle(X, Y, Z)` ,　euler\_angle(0) : roll …

// 1. 可以从旋转矩阵得到欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // 旋转顺序z,y,x; 得到欧拉角(yaw, pitch roll)

--------------------

### 角轴 ###

*  注意事项：

1.  角轴的表示的轴必须是单位向量, 角是弧度制
2.  角轴的表示遵守右手定则，大拇指朝上，四指的方向是正
3.  角轴１(-M\_PI/4, 0, 0, 1) 和 角轴2(M\_PI/4, 0, 0, -1) 表示的是同一个旋转，绕z轴旋转-45度

*  构造

// 1. 从角轴
    Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0,0,1)); //绕z轴旋转45度, 注意，角度制：弧度，轴：必须是单位的
    
    // 2. 从四元数
    Eigen::Quaterniond q1(1,1,1,1);
    Eigen::AngleAxisd rotation_vector(q1);　// q1可以不是归一化的，自动取q1的归一化之后的q取构造
    
    // 3.从旋转矩阵
    Eigen::Matrix3d R;
         R<< 0.707107,  -0.707107,  0,
             0.707107,   0.707107,  0,
             0,          0,         1;
    Eigen::AngleAxisd rotation_vector(R);

*  其他常用操作

1.  转到旋转矩阵 `toRotationMatrix() const`
2.  取角度(弧度制)`angle()`
3.  取轴(单位向量)`axis()` 自动单位化输出
4.  相反的旋转`inverse() const`

### 变换矩阵 ###

*  构造

// 1. 可以从角轴和平移
        Eigen::AngleAxisd n(M_PI/4, Eigen::Vector3d(0,0,1));
        Eigen::Vector3d t(1,1,1);
        
    
    
        Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
        T.rotate(n);
        T.pretranslate(t);
    
    
    
    
    // 2. 可以从四元数和平移
        Eigen::AngleAxisd n(M_PI/4, Eigen::Vector3d(0,0,1));
        Eigen::Quaterniond q(n);
        Eigen::Vector3d t(1,1,1);
        
    
    
        Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
        T.rotate(q);
        T.pretranslate(t);
    
    
    
    
    // 3. 可以从旋转矩阵和平移
        Eigen::AngleAxisd n(M_PI/4, Eigen::Vector3d(0,0,1));
        Eigen::Matrix3d R = n.matrix();
        Eigen::Vector3d t(1,1,1);
        
    
    
        Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
        T.rotate(R);
        T.pretranslate(t);
    
    
    
    
    // 4. 除了创建好对象再传递　旋转　和平移之外，还可以以旋转创建，再传递平移,以下以旋转矩阵为例，其他两种一样可以
        Eigen::Isometry3d T(R);
        T.pretranslate(t);

*  常用操作

1.  `p = T*v` T内部重载了*符号，虽然T是四维的, v和p都是3维的，但是相当于p = R*v+t的操作
2.  `translation()` 取平移部分
3.  `rotation()`　取旋转矩阵

--------------------

## 各种旋转之间的转化 ##

### 四元数和角轴 ###

单位四元数： ( q 0 , q 1 , q 2 , q 3 ) (q\_0, q\_1, q\_2, q\_3) (q0,q1,q2,q3)   角轴： n θ n\\theta nθ

*  四元数到角轴，得到轴**是单位的**

θ = 2 a r c cos ⁡ q 0 \\theta=2arc \\cos q\_0 θ=2arccosq0

n = \[ n x , n y , n z \] T = \[ q 1 , q 2 , q 3 \] T / s i n θ 2 n= \[n\_x, n\_y, n\_z\]^T = \[q\_1, q\_2, q\_3\]^T/sin\{\\frac\{\\theta\}\{2\}\} n=\[nx,ny,nz\]T=\[q1,q2,q3\]T/sin2θ

*  角轴到四元数轴， n = ( n x , n y , n z ) n= (n\_x, n\_y, n\_z) n=(nx,ny,nz)是单位的，得到**四元数是单位的**

q = \[ q 0 , q 1 , q 2 , q 3 \] T = \[ cos ⁡ θ 2 , n s i n θ 2 , \] T = \[ cos ⁡ θ 2 , n x s i n θ 2 , n y s i n θ 2 n z s i n θ 2 \] T q = \[q\_0, q\_1, q\_2, q\_3\]^T = \[\\cos\{\\frac\{\\theta\}\{2\}\}, \\textbf\{n\}sin \{\\frac\{\\theta\}\{2\}\},\]^T=\[\\cos\{\\frac\{\\theta\}\{2\}\}, n\_x sin \{\\frac\{\\theta\}\{2\}\},n\_y sin \{\\frac\{\\theta\}\{2\}\}n\_z sin \{\\frac\{\\theta\}\{2\}\}\]^T q=\[q0,q1,q2,q3\]T=\[cos2θ,nsin2θ,\]T=\[cos2θ,nxsin2θ,nysin2θnzsin2θ\]T

--------------------

### 四元数和旋转矩阵 ###

单位四元数： ( q 0 , q 1 , q 2 , q 3 ) (q\_0, q\_1, q\_2, q\_3) (q0,q1,q2,q3)   旋转矩阵R：

*  四元数到旋转矩阵

R = \[ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 − 2 q 0 q 3 2 q 1 q 3 + 2 q 0 q 2 2 q 1 q 2 + 2 q 0 q 3 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 − 2 q 0 q 1 2 q 1 q 3 − 2 q 0 q 2 2 q 2 q 3 + 2 q 0 q 1 1 − 2 q 1 2 − 2 q 2 2 \] R= \\left\[ \\begin\{matrix\} 1-2q\_2^2-2q\_3^2 & 2q\_1q\_2-2q\_0q\_3 & 2q\_1q\_3+2q\_0q\_2 \\\\ 2q\_1q\_2+2q\_0q\_3 & 1-2q\_1^2-2q\_3^2 & 2q\_2q\_3-2q\_0q\_1 \\\\ 2q\_1q\_3-2q\_0q\_2 & 2q\_2q\_3+2q\_0q\_1 & 1-2q\_1^2-2q\_2^2 \\end\{matrix\} \\right\] R=⎣⎡1−2q22−2q322q1q2\+2q0q32q1q3−2q0q22q1q2−2q0q31−2q12−2q322q2q3\+2q0q12q1q3\+2q0q22q2q3−2q0q11−2q12−2q22⎦⎤

*  旋转矩阵到四元数  
    假设 R = m i j , i , j ∈ \[ 1 , 2 , 3 \] R=\{m\_\{ij\}\},i,j\\in\[1,2,3\] R=mij,i,j∈\[1,2,3\]

q 0 = t r ( R ) + 1 2    t r ( R ) 代 表 迹 q\_0= \\frac\{\\sqrt\{tr(R)+1\}\}\{2\} \\space\\space tr(R)代表迹 q0=2tr(R)\+1  tr(R)代表迹

q 1 = m 23 − m 32 4 q 0 q\_1 =\\frac\{m\_\{23\}-m\_\{32\}\}\{4q\_0\} q1=4q0m23−m32

q 2 = m 31 − m 13 4 q 0 q\_2 = \\frac\{m\_\{31\}-m\_\{13\}\}\{4q\_0\} q2=4q0m31−m13

q 3 = m 12 − m 21 4 q 0 q\_3 =\\frac\{m\_\{12\}-m\_\{21\}\}\{4q\_0\} q3=4q0m12−m21

注意：当 q 0 → 0 q\_0\\rightarrow 0 q0→0的时候，也就是R的迹趋近－１，其他三个分量很大，导致解不稳定，则应用下面的公式转化

1.  如果 m a x ( m 11 , m 22 , m 33 ) = m 11 max( m\_\{11\}, m\_\{22\}, m\_\{33\}) = m\_\{11\} max(m11,m22,m33)=m11

t = 1 + m 11 − m 22 − m 33    t= \\sqrt\{1+m\_\{11\}-m\_\{22\}-m\_\{33\}\}\\space\\space t=1\+m11−m22−m33

q 0 = m 32 − m 23 t q\_0 =\\frac\{m\_\{32\}-m\_\{23\}\}\{t\} q0=tm32−m23

q 1 = t 4 q\_1 =\\frac\{t\}\{4\} q1=4t

q 2 = m 31 + m 13 t q\_2 = \\frac\{m\_\{31\}+m\_\{13\}\}\{t\} q2=tm31\+m13

q 3 = m 12 + m 21 t q\_3 =\\frac\{m\_\{12\}+m\_\{21\}\}\{t\} q3=tm12\+m21

1.  如果 m a x ( m 11 , m 22 , m 33 ) = m 22 max( m\_\{11\}, m\_\{22\}, m\_\{33\}) = m\_\{22\} max(m11,m22,m33)=m22

t = 1 − m 11 + m 22 − m 33    t= \\sqrt\{1-m\_\{11\}+m\_\{22\}-m\_\{33\}\}\\space\\space t=1−m11\+m22−m33

q 0 = m 13 − m 31 t q\_0 =\\frac\{m\_\{13\}-m\_\{31\}\}\{t\} q0=tm13−m31

q 1 = m 12 + m 21 t q\_1 = \\frac\{m\_\{12\}+m\_\{21\}\}\{t\} q1=tm12\+m21

q 2 = t 4 q\_2 = \\frac\{t\}\{4\} q2=4t

q 3 = m 32 + m 23 t q\_3 =\\frac\{m\_\{32\}+m\_\{23\}\}\{t\} q3=tm32\+m23

1.  如果 m a x ( m 11 , m 22 , m 33 ) = m 33 max( m\_\{11\}, m\_\{22\}, m\_\{33\}) = m\_\{33\} max(m11,m22,m33)=m33

t = 1 − m 11 − m 22 + m 33    t= \\sqrt\{1-m\_\{11\}-m\_\{22\}+m\_\{33\}\}\\space\\space t=1−m11−m22\+m33

q 0 = m 21 − m 12 t q\_0 =\\frac\{m\_\{21\}-m\_\{12\}\}\{t\} q0=tm21−m12

q 1 = m 13 + m 31 t q\_1 =\\frac\{m\_\{13\}+m\_\{31\}\}\{t\} q1=tm13\+m31

q 2 = m 23 − m 32 t q\_2 = \\frac\{m\_\{23\}-m\_\{32\}\}\{t\} q2=tm23−m32

q 3 = t 4 q\_3 =\\frac\{t\}\{4\} q3=4t

--------------------

### 四元数和欧拉角 ###

单位四元数： ( q 0 , q 1 , q 2 , q 3 ) (q\_0, q\_1, q\_2, q\_3) (q0,q1,q2,q3)   　欧拉角： ( y a w , p i t c h , r o l l ) (yaw, pitch, roll) (yaw,pitch,roll), 分别为绕Z轴、Y轴、X轴的旋转角度，记作 ψ , θ , ϕ \\psi,\\theta, \\phi ψ,θ,ϕ

*  四元数到欧拉角

r o l l = ϕ = arctan ⁡ ( 2 ( q 0 q 1 + q 2 q 3 ) 1 − 2 ( q 1 2 + q 2 2 ) ) roll = \\phi = \\arctan(\\frac\{2(q\_0q\_1+q\_2q\_3)\}\{1-2(q\_1^2+q\_2^2)\}) roll=ϕ=arctan(1−2(q12\+q22)2(q0q1\+q2q3))

p i t c h = θ = arcsin ⁡ ( 2 ( q 0 q 2 − q 3 q 1 ) ) pitch = \\theta =\\arcsin(2(q\_0q\_2-q\_3q\_1)) pitch=θ=arcsin(2(q0q2−q3q1))

y a w = ψ = arctan ⁡ ( 2 ( q 0 q 3 + q 1 q 2 ) 1 − 2 ( q 2 2 + q 3 2 ) ) yaw = \\psi = \\arctan(\\frac\{2(q\_0q\_3+q\_1q\_2)\}\{1-2(q\_2^2+q\_3^2)\}) yaw=ψ=arctan(1−2(q22\+q32)2(q0q3\+q1q2))

*  欧拉角到四元数

q = \[ q 0 q 1 q 2 q 3 \] = \[ w x y z \] = \[ cos ⁡ ψ 2 cos ⁡ θ 2 cos ⁡ ϕ 2 + sin ⁡ ψ 2 sin ⁡ θ 2 sin ⁡ ϕ 2 cos ⁡ ψ 2 cos ⁡ θ 2 sin ⁡ ϕ 2 − sin ⁡ ψ 2 sin ⁡ θ 2 cos ⁡ ϕ 2 cos ⁡ ψ 2 sin ⁡ θ 2 cos ⁡ ϕ 2 + sin ⁡ ψ 2 cos ⁡ θ 2 sin ⁡ ϕ 2 sin ⁡ ψ 2 cos ⁡ θ 2 cos ⁡ ϕ 2 − cos ⁡ ψ 2 sin ⁡ θ 2 sin ⁡ ϕ 2 \] q=\\left\[ \\begin\{matrix\} q\_0 \\\\ q\_1 \\\\ q\_2\\\\ q\_3 \\end\{matrix\} \\right\]=\\left\[ \\begin\{matrix\} w \\\\ x \\\\ y\\\\ z \\end\{matrix\} \\right\]=\\left\[ \\begin\{matrix\} \\cos\\frac\{\\psi\}\{2\} \\cos\\frac\{\\theta\}\{2\} \\cos\\frac\{\\phi\}\{2\} +\\sin\\frac\{\\psi\}\{2\}\\sin\\frac\{\\theta\}\{2\}\\sin\\frac\{\\phi\}\{2\} \\\\ \\cos\\frac\{\\psi\}\{2\}\\cos\\frac\{\\theta\}\{2\}\\sin\\frac\{\\phi\}\{2\}-\\sin\\frac\{\\psi\}\{2\}\\sin\\frac\{\\theta\}\{2\} \\cos\\frac\{\\phi\}\{2\} \\\\ \\cos\\frac\{\\psi\}\{2\} \\sin\\frac\{\\theta\}\{2\} \\cos\\frac\{\\phi\}\{2\} +\\sin\\frac\{\\psi\}\{2\}\\cos\\frac\{\\theta\}\{2\}\\sin\\frac\{\\phi\}\{2\}\\\\ \\sin\\frac\{\\psi\}\{2\} \\cos\\frac\{\\theta\}\{2\} \\cos\\frac\{\\phi\}\{2\}- \\cos\\frac\{\\psi\}\{2\} \\sin\\frac\{\\theta\}\{2\} \\sin\\frac\{\\phi\}\{2\} \\end\{matrix\} \\right\] q=⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤=⎣⎢⎢⎡wxyz⎦⎥⎥⎤=⎣⎢⎢⎡cos2ψcos2θcos2ϕ\+sin2ψsin2θsin2ϕcos2ψcos2θsin2ϕ−sin2ψsin2θcos2ϕcos2ψsin2θcos2ϕ\+sin2ψcos2θsin2ϕsin2ψcos2θcos2ϕ−cos2ψsin2θsin2ϕ⎦⎥⎥⎤

--------------------

### 角轴和旋转矩阵 ###

旋转角 θ \\theta θ和单位方向轴 n = ( x , y , z ) n=(x, y, z) n=(x,y,z)

*  角轴到旋转矩阵

R = \[ cos ⁡ θ + x 2 ( 1 − cos ⁡ θ ) − z sin ⁡ θ + x y ( 1 − cos ⁡ θ ) y sin ⁡ θ + x z ( 1 − cos ⁡ θ ) z sin ⁡ θ + x y ( 1 − cos ⁡ θ ) cos ⁡ θ + y 2 ( 1 − cos ⁡ θ ) − x sin ⁡ θ + y z ( 1 − cos ⁡ θ ) − y sin ⁡ θ + x z ( 1 − cos ⁡ θ ) x sin ⁡ θ + y z ( 1 − cos ⁡ θ ) cos ⁡ θ + z 2 ( 1 − cos ⁡ θ ) \] R=\\left\[ \\begin\{matrix\} \\cos\\theta+x^2(1-\\cos\\theta) & -z\\sin\\theta+xy(1-\\cos\\theta) & y\\sin\\theta+xz(1-\\cos\\theta)\\\\ z\\sin\\theta+xy(1-\\cos\\theta) & \\cos\\theta+y^2(1-\\cos\\theta) & -x\\sin\\theta+yz(1-\\cos\\theta) \\\\ -y\\sin\\theta+xz(1-\\cos\\theta) & x\\sin\\theta+yz(1-\\cos\\theta) & \\cos\\theta +z^2(1-\\cos\\theta) \\end\{matrix\} \\right\] R=⎣⎡cosθ\+x2(1−cosθ)zsinθ\+xy(1−cosθ)−ysinθ\+xz(1−cosθ)−zsinθ\+xy(1−cosθ)cosθ\+y2(1−cosθ)xsinθ\+yz(1−cosθ)ysinθ\+xz(1−cosθ)−xsinθ\+yz(1−cosθ)cosθ\+z2(1−cosθ)⎦⎤

*  旋转矩阵到角轴  
     θ = arccos ⁡ ( t r ( R ) − 1 2 ) \\theta = \\arccos(\\frac\{tr(R)-1\}\{2\}) θ=arccos(2tr(R)−1)
    
     n = R n \\textbf\{n\} = R\\textbf\{n\} n=Rn  
    关于旋转轴 n \\textbf\{n\} n，由于旋转轴的向量在旋转后不会变化，即 n = R n n=Rn n=Rn，也说明n是矩阵R的特征值１对应的特征向量，求解此方程，再归一化即可。

--------------------

### 角轴和欧拉角 ###

*  角轴到欧拉角

*  欧拉角到角轴

--------------------

### 欧拉角和旋转矩阵 ###

欧拉角： ( y a w , p i t c h , r o l l ) (yaw, pitch, roll) (yaw,pitch,roll), 分别为绕Z轴、Y轴、X轴的旋转角度，记作 ψ , θ , ϕ \\psi,\\theta, \\phi ψ,θ,ϕ。Ｂ:Body系，Ｗ: word系

*  欧拉角到旋转矩阵

R b w = \[ cos ⁡ ψ cos ⁡ θ sin ⁡ ψ cos ⁡ θ − sin ⁡ θ cos ⁡ ψ sin ⁡ θ sin ⁡ ϕ − sin ⁡ ψ cos ⁡ ϕ sin ⁡ ψ sin ⁡ θ sin ⁡ ϕ + cos ⁡ ψ cos ⁡ ϕ cos ⁡ θ sin ⁡ ϕ cos ⁡ ψ sin ⁡ θ cos ⁡ ϕ + sin ⁡ ψ sin ⁡ ϕ sin ⁡ ψ sin ⁡ θ cos ⁡ ϕ − cos ⁡ ψ sin ⁡ ϕ cos ⁡ θ cos ⁡ ϕ \] R\_\{bw\}= \\left\[ \\begin\{matrix\} \\cos\\psi\\cos\\theta & \\sin\\psi\\cos\\theta & -\\sin\\theta \\\\ \\cos\\psi\\sin\\theta\\sin\\phi-\\sin\\psi\\cos\\phi & \\sin\\psi\\sin\\theta\\sin\\phi +\\cos\\psi\\cos\\phi& \\cos\\theta\\sin\\phi\\\\ \\cos\\psi\\sin\\theta\\cos\\phi+\\sin\\psi\\sin\\phi& \\sin\\psi\\sin\\theta\\cos\\phi-\\cos\\psi\\sin\\phi & \\cos\\theta\\cos\\phi \\end\{matrix\} \\right\] Rbw=⎣⎡cosψcosθcosψsinθsinϕ−sinψcosϕcosψsinθcosϕ\+sinψsinϕsinψcosθsinψsinθsinϕ\+cosψcosϕsinψsinθcosϕ−cosψsinϕ−sinθcosθsinϕcosθcosϕ⎦⎤

R w b = R b w T = \[ cos ⁡ ψ cos ⁡ θ cos ⁡ ψ sin ⁡ θ sin ⁡ ϕ − sin ⁡ ψ cos ⁡ ϕ cos ⁡ ψ sin ⁡ θ cos ⁡ ϕ + sin ⁡ ψ sin ⁡ ϕ sin ⁡ ψ cos ⁡ ϕ sin ⁡ ψ sin ⁡ θ sin ⁡ ϕ + cos ⁡ ψ cos ⁡ ϕ sin ⁡ ψ sin ⁡ θ cos ⁡ ϕ − cos ⁡ ψ sin ⁡ ϕ − sin ⁡ θ cos ⁡ θ sin ⁡ ϕ cos ⁡ θ cos ⁡ ϕ \] R\_\{wb\}=R\_\{bw\}^T= \\left\[ \\begin\{matrix\} \\cos\\psi\\cos\\theta & \\cos\\psi\\sin\\theta\\sin\\phi-\\sin\\psi\\cos\\phi & \\cos\\psi\\sin\\theta\\cos\\phi+\\sin\\psi\\sin\\phi \\\\ \\sin\\psi\\cos\\phi & \\sin\\psi\\sin\\theta\\sin\\phi+\\cos\\psi\\cos\\phi & \\sin\\psi\\sin\\theta\\cos\\phi-\\cos\\psi\\sin\\phi \\\\ -\\sin\\theta & \\cos\\theta\\sin\\phi & \\cos\\theta\\cos\\phi \\end\{matrix\} \\right\] Rwb=RbwT=⎣⎡cosψcosθsinψcosϕ−sinθcosψsinθsinϕ−sinψcosϕsinψsinθsinϕ\+cosψcosϕcosθsinϕcosψsinθcosϕ\+sinψsinϕsinψsinθcosϕ−cosψsinϕcosθcosϕ⎦⎤

*  旋转矩阵到欧拉角  
    假设 R = m i j , i , j ∈ \[ 1 , 2 , 3 \] R=\{m\_\{ij\}\},i,j\\in\[1,2,3\] R=mij,i,j∈\[1,2,3\]

1.  正常情况：  
     r o l l = ϕ = arctan ⁡ ( m 32 m 33 ) roll = \\phi = \\arctan(\\frac\{m\_\{32\}\}\{m\_\{33\}\}) roll=ϕ=arctan(m33m32)

p i t c h = θ = arctan ⁡ ( − m 31 m 32 2 + m 33 2 ) pitch = \\theta = \\arctan(\\frac\{-m\_\{31\}\}\{\\sqrt\{m\_\{32\}^2+m\_\{33\}^2\}\}) pitch=θ=arctan(m322\+m332−m31)

y a w = ψ = arctan ⁡ ( m 21 m 11 ) yaw = \\psi = \\arctan(\\frac\{m\_\{21\}\}\{m\_\{11\}\}) yaw=ψ=arctan(m11m21)

1.  如果