[足式机器人]Part3 机构运动学与动力学分析与建模 Ch00-3(1) 刚体的位形 Configuration of Rigid Body

发布时间:2024年01月10日

本文仅供学习使用,总结很多本现有讲述运动学或动力学书籍后的总结,从矢量的角度进行分析,方法比较传统,但更易理解,并且现有的看似抽象方法,两者本质上并无不同。

2024年底本人学位论文发表后方可摘抄
若有帮助请引用
本文参考:
.

食用方法
如何表达刚体在空间中的位置与姿态
姿态参数如何表达?不同表达方式直接的转换关系?
旋转矩阵?转换矩阵?有什么意义和性质?转置代表什么?
如何表示连续变换?——与RPY有关
齐次坐标的意义——简化公式?
务必自己推导全部公式,并理解每个符号的含义

机构运动学与动力学分析与建模 Ch00-3 刚体的位形 Configuration of Rigid Body Part1


刚体的位形可以用六个独立(坐标)参数完全描述三个位置参数用于描述运动刚体上运动坐标系 { 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?] ?r1A?r2A?r3A?? ?

目前,人们采用不同的角度参数 θ ? \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 pF?=[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] [P1B?,P2B?,P2B?]=[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 ?P1B?P2B?P3B?? ?= ?i Aj ?Ak A? ?T ?P1A?P2A?P3A?? ?? ?[QBA?]T ?i Aj ?Ak A? ? ?T ?P1B?P2B?P3B?? ?= ?i Aj ?Ak A? ?T ?P1A?P2A?P3A?? ?? ?i Aj ?Ak A? ?T[QBA?] ?P1B?P2B?P3B?? ?= ?i Aj ?Ak A? ?T ?P1A?P2A?P3A?? ??[QBA?] ?P1B?P2B?P3B?? ?= ?P1A?P2A?P3A?? ?= ?P1B?P2B?P3B?? ??
上式写明:坐标系 { 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?] ?P1A?P2A?P3A?? ?= ?P1A?P2A?P3A?? ?同样成立。

同理,利用几何关系对图进行分析,进而求得罗德里格旋转公式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 pF?=R pF?+(v F×R pF?)sinθ+2[v F×(v F×R pF?)]sin22θ?=R pF?+v ~FR pF?sinθ+2(v ~F)2R pF?sin22θ??R pF?=[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 pF?=[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θv1A?v2A?(1?cosθ)+v3A?sinθv1A?v3A?(1?cosθ)?v2A?sinθ?v1A?v2A?(1?cosθ)?v3A?sinθ(v2A?)2(1?cosθ)+cosθv2A?v3A?(1?cosθ)+v1A?sinθ?v1A?v3A?(1?cosθ)+v2A?sinθv2A?v3A?(1?cosθ)?v1A?sinθ(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?]= ?q11?q21?q31??q12?q22?q32??q13?q23?q33?? ?,可计算出其轴角参数:
    θ = 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??q23?q13??q31?q21??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θ?= ?v1F?v2F?v3F?? ?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] ?γ1?Fγ2?Fγ3?F? ?= ?q1?q2??q1?q3??q1?q4??? ?

[ 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] ?q1?q2?q3?q4?? ?= ?1+γ2 ?1?1+γ2 ?γ1?F?1+γ2 ?γ2?F?1+γ2 ?γ3?F?? ?

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? ?= ?q11?q21?q31??q12?q22?q32??q13?q23?q33?? ?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 B?j ?A?i Bj ?A?j ?Bj ?A?k B?k 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 B?j ?A?i Bj ?A?j ?Bj ?A?k B?k A?i Bk A?j ?Bk A?k B? ?=[QBiA??QBjA??QBkA??]= ?QAiB?QAjB?QAkB?? ?
其中, [ 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] [QBiA??QBjA??QBkA??]中,每一项表示坐标系 { 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] . ?QAiB?QAjB?QAkB?? ?.中,每一项表示坐标系 { 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

文章来源:https://blog.csdn.net/LiongLoure/article/details/135498953
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。