Objekterkennung mit LiDAR

Autor: Peng

Aufgabenstellung

1. Einarbeitung in dier bestehende Software
2. Softwareentwurf in MATLAB
   1. Meilenstein 1:
   2. Ansteuerung des Hokuyo LiDAR
   3. Koordinatentransformation polar- zu karthesisch
   4. Testdokumentation der KOS-Trafo
   5. Objektbildung (z.B. Sukzessiv Edge Following)
   6. Testdokumentation der Objektbildung
   7. Meilenstein 2:
   8. Objekttracking (mit Kalman-Filter)
   9. Testdokumentation des Objekttrackings
   10. Attribute schätzen: v, a, B, T, Güte (s. Schnittstellendokument)
   11. Testdokumentation der Attribute mit Referenz
   12. Dokumentation im Wiki
3. Softwareentwurf in C
   1. Meilenstein 3:
   2. Ansteuerung des LiDAR
   3. Koordinatentransformation polar- zu karthesisch
   4. Testdokumentation der KOS-Trafo
   5. Objektbildung (z.B. Sukzessiv Edge Following)
   6. Testdokumentation der Objektbildung
   7. Meilenstein 4:
   8. Objekttracking (mit Kalman-Filter)
   9. Testdokumentation des Objekttrackings
   10. Attribute schätzen: v, a, B, T, Güte (s. Schnittstellendokument)
   11. Testdokumentation der Attribute mit Referenz
   12. Testoberfläche in Control Desk
   13. Dokumentation im Wiki

Softwareentwurf in MATLAB

Ansteuerung des LiDAR Hokuyo URG-04LX

Der VerbindungsCode in Matlab wird in SVN schon gefunden. Aber wird es beachtet, dass die Nummer des verwendeten seriellen COM Ports angepasst werden muss, damit dieses funktioniert.

% Stelle Verbindung mit LiDAR her
stComport='COM6';
SetupLidar;

Durch das Anrufsprogramm "LidarScan" wird der Radius erhalten.

% Das Resultat von LidarScan
[rangescan]=LidarScan(lidar);

Koordinatentransformation polar- zu kartesisch

Da der Laserscanner nur Polarkoordinaten ausgibt, ist es notwendig, diese in Kartesische Koordinaten zu transformieren, um die Figur darzustellen. Die folgend mathmatische Funktion wird im Programm verwendet.

$x = radius * cos(\vartheta )$

$y = radius * sin(\vartheta )$

Wegen des Einheit in Meter wird die Einheit im Programm umgerechnet.

% Das Koordinatenparameter in m
x(cnt)=rangescan(cnt)*sin(phi_rad(cnt))/1000;
y(cnt)=rangescan(cnt)*cos(phi_rad(cnt))/1000;

Koordinatentransformation LiDARfest zu Fahrzeugfest

Um das Hindernis zu vermeiden, wird LiDARfest in Fahrzeugfest transformiert.

LiDARfest KOS:

• Index: L (LiDARfest)
• Einheit/Skalierung: m
• Ursprung: Mitte
• x-Achse: nach links/rechts
• y-Achse: nach oben/unten
• z-Achse: keine vorhanden

Fahrzeugfestes KOS:

• Index: K (körperfest, karosseriefest)
• Einheit/Skalierung: mm
• Ursprung: Mitte Vorderkante des vorderen Stoßfängers des Fahrzeugs (z = 0)
• x-Achse: nach vorne (im Sinne der Hauptfahrtrichtung)
• y-Achse: nach links
• z-Achse: nach oben

Im Vergleich zueinander wird der Urspruch verschoben und die Achsen gewechelt.

• Die Verschiebung von Y-Achse: 0,08 m.
• Die Verschiebung von X-Achse: 0,145 m.

%Definition
distY=0,08;
distX=0,145;
[···]
%LiDARfest zu Fahrzeugfest KOS
for (cnt=1:length(Segment))    
   for (cnt=1:length(Segment))   
    plausible=0;
    if abs(Segment(cnt).LeftCartesian(1)-distY) < 2.0 && abs(Segment(cnt).LeftCartesian(2)-distX) <2.0  
    Y=[Segment(cnt).LeftCartesian(2)  Segment(cnt).NearbyCartesian(2) Segment(cnt).RightCartesian(2)];
    X=[Segment(cnt).LeftCartesian(1)  Segment(cnt).NearbyCartesian(1) Segment(cnt).RightCartesian(1)];  
    x=-Y-distX;
    y= X-distY;
    plot(x,y,'r')    
hold on

Objektbildung

Abb. 1: Beispiel Objektbildung, Segmente in rot, Ecken mit "O" markiert, Vordere Mitte mit "X"

Die Hauptaufgabe der Objektbildung ist es, die Daten des Hokuyo URG 04LX in sinnvolle Objekte, die der Realität entsprechen, zu segmentieren. Um das vorliegende Ziel zu erreichen, kann man das Verfahren „Sukzessiv Edge Following” [2]verwenden. Sukzessiv Edge Following bedeutet, dass ein Menge von Punkten zu drei Punkten reduziert wird. Die Verbindung dieser drei Punkte stellt eine Kante dar. Einerseits werden so Kannten bzw. Objekte erzeugt, andererseits kann das Rauschen beseitigt werden. Außerdem werden die Tiefe, die Breite, die Ausrichtungen und die Koordinaten X und Y der zentralen vornen Punkte.

Der folgende Code stellt vier Kanten beziehungsweise ein komplettes Objekt dar.

    Segment = SucessiveEdgeFollowing(rangescan); 
     [···]
  for (cnt=1:length(Segment))   
    plausible=0;
    if abs(Segment(cnt).LeftCartesian(1)-distY) < 2.0 && abs(Segment(cnt).LeftCartesian(2)-distX) <2.0  
    Y=[Segment(cnt).LeftCartesian(2)  Segment(cnt).NearbyCartesian(2) Segment(cnt).RightCartesian(2)];
    X=[Segment(cnt).LeftCartesian(1)  Segment(cnt).NearbyCartesian(1) Segment(cnt).RightCartesian(1)];  
    x=-Y-distX;
    y= X-distY;
    centerX = (x(1)+x(3))/2;
    centerY = (y(1)+y(3))/2;   
    m=[ x(1) 2*(centerX)-x(2) x(3)];
    n=[ y(1) 2*(centerY)-y(2) y(3)]; 
    plot(x,y,'r') 
    plot(m,n,'r')

a,b sind die Länge der Kannte. c ist die Länge der Diagonale. Alpha1 entsprecht die Differenz der Winkel von links bis Nearby. Alpha2 entsprecht die Differenz der Winkel von Nearby bis rechts. Die größte Differenz entsprecht für LiDAR den vornen Rand. Anders gesagt wird der Mitte des vornen Rang leicht gefunden. Der Code zeigt folglich:

%     Breite und Tiefe und X und Y Koordinate des Objektmittelpunktes
         a = sqrt((x(1)-x(2))^2+(y(1)-y(2))^2);
         b = sqrt((x(2)-x(3))^2+(y(2)-y(3))^2); 
         c=  sqrt((x(1)-x(3))^2+(y(1)-y(3))^2);
     Alpha1 = abs(Segment(cnt).Nearby(2)-Segment(cnt).Left(2));
     Alpha2 = abs(Segment(cnt).Right(2)-Segment(cnt).Nearby(2));

   if(Alpha1 >= Alpha2)
           B = a; T=b;
         center_vorneX = (x(1)+x(2))/2;
         center_vorneY = (y(1)+y(2))/2; 
   else
           B = b; T=a;
         center_vorneX = (x(3)+x(2))/2;
         center_vorneY = (y(3)+y(2))/2;
   end

Durch die Richtungskoeffizent wird die Objektausrichung ermittelt. Dann wird die Ausrichtungsdefinition des LiDars in die Ausrichtungsdefinition der Mathematik transformiert.

        K= (centerY-center_vorneY)/(centerX-center_vorneX);
        objausrichtung = rad2deg(atan((K)));
   
             if K>0
              if(centerY < center_vorneY)
                objausrichtung = objausrichtung+180;
              else
                objausrichtung = objausrichtung;
              end
          else
              if(centerY < center_vorneY)
                objausrichtung=objausrichtung+360;
              else
                objausrichtung=objausrichtung+180;
              end
           end

          if (K ==0)
              B= c; T=0;
              center_vorneX = (x(1)+x(3))/2;
              center_vorneY = (y(1)+y(3))/2;
              objausrichtung=90;
          end

          if x(3)-x(1)==0 && y(3)-y(1)~=0
              B= c; T=0;
              center_vorneX = (x(1)+x(3))/2;
              center_vorneY = (y(1)+y(3))/2;
              objausrichtung=0;
          end

Objektracking

Objekttracking ist mit dem Kalmanfilter,die Parameter beziehungsweise die Geschwindigkeit, die Koordinate x und y zu prognosizieren. Der Test wird in zwei Situationen eingeteilt.Einerseits ist es ein statisches Objekt,andereseits ist es ein dynamisches Objekt. Dabei wird zwischen statischen und dynamischen Objekten unterschieden.

Folglich zeigt die relevante Formel des Kalmanfilters:

Präikationsprozess:

1.Präikation des Zustandsverktor: $x_p = A\times x$

2.Präikation der kovarianzmatrix: $P_p = A \times P \times A' + Q$

Schäzprozess:

3.Kalman-Verstärkung: $K = P_p \times H' \times \frac {1} {H \times P_p \times H'+R}$

4.Korrektur der Zustandsschäzung: $x = x_p + K \times (z - H \times x_p)$

5.Korrektur der Kovarianzschäzung: $P = P_p - K \times H \times P_p$

Nach der Physik werden der Abstand, die Geschwindigkeit und die Beschleunigung dargestellt. Wenn der Abstand 1 ist, sind die Geschwindigkeit und die Beschleunigung jeweils $dt, \frac {dt^2} {2}$. Deswegen können A,Q berechnet werden. R bedeutet das Messrauchen und kann durch die Standardabweichung zum Quadrad beziehungsweise die Varianz berechnet werden.( [2] basierend auf den Code von Phil Kim und Herrn Prof. Dr.-Ing. Schneider) :

% Beim ersten Durchlauf Variblen initialisieren
if isempty(firstRun)
  % Abtastzeit
  dt = 0.1;
  
  % Systemmatrix der linearen Zustandsraumdarstellung
  A = [ 1   0   dt  0       (dt^2/2)  0
        0   1   0   dt    0         (dt^2/2)
        0   0   1   0       dt        0
        0   0   0   1       0         dt
        0   0   0   0       1         0
        0   0   0   0       0         1];
  
  %Beobachtungsmatrix der linearen Zustandsraumdarstellung
  H = [ 1  0  0  0  0  0
        0  1  0  0  0  0];
 
  % Kovarianzmatrix des Systemrauschens
  Q = 0.001*[ 1        0        dt       0        (dt^2/2)  0
            0        1        0        dt       0         (dt^2/2)
            dt       0        dt^2     0        (dt^3/2)  0
            0        dt       0        dt^2     0         (dt^3/2)
            (dt^2/2) 0        (dt^3/2) 0        (dt^4/4)  0 
            0        (dt^2/2) 0        (dt^3/2) 0         (dt^4/4)];
  % Kovarianzmatrix des Messrauschens
  R = [ 0.00006  0
        0  0.00003 ];

  % Systemzustandsvektor eines linearen dynamischen Systems   
  x = [0, 0, 0, 0, 0, 0]';
  % Kovarianzmatrix des Schätzfehlers
  P = 100*eye(6);
  
  firstRun = 1;
end

%% Kalman Gleichungen

% Prädikationsprozess
xp = A*x;               % Präikation des Zustandsverktor
Pp = A*P*A' + Q;        % Präikation der kovarianzmatrix

% Schätzprozess
K = Pp*H'*inv(H*Pp*H'+R);% Kalman-Verstärkung 
z = [xm ym]';            % Messwerte
x = xp + K*(z - H*xp);   % Korrektur der Zustandsschäzung
P = Pp - K*H*Pp;         % Korrektur der Kovarianzschäzung

% Rückgabewerte
xh = x(1); % x-Position 
yh = x(2); % y-Position
% Geschwindigkeit
vx = x(3);
vy = x(4);
% Beschleunigung
ax = x(5);
ay = x(6);

Mit dem Kalmanfilter werden die Situtationen mit statischen Objekten oder dynamitschen Objekten behandelt.

Abb. 2: Beispiel Statische Objekt, Ecken mit "O" markiert, Vordere Mitte mit "X".

Die Geschwindigkeit durch Lichtschranken berechnet die durchschnittliche Geschwindigkeit. Man zieht mit dem Faden das Objekt aus, damit der Kalmanfilter die durchschnittliche Geschwindigkeit und die Koordinaten X und Y berechnen kann.Folgendlich zeigt der Testaufbau des dynamischen Modells.

Abb. 4: Beispiel Testaufbau.

Abb. 4: Beispiel Bewegungsablauf des dynamischen Objektes, Ecken mit "O" markiert, Vordere Mitte mit "X".

Attribute schätzen

Zu diesem Teil ist Attribute zu schätzen. Es wird in zwei Teilen beziehungsweise den statischen Test des Objektes und den dynamischenTest des Objektes eingeteilt.

Aus der Tabelle kann man erkennen, dass die Messabweichung zwischen die Messwerte sehr klein ist. Deswegen sind die statischen Daten erfolgreich. Die Messwerte des dynamischen Objekt haben einige Probleme.Einige Breite und Tiefe haben einen Unterschied zwischen die aktuellen Breite und Tiefe. Einerseits wird nur einige Wertpaare gemesst ,andereseits wird während der Ausziehung mit dem Faden die Messwerte beeinflusst. Außerdem kann man von dem Bild oder der Tabelle erkennen, dass einige Linie, mit denen kein komplettes Objekt dargestellt wird, erzeugt wird. Anders gesagt ist es das Rauschen. Der Faden ist vielleicht den Einfluß darauf. Noch ein Problem ist , dass manche Tiefe nicht komplett eingescannt werden können. Deswegen zeigen einige Tiefe in der Tabelle seht klein. Anders gesagt beeinflußt die Objektausrichtung, die durch die Koordinate Y duch die Koordinate X beziehungsweise durch die Richtungskoeffizent berechnet wird. Die Lösung ist mehrmals zu messen.Dann kann die Messwerte ohne Einfluss bekommen werden.

Softwareentwurf in C++

Literatur und Dokumente

• [1] MATLAB Beispiel
• [2] Code vom SEF und Kalmanfilter

---

→ zurück zum Hauptartikel: SDE Praktikum Autonomes Fahren