本文共 9618 字,大约阅读时间需要 32 分钟。
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).
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+A1COC−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)y−q1=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+2dy−q1) 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(x−2dy−q2)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+(y−q1)2=lA1C2(x−2d)2+(y−q2)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)2q2=y+/−lA2C2−(x−2d)2The 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+(y−q1)2−lA1C2=0h2:(x−2d)2+(y−q2)2−lA2C2=0A = ∂ 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=∂x∂h=[∂x∂h1∂x∂h2∂y∂h1∂y∂h1]=[2(x+2d)2(x−2d)2(y−q1)2(y−q2)]=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=∂qa∂h=[∂q1∂h1∂q1∂h2∂q2∂h1∂q2∂h1]=[2(y−q1)002(y−q2)]=2l[sinϕ100sinϕ2] Thus,we have q a ˙ = − B − 1 A t \dot \mathbf{q_a}=-B^{-1}A\mathbf{t} qa˙=−B−1At.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)y−q1=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+2dy−q1]=lA1C[cosϕ1sinϕ1][cosϕ1sinϕ1][x+2dy−q1]=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(y−q1)−l=0Φ2:cosϕ2(x−2d)+sinϕ2(y−q2)−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}} Jtt−Jtaqa˙=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∂Φ1∂x∂Φ2∂y∂Φ1∂y∂Φ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∂Φ1∂q1∂Φ2∂q2∂Φ1∂q2∂Φ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(y−q1)00−sinϕ2(x−2d)+cosϕ2(y−q2)]
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(Jtt−Jtaqa˙)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}}] Jtt−Jtaqa˙=Jtpqd˙s.t.qd¨=Jtp−[J˙tt+Jtt˙−(J˙taqa˙+Jtaqa¨)−J˙tpqd˙]
转载地址:http://glekk.baihongyu.com/