Club robotique de Sophia-Antipolis

Accueil > Projets et études > Nos réalisations > Projets spéciaux > Le LIDAR de l’aspirateur Neato contrôlé par logique floue

Capteur LIDAR

Le LIDAR de l’aspirateur Neato contrôlé par logique floue

samedi 28 décembre 2013, par Frédéric Bernat

Cela faisait des années que j’en rêvais, mais les lidar accessibles dans le commerce étaient beaucoup trop cher, puis il y a quelques années est arrivé un nouvel aspirateur, le Neato, avec un capteur incroyable, las, le prix du capteur en pièce détaché est vendu aux alentours de 300$ (sic), j’ai dû encore attendre deux ans avant que des capteurs soient vendus d’occasion sur ebay à un prix raisonnable. J’ai enfin pu me le procurer pour une soixantaine d’euros. Yeeeessssss


L’avantage de ce lidar est qu’il est petit (ie : pas trop gros) et facilement interfaçable avec une carte arduino (MEGA 1280) grâce au driver écrit par Xevel.
(cf : que vous trouverez à l’adresse suivante
Source : https://github.com/Xevel/NXV11
Documentation : https://github.com/Xevel/NXV11/wiki).

J’ai bien évidement modifié le code afin de piloter le moteur du lidar avec la logique floue. Il m’a fallu du temps avant de trouver les bonnes courbes pour les fonctions de transfert, mais je suis content du résultat.

Première chose à faire est de fabriquer une petite interface afin de connecter le lidar à l’arduino (merci chenglung).
cf : http://forums.trossenrobotics.com/showthread.php?4470-The-Open-Lidar-Project-Hack-the-Neato-XV-11-Lidar-for-a-200-Bounty!/page18

L’image ci-dessous, nous montre le schéma électronique de l’interface :

Ce que j’ai traduit par :

Le Mosfet (IPS041L) sera soudé du côté cuivre, les résistances et la diode de l’autre côté. (voir pdf ci joint pour les caractéristiques du mosfet)

PDF - 109.3 ko

15 mn après, cela donne la réalisation suivante (plaque cuivrée, feutre indélébile et perchlorure de fer) :


Maintenant on passe à la phase de test, donc on connecte tous les bidules ensemble comme ci-dessous :

Et la patatra, ça marche pas le moteur tourne mais les données sont incohérentes. Zut soit je me suis fait avoir, soit il y a une erreur dans le code…. Bizarre. En fait le code arduino a évolué depuis les années 2010, pour envoyer un octet sur le port série on écrit plus Serial.print(value,BYTE) mais plus simplement Serial.write(value). (OUF !!!)

Bien on recompile le bigniou On lance l’application python XV-11_test.py et TADAAA !!!
Attention dans l’application vous devez spécifier le port série, mon arduino apparait sur le COM6, donc dans l’appli au niveau SETTINGS :

#---------- SETTINGS --------------
use_real_robot = True
com_port = "COM6"
baudrate = 115200

Comme on peut le constater, il faut ouvrir un port série sur l’arduino avec une vitesse de transfert de 115200.

Bon en fait ça a l’air simple comme ça, mais j’ai galéré une semaine pour arriver à ce résultat. J’ai dû dumper les trames dans un fichier, pour vérifier quelles étaient conformes à la version 2.4 du lidar (mon lidar est en 2.6). J’ai d’ailleurs pu constater que les trames étaient noyées dans des données parasites assez importantes. Voici le format des trames une fois nettoyées :

FA A0 00 4A E4 02 1A 01 E4 02 A7 01 E3 02 A5 01 E3 02 1A 01 DA 6A
FA A1 07 4A E4 02 1A 01 E4 02 1A 01 E6 02 1A 01 E7 02 1A 01 FD 6E
FA A2 07 4A E7 02 1A 01 EA 02 1A 01 EB 02 1A 01 ED 02 1A 01 BF 6E

Une trame fait 22 octets, elle commence par toujours FA, il en faut 90 pour obtenir les 360 degrés, à raison de 4 données par trames.

<Data 0> <Data 1> <Data 2> <Data 3>

où :
• L’octet de start est toujours 0xFA
• Index est l’index d’un des 90 paquets, allant de 0xA0 (paquet 0, données de 0 à 3) jusqu’à 0xF9 (paquet 89, données de 356 à 359).
• speed est l’information sur la vitesse de rotation sur deux octets, c’est du little-endian. Valeur nominale 18000 tr/heure à diviser par 60 pour obtenir le nombre de tours par minute.
• Data 0 à Data 3 sont les 4 données. Chacune est composée de 4 octets, organisés comme suit :

byte 0 : distance 7:0

byte 1 : invalid data flag , strength warning flag , distance 13:8

byte 2 : signal strength 7:0

byte 3 : signal strength 15:8

Quand le bit 7 du premier octet est levé, cela indique que la distance ne peut être calculée. Lorsque ce bit est mis, il semble que l’octet 0 contient un code d’erreur. Exemple de code d’erreurs : 0x02, 0x03, 0x21, 0x25, 0x35 or 0x50...
Quand c’est `21`, tous le bloc est `21 80 XX XX`, mais pour toutes les autres valeurs, le bloc de données est `YY 80 00 00 ...`

Le bit 6 de l’octet 1 est un avertissement lorsque la valeur est rapportée considérablement inférieure à ce qui est prévu à cette distance. Cela peut se produire lorsque le matériau a une réflectance faible (de matière noire ...), ou lorsque le point n’a pas la taille ou la forme attendue (matériau poreux, tissu transparent, grille, bord d’un objet ...), ou peut-être quand il y a des réflexions parasites (miroir, vitre. ..).

Les octets 2 et 3 sont les LSB et MSB de l’indication de la robustesse. Cette valeur peut être très élevée face à un rétro-réflecteur.
• checksum : checksum du paquet sur 2 octets

L’ algorithme (python) est le suivant, ‘data’ est la variable qui contient les 20 premiers byte de la trame
dans leur ordre d’arrivé :

{{def checksum(data):}}
    """Calcule et retourne la valeur du checksum en un entier."""
    # group the data by word, little-endian
    data_list = []
    for t in range(10):
        data_list.append( data[2*t] + (data[2*t+1]<<8) )
 
    # compute the checksum on 32 bits
    chk32 = 0
    for d in data_list:
        chk32 = (chk32 << 1) + d
 
    # return a value wrapped around on 15bits, and truncated to still fit into 15 bits
    checksum = (chk32 & 0x7FFF) + ( chk32 >> 15 ) # wrap around to fit into 15 bits
    checksum = checksum & 0x7FFF # truncate to 15 bits
    return int( checksum )

Pour plus d’informations, je vous invite à consulter les sites suivants :

* http://xv11hacking.wikispaces.com/LIDAR+Sensor
* http://bombilee.blogspot.fr/2010/12/arduino-mega-xv-11-lds-motor-dirver.html
* https://sites.google.com/site/chenglung/home/xv-11-lds-v2-4-13386-fimware-data
* http://www.xevel.fr/blog/index.php?category/Hacking/Neato-XV-11
* http://random-workshop.blogspot.fr/2010/12/simple-circuit-to-control-lidar-unit.html

La première chose qu’il faut savoir, c’est que le capteur doit tourner à 300 tr/mn de façon stable pour fonctionner correctement (ie : avoir un maximum de données valides).

C’est là qu’intervient la logique floue.
Afin de concevoir un contrôleur Flou, il nous faut des paramètres nous permettant de calculer la modification du signal PWM à effectuer.
Le premier paramètre sera la mesure de l’erreur de vitesse de rotation par rapport à la vitesse de référence (ie : la vitesse désirée). Soit

E = Vref –Vcur

Cela nous donne une première indication sur la correction à faire, mais ce n’est pas suffisant pour avoir un contrôle optimal. On va utiliser un autre paramètre, calculé à partir du premier, qui est l’évolution de l’erreur entre deux intervalles de mesure. Ce paramètre peut être considéré comme la dérivée de l’erreur soit l’accélération ou la décélération de l’erreur. Nous avons donc l’équation suivante :

∆E= Ecur – Eprec

Je me suis basé sur plusieurs articles (dont vous trouvez les références ci-après) pour réaliser mon contrôleur flou.

* Speed Control of DC Motor using Fuzzy Logic based on LabView
Salim, Jyoti Ohri, Naveen
* Commande DTC par Logique Floue d’un moteur aà Induction Alimenté par onduleur et tension PMW
F. Kadri
* Design et Implementation of Real Time DC motor Control using Fuzzy Logic
Waleed Abd El-Meged El-Badry
* Fuzzy Logic Microcontroller Implementation For DC motor Speed Control
Yodyium Tipsuwan, Mo-Yuen Chow
* Fuzzy Logic Based Method of Speed of DC Motor
Jaydeep Chakravorty, Ruschika Sharma
* Model reference Linear Adaptative Control of DC Motor Fuzzy Controller
A.Suresh M. Subba Rao Y.S.Kishore BAbu
* Reduced Base Fuzzy Controller for Performance Enhancement of Inline PI Controller
Vishal Verma
* Simulation of Optimal Speed Control for a DC Motor Using Conventional PID Controller and Fuzzy Logic Controller
Ritu Soni, DBV Singh, Pramod Pandey and Priyanka Sharma
* Induction Motor Speed Control using Fuzzy Logic Controller
V.Chitra and R.S Prabhakar
* Fuzzy Logic Speed Control of a DC Motor
Seda Aydemir, Serkan Sezen H. Metin Ertunc
* PMDC Motor Speed Control with Fuzzy Logic Algorithm Using PIC16F877 Micro Controller and Plotting Data on Monitor
Ahmet Isik, Oktay Karakaya, P. Alper Oner, Mehmet Kubilay Eker

Le tableau ci-dessous donne les règles de logique utilisées :

NM : Negative Medium
NS : Negative Small
ZE : Zero
PS : Positif Small
PM : Positive Medium

Exemple :
IF E=NM AND ∆E=PM THEN PMW=ZE
On ajoute la valeur de sortie PMW à la consigne de commande du moteur.

Fonction de transfert pour le premier paramètre :

Fonction de transfert pour le deuxième paramètre :

Fonction de transfert pour le paramètre de sortie (deffuzyfication)

Pour le calcul des valeurs de la première fonction de transfert, on part de l’erreur minimale souhaité, dans notre cas 18000 est la valeur de référence correspondant à 300 tr/mn (18000/60=300).

Si on prend comme approché souhaitable 18000 ± 100 cela correspond à 300 ± 1. La courbe pour la fonction de transfert proche de 0, sera donc définie entre -100 et +100.

La consigne PWM pour obtenir une valeur de 300 tr/mn est ≈170.

Le problème que je n’ai pas résolu, c’est que la commande PMW se fait sur un octet donc une valeur comprise entre 0 et 255. Ce qui n’est pas optimal puisque pour une valeur de pmw de 170, la vitesse moteur oscille entre 300 et 304, à 171 entre 305 et 310 et à 169 elle oscille entre 295 et 300. (Il y a sans doute également, un balourd et des frottements qui génèrent des fluctuations dans la vitesse de rotation).

Une fois atteinte la vitesse approximative de 300 tr/mn je me retrouve à jouer avec + ou – 1 comme consigne de commande ce qui est trop étroit pour avoir un contrôle fin de la vitesse. Afin d’éviter d’avoir des oscillations trop importantes, je ne rafraîchi la vitesse de rotation que tous les 1/6 de tour (soit une fois toute les 15 trames).

Je pourrais affiner le contrôle de la vitesse si la commande pmw se faisait sur 2 octets (ou même 10 bits). J’ai bien essayé de modifier les caractéristique des sorties pmw en jouant avec le TIMER 1, mais du coup la fréquence passe de 976Hz à plus de 15KHz. Je me retrouve avec le même problème de résolution trop étroite.

Au final, la vitesse de rotation oscille entre 298 et 302 tr/mn, avec quelques aléas, cela est suffisant pour avoir une bonne qualité de signal, comme on peut le voir sur la deuxième figure (voir plus haut).

Voici mon code :

Zip - 4 ko

Voici le code python de l’interface graphique, il suffit d’installer python 2.7 et PySerial, pour l’utiliser.

Zip - 608 ko

Bien, maintenant que nous avons 360 points autour du robot nous donnant la distance des objets qui l’entoure, il faut les traiter afin dans extraire une information utilisable par le robot lui permettant de poursuivre le but qu’on lui a assigné.

Là où cela se complique, c’est le volume de données par secondes que l’on récupère du capteur. Ce volume est beaucoup trop important pour être directement exploité par une carte arduino, je ne parle même pas de les stocker sous une forme ou une autre afin de faire du mapping.(sauf à rajouter de la ram). Il y a bien un algorithme the SLAM, appelé TinySLAM qui fait 200 lignes de code C, mais il demande beaucoup de mémoire.

On le trouve sur : http://openslam.org/tinyslam.html

Le lidar renvoi 360 mesures de distance à la fréquence de 5hz. C’est-à-dire 5 fois par seconde. Ce qui nous fait 1800 entiers à traiter par seconde

Il faut donc traiter cette information brut assez rapidement afin de pouvoir en extraire une information condensé et exploitable, pour que notre robot se déplace en évitant les obstacles, en suivant un mur, etc…

De plus je veux pouvoir exploiter les remarquables possibilités de la logique floue.

Dans ce cas, si j’utilise directement les données bruts, il me faudrait écrire (sur la base de 3 adjectifs pour une fonction sur la distance), 3 à la puissance 360 règles d’inférences pour tenir compte de tous les cas possible. (sic)

Je vais donc fusionner les 360 degrés de données en 10 secteur de 36°, réduisant la problématique à un robot possédant dix capteurs de distance sur son pourtour, une moyenne de distance est calculée sur chaque secteur (ou plus simplement le distance la plus courte). Si on reprend l’idée d’une fonction flou sur la distance je me retrouve à écrire 3 à la puissance 10 règles d’inférences, ce qui est encore beaucoup trop.

La solution est de transformer l’ensemble des informations en un vecteur de perception représentant l’objet le plus proche du robot son angle par rapport à la direction de la trajectoire du robot et une estimation de l’évolution de ce vecteur perception.

Ceci fera l’objet du second article.

Vos commentaires

  • Le 19 novembre 2015 à 13:37, par Fabien En réponse à : Le LIDAR de l’aspirateur Neato contrôlé par logique floue

    Bonjour,
    Je fais partie d’une équipe participante à la coupe de france de robotique (ENAC robotique), et cette année, nous voudrions utiliser ce LIDAR pour faire du positionnement absolu sur la table (avec des balises passives).
    Je voudrais savoir si ce LIDAR est simple à utiliser (j’ai cru comprendre que oui sur ce site), quelle est sa précision angulaire (on a besoin de moins de 1.2° pour detecter les balises), et tout autre chose qui pourrait être utile de savoir.
    J’ai cru comprendre qu’il renvoie un point par degrès, mais je n’ai pas vu cette info clairement marquée. Cela est t’il toujours le cas, ou cela dépend il de la vitesse de rotation ?
    Et petite question bonus : si le robot toune sur lui même (accélération en rotation), la mesure est elle à son juste angle (par rapport au robot), ou l’angle est t-il biasé ?
    Et est ce que cela n’introduit pas un bias angulaire sur toutes les prochaines mesure ?
    Bref, beaucoup de questions auxquelles j’espère que vous pourrez répondre.

    Répondre à ce message

  • Le 16 mars 2014 à 23:15, par Blop En réponse à : Le LIDAR de l’aspirateur Neato contrôlé par logique floue

    Bonjour,
    Merci pour l’article, mais il y a une question de fond auquel je ne trouve pas de réponse (peut-être ai-je mal lu). Quel est l’intérêt de la logique floue pour contrôler la vitesse du moteur ? Une vitesse fixe ne suffit pas ?
    Sinon, il semble que Xevel fasse une board pour connecter le xv-11 au pc directement : https://github.com/Xevel/usb2lds

    • Le 17 mars 2014 à 10:14, par Frédéric Bernat En réponse à : Le LIDAR de l’aspirateur Neato contrôlé par logique floue

      Bonjour,
      Il ne suffit pas d’appliquer une tension pour avoir une vitesse de rotation constante.
      de plus chaque moteur même fabriqué en série n’aura pas exactement la même vitesse de rotation pour la même tension appliquée.
      Enfin, s’agissant d’un capteur, on doit amener le lidar à un vitesse de rotation la plus constante possible , en essayant de palier les frottements, les problèmes d’inerties ainsi que les changements d’orientations du robot ou du support du lidar.
      Xevel a eu la gentillesse de me fournir le module en question, il utilise un régulateur PID pour le contrôle du moteur. Je publierai un article sur le sujet prochainement.

    Répondre à ce message

  • Le 14 janvier 2014 à 20:32, par Llorenç En réponse à : Le LIDAR de l’aspirateur Neato contrôlé par logique floue

    Bonjour,

    Je ne savais pas ce qu’était un LIDAR. => http://www.mosaic3d.com/quest-ce-que-le-lidar/
    Maintenant c’est fait.

    Merci pour cet article sympathique !
    Llç

    Répondre à ce message

  • Le 6 janvier 2014 à 10:05, par Dyna En réponse à : Le LIDAR de l’aspirateur Neato contrôlé par logique floue

    Bonjour,

    Merci bien pour cet article, j’ai du coup refais une recherche sur Ebay, et je suis également tombé sur une bonne affaire ! Ca fait des mois que j’étais aussi à l’affut !

    Répondre à ce message

Un message, un commentaire ?

modération a priori

Attention, votre message n’apparaîtra qu’après avoir été relu et approuvé.

Qui êtes-vous ?

Pour afficher votre trombine avec votre message, enregistrez-la d’abord sur gravatar.com (gratuit et indolore) et n’oubliez pas d’indiquer votre adresse e-mail ici.

Ajoutez votre commentaire ici
  • Ce formulaire accepte les raccourcis SPIP [->url] {{gras}} {italique} <quote> <code> et le code HTML <q> <del> <ins>. Pour créer des paragraphes, laissez simplement des lignes vides.

Ajouter un document