0
$\begingroup$

I'm trying to control a robot and I have somewhere bug and I can't found it. I would be grateful if is there anybody who would check my equations. The robot diagram can be found here 1. I would like to obtain differential equations of the robot by using the Lagrange equation of motion. Here is what have I done.

$$x_1 = X + L_2\cos\theta_2 + P_1\cos (\theta_1 + \theta_2) $$ $$ z_1 = Z + L_2\sin\theta_2 + P_1\sin (\theta_1 + \theta_2)$$

$$\dot{x}_1 = - L_2\sin{\theta}_2\dot{\theta}_2 - P_1\sin(\theta_1 + \theta_2)(\dot{\theta}_1 + \dot{\theta}_2) $$ $$ z_1 = L_2\cos{\theta_2}\dot{\theta_2} + P_1\cos(\theta_1 + \theta_2)(\dot{\theta}_1 + \dot{\theta}_2)$$

$$x_2 = X + P_2\cos\theta_2 $$ $$z_2 = Z + P_2\sin\theta_2 $$

$$\dot{x}_2 = -P_2\sin\theta_2\dot{\theta}_2 $$ $$\dot{z}_2 = P_2\cos\theta_2\dot{\theta}_2 $$

$$x_m = X + L_2\cos\theta_2 + P_m\cos (\theta_1 + \theta_2 + \theta_m) $$ $$ z_m = Z + L_2\sin\theta_2 + P_m\sin (\theta_1 + \theta_2 + \theta_m)$$

$$\dot{x}_m = - L_2\sin\theta_2\dot{\theta}_2 - P_m\sin(\theta_1 + \theta_2 + \theta_m)(\dot{\theta}_1 + \dot{\theta}_2 + \dot{\theta}_m)$$ $$ \dot{z}_m = L_2\cos\theta_2\dot{\theta}_2 + P_m\cos(\theta_1 + \theta_2 + \theta_m)(\dot{\theta}_1 + \dot{\theta}_2 + \dot{\theta}_m)$$

Potential energy:

$$T_1 = \frac{1}{2}M_1(\dot{x}_1^2 + \dot{y}_1^2) + \frac{1}{2}I_1(\dot{\theta_1} + \dot{\theta_2})^2$$

$$T_2 = \frac{1}{2}M_2(\dot{x}_2^2 + \dot{y}_2^2) + \frac{1}{2}I_2\dot{\theta}_2^2$$

$$T_m = \frac{1}{2}M_m(\dot{x}_m^2 + \dot{y}_m^2) + \frac{1}{2}I_m(\dot{\theta}_1 + \dot{\theta}_2 + \dot{\theta}_m)^2$$

Kinetic energy: Gravity which is pointing downward $$ U_1 = M_1gy_1 $$ $$ U_2 = M_2gy_2 $$ $$ U_m = M_mgy_m $$

Spring with stiffness coefficient K: $$ \Delta L_p = \sqrt{L_m^2 + L_k^2 - 2L_kL_m\cos(\theta_m + \theta_1-\pi))} - L_{p0} $$ $$ Up = 1/2K({\Delta L_p})^2 $$

Where coefficents are:

  • $M_1$: Mass of link 1 in center of gravity
  • $M_2$: Mass of link 2 in center of gravity
  • $M_m$: Mass of link m in center of gravity
  • $L_1$: Length of link 1
  • $L_2$: Length of link 2
  • $L_m$: Length of link m
  • $P_1$: Position of center of mass of link 1
  • $P_2$: Position of center of mass of link 2
  • $P_m$: Position of center of mass of link m
  • $L_{p0}$: Initial lenght of the spring

Generalized coordiantes are angles $\vec{q} = [\theta_1, \theta_2, \theta_m]$, $\vec{\dot{q}} = [\dot{\theta}_1, \dot{\theta}_2, \dot{\theta}_m]$.

Generalize forces $Q = [0 0 \tau_m]$ is torque on motor which is connected in $\theta_m$ angle.

Lagrange equation of motion:

$$L = T - U$$ $$ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q}_i} \right) - \frac{\partial L}{\partial q_i} = Q $$

from this i will obtain 3 very long equations, so I will not include them. I done this in Matlab. After that I used substitution:

$$\dot{\Theta}_1 = \ddot{\theta}_1$$ $$\Theta_1 = \dot{\theta}_1$$

$$\dot{\Theta}_2 = \ddot{\theta}_2$$ $$\Theta_2 = \dot{\theta}_2$$

$$\dot{\Theta}_m = \ddot{\theta}_m$$ $$\Theta_m = \dot{\theta}_m$$

and solved them to receive: $$\dot{\theta}_1 = \Theta_1$$ $$\dot{\theta}_2 = \Theta_2$$ $$\dot{\theta}_m = \Theta_m$$ $$\dot{\Theta}_1 = f_1(\theta_1, \theta_2, \theta_m, \Theta_1, \Theta_2, \Theta_m, \tau_m)$$ $$\dot{\Theta}_2 = f_1(\theta_1, \theta_2, \theta_m, \Theta_1, \Theta_2, \Theta_m, \tau_m)$$ $$\dot{\Theta}_m = f_1(\theta_1, \theta_2, \theta_m, \Theta_1, \Theta_2, \Theta_m, \tau_m)$$

When I try to solve this in ode45 in Matlab it acts unrealistically. When I try linearization is an arbitrary working point then matrix A and B have numbers in 1e4. I tried many types of control but all failed. I check everything but I'm unable to say whenever are the equations correct.

Thank you for your help.

$\endgroup$
0
$\begingroup$

Follows a MATHEMATICA script which simulates the movement equations for a set of parameters specified in parms

p0 = {X, Z};
p1 = p0 + L2 {Cos[theta2[t]], Sin[theta2[t]]} + P1 {Cos[theta1[t] + theta2[t]], Sin[theta1[t] + theta2[t]]};
p2 = p0 + P2 {Cos[theta2[t]], Sin[theta2[t]]};
pm = p0 + L2 {Cos[theta2[t]], Sin[theta2[t]]} + Pm {Cos[theta1[t] + theta2[t] + thetam[t]], Sin[theta1[t] + theta2[t] + thetam[t]]};
v1 = D[p1, t];
v2 = D[p2, t];
vm = D[pm, t];
T1 = 1/2 M1 p1.p1 + 1/2 I1 D[theta1[t] + theta2[t], t]^2;
T2 = 1/2 M2 p2.p2 + 1/2 I2 D[theta2[t], t]^2;
Tm = 1/2 Mm pm.pm + 1/2 Im D[theta1[t] + theta2[t] + thetam[t], t]^2;
T = T1 + T2 + Tm;
delLp = Sqrt[Lm^2 + Lk^2 - 2 Lm Lk Cos[thetam[t] + theta1[t] - Pi]] - Lp0;
V = g (M1 p1 + M2 p2 + Mm pm).{0, 1} + 1/2 K0 delLp^2;
L = T - V;
vars = {theta1[t], theta2[t], thetam[t]};
torque = {0, 0, tau};
equsmov = D[Grad[L, D[vars, t]], t] - Grad[L, vars] - torque;

parms = {L2 -> 1, P2 -> 1/2, L1 -> 1/2, P1 -> 0.5, Lm -> 1/4, Pm -> 1/8, Lk -> 3/4, I1 -> 1, I2 -> 2, Im -> 1/2, K0 -> 1000, Lp0 -> 1/2, g -> 10, M1 -> 1, M2 -> 2, Mm -> 1/2, X -> 0, Z -> 0};

ode = Thread[(equsmov /. parms /. tau -> 0) == 0];

tmax = 10;
cinits = {theta1[0] == 0.01, theta2[0] == -Pi/2, thetam[0] == 0, theta1'[0] == 0, theta2'[0] == 0, thetam'[0] == 0};
solthetas = NDSolve[Join[ode, cinits], vars, {t, 0, tmax}][[1]];

ParametricPlot[Evaluate[{p1, p2, pm} /. parms /. solthetas], {t, 0, tmax}]

Follows a simulation plot showing in abscense of input, and hanging, the pendular behavior.

enter image description here

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.