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

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” 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. Code zeigt folglich:

    Segment = SucessiveEdgeFollowing(rangescan); 
     [···]
%     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);
         centerX = (x(1)+x(3))/2;centerY = (y(1)+y(3))/2;
   if(a<b)
           B = b; T=a;
         center_vorneX = (x(3)+x(2))/2;
         center_vorneY = (y(3)+y(2))/2; 
   else
           B = a; T=b;
         center_vorneX = (x(1)+x(2))/2;
         center_vorneY = (y(1)+y(2))/2;
   end

        K= (centerY-center_vorneY)/(centerX-center_vorneX);
        objausrichtung = rad2deg(atan((K)));
          if K>0
              if(centerY<center_vorneY)
                objausrichtung=objausrichtung-90;
              else
              objausrichtung=90-objausrichtung;
              end
          else
               if(centerY<center_vorneY)
               objausrichtung = 90-objausrichtung;
               else    
               objausrichtung =-(objausrichtung+90);
               end
          end
           
          if (K == inf ||K == -inf || K ==0 || y(1)-y(2))/(x(1)-x(2)) == (y(1)-y(3))/(x(1)-x(3))
              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 Geschwendigkeit, 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$

Der entsprechende Parameter beziehungsweise A, Q, H, R und P zeigt folglich([0]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);

Softwareentwurf in C++

Getting started

• MATLAB Beispiel

---

→ zurück zum Hauptartikel: SDE Praktikum Autonomes Fahren