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

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(-\frac{d}{2},q_1)$ and $A_2(\frac{d}{2},q_2)$,thus point H is the mid-point of $A_1$ and$A_2$,its position is$(0,\frac{q_1+q_2}{2})$.

• Compute the vector $A_{2}H$ $$A_{2}H=H-A_2=(-\frac{d}{2},\frac{q_1-q_2}{2})$$
• There are two solutions for the point A 13 , obtained by the $90^o$ rotation of the vector $A_{2}H$, normalization and multiplying by the
  distance h, giving$$HC=\gamma \frac{h}{a}\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]A_{2}H$$
  where $a=||A_{2}H||$ and $h=\sqrt{l^2-a^2}$
  $$\begin{gathered} OC=O{O}_2+O_2{A}_2+A_{2}H+HC \\ 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] \end{gathered}$$
  Thus we have $$\begin{gathered} x=OC(1) \\ y=OC(2) \end{gathered}$$

The model which gives the passive angle position [${\varphi }_1$ ${\varphi }_2$] = passive angles(x,y,q1,q2)

$$OC=O{O}_i+O_i{A}_i+A_{i}C$$ For leg1:$$\begin{gathered} OC=O{O}_1+O_1{A}_1+A_{1}C \\ OC-O{O}_1-O_1{A}_1=A_{1}C \end{gathered}$$ We have $$\begin{gathered} x+\frac{d}{2}=l_{A_{1}C}cos{\varphi }_1(1) \\ y-q_1=l_{A_{1}C}sin{\varphi }_1(2) \end{gathered}$$ $\frac{(2)}{(1)}$,we get $${\varphi }_1=atan2(\frac{y-q_1}{x+\frac{d}{2}})$$ After using the same process,we have $${\varphi }_2=atan2(\frac{y-q_2}{x-\frac{d}{2}})$$

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

From equation(1) and(2),we have$$\begin{gathered} (x+\frac{d}{2}{)}^2+(y-q_1{)}^2=l_{A_{1}C}^2 \\ (x-\frac{d}{2}{)}^2+(y-q_2{)}^2=l_{A_{2}C}^2 \end{gathered}$$ such that $$\begin{gathered} q_1=y+/-\sqrt{l_{A_{1}C}^2-(x+\frac{d}{2}{)}^2} \\ {q}_2=y+/-\sqrt{l_{A_{2}C}^2-(x-\frac{d}{2}{)}^2} \end{gathered}$$

Velocity analysis of a Biglide

The direct kinematic model which gives

[xd, yd] = dkm(q1d,q2d,x,y,q1,q2,${\varphi }_1$,${\varphi }_2$)

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

$$\begin{gathered} h_1:(x+\frac{d}{2}{)}^2+(y-q_1{)}^2-l_{A_{1}C}^2=0 \\ {h}_2:(x-\frac{d}{2}{)}^2+(y-q_2{)}^2-l_{A_{2}C}^2=0 \end{gathered}$$

$$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{\varphi }_1& sin{\varphi }_1\\ cos{\varphi }_2& sin{\varphi }_2\end{array}\right]$$

$$B=\frac{\partial \mathbf{h}}{\partial {\mathbf{q}}_{\mathbf{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{\varphi }_1& 0\\ 0& sin{\varphi }_2\end{array}\right]$$ Thus,we have $\dot{{\mathbf{q}}_{\mathbf{a}}}=-B^{-1}A\mathbf{t}$.

The model that provides the passive joint velocities

[${\varphi }_{1}d$,${\varphi }_{2}d$] =passiveangleskm(xd,yd,q1d,q2d,x,y,q1,q2,${\varphi }_1$,${\varphi }_2$) $$\begin{gathered} x+\frac{d}{2}=l_{A_{1}C}cos{\varphi }_1(1) \\ y-q_1=l_{A_{1}C}sin{\varphi }_1(2) \end{gathered}$$we rewrite it into matrix formula:

$$\begin{gathered} \left[\begin{array}{c}x+\frac{d}{2}\\ y-q_1\end{array}\right]=l_{A_{1}C}\left[\begin{array}{c}cos{\varphi }_1\\ sin{\varphi }_1\end{array}\right] \\ [cos{\varphi }_{1}sin{\varphi }_1]\left[\begin{array}{c}x+\frac{d}{2}\\ y-q_1\end{array}\right]=l_{A_{1}C} \end{gathered}$$

We obtained$$\begin{gathered} {\Phi }_1:cos{\varphi }_1(x+\frac{d}{2})+sin{\varphi }_1(y-q_1)-l=0 \\ {\Phi }_2:cos{\varphi }_2(x-\frac{d}{2})+sin{\varphi }_2(y-q_2)-l=0 \end{gathered}$$ To calculate passive joint velocity，we use $J_t\mathbf{t}-J_{t_a}\dot{{\mathbf{q}}_{\mathbf{a}}}=J_{tp}\dot{{\mathbf{q}}_{\mathbf{d}}}$. $$J_t=\frac{\partial \mathbf{\Phi }}{\partial \mathbf{x}}-J_{ta}=\frac{\partial \mathbf{\Phi }}{\partial {\mathbf{q}}_{\mathbf{a}}}{J}_{td}=-\frac{\partial \mathbf{\Phi }}{\partial {\mathbf{q}}_{\mathbf{d}}}$$ $$J_t=\frac{\partial \mathbf{\Phi }}{\partial \mathbf{x}}=\left[\begin{array}{cc}\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}{cc}cos{\varphi }_1& sin{\varphi }_1\\ cos{\varphi }_2& sin{\varphi }_2\end{array}\right]$$ $$-J_{ta}=\frac{\partial \mathbf{\Phi }}{\partial {\mathbf{q}}_{\mathbf{a}}}=\left[\begin{array}{cc}\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}{cc}sin{\varphi }_1& 0\\ 0& sin{\varphi }_2\end{array}\right]$$

$$J_{td}=-\frac{\partial \mathbf{\Phi }}{\partial {\mathbf{q}}_{\mathbf{d}}}=-\left[\begin{array}{cc}\frac{\partial {\Phi }_1}{\partial {\varphi }_1}& \frac{\partial {\Phi }_1}{\partial {\varphi }_2}\\ \frac{\partial {\Phi }_2}{\partial {\varphi }_1}& \frac{\partial {\Phi }_2}{\partial {\varphi }_2}\end{array}\right]=-\left[\begin{array}{cc}-sin{\varphi }_1(x+\frac{d}{2})+cos{\varphi }_1(y-q_1)& 0\\ 0& -sin{\varphi }_2(x-\frac{d}{2})+cos{\varphi }_2(y-q_2)\end{array}\right]$$

Thus $\dot{{\mathbf{q}}_{\mathbf{d}}}=J_{tp}(J_t\mathbf{t}-J_{t_a}\dot{{\mathbf{q}}_{\mathbf{a}}})$

Acceleration analysis of a Biglide

• The direct kinematic model which gives the direct kinematic model
  [xdd, ydd] = dkm2(q1dd,q2dd, xd,yd,q1d,q2d,${\varphi }_{1}d$,${\varphi }_{2}d$,
  x,y,q1,q2,${\varphi }_1$,${\varphi }_2$)
  $$At+B\dot{q}=0s.t.\dot{A}t+A\dot{t}+\dot{B}\dot{q}+B\ddot{t}=0$$
• The model that provides the passive angle acceleration
  [${\varphi }_{1}dd$,${\varphi }_{2}dd$]=passive angleskm2(xdd,ydd,q1dd,q2dd,xd,yd,q1d,q2d,${\varphi }_{1}d$,${\varphi }_{2}d$,x,y,q1,q2,${\varphi }_1$,${\varphi }_2$)

$$J_t\mathbf{t}-J_{t_a}\dot{{\mathbf{q}}_{\mathbf{a}}}=J_{tp}\dot{{\mathbf{q}}_{\mathbf{d}}}s.t.\ddot{{\mathbf{q}}_{\mathbf{d}}}=J_{tp}^{-}[{\dot{J}}_t\mathbf{t}+J_t\dot{\mathbf{t}}-({\dot{J}}_{t_a}\dot{{\mathbf{q}}_{\mathbf{a}}}+J_{t_a}\ddot{{\mathbf{q}}_{\mathbf{a}}})-{\dot{J}}_{tp}\dot{{\mathbf{q}}_{\mathbf{d}}}]$$

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