机构运动学与动力学分析与建模 Ch00-1-2 完整定常系统——杆组RRR
1. RRR杆组
1.1 公式推导
1.1.1几何法
已知杆组RRR两端回转副位置参数： A : ( x A , y A ) , C : ( x C , y C ) A:(x_A,y_A),C:(x_C,y_C) A:(xA,yA),C:(xC,yC) ，求解中间回转副B位置参数（要求B点位置存在——满足三角形存在条件）
其中，AC长为： l A C = ( x C − x A ) 2 + ( y C − y A ) 2 l_{AC}=\sqrt{(x_C-x_A)^2+(y_C-y_A)^2} lAC=(xC−xA)2+(yC−yA)2 ，且有：
{ φ = a r c tan ( z C − z A x C − x A ) ∈ ( − π 2 , π 2 ) d = l A C α 1 = a r c cos ( l A B 2 + l A C 2 − l B C 2 2 l A B l A C ) ∈ ( 0 , π ) α 2 = a r c cos ( l A B 2 + l B C 2 − l A C 2 2 l A B l B C ) ∈ ( 0 , π ) α 3 = a r c cos ( l A C 2 + l B C 2 − l A B 2 2 l A C l B C ) ∈ ( 0 , π ) \left\{ \begin{array}{c} \varphi =\mathrm{arc}\tan \left( \frac{z_C-z_A}{x_C-x_A} \right) \in (-\frac{\pi}{2},\frac{\pi}{2})\\ d=l_{AC}\\ \alpha _1=\mathrm{arc}\cos \left( \frac{ {l_{AB}}^2+{l_{AC}}^2-{l_{BC}}^2}{2l_{AB}l_{AC}} \right) \in (0,\pi )\\ \alpha _2=\mathrm{arc}\cos \left( \frac{ {l_{AB}}^2+{l_{BC}}^2-{l_{AC}}^2}{2l_{AB}l_{BC}} \right) \in (0,\pi )\\ \alpha _3=\mathrm{arc}\cos \left( \frac{ {l_{AC}}^2+{l_{BC}}^2-{l_{AB}}^2}{2l_{AC}l_{BC}} \right) \in (0,\pi )\\ \end{array} \right. ⎩ ⎨ ⎧φ=arctan(xC−xAzC−zA)∈(−2π,2π)d=lACα1=arccos(2lABlAClAB2+lAC2−lBC2)∈(0,π)α2=arccos(2lABlBClAB2+lBC2−lAC2)∈(0,π)α3=arccos(2lAClBClAC2+lBC2−lAB2)∈(0,π)
建立闭环矢量方程： R ⃗ F A + l ⃗ A B + l ⃗ B C = R ⃗ F C \vec{R}_{FA}+\vec{l}_{AB}+\vec{l}_{BC}=\vec{R}_{FC} R FA+l AB+l BC=R FC ，向固定坐标系基矢量投影，可得：
{ i ^ f : x A + l A B cos θ A + l B C cos θ B = x C j ^ f : y A + l A B sin θ A + l B C sin θ B = y C \left\{ \begin{array}{l} \hat{i}^f:x_A+l_{AB}\cos \theta _A+l_{BC}\cos \theta _B=x_C\\ \hat{j}^f:y_A+l_{AB}\sin \theta _A+l_{BC}\sin \theta _B=y_C\\ \end{array} \right. { i^f:xA+lABcosθA+lBCcosθB=xCj^f:yA+lABsinθA+lBCsinθB=yC
其中，位置参数 A : ( x A , y A ) , C : ( x C , y C ) A:(x_A,y_A),C:(x_C,y_C) A:(xA,yA),C:(xC,yC)已知 ，杆长 l A B , l B C l_{AB},l_{BC} lAB,lBC已知，求解可得：
θ A 1 = { φ + α 1 ( x C − x A ≥ 0 ) φ + α 1 + π ( x C − x A ≤ 0 ) , θ A 2 = { φ − α 1 ( x C − x A ≥ 0 ) φ − α 1 + π ( x C − x A ≤ 0 ) \theta _{A1}=\left\{ \begin{array}{c} \varphi +\alpha _1(x_C-x_A\ge 0)\\ \varphi +\alpha _1+\pi (x_C-x_A\le 0)\\ \end{array} \right. ,\theta _{A2}=\left\{ \begin{array}{c} \varphi -\alpha _1(x_C-x_A\ge 0)\\ \varphi -\alpha _1+\pi (x_C-x_A\le 0)\\ \end{array} \right. θA1={ φ+α1(xC−xA≥0)φ+α1+π(xC−xA≤0),θA2={ φ−α1(xC−xA≥0)φ−α1+π(xC−xA≤0)
θ B 1 = θ A 1 − π + α 2 , θ B 2 = θ A 2 − π − α 2 \theta _{B1}=\theta _{A1}-\pi +\alpha _2,\theta _{B2}=\theta _{A2}-\pi -\alpha _2 θB1=θA1−π+α2,θB2=θA2−π−α2
则B点坐标为： ( x A + l A B cos θ A , y A + l A B sin θ A ) (x_A+l_{AB}\cos \theta _A,y_A+l_{AB}\sin \theta _A) (xA+lABcosθA,yA+lABsinθA) 。
-
构件运动参数：
将闭环矢量方程对时间 t t t 求导：