[足式机器人]Part3 机构运动学与动力学分析与建模 Ch00-2(3) 质量刚体的在坐标系下运动

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

2024年底本人学位论文发表后方可摘抄
若有帮助请引用
本文参考：
黎 旭,陈 强 洪,甄 文 强 等.惯 性 张 量 平 移 和 旋 转 复 合 变 换 的 一 般 形 式 及 其 应 用[J].工 程 数 学 学 报,2022,39(06):1005-1011.

食用方法
质量点的动量与角动量
刚体的动量与角动量——力与力矩的关系
惯性矩阵的表达与推导——在刚体运动过程中的作用
惯性矩阵在不同坐标系下的表达

2.2.3 欧拉方程 Euler equation - 2

• 进而分析 ${\vec{H}}_{{\Sigma }_{\mathrm{M}}}^F=m_{\mathrm{total}}?{\vec{R}}_{\mathrm{G}}^F\times {\vec{V}}_{\mathrm{G}}^F+\int \left({\vec{R}}_{{\mathrm{GP}}_{\mathrm{i}}}^F?{\vec{R}}_{{\mathrm{GP}}_{\mathrm{i}}}^F\right){\vec{\omega }}_{\mathrm{M}}^F\mathrm{d}{m}_{\mathrm{i}}?\int \left({\vec{R}}_{{\mathrm{GP}}_{\mathrm{i}}}^F?{\vec{\omega }}_{\mathrm{M}}^F\right){\vec{R}}_{{\mathrm{GP}}_{\mathrm{i}}}^F\mathrm{d}{m}_{\mathrm{i}}$，有：
  $$\begin{array}{rl}& {\vec{H}}_{{\Sigma }_{\mathrm{M}}}^F=m_{\mathrm{total}}?{\vec{R}}_{\mathrm{G}}^F\times {\vec{V}}_{\mathrm{G}}^F+\int \left({{\vec{R}}_{{\mathrm{GP}}_{\mathrm{i}}}^F}^{\mathrm{T}}{\vec{R}}_{{\mathrm{GP}}_{\mathrm{i}}}^F?E^{3\times 3}?{\vec{R}}_{{\mathrm{GP}}_{\mathrm{i}}}^F{{\vec{R}}_{{\mathrm{GP}}_{\mathrm{i}}}^F}^{\mathrm{T}}\right)\mathrm{d}{m}_{\mathrm{i}}?{\vec{\omega }}_{\mathrm{M}}^F=m_{\mathrm{total}}?{\vec{R}}_{\mathrm{G}}^F\times {\vec{V}}_{\mathrm{G}}^F+{\left[I\right]}_{{\Sigma }_{\mathrm{M}}/\mathrm{G}}^F?{\vec{\omega }}_{\mathrm{M}}^F\\ & {\vec{H}}_{{\Sigma }_{\mathrm{M}}/\mathrm{G}}^F={\vec{H}}_{{\Sigma }_{\mathrm{M}}}^F?m_{\mathrm{total}}?{\vec{R}}_{\mathrm{G}}^F\times {\vec{V}}_{\mathrm{G}}^F={\left[I\right]}_{{\Sigma }_{\mathrm{M}}/\mathrm{G}}^F?{\vec{\omega }}_{\mathrm{M}}^F\end{array}$$
  则相对于质心点 $G$ 存在：
  $$?\begin{array}{l}{\vec{\tau }}_{\mathrm{G}}^F=\frac{\mathrm{d}{\vec{h}}_{\mathrm{G}}^F}{\mathrm{dt}}\\ {\vec{\tau }}_{\mathrm{G}/\mathrm{O}}^F=\frac{\mathrm{d}{\vec{h}}_{\mathrm{G}/\mathrm{O}}^F}{\mathrm{dt}}+{\vec{V}}_{\mathrm{O}}^F\times {\vec{P}}_{\mathrm{G}}^F\\ {\vec{P}}_{\mathrm{G}}^F=m_{\mathrm{total}}{\vec{V}}_{\mathrm{G}}^F\end{array}$$
• 对${\vec{H}}_{{\Sigma }_{\mathrm{M}}/\mathrm{O}}^F$进一步推导，可得：
  $$\begin{array}{rl}{\vec{H}}_{{\Sigma }_{\mathrm{M}}/\mathrm{O}}^F& =\sum _i^N{\vec{R}}_{{\mathrm{OP}}_{\mathrm{i}}}^F\times {\vec{P}}_{{\mathrm{P}}_{\mathrm{i}}}^F=\sum _i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?{\vec{R}}_{{\mathrm{OP}}_{\mathrm{i}}}^F\times \left({\vec{\omega }}^F\times {\vec{R}}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)=\sum _i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?{\widetilde{\stackrel{⃗}{R}}}_{{\mathrm{OP}}_{\mathrm{i}}}^F?\left({\widetilde{\stackrel{⃗}{\omega }}}^F?{\vec{R}}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)\\ & =\sum _i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?{\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}0& ?z_{{\mathrm{OP}}_{\mathrm{i}}}^F& y_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ z_{{\mathrm{OP}}_{\mathrm{i}}}^F& 0& ?x_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ ?y_{{\mathrm{OP}}_{\mathrm{i}}}^F& x_{{\mathrm{OP}}_{\mathrm{i}}}^F& 0\end{array}\right]?{\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left(\left[\begin{array}{ccc}0& ?w_{{\mathrm{z}}_{\mathrm{Pi}}}^F& w_{{\mathrm{y}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{z}}_{\mathrm{Pi}}}^F& 0& ?w_{{\mathrm{x}}_{\mathrm{Pi}}}^F\\ ?w_{{\mathrm{y}}_{\mathrm{Pi}}}^F& w_{{\mathrm{x}}_{\mathrm{Pi}}}^F& 0\end{array}\right]?\left[\begin{array}{c}{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ y_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ z_{{\mathrm{OP}}_{\mathrm{i}}}^F\end{array}\right]\right)\\ & =\sum _i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?{\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\left[{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right]w_{{\mathrm{x}}_{\mathrm{Pi}}}^F?\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)w_{{\mathrm{y}}_{\mathrm{Pi}}}^F?\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\\ ?\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)w_{{\mathrm{x}}_{\mathrm{Pi}}}^F+\left[{\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right]w_{{\mathrm{y}}_{\mathrm{Pi}}}^F?\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\\ ?\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)w_{{\mathrm{x}}_{\mathrm{Pi}}}^F?\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)w_{{\mathrm{y}}_{\mathrm{Pi}}}^F+\left[{\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right]w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\end{array}\right]\\ & =\sum _i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?{\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2& ?x_{{\mathrm{OP}}_{\mathrm{i}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F& ?x_{{\mathrm{OP}}_{\mathrm{i}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ ?y_{{\mathrm{OP}}_{\mathrm{i}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F& {\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2& ?y_{{\mathrm{OP}}_{\mathrm{i}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ ?z_{{\mathrm{OP}}_{\mathrm{i}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F& ?z_{{\mathrm{OP}}_{\mathrm{i}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F& {\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\end{array}\right]\left[\begin{array}{c}{w}_{{\mathrm{x}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{y}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\end{array}\right]\\ & ={\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left[{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right]& ?{\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?x_{{\mathrm{OP}}_{\mathrm{i}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F& ?{\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)\\ ?{\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)& {\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left[{\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right]& ?{\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)\\ ?{\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)& ?{\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)& {\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left[{\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right]\end{array}\right]\left[\begin{array}{c}{w}_{{\mathrm{x}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{y}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\end{array}\right]\\ & ={\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{I}_{\mathrm{xx}}& I_{\mathrm{xy}}& I_{\mathrm{xz}}\\ I_{\mathrm{yx}}& I_{\mathrm{yy}}& I_{\mathrm{yz}}\\ I_{\mathrm{zx}}& I_{\mathrm{zy}}& I_{\mathrm{zz}}\end{array}\right]\left[\begin{array}{c}{w}_{{\mathrm{x}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{y}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\end{array}\right]={\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{I}_{\mathrm{xx}}{w}_{{\mathrm{x}}_{\mathrm{Pi}}}^F+I_{\mathrm{xy}}{w}_{{\mathrm{y}}_{\mathrm{Pi}}}^F+I_{\mathrm{xz}}{w}_{{\mathrm{z}}_{\mathrm{Pi}}}^F\\ I_{\mathrm{yx}}{w}_{{\mathrm{x}}_{\mathrm{Pi}}}^F+I_{\mathrm{yy}}{w}_{{\mathrm{y}}_{\mathrm{Pi}}}^F+I_{\mathrm{yz}}{w}_{{\mathrm{z}}_{\mathrm{Pi}}}^F\\ I_{\mathrm{zx}}{w}_{{\mathrm{x}}_{\mathrm{Pi}}}^F+I_{\mathrm{zy}}{w}_{{\mathrm{y}}_{\mathrm{Pi}}}^F+I_{\mathrm{zz}}{w}_{{\mathrm{z}}_{\mathrm{Pi}}}^F\end{array}\right]\\ & ={\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{H}_{\mathrm{x}}\\ H_{\mathrm{y}}\\ H_{\mathrm{z}}\end{array}\right]\end{array}$$

其中：

• 若有：$\vec{\omega }={\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{\omega }_1\\ {\omega }_2\\ {\omega }_3\end{array}\right],\vec{R}={\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{r}_1\\ r_2\\ r_3\end{array}\right]$，则有如下叉乘的计算：
  $$\vec{\omega }\times \vec{R}=\widetilde{\stackrel{⃗}{\omega }}?\vec{R}={\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left(\left[\begin{array}{ccc}0& ?{\omega }_3& {\omega }_2\\ {\omega }_3& 0& ?{\omega }_1\\ ?{\omega }_2& {\omega }_1& 0\end{array}\right]?\left[\begin{array}{c}{r}_1\\ r_2\\ r_3\end{array}\right]\right)$$
• ${\vec{H}}_{{\Sigma }_{\mathrm{M}}/\mathrm{O}}^F$表示刚体${\Sigma }_{\mathrm{M}}$相对于(with respect to/W.R.T)点 $O$ 的角动量在固定坐标系 { F } \left\{ F \right\} {F}的表达。其投影分量满足：
  $$\left[\begin{array}{c}{H}_{\mathrm{x}}\\ H_{\mathrm{y}}\\ H_{\mathrm{z}}\end{array}\right]=\left[\begin{array}{c}{I}_{\mathrm{xx}}{w}_{{\mathrm{x}}_{\mathrm{Pi}}}^F+I_{\mathrm{xy}}{w}_{{\mathrm{y}}_{\mathrm{Pi}}}^F+I_{\mathrm{xz}}{w}_{{\mathrm{z}}_{\mathrm{Pi}}}^F\\ I_{\mathrm{yx}}{w}_{{\mathrm{x}}_{\mathrm{Pi}}}^F+I_{\mathrm{yy}}{w}_{{\mathrm{y}}_{\mathrm{Pi}}}^F+I_{\mathrm{yz}}{w}_{{\mathrm{z}}_{\mathrm{Pi}}}^F\\ I_{\mathrm{zx}}{w}_{{\mathrm{x}}_{\mathrm{Pi}}}^F+I_{\mathrm{zy}}{w}_{{\mathrm{y}}_{\mathrm{Pi}}}^F+I_{\mathrm{zz}}{w}_{{\mathrm{z}}_{\mathrm{Pi}}}^F\end{array}\right]=\left[\begin{array}{ccc}{I}_{\mathrm{xx}}& I_{\mathrm{xy}}& I_{\mathrm{xz}}\\ I_{\mathrm{yx}}& I_{\mathrm{yy}}& I_{\mathrm{yz}}\\ I_{\mathrm{zx}}& I_{\mathrm{zy}}& I_{\mathrm{zz}}\end{array}\right]\left[\begin{array}{c}{w}_{{\mathrm{x}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{y}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\end{array}\right]=\left[I\right]\left[\begin{array}{c}{w}_{{\mathrm{x}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{y}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\end{array}\right]$$
• 矩阵$\left[I\right]$常被称为{惯性矩阵Inertia-matrix，有：${\vec{H}}_{{\Sigma }_{\mathrm{M}}/\mathrm{O}}^F=\left[I\right]{\vec{\omega }}^F$，其中：
  $$\begin{array}{rl}\left[I\right]& =\left[\begin{array}{ccc}{I}_{\mathrm{xx}}& I_{\mathrm{xy}}& I_{\mathrm{xz}}\\ I_{\mathrm{yx}}& I_{\mathrm{yy}}& I_{\mathrm{yz}}\\ I_{\mathrm{zx}}& I_{\mathrm{zy}}& I_{\mathrm{zz}}\end{array}\right]\\ & =\left[\begin{array}{ccc}{\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left[{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right]& ?{\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?x_{{\mathrm{OP}}_{\mathrm{i}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F& ?{\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)\\ ?{\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)& {\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left[{\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right]& ?{\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)\\ ?{\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)& ?{\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)& {\sum }_i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left[{\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right]\end{array}\right]\end{array}$$

上式的实际推导过程，是进行两次转置变化，在实际过程中可以理解成，适用于矩阵与矢量相乘的张量Tensor乘法，因此也可将惯性矩阵$\left[I\right]$称为惯性张量Inertia Tensor。而采用基于拉格朗日恒等式证明的三个向量的双重矢积公式，可能更利于理解：

• 三个向量的双重矢积公式：$\left({\vec{r}}_1\times {\vec{r}}_2\right)\times {\vec{r}}_3=\left({\vec{r}}_1?{\vec{r}}_3\right){\vec{r}}_2?\left({\vec{r}}_2?{\vec{r}}_3\right){\vec{r}}_1$
  $$\begin{array}{rl}{\vec{H}}_{{\Sigma }_{\mathrm{M}}/\mathrm{O}}^F& =\sum _i^N{\vec{R}}_{{\mathrm{OP}}_{\mathrm{i}}}^F\times {\vec{P}}_{{\mathrm{P}}_{\mathrm{i}}}^F=\sum _i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?{\vec{R}}_{{\mathrm{OP}}_{\mathrm{i}}}^F\times \left({\vec{\omega }}^F\times {\vec{R}}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)=\sum _i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?\left[\left({\vec{R}}_{{\mathrm{OP}}_{\mathrm{i}}}^F?{\vec{R}}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right){\vec{\omega }}^F?\left({\vec{\omega }}^F?{\vec{R}}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right){\vec{R}}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right]\\ & =\sum _i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?{\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left[\left({\left[\begin{array}{c}{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ y_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ z_{{\mathrm{OP}}_{\mathrm{i}}}^F\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ y_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ z_{{\mathrm{OP}}_{\mathrm{i}}}^F\end{array}\right]\right)\left[\begin{array}{c}{w}_{{\mathrm{x}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{y}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\end{array}\right]?\left({\left[\begin{array}{c}{w}_{{\mathrm{x}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{y}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ y_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ z_{{\mathrm{OP}}_{\mathrm{i}}}^F\end{array}\right]\right)\left[\begin{array}{c}{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ y_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ z_{{\mathrm{OP}}_{\mathrm{i}}}^F\end{array}\right]\right]\\ & =\sum _i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?{\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left[\left[\begin{array}{c}\left({\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right)w_{{\mathrm{x}}_{\mathrm{Pi}}}^F\\ \left({\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right)w_{{\mathrm{y}}_{\mathrm{Pi}}}^F\\ \left({\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right)w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\end{array}\right]?\left[\begin{array}{c}\left(w_{{\mathrm{x}}_{\mathrm{Pi}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F+w_{{\mathrm{y}}_{\mathrm{Pi}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F+w_{{\mathrm{z}}_{\mathrm{Pi}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)x_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ \left(w_{{\mathrm{x}}_{\mathrm{Pi}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F+w_{{\mathrm{y}}_{\mathrm{Pi}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F+w_{{\mathrm{z}}_{\mathrm{Pi}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)y_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ \left(w_{{\mathrm{x}}_{\mathrm{Pi}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F+w_{{\mathrm{y}}_{\mathrm{Pi}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F+w_{{\mathrm{z}}_{\mathrm{Pi}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)z_{{\mathrm{OP}}_{\mathrm{i}}}^F\end{array}\right]\right]\\ & =\sum _i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?{\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\left[{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right]w_{{\mathrm{x}}_{\mathrm{Pi}}}^F?\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)w_{{\mathrm{y}}_{\mathrm{Pi}}}^F?\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\\ ?\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)w_{{\mathrm{x}}_{\mathrm{Pi}}}^F+\left[{\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right]w_{{\mathrm{y}}_{\mathrm{Pi}}}^F?\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\\ ?\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)w_{{\mathrm{x}}_{\mathrm{Pi}}}^F?\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)w_{{\mathrm{y}}_{\mathrm{Pi}}}^F+\left[{\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\right]w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\end{array}\right]\\ & =\sum _i^N{m}_{{\mathrm{P}}_{\mathrm{i}}}?{\left[\begin{array}{c}\hat{I}\\ \hat{J}\\ \hat{K}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2& ?x_{{\mathrm{OP}}_{\mathrm{i}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F& ?x_{{\mathrm{OP}}_{\mathrm{i}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ ?y_{{\mathrm{OP}}_{\mathrm{i}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F& {\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(z_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2& ?y_{{\mathrm{OP}}_{\mathrm{i}}}^F{z}_{{\mathrm{OP}}_{\mathrm{i}}}^F\\ ?z_{{\mathrm{OP}}_{\mathrm{i}}}^F{x}_{{\mathrm{OP}}_{\mathrm{i}}}^F& ?z_{{\mathrm{OP}}_{\mathrm{i}}}^F{y}_{{\mathrm{OP}}_{\mathrm{i}}}^F& {\left(x_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2+{\left(y_{{\mathrm{OP}}_{\mathrm{i}}}^F\right)}^2\end{array}\right]\left[\begin{array}{c}{w}_{{\mathrm{x}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{y}}_{\mathrm{Pi}}}^F\\ w_{{\mathrm{z}}_{\mathrm{Pi}}}^F\end{array}\right]\end{array}$$