机构运动学与动力学分析与建模 Ch00-3刚体的位形 Configuration of Rigid BodyPart3
3.8 点、线、面、向量在坐标系下的表达
对于固定坐标系下同一点/向量，在不同坐标系 { A } , { B } \left\{ A \right\} ,\left\{ B \right\} {A},{B}下进行表达，存在如下转换关系：
R ⃗ V e c t o r A = [ Q B A ] R ⃗ V e c t o r B \vec{R}_{\mathrm{Vector}}^{A}=\left[ Q_{\mathrm{B}}^{A} \right] \vec{R}_{\mathrm{Vector}}^{B} R VectorA=[QBA]R VectorB
R ⃗ P A = [ Q B A ] 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
对于固定坐标系下同一线/面，在不同坐标系 { A } , { B } \left\{ A \right\} ,\left\{ B \right\} {A},{B}下进行表达，存在如下转换关系：
R ⃗ P A + λ R ⃗ V e c t o r A = [ Q B A ] R ⃗ P B + R ⃗ B A + λ [ Q B A ] R ⃗ V e c t o r B = [ Q B A ] ( R ⃗ P B + λ R ⃗ V e c t o r B ) + 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 r 1 A + λ 2 R ⃗ V e c t o r 2 A = [ Q B A ] R ⃗ P B + R ⃗ B A + λ 1 [ Q B A ] R ⃗ V e c t o r 1 B + λ 2 [ Q B A ] R ⃗ V e c t o r 2 B = [ Q B A ] ( R ⃗ P B + λ 1 R ⃗ V e c t o r 1 B + λ 2 R ⃗ V e c t o r 2 B ) + 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+λ1R Vector1A+λ2R Vector2A=[QBA]R PB+R BA+λ1[QBA]R Vector1B+λ2[QBA]R Vector2B=[QBA](R PB+λ1R Vector1B+λ2R Vector2B)+R BA
3.8.1 线的特征
- 线的单位方向 l ⃗ \vec{l} l
已知平面上存在点 P 1 ( x 1 , y 1 , z 1 ) , P 2 ( x 2 , y 2 , z 2 ) 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 ⇀ ∣ 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 = P1P2 P1P2 =(x2−x1)2+(y2−y1)2+(z2−z1)2 (x2−x1)i +(y2−y1)j +(z2−z1)k - 线的姿态参数 ( θ , ϕ ) \left( \theta ,\phi \right) (θ,ϕ)（见1.2.3）
已知平面上存在点 P 1 ( x 1 , y 1 , z 1 ) , P 2 ( x 2 , y 2 , z 2 ) 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 ,\phi \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 P1P2F=(x2−x1)i +(y2−y1)j +(z2−z1)k = P1P2 (cosϕsinθi +sinϕsinθj +cosϕk )⇒⎩ ⎨ ⎧ϕ=arccos P1P2 z2−z1 θ=arcsin P1P2 ((x2−x1)2+(y2−y1)2 )−2π(∣y2−y1∣y2−y1−1)
3.8.2 面的特征
-
法矢量 n ⃗ \vec{n} n
已知平面上存在点 P 1 ( x 1 , y 1 , z 1 ) , P 2 ( x 2 , y 2 , z 2 ) , P 3 ( 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 = ( 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 =P1P2 ×P1P3 = i x2−x1x3−x1j y2−y1y3−y1k z2−z1z3−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 ( x 1 , y 1 , z 1 ) , P 2 ( x 2 , y 2 , z 2 ) , P 3 ( 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 ⇀ ∣ 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= P1P2 P1P2 =(x2−x1)2+(y2−y1)2+(z2−z1)2 (x2−x1)i +(y2−y1)j +(z2−z1)k , k ⃗ M = a i ⃗ F + b j ⃗ F + c k ⃗ 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}^M j M=k M×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= q11q21q31q12q22q32q13q23q33
将该矩阵内的元素带入上述小节中对应的转换关系，即可得到对应表达下的姿态参数。
3.9 简单的示例与计算