Planificació de Trajectòria: Polinomi Cúbic (PTP) 🛣️

Aquest mètode (Polinomi de 3r grau) genera una trajectòria suau entre un punt inicial ($q_0$) i un punt final ($q_f$), garantint sempre que la velocitat inicial i final siguin zero per evitar moviments bruscos.

Entrada de Dades de l'Usuari 🕹️

Suposem que volem moure la nostra articulació (per exemple, l'angle d'un motor) segons aquests paràmetres:

$ q(t_0) = \boldsymbol{q_0} = \mathbf{10^\circ} $ (Posició Inicial)

$ q(t_f) = \boldsymbol{q_f} = \mathbf{70^\circ} $ (Posició Final)

$ t_f = \mathbf{3 \text{ segons}} $ (Temps de Transició)

Nota: Per simplificar, assumim temps inicial $ t_0 = 0 \text{ s} $, velocitat inicial $ \dot{q}_0 = 0 $ i velocitat final $ \dot{q}_f = 0 $.

Fórmula i Solució Pas a Pas 📐

1. La Trajectòria i les seves Derivades

El polinomi de tercer grau té quatre coeficients ($a_0, a_1, a_2, a_3$) que hem de trobar:

$$ q(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 $$

Les seves derivades (Velocitat i Acceleració) són:

$$ \dot{q}(t) = a_1 + 2 a_2 t + 3 a_3 t^2 \quad \text{(Velocitat)} $$ $$ \ddot{q}(t) = 2 a_2 + 6 a_3 t \quad \text{(Acceleració)} $$

2. Aplicació de les Condicions de Contorn

Apliquem les quatre condicions (posició i velocitat a $t=0$ i $t=t_f$) per crear un sistema de 4 equacions:

Condició 1: Posició Inicial ($t=0$)

$ q(0) = q_0 = a_0 + a_1(0) + a_2(0) + a_3(0) \quad \Rightarrow \quad \mathbf{a_0 = q_0} $
Substituint dades: $ \mathbf{a_0 = 10} $

Condició 2: Velocitat Inicial ($t=0$)

$ \dot{q}(0) = \dot{q}_0 = a_1 + 2 a_2(0) + 3 a_3(0) \quad \Rightarrow \quad \mathbf{a_1 = \dot{q}_0} $
Com que $\dot{q}_0 = 0$: $ \mathbf{a_1 = 0} $

Condició 3: Posició Final ($t=t_f$)

$ q(t_f) = q_f = a_0 + a_1 t_f + a_2 t_f^2 + a_3 t_f^3 $

Condició 4: Velocitat Final ($t=t_f$)

$ \dot{q}(t_f) = \dot{q}_f = a_1 + 2 a_2 t_f + 3 a_3 t_f^2 $

3. Resolució del Sistema (Substitució de Valors)

Amb $a_0=10$, $a_1=0$, $q_f=70$ i $t_f=3$, simplifiquem i resolem per $a_2$ i $a_3$:

De la Condició 4 (Velocitat Final):

$ 0 = 0 + 2 a_2 (3) + 3 a_3 (3)^2 \quad \Rightarrow \quad 0 = 6 a_2 + 27 a_3 $
Aïllem $a_2$: $ a_2 = - \frac{27}{6} a_3 = \mathbf{-4.5 a_3} $

De la Condició 3 (Posició Final):

$ 70 = 10 + 0(3) + a_2 (3)^2 + a_3 (3)^3 $
$ 60 = 9 a_2 + 27 a_3 $
Substituïm $a_2 = -4.5 a_3$ a l'equació:
$ 60 = 9 (-4.5 a_3) + 27 a_3 $
$ 60 = -40.5 a_3 + 27 a_3 $
$ 60 = -13.5 a_3 \quad \Rightarrow \quad \mathbf{a_3 \approx -4.444} $

Càlcul Final de $a_2$:

$ a_2 = -4.5 a_3 = -4.5 (-4.444) \quad \Rightarrow \quad \mathbf{a_2 = 20} $

4. Solució Final de la Trajectòria 🚀

Substituint tots els coeficients trobats:

$$ \mathbf{a_0 = 10} \quad \mathbf{a_1 = 0} \quad \mathbf{a_2 = 20} \quad \mathbf{a_3 = -4.444} $$

L'equació de la trajectòria de l'articulació és:

$$ \mathbf{q(t) = 10 + 20 t^2 - 4.444 t^3} $$

Aquesta funció dóna la posició de l'articulació (en graus) per a qualsevol instant $t$ entre 0 i 3 segons. El controlador utilitzarà aquesta funció per calcular el moment $\boldsymbol{\tau}$ necessari a cada pas de temps (utilitzant la dinàmica).

No connectat

Planificació i Dinàmica de Trajectòria 2-DOF amb Traça 🦾

Entrada de Dades

Articulació 1

Articulació 2

Paràmetres comuns

Resultats

Animació del Braç 2-DOF i Traça 🧭

Planificació de Trajectòria: Polinomi Cúbic (PTP) 🛣️

Entrada de Dades de l'Usuari 🕹️

Fórmula i Solució Pas a Pas 📐

1. La Trajectòria i les seves Derivades

2. Aplicació de les Condicions de Contorn

Condició 1: Posició Inicial (\(t=0\))

Condició 2: Velocitat Inicial (\(t=0\))

Condició 3: Posició Final (\(t=t_f\))

Condició 4: Velocitat Final (\(t=t_f\))

3. Resolució del Sistema (Substitució de Valors)

De la Condició 4 (Velocitat Final):

De la Condició 3 (Posició Final):

Càlcul Final de \(a_2\):

4. Solució Final de la Trajectòria 🚀

Codi Arduino per Braccio (2 servos via WebSerial)