Step 7 Strecken finden aus Coordinaten eines LaserScanners

Guten Tag,
Ich habe die ehrvolle Aufgabe bekommen einen Lichtschnittsensor in eine Produktionslinie einzubinden. Hierbei handelt es sich um das Fabrikat: "Leuze LPS 36/EN".
Von dem Sensor bekomme ich 760 Punkte mit X und Y Coordinaten. Meine Aufgabe ist es Objekte(Bretter mit Waldkante) zu vermessen. Es wäre ansich kein Problem wenn das Objekt immer eben daliegen würde(Siehe Bild 1)
Unbenannt.pngBild 1
Ich habe mir gedacht ich könnte das Problem Lösen indem ich nach geraden Strecken suche und die ungerade Waldkante über einfache Division ausrechnen könnte.

Jetzt kommt mein Problem:
Ich brauche einen Algorithmus mit welchen ich Strecken rein aus Coordinaten bestimmen kann.

Ich hatte bereits eine Idee, jedoch ist sie soo rechenintensiv, dass mir meine 319-3pn/dp mit einer Zykluszeit von über 70ms das leben schwer macht.

Hat irgendjemand eine idee?
Danke schonmal im Vorraus :-)


Info:
Ich habe alle Messpunkt in einem DB(DB_Messdaten) abgelegt



STRUCT
Messpunkt : ARRAY [1 .. 750 ] OF //vorläufige Platzhaltervariable
"Messpunkt";
END_STRUCT ;
BEGIN
Messpunkt[1].YWert := 0;
Messpunkt[1].XWert := 0;
Messpunkt[2].YWert := 0;
Messpunkt[2].XWert := 0;
Messpunkt[3].YWert := 0;
Messpunkt[3].XWert := 0;
Messpunkt[4].YWert := 0;
Messpunkt[4].XWert := 0;

etc.


Hier mein Testprogramm:

[code]

FUNCTION "StraightFinder" : VOID
TITLE =Straight Finder
//
//
//Dieses Programm dient zum erkennen von Geraden, aus der Datenwolke des LPS
//
//
//
AUTHOR : Oberm
VERSION : 0.1


VAR_INPUT
Extern_mode : BOOL ;
IN_Aufloes : INT ;
IN_min_laenge : INT ;
END_VAR
VAR_TEMP
Aufloesung : INT ;
Pointer_01 : DWORD ; //Wird mehrmals für verschiedenes verwendet!!!!!
step01 : INT ;
Pointer_02 : DWORD ; //Wird mehrmals für verschiedenes verwendet!!!!!
Punkt_A1_Pointer : DWORD ;
step2 : INT ;
Punkt_B1_Pointer : DWORD ;
Hoehendiff : INT ;
Laengendiff : INT ;
Steigung : REAL ;
min_laenge : INT ;
step3 : DINT ;
Pointer_03 : DINT ;
Temp_distance : INT ;
Vergl_wert : INT ;
Pointer_04 : DINT ;
Anz_Punkt : INT ;
theo_anz_Punkt : DINT ;
step4 : INT ;
END_VAR
BEGIN
NETWORK
TITLE =Grundparameter

//Auflösung festlegen (zwischen 0 - 20 empfohlen) 0 = max
UN #Extern_mode;
SPBN m01;
L 450;
T #min_laenge;

L 10;
T #Aufloesung;

SPA m02;
m01: NOP 0;
L #IN_min_laenge;
T #min_laenge;


L #IN_Aufloes;
T #Aufloesung;
m02: NOP 0;


NETWORK
TITLE =Anfang Suchen
//
//Den ersten relevanten Messpunkt suchen
AUF "Messdaten";
L P#36.0;
T #Pointer_01;

L 365;
next: T #step01;
L DBW [#Pointer_01];
L 0;
<>I ;
SPB m03; //Escape

L #Pointer_01;
L P#4.0;
+D ;
T #Pointer_01;

L #step01;
LOOP next;
m03: NOP 0;



NETWORK
TITLE =Straight Finder Main Programm
//Von einem festen Punkt(A1) wird versucht eine Strecke zu finden, welche
//möglichst lang ist. Hierzu wird am Anfang die kleinstmöglich Strecke gesucht und
//dann Stück für Stück verlängert. Strecke A1->B1
//
//Das Programm soll nun überprüfen, anhand der Punkte die auf dieser Strecke
//liegen, ob diese Gerade ist! Hierzu wird vorher definiert, wie viele Punkt auf
//dieser Strecke liegen müssten (minus Fehlertoleranz). Sind nun deutlich weniger
//Punkte auf dieser Strecke als zuvor, ist die geradheit der Strecke nichtmehr
//gewährleistet!!! Die letzte gültige Strecke soll in einem DB Abgespeichert
//werden und der Punkt A1 wird auf die Position des Punktes B1 gesetzt! Nun
//beginnt das Prozedere von vorn mit einem neuen Punkt A1!!!
//
//Vorbereiten
AUF DI 2;

L P#0.0;
T #Pointer_02;


L #Pointer_01;
T #Punkt_A1_Pointer;


L #Pointer_01;
L P#4.0;
+D ;
T #Punkt_B1_Pointer;



// Hier Beginnt es !!!
L 1000; //Dient als obergrenze falls fehler im Programm sind
nex3: T #step4;

L DBW [#Punkt_B1_Pointer];
L DBW [#Punkt_A1_Pointer];
-I ;
T #Hoehendiff;


L #Punkt_A1_Pointer;
L P#2.0;
+D ;
T #Pointer_01;

L #Punkt_B1_Pointer;
L P#2.0;
+D ;
T #Pointer_02;


L DBW [#Pointer_02];
L DBW [#Pointer_01];
-I ;
T #Laengendiff;

L #Hoehendiff;
ITD ;
DTR ;
L #Laengendiff;
ITD ;
DTR ;
/R ;
T #Steigung;

//Vorbereitung für 2. Loop

L #Punkt_A1_Pointer;
L P#6.0;
+D ;
T #Pointer_03;
L 0;
T #Temp_distance;
L 0;
T #Anz_Punkt;






L #Punkt_B1_Pointer;
L #Punkt_A1_Pointer;
-D ;
SRD 3;
L 4;
/D ;
T #theo_anz_Punkt;

nex2: T #step3;
L DBW [#Pointer_03];
L DBW [#Pointer_01];

-I ;
T #Temp_distance;


L #Temp_distance;
DTR ;
L #Steigung;

*R ;
L DBW [#Punkt_A1_Pointer];
DTR ;
+R ;
RND ;
T #Vergl_wert;


L #Pointer_03;
L P#2.0;
-D ;
T #Pointer_04;

//Überprüfen ob Punkt auf der Strecke liegt
U( ;
L #Vergl_wert;
L #Aufloesung;
-I ;
L DBW [#Pointer_04];

<=I ;
) ;
U( ;
L #Vergl_wert;
L #Aufloesung;
+I ;
L DBW [#Pointer_04];

>=I ;
) ;
//falls der Punkt auf der Strecke liegt wird dieser Gewertet
SPBN m001;
L #Anz_Punkt;
L 1;
+I ;
T #Anz_Punkt;
m001: NOP 0;

//nächsten Punkt auf der Strecke vorbreiten

L #Pointer_03;
L P#4.0;
+D ;
T #Pointer_03;


L #step3;
LOOP nex2;

//Überprüfen ob es sich um eine Strecke Handelt

L #Anz_Punkt;
L 3; //Fehlertolleranz
+I ;
L #theo_anz_Punkt;
<I ;
SPB m002; //Loop beenden falls die Strecke nichtmehr gewährleistet ist

//Strecke verlängern
L #Punkt_B1_Pointer;
L P#4.0;
+D ;
T #Punkt_B1_Pointer;

L #step4;

LOOP nex3;

m002: NOP 0;

//--Die Arbeit am Programm habe ich ab hier Pausiert um zu Testen ob er überhaupt die Erste Strecke findet--//

L #Anz_Punkt;


L #Temp_distance;
[code]

Angehängte Grafiken

• Unbenannt.png (2,3 KB)