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

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

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

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

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

• • 3.8 点、线、面、向量在坐标系下的表达
  • • 3.8.1 线的特征
    • 3.8.2 面的特征
  • 3.9 简单的示例与计算

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

对于固定坐标系下同一点/向量，在不同坐标系$\left\{A\right\},\left\{B\right\}$下进行表达，存在如下转换关系：
R⃗VectorA= [ QBA] R⃗VectorB\vec{R}_{\mathrm{Vector}}^{A}=\left[ Q_{\mathrm{B}}^{A} \right] \vec{R}_{\mathrm{Vector}}^{B} R VectorA​=[QBA​]R VectorB​
R⃗P A= [ QBA] R⃗P B+ R⃗B A\vec{R}_{\mathrm{P}}^{A}=\left[ Q_{\mathrm{B}}^{A} \right] \vec{R}_{\mathrm{P}}^{B}+\vec{R}_{\mathrm{B}}^{A} R PA​=[QBA​]R PB​+R BA​
对于固定坐标系下同一线/面，在不同坐标系$\left\{A\right\},\left\{B\right\}$下进行表达，存在如下转换关系：
R⃗P A+λ R⃗VectorA= [ QBA] R⃗P B+ R⃗B A+λ [ QBA] R⃗VectorB= [ QBA] (R ⃗PB+ λR ⃗ V e c t o rB)+ R⃗B A\vec{R}_{\mathrm{P}}^{A}+\lambda \vec{R}_{\mathrm{Vector}}^{A}=\left[ Q_{\mathrm{B}}^{A} \right] \vec{R}_{\mathrm{P}}^{B}+\vec{R}_{\mathrm{B}}^{A}+\lambda \left[ Q_{\mathrm{B}}^{A} \right] \vec{R}_{\mathrm{Vector}}^{B}=\left[ Q_{\mathrm{B}}^{A} \right] \left( \vec{R}_{\mathrm{P}}^{B}+\lambda \vec{R}_{\mathrm{Vector}}^{B} \right) +\vec{R}_{\mathrm{B}}^{A} R PA​+λR VectorA​=[QBA​]R PB​+R BA​+λ[QBA​]R VectorB​=[QBA​](R PB​+λR VectorB​)+R BA​
R⃗P A+ λ 1 R⃗ V e c t o r1A+ λ 2 R⃗ V e c t o r2A= [ QBA] R⃗P B+ R⃗B A+ λ 1 [ QBA] R⃗ V e c t o r1B+ λ 2 [ QBA] R⃗ V e c t o r2B= [ QBA] (R ⃗PB+ λ1 R ⃗ Vector1B+ λ2 R ⃗ Vector2B)+ R⃗B A\vec{R}_{\mathrm{P}}^{A}+\lambda _1\vec{R}_{\mathrm{Vector}_1}^{A}+\lambda _2\vec{R}_{\mathrm{Vector}_2}^{A}=\left[ Q_{\mathrm{B}}^{A} \right] \vec{R}_{\mathrm{P}}^{B}+\vec{R}_{\mathrm{B}}^{A}+\lambda _1\left[ Q_{\mathrm{B}}^{A} \right] \vec{R}_{\mathrm{Vector}_1}^{B}+\lambda _2\left[ Q_{\mathrm{B}}^{A} \right] \vec{R}_{\mathrm{Vector}_2}^{B}=\left[ Q_{\mathrm{B}}^{A} \right] \left( \vec{R}_{\mathrm{P}}^{B}+\lambda _1\vec{R}_{\mathrm{Vector}_1}^{B}+\lambda _2\vec{R}_{\mathrm{Vector}_2}^{B} \right) +\vec{R}_{\mathrm{B}}^{A} R PA​+λ1​R Vector1​A​+λ2​R Vector2​A​=[QBA​]R PB​+R BA​+λ1​[QBA​]R Vector1​B​+λ2​[QBA​]R Vector2​B​=[QBA​](R PB​+λ1​R Vector1​B​+λ2​R Vector2​B​)+R BA​

3.8.1 线的特征

• 线的单位方向 l⃗ \vec{l}l
  已知平面上存在点 P 1 ( x1, y1, z1), P 2 ( x2, y2, z2)P_1\left( x_1,y_1,z_1 \right) ,P_2\left( x_2,y_2,z_2 \right) P1​(x1​,y1​,z1​),P2​(x2​,y2​,z2​)，则其单位方向向量 l ⃗\vec{l} l 为：
  l⃗= P 1 P 2⇀ ∣P1P2 ⇀∣=( 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 ( x1, y1, z1), P 2 ( x2, y2, z2)P_1\left( x_1,y_1,z_1 \right) ,P_2\left( x_2,y_2,z_2 \right) P1​(x1​,y1​,z1​),P2​(x2​,y2​,z2​)，则其球坐标系姿态角$\left(\theta ,\varphi \right)$, 为：
  R ⃗ P1P2 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 ⃗)⇒ { ϕ = arccos ⁡ (z 2− z 1∣P 1 P 2⇀∣ )θ = arcsin ⁡(( x 2− x 1)2+ ( y 2− y 1)2 )− π2(y 2− y 1∣ y 2− y 1∣ −1)∣P1P2 ⇀∣\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 ⃗\vec{n} n
  已知平面上存在点 P1( x 1, y 1, z 1), P2( x 2, y 2, z 2), P3( x 3, y 3, z 3) 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)P1​(x1​,y1​,z1​),P2​(x2​,y2​,z2​),P3​(x3​,y3​,z3​), 则其法矢量 n⃗ \vec{n}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= ( 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)   \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​)​

• 平面的姿态参数
  已知平面上存在点 P1( x 1, y 1, z 1), P2( x 2, y 2, z 2), P3( x 3, y 3, z 3) 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)P1​(x1​,y1​,z1​),P2​(x2​,y2​,z2​),P3​(x3​,y3​,z3​), 令i ⃗M= P 1 P 2⇀ ∣P1P2 ⇀∣=( 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=ai ⃗F+ bj ⃗F+ ck ⃗F a 2+ b 2+ c 2 , { 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{k}^M=\frac{a\vec{i}^F+b\vec{j}^F+c\vec{k}^F}{\sqrt{a^2+b^2+c^2}},\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}k M=a2+b2+c2 ​ai F+bj ​F+ck F​,⎩ ⎨ ⎧​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​)​, 根据笛卡尔坐标系的基矢量转换关系：j ⃗M=k ⃗M×i ⃗M \vec{j}^M=\vec{k}^M\times \vec{i}^Mj ​M=k M×i M
  可得：
  [i ⃗M j ⃗M k ⃗M]= [ QFM] [i ⃗F j ⃗F k ⃗F]; [ QMF]= [ Q F M]T= [ q11q12q13q21q22q23q31q32q33]\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 简单的示例与计算