Difference between revisions of "Projets:Perso:2013:Module de localisation"

From Electrolab
Jump to: navigation, search
(Principe de transmission infrarouge)
(Emetteur Infrarouge)
Line 104: Line 104:
 
Ainsi, il est possible d’émettre avec plusieurs balises d’émission selon les rapports cyclique différents,  qu’un récepteur pourra identifier pour différencier les balises.
 
Ainsi, il est possible d’émettre avec plusieurs balises d’émission selon les rapports cyclique différents,  qu’un récepteur pourra identifier pour différencier les balises.
  
=== Emetteur Infrarouge ===
+
=== Émetteur Infrarouge ===
 +
Les cartes d’émission infrarouge sont basées sur le principe ci-dessus. La version présentée ici est déployée de manière autonome dans une balise qui possède sa propre alimentation, ainsi qu’un Dip switch pour la sélection du rapport cyclique.
 +
==== Étage électrique ====
 +
La commande de la LED ne peut pas se faire directement par le microcontrôleur. En effet, la puissance nécessaire à l’émission de la LED est trop importante pour le microcontrôleur. Il est donc nécessaire d’ajouter un étage de puissance par l’intermédiaire d’un transistor [type BC547]. <br />
 +
[[File:Alimentation_LED_IR.jpg]]<br />
 +
Il faut rajouter un montage pour deux LED, mais il est possible de tous les commander avec la même sortie du microcontrôleur. Les LED utilisées sont des TSAL 6200 Vishay.<br />
 +
Le signal Sig est associé à la variable _RB6 (en sortie), et le Dip switch est connecté aux ports _RB8 à _RB15 (en entrée).<br />
 +
[[File:DIL_Switch.jpg]]<br />
 +
==== Algorithmique ====
 +
Pour l’émetteur infrarouge, les fonctions suivantes sont utilisées :
 +
* Configuration de l’horloge : Fcy = 40 MHz, configuration 2 (cf tableau xx),
 +
* Initialisation des timers :
 +
** Timer 1 : 20 ms, ainsi qu’un compteur interne de 25 pour générer un LED clignotant à 1 Hz,
 +
** Timer 2 : 7µs
 +
** Timer 3 : 9µs, ainsi qu’un compteur interne de 100 permettant une interruption à 900 µs.
 +
===== Timer 1 interrupt =====
 +
Le timer 4 génère un clignotement de LED à 2 Hz, utilisé comme bit de vie.
 +
: TMR1=0;
 +
: if(count>=25)
 +
: {
 +
:: _RB7 = !_RB7;
 +
:: count = 0;
 +
: }
 +
: else
 +
: {
 +
:: count = count + 1;
 +
: }
 +
: IFS0bits.T1IF=0;
 +
La variable count est globale, utilisée seulement ici.<br />
 +
Pour rappel, l’opérateur « ! » indique l’inverse d’une variable logique (ie son complément à 1). Ainsi, la fonction _RB7 = !_RB7 signifie que la variable _RB7 (port _RB7 utilisé comme E/S directe, sans passer par le registre PORTB) est mise à son complémentaire à 1 (0 si elle valait 1, 1 sinon).
 +
===== Timer 2 interrupt =====
 +
Le timer 2 génère la porteuse du signal infrarouge. Il déclenche une interruption toutes les 7 µs, qui est ensuite associée à un compteur à 4 pas. Une fois sur quatre, l’interruption positionne le bit_6 (commande des LED) à « x » (1 ou 0 selon l’état haut ou bas du code winchester), et trois fois sur quatre le bit_6 reste à 0. La valeur de x est générée par le timer 3.
 +
: TMR2=0;
 +
: if(count_porteuse == 1)
 +
: {
 +
:: _RB6 = data_out;
 +
:: count_porteuse = count_porteuse + 1;
 +
: }
 +
: else
 +
: {
 +
:: _RB6 = 0;
 +
:: if(count_porteuse >= 4)
 +
:: {
 +
::: count_porteuse = 1;
 +
:: }
 +
:: else
 +
:: {
 +
::: count_porteuse = count_porteuse + 1;
 +
:: }
 +
: }
 +
: IFS0bits.T2IF=0;
 +
La variable count_porteuse est globale, utilisée uniquement ici.<br />
 +
La variable data_out est globale, utilisée ici en lecture et dans timer 3 en écriture.
 +
===== Timer 3 interrupt =====
 +
Le timer 3 va générer l’impulsion entre 900 et 1800 µs. Pour cela, une interruption toutes les 9µs ainsi qu’un compteur >ADU<
 +
: TMR3=0;
 +
: if(elapse >= pulse)
 +
: {
 +
:: data_out = !data_out;
 +
:: elapse = 0;
 +
:: data_in = (_RB4*2) + _RA4;
 +
:: switch (data_in)
 +
:: {
 +
::: case 0 :
 +
:::: long_bit = 100;
 +
:::: short_bit = 200;
 +
::: break;
 +
::: case 1 :
 +
:::: long_bit = 100;
 +
:::: short_bit = 100;
 +
::: break;
 +
::: case 2 :
 +
:::: long_bit = 200;
 +
:::: short_bit = 100;
 +
::: break;
 +
::: case 3 :
 +
:::: long_bit = 150;
 +
:::: short_bit = 150;
 +
::: break;
 +
:: }
 +
:: if(data_out == 1)
 +
:: {
 +
::: pulse = long_bit;
 +
:: }
 +
:: else
 +
:: {
 +
::: pulse = short_bit;
 +
:: }
 +
: }
 +
: else
 +
: {
 +
:: elapse = elapse + 1;
 +
: }
 +
: IFS0bits.T3IF=0;
 +
===== Initialisation du signal =====
 +
ADU
 +
=== Récepteur infrarouge ===

Revision as of 21:11, 12 March 2014

Localisation par balisage infrarouge

Cette page présente mes travaux actuels sur la fabrication d'un système de balisage infrarouge pour mobile.
Le système contient les fonctions suivantes :

  • Emission infrarouge de 4 balises autonomes,
  • Réception infrarouge sur une tourelle,
  • Contrôle d'un moteur pas à pas d'entrainement de la tourelle.

En plus de la fonction de triangulation associée au balisage, le système intègre en permanence deux entrées codeuses, assurant ainsi l'odométrie du mobile.
Les données d'odométrie sont ensuite intégrées aux données de balisage en permanence par l'intermédiaire d'un filtre de Kalman.

Un troisième volet de ce projet est le calcul en permanence d'une "void area", basée sur un réseau de capteurs ultrason déployés sur le mobile.

Dans un second temps, un autre projet viendra compléter celui-ci avec la réalisation d'un système de contrôle commande des moteurs (en DC, stepper DC puis en BLDC) avec un calcul de trajectoire basé sur les splines de Catmull-Rom.


Système de balisage infrarouge

Le principe de balisage infrarouge consiste à utiliser des balises émettant un signal infrarouge que va détecter un plot central, déployé sur une structure mobile roulante par exemple.
Le concept est développé dans le cadre de la coupe de France de robotique, et permet de disposer facilement d’une information de localisation sur la table de 3x2m.
Table de jeu - Localisation IR.jpg
Le but est d’obtenir la meilleur estimation de x et y afin d’avoir la position du robot sur la table, ainsi que son orientation.
Le principe appliqué est celui de la triangulation, basée sur une mesure d’angle (il existe le même principe avec les distances, tout n’étant au final qu’une application des équations de trigonométrie).
Le choix du balisage infrarouge est lié au faible coût de la mise en œuvre (une centaine d’euros tout compris) et à la relativement grande précision du système.
Il sera couplé, dans la suite de ce document, à un dispositif d’odométrie et un dispositif de sonars permettant d’affiner la position courante du robot et de créer une « void area » autour du robot indiquant les zones accessibles ou non au robot.
Table de jeu - Void area et conduite.jpg


Les concepts définis par la suite s'appuient principalement sur deux éléments :

Le détail de l'utilisation de ces composants est donné dans les pages associées.

Balisage infrarouge

Le principe de la triangulation est de mesurer trois angles entre des points fixes et un point mobile, et de retrouver, connaissant la position des points fixes, la position du point mobile. La manœuvre est limité à la détection d’objets à l’intérieur de la zone formée par les points fixes (c’est peut-être possible en dehors, mais je n’ai pas fait l’analyse, pas besoin).
Le nombre limite de points fixes est au minimum de 3, dans le cadre de l’étude 4 seront utilisés (par extension de la version à 3 points). Plus il y a de points fixes, meilleure est la précision, mais plus long seront les traitements.
Tout d’abord, définissons quelques variables. Le schéma ci-dessous représente la table sur laquelle est porté un repère orthonormé qui va servir de base aux calculs. Deux angles sont définis :

  • θAB,
  • θAC.

Triangulation - principe.jpg
Le calcul de ces deux angles sera précisé plus bas. Pour l’instant, partons du principe qu’ils sont connus. Par triangulation, nous allons calculer xM et yM en calculant un certain nombre de variables de cette figure qui nous donnerons, in fine, les coordonnées du point M.
Par analyse du schéma, on pose :

xM = AM x Cos[a]
yM = AM x Sin[a]

Nota : pour ceux qui préfèrent travailler en polaire, il est possible d’utiliser AM (ie r) et a (ie θ) directement, par exemple pour trianguler la position d’un outil sur un bras SCARA.
Le calcul de la variable AM se base sur les trois formules suivantes :

  • Loi des sinus : MB /Sin[a] = MA/Sin[Pi - θAB - a] = AB/Sin[θAB]
  • Sin[Pi - x] = Sin[x]
  • Sin[a+b] = Sin[a] x Cos[b] + Sin[b] x Cos[a]

On définit ainsi :

MB = (AB x Sin[a]) / Sin[θAB] = (AM x Sin[a]) / Sin[Pi - θAB - a]
Avec : Sin[Pi - θAB - a] = Sin[θAB] x Cos[a] + Sin[a] x Cos[θAB]

D’où, tous calculs fait :

AM = (AB x Sin[a]) / Tan[θAB] + AB x Cos[a]

Le calcul de la variable [a] se base sur les formules suivantes :

  • Loi des sinus : MB /Sin[a] = MA/Sin[Pi - θAB - a] = AB/Sin[θAB]
  • Sin[Pi - x] = Sin[x]
  • Sin[a+b] = Sin[a] x Cos[b] + Sin[b] x Cos[a]
  • Sin[PI/2 + x] = Cos[x]
  • Cos[a-b] = Cos[a] x Cos[b] + Sin[a] x Sin[b]

Dans le schéma ci-dessus, on applique la formule des sinus :

AC / Sin[θAC] = AM / Sin[{ACM}]
AB / Sin[θAB] = AM / Sin[{MBA}]

Sachant que les angles :

{ACM} = PI/2 – a - θAC
{MBA} = PI – a - θAB
AC / Sin[θAC] = AM / Cos[a - θAC]
AB / Sin[θAB] = AM / Sin[a + θAB]

Il ne reste plus qu’à identifier [a] en joignant les équations en AM.
  Tous calculs faits :

(AC x Tan[θAC]) x Cos[a] + AC x Sin[a] = (AB x Tan[θAB]) x Sin[a] + AB x Cos[a]
a = Atan[((AC / Tan[θAC]) - AB) / ((AB / Tan[θAB]) - AC)]

Le calcul de [xM, yM] est réalisé en permanence. La précision de la position dépendra en grande partie de la précision de la mesure des angles, qui est liée à la technologie de mesure.
Sur la base de ce qui précède, et sans tenir compte pour l’instant des erreurs de mesure, la triangulation est possible en mesurant au moins deux des quatre angles formées par les 4 balises aux 4 coins de la table et la tourelle de réception.

Détail des applications

Le chapitre suivant va décrire plusieurs applications complètes et unitaires à base de microcontrôleur DsPIC33F, qui reprennent toutes les fonctions vues ci-dessus. Chacune de ces applications peut être reprise dans un projet quelconque sans adaptation particulière, en tenant compte toutefois de l’ICD associée à chaque carte.
Les applications suivantes sont présentées :

  • Emetteur infrarouge,
  • Récepteur infrarouge,
  • Lecture sonars,
  • Driver moteur pas à pas,
  • Driver servomoteur.

Chaque application va utiliser le même type de µC, des DsPic33FJ64MC802, et la même communication en ECAN pour transmettre ou recevoir des données du Blackfin ou de tout autre composant disposant de module CAN.
Les dimensions proposées sont également standardisées, pour permettre l’intégration dans un fond de panier spécifique.

Principe de transmission infrarouge

Le principal problème lors d’une transmission infrarouge est lié aux perturbations de la lumière environnante. En effet, une émission directe sans codage sera extrêmement perturbé, probablement inutilisable.
Pour éviter ce problème, le signal émis en infrarouge est encodé, selon le principe suivant :
Code winchester.jpg
Le signal émis transmet une information codé selon le principe du code Winchester, qui transmet les informations avec une donnée de type front (montant / descendant) au lieu de logique (haut / bas).
L’amplitude du signal est très limitée : entre 889µs et 1778µs, au-delà le récepteur est saturé. Les états hauts sont également codés par une porteuse de rapport cyclique ¼, d’amplitude 7µs haut / 22µs bas.
Il est nécessaire de respecter ces contraintes pour pouvoir utiliser un récepteur intégré, qui détecte automatiquement ce type de codage et le transforme en signal logique (0/1).
L’utilisation d’un code winchester présente plusieurs avantages, mais aussi l’inconvénient d’être assez difficile à décoder. Pour simplifier, nous utiliserons (dans un premier temps) un autre moyen de distinguer un signal d’un autre : le rapport cyclique.
Entre 900 µs et 1800 µs, les amplitudes seront retransmises en l’état par le module de réception IR. Dans cette fenêtre, un rapport cyclique de 900 / 1200 pourra être distingué d’un rapport cyclique 900 / 1500 ou 1500 / 1500 etc.
Rapport cyclique.jpg
Ainsi, il est possible d’émettre avec plusieurs balises d’émission selon les rapports cyclique différents, qu’un récepteur pourra identifier pour différencier les balises.

Émetteur Infrarouge

Les cartes d’émission infrarouge sont basées sur le principe ci-dessus. La version présentée ici est déployée de manière autonome dans une balise qui possède sa propre alimentation, ainsi qu’un Dip switch pour la sélection du rapport cyclique.

Étage électrique

La commande de la LED ne peut pas se faire directement par le microcontrôleur. En effet, la puissance nécessaire à l’émission de la LED est trop importante pour le microcontrôleur. Il est donc nécessaire d’ajouter un étage de puissance par l’intermédiaire d’un transistor [type BC547].
Alimentation LED IR.jpg
Il faut rajouter un montage pour deux LED, mais il est possible de tous les commander avec la même sortie du microcontrôleur. Les LED utilisées sont des TSAL 6200 Vishay.
Le signal Sig est associé à la variable _RB6 (en sortie), et le Dip switch est connecté aux ports _RB8 à _RB15 (en entrée).
DIL Switch.jpg

Algorithmique

Pour l’émetteur infrarouge, les fonctions suivantes sont utilisées :

  • Configuration de l’horloge : Fcy = 40 MHz, configuration 2 (cf tableau xx),
  • Initialisation des timers :
    • Timer 1 : 20 ms, ainsi qu’un compteur interne de 25 pour générer un LED clignotant à 1 Hz,
    • Timer 2 : 7µs
    • Timer 3 : 9µs, ainsi qu’un compteur interne de 100 permettant une interruption à 900 µs.
Timer 1 interrupt

Le timer 4 génère un clignotement de LED à 2 Hz, utilisé comme bit de vie.

TMR1=0;
if(count>=25)
{
_RB7 = !_RB7;
count = 0;
}
else
{
count = count + 1;
}
IFS0bits.T1IF=0;

La variable count est globale, utilisée seulement ici.
Pour rappel, l’opérateur « ! » indique l’inverse d’une variable logique (ie son complément à 1). Ainsi, la fonction _RB7 = !_RB7 signifie que la variable _RB7 (port _RB7 utilisé comme E/S directe, sans passer par le registre PORTB) est mise à son complémentaire à 1 (0 si elle valait 1, 1 sinon).

Timer 2 interrupt

Le timer 2 génère la porteuse du signal infrarouge. Il déclenche une interruption toutes les 7 µs, qui est ensuite associée à un compteur à 4 pas. Une fois sur quatre, l’interruption positionne le bit_6 (commande des LED) à « x » (1 ou 0 selon l’état haut ou bas du code winchester), et trois fois sur quatre le bit_6 reste à 0. La valeur de x est générée par le timer 3.

TMR2=0;
if(count_porteuse == 1)
{
_RB6 = data_out;
count_porteuse = count_porteuse + 1;
}
else
{
_RB6 = 0;
if(count_porteuse >= 4)
{
count_porteuse = 1;
}
else
{
count_porteuse = count_porteuse + 1;
}
}
IFS0bits.T2IF=0;

La variable count_porteuse est globale, utilisée uniquement ici.
La variable data_out est globale, utilisée ici en lecture et dans timer 3 en écriture.

Timer 3 interrupt

Le timer 3 va générer l’impulsion entre 900 et 1800 µs. Pour cela, une interruption toutes les 9µs ainsi qu’un compteur >ADU<

TMR3=0;
if(elapse >= pulse)
{
data_out = !data_out;
elapse = 0;
data_in = (_RB4*2) + _RA4;
switch (data_in)
{
case 0 :
long_bit = 100;
short_bit = 200;
break;
case 1 :
long_bit = 100;
short_bit = 100;
break;
case 2 :
long_bit = 200;
short_bit = 100;
break;
case 3 :
long_bit = 150;
short_bit = 150;
break;
}
if(data_out == 1)
{
pulse = long_bit;
}
else
{
pulse = short_bit;
}
}
else
{
elapse = elapse + 1;
}
IFS0bits.T3IF=0;
Initialisation du signal

ADU

Récepteur infrarouge