Exercice 2:
Modèles direct et inverse
d'un robot plan 3R
Introduction à la Robotique
TD 1 - Modélisation Mécanique
-
Robot Plan 3R
- 3 segments (l1, l2 et l3)
- 3 pivots θ1 , θ2 , θ3 d'axe z
3 DDL dans le plan (x, y, θz ) - coordonnées généralisées
3 actionneurs (θ1 , θ2 , θ3) - coordonées articulaires
- Modèle direct: Trouver ( x, y, θf ) en connaissant (θ1 , θ2 , θ3)
- Modèle inverse: Trouver (θ1 , θ2 , θ3) en connaissant (x,y,θf )
Modèle géométrique direct
OE = (x, y, θz) = f(θ1 , θ2 , θ3 )
R1'
R1
R2
R3
R2'
R1
R2
R3
R0
^0H_1 = \begin{pmatrix}
\cos(\theta_1) & -\sin(\theta_1) & l_1 \cos(\theta_1) \\
\sin(\theta_1) & \cos(\theta_1) & l_1 \sin(\theta_1)\\
0 & 0 & 1 \\ \end{pmatrix}
^1H_2 = \begin{pmatrix}
\cos(\theta_2) & -\sin(\theta_2) & l_2 \cos(\theta_2) \\
\sin(\theta_2) & \cos(\theta_1) & l_2 \sin(\theta_2)\\
0 & 0 & 1 \\ \end{pmatrix}
^2H_3 = \begin{pmatrix}
\cos(\theta_3) & -\sin(\theta_3) & l_3 \cos(\theta_3) \\
\sin(\theta_3) & \cos(\theta_3) & l_3 \sin(\theta_3)\\
0 & 0 & 1 \\ \end{pmatrix}
^0H_3 =\, ^0H_1 \times \,^1H_2 \times \,^2H_3
\begin{bmatrix}
x\\
y\\
1\\
\end{bmatrix}
= \, ^0H_3 \times
\begin{bmatrix}
0 \\ 0 \\ 1 \\
\end{bmatrix}
\begin{bmatrix}
x\\
y\\
1\\
\end{bmatrix}
= \, ^0H_3 \times
\, ^3E
\begin{array}{ccl}
x &=& H_{\left[31\right]} \\
y &=& H_{\left[32\right]} \\
\end{array}
\begin{array}{ccl}
x &=& H_{\left[31\right]} \\
y &=& H_{\left[32\right]} \\
\theta_z &=& \theta_1 +\theta_2 +\theta_3
\end{array}
^0\vec{OA} = l_1 \cos{(\theta_1)}\, \vec{x_o} + l_1 \sin{(\theta_1)}\, \vec{y_o}
^0\vec{AB} = l_2 \cos{(\theta_1+\theta_2)}\, \vec{x_o} + l_2 \sin{(\theta_1+\theta_2)}\,\vec{y_o}
^0\vec{BE} = l_3\cos{(\theta_1+\theta_2+\theta_3)}\, \vec{x_o} + l_3 \sin{(\theta_1+\theta_2+\theta_3)}\, \vec{y_o}
^0\vec{OE}=
\begin{bmatrix} x \\ y\\ \end{bmatrix}
=
\begin{bmatrix}
l_1\cos{(\theta_1)}+ l_2\cos{(\theta_1+\theta_2)}+ l_3\cos{(\theta_1+\theta_2+\theta_3)} \\
l_1\sin{(\theta_1)}+ l_2\sin{(\theta_1+\theta_2)}+ l_3\sin{(\theta_1+\theta_2+\theta_3)} \\
\end{bmatrix}
Avec des équations analytiques
\theta_z = \theta_1 +\theta_2 +\theta_3
Modèle géométrique inverse
(θ1 , θ2 , θ3 ) = f -1(x, y, θz)
\left\{
\begin{array}{ccl} x &=&
l_1\cos{(\theta_1)}+ l_2\cos{(\theta_1+\theta_2)}+ l_3\cos{(\theta_1+\theta_2+\theta_3)} \\
y &=& l_1\sin{(\theta_1)}+ l_2\sin{(\theta_1+\theta_2)}+ l_3\sin{(\theta_1+\theta_2+\theta_3)} \\
\theta_z &=& \theta_1+\theta_2+\theta_3
\end{array}
\right.
\left\{
\begin{matrix} x - l_3\cos{(\theta_z)} &=&
l_1\cos{(\theta_1)}+ l_2\cos{(\theta_1+\theta_2)} \\
y - l_3\sin{(\theta_z)} &=& l_1\sin{(\theta_1)}+ l_2\sin{(\theta_1+\theta_2)} \\
\end{matrix}
\right.
\begin{matrix}
(1) \\
(2)\\
\end{matrix}
(l_1\cos{(\theta_1)}+ l_2\cos{(\theta_1+\theta_2)} )^2 = \\
\begin{array} {lll}
l_1^2\cos^2{(\theta_1)} + l_2^2\cos^2{(\theta_1+\theta_2)} + 2\, l_1\cos{(\theta_1)}\, l_2\cos{(\theta_1+\theta_2)}
\end{array}
(1)^2 :
MGD
\left\{
\begin{matrix} x - l_3\cos{(\theta_z)} &=&
l_1\cos{(\theta_1)}+ l_2\cos{(\theta_1+\theta_2)} \\
y - l_3\sin{(\theta_z)} &=& l_1\sin{(\theta_1)}+ l_2\sin{(\theta_1+\theta_2)} \\
\end{matrix}
\right.
\begin{matrix}
(1) \\
(2)\\
\end{matrix}
\begin{array} {lll}
l_1^2\cos^2{(\theta_1)} + l_2^2\cos^2{(\theta_1+\theta_2)} + 2\, l_1\cos{(\theta_1)}\, l_2\cos{(\theta_1+\theta_2)}
\end{array}
(1)^2 :
l_1^2\sin^2{(\theta_1)} + l_2^2\sin^2{(\theta_1+\theta_2)} + 2\, l_1\sin{(\theta_1)}\, l_2\sin{(\theta_1+\theta_2)}
(l_1\sin{(\theta_1)}+ l_2\sin{(\theta_1+\theta_2)} )^2 = \\
(2)^2 :
\left\{
\begin{matrix} x - l_3\cos{(\theta_z)} &=&
l_1\cos{(\theta_1)}+ l_2\cos{(\theta_1+\theta_2)} \\
y - l_3\sin{(\theta_z)} &=& l_1\sin{(\theta_1)}+ l_2\sin{(\theta_1+\theta_2)} \\
\end{matrix}
\right.
\begin{matrix}
(1) \\
(2)\\
\end{matrix}
\begin{array} {lll}
l_1^2\cos^2{(\theta_1)} + l_2^2\cos^2{(\theta_1+\theta_2)} + 2\, l_1\cos{(\theta_1)}\, l_2\cos{(\theta_1+\theta_2)}
\end{array}
(1)^2 :
l_1^2\sin^2{(\theta_1)} + l_2^2\sin^2{(\theta_1+\theta_2)} + 2\, l_1\sin{(\theta_1)}\, l_2\sin{(\theta_1+\theta_2)}
(2)^2 :
(x - l_3\cos{\theta_z)^2 } + (y - l_3\sin{\theta_z)^2 } =
l_1^2 + l_2^2 + 2l_1l_2\cos{(\theta_2)} \\
(1)^2 + (2)^2 :
\cos(a-b) = \cos(a)\cos(b)+\sin(a)\sin(b)
(x - l_3\cos{\theta_z)^2 } + (y - l_3\sin{\theta_z)^2 } =
l_1^2 + l_2^2 + 2l_1l_2\cos{(\theta_2)} \\
(x - l_3\cos{\theta_z)^2 } + (y - l_3\sin{\theta_z)^2 } -
l_1^2 - l_2^2 = 2l_1l_2\cos{(\theta_2)} \\
\left.
\begin{array}{lcl}
\cos{(\theta_2)} &=& \frac{(x - l_3\cos{\theta_z)^2 } + (y - l_3\sin{\theta_z)^2 } -
l_1^2 - l_2^2 }{(2l_1l_2) } \\[6pt]
\sin (\theta_2) &=& \pm \sqrt{1-\cos^2(\theta_2)} \\[6pt]
\end{array}
\right\} \arctan(.)
\Rightarrow \quad \theta_2 \quad ou \quad -\theta_2
\begin{array}{cl}
S_1:& (\theta_1, \theta_2, \theta_3)\\
S_2:& (\theta'_1, -\theta_2, \theta'_3)\\
\end{array}
DEUX configurations avec deux θ2 opposés
\left\{
\begin{matrix} x - l_3\cos{(\theta_z)} &=&
l_1\cos{(\theta_1)}+ l_2\cos{(\theta_1+\theta_2)} \\
y - l_3\sin{(\theta_z)} &=& l_1\sin{(\theta_1)}+ l_2\sin{(\theta_1+\theta_2)} \\
\end{matrix}
\right.
\left\{
\begin{matrix}
(l_1+l_2\cos\theta_2)\cos (\theta_1) - (l_2\sin\theta_2)\sin(\theta_1) = x - l_3\cos(\theta_z)\\
(l_1+l_2\cos\theta_2)\sin (\theta_1) + (l_2\sin\theta_2)\cos(\theta_1) = y - l_3\sin(\theta_z)
\end{matrix}
\right.
\left\{
\begin{matrix}
\cos(\theta_1) = \frac {k_2w_2 + k_1w_1}{k_1^2 + k_2^2 } \\[4pt]
\sin(\theta_1) = \frac {k_1w_2 - k_2w_1}{k_1^2 + k_2^2 } \\[4pt]
\end{matrix}
\right.
\Rightarrow
\left\{
\begin{array}{ccr}
\theta_1 &pour& \theta_2 \\[2pt]
\theta'_1 &pour& -\theta_2\\[2pt]
\end{array}
\right.
k1
k1
k2
k2
w1
w2
\left\{
\begin{array}{ccl}
\theta_3 &=& \theta_z - (\theta_1 + \theta_2)\\
\theta'_3 &=& \theta_z - (\theta'_1 - \theta_2)
\end {array}
\right.
\begin{array}{cl}
S_1:& (\theta_1, \theta_2, \theta_3)\\
S_2:& (\theta'_1, -\theta_2, \theta'_3)\\
\end{array}
Deux Configurations qui donnent les mêmes Positions et Orientations finales
\begin{array}{cl}
S_1:& (\theta_1, \theta_2, \theta_3)\\
\end{array}
\begin{array}{cl}
S_2:& (\theta'_1, -\theta_2, \theta'_3)\\
\end{array}
Domaine atteignable
l3 = 0 -> cas 2R
-1 \leqslant \cos\theta_2 \leqslant 1 \quad et \quad l_3 = 0
l_1^2 + l_2^2 - 2l_1l_2 \leqslant x^2 + y^2 \leqslant
l_1^2 + l_2^2 + 2l_1l_2 \\
(l_1-l_2)^2 \leqslant x^2 + y^2 \leqslant
(l_1+l_2)^2
(x - l_3\cos{\theta_z)^2 } + (y - l_3\sin{\theta_z)^2 } =
l_1^2 + l_2^2 + 2l_1l_2\cos{(\theta_2)} \\
(l_1-l_2)^2 \leqslant x^2 + y^2 \leqslant
(l_1+l_2)^2
Le robot peut atteindre les points (x,y)
s'ils sont sur le disque gris
r1
r2
r1
r2
Modèles Cinématiques
-
MCD
-
MCI
(\dot{x},\dot{y},\dot{\theta}_z) = f(\dot{\theta}_1, \dot{\theta}_2, \dot{\theta}_3)
(\dot{\theta}_1, \dot{\theta}_2, \dot{\theta}_3) = f^{-1}(\dot{x},\dot{y},\dot{\theta}_z)
\frac{\partial}{\partial t}
MCD = MGD(t)
\left\{
\begin{array}{ccl} x &=&
l_1\cos{(\theta_1)}+ l_2\cos{(\theta_1+\theta_2)}+ l_3\cos{(\theta_1+\theta_2+\theta_3)} \\[4pt]
y &=& l_1\sin{(\theta_1)}+ l_2\sin{(\theta_1+\theta_2)}+ l_3\sin{(\theta_1+\theta_2+\theta_3)} \\[4pt]
\theta_z &=& \theta_1+\theta_2+\theta_3
\end{array}
\right.
MGD
\left\{
\begin{array}{ccl}
\frac{\partial}{\partial t} x&=&
\frac{\partial}{\partial t} \left[
l_1\cos{(\theta_1)}+ l_2\cos{(\theta_1+\theta_2)} +
l_3\cos{(\theta_1+\theta_2+\theta_3)} \right] \\[6pt]
\frac{\partial}{\partial t} y &=&
\frac{\partial}{\partial t} \left[
l_1\sin{(\theta_1)}
+ l_2\sin{(\theta_1+\theta_2)}
+ l_3\sin{(\theta_1+\theta_2+\theta_3)} \right] \\[6pt]
\frac{\partial}{\partial t} \theta_z &=&
\frac{\partial}{\partial t} \left[ \theta_1+ \theta_2+ \theta_3 \right]
\end{array}
\right.
MCD
\left\{
\begin{array}{ccl}
\frac{\partial}{\partial t} x&=&
\frac{\partial}{\partial t} \left[
l_1\cos{(\theta_1)}+ l_2\cos{(\theta_1+\theta_2)} +
l_3\cos{(\theta_1+\theta_2+\theta_3)} \right] \\[2pt]
\frac{\partial}{\partial t} y &=&
\frac{\partial}{\partial t} \left[
l_1\sin{(\theta_1)}
+ l_2\sin{(\theta_1+\theta_2)}
+ l_3\sin{(\theta_1+\theta_2+\theta_3)} \right] \\[2pt]
\frac{\partial}{\partial t} \theta_z &=&
\frac{\partial}{\partial t} \left[ \theta_1+ \theta_2+ \theta_3 \right]
\end{array}
\right.
MCD
\left\{
\begin{array}{ccl}
\dot{x}&=&
- l_1 \dot{\theta_1} \sin{(\theta_1)}
- l_2 (\dot{\theta}_{12}) \sin{(\theta_{12})}
- l_3 (\dot{\theta}_{123}) \sin{(\theta_{123})} \\[6pt]
\dot{y} &=&
l_1 \dot{\theta_1}\cos{(\theta_1)}
+ l_2 (\dot{\theta}_{12})\cos{(\theta_{12})}
+ l_3 (\dot{\theta}_{123}) \cos{(\theta_{123})} \\[6pt]
\dot{\theta_z} &=&
\dot{\theta_1}+ \dot{\theta_2}+ \dot{\theta_3}
\end{array}
\right.
\frac{\partial}{\partial t} \cos(\theta)= - \dot{\theta}\sin({\theta}) \quad et\quad
\frac{\partial}{\partial t} \sin(\theta)= \dot{\theta}\cos({\theta})
\left\{
\begin{array}{ccl}
\dot{x}&=&
- l_1 \dot{\theta_1} \sin{(\theta_1)}
- l_2 (\dot{\theta}_{12}) \sin{(\theta_{12})}
- l_3 (\dot{\theta}_{123}) \sin{(\theta_{123})} \\[6pt]
\dot{y} &=&
l_1 \dot{\theta_1}\cos{(\theta_1)}
+ l_2 (\dot{\theta}_{12})\cos{(\theta_{12})}
+ l_3 (\dot{\theta}_{123}) \cos{(\theta_{123})} \\[6pt]
\dot{\theta_z} &=&
\dot{\theta_1}+ \dot{\theta_2}+ \dot{\theta_3}
\end{array}
\right.
\begin{bmatrix}
\dot{x} \\[4pt]
\dot{y} \\[4pt]
\dot{\theta}_z \\[4pt]
\end{bmatrix}
=
\begin{bmatrix}
- l_1 \sin{(\theta_1)} - l_2 \sin{(\theta_{12})} - l_3 \sin{(\theta_{123})} & - l_2 \sin{(\theta_{12})} - l_3 \sin{(\theta_{123})} & - l_3 \sin{(\theta_{123})}\\[8pt]
l_1 \cos{(\theta_1)} + l_2 \cos{(\theta_{12})} + l_3 \cos{(\theta_{123})} & l_2 \cos{(\theta_{12})} + l_3 \cos{(\theta_{123})} & l_3 \cos{(\theta_{123})}\\[8pt]
1 & 1 & 1
\end{bmatrix}
\begin{bmatrix}
\dot{\theta}_1 \\[4pt]
\dot{\theta}_2 \\[4pt]
\dot{\theta}_3 \\[4pt]
\end{bmatrix}
Matrice Jacobienne
\dot{X} = J \dot{\Theta}
\left\{
\begin{array}{ccr}
\dot{x}&=&
[ -l_1 \sin{(\theta_1)} -l_2 \sin{(\theta_{12})} - l_3 \sin{(\theta_{123})}] \quad \dot{\theta}_{1} \\[4pt]
& &- [ l_2 \sin{(\theta_{12})} +l_3 \sin{(\theta_{123})} ] \quad \dot{\theta}_{2} \\[4pt]
& &- l_3 \sin{(\theta_{123})} \quad \dot{\theta}_{3} \\[6pt]
\dot{y}&= \dots & \\
\dot{\theta_z}&= \dots & \\
\end{array}
\right\}
J =
\begin{bmatrix}
\frac{\partial x}{\partial \theta_1} & \frac{\partial x}{\partial \theta_2} & \frac{\partial x}{\partial \theta_3} \\[4pt]
\frac{\partial y}{\partial \theta_1} & \frac{\partial y}{\partial \theta_2} & \frac{\partial y}{\partial \theta_3} \\[4pt]
\frac{\partial z}{\partial \theta_1} & \frac{\partial z}{\partial \theta_2} & \frac{\partial z}{\partial \theta_3} \\[4pt]
\end{bmatrix}
\dot{X} = J \dot{\Theta}
J^{-1}\dot{X} =\dot{\Theta}
Matrice Jacobienne
J =
\begin{bmatrix}
\frac{\partial x}{\partial \theta_1} & \frac{\partial x}{\partial \theta_2} \\[4pt]
\frac{\partial y}{\partial \theta_1} & \frac{\partial y}{\partial \theta_2} \\[4pt]
\end{bmatrix}
J^{-1}=\begin{bmatrix}
j_{11} & j_{12} \\
j_{21} & j_{22}
\end{bmatrix}^{-1}
=\frac{1}{ \det(J)}
\begin{bmatrix}
j_{22} & -j_{12} \\
-j_{21} & j_{11}
\end{bmatrix}
\det A = \begin{vmatrix} a & b \\ c & d \\ \end{vmatrix}=ad-bc
Matrice Jacobienne
Cas 2R: l3 = 0; θ3 = 0
J =
\begin{bmatrix}
- l_1 \sin{(\theta_1)} - l_2 \sin{(\theta_{12})} & - l_2 \sin{(\theta_{12})} \\[8pt]
l_1 \cos{(\theta_1)} + l_2 \cos{(\theta_{12})} & l_2 \cos{(\theta_{12})} \\[8pt]
\end{bmatrix}
\begin{array}{ccl}
\det (J) &=&
[- l_1 \sin{\theta_1} - l_2 \sin{\theta_{12}}] \times l_2 \cos{\theta_{12}} \\
& &+ l_2 \sin{\theta_{12}} \times [l_1 \cos{\theta_1} + l_2 \cos{\theta_{12}}]
\end{array}
J^{-1}
=\frac{1}{ \det(J)}
\begin{bmatrix}
j_{22} & -j_{12} \\
-j_{21} & j_{11}
\end{bmatrix}
Non-inversible si det(J) = 0
Matrice Jacobienne
\begin{array}{ccl}
\det (J) &=&
[- l_1 \sin{\theta_1} - l_2 \sin{\theta_{12}}] \times l_2 \cos{\theta_{12}} \\
& &+ l_2 \sin{\theta_{12}} \times [l_1 \cos{\theta_1} + l_2 \cos{\theta_{12}}]
\end{array}
\begin{array}{ccl}
\det (J) &=&
l_1 \; l_2\; \sin{\theta_2}
\end{array}
J non-inversible si sin θ2 = 0
\Rightarrow \theta_2 = n \pi
Si (2) est aligné avec (1)
⇒ Configuration Singulière