[足式机器人]Part3 机构运动学与动力学分析与建模 Ch00-1 坐标系与概念基准

机构运动学与动力学分析与建模 Ch00-1 坐标系与概念基准

1. 空间坐标系

1. 空间坐标系

坐标系Coordinates是一个用于描述 n n n维系统中某一状态参数的坐标表示系统：对于同一 n n n维系统的状态参数可用不同的坐标系进行表示，即具有不同的 基底Basis（基矢量） ；而对于不同的坐标系而言，表示同一状态参数存在对应关系，因此坐标系之间也存在着 变化关系，这种变化关系的本质是不同坐标系的基底之间的转换。

坐标系的表达： { F } \left\{ F \right\} {F}, { M } \left\{ M \right\} {M}，其中“ { } \{\} {}”符号特指坐标系，一般用 { F } \left\{ F \right\} {F}表示固定坐标系 Fixed， { M } \left\{ M \right\} {M}表示运动坐标系 Moving；对于部分坐标系也可以称为标架Frame。
基矢量的表达： i ^ , j ^ , k ^ \hat{i},\hat{j},\hat{k} i^,j^,k^，其中 “ ^ \hat{ } ^ ” 符号特指基矢量;对于固定坐标系 { F } \left\{ F \right\} {F}，其基矢量特定为： I ^ , J ^ , K ^ \hat{I},\hat{J},\hat{K} I^,J^,K^，对于其他运动坐标系而言，以坐标系 { M } \left\{ M \right\} {M}为例，其基矢量可写成： i ^ M , j ^ M , k ^ M \hat{i}^M,\hat{j}^M,\hat{k}^M i^M,j^M,k^M
标架的表达：以 { M } \left\{ M \right\} {M}为例，其运动标架可表示为： { M : ( i ^ M , j ^ M , k ^ M ) } \left\{ M:\left( \hat{i}^M,\hat{j}^M,\hat{k}^M \right) \right\} {M:(i^M,j^M,k^M)}

1.1 笛卡尔坐标系 Cartesian coordinate system

笛卡尔坐标系是我们在三维空间中常用的表示空间运动的坐标系，基于观测位置与对象的不同，还有GPS坐标系等其他表达方式。对于任一笛卡尔坐标系，视其基矢量为 X ^ 1 , X ^ 2 , X ^ 3 \hat{X}_1,\hat{X}_2,\hat{X}_3 X^1,X^2,X^3，对于该空间内任一点 P P P 都可以这组基矢量进行表达。

1.2 点Partical的运动与表达

对于空间中任意一点 P P P而言，其位置Position表述该系统空间的一种状态参数，因此其在各基矢量上的标量分量即为对应的坐标参数。因此该点 P P P在笛卡尔坐标系中的表达为：
R ⃗ P X = P ⃗ = P 1 X ^ 1 + P 2 X ^ 2 + P 3 X ^ 3 \vec{R}_{\mathrm{P}}^{X}=\vec{P}=P_1\hat{X}_1+P_2\hat{X}_2+P_3\hat{X}_3 R PX=P =P1X^1+P2X^2+P3X^3

1.2.1 笛卡尔直角坐标系

{ F : ( X ^ 1 , X ^ 2 , X ^ 3 ) } = { F : ( I ^ , J ^ , K ^ ) } \left\{ F:\left( \hat{X}_1,\hat{X}_2,\hat{X}_3 \right) \right\} =\left\{ F:\left( \hat{I},\hat{J},\hat{K} \right) \right\} {F:(X^1,X^2,X^3)}={F:(I^,J^,K^)}
[足式机器人]Part3 机构运动学与动力学分析与建模 Ch00-1 坐标系与概念基准-LMLPHP

对于状态空间中一点 P P P，其在固定坐标系标架 { F : ( I ^ , J ^ , K ^ ) } \left\{ F:\left( \hat{I},\hat{J},\hat{K} \right) \right\} {F:(I^,J^,K^)}上基矢量的投影参数为 ( P 1 , P 2 , P 3 ) \left( P_1,P_2,P_3 \right) (P1,P2,P3)，因此可将点 P P P 在笛卡尔直角坐标系中进行表述
R ⃗ P F = P 1 I ^ + P 2 J ^ + P 3 K ^ = [ I ^ J ^ K ^ ] T [ P 1 P 2 P 3 ] \vec{R}_{\mathrm{P}}^{F}=P_1\hat{I}+P_2\hat{J}+P_3\hat{K}=\left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} P_1\\ P_2\\ P_3\\ \end{array} \right] R PF=P1I^+P2J^+P3K^= I^J^K^ T P1P2P3

进而可以求解其速度Velocity参数 V ⃗ P F \vec{V}_{\mathrm{P}}^{F} V PF 为：
V ⃗ P F = R ⃗ ˙ P F = d R ⃗ P F d t = [ I ^ ˙ ↗ 0 J ^ ˙ ↗ 0 K ^ ˙ ↗ 0 ] T [ P 1 P 2 P 3 ] + [ I ^ J ^ K ^ ] T [ P ˙ 1 P ˙ 2 P ˙ 3 ] \vec{V}_{\mathrm{P}}^{F}=\dot{\vec{R}}_{\mathrm{P}}^{F}=\frac{\mathrm{d}\vec{R}_{\mathrm{P}}^{F}}{\mathrm{dt}}=\left[ \begin{array}{c} \dot{\hat{I}}_{\nearrow 0}\\ \dot{\hat{J}}_{\nearrow 0}\\ \dot{\hat{K}}_{\nearrow 0}\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} P_1\\ P_2\\ P_3\\ \end{array} \right] +\left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} \dot{P}_1\\ \dot{P}_2\\ \dot{P}_3\\ \end{array} \right] V PF=R ˙PF=dtdR PF= I^˙↗0J^˙↗0K^˙↗0 T P1P2P3 + I^J^K^ T P˙1P˙2P˙3
其加速度acceleration参数 a ⃗ P F \vec{a}_{\mathrm{P}}^{F} a PF 为：
a ⃗ P F = V ⃗ ˙ P F = R ⃗ ¨ P F = d V ⃗ P F d t = [ I ^ ˙ ↗ 0 J ^ ˙ ↗ 0 K ^ ˙ ↗ 0 ] T [ P ˙ 1 P ˙ 2 P ˙ 3 ] + [ I ^ J ^ K ^ ] T [ P ¨ 1 P ¨ 2 P ¨ 3 ] \vec{a}_{\mathrm{P}}^{F}=\dot{\vec{V}}_{\mathrm{P}}^{F}=\ddot{\vec{R}}_{\mathrm{P}}^{F}=\frac{\mathrm{d}\vec{V}_{\mathrm{P}}^{F}}{\mathrm{dt}}=\left[ \begin{array}{c} \dot{\hat{I}}_{\nearrow 0}\\ \dot{\hat{J}}_{\nearrow 0}\\ \dot{\hat{K}}_{\nearrow 0}\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} \dot{P}_1\\ \dot{P}_2\\ \dot{P}_3\\ \end{array} \right] +\left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} \ddot{P}_1\\ \ddot{P}_2\\ \ddot{P}_3\\ \end{array} \right] a PF=V ˙PF=R ¨PF=dtdV PF= I^˙↗0J^˙↗0K^˙↗0 T P˙1P˙2P˙3 + I^J^K^ T P¨1P¨2P¨3

1.2.2 笛卡尔柱坐标系

{ F : ( X ^ 1 , X ^ 2 , X ^ 3 ) } = { F : ( X ^ r , X ^ θ , K ^ ) } \left\{ F:\left( \hat{X}_1,\hat{X}_2,\hat{X}_3 \right) \right\} =\left\{ F:\left( \hat{X}_{\mathrm{r}},\hat{X}_{\mathrm{\theta}},\hat{K} \right) \right\} {F:(X^1,X^2,X^3)}={F:(X^r,X^θ,K^)}
[足式机器人]Part3 机构运动学与动力学分析与建模 Ch00-1 坐标系与概念基准-LMLPHP
对于不同的坐标系，点 P P P 在状态空间中并没有发生变化，而由于基矢量的变化导致其投影参数发生改变。在柱坐标系中，点 P P P 表述为：
R ⃗ P C = P ⃗ = P 1 ′ X ^ r + P 2 ′ X ^ θ + P 3 ′ K ^ \vec{R}_{\mathrm{P}}^{\mathrm{C}}=\vec{P}=P_1\mathrm{'}\hat{X}_{\mathrm{r}}+P_2\mathrm{'}\hat{X}_{\theta}+P_3\mathrm{'}\hat{K} R PC=P =P1′X^r+P2′X^θ+P3′K^
对于投影参数而言， P 1 ′ P_1\mathrm{'} P1′表示 X ^ r \hat{X}_{\mathrm{r}} X^r方向上的长度参数，而 P 2 ′ P_2\mathrm{'} P2′表示 X ^ θ \hat{X}_{\mathrm{\theta}} X^θ方向上的角度参数，而单纯的角度参数在实际的矢量运算过程中是比较难于理解的，因此对柱坐标系而言，实际上是将该方向上的已知投影参数转换到直角坐标系下进行表示

若已知柱坐标系下点 P P P 的投影参数 P = ( r , θ , k ) P=\left( r,\theta ,k \right) P=(r,θ,k)，其位置方程在直角坐标系下的表示为：
R ⃗ P C = P ⃗ = r cos ⁡ θ I ^ + r sin ⁡ θ J ^ + k K ^ \vec{R}_{\mathrm{P}}^{\mathrm{C}}=\vec{P}=r\cos \theta \hat{I}+r\sin \theta \hat{J}+k\hat{K} R PC=P =rcosθI^+rsinθJ^+kK^
可视为： [ P 1 P 2 P 3 ] = [ r cos ⁡ θ r sin ⁡ θ k ] \left[ \begin{array}{c} P_1\\ P_2\\ P_3\\ \end{array} \right] =\left[ \begin{array}{c} r\cos \theta\\ r\sin \theta\\ k\\ \end{array} \right] P1P2P3 = rcosθrsinθk ，对速度参数 V ⃗ P C \vec{V}_{\mathrm{P}}^{\mathrm{C}} V PC进行求解：
V ⃗ P C = R ⃗ ˙ P C = d R ⃗ P C d t = [ I ^ J ^ K ^ ] T [ P ˙ 1 P ˙ 2 P ˙ 3 ] = [ I ^ J ^ K ^ ] T [ d r d t cos ⁡ θ − r d θ d t sin ⁡ θ d r d t sin ⁡ θ + r d θ d t cos ⁡ θ d k d t ] \vec{V}_{\mathrm{P}}^{\mathrm{C}}=\dot{\vec{R}}_{\mathrm{P}}^{\mathrm{C}}=\frac{\mathrm{d}\vec{R}_{\mathrm{P}}^{\mathrm{C}}}{\mathrm{dt}}=\left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} \dot{P}_1\\ \dot{P}_2\\ \dot{P}_3\\ \end{array} \right] =\left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} \frac{\mathrm{d}r}{\mathrm{dt}}\cos \theta -r\frac{\mathrm{d}\theta}{\mathrm{dt}}\sin \theta\\ \frac{\mathrm{d}r}{\mathrm{dt}}\sin \theta +r\frac{\mathrm{d}\theta}{\mathrm{dt}}\cos \theta\\ \frac{\mathrm{d}k}{\mathrm{dt}}\\ \end{array} \right] V PC=R ˙PC=dtdR PC= I^J^K^ T P˙1P˙2P˙3 = I^J^K^ T dtdrcosθ−rdtdθsinθdtdrsinθ+rdtdθcosθdtdk

当 d r d t = 0 \frac{\mathrm{d}r}{\mathrm{dt}}=0 dtdr=0， d k d t = 0 \frac{\mathrm{d}k}{\mathrm{dt}}=0 dtdk=0 时，即点 P P P 不在矢径方向上运动，仅绕 K ^ \hat{K} K^ 进行平面上的纯回转，可将上式简化为：
V ⃗ P C ∣ d r d t = 0 , d k d t = 0 = [ I ^ J ^ K ^ ] T [ − r d θ d t sin ⁡ θ r d θ d t cos ⁡ θ 0 ] = r θ ˙ [ I ^ J ^ K ^ ] T [ − sin ⁡ θ cos ⁡ θ 0 ] \left. \vec{V}_{\mathrm{P}}^{\mathrm{C}} \right|_{\frac{\mathrm{d}r}{\mathrm{dt}}=0,\frac{\mathrm{d}k}{\mathrm{dt}}=0}=\left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} -r\frac{\mathrm{d}\theta}{\mathrm{dt}}\sin \theta\\ r\frac{\mathrm{d}\theta}{\mathrm{dt}}\cos \theta\\ 0\\ \end{array} \right] =r\dot{\theta}\left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} -\sin \theta\\ \cos \theta\\ 0\\ \end{array} \right] V PC dtdr=0,dtdk=0= I^J^K^ T −rdtdθsinθrdtdθcosθ0 =rθ˙ I^J^K^ T −sinθcosθ0

对上式进一步求解其加速度参数 a ⃗ P C ∣ d r d t = 0 , d k d t = 0 \left. \vec{a}_{\mathrm{P}}^{\mathrm{C}} \right|_{\frac{\mathrm{d}r}{\mathrm{dt}}=0,\frac{\mathrm{d}k}{\mathrm{dt}}=0} a PC dtdr=0,dtdk=0：
a ⃗ P C ∣ d r d t = 0 , d k d t = 0 = r θ ¨ [ I ^ J ^ K ^ ] T [ − sin ⁡ θ cos ⁡ θ 0 ] + r θ ˙ 2 [ I ^ J ^ K ^ ] T [ − cos ⁡ θ − sin ⁡ θ 0 ] = α ⃗ P C × R ⃗ P C + ω ⃗ P C × V ⃗ P C \left. \vec{a}_{\mathrm{P}}^{\mathrm{C}} \right|_{\frac{\mathrm{d}r}{\mathrm{dt}}=0,\frac{\mathrm{d}k}{\mathrm{dt}}=0}=r\ddot{\theta}\left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} -\sin \theta\\ \cos \theta\\ 0\\ \end{array} \right] +r\dot{\theta}^2\left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} -\cos \theta\\ -\sin \theta\\ 0\\ \end{array} \right] =\vec{\alpha}_{\mathrm{P}}^{\mathrm{C}}\times \vec{R}_{\mathrm{P}}^{\mathrm{C}}+\vec{\omega}_{\mathrm{P}}^{\mathrm{C}}\times \vec{V}_{\mathrm{P}}^{\mathrm{C}} a PC dtdr=0,dtdk=0=rθ¨ I^J^K^ T −sinθcosθ0 +rθ˙2 I^J^K^ T −cosθ−sinθ0 =α PC×R PC+ω PC×V PC
若考虑真实的向量表达，则柱坐标系中，点 P P P 还可以表述为：
R ⃗ P C = r ( θ ) X ^ r ( θ ) + k K ^ \vec{R}_{\mathrm{P}}^{\mathrm{C}}=r\left( \theta \right) \hat{X}_{\mathrm{r}\left( \theta \right)}+k\hat{K} R PC=r(θ)X^r(θ)+kK^
其中：
[ X ^ r X ^ θ ] = [ cos ⁡ θ sin ⁡ θ − sin ⁡ θ cos ⁡ θ ] [ I ^ J ^ ] \left[ \begin{array}{c} \hat{X}_{\mathrm{r}}\\ \hat{X}_{\mathrm{\theta}}\\ \end{array} \right] =\left[ \begin{matrix} \cos \theta& \sin \theta\\ -\sin \theta& \cos \theta\\ \end{matrix} \right] \left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \end{array} \right] [X^rX^θ]=[cosθ−sinθsinθcosθ][I^J^]
进而可得：
[ X ^ ˙ r X ^ ˙ θ ] = [ cos ⁡ θ − θ ˙ sin ⁡ θ sin ⁡ θ + θ ˙ cos ⁡ θ − sin ⁡ θ − θ ˙ cos ⁡ θ cos ⁡ θ − θ ˙ sin ⁡ θ ] [ I ^ J ^ ] = [ 0 θ ˙ − θ ˙ 0 ] [ X ^ r X ^ θ ] \left[ \begin{array}{c} \dot{\hat{X}}_{\mathrm{r}}\\ \dot{\hat{X}}_{\mathrm{\theta}}\\ \end{array} \right] =\left[ \begin{matrix} \cos \theta -\dot{\theta}\sin \theta& \sin \theta +\dot{\theta}\cos \theta\\ -\sin \theta -\dot{\theta}\cos \theta& \cos \theta -\dot{\theta}\sin \theta\\ \end{matrix} \right] \left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \end{array} \right] =\left[ \begin{matrix} 0& \dot{\theta}\\ -\dot{\theta}& 0\\ \end{matrix} \right] \left[ \begin{array}{c} \hat{X}_{\mathrm{r}}\\ \hat{X}_{\mathrm{\theta}}\\ \end{array} \right] [X^˙rX^˙θ]=[cosθ−θ˙sinθ−sinθ−θ˙cosθsinθ+θ˙cosθcosθ−θ˙sinθ][I^J^]=[0−θ˙θ˙0][X^rX^θ]
对于 X ^ r \hat{X}_{\mathrm{r}} X^r与 X ^ θ \hat{X}_{\theta} X^θ而言有： X ^ ˙ r = θ ˙ X ^ θ , X ^ θ = − θ ˙ X ^ ˙ r \dot{\hat{X}}_{\mathrm{r}}=\dot{\theta}\hat{X}_{\theta},\hat{X}_{\theta}=-\dot{\theta}\dot{\hat{X}}_{\mathrm{r}} X^˙r=θ˙X^θ,X^θ=−θ˙X^˙r，此处的 X ^ θ \hat{X}_{\theta} X^θ表示的是垂直于基矢量 X ^ r \hat{X}_{\mathrm{r}} X^r的切矢量，与 X ^ θ \hat{X}_{\theta} X^θ不同。

虽然在三维系统中正常应该具有三个基矢量，而在上式中只有两个基矢量，但其投影参数与矢径上的基矢量为另一个参数 θ \theta θ的函数，因此该式为真实表达形式，同样可得其速度与加速度参数为：
{ V ⃗ P C = R ⃗ ˙ P C = r ˙ X ^ r + r X ^ ˙ r + k ˙ K ^ = r ˙ X ^ r + r θ ˙ X ^ θ + k ˙ K ^ a ⃗ P C = V ⃗ ˙ P C = r ¨ X ^ r + r ˙ X ^ ˙ r + r ˙ θ ˙ X ^ θ + r θ ¨ X ^ θ + r θ ˙ X ^ ˙ θ + k ¨ K ^ = r ¨ X ^ r + r ˙ θ ˙ X ^ θ + r ˙ θ ˙ X ^ θ + r θ ¨ X ^ θ − r θ ˙ 2 X ^ r + k ¨ K ^ \left\{ \begin{array}{c} \vec{V}_{\mathrm{P}}^{\mathrm{C}}=\dot{\vec{R}}_{\mathrm{P}}^{\mathrm{C}}=\dot{r}\hat{X}_{\mathrm{r}}+r\dot{\hat{X}}_{\mathrm{r}}+\dot{k}\hat{K}=\dot{r}\hat{X}_{\mathrm{r}}+r\dot{\theta}\hat{X}_{\mathrm{\theta}}+\dot{k}\hat{K}\\ \vec{a}_{\mathrm{P}}^{\mathrm{C}}=\dot{\vec{V}}_{\mathrm{P}}^{\mathrm{C}}=\ddot{r}\hat{X}_{\mathrm{r}}+\dot{r}\dot{\hat{X}}_{\mathrm{r}}+\dot{r}\dot{\theta}\hat{X}_{\mathrm{\theta}}+r\ddot{\theta}\hat{X}_{\mathrm{\theta}}+r\dot{\theta}\dot{\hat{X}}_{\mathrm{\theta}}+\ddot{k}\hat{K}=\ddot{r}\hat{X}_{\mathrm{r}}+\dot{r}\dot{\theta}\hat{X}_{\mathrm{\theta}}+\dot{r}\dot{\theta}\hat{X}_{\mathrm{\theta}}+r\ddot{\theta}\hat{X}_{\mathrm{\theta}}-r\dot{\theta}^2\hat{X}_{\mathrm{r}}+\ddot{k}\hat{K}\\ \end{array} \right. ⎩ ⎨ ⎧V PC=R ˙PC=r˙X^r+rX^˙r+k˙K^=r˙X^r+rθ˙X^θ+k˙K^a PC=V ˙PC=r¨X^r+r˙X^˙r+r˙θ˙X^θ+rθ¨X^θ+rθ˙X^˙θ+k¨K^=r¨X^r+r˙θ˙X^θ+r˙θ˙X^θ+rθ¨X^θ−rθ˙2X^r+k¨K^
对上式)进行化简，可得：
{ V ⃗ P C = r ˙ X ^ r + r θ ˙ X ^ θ + k ˙ K ^ a ⃗ P C = ( r ¨ − r θ ˙ 2 ) X ^ r + ( 2 r ˙ θ ˙ + r θ ¨ ) X ^ θ + k ¨ K ^ \left\{ \begin{array}{c} \vec{V}_{\mathrm{P}}^{\mathrm{C}}=\dot{r}\hat{X}_{\mathrm{r}}+r\dot{\theta}\hat{X}_{\theta}+\dot{k}\hat{K}\\ \vec{a}_{\mathrm{P}}^{\mathrm{C}}=\left( \ddot{r}-r\dot{\theta}^2 \right) \hat{X}_{\mathrm{r}}+\left( 2\dot{r}\dot{\theta}+r\ddot{\theta} \right) \hat{X}_{\theta}+\ddot{k}\hat{K}\\ \end{array} \right. {V PC=r˙X^r+rθ˙X^θ+k˙K^a PC=(r¨−rθ˙2)X^r+(2r˙θ˙+rθ¨)X^θ+k¨K^

其中： r θ ¨ r\ddot{\theta} rθ¨ 称为欧拉项Eulerian term， 2 r ˙ θ ˙ 2\dot{r}\dot{\theta} 2r˙θ˙ 称为科里奥利项Coriolis term。

1.2.3 笛卡尔球坐标系

{ F : ( X ^ 1 , X ^ 2 , X ^ 3 ) } = { F : ( X ^ r , X ^ θ , X ^ ϕ ) } \left\{ F:\left( \hat{X}_1,\hat{X}_2,\hat{X}_3 \right) \right\} =\left\{ F:\left( \hat{X}_{\mathrm{r}},\hat{X}_{\mathrm{\theta}},\hat{X}_{\mathrm{\phi}} \right) \right\} {F:(X^1,X^2,X^3)}={F:(X^r,X^θ,X^ϕ)}
[足式机器人]Part3 机构运动学与动力学分析与建模 Ch00-1 坐标系与概念基准-LMLPHP

笛卡尔球坐标系也可以基于投影参数 P = ( r , θ , ϕ ) P=\left( r,\theta ,\mathrm{\phi} \right) P=(r,θ,ϕ) 在直角坐标系中进行表达，则点 P P P 的运动参数为：
R ⃗ P S = [ I ^ J ^ K ^ ] T [ r sin ⁡ ϕ cos ⁡ θ r sin ⁡ ϕ sin ⁡ θ r cos ⁡ ϕ ] \vec{R}_{\mathrm{P}}^{S}=\left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} r\sin \phi \cos \theta\\ r\sin \phi \sin \theta\\ r\cos \phi\\ \end{array} \right] R PS= I^J^K^ T rsinϕcosθrsinϕsinθrcosϕ
V ⃗ P S = [ I ^ J ^ K ^ ] T [ r ˙ sin ⁡ ϕ cos ⁡ θ + r ϕ ˙ cos ⁡ ϕ cos ⁡ θ − r θ ˙ sin ⁡ ϕ sin ⁡ θ r ˙ sin ⁡ ϕ sin ⁡ θ + r ϕ ˙ cos ⁡ ϕ sin ⁡ θ + r θ ˙ sin ⁡ ϕ cos ⁡ θ r ˙ cos ⁡ ϕ − r ϕ ˙ sin ⁡ ϕ ] \vec{V}_{\mathrm{P}}^{S}=\left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} \dot{r}\sin \phi \cos \theta +r\dot{\phi}\cos \phi \cos \theta -r\dot{\theta}\sin \phi \sin \theta\\ \dot{r}\sin \phi \sin \theta +r\dot{\phi}\cos \phi \sin \theta +r\dot{\theta}\sin \phi \cos \theta\\ \dot{r}\cos \phi -r\dot{\phi}\sin \phi\\ \end{array} \right] V PS= I^J^K^ T r˙sinϕcosθ+rϕ˙cosϕcosθ−rθ˙sinϕsinθr˙sinϕsinθ+rϕ˙cosϕsinθ+rθ˙sinϕcosθr˙cosϕ−rϕ˙sinϕ
a ⃗ P S = [ I ^ J ^ K ^ ] T [ ( r ¨ − r ϕ ˙ 2 − r θ ˙ 2 ) sin ⁡ ϕ cos ⁡ θ + ( 2 r ˙ ϕ ˙ + r ϕ ¨ ) cos ⁡ ϕ cos ⁡ θ − ( 2 r ˙ θ ˙ + r θ ¨ ) sin ⁡ ϕ sin ⁡ θ − ( 2 r θ ˙ ϕ ˙ ) cos ⁡ ϕ sin ⁡ θ ( r ¨ − r ϕ ˙ 2 − r θ ˙ 2 ) sin ⁡ ϕ sin ⁡ θ + ( 2 r ˙ ϕ ˙ + r ϕ ¨ ) cos ⁡ ϕ sin ⁡ θ + ( 2 r ˙ θ ˙ + r θ ¨ ) sin ⁡ ϕ cos ⁡ θ + ( 2 r θ ˙ ϕ ˙ ) cos ⁡ ϕ cos ⁡ θ ( r ¨ − r ϕ ˙ 2 ) cos ⁡ ϕ − ( 2 r ˙ ϕ ˙ + r ϕ ¨ ) sin ⁡ ϕ ] \vec{a}_{\mathrm{P}}^{S}=\left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} \left( \ddot{r}-r\dot{\phi}^2-r\dot{\theta}^2 \right) \sin \phi \cos \theta +\left( 2\dot{r}\dot{\phi}+r\ddot{\phi} \right) \cos \phi \cos \theta -\left( 2\dot{r}\dot{\theta}+r\ddot{\theta} \right) \sin \phi \sin \theta -\left( 2r\dot{\theta}\dot{\phi} \right) \cos \phi \sin \theta\\ \left( \ddot{r}-r\dot{\phi}^2-r\dot{\theta}^2 \right) \sin \phi \sin \theta +\left( 2\dot{r}\dot{\phi}+r\ddot{\phi} \right) \cos \phi \sin \theta +\left( 2\dot{r}\dot{\theta}+r\ddot{\theta} \right) \sin \phi \cos \theta +\left( 2r\dot{\theta}\dot{\phi} \right) \cos \phi \cos \theta\\ \left( \ddot{r}-r\dot{\phi}^2 \right) \cos \phi -\left( 2\dot{r}\dot{\phi}+r\ddot{\phi} \right) \sin \phi\\ \end{array} \right] a PS= I^J^K^ T (r¨−rϕ˙2−rθ˙2)sinϕcosθ+(2r˙ϕ˙+rϕ¨)cosϕcosθ−(2r˙θ˙+rθ¨)sinϕsinθ−(2rθ˙ϕ˙)cosϕsinθ(r¨−rϕ˙2−rθ˙2)sinϕsinθ+(2r˙ϕ˙+rϕ¨)cosϕsinθ+(2r˙θ˙+rθ¨)sinϕcosθ+(2rθ˙ϕ˙)cosϕcosθ(r¨−rϕ˙2)cosϕ−(2r˙ϕ˙+rϕ¨)sinϕ

在笛卡尔球坐标中，如图所示,可将点 P P P 的位置表述为:
R ⃗ p s = r x ^ r \vec{R}_{\mathrm{p}}^{s}=r\hat{x}_{\mathrm{r}} R ps=rx^r
其中：
[ X ^ ϕ X ^ θ X ^ r ] = [ cos ⁡ ϕ cos ⁡ θ cos ⁡ ϕ sin ⁡ θ − sin ⁡ ϕ − sin ⁡ θ cos ⁡ θ 0 sin ⁡ ϕ cos ⁡ θ sin ⁡ ϕ sin ⁡ θ cos ⁡ ϕ ] [ I ^ J ^ K ^ ] \left[ \begin{array}{c} \hat{X}_{\mathrm{\phi}}\\ \hat{X}_{\mathrm{\theta}}\\ \hat{X}_{\mathrm{r}}\\ \end{array} \right] =\left[ \begin{matrix} \cos \phi \cos \theta& \cos \phi \sin \theta& -\sin \phi\\ -\sin \theta& \cos \theta& 0\\ \sin \phi \cos \theta& \sin \phi \sin \theta& \cos \phi\\ \end{matrix} \right] \left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] X^ϕX^θX^r = cosϕcosθ−sinθsinϕcosθcosϕsinθcosθsinϕsinθ−sinϕ0cosϕ I^J^K^
进而求得：
[ X ^ ˙ ϕ X ^ ˙ θ X ^ ˙ r ] = [ − ϕ ˙ sin ⁡ ϕ cos ⁡ θ − θ ˙ cos ⁡ ϕ sin ⁡ θ − ϕ ˙ sin ⁡ ϕ sin ⁡ θ + θ ˙ cos ⁡ ϕ cos ⁡ θ − ϕ ˙ cos ⁡ ϕ − θ ˙ cos ⁡ θ − θ ˙ sin ⁡ θ 0 ϕ ˙ cos ⁡ ϕ cos ⁡ θ − θ ˙ sin ⁡ ϕ sin ⁡ θ ϕ ˙ cos ⁡ ϕ sin ⁡ θ + θ ˙ sin ⁡ ϕ cos ⁡ θ − ϕ ˙ sin ⁡ ϕ ] [ I ^ J ^ K ^ ] = [ 0 θ ˙ cos ⁡ ϕ − ϕ ˙ − θ ˙ cos ⁡ ϕ 0 − θ ˙ sin ⁡ ϕ ϕ ˙ θ ˙ sin ⁡ ϕ 0 ] [ X ^ ϕ X ^ θ X ^ r ] \begin{split} \left[ \begin{array}{c} \dot{\hat{X}}_{\mathrm{\phi}}\\ \dot{\hat{X}}_{\mathrm{\theta}}\\ \dot{\hat{X}}_{\mathrm{r}}\\ \end{array} \right] &=\left[ \begin{matrix} -\dot{\phi}\sin \phi \cos \theta -\dot{\theta}\cos \phi \sin \theta& -\dot{\phi}\sin \phi \sin \theta +\dot{\theta}\cos \phi \cos \theta& -\dot{\phi}\cos \phi\\ -\dot{\theta}\cos \theta& -\dot{\theta}\sin \theta& 0\\ \dot{\phi}\cos \phi \cos \theta -\dot{\theta}\sin \phi \sin \theta& \dot{\phi}\cos \phi \sin \theta +\dot{\theta}\sin \phi \cos \theta& -\dot{\phi}\sin \phi\\ \end{matrix} \right] \left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] \\ &=\left[ \begin{matrix} 0& \dot{\theta}\cos \phi& -\dot{\phi}\\ -\dot{\theta}\cos \phi& 0& -\dot{\theta}\sin \phi\\ \dot{\phi}& \dot{\theta}\sin \phi& 0\\ \end{matrix} \right] \left[ \begin{array}{c} \hat{X}_{\mathrm{\phi}}\\ \hat{X}_{\mathrm{\theta}}\\ \hat{X}_{\mathrm{r}}\\ \end{array} \right] \end{split} X^˙ϕX^˙θX^˙r = −ϕ˙sinϕcosθ−θ˙cosϕsinθ−θ˙cosθϕ˙cosϕcosθ−θ˙sinϕsinθ−ϕ˙sinϕsinθ+θ˙cosϕcosθ−θ˙sinθϕ˙cosϕsinθ+θ˙sinϕcosθ−ϕ˙cosϕ0−ϕ˙sinϕ I^J^K^ = 0−θ˙cosϕϕ˙θ˙cosϕ0θ˙sinϕ−ϕ˙−θ˙sinϕ0 X^ϕX^θX^r
进而求得其速度参数为：
V ⃗ P S = R ⃗ ˙ P S = r ˙ X ^ r + r X ^ ˙ r = r ˙ X ^ r + r ϕ ˙ X ^ ϕ + r θ ˙ sin ⁡ ϕ X ^ θ \vec{V}_{\mathrm{P}}^{S}=\dot{\vec{R}}_{\mathrm{P}}^{S}=\dot{r}\hat{X}_{\mathrm{r}}+r\dot{\hat{X}}_{\mathrm{r}}=\dot{r}\hat{X}_{\mathrm{r}}+r\dot{\phi}\hat{X}_{\mathrm{\phi}}+r\dot{\theta}\sin \phi \hat{X}_{\mathrm{\theta}} V PS=R ˙PS=r˙X^r+rX^˙r=r˙X^r+rϕ˙X^ϕ+rθ˙sinϕX^θ
角速度参数为：
ω ⃗ = θ ˙ sin ⁡ ϕ X ^ ϕ − θ ˙ cos ⁡ ϕ X ^ r + ϕ ˙ X ^ θ \vec{\omega}=\dot{\theta}\sin \phi \hat{X}_{\mathrm{\phi}}-\dot{\theta}\cos \phi \hat{X}_{\mathrm{r}}+\dot{\phi}\hat{X}_{\mathrm{\theta}} ω =θ˙sinϕX^ϕ−θ˙cosϕX^r+ϕ˙X^θ
加速度参数为：
a ⃗ P S = V ⃗ ˙ P S = { r ¨ X ^ r + r ˙ X ^ ˙ r + r ˙ ϕ ˙ X ^ ϕ + r ϕ ¨ X ^ ϕ + r ϕ X ^ ˙ ϕ + r ˙ θ ˙ sin ⁡ ϕ X ^ θ + r θ ¨ sin ⁡ ϕ X ^ θ + r θ ˙ ϕ ˙ cos ⁡ ϕ X ^ θ + r θ ˙ sin ⁡ ϕ X ^ ˙ θ = { r ¨ X ^ r + r ˙ ( ϕ ˙ X ^ ϕ + θ ˙ sin ⁡ ϕ X ^ θ ) + ( r ˙ ϕ ˙ + r ϕ ¨ ) X ^ ϕ + r ϕ ( θ ˙ cos ⁡ ϕ X ^ θ − ϕ ˙ X ^ r ) + ( r ˙ θ ˙ sin ⁡ ϕ + r θ ¨ sin ⁡ ϕ + r θ ˙ ϕ ˙ cos ⁡ ϕ ) X ^ θ + r θ ˙ sin ⁡ ϕ ( − θ ˙ cos ⁡ ϕ X ^ ϕ − θ ˙ sin ⁡ ϕ X ^ r ) = ( r ¨ − r ϕ ϕ ˙ − r θ ˙ 2 sin ⁡ ϕ 2 ) X ^ r + ( 2 r ˙ ϕ ˙ + r ϕ ¨ − r θ ˙ 2 sin ⁡ ϕ cos ⁡ ϕ ) X ^ ϕ + [ ( 2 r ˙ θ ˙ + r θ ¨ ) sin ⁡ ϕ + ( r θ ˙ ϕ ˙ + r ϕ θ ˙ ) cos ⁡ ϕ ] X ^ θ \begin{split} \vec{a}_{\mathrm{P}}^{S}&=\dot{\vec{V}}_{\mathrm{P}}^{S}=\begin{cases} \ddot{r}\hat{X}_{\mathrm{r}}+\dot{r}\dot{\hat{X}}_{\mathrm{r}}+\dot{r}\dot{\phi}\hat{X}_{\mathrm{\phi}}+r\ddot{\phi}\hat{X}_{\mathrm{\phi}}+r\phi \dot{\hat{X}}_{\mathrm{\phi}}\\ +\dot{r}\dot{\theta}\sin \phi \hat{X}_{\mathrm{\theta}}+r\ddot{\theta}\sin \phi \hat{X}_{\mathrm{\theta}}+r\dot{\theta}\dot{\phi}\cos \phi \hat{X}_{\mathrm{\theta}}+r\dot{\theta}\sin \phi \dot{\hat{X}}_{\mathrm{\theta}}\\ \end{cases} \\ &=\begin{cases} \ddot{r}\hat{X}_{\mathrm{r}}+\dot{r}\left( \dot{\phi}\hat{X}_{\mathrm{\phi}}+\dot{\theta}\sin \phi \hat{X}_{\mathrm{\theta}} \right) +\left( \dot{r}\dot{\phi}+r\ddot{\phi} \right) \hat{X}_{\mathrm{\phi}}+r\phi \left( \dot{\theta}\cos \phi \hat{X}_{\mathrm{\theta}}-\dot{\phi}\hat{X}_{\mathrm{r}} \right)\\ +\left( \dot{r}\dot{\theta}\sin \phi +r\ddot{\theta}\sin \phi +r\dot{\theta}\dot{\phi}\cos \phi \right) \hat{X}_{\mathrm{\theta}}+r\dot{\theta}\sin \phi \left( -\dot{\theta}\cos \phi \hat{X}_{\mathrm{\phi}}-\dot{\theta}\sin \phi \hat{X}_{\mathrm{r}} \right)\\ \end{cases} \\ &=\left( \ddot{r}-r\phi \dot{\phi}-r\dot{\theta}^2\sin \phi ^2 \right) \hat{X}_{\mathrm{r}}+\left( 2\dot{r}\dot{\phi}+r\ddot{\phi}-r\dot{\theta}^2\sin \phi \cos \phi \right) \hat{X}_{\mathrm{\phi}}+\left[ \left( 2\dot{r}\dot{\theta}+r\ddot{\theta} \right) \sin \phi +\left( r\dot{\theta}\dot{\phi}+r\phi \dot{\theta} \right) \cos \phi \right] \hat{X}_{\mathrm{\theta}} \end{split} a PS=V ˙PS={r¨X^r+r˙X^˙r+r˙ϕ˙X^ϕ+rϕ¨X^ϕ+rϕX^˙ϕ+r˙θ˙sinϕX^θ+rθ¨sinϕX^θ+rθ˙ϕ˙cosϕX^θ+rθ˙sinϕX^˙θ=⎩ ⎨ ⎧r¨X^r+r˙(ϕ˙X^ϕ+θ˙sinϕX^θ)+(r˙ϕ˙+rϕ¨)X^ϕ+rϕ(θ˙cosϕX^θ−ϕ˙X^r)+(r˙θ˙sinϕ+rθ¨sinϕ+rθ˙ϕ˙cosϕ)X^θ+rθ˙sinϕ(−θ˙cosϕX^ϕ−θ˙sinϕX^r)=(r¨−rϕϕ˙−rθ˙2sinϕ2)X^r+(2r˙ϕ˙+rϕ¨−rθ˙2sinϕcosϕ)X^ϕ+[(2r˙θ˙+rθ¨)sinϕ+(rθ˙ϕ˙+rϕθ˙)cosϕ]X^θ

1.2.4 曲线坐标系:Frenet标架（详见微分几何-曲线论内容）

[足式机器人]Part3 机构运动学与动力学分析与建模 Ch00-1 坐标系与概念基准-LMLPHP
其中， s s s 为曲线的弧长参数， ρ \rho ρ 为曲线的曲率半径；则有： α ⃗ ˙ = s ˙ ρ β ⃗ \dot{\vec{\alpha}}=\frac{\dot{s}}{\rho}\vec{\beta} α ˙=ρs˙β ，其中， s ¨ \ddot{s} s¨ 为切向加速度， s ˙ 2 ρ \frac{\dot{s}^2}{\rho} ρs˙2 为向心加速度，整理出：
{ V ⃗ P F = R ⃗ ˙ P F = s ˙ α ⃗ a ⃗ P F = V ⃗ ˙ P F = s ¨ α ⃗ + s ˙ α ⃗ ˙ = s ¨ α ⃗ + s ˙ 2 ρ β ⃗ \left\{ \begin{array}{c} \vec{V}_{\mathrm{P}}^{F}=\dot{\vec{R}}_{\mathrm{P}}^{F}=\dot{s}\vec{\alpha}\\ \vec{a}_{\mathrm{P}}^{F}=\dot{\vec{V}}_{\mathrm{P}}^{F}=\ddot{s}\vec{\alpha}+\dot{s}\dot{\vec{\alpha}}=\ddot{s}\vec{\alpha}+\frac{\dot{s}^2}{\rho}\vec{\beta}\\ \end{array} \right. ⎩ ⎨ ⎧V PF=R ˙PF=s˙α a PF=V ˙PF=s¨α +s˙α ˙=s¨α +ρs˙2β

且有角速度 ω ⃗ = ω α α ⃗ + ω β β ⃗ + ω γ γ ⃗ \vec{\omega}=\omega _{\mathrm{\alpha}}\vec{\alpha}+\omega _{\beta}\vec{\beta}+\omega _{\mathrm{\gamma}}\vec{\mathrm{\gamma}} ω =ωαα +ωββ +ωγγ ，求解下式： α ⃗ ˙ = ω γ β ⃗ = s ˙ ρ β ⃗ , β ⃗ ˙ = − ω α β ⃗ = s ˙ d γ ⃗ d s , ω β = 0 \dot{\vec{\alpha}}=\omega _{\mathrm{\gamma}}\vec{\beta}=\frac{\dot{s}}{\rho}\vec{\beta},\dot{\vec{\beta}}=-\omega _{\mathrm{\alpha}}\vec{\beta}=\dot{s}\frac{\mathrm{d}\vec{\mathrm{\gamma}}}{\mathrm{d}s},\omega _{\beta}=0 α ˙=ωγβ =ρs˙β ,β ˙=−ωαβ =s˙dsdγ ,ωβ=0

1.2.5 广义坐标系 Generalized coordinates system

对于不固定的单位矢量而言(如上述的柱坐标系中的 X ^ r \hat{X}_{\mathrm{r}} X^r与 X ^ θ \hat{X}_{\theta} X^θ，球坐标系中的 ( X ^ ϕ , X ^ θ , X ^ r ) \left( \hat{X}_{\mathrm{\phi}},\hat{X}_{\mathrm{\theta}},\hat{X}_{\mathrm{r}} \right) (X^ϕ,X^θ,X^r))，在表达运动时可能更为方便。认为广义坐标generalized coordinates是用来描述系统形位的相互独立广义坐标矢量的投影参数坐标，即在该广义坐标系下描述任意矢量，则有：
r ⃗ ˙ p e = ( q ˙ 1 e ⃗ 1 + q ˙ 2 e ⃗ 2 + ⋯ + q ˙ n e ⃗ n ) + ( Q 1 e ⃗ ˙ 1 + q 2 e ⃗ ˙ 2 + ⋯ + q n e ⃗ ˙ n ) = ( q ˙ 1 e ⃗ 1 + q ˙ 2 e ⃗ 2 + ⋯ + q ˙ n e ⃗ n ) + ω ⃗ e × r ⃗ p e \dot{\vec{r}}_{\mathrm{p}}^{e}=\left( \dot{q}_1\vec{e}_1+\dot{q}_2\vec{e}_2+\cdots +\dot{q}_{\mathrm{n}}\vec{e}_{\mathrm{n}} \right) +\left( Q_1\dot{\vec{e}}_1+q_2\dot{\vec{e}}_2+\cdots +q_{\mathrm{n}}\dot{\vec{e}}_{\mathrm{n}} \right) =\left( \dot{q}_1\vec{e}_1+\dot{q}_2\vec{e}_2+\cdots +\dot{q}_{\mathrm{n}}\vec{e}_{\mathrm{n}} \right) +\vec{\omega}^e\times \vec{r}_{\mathrm{p}}^{e} r ˙pe=(q˙1e 1+q˙2e 2+⋯+q˙ne n)+(Q1e ˙1+q2e ˙2+⋯+qne ˙n)=(q˙1e 1+q˙2e 2+⋯+q˙ne n)+ω e×r pe
对于三维空间而言，则有：
R ⃗ ˙ P E = ( q ˙ 1 + ω 2 q 3 − ω 3 q 2 ) e ⃗ 1 + ( q ˙ 2 + ω 3 q 1 − ω 1 q 3 ) e ⃗ 2 + ( q ˙ 3 + ω 1 q 2 − ω 2 q 1 ) e ⃗ 3 \dot{\vec{R}}_{\mathrm{P}}^{E}=\left( \dot{q}_1+\omega _2q_3-\omega _3q_2 \right) \vec{e}_1+\left( \dot{q}_2+\omega _3q_1-\omega _1q_3 \right) \vec{e}_2+\left( \dot{q}_3+\omega _1q_2-\omega _2q_1 \right) \vec{e}_3 R ˙PE=(q˙1+ω2q3−ω3q2)e 1+(q˙2+ω3q1−ω1q3)e 2+(q˙3+ω1q2−ω2q1)e 3

1.3 矢量Vector在坐标系下的表示与关系转换

对于质量点而言 P P P ，其可将其在固定坐标系 { F } \left\{ F \right\} {F}（默认为直角坐标系）下进行表示。同理，对于运动坐标系 { M } \left\{ M \right\} {M}而言，点 P P P 为运动刚体上一点，在运动坐标系下的表达为: R ⃗ P M \vec{R}_{\mathrm{P}}^{M} R PM，则其矢量在固定坐标系下的表达为： ( R ⃗ P M ) F \left( \vec{R}_{\mathrm{P}}^{M} \right) ^F (R PM)F，或写成矢量表达形式为： R ⃗ O M P M → R ⃗ O M P F \vec{R}_{\mathrm{O}^{\mathrm{M}}\mathrm{P}}^{M}\rightarrow \vec{R}_{\mathrm{O}^{\mathrm{M}}\mathrm{P}}^{F} R OMPM→R OMPF
R ⃗ P M = P 1 M i ^ M + P 2 M j ^ M + P 3 M k ^ M = [ i ^ M j ^ M k ^ M ] T [ P 1 M P 2 M P 3 M ] = ( [ Q M F ] T [ I ^ J ^ K ^ ] ) T [ P 1 M P 2 M P 3 M ] = [ I ^ J ^ K ^ ] T [ Q M F ] [ P 1 M P 2 M P 3 M ] = ( R ⃗ P M ) F \begin{split} \vec{R}_{\mathrm{P}}^{M}&={P_1}^M\hat{i}^M+{P_2}^M\hat{j}^M+{P_3}^M\hat{k}^M=\left[ \begin{array}{c} \hat{i}^M\\ \hat{j}^M\\ \hat{k}^M\\ \end{array} \right] ^{\mathrm{T}}\left[ \begin{array}{c} {P_1}^M\\ {P_2}^M\\ {P_3}^M\\ \end{array} \right] =\left( \left[ Q_{\mathrm{M}}^{F} \right] ^{\mathrm{T}}\left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] \right) ^{\mathrm{T}}\left[ \begin{array}{c} {P_1}^M\\ {P_2}^M\\ {P_3}^M\\ \end{array} \right] \\ &=\left[ \begin{array}{c} \hat{I}\\ \hat{J}\\ \hat{K}\\ \end{array} \right] ^{\mathrm{T}}\left[ Q_{\mathrm{M}}^{F} \right] \left[ \begin{array}{c} {P_1}^M\\ {P_2}^M\\ {P_3}^M\\ \end{array} \right] =\left( \vec{R}_{\mathrm{P}}^{M} \right) ^F \end{split} R PM=P1Mi^M+P2Mj^M+P3Mk^M= i^Mj^Mk^M T P1MP2MP3M = [QMF]T I^J^K^ T P1MP2MP3M = I^J^K^ T[QMF] P1MP2MP3M =(R PM)F

LiongLoure