Autor: Peng

Aufgabenstellung

Einarbeitung in dier bestehende Software
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
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” [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')

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

%     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

        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 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 ( [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 Geschwendigkeit durch Lichtschranken berechnet die durchschnittliche Geschwendigkeit. Man zieht mit dem Faden das Objekt aus, damit der Kalmanfilter die durchschnittliche Geschwendigkeit 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. 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

Objekterkennung mit LiDAR

Inhaltsverzeichnis