Projekt 81: Inbetriebnahme und Objekttracking des LiDAR-Lite v3

Garmin LiDAR lite v3

→ zurück zur Übersicht: WS 18/19: Fachpraktikum Elektrotechnik (MTR)

Autor: Isaac Mpidi-Bita und Thomas Miska
Betreuer: Prof. Schneider

Aufgabe

Inbetriebnahme eines Low-Cost- LiDAR sowie eine Objektbildung und -tracking zum visualisieren der aufgenommen Daten.

Erwartungen an die Projektlösung

• Inbetriebnahme des LIDAR-Lite v3 von Garmin mit Arduino
  
• Erstellung eines Matlab und Simulink Treibers zur Echtzeitverarbeitung
• Recherchieren Sie geeignete Verfahren zur Objektbildung und zum Objekttracking
• Setzen Sie eines dieser Verfahren um.
• Bewertung der Ergebnisse mit geeigneter Referenz (z.B. Topcon Total Station)
• Softwareentwicklung nach HSHL Standard in SVN
• Darstellung der Funktion des LiDARs in einem YouTube Video
• Test und wissenschaftliche Dokumentation
• Live Vorführung während der Abschlusspräsentation

Hinweis: In Projekt 82 entsteht eine 3D-Schwenkeinheit. Beachten Sie, dass die 3D Daten des Sensors sowie die horizontale und vertikale Winkelmessung in Echtzeit verarbeitet werden muss. Stimmen Sie die Schnittstellen frühzeitig ab.

Einleitung

Für die Carolo Cup AG wird ein LiDAR benötigt, sodass das Fahrzeug Objekte erkennen kann.

Dieses Projekt widmet sich der ersten Inbetriebnahme des LiDAR, sowie eine grafische Objektbildung und wird während der Lehrveranstaltung "GET-Fachpraktikum" durchgeführt.

Als Basis für das Projekt, dient das zeitgleich stattfindende Projekt: 3D Schwenkeinheit für einen LiDAR Like v3. Hierbei wird die Halterung des LiDARs konstruiert und erstellt, sowie der elektrische Aufbau durchgeführt.

Projektplanung

Verwendete Software

• Arduino Software IDE 1.8.1
• MATLAB/Simulink 2018a
• Fritzing
• Processing
• Tortoise SVN

Technische Übersicht des LiDAR-Lite v3

Eigenschaft	Daten
Abmessung	48 mm x 40 mm x 20 mm
Spannungsversorgung	4.75 V bis 6 V
Stromverbrauch	105 mA, Leerlauf 130 mA, kontinuerlich
Messbereich	40 m
Genauigkeit	+/- 2,5 cm
Schnittstelle	I2C oder PWM

Hardwareaufbau

Schaltplan

Steckplatine

Mathematischen Hilfsmittel

Allgemeine Darstellung der Kugelkoordinaten

Geographische Koordinatensysteme

Für die Darstellung des Bildes werden Punkten einzeln aufgenommen. Ein Punkt wird in der kartesischen Koordinaten mit $x$-, $y$-, und $z$-Wert $P = (x,y,z)$ gekennzeichnet. In diesem Projekt wurde zwei Servomotoren und den LiDAR Lite v3 zur Ermittlung von Punkten. Die Punkten werden somit in Kugelkoordinaten dargestellt. Der von LiDAR ermittelte Abstand wird als Radius $r$, der obere und der untere Servomotor werden jeweils mit $\theta$ und $\varphi$ gekennzeichnet. Für die spätere Montage im Fahrzeug des Carolo Cup AG werden wir uns per Konvention an Fahrzeugkoordinatensystem anhalten. Deshalb benennen wir bereits in dieser Projekt $\varphi$ als YAW-Winkel und $\theta$ als PITCH-Winkel. Deshalb spielen Koordinatensystem eine wichtige Rolle. Die Umrechnung von Kugelkoordinatensystem ins kartesische erfolgt durch:

$\begin{array}{cll} x &=& r \cdot \sin \theta \cdot \cos \varphi \\ y &=& r \cdot \sin \theta \cdot \sin \varphi \\ z &=& r \cdot \cos \theta \end{array}$

Für die Vorführung wird das LiDAR-System nicht verwendet werden. Hierfür wird ein Darstellung des Raums nachgebildet. Aus diesem Grund wird eine andere variante der Repräsentation der Kugelkoordinaten angewendet. Hierfür wird die Geographischen Koordinatensystem angewendet. Der Unterschied liegt an die Richtung des Winkel $\theta$ . Der Winkel $\theta_{neu}$ eilt um $\frac{\pi}{2}$ vor. Und somit $\theta_{neu} =\frac{\pi}{2} - \theta$ . In der Allgemeine Formel eingesetzt, erhalten wir:

$\begin{array}{cll} x &=& r \cdot \sin (\theta_{neu}) \cdot \cos \varphi \\ y &=& r \cdot \sin (\theta_{neu}) \cdot \sin \varphi \\ z &=& r \cdot \cos (\theta_{neu}) \end{array}$

Mit der Erkenntnis, dass Sinus und Cosinus ebenfalls um $\frac{\pi}{2}$ phasenverschoben sind, können wir die Gleichungen so darstellen:

$\begin{array}{cll} x &=& r \cdot \cos \theta \cdot \cos \varphi \\ y &=& r \cdot \cos \theta \cdot \sin \varphi \\ z &=& r \cdot \sin \theta \end{array}$

Die Koordinatentransformation lässt sich in MATLAB folgendermaßen implementieren:

function [x y z] = transformCoord2(radius, yawAngle, pitchAngle)
   % transformCoord2- Description
   % Die Funktion wandelt die Messpunkte von Kugelkoordinaten 
   % ins kartesische
   % Syntax: [x y z] = transformCoord2(radius, yawAngle, pitchAngle)

   %% Winkelumrechnung von Degree zu Bogenmaß
   theta = double(pitchAngle * pi / 180);  
   phi = double(yawAngle * pi / 180); 

   %% Koordinatentransformation
    x = radius * cos (theta) * cos (phi);
    y = radius * cos (theta) * sin (phi);
    z = radius * sin (theta);

end

Steuerung mit Arduino IDE

Die erste Inbetriebnahme des Sensors wurde mit der Arduino IDE durchgeführt. Dieser Schritt diente der ersten praktischen Auseinandersetzung mit dem Sensor, sowie ein Funktionstest des Sensors mit dem geplanten Schaltplan.

Der folgende Code ermöglicht ein 3D-Scan in einem 180° Drehbereich (bedingt durch die Servomotoren).

void loop() {
  //delay(50);
  //Sweep Yaw servomotor
  for (yawAngle = 0; yawAngle <= 180; yawAngle += YAW_STEP) {
    servoYaw.write(yawAngle);
    
    //Sweep Pitch servomotor. The direction depends on the current directory
    if(pitchAngle < 90){
      for (pitchAngle = 0; pitchAngle <= 180;pitchAngle+= PITCH_STEP){
        servoPitch.write(pitchAngle);
        sendMeasurement();
      }
    } else {
      for (pitchAngle = 180; pitchAngle >= 0;pitchAngle-= PITCH_STEP){
        servoPitch.write(pitchAngle);
        sendMeasurement();
      }
    }
  }
}

// Function to acquire, convert and send the measurement.
void sendMeasurement(){
  //delay(DELAY_BETWEEN_SAMPLES);
  
  // Get spherical coordinates
  r = myLidarLite.distance(false);
  theta = (float)yawAngle * PI / 180.0f;
  phi = (float)pitchAngle * PI / 180.0f;

  // Convert and send them
  sprintf(s,"%d %d %d\n\0",(int)(r*cos(phi)*cos(theta)),(int)(r*cos(phi)*sin(theta)),(int)(r*sin(phi)));
  Serial.print(s);
}
end

Um die Messwerte visuell darstellen zu können, wurde ein zusätzliches Programm verwendet. Das Programm heißt Processing und erstellt aus diesen Messwerten eine PointCloud. Mit dieser PointCloud konnte anschließend ein 3D Plot erstellt werden. Nachfolgend befindet sich der Code für das erstellen der PointCloud.

void draw() {
  // Prepare window & drawing
  perspective();
  background(33);
  stroke(255,255,255);
  sphere(0.5f);
  fill(50);
  ellipse(0, 0, 10, 10);

  //Read Serial Port (if we can)
  String serialString = serial.readStringUntil('\n');
  if (serialString != null) {
    String[] coordinates = split(serialString, ' ');
    if (coordinates.length == 3) {
      pointList.add(new PVector(float(coordinates[0]), float(coordinates[1]), float(coordinates[2])));
    }
  }
 
  // Draw the actual point cloud
  for (int index = 0; index < pointList.size(); index++) {
    PVector v = pointList.get(index);
    if (index == pointList.size() - 1) {
      // Draw a line between Lidar and last point
      stroke(255, 15, 15);
      line(0, 0, 0, v.x,v.y,v.z);
    }
    // Draw point with a variable color
    stroke(255-v.z, 255-v.y, 255-v.x);
    point(v.x, v.y, v.z);
  }
}

// Handle keyboard events : movement, save & clear all points.
void keyReleased() {
  if (key =='x') {
    // erase all points
    pointList.clear();
  } else if(key == 's'){
    saveToFile();
  }
  else if (key == CODED) {
    if (keyCode == UP) {
      cam.rotateX(angleIncrement);
    } else if (keyCode == DOWN) {
      cam.rotateX(-angleIncrement);
    }else if (keyCode == LEFT) {
      cam.rotateY(angleIncrement);
    }else if (keyCode == RIGHT) {
      cam.rotateY(-angleIncrement);
    }
  }
}

Durch diesen ersten erfolgreichen Test konnten wir nun Probleme erkennen und diese im Verlauf des Projektes berücksichtigen. Probleme die uns aufgefallen waren:

• Arduino in seiner Leistung eingeschränkt
• Koordinatentransformation wird benötigt
• Der LiDAR ist in seinem Messbereich eingeschränkt

Einbindung von MATLAB

Nach der ersten erfolgreichen Durchführung mit dem Arduino Code, wurde dieser in MATLAB/Simulink umgesetzt.

%% Auslesen der Daten aus der serielle Schnittstelle 
disp('============================================================')
disp('                 Datenaufnahme vom Arduino                  ')
disp('============================================================')
tic;
for i=1:1:10000
    
    received = fgets(sObject);
    disp(i);
    zwischenspeicher = strsplit(received,' ');
    
    %% Daten vorverarbeitung
    rohdatenDouble(i,1)= cellfun(@str2double,zwischenspeicher(1));% Radius
    rohdatenDouble(i,2)= cellfun(@str2double,zwischenspeicher(2));% Yaw
    rohdatenDouble(i,3) = cellfun(@str2double,zwischenspeicher(3));% Pitch
    
    [transFdaten(i,1) transFdaten(i,2) transFdaten(i,3)] = transformCoord(...
                        rohdatenDouble(i,1), ...
                        rohdatenDouble(i,2), ...
                        rohdatenDouble(i,3));
end

dlmwrite('messwerte.xyz',transFdaten, 'delimiter','\t');

timePart = toc;
time = time + timePart;
disp('============================================================')
disp('Die Messdaten wurde erfolgreich aufgenommen und gespeichert.')
disp(['         benötigte Zeit: ', num2str(timePart), ' Sekunden'] )
disp('============================================================')

%% Erstellung eines Point Cloud 
disp('============================================================')
disp('                     Messdaten laden                        ')
disp('============================================================')
tic;

xyzPoints = load('messwerte.xyz');
ptCloud = pointCloud(xyzPoints);


timePart = toc;
time = time + timePart;
disp('============================================================')
disp('               Laden der Daten erfolgreich.                 ')
disp(['         benötigte Zeit: ', num2str(timePart), ' Sekunden'] )
disp('============================================================')

%% Festlegung von Intervellen
disp('============================================================')
disp('               Ausgabe der Ergebnissen                      ')
disp('============================================================')
tic;

radius = 300;    % in Zentimeter

xRangeLimit = [-radius radius]; 
yRangeLimit = [-radius radius];  
zRangeLimit =[-radius radius]; 
player = pcplayer(xRangeLimit, yRangeLimit, zRangeLimit);

Zusammenfassung

Lessons Learned

• praktische Projektplanung
• Verwenden eines LiDAR Sensors
• Koordinatentransformation
• PointCloud (LiDAR Messwerte 3 dimensional plotten)

Literatur

--- → zurück zur Übersicht: WS 18/19: Fachpraktikum Elektrotechnik (MTR)