LidarPreProcessor

Introductie

Al sinds jaar en dag gebruiken we afstandssensoren op onze robots. Ultrasoon, Sharp PSD sensoren en recent de VL53-serie Time-of flight sensoren van STM. Dit zijn sensoren met 1 dimensie: ze kijken - met een zekere openingshoek - in een bepaalde richting en geven een enkele waarde terug.

In het hogere segment van de markt zijn al jaren 2D lidars beschikbaar: vaak roterende sensoren die in 2 dimensies (hoek/afstand) informatie geven over de omgeving. Deze sensor leveren veel informatie en zijn uitermate geschikt voor 'high-end' technieken als SLAM. En omdat ook de prijs van dit soort sensoren nog niet zo lang geleden ruim boven 1000 euro lag, was dit niet het type sensor wat je als hobbyist zomaar zou gebruiken.

Maar in de afgelopen jaren is prijs van 2D lidars flink gedaald en toegankelijk geworden voor hobbyisten. In eerste instantie werden deze - nog steeds vrij dure sensoren - vooral gebruikt in combinatie met ROS. Ros heeft modules om optimaal gebruik te maken van de grote stroom aan informatie die deze sensor oplevert.

De prijsdaling heeft zich echter voorgezet en inmiddels zijn 2D lidars te koop voor minder dan 100 euro. En daarmee worden ze ook een alternatief voor de klassieke afstandssensoren die we gebruiken. Want alhoewel de lidar nog niet goedkoper is dan drie of vijf klassieke sensoren, heeft deze ook voordelen. Zoals 360 graden zicht, kleine openingshoek, hoekinformatie en grote maximale afstand.

Keerzijde is dat deze sensoren een grote stroom data geven - 1000 tot 5000 metingen per seconde is heel gebruikelijk - die de ontvanger moet verwerken. Uitermate geschikt voor ROS en geen probleem voor een systeem met Linux of Windows. Maar geen sinecure voor onze 'bare metal' geprogrammeerde controllers.

Het idee

Een lidar zou dus een handvol sensoren kunnen vervangen, maar lidar levert veel meer informatie dan we nodig hebben. De verwerking is een belasting voor het systeem, maar ook voor de programmeur. Die krijgt immers veel meer data en dat maakt beslissen niet makkelijk. En zo ontstond het idee: laten we een module maken die de data van de lidar verwerkt tot de informatie die we nodig hebben voor de besturing van de robot. Zoals we gewend zijn van de klassieke afstandssensoren, plus een beetje meer waar dit (eenvoudig) beschikbaar is.

De uitwerking

De kern van de module is een Maple Mini met STM32F1 processor. De module is geprogrammeerd om de data van de lidar te verwerken en is te benaderen als i2c slave. Schematisch ziet dit er als volgt uit:

Hardware

Voor de hardware is gekozen voor een Maple Mini bordje. Dit is een STM32-bordje dat met de Arduino IDE geprogrammeerd kan worden. Het bordje is aanzienlijk sneller en heeft meer geheugen dan de ATMega serie. De aansluitingen van het bordje is hieronder weergegeven.

Aansluitingen

• Op de connector YDLidar (links, 5 pins) wordt de lidar aangesloten (YDLidar X2 en X4 worden ondersteund).
• Plaats de Jumper (links) in geval van de YDLidar X2.
• De serial port (rechts boven, 4 pinnen) is voor debug (aanbevolen).
• De 4 pin SWD connector (rechts midden) is optioneel en alleen nodig indien de Maple bootloader niet gebruikt wordt.
• De i2c connector (rechts onder, 4 pins) wordt gebruikt om de module in te stellen en uit te lezen.

Voeding

Details van de voeding zijn niet weergegeven. De processormodule maakt van 5V zijn eigen 3V3 voeding. De lidar heeft een stabiele 5V voeding nodig en verbruikt tot 500 mA. Een goede optie is om de module te voorzien van een eigen regulator met voldoende capaciteit.

Firmware

De Frimware, de software voor de Maple Mini, is geschreven in de Arduino IDE. Om dit mogelijk te maken is de IDE voorzien van de STM32Duino plugin.

De Maple Mini ontvangt en bewerkt de grote datastroom van de lidar. Door de beperkte rekenkracht en geheugen van de STM32 ligt Simultaneous localization and mapping (SLAM) buiten bereik.Wat wel mogelijk blijkt zijn een ‘sensor array’ van maximaal 16 segmenten en acht ‘virtuele sensoren’.

Sensor Array

De sensor array bestaat uit een aantal aaneengesloten segmenten. In bovenstaand voorbeeld zijn dit 9 segmenten van 20 graden. Ieder van deze segmenten geeft de kleinst gemeten afstand terug en op deze manier wordt – in dit voorbeeld – 180 graden afgedekt voor bijvoorbeeld obstakel vermijden.

Virtuele sensoren

Naast de sensor array bevat de module 8 virtuele sensoren. Voor ieder van deze sensoren kan de start¬hoek en openingshoek worden ingesteld. In bovenstaand voorbeeld zijn twee van dit soort sensoren weergegeven. De eerste met een kleine openingshoek naar achteren en de tweede met een grote openingshoek naar voren. De virtuele sensoren geven, naast de minimale afstand, ook de hoek van de minimale afstand terug. Hiermee is het mogelijk om objecten als blikken te vinden (tenminste, als je ze kunt onderscheiden van de wand…).

Library

Om de module te gebruiken kan deze via i2c worden aangesloten op (bijvoorbeeld) een Arduino. Een library voor de Arduino maakt aansturing van de module eenvoudig. De library, of eigenlijk een include file (.h), kun ja aan je project kunt toevoegen. Deze library verzorgt de i2c communiceert met de module. Je kunt de module instellen en uitlezen via eenvoudige functies.

Het demo programma ziet er als volgt uit:

#include <Wire.h>
#include "LppMaster.h"
#include "JsShared.h"
TLpp Lpp;

void setup() {
  Serial.begin(115200);      // start serial
  fdevopen( &my_putc, 0);    // Koppel printf uitvoer (stdout) via my_putc() aan de serial port.
 
  Wire.begin();              // start I2C

  if (Lpp.begin()) {
     // init success
     Lpp.SetOffsetDegrees(180);       // Align lidar with robotlib coordinate system
     Lpp.SetReverse(1);

     Lpp.ArraySetup(90, 20, 9);    // vanaf 90 graden, segmenten van 20 graden, 9 stuks
     Lpp.SensorSetup(0, 90, 180);  // Sensor 0, vanaf 90 graden, segment van 180 graden

     // Lees en print status (ter informatie)
     Lpp.ReadStatus();
     Lpp.PrintStatus();

     Lpp.Start();
  } else {
     printf("LPP I2C error.\n");
  }

  Lpp.Start();
}
 
void loop()
{

  Lpp.ReadSensors();
  printf("Sensor 0, graden: %d, afstand: %d\n", Lpp.Sensor[0].Angle/32, Lpp.Sensor[0].Distance);

  Lpp.ReadArray();
  printf("Array - eerste 3 segmenten: %d %d %d\n", Lpp.Array[0].Distance, Lpp.Array[1].Distance, Lpp.Array[2].Distance);

  delay(500);
}

Bovenstaand voorbeeldprogramma is, inclusief de 'libraries', te downloaden van Github