博客
关于我
强烈建议你试试无所不能的chatGPT,快点击我
二并联杆的运动学分析求解末端执行器的位置,速度,加速度与系统输入的关系
阅读量:780 次
发布时间:2019-03-24

本文共 9618 字,大约阅读时间需要 32 分钟。

Content

在这里插入图片描述

Part one:Direct Kinematic analysis of a Biglide

Geometric analysis

The direct geometric model which gives [x, y] = dgm(q1,q2,gamma)(Two prismatic joints are actuated)

在这里插入图片描述

We know the position of A 1 ( − d 2 , q 1 ) A_1(-\frac{d}{2},q_1) A1(2d,q1) and A 2 ( d 2 , q 2 ) A_2(\frac{d}{2},q_2) A2(2d,q2),thus point H is the mid-point of A 1 A_1 A1 and A 2 A_2 A2,its position is ( 0 , q 1 + q 2 2 ) (0,\frac{q_1+q_2}{2}) (0,2q1+q2).

  • Compute the vector A 2 H → \overrightarrow{A_2H} A2H A 2 H → = H − A 2 = ( − d 2 , q 1 − q 2 2 ) \overrightarrow{A_2H}=H-A_2=(-\frac{d}{2},\frac{q_1-q_2}{2}) A2H =HA2=(2d,2q1q2)
  • There are two solutions for the point A 13 , obtained by the 9 0 o 90^o 90o rotation of the vector A 2 H → \overrightarrow{A_2H} A2H , normalization and multiplying by the
    distance h, giving H C → = γ h a [ 0 − 1 1 0 ] A 2 H → \overrightarrow{H C}=\gamma \frac{h}{a}\left[\begin{array}{cc} 0 & -1 \\ 1 & 0 \end{array}\right] \overrightarrow{A_{2} H} HC =γah[0110]A2H
    where a = ∣ ∣ A 2 H → ∣ ∣ a=|| \overrightarrow{A_{2} H}|| a=A2H and h = l 2 − a 2 h=\sqrt{l^2-a^2} h=l2a2
    O C → = O O 2 → + O 2 A 2 → + A 2 H → + H C → O C → = [ d 2 0 ] + [ 0 q 2 ] + [ − d 2 q 1 − a 2 2 ] + γ h a [ 0 − 1 1 0 ] [ − d 2 q 1 − q 2 2 ] \overrightarrow{OC}=\overrightarrow{OO_2}+\overrightarrow{O_2A_2}+\overrightarrow{A_2H}+\overrightarrow{HC}\\ \overrightarrow{OC}=\left[\begin{array}{c} \frac{d}{2} \\ 0 \end{array}\right]+\left[\begin{array}{c} 0 \\ q_2 \end{array}\right]+\left[\begin{array}{c} -\frac{d}{2} \\ \frac{q_1-a_2}{2} \end{array}\right]+\gamma\frac{h}{a}\left[\begin{array}{cc} 0&-1 \\ 1&0 \end{array}\right]\left[\begin{array}{c} -\frac{d}{2} \\ \frac{q_1-q_2}{2} \end{array}\right] OC =OO2 +O2A2 +A2H +HC OC =[2d0]+[0q2]+[2d2q1a2]+γah[0110][2d2q1q2]
    Thus we have x = O C → ( 1 ) y = O C → ( 2 ) x=\overrightarrow{OC}(1) \\y=\overrightarrow{OC}(2) x=OC (1)y=OC (2)

The model which gives the passive angle position [ ϕ 1 \phi_1 ϕ1 ϕ 2 \phi_2 ϕ2] = passive angles(x,y,q1,q2)

O C → = O O i → + O i A i → + A i C → \overrightarrow{OC}=\overrightarrow{OO_i}+\overrightarrow{O_iA_i}+\overrightarrow{A_iC} OC =OOi +OiAi +AiC
For leg1: O C → = O O 1 → + O 1 A 1 → + A 1 C → O C → − O O 1 → − O 1 A 1 → = A 1 C → \overrightarrow{OC}=\overrightarrow{OO_1}+\overrightarrow{O_1A_1}+\overrightarrow{A_1C}\\ \overrightarrow{OC}-\overrightarrow{OO_1}-\overrightarrow{O_1A_1}=\overrightarrow{A_1C} OC =OO1 +O1A1 +A1C OC OO1 O1A1 =A1C
We have x + d 2 = l A 1 C c o s ϕ 1 ( 1 ) y − q 1 = l A 1 C s i n ϕ 1 ( 2 ) x+\frac{d}{2}=l_{A_1C}cos\phi_1\quad(1)\\ y-q_1=l_{A_1C}sin\phi_1\quad(2) x+2d=lA1Ccosϕ1(1)yq1=lA1Csinϕ1(2)
( 2 ) ( 1 ) \frac{(2)}{(1)} (1)(2),we get ϕ 1 = a t a n 2 ( y − q 1 x + d 2 ) \phi_1=atan2(\frac{y-q_1}{x+\frac{d}{2}}) ϕ1=atan2(x+2dyq1)
After using the same process,we have
ϕ 2 = a t a n 2 ( y − q 2 x − d 2 ) \phi_2=atan2(\frac{y-q_2}{x-\frac{d}{2}}) ϕ2=atan2(x2dyq2)

We would like to get inverse geometric model[q1,q2]=igm[x,y]

From equation(1) and(2),we have ( x + d 2 ) 2 + ( y − q 1 ) 2 = l A 1 C 2 ( x − d 2 ) 2 + ( y − q 2 ) 2 = l A 2 C 2 (x+\frac{d}{2})^2+(y-q_1)^2=l_{A_1C}^2\\(x-\frac{d}{2})^2+(y-q_2)^2=l_{A_2C}^2 (x+2d)2+(yq1)2=lA1C2(x2d)2+(yq2)2=lA2C2
such that q 1 = y + / − l A 1 C 2 − ( x + d 2 ) 2 q 2 = y + / − l A 2 C 2 − ( x − d 2 ) 2 q_1=y+/-\sqrt{l_{A_1C}^2-(x+\frac{d}{2})^2} \\q_2=y+/-\sqrt{l_{A_2C}^2-(x-\frac{d}{2})^2} q1=y+/lA1C2(x+2d)2 q2=y+/lA2C2(x2d)2

Velocity analysis of a Biglide

The direct kinematic model which gives

[xd, yd] = dkm(q1d,q2d,x,y,q1,q2, ϕ 1 \phi_1 ϕ1, ϕ 2 \phi_2 ϕ2)

We would like to find A t + B q ˙ = 0 At+B\dot q=0 At+Bq˙=0,where t = x ˙ = [ x ˙ , y ˙ ] T t=\dot \mathbf{x}=[\dot x,\dot y]^T t=x˙=[x˙,y˙]Twe have

h 1 : ( x + d 2 ) 2 + ( y − q 1 ) 2 − l A 1 C 2 = 0 h 2 : ( x − d 2 ) 2 + ( y − q 2 ) 2 − l A 2 C 2 = 0 h_1:(x+\frac{d}{2})^2+(y-q_1)^2-l_{A_1C}^2=0\\h_2:(x-\frac{d}{2})^2+(y-q_2)^2-l_{A_2C}^2=0 h1:(x+2d)2+(yq1)2lA1C2=0h2:(x2d)2+(yq2)2lA2C2=0

A = ∂ h ∂ x = [ ∂ h 1 ∂ x ∂ h 1 ∂ y ∂ h 2 ∂ x ∂ h 1 ∂ y ] = [ 2 ( x + d 2 ) 2 ( y − q 1 ) 2 ( x − d 2 ) 2 ( y − q 2 ) ] = 2 l [ c o s ϕ 1 s i n ϕ 1 c o s ϕ 2 s i n ϕ 2 ] A=\frac{\partial \mathbf{h}}{\partial \mathbf{x}}=\left[\begin{array}{cc} \frac{\partial h_1}{\partial x} &\frac{\partial h_1}{\partial y} \\ \frac{\partial h_2}{\partial x} & \frac{\partial h_1}{\partial y} \end{array}\right] =\left[\begin{array}{cc} 2(x+\frac{d}{2})&2(y-q_1)\\ 2(x-\frac{d}{2})&2(y-q_2) \end{array}\right]=2l\left[\begin{array}{cc} cos\phi_1&sin\phi_1\\ cos\phi_2&sin\phi_2 \end{array}\right] A=xh=[xh1xh2yh1yh1]=[2(x+2d)2(x2d)2(yq1)2(yq2)]=2l[cosϕ1cosϕ2sinϕ1sinϕ2]

B = ∂ h ∂ q a = [ ∂ h 1 ∂ q 1 ∂ h 1 ∂ q 2 ∂ h 2 ∂ q 1 ∂ h 1 ∂ q 2 ] = [ 2 ( y − q 1 ) 0 0 2 ( y − q 2 ) ] = 2 l [ s i n ϕ 1 0 0 s i n ϕ 2 ] B=\frac{\partial \mathbf{h}}{\partial \mathbf{q_a}}=\left[\begin{array}{cc} \frac{\partial h_1}{\partial q_1} &\frac{\partial h_1}{\partial q_2} \\ \frac{\partial h_2}{\partial q_1} & \frac{\partial h_1}{\partial q_2} \end{array}\right] =\left[\begin{array}{cc} 2(y-q_1)&0\\ 0&2(y-q_2) \end{array}\right]=2l\left[\begin{array}{cc} sin\phi_1&0\\ 0&sin\phi_2 \end{array}\right] B=qah=[q1h1q1h2q2h1q2h1]=[2(yq1)002(yq2)]=2l[sinϕ100sinϕ2]
Thus,we have q a ˙ = − B − 1 A t \dot \mathbf{q_a}=-B^{-1}A\mathbf{t} qa˙=B1At.

The model that provides the passive joint velocities

[ ϕ 1 d \phi_1d ϕ1d, ϕ 2 d \phi_2d ϕ2d] =passiveangleskm(xd,yd,q1d,q2d,x,y,q1,q2, ϕ 1 \phi_1 ϕ1, ϕ 2 \phi_2 ϕ2)
x + d 2 = l A 1 C c o s ϕ 1 ( 1 ) y − q 1 = l A 1 C s i n ϕ 1 ( 2 ) x+\frac{d}{2}=l_{A_1C}cos\phi_1\quad(1)\\ y-q_1=l_{A_1C}sin\phi_1\quad(2) x+2d=lA1Ccosϕ1(1)yq1=lA1Csinϕ1(2)we rewrite it into matrix formula:

[ x + d 2 y − q 1 ] = l A 1 C [ c o s ϕ 1 s i n ϕ 1 ] [ c o s ϕ 1 s i n ϕ 1 ] [ x + d 2 y − q 1 ] = l A 1 C \left[\begin{array}{c} x+\frac{d}{2} \\ y-q_1 \end{array}\right] =l_{A_1C}\left[\begin{array}{c} cos\phi_1\\ sin\phi_1 \end{array}\right]\\ [cos\phi_1\quad sin\phi_1]\left[\begin{array}{c} x+\frac{d}{2} \\ y-q_1 \end{array}\right] =l_{A_1C}\\ [x+2dyq1]=lA1C[cosϕ1sinϕ1][cosϕ1sinϕ1][x+2dyq1]=lA1C

We obtained Φ 1 : c o s ϕ 1 ( x + d 2 ) + s i n ϕ 1 ( y − q 1 ) − l = 0 Φ 2 : c o s ϕ 2 ( x − d 2 ) + s i n ϕ 2 ( y − q 2 ) − l = 0 \Phi_1:cos\phi_1(x+\frac{d}{2})+sin\phi_1(y-q_1)-l=0\\ \Phi_2:cos\phi_2(x-\frac{d}{2})+sin\phi_2(y-q_2)-l=0 Φ1:cosϕ1(x+2d)+sinϕ1(yq1)l=0Φ2:cosϕ2(x2d)+sinϕ2(yq2)l=0
To calculate passive joint velocity,we use J t t − J t a q a ˙ = J t p q d ˙ J_t\mathbf{t}-J_{t_a}\dot\mathbf{q_a}=J_{tp}\dot\mathbf{q_{d}} JttJtaqa˙=Jtpqd˙.
J t = ∂ Φ ∂ x − J t a = ∂ Φ ∂ q a J t d = − ∂ Φ ∂ q d J_t=\frac{\partial \mathbf{\Phi}}{\partial \mathbf{x}}\quad-J_{ta}=\frac{\partial \mathbf{\Phi}}{\partial \mathbf{q_a}}\quad J_{td}=-\frac{\partial \mathbf{\Phi}}{\partial \mathbf{q_d}} Jt=xΦJta=qaΦJtd=qdΦ
J t = ∂ Φ ∂ x = [ ∂ Φ 1 ∂ x ∂ Φ 1 ∂ y ∂ Φ 2 ∂ x ∂ Φ 2 ∂ y ] = [ c o s ϕ 1 s i n ϕ 1 c o s ϕ 2 s i n ϕ 2 ] J_t=\frac{\partial \mathbf{\Phi}}{\partial \mathbf{x}}=\left[\begin{array}{c} \frac{\partial \Phi_1}{\partial x}&\frac{\partial \Phi_1}{\partial y} \\ \frac{\partial \Phi_2}{\partial x}&\frac{\partial \Phi_2}{\partial y} \end{array}\right]= \left[\begin{array}{c} cos\phi_1&sin\phi_1\\ cos\phi_2&sin\phi_2 \end{array}\right] Jt=xΦ=[xΦ1xΦ2yΦ1yΦ2]=[cosϕ1cosϕ2sinϕ1sinϕ2]
− J t a = ∂ Φ ∂ q a = [ ∂ Φ 1 ∂ q 1 ∂ Φ 1 ∂ q 2 ∂ Φ 2 ∂ q 1 ∂ Φ 2 ∂ q 2 ] = [ s i n ϕ 1 0 0 s i n ϕ 2 ] -J_{ta}=\frac{\partial \mathbf{\Phi}}{\partial \mathbf{q_a}}= \left[\begin{array}{c} \frac{\partial \Phi_1}{\partial q_1}&\frac{\partial \Phi_1}{\partial q_2} \\ \frac{\partial \Phi_2}{\partial q_1}&\frac{\partial \Phi_2}{\partial q_2} \end{array}\right]= \left[\begin{array}{c} sin\phi_1&0\\ 0&sin\phi_2 \end{array}\right] Jta=qaΦ=[q1Φ1q1Φ2q2Φ1q2Φ2]=[sinϕ100sinϕ2]

J t d = − ∂ Φ ∂ q d = − [ ∂ Φ 1 ∂ ϕ 1 ∂ Φ 1 ∂ ϕ 2 ∂ Φ 2 ∂ ϕ 1 ∂ Φ 2 ∂ ϕ 2 ] = − [ − s i n ϕ 1 ( x + d 2 ) + c o s ϕ 1 ( y − q 1 ) 0 0 − s i n ϕ 2 ( x − d 2 ) + c o s ϕ 2 ( y − q 2 ) ] J_{td}=-\frac{\partial \mathbf{\Phi}}{\partial \mathbf{q_d}}=-\left[\begin{array}{c} \frac{\partial \Phi_1}{\partial \phi_1}&\frac{\partial \Phi_1}{\partial \phi_2} \\ \frac{\partial \Phi_2}{\partial \phi_1}&\frac{\partial \Phi_2}{\partial \phi_2} \end{array}\right]= -\left[\begin{array}{c} -sin\phi_1(x+\frac{d}{2})+cos\phi_1(y-q_1)&0\\ 0&-sin\phi_2(x-\frac{d}{2})+cos\phi_2(y-q_2) \end{array}\right] Jtd=qdΦ=[ϕ1Φ1ϕ1Φ2ϕ2Φ1ϕ2Φ2]=[sinϕ1(x+2d)+cosϕ1(yq1)00sinϕ2(x2d)+cosϕ2(yq2)]

Thus q d ˙ = J t p ( J t t − J t a q a ˙ ) \dot\mathbf{q_{d}}=J_{tp}(J_t\mathbf{t}-J_{t_a}\dot\mathbf{q_a}) qd˙=Jtp(JttJtaqa˙)

Acceleration analysis of a Biglide

  • The direct kinematic model which gives the direct kinematic model
    [xdd, ydd] = dkm2(q1dd,q2dd, xd,yd,q1d,q2d, ϕ 1 d \phi_1d ϕ1d, ϕ 2 d \phi_2d ϕ2d,
    x,y,q1,q2, ϕ 1 \phi_1 ϕ1, ϕ 2 \phi_2 ϕ2)
    A t + B q ˙ = 0 s . t . A ˙ t + A t ˙ + B ˙ q ˙ + B t ¨ = 0 At+B\dot q=0\quad s.t.\quad \dot At+A\dot t+\dot B\dot q+B\ddot t=0 At+Bq˙=0s.t.A˙t+At˙+B˙q˙+Bt¨=0
  • The model that provides the passive angle acceleration
    [ ϕ 1 d d \phi_1dd ϕ1dd, ϕ 2 d d \phi_2dd ϕ2dd]=passive angleskm2(xdd,ydd,q1dd,q2dd,xd,yd,q1d,q2d, ϕ 1 d \phi_1d ϕ1d, ϕ 2 d \phi_2d ϕ2d,x,y,q1,q2, ϕ 1 \phi_1 ϕ1, ϕ 2 \phi_2 ϕ2)

J t t − J t a q a ˙ = J t p q d ˙ s . t . q d ¨ = J t p − [ J ˙ t t + J t t ˙ − ( J ˙ t a q a ˙ + J t a q a ¨ ) − J ˙ t p q d ˙ ] J_t\mathbf{t}-J_{t_a}\dot\mathbf{q_a}=J_{tp}\dot\mathbf{q_{d}}\quad s.t.\quad \ddot\mathbf{q_{d}}=J_{tp}^{-}[\dot J_t\mathbf{t}+J_t\dot\mathbf{t}-(\dot J_{t_a}\dot\mathbf{q_a}+J_{t_a}\ddot\mathbf{q_a})-\dot J_{tp}\dot\mathbf{q_{d}}] JttJtaqa˙=Jtpqd˙s.t.qd¨=Jtp[J˙tt+Jtt˙(J˙taqa˙+Jtaqa¨)J˙tpqd˙]

转载地址:http://glekk.baihongyu.com/

你可能感兴趣的文章