轮腿机器人-五连杆与VMC
1.五连杆正运动学分析
如图所示为五连杆结构图，其中A，E 为机器人腿部控制的两个电机，θ~1~ ,θ~4~可以通过电机的编码器测得。五连杆控制任务主要关注机构末端C 点位置，其位置用直角坐标表示为(C~x~ ,C~y~ )，极坐标系用(L~0~ ,θ~0~ )表示。
根据上述五连杆结构图可以列出以下等式：
{ B x + L 2 ∗ c o s ( θ 2 ) = D x + L 3 ∗ c o s ( θ 3 ) B y + L 2 ∗ s i n ( θ 2 ) = D y + L 3 ∗ s i n ( θ 3 ) \begin{equation} \begin{cases} B_{x}+L_{2}*{\color{Green} cos(\theta {2})} =D{x}+L_{3}*{\color{Green} cos(\theta {3})} \\ B{y}+L_{2}*{\color{Orange} sin(\theta {2})} =D{y}+L_{3}*{\color{Orange} sin(\theta _{3})} \end{cases} \tag{1} \end{equation} {Bx+L2∗cos(θ2)=Dx+L3∗cos(θ3)By+L2∗sin(θ2)=Dy+L3∗sin(θ3)(1)
对公式(1)移项，并在等式两边进行平方有：
{ ( B x + L 2 ∗ c o s ( θ 2 ) − D x ) 2 = ( L 3 ∗ c o s ( θ 3 ) ) 2 ( B y + L 2 ∗ s i n ( θ 2 ) − D y ) 2 = ( L 3 ∗ s i n ( θ 3 ) ) 2 \begin{equation} \begin{cases} (B_{x}+L_{2}*{\color{Green} cos(\theta {2})} -D{x})^{2}=(L_{3}*{\color{Green} cos(\theta {3})})^{2} \\ (B{y}+L_{2}*{\color{Orange} sin(\theta {2})} - D{y})^{2}=(L_{3}*{\color{Orange} sin(\theta _{3})})^{2} \end{cases} \tag{2} \end{equation} {(Bx+L2∗cos(θ2)−Dx)2=(L3∗cos(θ3))2(By+L2∗sin(θ2)−Dy)2=(L3∗sin(θ3))2(2)
将平方展开有：
{ ( B x − D x ) 2 + 2 ∗ ( B x − D x ) ∗ L 2 ∗ c o s ( θ 2 ) + ( L 2 ∗ c o s ( θ 2 ) ) 2 = ( L 3 ∗ c o s ( θ 3 ) ) 2 ( B y − D y ) 2 + 2 ∗ ( B y − D y ) ∗ L 2 ∗ s i n ( θ 2 ) + ( L 2 ∗ s i n ( θ 2 ) ) 2 = ( L 3 ∗ s i n ( θ 3 ) ) 2 \begin{equation} \begin{cases} (B_{x}-D_{x})^{2}+2*(B_{x}-D_{x})*L_{2}*{\color{Green} cos(\theta {2})}+ (L{2}*{\color{Green} cos(\theta {2})})^{2}=(L{3}*{\color{Green} cos(\theta {3})})^{2} \\ (B{y} - D_{y})^{2}+2*(B_{y} - D_{y})*L_{2}*{\color{Orange} sin(\theta {2})}+(L{2}*{\color{Orange} sin(\theta {2})})^{2}=(L{3}*{\color{Orange} sin(\theta _{3})})^{2} \end{cases} \tag{3} \end{equation} {(Bx−Dx)2+2∗(Bx−Dx)∗L2∗cos(θ2)+(L2∗cos(θ2))2=(L3∗cos(θ3))2(By−Dy)2+2∗(By−Dy)∗L2∗sin(θ2)+(L2∗sin(θ2))2=(L3∗sin(θ3))2(3)
对公式(3)内部两个等式相加并移项有：
K ∗ s i n ( θ 2 ) + M ∗ c o s ( θ 2 ) = C \begin{equation} K*{\color{Orange} sin(\theta _{2})}+M*{\color{Green} cos(\theta _{2})}=C \tag{4} \end{equation} K∗sin(θ2)+M∗cos(θ2)=C(4)
{ K = 2 ∗ ( B y − D y ) ∗ L 2 M = 2 ∗ ( B x − D x ) ∗ L 2 P = 2 ∗ [ ( L 3 ) 2 − ( L 2 ) 2 ] L B D = ( B x − D x ) 2 + ( B y − D y ) 2 C = P − ( L B D ) 2 \begin{cases} K=2*(B_{y} - D_{y})*L_{2} \\M=2*(B_{x}-D_{x})*L_{2} \\P=2*[(L_{3})^{2}-(L_{2})^{2}] \\L_{BD}=\sqrt{(B_{x}-D_{x})^{2}+(B_{y} - D_{y})^{2}} \\C=P-(L_{BD} )^2 \end{cases} ⎩ ⎨ ⎧K=2∗(By−Dy)∗L2M=2∗(Bx−Dx)∗L2P=2∗[(L3)2−(L2)2]LBD=(Bx−Dx)2+(By−Dy)2 C=P−(LBD)2
使用二倍角法对公式(4)进一步化简，已知：
{ t a n θ 2 = s i n ( θ ) 1 + c o s ( θ ) c o s ( θ ) = c o s 2 θ 2 − s i n 2 θ 2 = 2 ∗ c o s 2 θ 2 − 1 c o s 2 θ 2 − s i n 2 θ 2 = 1 \begin{cases} {\color{Purple} tan\frac{\theta }{2}} = \frac{{\color{Orange} sin(\theta )} }{1+{\color{Green} cos(\theta )} } \\{\color{Green} cos(\theta )} {\color{Green} ={\color{Green} cos^2\frac{\theta }{2}}} - {\color{Orange} sin^2\frac{\theta }{2}} =2*{\color{Green} cos^2\frac{\theta }{2}} -1 \\{\color{Green} cos^2\frac{\theta }{2}} - {\color{Orange} sin^2\frac{\theta }{2}} =1 \end{cases} ⎩ ⎨ ⎧tan2θ=1+cos(θ)sin(θ)cos(θ)=cos22θ−sin22θ=2∗cos22θ−1cos22θ−sin22θ=1
当 1 + c o s ( θ ) ≠ 0 1+{\color{Green} cos(\theta )} \ne 0 1+cos(θ)=0，对公式(4)进行如下变化，其中 τ = 1 + c o s ( θ ) \tau=1+{\color{Green}cos(\theta)} τ=1+cos(θ):
τ 2 ∗ ( 2 ∗ K ∗ s i n ( θ 2 ) τ + 2 ∗ M ∗ c o s ( θ 2 ) τ − 2 ∗ C τ ) = 0 \begin{equation} \frac{\tau}{2} *(\frac{2*K*{\color{Green} sin(\theta_{2})} }{\tau}+\frac{2*M*{\color{Orange} cos(\theta_{2})} }{\tau}-\frac{2*C}{\tau} )=0 \tag{5} \end{equation} 2τ∗(τ2∗K∗sin(θ2)+τ2∗M∗cos(θ2)−τ2∗C)=0(5)
使用二倍角对公式(5)进行展开并进行化简得：
1 + c o s ( θ 2 ) 2 ∗ [ ( C − M ) ∗ t a n 2 θ 2 2 + 2 ∗ K ∗ t a n ( θ 2 2 ) + ( M + C ) ] \begin{equation} \frac{1+{\color{Green} cos(\theta_{2} )} }{2}*[(C-M)*{\color{Purple} tan^2\frac{\theta_{2} }{2}} +2*K*{\color{Purple} tan(\frac{\theta_{2} }{2})} +(M+C) ] \tag{6} \end{equation} 21+cos(θ2)∗[(C−M)∗tan22θ2+2∗K∗tan(2θ2)+(M+C)](6)
根据公式(6)得到了一个关于 t a n ( θ 2 2 ) {\color{Purple} tan(\frac{\theta_{2} }{2})} tan(2θ2)的一元二次方程，其求根判别式为：
△ = ( 2 ∗ K ) 2 − 4 ∗ ( C − M ) ∗ ( M + C ) = 4 ( K 2 + M 2 − C 2 ) \bigtriangleup =(2*K)^2-4*(C-M)*(M+C)=4(K^2+M^2-C^2) △=(2∗K)2−4∗(C−M)∗(M+C)=4(K2+M2−C2)
当 △ ≥ 0 \bigtriangleup\ge 0 △≥0时，可以解出 θ 2 \theta_{2} θ2:
θ 2 = 2 ∗ a r c t a n ( K ± ( K 2 + M 2 − C 2 ) M − C ) \theta _{2}=2*arctan(\frac{K\pm \sqrt{(K^2+M^2-C^2)} }{M-C} ) θ2=2∗arctan(M−CK±(K2+M2−C2) )
通过 θ 1 \theta_{1} θ1即可解算出 C C C点的直角坐标有：
{ C x = L 1 ∗ c o s ( θ 1 ) + L 2 ∗ c o s ( θ 2 ) C y = L 1 ∗ s i n ( θ 1 ) + L 2 ∗ s i n ( θ 2 ) \begin{equation} \begin{cases} C_{x}=L_{1}*{\color{Orange} cos(\theta {1})} +L{2}*{\color{Orange} cos(\theta_{2})} \\C_{y}=L_{1}*{\color{Green} sin(\theta {1})} +L{2}*{\color{Green} sin(\theta_{2})} \end{cases} \tag{7} \end{equation} {Cx=L1∗cos(θ1)+L2∗cos(θ2)Cy=L1∗sin(θ1)+L2∗sin(θ2)(7)
进一步推导得到极坐标为：
{ L 0 = ( C x − L 5 ) 2 + C y 2 θ 0 = a r c t a n C y C x − L 5 2 \begin{equation} \begin{cases} L_{0}=\sqrt{(C_{x}-L_{5})^2+C_{y}^2} \\\theta_{0}=arctan\frac{C_{y}}{C_{x}-\frac{L_{5}}{2} } \end{cases} \tag{8} \end{equation} ⎩ ⎨ ⎧L0=(Cx−L5)2+Cy2 θ0=arctanCx−2L5Cy(8)
2.参考文献
https://zhuanlan.zhihu.com/p/613007726
[1]于红英,唐德威,王建宇.平面五杆机构运动学和动力学特性分析[J].哈尔滨工业大学学报,2007(06):940-943.
[2]谢惠祥.四足机器人对角小跑步态虚拟模型直觉控制方法研究[D].国防科学技术大学,2015.