CALCULS DE TRAJECTOIRES

CALCULS DE TRAJECTOIRES
CALCUL DE LA POSITION REELLE APRES DEPLACEMENT
GENERATION DES CONSIGNES
OPTIMISATION DES TRAJECTOIRES
Calcul de position :
Le calcul de position par odométrie consiste en la récupération des déplacements réels des
roues du robot. A partir de ces données, on va calculer le déplacement relatif [dx,dy] et la
variation d’orientation β du robot.
M1
d : Distance entre les roues, dans l’unité de
mesure des codeurs.
dy
Mi : Distance parcourue par une roue
R
M2
R : Rayon de courbure
β
d
M1 = β.(R+ d )
2
M 2 = β.(R− d )
2
: variation d’angle décrit par le robot (radian)
dx
M1 = βR+ β d
2
M 2 = βR− β d
2
R= M1 + M 2
2.β
β = M1 − M 2
d
dx =R.(1−cos(β))
dy= R.sin(β)
Ces formules sont valables quelques soient le sens de déplacement du robot (avant ou
arrière), et le sens du virage (gauche ou droite).
Pour indication, on obtient les valeurs d’angle et de rayon de courbure suivantes, selon
l’évolution du robot :
y
R<0
β<0
R>0
β>0
x
R<0
β>0
R>0
β<0
2
Système d’unité du robot :
Comme le robot ne connaît pas plus le mètre que le feet ou les inchs, on va créer une
unité. Ce pourrait être le broglub, très courus sur Araounda la nébuleuse, ou le Casta (un
sous multiple du tour de poitrine de Leatitia, valeur universelle en soit). Non, ce sera
l’Intervalle Robot « Ir ».
Pour donner une valeur de correspondance, je donne le calcul :
Prenez un moteur réducteur + codeur d’un rapport de 1/6,3
Comptez 500 ppt pour le codeur
Rajoutez un rapport de 0,75 de par la transmission par engrenage.
Mettez une roue de 99,76 mm de diamètre, soit un périmètre de 313,4 mm
Prenez un shaker, secouez, servez chaud : 13,4014 impulsions / mm
Soit le Ir à 74,61905 µm.
La distance entre les deux roues du robot est de 1716,3831 Ir.
Le point de référence pour toutes les
commandes se trouve exactement
entre les deux roues.
d
3
Génération des consignes :
Calcul des consignes M1=f1(dx,dy) et M2=f2(dx,dy) à envoyer aux moteurs. Ces calculs sont
donné pour un vecteur relatif (coordonnés par rapport à la position actuelle du robot).
dx=R.(1−cos(β))
dy=R..sin(β)
dx.sin(β)=dy.(1−cos(β))
dx.2.sin(ϕ).cos(ϕ)=dy.2.sin ²(ϕ)
dx.cos(ϕ)=dy.sin(ϕ)
dx = tan(ϕ)
dy
On pose β =2.ϕ
β =2.arctan(dx )
M1 = β(R + d )
2
dy
R=
dy
sin(β)
M 2 = β(R− d )
2
Rectification de trajectoires :
Ces formules utilisée seules cela pourraient induire des trajectoires qui prennent trop
de débattement. L’extrême étant un déplacement avec dy=0, le robot décrit un demi cercle
complet, et l’angle final est égal à 180°
Marche avant
β
β
y
V
De
α
Marche arrière
x
4
On remarque que β = 2.α. L’orientation finale du robot est le double de l’orientation du
vecteur de consigne.
On remarque également que De, la valeur du ventre entre la corde et l’arc de cercle, est
d’autant plus élevé que α l’est.
On en déduit que pour α comprit entre 0° et 22,5°, la déviation du robot reste acceptable (0°
à 45°) et De faible. Cela impose que dy≥2,41.dx .
A titre indicatif, on peut estimer l’écart relatif ∆ :
∆=2. De
v
R− R²− 1 (dx² +dy²)
4
∆ = 2.
dx² +dy²
Cette valeur est l’expression de l’erreur maximale lors du déplacement. La valeur max est
obtenue pour dy=0, De= dx et ∆ = 1
2
5
Modification d’orientation :
Plutôt que de faire une trajectoire à la con, il est plus futé de mettre le robot dans une
orientation favorable. Pour cela, on effectue une rotation sur place, dx=dy=0. Ensuite, on
prendra le chemin le plus court pour se placer en [dx,dy]
On prendra logiquement pour β, l’angle α du vecteur de consigne : α = Arc tan( dx )
dy
M 1 = β. d
2
M 2 =−β. d
2
Bien entendus, dans ce cas, il est impératif de recalculer l’orientation du robot après
exécution de la commande de rotation, pour prendre en compte l’erreur dans la commande
de déplacement. On obtient un logigrame de ce genre :
Début
dy≥2,41.dx ?
Calcul de M1 et
M2 réorientation
Réorientation
Calcul position et
orientation réelle
Calcul [dx’,dy’]
dans la nouvelle
base
Calcul de M1 et
M2 déplacement
Exécution
déplacement
fin
6
Calcul du vecteur de consigne dans une nouvelle base de même
origine :
En modifiant l’orientation à l’origine du robot, on modifie également les coordonnées du
vecteur. C’est même le but d’une réorientation.
dy’
dy
δ
dx'=dx.cos(δ)−dy.sin(δ)
dx’
dx
dy'=dy.cos(δ)+dx.sin(δ)
Calcul du vecteur de consigne dans une base colinéaire :
On ne pourra éviter un léger déplacement pendant une réorientation. De plus, cette fonction
sera utilisé par les générations avancées de trajectoires.
Un brave français du nom de
Michel Chasles en a déjà parlé,
mais on s’en lasse pas :
dx'=dx−Vx
dy'=dy−Vy
V
7
Généralisation pour un déplacement quelconque :
Simple, on tient compte des deux phénomènes :
dx'=(dx−Vx).cos(δ)−(dy −Vy).sin(δ)
dy'=(dy −Vy).cos(δ)+(dx−Vx).sin(δ)
Optimisation de trajectoire, prise en compte d’une consigne
d’angle :
Il peut être intéressant d’arriver au point désigné avec un angle précis. Pour cela, on
peut calculer un point de retrait. Bien entendu, il est des orientations particulières pour
lesquelles cette fonctionnalité n’est pas judicieuse. On peut alors définir des conditions
particulières d’utilisation.
Une consigne complète ne se
contente pas d’un déplacement. On
doit également prendre en compte
l’angle souhaité à l’arrivée.
Ici, en rouge, le vecteur de
consigne de déplacement. En vert,
la consigne d’orientation finale.
Restrictions :
•
•
Le vecteur de déplacement doit être suffisamment important pour que la modification
de trajectoire ne soit pas trop sensible.
Si l’angle final est égal à l’opposé de l’angle du vecteur de déplacement, à une
variation près. Cela revient à rouler sur l’objectif avant de se placer. C’est souvent
inutile de prendre des précautions.
Ces restrictions, sous forme mathématique s’expriment par :
•
•
dx²+dy² > Vmin. Vmin représente le module minimum du vecteur de déplacement
au carré.
[ – + ; + + ]. µ représente l’ouverture d’angle à éviter.
8
2µ
C
Calcul du point de retrait C:
Le vecteur de retrait est calculé sur une base de longueur fixe Vret, et une orientation égale
à l’orientation finale souhaitée.
Vretx = Vret.sin( )
Vrety = Vret.cos( )
Cx = dx - Vret.sin( )
Cy = dy - Vret.cos( )
La longueur Vret est à déterminer, bien entendus, toujours dans le système d’unité du robot.
Pour effectuer cette trajectoire, on réalisera les éléments suivant :
1. Calcul du point C.
2. Si nécessaire modification d’orientation vers le point C.
3. Si nécessaire, re-calcul des points C et objectif final (point et angle) une fois
modification d’orientation effectuée.
4. Déplacement vers le point C.
5. Calcul du nouvel angle et du nouvelles coordonnées du point final dans la base du
robot en C.
6. ré-Orientation dans l’angle .
7. Calcul des coordonnées du point final.
8. Déplacement vers le point final.
9