本文仅供学习使用，总结很多本现有讲述运动学或动力学书籍后的总结，从矢量的角度进行分析，方法比较传统，但更易理解，并且现有的看似抽象方法，两者本质上并无不同。
2024年底本人学位论文发表后方可摘抄
若有帮助请引用
本文参考：
.
食用方法如何表达刚体在空间中的位置与姿态
姿态参数如何表达？不同表达方式直接的转换关系？
旋转矩阵？转换矩阵？有什么意义和性质？转置代表什么？
如何表示连续变换？------与RPY有关
齐次坐标的意义------简化公式？
务必自己推导全部公式，并理解每个符号的含义
机构运动学与动力学分析与建模 Ch00-3 刚体的位形 Configuration of Rigid Body Part1
- [3. 转换矩阵与旋转矩阵------刚体的位置与姿态描述](#3. 转换矩阵与旋转矩阵——刚体的位置与姿态描述)
-
- [3.1 轴角变换](#3.1 轴角变换)
- [3.2 罗德里格变换Rodrigues' Transform](#3.2 罗德里格变换Rodrigues’ Transform)
- [3.3 方向余弦变换](#3.3 方向余弦变换)
刚体的位形可以用六个独立（坐标）参数完全描述 ：三个位置参数 用于描述运动刚体上运动坐标系 { M } \left\{ M \right\} {M}原点 M M M在固定坐标系 { F } \left\{ F \right\} {F}的投影参数，三个转动参数 用于描述运动坐标系 { M } \left\{ M \right\} {M}的基矢量相对于固定坐标系 { F } \left\{ F \right\} {F}的基矢量的姿态，而描述这种姿态的变换，则是需要确定矩阵 [ Q M F ] \left[ Q_{\mathrm{M}}^{F} \right] [QMF]。
因此为描述空间坐标系中任意一刚体的运动状态，首先需要描述刚体的位置矢量 R ⃗ M F \vec{R}{\mathrm{M}}^{F} R MF与姿态矩阵 [ Q M F ] \left[ Q{\mathrm{M}}^{F} \right] [QMF]
广义参考系坐标 Reference Coordinates：为方便后续动力学方程的建立与推导，常用广义坐标矢量参数 q ⃗ M F \vec{q}{\mathrm{M}}^{F} q MF来描述运动刚体的形位，其中：
q ⃗ M F = [ R ⃗ M F , θ ⃗ M F ] \vec{q}{\mathrm{M}}^{F}=\left[ \vec{R}{\mathrm{M}}^{F},\vec{\theta}{\mathrm{M}}^{F} \right] q MF=[R MF,θ MF]- θ ⃗ M F \vec{\theta}{\mathrm{M}}^{F} θ MF可以用多种方法来描述（通常包含3或4个角度参数 ），这些角度参数用于描述矩阵 [ Q M F ] \left[ Q{\mathrm{M}}^{F} \right] [QMF]
对于刚体的运动状态而言，其运动坐标系的原点 M M M的位置矢量 R ⃗ M F \vec{R}_{\mathrm{M}}^{F} R MF表示与点的运动状态表示相同，因此需要探究如何用角度参数来描述转换矩阵。
3. 转换矩阵与旋转矩阵------刚体的位置与姿态描述
转换矩阵用于表述两个坐标系 { A : ( i ⃗ A , j ⃗ A , k ⃗ A ) } \left\{ A:\left( \vec{i}^A,\vec{j}^A,\vec{k}^A \right) \right\} {A:(i A,j A,k A)} 与 { B : ( i ⃗ B , j ⃗ B , k ⃗ B ) } \left\{ B:\left( \vec{i}^B,\vec{j}^B,\vec{k}^B \right) \right\} {B:(i B,j B,k B)}的基矢量之间的转换关系：
i ⃗ B j ⃗ B k ⃗ B \] = \[ Q B A \] T \[ i ⃗ A j ⃗ A k ⃗ A \] \\left\[ \\begin{array}{c} \\vec{i}\^B\\\\ \\vec{j}\^B\\\\ \\vec{k}\^B\\\\ \\end{array} \\right\] =\\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \^{\\mathrm{T}}\\left\[ \\begin{array}{c} \\vec{i}\^A\\\\ \\vec{j}\^A\\\\ \\vec{k}\^A\\\\ \\end{array} \\right\] i Bj Bk B =\[QBA\]T i Aj Ak A 其中，转换矩阵 \[ Q B A \] T \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \^{\\mathrm{T}} \[QBA\]T表示坐标系 { B } \\left\\{ B \\right\\} {B}的基矢量在坐标系 { A } \\left\\{ A \\right\\} {A}中的表达，可将向量在不同的基矢量坐标系下进行表示。特殊的：若将基矢量替换成对应基矢量的向量投影，则可以表示为：两个原点重合的坐标系中，对同一向量的不同表达的转换关系； 上式也可以理解为：对坐标系 { A : ( i ⃗ A , j ⃗ A , k ⃗ A ) } \\left\\{ A:\\left( \\vec{i}\^A,\\vec{j}\^A,\\vec{k}\^A \\right) \\right\\} {A:(i A,j A,k A)}进行了 \[ Q B A \] \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \[QBA\]的旋转，此时将转换矩阵与向量的运算理解为张量与向量的运算，即得到了旋转后的向量在坐标系 { A } \\left\\{ A \\right\\} {A}中的表达，此时实际上，对原始坐标系 { A } \\left\\{ A \\right\\} {A}的基矢量同样进行了旋转，形成了新坐标系 { B } \\left\\{ B \\right\\} {B}的基矢量，其仍在坐标系 { A } \\left\\{ A \\right\\} {A}下表达。 \[ r 1 A ′ r 2 A ′ r 3 A ′ \] = \[ Q B A \] \[ r 1 A r 2 A r 3 A \] \\left\[ \\begin{array}{c} {r_{1}\^{A}}\^{\\prime}\\\\ {r_{2}\^{A}}\^{\\prime}\\\\ {r_{3}\^{A}}\^{\\prime}\\\\ \\end{array} \\right\] =\\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \\left\[ \\begin{array}{c} r_{1}\^{A}\\\\ r_{2}\^{A}\\\\ r_{3}\^{A}\\\\ \\end{array} \\right\] r1A′r2A′r3A′ =\[QBA\] r1Ar2Ar3A 目前，人们采用不同的角度参数 θ ⃗ \\vec{\\theta} θ 来对旋转矩阵进行描述 > * **Representing an orientation ------ from definition** > 将原矢量进行旋转变换，得到该坐标系下新矢量的坐标投影参数： > R ⃗ p ′ F = \[ Q B A \] R ⃗ p F \\vec{R}_{\\mathrm{p}\^{\\prime}}\^{F}=\\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \\vec{R}_{\\mathrm{p}}\^{F} R p′F=\[QBA\]R pF > * **Changing the reference frame** > 对坐标系进行转换，基于坐标系 { B } \\left\\{ B \\right\\} {B}中的该矢量的坐标投影参数 R ⃗ p B \\vec{R}_{\\mathrm{p}}\^{B} R pB，得到该矢量在坐标系 { A } \\left\\{ A \\right\\} {A}中的坐标投影参数 R ⃗ p A \\vec{R}_{\\mathrm{p}}\^{A} R pA： > R ⃗ p A = \[ Q B A \] R ⃗ p B \\vec{R}_{\\mathrm{p}}\^{A}=\\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \\vec{R}_{\\mathrm{p}}\^{B} R pA=\[QBA\]R pB ### 3.1 轴角变换  **假设两个坐标系 { A } \\left\\{ A \\right\\} {A}与 { B } \\left\\{ B \\right\\} {B}的原点重合** ，其中坐标系 { B } \\left\\{ B \\right\\} {B}为坐标系 { A } \\left\\{ A \\right\\} {A}绕轴 v ⃗ F \\vec{v}\^F v F（单位向量）旋转 θ \\theta θ所得到的。因此对于坐标系 { A } \\left\\{ A \\right\\} {A}中的点 P P P，经过转换后，得到点 P ′ P\^{\\prime} P′，此时点 P ′ P\^{\\prime} P′在坐标系 { B } \\left\\{ B \\right\\} {B}中的矢量投影与点 P P P在坐标系 { A } \\left\\{ A \\right\\} {A}中的投影分量相同。而在转换过程中，点 P ′ P\^{\\prime} P′在坐标系 { A } \\left\\{ A \\right\\} {A}中的表达发生变化，即有： \[ P ′ 1 B , P ′ 2 B , P ′ 2 B \] = \[ P 1 A , P 2 A , P 2 A \] \\left\[ {P\^{\\prime}}_{1}\^{\\mathrm{B}},{P\^{\\prime}}_{2}\^{\\mathrm{B}},{P\^{\\prime}}_{2}\^{\\mathrm{B}} \\right\] =\\left\[ P_{1}\^{A},P_{2}\^{A},P_{2}\^{A} \\right\] \[P′1B,P′2B,P′2B\]=\[P1A,P2A,P2A\]，因此对式 \[ i ⃗ B j ⃗ B k ⃗ B \] = \[ Q B A \] T \[ i ⃗ A j ⃗ A k ⃗ A \] \\left\[ \\begin{array}{c} \\vec{i}\^B\\\\ \\vec{j}\^B\\\\ \\vec{k}\^B\\\\ \\end{array} \\right\] =\\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \^{\\mathrm{T}}\\left\[ \\begin{array}{c} \\vec{i}\^A\\\\ \\vec{j}\^A\\\\ \\vec{k}\^A\\\\ \\end{array} \\right\] i Bj Bk B =\[QBA\]T i Aj Ak A 有： \[ i ⃗ B j ⃗ B k ⃗ B \] T \[ P 1 B P 2 B P 3 B \] = \[ i ⃗ A j ⃗ A k ⃗ A \] T \[ P 1 A P 2 A P 3 A \] ⇒ ( \[ Q B A \] T \[ i ⃗ A j ⃗ A k ⃗ A \] ) T \[ P 1 B P 2 B P 3 B \] = \[ i ⃗ A j ⃗ A k ⃗ A \] T \[ P 1 A P 2 A P 3 A \] ⇒ \[ i ⃗ A j ⃗ A k ⃗ A \] T \[ Q B A \] \[ P 1 B P 2 B P 3 B \] = \[ i ⃗ A j ⃗ A k ⃗ A \] T \[ P 1 A P 2 A P 3 A \] ⇒ \[ Q B A \] \[ P 1 B P 2 B P 3 B \] = \[ P 1 A P 2 A P 3 A \] = \[ P ′ 1 B P ′ 2 B P ′ 3 B \] \\begin{split} \&\\left\[ \\begin{array}{c} \\vec{i}\^B\\\\ \\vec{j}\^B\\\\ \\vec{k}\^B\\\\ \\end{array} \\right\] \^{\\mathrm{T}}\\left\[ \\begin{array}{c} P_{1}\^{\\mathrm{B}}\\\\ P_{2}\^{\\mathrm{B}}\\\\ P_{3}\^{\\mathrm{B}}\\\\ \\end{array} \\right\] =\\left\[ \\begin{array}{c} \\vec{i}\^A\\\\ \\vec{j}\^A\\\\ \\vec{k}\^A\\\\ \\end{array} \\right\] \^{\\mathrm{T}}\\left\[ \\begin{array}{c} P_{1}\^{A}\\\\ P_{2}\^{A}\\\\ P_{3}\^{A}\\\\ \\end{array} \\right\] \\\\ \&\\Rightarrow \\left( \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \^{\\mathrm{T}}\\left\[ \\begin{array}{c} \\vec{i}\^A\\\\ \\vec{j}\^A\\\\ \\vec{k}\^A\\\\ \\end{array} \\right\] \\right) \^{\\mathrm{T}}\\left\[ \\begin{array}{c} P_{1}\^{\\mathrm{B}}\\\\ P_{2}\^{\\mathrm{B}}\\\\ P_{3}\^{\\mathrm{B}}\\\\ \\end{array} \\right\] =\\left\[ \\begin{array}{c} \\vec{i}\^A\\\\ \\vec{j}\^A\\\\ \\vec{k}\^A\\\\ \\end{array} \\right\] \^{\\mathrm{T}}\\left\[ \\begin{array}{c} P_{1}\^{A}\\\\ P_{2}\^{A}\\\\ P_{3}\^{A}\\\\ \\end{array} \\right\] \\\\ \&\\Rightarrow \\left\[ \\begin{array}{c} \\vec{i}\^A\\\\ \\vec{j}\^A\\\\ \\vec{k}\^A\\\\ \\end{array} \\right\] \^{\\mathrm{T}}\\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \\left\[ \\begin{array}{c} P_{1}\^{\\mathrm{B}}\\\\ P_{2}\^{\\mathrm{B}}\\\\ P_{3}\^{\\mathrm{B}}\\\\ \\end{array} \\right\] =\\left\[ \\begin{array}{c} \\vec{i}\^A\\\\ \\vec{j}\^A\\\\ \\vec{k}\^A\\\\ \\end{array} \\right\] \^{\\mathrm{T}}\\left\[ \\begin{array}{c} P_{1}\^{A}\\\\ P_{2}\^{A}\\\\ P_{3}\^{A}\\\\ \\end{array} \\right\] \\\\ \&\\Rightarrow \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \\left\[ \\begin{array}{c} P_{1}\^{\\mathrm{B}}\\\\ P_{2}\^{\\mathrm{B}}\\\\ P_{3}\^{\\mathrm{B}}\\\\ \\end{array} \\right\] =\\left\[ \\begin{array}{c} P_{1}\^{A}\\\\ P_{2}\^{A}\\\\ P_{3}\^{A}\\\\ \\end{array} \\right\] =\\left\[ \\begin{array}{c} {P\^{\\prime}}_{1}\^{\\mathrm{B}}\\\\ {P\^{\\prime}}_{2}\^{\\mathrm{B}}\\\\ {P\^{\\prime}}_{3}\^{\\mathrm{B}}\\\\ \\end{array} \\right\] \\end{split} i Bj Bk B T P1BP2BP3B = i Aj Ak A T P1AP2AP3A ⇒ \[QBA\]T i Aj Ak A T P1BP2BP3B = i Aj Ak A T P1AP2AP3A ⇒ i Aj Ak A T\[QBA\] P1BP2BP3B = i Aj Ak A T P1AP2AP3A ⇒\[QBA\] P1BP2BP3B = P1AP2AP3A = P′1BP′2BP′3B 上式写明：坐标系 { B } \\left\\{ B \\right\\} {B}中，点 P P P与点 P ′ P\^{\\prime} P′之间的旋转关系。此时 P ′ P\^{\\prime} P′为运动刚体上的固定点，对点 P P P在坐标系 { B } \\left\\{ B \\right\\} {B}下的投影参数进行 \[ Q B A \] T \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \^{\\mathrm{T}} \[QBA\]T旋转变化所得到的点 P ′ P\^{\\prime} P′在坐标系 { B } \\left\\{ B \\right\\} {B}下的投影参数。同时，对于 P P P与 P ′ P\^{\\prime} P′而言，其在某坐标系下表达的旋转关系是一致的，因此对于： \[ Q B A \] \[ P 1 A P 2 A P 3 A \] = \[ P ′ 1 A P ′ 2 A P ′ 3 A \] \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \\left\[ \\begin{array}{c} P_{1}\^{A}\\\\ P_{2}\^{A}\\\\ P_{3}\^{A}\\\\ \\end{array} \\right\] =\\left\[ \\begin{array}{c} {P\^{\\prime}}_{1}\^{A}\\\\ {P\^{\\prime}}_{2}\^{A}\\\\ {P\^{\\prime}}_{3}\^{A}\\\\ \\end{array} \\right\] \[QBA\] P1AP2AP3A = P′1AP′2AP′3A 同样成立。 同理，利用几何关系对图进行[分析](https://blog.csdn.net/qq_37680545/article/details/123415191?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522168385257216800182146629%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=168385257216800182146629&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-1-123415191-null-null.142%5Ev87%5Econtrol,239%5Ev2%5Einsert_chatgpt&utm_term=Rodrigues%20Rotation%20Formula&spm=1018.2226.3001.4187)，进而求得`罗德里格旋转公式Rodrigues' Rotation Formula`： R ⃗ p ′ F = R ⃗ p F + ( v ⃗ F × R ⃗ p ′ F ) sin θ + 2 \[ v ⃗ F × ( v ⃗ F × R ⃗ p ′ F ) \] sin 2 θ 2 = R ⃗ p F + v ⃗ \~ F R ⃗ p ′ F sin θ + 2 ( v ⃗ \~ F ) 2 R ⃗ p ′ F sin 2 θ 2 ⇒ R ⃗ p ′ F = \[ E + v ⃗ \~ F sin θ + 2 ( v ⃗ \~ F ) 2 sin θ 2 \] R ⃗ p F = \[ Q B A \] R ⃗ p F \\begin{split} \&\\vec{R}_{\\mathrm{p}\^{\\prime}}\^{F}=\\vec{R}_{\\mathrm{p}}\^{F}+\\left( \\vec{v}\^F\\times \\vec{R}_{\\mathrm{p}\^{\\prime}}\^{F} \\right) \\sin \\theta +2\\left\[ \\vec{v}\^F\\times \\left( \\vec{v}\^F\\times \\vec{R}_{\\mathrm{p}\^{\\prime}}\^{F} \\right) \\right\] \\sin \^2\\frac{\\theta}{2}=\\vec{R}_{\\mathrm{p}}\^{F}+\\tilde{\\vec{v}}\^F\\vec{R}_{\\mathrm{p}\^{\\prime}}\^{F}\\sin \\theta +2\\left( \\tilde{\\vec{v}}\^F \\right) \^2\\vec{R}_{\\mathrm{p}\^{\\prime}}\^{F}\\sin \^2\\frac{\\theta}{2} \\\\ \&\\Rightarrow \\vec{R}_{\\mathrm{p}\^{\\prime}}\^{F}=\\left\[ E+\\tilde{\\vec{v}}\^F\\sin \\theta +2\\left( \\tilde{\\vec{v}}\^F \\right) \^2\\sin \\frac{\\theta}{2} \\right\] \\vec{R}_{\\mathrm{p}}\^{F}=\\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \\vec{R}_{\\mathrm{p}}\^{F} \\end{split} R p′F=R pF+(v F×R p′F)sinθ+2\[v F×(v F×R p′F)\]sin22θ=R pF+v \~FR p′Fsinθ+2(v \~F)2R p′Fsin22θ⇒R p′F=\[E+v \~Fsinθ+2(v \~F)2sin2θ\]R pF=\[QBA\]R pF 而上式给出了：坐标系 { A } \\left\\{ A \\right\\} {A}中，点 P P P与点 P ′ P\^{\\prime} P′之间的转换关系。此时 P P P为运动刚体上的固定点，对点 P P P在坐标系 { A } \\left\\{ A \\right\\} {A}下的投影参数进行 \[ Q B A \] \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \[QBA\]旋转变化，所得到的点 P ′ P\^{\\prime} P′在坐标系 { A } \\left\\{ A \\right\\} {A}下的投影参数。 > 可见，对于旋转矩阵 \[ Q B A \] \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \[QBA\]有三种含义： > > * 将**原矢量进行旋转变换** ，得到该坐标系下新矢量的坐标投影参数： R ⃗ p ′ F = \[ Q B A \] R ⃗ p F \\vec{R}_{\\mathrm{p}\^{\\prime}}\^{F}=\\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \\vec{R}_{\\mathrm{p}}\^{F} R p′F=\[QBA\]R pF； > * 对**坐标系进行转换** ，基于坐标系 { B } \\left\\{ B \\right\\} {B}中的该矢量的坐标投影参数 R ⃗ p B \\vec{R}_{\\mathrm{p}}\^{B} R pB，得到该矢量在坐标系 { A } \\left\\{ A \\right\\} {A}中的坐标投影参数 R ⃗ p A \\vec{R}_{\\mathrm{p}}\^{A} R pA： R ⃗ p A = \[ Q B A \] R ⃗ p B \\vec{R}_{\\mathrm{p}}\^{A}=\\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \\vec{R}_{\\mathrm{p}}\^{B} R pA=\[QBA\]R pB； > * 坐标系 { A } \\left\\{ A \\right\\} {A}中的基矢量在坐标系 { B } \\left\\{ B \\right\\} {B}中的表达： \[ i ⃗ B j ⃗ B k ⃗ B \] = \[ Q B A \] T \[ i ⃗ A j ⃗ A k ⃗ A \] \\left\[ \\begin{array}{c} \\vec{i}\^B\\\\ \\vec{j}\^B\\\\ \\vec{k}\^B\\\\ \\end{array} \\right\] =\\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \^{\\mathrm{T}}\\left\[ \\begin{array}{c} \\vec{i}\^A\\\\ \\vec{j}\^A\\\\ \\vec{k}\^A\\\\ \\end{array} \\right\] i Bj Bk B =\[QBA\]T i Aj Ak A 。 对罗德里格旋转公式进一步进行变换，将其改写为 \[ Q B A \] = E + v ⃗ \~ F sin θ + 2 ( v ⃗ \~ F ) 2 ( 1 − cos θ ) \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] =E+\\tilde{\\vec{v}}\^F\\sin \\theta +2\\left( \\tilde{\\vec{v}}\^F \\right) \^2\\left( 1-\\cos \\theta \\right) \[QBA\]=E+v \~Fsinθ+2(v \~F)2(1−cosθ)，进而利用泰勒展开式，将旋转矩阵 \[ Q B A \] \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \[QBA\]进一步改写： \[ Q B A \] = E + θ v ⃗ \~ F + ( θ ) 2 2 ! ( v ⃗ \~ ) 2 + ( θ ) 3 3 ! ( v ⃗ \~ ) 3 + ⋯ + ( θ ) n n ! ( v ⃗ \~ ) n = e θ v ⃗ \~ \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] =E+\\theta \\tilde{\\vec{v}}\^F+\\frac{\\left( \\theta \\right) \^2}{2!}\\left( \\tilde{\\vec{v}} \\right) \^2+\\frac{\\left( \\theta \\right) \^3}{3!}\\left( \\tilde{\\vec{v}} \\right) \^3+\\cdots +\\frac{\\left( \\theta \\right) \^n}{n!}\\left( \\tilde{\\vec{v}} \\right) \^n=e\^{\\theta \\tilde{\\vec{v}}} \[QBA\]=E+θv \~F+2!(θ)2(v \~)2+3!(θ)3(v \~)3+⋯+n!(θ)n(v \~)n=eθv \~ 可将轴角变换的转换矩阵写成**指数形式**。 * 综合上述推导，可得到轴角变换的旋转矩阵 \[ Q B A \] \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \[QBA\]为： \[ Q B A \] = \[ ( v 1 A ) 2 ( 1 − cos θ ) + cos θ v 1 A v 2 A ( 1 − cos θ ) − v 3 A sin θ v 1 A v 3 A ( 1 − cos θ ) + v 2 A sin θ v 1 A v 2 A ( 1 − cos θ ) + v 3 A sin θ ( v 2 A ) 2 ( 1 − cos θ ) + cos θ v 2 A v 3 A ( 1 − cos θ ) − v 1 A sin θ v 1 A v 3 A ( 1 − cos θ ) − v 2 A sin θ v 2 A v 3 A ( 1 − cos θ ) + v 1 A sin θ ( v 3 A ) 2 ( 1 − cos θ ) + cos θ \] \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] =\\left\[ \\begin{matrix} \\left( v_{1}\^{A} \\right) \^2\\left( 1-\\cos \\theta \\right) +\\cos \\theta\& v_{1}\^{A}v_{2}\^{A}\\left( 1-\\cos \\theta \\right) -v_{3}\^{A}\\sin \\theta\& v_{1}\^{A}v_{3}\^{A}\\left( 1-\\cos \\theta \\right) +v_{2}\^{A}\\sin \\theta\\\\ v_{1}\^{A}v_{2}\^{A}\\left( 1-\\cos \\theta \\right) +v_{3}\^{A}\\sin \\theta\& \\left( v_{2}\^{A} \\right) \^2\\left( 1-\\cos \\theta \\right) +\\cos \\theta\& v_{2}\^{A}v_{3}\^{A}\\left( 1-\\cos \\theta \\right) -v_{1}\^{A}\\sin \\theta\\\\ v_{1}\^{A}v_{3}\^{A}\\left( 1-\\cos \\theta \\right) -v_{2}\^{A}\\sin \\theta\& v_{2}\^{A}v_{3}\^{A}\\left( 1-\\cos \\theta \\right) +v_{1}\^{A}\\sin \\theta\& \\left( v_{3}\^{A} \\right) \^2\\left( 1-\\cos \\theta \\right) +\\cos \\theta\\\\ \\end{matrix} \\right\] \[QBA\]= (v1A)2(1−cosθ)+cosθv1Av2A(1−cosθ)+v3Asinθv1Av3A(1−cosθ)−v2Asinθv1Av2A(1−cosθ)−v3Asinθ(v2A)2(1−cosθ)+cosθv2Av3A(1−cosθ)+v1Asinθv1Av3A(1−cosθ)+v2Asinθv2Av3A(1−cosθ)−v1Asinθ(v3A)2(1−cosθ)+cosθ * 同理对于任意一已知旋转矩阵 \[ Q B A \] = \[ q 11 q 12 q 13 q 21 q 22 q 23 q 31 q 32 q 33 \] \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] =\\left\[ \\begin{matrix} q_{11}\& q_{12}\& q_{13}\\\\ q_{21}\& q_{22}\& q_{23}\\\\ q_{31}\& q_{32}\& q_{33}\\\\ \\end{matrix} \\right\] \[QBA\]= q11q21q31q12q22q32q13q23q33 ，可计算出其轴角参数： θ = a r c cos ( q 11 + q 22 + q 33 − 1 2 ) v ⃗ F = 1 2 sin θ \[ q 32 − q 23 q 13 − q 31 q 21 − q 12 \] \\begin{split} \\theta \&=\\mathrm{arc}\\cos \\left( \\frac{q_{11}+q_{22}+q_{33}-1}{2} \\right) \\\\ \\vec{v}\^F\&=\\frac{1}{2\\sin \\theta}\\left\[ \\begin{array}{c} q_{32}-q_{23}\\\\ q_{13}-q_{31}\\\\ q_{21}-q_{12}\\\\ \\end{array} \\right\] \\end{split} θv F=arccos(2q11+q22+q33−1)=2sinθ1 q32−q23q13−q31q21−q12 ### 3.2 罗德里格变换Rodrigues' Transform 结合上节所述内容，定义`罗德里格参数Rodriguez Paremeters`为： γ ⃗ F = v ⃗ F tan θ 2 = \[ v 1 F v 2 F v 3 F \] tan θ 2 = \[ γ 1 F γ 2 F γ 3 F \] \\vec{\\gamma}\^F=\\vec{v}\^F\\tan \\frac{\\theta}{2}=\\left\[ \\begin{array}{c} v_{1}\^{F}\\\\ v_{2}\^{F}\\\\ v_{3}\^{F}\\\\ \\end{array} \\right\] \\tan \\frac{\\theta}{2}=\\left\[ \\begin{array}{c} \\gamma _{1}\^{F}\\\\ \\gamma _{2}\^{F}\\\\ \\gamma _{3}\^{F}\\\\ \\end{array} \\right\] γ F=v Ftan2θ= v1Fv2Fv3F tan2θ= γ1Fγ2Fγ3F 进而将**罗德里格旋转公式** 改写为： \[ Q M F \] = E + 2 1 + ( γ ) 2 ( γ ⃗ \~ F + ( γ ⃗ \~ F ) 2 ) , γ = ( γ ⃗ \~ F ) T γ ⃗ \~ F = tan 2 θ 2 \\left\[ Q_{\\mathrm{M}}\^{F} \\right\] =E+\\frac{2}{1+\\left( \\gamma \\right) \^2}\\left( \\tilde{\\vec{\\gamma}}\^F+\\left( \\tilde{\\vec{\\gamma}}\^F \\right) \^2 \\right) ,\\gamma =\\left( \\tilde{\\vec{\\gamma}}\^F \\right) \^{\\mathrm{T}}\\tilde{\\vec{\\gamma}}\^F=\\tan \^2\\frac{\\theta}{2} \[QMF\]=E+1+(γ)22(γ \~F+(γ \~F)2),γ=(γ \~F)Tγ \~F=tan22θ 而罗德里格变换的旋转矩阵 \[ Q B A \] \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \[QBA\]为： \[ Q B A \] = \[ 1 + ( γ 1 F ) 2 − ( γ 2 F ) 2 − ( γ 3 F ) 2 2 ( γ 1 F γ 2 F − γ 3 F ) 2 ( γ 1 F γ 3 F + γ 2 F ) 2 ( γ 1 F γ 2 F + γ 3 F ) 1 − ( γ 1 F ) 2 + ( γ 2 F ) 2 − ( γ 3 F ) 2 2 ( γ 2 F γ 3 F − γ 1 F ) 2 ( γ 1 F γ 3 F − γ 2 F ) 2 ( γ 2 F γ 3 F + γ 1 F ) 1 − ( γ 1 F ) 2 − ( γ 2 F ) 2 + ( γ 3 F ) 2 \] \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] =\\left\[ \\begin{matrix} 1+\\left( \\gamma _{1}\^{F} \\right) \^2-\\left( \\gamma _{2}\^{F} \\right) \^2-\\left( \\gamma _{3}\^{F} \\right) \^2\& 2\\left( \\gamma _{1}\^{F}\\gamma _{2}\^{F}-\\gamma _{3}\^{F} \\right)\& 2\\left( \\gamma _{1}\^{F}\\gamma _{3}\^{F}+\\gamma _{2}\^{F} \\right)\\\\ 2\\left( \\gamma _{1}\^{F}\\gamma _{2}\^{F}+\\gamma _{3}\^{F} \\right)\& 1-\\left( \\gamma _{1}\^{F} \\right) \^2+\\left( \\gamma _{2}\^{F} \\right) \^2-\\left( \\gamma _{3}\^{F} \\right) \^2\& 2\\left( \\gamma _{2}\^{F}\\gamma _{3}\^{F}-\\gamma _{1}\^{F} \\right)\\\\ 2\\left( \\gamma _{1}\^{F}\\gamma _{3}\^{F}-\\gamma _{2}\^{F} \\right)\& 2\\left( \\gamma _{2}\^{F}\\gamma _{3}\^{F}+\\gamma _{1}\^{F} \\right)\& 1-\\left( \\gamma _{1}\^{F} \\right) \^2-\\left( \\gamma _{2}\^{F} \\right) \^2+\\left( \\gamma _{3}\^{F} \\right) \^2\\\\ \\end{matrix} \\right\] \[QBA\]= 1+(γ1F)2−(γ2F)2−(γ3F)22(γ1Fγ2F+γ3F)2(γ1Fγ3F−γ2F)2(γ1Fγ2F−γ3F)1−(γ1F)2+(γ2F)2−(γ3F)22(γ2Fγ3F+γ1F)2(γ1Fγ3F+γ2F)2(γ2Fγ3F−γ1F)1−(γ1F)2−(γ2F)2+(γ3F)2 * 罗德里格参数与欧拉参数的转换 \[ γ 1 F γ 2 F γ 3 F \] = \[ q 2 q 1 q 3 q 1 q 4 q 1 \] \\left\[ \\begin{array}{c} {\\gamma _1}\^F\\\\ {\\gamma _2}\^F\\\\ {\\gamma _3}\^F\\\\ \\end{array} \\right\] =\\left\[ \\begin{array}{c} \\frac{q_2}{q_1}\\\\ \\frac{q_3}{q_1}\\\\ \\frac{q_4}{q_1}\\\\ \\end{array} \\right\] γ1Fγ2Fγ3F = q1q2q1q3q1q4 \[ q 1 q 2 q 3 q 4 \] = \[ 1 1 + γ 2 γ 1 F 1 + γ 2 γ 2 F 1 + γ 2 γ 3 F 1 + γ 2 \] \\left\[ \\begin{array}{c} q_1\\\\ q_2\\\\ q_3\\\\ q_4\\\\ \\end{array} \\right\] =\\left\[ \\begin{array}{c} \\frac{1}{\\sqrt{1+\\gamma \^2}}\\\\ \\frac{{\\gamma _1}\^F}{\\sqrt{1+\\gamma \^2}}\\\\ \\frac{{\\gamma _2}\^F}{\\sqrt{1+\\gamma \^2}}\\\\ \\frac{{\\gamma _3}\^F}{\\sqrt{1+\\gamma \^2}}\\\\ \\end{array} \\right\] q1q2q3q4 = 1+γ2 11+γ2 γ1F1+γ2 γ2F1+γ2 γ3F ### 3.3 方向余弦变换 由上节可知，转换矩阵 \[ Q B A \] \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \[QBA\]表示坐标系 { B } \\left\\{ B \\right\\} {B}中的基矢量在坐标系 { A } \\left\\{ A \\right\\} {A}中的表达，即： \[ i ⃗ B j ⃗ B k ⃗ B \] = \[ Q B A \] T \[ i ⃗ A j ⃗ A k ⃗ A \] = \[ q 11 q 12 q 13 q 21 q 22 q 23 q 31 q 32 q 33 \] T \[ i ⃗ A j ⃗ A k ⃗ A \] \\left\[ \\begin{array}{c} \\vec{i}\^B\\\\ \\vec{j}\^B\\\\ \\vec{k}\^B\\\\ \\end{array} \\right\] =\\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \^{\\mathrm{T}}\\left\[ \\begin{array}{c} \\vec{i}\^A\\\\ \\vec{j}\^A\\\\ \\vec{k}\^A\\\\ \\end{array} \\right\] =\\left\[ \\begin{matrix} q_{11}\& q_{12}\& q_{13}\\\\ q_{21}\& q_{22}\& q_{23}\\\\ q_{31}\& q_{32}\& q_{33}\\\\ \\end{matrix} \\right\] \^{\\mathrm{T}}\\left\[ \\begin{array}{c} \\vec{i}\^A\\\\ \\vec{j}\^A\\\\ \\vec{k}\^A\\\\ \\end{array} \\right\] i Bj Bk B =\[QBA\]T i Aj Ak A = q11q21q31q12q22q32q13q23q33 T i Aj Ak A 进而将转换矩阵内的元素展开： \[ Q B A \] T = \[ i ⃗ A ⋅ i ⃗ B j ⃗ A ⋅ i ⃗ B k ⃗ A ⋅ i ⃗ B i ⃗ A ⋅ j ⃗ B j ⃗ A ⋅ j ⃗ B k ⃗ A ⋅ j ⃗ B i ⃗ A ⋅ k ⃗ B j ⃗ A ⋅ k ⃗ B k ⃗ A ⋅ k ⃗ B \] \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \^{\\mathrm{T}}=\\left\[ \\begin{matrix} \\vec{i}\^A\\cdot \\vec{i}\^B\& \\vec{j}\^A\\cdot \\vec{i}\^B\& \\vec{k}\^A\\cdot \\vec{i}\^B\\\\ \\vec{i}\^A\\cdot \\vec{j}\^B\& \\vec{j}\^A\\cdot \\vec{j}\^B\& \\vec{k}\^A\\cdot \\vec{j}\^B\\\\ \\vec{i}\^A\\cdot \\vec{k}\^B\& \\vec{j}\^A\\cdot \\vec{k}\^B\& \\vec{k}\^A\\cdot \\vec{k}\^B\\\\ \\end{matrix} \\right\] \[QBA\]T= i A⋅i Bi A⋅j Bi A⋅k Bj A⋅i Bj A⋅j Bj A⋅k Bk A⋅i Bk A⋅j Bk A⋅k B 进一步观察，可以将该矩阵转化为： \[ Q B A \] T = \[ i ⃗ A ⋅ i ⃗ B j ⃗ A ⋅ i ⃗ B k ⃗ A ⋅ i ⃗ B i ⃗ A ⋅ j ⃗ B j ⃗ A ⋅ j ⃗ B k ⃗ A ⋅ j ⃗ B i ⃗ A ⋅ k ⃗ B j ⃗ A ⋅ k ⃗ B k ⃗ A ⋅ k ⃗ B \] = \[ Q B i A Q B j A Q B k A \] = \[ Q A i B Q A j B Q A k B \] \\left\[ Q_{\\mathrm{B}}\^{A} \\right\] \^{\\mathrm{T}}=\\left\[ \\begin{matrix} \\vec{i}\^A\\cdot \\vec{i}\^B\& \\vec{j}\^A\\cdot \\vec{i}\^B\& \\vec{k}\^A\\cdot \\vec{i}\^B\\\\ \\vec{i}\^A\\cdot \\vec{j}\^B\& \\vec{j}\^A\\cdot \\vec{j}\^B\& \\vec{k}\^A\\cdot \\vec{j}\^B\\\\ \\vec{i}\^A\\cdot \\vec{k}\^B\& \\vec{j}\^A\\cdot \\vec{k}\^B\& \\vec{k}\^A\\cdot \\vec{k}\^B\\\\ \\end{matrix} \\right\] =\\left\[ \\begin{matrix} Q_{\\mathrm{Bi}}\^{A}\& Q_{\\mathrm{Bj}}\^{A}\& Q_{\\mathrm{Bk}}\^{A}\\\\ \\end{matrix} \\right\] =\\left\[ \\begin{array}{c} Q_{\\mathrm{Ai}}\^{B}\\\\ Q_{\\mathrm{Aj}}\^{B}\\\\ Q_{\\mathrm{Ak}}\^{B}\\\\ \\end{array} \\right\] \[QBA\]T= i A⋅i Bi A⋅j Bi A⋅k Bj A⋅i Bj A⋅j Bj A⋅k Bk A⋅i Bk A⋅j Bk A⋅k B =\[QBiAQBjAQBkA\]= QAiBQAjBQAkB 其中， \[ Q B i A Q B j A Q B k A \] \\left\[ \\begin{matrix} Q_{\\mathrm{Bi}}\^{A}\& Q_{\\mathrm{Bj}}\^{A}\& Q_{\\mathrm{Bk}}\^{A}\\\\ \\end{matrix} \\right\] \[QBiAQBjAQBkA\]中，每一项表示坐标系 { B } \\left\\{ B \\right\\} {B}中的基矢量在坐标系 { A } \\left\\{ A \\right\\} {A}下的表达，而 \[ Q A i B Q A j B Q A k B \] . \\left\[ \\begin{array}{c} Q_{\\mathrm{Ai}}\^{B}\\\\ Q_{\\mathrm{Aj}}\^{B}\\\\ Q_{\\mathrm{Ak}}\^{B}\\\\ \\end{array} \\right\] . QAiBQAjBQAkB .中，每一项表示坐标系 { A } \\left\\{ A \\right\\} {A}中的基矢量在坐标系 { B } \\left\\{ B \\right\\} {B}下的表达。因此该形式的矩阵被称为`方向余弦矩阵Direction Cosine Matrix`。 1 2 3 4 5 6 7 8 9 1 2 3 4 5 6 7 8 9