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

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

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

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

---

3.8 点、线、面、向量在坐标系下的表达

对于固定坐标系下同一点/向量，在不同坐标系$\left\{A\right\},\left\{B\right\}$下进行表达，存在如下转换关系：
$$R_{\mathrm{Vector}}^A=\left[Q_{\mathrm{B}}^A\right]R_{\mathrm{Vector}}^B$$
$$R_{\mathrm{P}}^A=\left[Q_{\mathrm{B}}^A\right]R_{\mathrm{P}}^B+R_{\mathrm{B}}^A$$
对于固定坐标系下同一线/面，在不同坐标系$\left\{A\right\},\left\{B\right\}$下进行表达，存在如下转换关系：
$$R_{\mathrm{P}}^A+\lambda R_{\mathrm{Vector}}^A=\left[Q_{\mathrm{B}}^A\right]R_{\mathrm{P}}^B+R_{\mathrm{B}}^A+\lambda \left[Q_{\mathrm{B}}^A\right]R_{\mathrm{Vector}}^B=\left[Q_{\mathrm{B}}^A\right]\left(R_{\mathrm{P}}^B+\lambda R_{\mathrm{Vector}}^B\right)+R_{\mathrm{B}}^A$$
$$R_{\mathrm{P}}^A+{\lambda }_1{R}_{{\mathrm{Vector}}_1}^A+{\lambda }_2{R}_{{\mathrm{Vector}}_2}^A=\left[Q_{\mathrm{B}}^A\right]R_{\mathrm{P}}^B+R_{\mathrm{B}}^A+{\lambda }_1\left[Q_{\mathrm{B}}^A\right]R_{{\mathrm{Vector}}_1}^B+{\lambda }_2\left[Q_{\mathrm{B}}^A\right]R_{{\mathrm{Vector}}_2}^B=\left[Q_{\mathrm{B}}^A\right]\left(R_{\mathrm{P}}^B+{\lambda }_1{R}_{{\mathrm{Vector}}_1}^B+{\lambda }_2{R}_{{\mathrm{Vector}}_2}^B\right)+R_{\mathrm{B}}^A$$

3.8.1 线的特征

• 线的单位方向$l$
  已知平面上存在点$P_1\left(x_1,y_1,z_1\right),P_2\left(x_2,y_2,z_2\right)$，则其单位方向向量$l$为：
  l ⃗ = P 1 P 2 ⇀ ∣ P 1 P 2 ⇀ ∣ = ( x 2 − x 1 ) i ⃗ + ( y 2 − y 1 ) j ⃗ + ( z 2 − z 1 ) k ⃗ ( x 2 − x 1 ) 2 + ( y 2 − y 1 ) 2 + ( z 2 − z 1 ) 2 \vec{l}=\frac{\overrightharpoon{P_1P_2}}{\left| \overrightharpoon{P_1P_2} \right|}=\frac{\left( x_2-x_1 \right) \vec{i}+\left( y_2-y_1 \right) \vec{j}+\left( z_2-z_1 \right) \vec{k}}{\sqrt{\left( x_2-x_1 \right) ^2+\left( y_2-y_1 \right) ^2+\left( z_2-z_1 \right) ^2}} l = ​P1​P2​ ​ ​P1​P2​ ​​=(x2​−x1​)2+(y2​−y1​)2+(z2​−z1​)2 ​(x2​−x1​)i +(y2​−y1​)j ​+(z2​−z1​)k ​
• 线的姿态参数$\left(\theta ,\varphi \right)$（见1.2.3）
  已知平面上存在点$P_1\left(x_1,y_1,z_1\right),P_2\left(x_2,y_2,z_2\right)$，则其球坐标系姿态角$\left(\theta ,\varphi \right)$, 为：
  R ⃗ P 1 P 2 F = ( x 2 − x 1 ) i ⃗ + ( y 2 − y 1 ) j ⃗ + ( z 2 − z 1 ) k ⃗ = ∣ P 1 P 2 ⇀ ∣ ( cos ⁡ ϕ sin ⁡ θ i ⃗ + sin ⁡ ϕ sin ⁡ θ j ⃗ + cos ⁡ ϕ k ⃗ ) ⇒ { ϕ = a r c cos ⁡ ( z 2 − z 1 ∣ P 1 P 2 ⇀ ∣ ) θ = a r c sin ⁡ ( ( x 2 − x 1 ) 2 + ( y 2 − y 1 ) 2 ) − π 2 ( y 2 − y 1 ∣ y 2 − y 1 ∣ − 1 ) ∣ P 1 P 2 ⇀ ∣ \vec{R}_{P_1P_2}^{F}=\left( x_2-x_1 \right) \vec{i}+\left( y_2-y_1 \right) \vec{j}+\left( z_2-z_1 \right) \vec{k}=\left| \overrightharpoon{P_1P_2} \right|\left( \cos \phi \sin \theta \vec{i}+\sin \phi \sin \theta \vec{j}+\cos \phi \vec{k} \right) \\ \Rightarrow \begin{cases} \phi =\mathrm{arc}\cos \left( \frac{z_2-z_1}{\left| \overrightharpoon{P_1P_2} \right|} \right)\\ \theta =\mathrm{arc}\sin \frac{\left( \sqrt{\left( x_2-x_1 \right) ^2+\left( y_2-y_1 \right) ^2} \right) -\frac{\pi}{2}\left( \frac{y_2-y_1}{\left| y_2-y_1 \right|}-1 \right)}{\left| \overrightharpoon{P_1P_2} \right|}\\ \end{cases} R P1​P2​F​=(x2​−x1​)i +(y2​−y1​)j ​+(z2​−z1​)k = ​P1​P2​ ​ ​(cosϕsinθi +sinϕsinθj ​+cosϕk )⇒⎩ ⎨ ⎧​ϕ=arccos ​ ​P1​P2​ ​ ​z2​−z1​​ ​θ=arcsin ​P1​P2​ ​ ​((x2​−x1​)2+(y2​−y1​)2 ​)−2π​(∣y2​−y1​∣y2​−y1​​−1)​​

3.8.2 面的特征

• 法矢量$n$
  已知平面上存在点$P_1\left(x_1,y_1,z_1\right),P_2\left(x_2,y_2,z_2\right),P_3\left(x_3,y_3,z_3\right)$, 则其法矢量$n$为：
  n ⃗ = P 1 P 2 ⇀ × P 1 P 3 ⇀ = ∣ i ⃗ j ⃗ k ⃗ x 2 − x 1 y 2 − y 1 z 2 − z 1 x 3 − x 1 y 3 − y 1 z 3 − z 1 ∣ = a i ⃗ + b j ⃗ + c k ⃗ ; n ⃗ ( a , b , c ) { a = ( y 2 − y 1 ) ( z 3 − z 1 ) − ( y 3 − y 1 ) ( z 2 − z 1 ) b = ( z 2 − z 1 ) ( x 3 − x 1 ) − ( z 3 − z 1 ) ( x 2 − x 1 ) c = ( x 2 − x 1 ) ( y 3 − y 1 ) − ( x 3 − x 1 ) ( y 2 − y 1 )    \vec{n}=\overrightharpoon{P_1P_2}\times \overrightharpoon{P_1P_3}=\left| \begin{matrix} \vec{i}& \vec{j}& \vec{k}\\ x_2-x_1& y_2-y_1& z_2-z_1\\ x_3-x_1& y_3-y_1& z_3-z_1\\ \end{matrix} \right|=a\vec{i}+b\vec{j}+c\vec{k};\vec{n}\left( a,b,c \right) \\ \begin{cases} a=\left( y_2-y_1 \right) \left( z_3-z_1 \right) -\left( y_3-y_1 \right) \left( z_2-z_1 \right)\\ b=\left( z_2-z_1 \right) \left( x_3-x_1 \right) -\left( z_3-z_1 \right) \left( x_2-x_1 \right)\\ c=\left( x_2-x_1 \right) \left( y_3-y_1 \right) -\left( x_3-x_1 \right) \left( y_2-y_1 \right) \,\,\\ \end{cases} n =P1​P2​ ​×P1​P3​ ​= ​i x2​−x1​x3​−x1​​j ​y2​−y1​y3​−y1​​k z2​−z1​z3​−z1​​ ​=ai +bj ​+ck ;n (a,b,c)⎩ ⎨ ⎧​a=(y2​−y1​)(z3​−z1​)−(y3​−y1​)(z2​−z1​)b=(z2​−z1​)(x3​−x1​)−(z3​−z1​)(x2​−x1​)c=(x2​−x1​)(y3​−y1​)−(x3​−x1​)(y2​−y1​)​

• 平面的姿态参数
  已知平面上存在点$P_1\left(x_1,y_1,z_1\right),P_2\left(x_2,y_2,z_2\right),P_3\left(x_3,y_3,z_3\right)$, 令 i ⃗ M = P 1 P 2 ⇀ ∣ P 1 P 2 ⇀ ∣ = ( x 2 − x 1 ) i ⃗ + ( y 2 − y 1 ) j ⃗ + ( z 2 − z 1 ) k ⃗ ( x 2 − x 1 ) 2 + ( y 2 − y 1 ) 2 + ( z 2 − z 1 ) 2 \vec{i}^M=\frac{\overrightharpoon{P_1P_2}}{\left| \overrightharpoon{P_1P_2} \right|}=\frac{\left( x_2-x_1 \right) \vec{i}+\left( y_2-y_1 \right) \vec{j}+\left( z_2-z_1 \right) \vec{k}}{\sqrt{\left( x_2-x_1 \right) ^2+\left( y_2-y_1 \right) ^2+\left( z_2-z_1 \right) ^2}} i M= ​P1​P2​ ​ ​P1​P2​ ​​=(x2​−x1​)2+(y2​−y1​)2+(z2​−z1​)2 ​(x2​−x1​)i +(y2​−y1​)j ​+(z2​−z1​)k ​, $k^M=\frac{a{i}^F+b{j}^F+c{k}^F}{\sqrt{a^2+b^2+c^2}},\{\begin{array}{l}a=\left(y_2-y_1\right)\left(z_3-z_1\right)-\left(y_3-y_1\right)\left(z_2-z_1\right)\\ b=\left(z_2-z_1\right)\left(x_3-x_1\right)-\left(z_3-z_1\right)\left(x_2-x_1\right)\\ c=\left(x_2-x_1\right)\left(y_3-y_1\right)-\left(x_3-x_1\right)\left(y_2-y_1\right)\end{array}$, 根据笛卡尔坐标系的基矢量转换关系：$j^M=k^M\times i^M$
  可得：
  [ i ⃗ M j ⃗ M k ⃗ M ] = [ Q F M ] [ i ⃗ F j ⃗ F k ⃗ F ] ; [ Q M F ] = [ Q F M ] T = [ q 11 q 12 q 13 q 21 q 22 q 23 q 31 q 32 q 33 ] \left[ \begin{array}{c} \vec{i}^M\\ \vec{j}^M\\ \vec{k}^M\\ \end{array} \right] =\left[ Q_{\mathrm{F}}^{M} \right] \left[ \begin{array}{c} \vec{i}^F\\ \vec{j}^F\\ \vec{k}^F\\ \end{array} \right] ;\left[ Q_{\mathrm{M}}^{F} \right] =\left[ Q_{\mathrm{F}}^{M} \right] ^{\mathrm{T}}=\left[ \begin{matrix} q_{11}& q_{12}& q_{13}\\ q_{21}& q_{22}& q_{23}\\ q_{31}& q_{32}& q_{33}\\ \end{matrix} \right] ​i Mj ​Mk M​ ​=[QFM​] ​i Fj ​Fk F​ ​;[QMF​]=[QFM​]T= ​q11​q21​q31​​q12​q22​q32​​q13​q23​q33​​ ​
  将该矩阵内的元素带入上述小节中对应的转换关系，即可得到对应表达下的姿态参数。

3.9 简单的示例与计算