Laser-Positionsscanner

[WL]

Hallo Kreative,
wie bereits hier https://www.roboternetz.de/community/threads/51543-Positionsbestimmung-im-Raum angekündigt will ich mal endlich anfangen………………..

Beschrieben wird ein Laserscanner für die absolute Positionsbestimmung Eurer Roboter. Dies ist (glaube ich) KEIN ANFÄNGERPROJEKT !!! :gift
Auch nicht gerade preiswert was die Hardware anbetrifft. Außerdem sind entsprechende Werkzeuge gefragt, die nicht jeder in der Bastelstube stehen hat.
Trotzdem nicht unmöglich dieses zu reproduzieren bzw. als Grundlage für eigene Ideen zu verwenden.

Alles ist noch nicht ausgearbeitet. Ich werde in lockeren Abständen erweitern! Das ganze Projekt läuft schon seit mehreren Jahren und ich muss teilweise richtig überlegen was ich da vor Jahren zusammengerührt habe. Bitte nicht drängeln………………

U.a. diese Quellen waren außer Oberallgeiers Beiträgen https://www.roboternetz.de/community/threads/36932-Navigation-mittels-Triangulation(nochmals vielen Dank an dieser Stelle) Lieferanten für dieses interessante und lehrreiche Projekt:
http://www.seattlerobotics.org/encoder/200109/lasernav.htm
http://www.philohome.com/sensors/lasersensor.htm (http://www.seattlerobotics.org/encoder/200109/lasernav.htmhttp://www.philohome.com/sensors/lasersensor.htm)


Wir beginnen mit etwas Theorie (Geometrie > Stichworte: Thales / Dreieck / Kreis):
Die Thales_x.pdf sind im Anhang versteckt. Es ist anzuraten diese vorher zu öffnen. Ein Bild sagt mehr.............. EDIT 03.10.2011 ! Es waren Fehler in den PDFs

Thales_1.pdf

Das grüne Rechteck ist die Spielfläche (z.B. damfino's Rasen). Die Punkte A, B und C stellen die Reflektoren dar. Die Abstände zwischen A und B respektive B und C müssen bekannt sein und werden dem µC als Variablen oder Konstanten mitgeteilt. Die Reflektoren müssen auf einer Geraden liegen (es geht auch in beliebigen Winkeln, diese Anordnung hat jedoch Vorteile, näheres später dazu).
Der gelbe Kreis stellt den Roboter dar, der kleine schwarze Pfeil seine Fahrtrichtung (Nase). Der Positionsscanner ist (praktischerweise) im Mittelpunkt angeordnet. Von hier aus werden die Winkel Alpha (zwischen A und B) , Beta (zwischen B und C) und Gamma (zwischen C und A) gemessen. Je genauer diese Winkel ermittelt werden umso höher die Auflösung der Absolut-Position (die Auflösung ist nicht gleichmäßig auf dem Feld verteilt!).
Als nächstes errichten wir lotrechte Linien bei A_B/2 und B_C/2. Diese Linien bilden später die X-Koordinaten für 2 Kreise mit deren Hilfe wir die Position des Laserscanners ermitteln.
Thales_2.pdf


Xm1 = T_ab / 2 : Xm2 = T_bc / 2 : Xm2 = Xm2 + T_ab



Die Kreise haben ihre X-Koordinaten immer auf diesen Hilfslinien und der Kreis läuft immer durch die Punkte A und B bzw. B und C.
Nun gilt es den Mittelpunkt (Y-Koordinate) der Kreise zu bestimmen. Dazu werden die gemessenen Winkel verdoppelt und damit die folgenden Dreiecke berechnet.
Thales_3.pdf



K1winkel2 = Beta * 2 : K2winkel2 = Alpha * 2 'Thaleskreis !



Thales_4.pdf



K1alpha = 180 -k1winkel2 : K1alpha = K1alpha / 2 'Mittelpunkt und Radius 1.Kreis
K1rad1 = Pi * K1alpha : K1rad1 = K1rad1 / 180
K1rad2 = Pi * K1winkel2 : K1rad2 = K1rad2 / 180
R1 = Sin(k1rad1) : R1 = R1 * T_ab : R1 = R1 / Sin(k1rad2)
Ym1 = Sin(k1rad1) : Ym1 = Ym1 * R1

K2alpha = 180 -k2winkel2 : K2alpha = K2alpha / 2 'Mittelpunkt und Radius 2.Kreis
K2rad1 = Pi * K2alpha : K2rad1 = K2rad1 / 180
K2rad2 = Pi * K2winkel2 : K2rad2 = K2rad2 / 180
R2 = Sin(k2rad1) : R2 = R2 * T_bc : R2 = R2 / Sin(k2rad2)
Ym2 = Sin(k2rad1) : Ym2 = Ym2 * R2



Da ich für hohe Mathematik zu dumm bin habe ich mich mal hier bedient: http://www.antonis.de/faq/progs (kreisnit.bas)
Nun brauchen wir nur noch die Kreise zu zeichnen :


Thales_5.pdf



If R1 < R2 Then 'der erste Radius muß größer als der 2. sein
Ht = Xm2 : Xm2 = Xm1 : Xm1 = Ht 'Swap
Ht = Ym2 : Ym2 = Ym1 : Ym1 = Ht
Ht = R2 : R2 = R1 : R1 = Ht
End If

Call Schnitt_kreis(xm1 , Ym1 , R1 , Xm2 , Ym2 , R2 , Xs1 , Ys1 , Xs2 , Ys2 , M) 'Schnittpunkte ermitteln



und die Schnittpunkte der beiden Kreise auszuwerten.
Ein Schnittpunkt liegt auf B (Y=0) und ist leicht auszusortieren :


I = Round(ys2)
If I = 0 Then 'Schnittpunkt liegt auf "B"
Ist_x_gemessen = Xs1 : Ist_y_gemessen = Ys1 'Schnittpunkt 1
Else
Ist_x_gemessen = Xs2 : Ist_y_gemessen = Ys2 'Schnittpunkt 2
End If

Und siehe da, der Roboter steht genau auf dem Schnittpunkt !!! War doch ganz easy, nicht war ? ;)

Als zusätzliche Information bekommt man den Winkel des Robbys im Raum. Dieser wird aus dem Winkel zum Reflektor B abgeleitet.
Nun haben wir alle Informationen um das Gefährt mit der Odometrie auf den (nächsten) Zielpunkt zu navigieren.
Thales_6.pdf



Weiter geht es mit dem eigentlichen Scanner (bald.....----->Theorie) ..............

[WL]

Da soweit alles klar ist geht es weiter mit der Hardware.........

Funktion Positionsscanner:
(Blockschaltbild.pdf im Anhang)

Der Scanner besteht mechanisch aus einem Rotor (grün) und einem feststehenden Teil (blau & grau).
Der Rotor ist (leicht) gebremst so dass das Getriebespiel nicht zum Tragen kommt.

Auf dem Rotor ist neben der Energieversorgung der modulierte Laser-Emitter und das Empfangsmodul untergebracht. Die empfangenen Signale werden über Infrarot durch die Antriebs(hohl)welle zum feststehenden Teil geleitet.

Am Rotor ist eine Zunge befestigt die den Nullpunkt (0°) im Zusammenhang mit dem Referenzsignal des Inkrementalgebers bildet. Mit diesem wird der Zähler zurückgesetzt.

Das ganze bildet einen durch die Bakenreflektoren getriggerten (hochauflösenden) Winkelmesser.

Durch den Inkrementalgeber wird der Zähler fortlaufend hochgezählt (im Beispiel von 0.....144000). Wird eine Bake erkannt wird über das Monoflop (MP2) dessen Zeitkonstante über der Periodendauer der Modulation liegt und über das Monoflop (MP4) das Latch getriggert und damit der momentane Winkel "eingefroren". Gleichzeitig kann der µC mit Int0 in die Interruptverarbeitung verzweigen und den Winkel über PortA abholen. Die Anzahl der 50KHz Impulse wird über Counter T1 gezählt. Es liegt nun an der Software das Ende des Impulspaketes festzustellen und mit PortB.3 (MP3) die Übernahme dieses Winkels erneut zu triggern und auszulesen.

Als endgültiger Winkel für die weiterführende Berechnung wird die Mitte zwischen Anfang und Ende der Bake herangezogen. Da die Bake ein rundes Profil hat und somit quasi der Mittelpunkt verwendet wird ist das ganze damit (weitgehend) Richtungsunabhängig.

Impulsdiagramm für ein Bakensignal (ideal) :

20015

Fragen hierzu?

Als nächstes dann die konkrete Schaltung.............
\\:D/

[WL]

Die Lichtschranke:

20066

(Lichtschranke_prinzip.pdf im Anhang)

Das rote Drehteil beinhaltet die Laserdiode (Kollimator) als Sender, einen Spiegel (45°) und den Empfänger nebst Sammellinse (90°) .
Zum Ausrichten der Komponenten zueinander ist das rote Teil auf der weißen Grundplatte drehbar angeordnet. Das ganze ist mittig auf der Platine aufgeklebt.
Auf der Unterseite der Platine sitzt die Infrarot-Sendediode und die Sekundärspule des Royerconverters.
20067

Auf der Abbildung oben sind die beiden Spulenhälften des Royerconverters etwas auseinandergezogen. Unter der Grundplatte (grünes FR4) ist die Mechanik und die µC-Platine verborgen. In diesem Prototypen habe ich versucht den gesamten Scanner kardanisch aufzuhängen (Weißer Ring im runden roten Gehäuse). Damit soll sich der Scanner selbsttätig waagerecht ausrichten.
20068
Links oben der Motor, vorn das Schneckengetriebe, rechts unten der Inkrementalgeber und in der Mitte die Hohlwelle durch die die Empfangssignale per IR geschickt werden.
Die Metallzunge auf dem Schneckenrad betätigt die Gabellichtschranke auf der µC-Platine (0°).

20069
Die (Prototyp)-µC-Platine:
In der Mitte das Weiße Drehteil trägt eine Sammellinse für die IR-Empfangsdiode. Links daneben die Gabellichtschanke. Das Flachbandkabel verbindet zum Inkrementalgeber, ganz links unten der Motoranschluß und rechts daneben der Royerconverter. Links oben der Zähler und die Latches.

[WL]

....hat leider nicht mehr reingepasst:



Für die kardanische Aufhängung und das runde Gehäuse (KG-Rohr) hat sich diese Bauform als ungünstig herausgestellt. In den Eagle-Files (Anhang) ist das bereits geändert. Die Platine muß an der großen runden Markierung "gestutzt" werden.

Rechts oben der 8-polige Steckverbinder ist die Verbindung zum übergeordneten System.


Weiter dann mit der Software........................

[Bernhard667]

Hallo WL,

das sieht ja sehr gut aus.
Welche Resultate kannst Du damit erreichen:
-Reichweite
-Genauigkeit
-Welche Umgebungsbedingungen (Tageslicht im Freien)

Bernhard

[vohopri]

Hallo Willi,

wie wird zwischen Bakenreflexionen und anderen Reflexionen unterschieden?

[WL]

@Bernhard667



-Reichweite
-Genauigkeit
-Welche Umgebungsbedingungen (Tageslicht im Freien)



Die Reichweite wird vom Kollimator(Leistung und Strahldurchmesser) und dem Reflektormaterial bestimmt.
Außerdem sollte (muß) Sender und Empfänger auf der optischen Achse zum Reflektor liegen.
Mit einem 1mW Kollimator und der 3M-Folie habe ich ca.10m erreicht.

Die Genauigkeit ist abhängig von der Auflösung des Winkelmessers (im Beispiel 144000/U entspr.0,0025°), dem mechanischen Aufbau (Präzision und Spielfreiheit des Getriebes) und der Position auf dem Feld (NICHT linear !).
Auf einer kleineren Testfläche(5*4m) würde ich mal für diesen Prototypen ganz vorsichtig ~ 1cm angeben.

Tageslicht spielt eigentlich keine Rolle. Es sei denn, die Sonne steht sehr tief und in einer Linie mit Scanner und Reflektor.


@vohopri




wie wird zwischen Bakenreflexionen und anderen Reflexionen unterschieden?


Das ist in der Tat ein Problem: :(
z.B. Rundglas (Flaschen) oder Flachglas (Fensterscheiben) sind für die Lichtschranke (gute) Reflektoren.

Bis auf weiteres:
Entweder die Verstärkung des Empfängers verringern (weniger Reichweite) oder Refexionen vermeiden !

Notwendig, aber noch nicht realisiert:
Im Normalfall werden 3 Baken/U detektiert. Wenn es mehr sind muß eine Plausibilitätskontrolle die n.i.O. Reflexionen ausfiltern (auf die ToDo-Liste).
Wenn gleich nach dem Einschalten schon mehr als 3 erkannt werden ist diese Blase aber geplatzt !

[WL]

Nun zur Software.........

Funktion Winkelerfassung:
(Impulse_Software.pdf in .zip)

Wie schon weiter oben erwähnt kommen die Impulse leider nicht immer als geschlossene Formation an (ideal) sondern sind oft "zerhackt" (Reflektor verschmutzt o.ä.). Dies muss die Software ausbügeln:

Bei einer Umdrehung des Scanners werden die Winkelinformationen (in der Int0) in die Arrays "Wa(x)" >(für Winkelanfang) und "We(x)" >(für Winkelende) eingetütet (schwarz in der Abb.).




'--------------------------------------------------------------------------
Int0_int:
Counter1 = 0 'hier werden die empfangenen Impulse gezählt
Led = 0
Laser_anf_low = Pina : Portc.6 = 1 : Waitus 2 '&b00 lesen
Laser_anf_mid = Pina : Portc.6 = 0 : Portc.7 = 1 : Waitus 2 '&b01 lesen
Laser_anf_high = Pina : Portc.7 = 0 '&b10 lesen
Waitus 40 '~2 Impulse ausblenden
Bitwait Pind.2 , Reset 'Burst Ende
Lesezaehler = 1 'Zählerwert speichern
Waitus 5
Lesezaehler = 0
Waitus 5
Laser_end_low = Pina : Portc.6 = 1 : Waitus 2 '&b00 lesen
Laser_end_mid = Pina : Portc.6 = 0 : Portc.7 = 1 : Waitus 2 '&b01 lesen
Laser_end_high = Pina : Portc.7 = 0 '&b10 lesen
Waitus 2
Target_quali = Counter1 'Anzahl der empfangenen Impulse
Toggle Buzzer 'kurze akustische Rückmeldung
Incr Anz_target 'Anzahl der Impulspakete
Winkelanz(anz_target) = Target_quali 'Impulse > Array
Wa(anz_target) = Laser_anf 'Winkel_anfang > Array
We(anz_target) = Laser_ende 'Winkel_ende > Array
Waitus 40 'Wartezeit
Led = 1
Eifr.0 = 1 'evt. anstehenden Interrupt löschen
Return






'-------------------------------------------------------------------------------
Sub Positionserfassung 'hier werden die Empfangsdaten ausgewertet
If Umdrehung_beendet = 1 Then
Anz_target_fertig = 1 'Ältere BASCOM-Versionen können nicht mit Index "0" !
If Anz_target > 1 Then
Temp_wa(1) = Wa(1) 'Anfang merken
For N = 1 To Anz_target 'Schleife über alle Pakete
Diff = Wa(n + 1) - We(n) 'Differenz zwischen den Paketen
If Diff < 0 Then Diff = Diff + 144000 '"0"-Durchgang
If Diff > 0 And Diff < 700 Then '700 ist die Toleranz (~1,7°)
Winkelanz(anz_target_fertig) = Winkelanz(anz_target_fertig) + Winkelanz(n + 1) 'nur Impulse addieren
Else
Wa(anz_target_fertig) = Temp_wa(anz_target_fertig) 'Anfang eintüten
We(anz_target_fertig) = We(n) 'Ende eintüten
Templ1 = We(anz_target_fertig) - Wa(anz_target_fertig) 'Differenz
If Templ1 < 0 Then 'auf "0"-Durchgang prüfen
Templ1 = Templ1 + 144000 'ggf. korrigieren
End If
Templ1 = Templ1 \ 2
Templ1 = Templ1 + Wa(anz_target_fertig)
If Templ1 > 144000 Then '"0"-Durchgang ?
Templ1 = Templ1 - 144000 'ggf. korrigieren
End If
Winkel(anz_target_fertig) = Templ1 'endlich geschafft !
Incr Anz_target_fertig 'nächstes Paket
Temp_wa(anz_target_fertig) = Wa(n + 1) 'nächsten Anfang merken
Winkelanz(anz_target_fertig) = Winkelanz(n + 1) 'nächste Impulsanzahl merken
End If
Next N
End If
Decr Anz_target_fertig 's.o.
If Anz_target_fertig = 3 Then '!!!!!!! NUR 3 BAKEN ERGEBEN SINNVOLLE WERTE !!!!!!!!!
W1 = Winkel(1) \ 4 'für Übertragung zum MASTER bevor evtl. ge"swap"t wird !
W2 = Winkel(2) \ 4 'eigentlich wird nur Winkel(2) gebraucht
W3 = Winkel(3) \ 4
W1_word = W1 'für 16Bit-Übermittlung (TWI)
W2_word = W2
W3_word = W3
Call Winkel_sortieren
Call Positionsberechnung(t_ab , T_bc)
Lesefehler = 0 'Gut
Sound Buzzer , 3 , 3700 'Gut
Berechnen_winkel = Gamma \ 2 'wann kann ich loslegen ?
Berechnen_winkel = Berechnen_winkel * 400
Berechnen_winkel = Berechnen_winkel + Winkel(2)
If Berechnen_winkel > 144000 Then
Berechnen_winkel = Berechnen_winkel - 144000
End If
If Berechnen_winkel > 143360 Then Berechnen_winkel = 0 'letztes Stück passt nicht ganz
Berechnen_fenster = Berechnen_winkel + 2048
Else
Ist_x_gemessen = 0 : Ist_y_gemessen = 0
Incr Lesefehler
Sound Buzzer , 3 , 700 'Schlecht
End If
X_word = Ist_x_gemessen 'für 16Bit-Übermittlung (TWI)
Y_word = Ist_y_gemessen
Anz_target_merk = Anz_target
Quali_merk = 0
For N = 1 To 50
Quali_merk = Quali_merk + Winkelanz(n) 'für die Statistik
Next N
Portb.4 = 1 'Ich habe frische Daten für MASTER (HS)
Umdrehung_beendet = 0 '"aufräumen" hat die Mutti gesagt
Anz_target = 0
For N = 1 To 50
Winkelanz(n) = 0
Winkel(n) = 0
Wa(n) = 288000
Temp_wa(n) = 0
We(n) = 0
Next N
End If
End Sub



Nach dem Ereignis "Umdrehung_beendet" (blau in der Abb. >später mehr dazu) werden diese Arrays auf "Zusammenhang" abgefragt und das Resultat wieder ab Wa(1) & We(1) abgelegt.
Also steht jetzt im Array:
Wa(1) & We(1) >1.Paket
Wa(2) & We(2) >2.Paket
Wa(3) & We(3) >3.Paket

Nun müssen wir nur noch die Mitte ausrechnen:
[ Wa+((We-Wa)/2) ] kommt nach Winkel(x) (in der Abb. rot).
Weiterhin wird die Anzahl der zusammenhängenden Pakete in der Variablen Anz_Target_fertig abgelegt. Wenn diese den Inhalt 3 hat kann weitergearbeitet werden, wenn nicht > Pech gehabt !!!
Das Array "Winkelanz(x)" beinhaltet jetzt die Anzahl der Impulse pro Paket und gestattet es Rückschlüsse auf den Abstand zur Bake und/oder die Qualität der Reflexion zu ziehen.


Fragen hierzu ?



Als nächstes ist das Problem zu lösen:
"Der Roboter dreht sich ja auch !" (Sub Winkel_sortieren "Berechnen_winkel" / "Berechnen_fenster" u.s.w.)

[vohopri]

Hallo Willi,

da gibt es anscheinend schon recht interessante Erfahrungen.

Plausibilitätskontrolle ist bei der Navi an vielen Stellen nötig, damits zuverlässig wird. Das system kann ja anhand der Karte wissen, wie viele Baken sichtbar sein müssen. Dan können die stärksten Signale weiter verrechnet werden, während die schwächeren verworfen werden.

Abhängig von der möglichen Häufigkeit der Peilungen können die Peilungen auch geprüft werden, ob sie nahe genug an der Letzten sind, oder ob eine einen Hüpfer gemacht hat.

Du wirst ja sehen, was im Einsatz dann notwendig sein wird.

[WL]

Hallo vohopri,



da gibt es anscheinend schon recht interessante Erfahrungen.



Nach ca. 2 Jahren Entwicklung bleibt das nicht aus. ;)



Dan können die stärksten Signale weiter verrechnet werden, während die schwächeren verworfen werden.



Die stärksten Signale müssen nicht unbedingt von den 3 Baken kommen (meist ist es aber so....).
Flachglas z.B. liefert nur ein kurzes Impulspaket.

Am besten sind natürlich aktive Baken. Dann wird es eindeutig und die Geometrie des Spielfeldes kann wesentlich komplexer werden (>ToDo_2012).



Abhängig von der möglichen Häufigkeit der Peilungen können die Peilungen auch geprüft werden, ob sie nahe genug an der Letzten sind, oder ob eine einen Hüpfer gemacht hat.


Im Augenblick verwende ich das ganze für eine "Flächenbearbeitung" so:

-1. Startpunkt Ladestation: Position und Winkel ermitteln
-2. Odometrie danach korrigieren
-3. nach >= 7m Strecke abfahren und Zielpunkt erreicht: Position und Winkel ermitteln (wenn nicht möglich> nächsten Zielpunkt anfahren / Fehler incr.)
-4. Odometrie korrigieren
-5. u.s.w. (3. & 4.) bis Fläche fertig
-6. Position und Winkel ermitteln dann zurück in Ladestation


Während der Fahrt macht die Messung wenig / keinen Sinn. Wenn man den Winkel zur Bake B zeitnah an das übergeordnete System melden würde könnte man zumindest Rückschlüsse auf die Ausrichtung ziehen (Ist / Soll-Vergleich).



Du wirst ja sehen, was im Einsatz dann notwendig sein wird.


Das ganze Projekt ist noch längst nicht abgeschlossen.................

[Bernhard667]

Hallo WL,
also das interessiert mich hier stark.
Ich hatte auch schon mal am Thema automatisches Rasenmähen gearbeitet. Damals wolte ich mit aktiven Ultraschallbaken die Navigation erledigen. Verlor dann aber die Lust.
Als ich mit dem Thema schon durch war, fiel mir noch die ultimative Lösung für Navigation ein, rotierender Laser. Offensichtlich hattest Du die gleiche Idee.

Die Genauigkeit scheint ja sehr gut zu sein.
Du modulierst das Lasersignal. Das scheint zu reichen, Falschlicht zu unterdrücken. Wenn Du evtl noch optisch filtern willst/musst, schauch doch mal auf meine Homepage.
Du rotierst offensichtlich einen punktförmigen Laser und mußt deshalb den Reflektor auch treffen. Wie wäre es einen Linienlaser zu rotieren. Der trifft viel leichter die Bake.
Die Bake ist 3M Folie, schreibst Du. Ist das so eine "Katzenaugen" Folie.
Ich hatte geplant, permanent zu rotieren, auch während der Fahrt. Du scheinst nur gelegentlich zu messen. Hat das einen speziellen Grund?

Bernhard

[021aet04]

Die 3M Folie ist vermutlich eine normale Reflektorfolie. Das Prinzip ist das gleiche wie bei einem Katzenauge nur das hier die Tiefe geringer ist. Das Katzenauge ist einige mm stark, die Folie ist eben eine Folie und dadurch sehr dünn. Diese hat aber auch einen Nachteil, sie reflektiert nicht so stark wie ein Katzenauge. Ich habe letzte Woche getestet ein Katzenauge (das mit dem Stapler zerstört wurde) gegen eine Folie (für Drehzahlmessgerät) zu tauschen, da ich keinen neuen Reflektor montieren wollte (der ist öfter zu tauschen). Dieser Reflektor ist für ein Rolltor. Ein Sicherheitslichtschranke damit das Tor wieder auffährt wenn der LS beim Schließen des Tores unterbrochen ist.

PS: 3M ist ein Hersteller der diese Folie u.A. herstellt.

http://de.wikipedia.org/wiki/Reflektorfolie

MfG Hannes

[WL]

Hallo Bernhard667,
schön das hier mal Leben reinkommt........


Die Genauigkeit scheint ja sehr gut zu sein.
Ich bin selbst überrascht wie gut das klappt.



Du rotierst offensichtlich einen punktförmigen Laser und mußt deshalb den Reflektor auch treffen. Wie wäre es einen Linienlaser zu rotieren. Der trifft viel leichter die Bake.


Die verwendeten Reflektoren bestehen aus einem Rohr (3cm Durchmesser / 1m hoch) mit der Diamond-Folie von 3M.
Bei nicht zu hügeligen Gelände reicht das. Außerdem ist da noch die kardanische Aufhängung...........

Ein Linienlaser verteilt seine Strahlleistung auf ein schmales Band von x Metern Länge (je nach Spreizwinkel und Abstand) . Da kommt u.U. so gut wie nichts mehr zurück.
Der Empfänger kann dann nicht mehr so simpel aufgebaut sein !



Die Bake ist 3M Folie, schreibst Du. Ist das so eine "Katzenaugen" Folie.


Ich kenne verschiedene Reflektorfolien. Diese scheint die geeignetste zu sein. Ich kenne keine "Katzenaugen" mit rundem Profil.



Ich hatte geplant, permanent zu rotieren, auch während der Fahrt. Du scheinst nur gelegentlich zu messen. Hat das einen speziellen Grund?


Rotation ist dauernd gegeben um (grob) die Winkel zu erfassen (im nächsten Beitrag näheres). Präzise Erfassung ist aber nur bei stehenden Fzg. möglich.
Eine Umdrehung dauert mindestens 1 Sekunde. Je nach Geschwindigkeit Deines Fahrzeug's verändern sich die währenddessen die Winkel zu den Baken (erheblich).
Noch schlimmer ist das bei Kurven oder rotieren des Fahrzeuges. Das wird so nichts !!!
Wie schon erwähnt wird hauptsächlich mit der Odometrie gefahren und nur ab und zu korrigiert.



@021aet04

Wie gesagt, es gibt verschiedene Folien..............
Ich verwende diese: 3M Diamond Grade(tm) Serie 983 silber

[WL]

!!! Als erstes muss ich mich dafür entschuldigen das ich in den bisherigen PDFs die Winkel Alpha und Beta vertausch habe !!!
In diesem Beitrag ist das berücksichtigt.
Kann ich das nachträglich in den entspr. Beiträgen "geraderücken" ?>>>>>>>>> EDIT 03.10.2011 ! Schon erledigt !



Der Roboter dreht sich .........



Funktion Winkelauswertung:
(Winkelauswertung.pdf)


Hier geht es darum die Winkel Alpha, Beta und Gamma sicher und eindeutig zu bestimmen. Rohkost hierfür ist das Array Winkel(1..3). Durch die Anordnung der Baken in einer Linie ist gewährleistet das unabhängig vom Standpunkt des Scanners auf dem Spielfeld der Winkel Gamma immer größer als 180° ist. Dieser Umstand hilft uns den Algorithmus einfach und effektiv zu gestalten.
In den beiden Beispielen im PDF wurden die 3 Winkel Winkel(1), Winkel(2) und Winkel(3) ab 0° in aufsteigender Reihenfolge erfasst.

Winkel(1) korrespondiert allerdings nicht immer mit Bake C u.s.w. (z.B. wenn der Roboter sich gedeht hat) !


'-------------------------------------------------------------------------------
Sub Winkel_sortieren
Pruef = Winkel(2) - Winkel(1)
If Pruef > 72000 Then
Swap Winkel(1) , Winkel(3) '0° in Beta
Swap Winkel(1) , Winkel(2)
End If

Pruef = Winkel(3) - Winkel(2)
If Pruef > 72000 Then
Swap Winkel(1) , Winkel(3) '0° in Alpha
Swap Winkel(2) , Winkel(3)
End If

Alpha = Winkel(2) - Winkel(1)
Sig = Sgn(alpha)
If Sig = -1 Then
Alpha = 144000 + Alpha
End If

Beta = Winkel(3) - Winkel(2)
Sig = Sgn(beta)
If Sig = -1 Then
Beta = 144000 + Beta
End If

Gamma = Winkel(1) - Winkel(3)
Sig = Sgn(gamma)
If Sig = -1 Then
Gamma = 144000 + Gamma
End If

Alpha = Alpha / 400 : Beta = Beta / 400 : Gamma = Gamma / 400

End Sub



Die Sub Winkel_sortieren durchsucht das Array Winkel(1..3) indirekt nach 0°-Durchgängen in den einzelnen Abständen.
Sollte dieser Umstand auftreten (z.B. in der oberen Abbildung :111360-12400=98960 und damit >72000) wird das Array "geswapt" :
Also steht jetzt im Array:
Winkel(1) =111360 / Winkel(2) =124640 / Winkel(3) =12400


Damit haben wir das ganze so "verdeht" als wenn wir hinter dem Roboter (auf der Gegengeraden) stehen würden und die Baken (gegen den Uhrzeigersinn) in der Reihenfolge C - B - A scannen.

Alpha ist also : 124640 - 111360 = 13280 / 400 = 33,2°
Beta ………… : 12400 - 124640 = -112240 + 144000 = 31760 / 400 = 79,4°
Gamma……... : 111360 - 12400 = 98960 / 400 = 247,4°

Das untere Beispiel könnt Ihr ja mal selbst rechnen !

Fragen hierzu ?


Als nächstes ist nur noch zu klären wann (besser: wo) innerhalb einer Umdrehung am sinnvollsten die Berechnung gestartet werden soll ………………………………….

[WL]

und zum Schluß .........

Winkelauswertung wann / wo :
(Winkelauswertung_wo.pdf)


Jetzt müssen wir nur noch festlegen wann die Berechnung innerhalb der Umdrehung starten soll. Wenn der Roboter in unserem Spielfeld z.B. links oben sehr nahe an der Bake A steht ist der Winkel Alpha klein. Die Baken B und C stehen also (vom Roboter aus gesehen) sehr eng zusammen. Wenn wir in diesem Bereich die Berechnung starten (dauert durch die ausgiebige Fließkomma-Bearbeitung doch ein wenig länger) geraten die Variablen (durch zwischenzeitliche Empfangssignale der Bake B) durcheinander.

Besser ist es die Berechnung zu starten wenn gerade nicht viel los ist :

Gamma ist auf dem gesamten Feld >180°. Um ein wenig "Bewegungsfreiheit" während der Fahrt zu haben (ist für die laufende Korrektur des nachfolgend beschriebenen Winkels günstig) liegt dieser auf der Hälfte von Gamma. In diesem Bereich ist normalerweise nicht mit Empfangssignalen zu rechnen.

Damit kann sich der Roboter relativ weit drehen ohne sich gleich wieder selbst zu stören.

Als erstes wird dazu eine Umdrehung in ca. 5° Segmente grob aufgeteilt (144000 / 2048 ~ 70 Segmente). Dies wird u.a. durch Int1 geleistet (vergl. Blockschaltbild/Hardware). Int1_int ist kurz gehalten und beeinflusst durch relativ große zeitliche Abstände die anderen Programmteile nur wenig. Gleichzeitig wird von Int1_int geprüft ob der Winkel "Gamma/2" innerhalb des aktuellen Segmentes liegt. Ist dies der Fall wird die Flagge "Umdrehung_beendet" gesetzt und hiermit dem Hauptprogramm die Freigabe für die endgültige Berechnung der Positionsdaten gegeben.



Int1_int: 'Auswerte- "Raster"
Berechnen_step = Berechnen_step + 2048
If Berechnen_step >= Ber_winkel_anf And Berechnen_step <= Ber_winkel_end And Umdrehung_beendet = 0 Then
Umdrehung_beendet = 1 'ab hier Auswertung starten
Incr Runde
End If
Return




Mit dem "0" -setzen des Hardwarezählers wird auch Int2 aktiviert:



Int2_int: 'HW-Zähler wird zurückgesetzt
Berechnen_step = 0
Return


……………………..und damit auch der Segmentzähler "Berechnen_step".


Der "Berechnen-Winkel" bei "Gamma/2":

Zur Verbesserung der Struktur habe ich diesen Teil (wurde weiter oben schon erwähnt) mal in eine Unterroutine gepackt:


Impuls_pakete: 'alle zusammenhängenden Impulspakete ermitteln

Anz_target_fertig = 1 'Ältere BASCOM-Versionen können nicht mit Index "0" !
If Anz_target > 1 Then
Temp_wa(1) = Wa(1) 'Anfang merken
For N = 1 To Anz_target 'Schleife über alle (Einzel-)Pakete
Diff = Wa(n + 1) - We(n) 'Differenz zwischen den Paketen
If Diff < 0 Then Diff = Diff + 144000 '"0"-Durchgang
If Diff > 0 And Diff < 700 Then '700 ist die Toleranz (~1,7°)
Winkelanz(anz_target_fertig) = Winkelanz(anz_target_fertig) + Winkelanz(n + 1) 'nur Impulse addieren
Else
Wa(anz_target_fertig) = Temp_wa(anz_target_fertig) 'Anfang eintüten
We(anz_target_fertig) = We(n) 'Ende eintüten
Templ1 = We(anz_target_fertig) - Wa(anz_target_fertig) 'Differenz
If Templ1 < 0 Then 'auf "0"-Durchgang prüfen
Templ1 = Templ1 + 144000 'ggf. korrigieren
End If
Templ1 = Templ1 \ 2 'Mitte !
Templ1 = Templ1 + Wa(anz_target_fertig)
If Templ1 > 144000 Then '"0"-Durchgang ?
Templ1 = Templ1 - 144000 'ggf. korrigieren
End If
Winkel(anz_target_fertig) = Templ1 'FERTIG !
Incr Anz_target_fertig 'nächstes Paket
Temp_wa(anz_target_fertig) = Wa(n + 1) 'nächsten Anfang merken
Winkelanz(anz_target_fertig) = Winkelanz(n + 1) 'nächste Impulsanzahl merken
End If
Next N
End If

Decr Anz_target_fertig 's.o.

Return




Die Suche nach: "Winkel(3) + Gamma /2" :

Die Routine Suche_AlBeGa wird immer dann aufgerufen wenn keine Position ermittelt werden kann weil die Anzahl Baken <> 3 ist oder der Roboter eine schnelle Drehung macht (s.o.) und zum Initialisieren. Hier wird keine Position ermittelt, sondern nur die Winkel(1..3) und damit Alpha, Beta und Gamma. Damit das ganze bei z.B. einer 4. Reflexion nicht hängenbleibt gibt es als Abbruchkriterium noch ein Timeout. Es liegt dann vorerst an der übergeordneten Instanz entsprechend zu reagieren. Z.B. könnte ein anderer Standort angefahren werden.
Möglich wäre aber auch eine Plausibilitätskontrolle und damit Ausblendung der "falschen" Bake.



Suche_AlBeGa:

Timeout = 0
Ber_winkel_anf = Berechnen_step - 2048 'Berechnen_step hat hier zufälligen Wert !
If Ber_winkel_anf < 0 Then
Ber_winkel_anf = Ber_winkel_anf + 144000
End If
Ber_winkel_end = Ber_winkel_anf + 2048 'Fenster knapp vor Berechnen_step

Umdrehung_beendet = 0 'auf ein neues...........
Anz_target = 0
Do
If Umdrehung_beendet = 1 Then

Gosub Impuls_pakete 'alle zusammenhängenden Impulspakete ermitteln

If Anz_target_fertig = 3 then

Call Winkel_sortieren 'Alpha / Beta / Gamma bestimmen

Ber_winkel_anf = Gamma \ 2
Ber_winkel_anf = Ber_winkel_anf * 400
Ber_winkel_anf = Ber_winkel_anf + Winkel(3) 'Berechnung auf der "Gegengeraden"

If Ber_winkel_anf > 144000 Then 'u.U. korrigieren
Ber_winkel_anf = Ber_winkel_anf - 144000
End If
If Ber_winkel_anf >= 141900 Then Ber_winkel_anf = 0 'letztes Segment

Ber_winkel_end = Ber_winkel_anf + 2048
End If
End If
Loop Until Anz_target_fertig = 3 Or Timeout > 2000

Umdrehung_beendet = 0
Anz_target = 0
For N = 1 To 50
Winkelanz(n) = 0
Winkel(n) = 0
Wa(n) = 288000
Temp_wa(n) = 0
We(n) = 0
Next N

If Anz_target_fertig = 3 Then
Lesefehler = 0
Turm_status.0 = 1 'Orientierung vorh.
Turm_status.1 = 0 'KEINE gültigen Daten vorh.
Else
incr Lesefehler
Turm_status.0 = 0 'Kopflos.........
Turm_status.1 = 0 'KEINE gültigen Daten vorh.
Sound Buzzer , 200 , 250
Sound Buzzer , 200 , 350
Sound Buzzer , 200 , 250
Sound Buzzer , 200 , 350
End If
Return





Als nächstes:
Das komplette Programm (TWI-Slave).

[WL]

Und nun die gesamte Slave-Software:
(Scanner_106.BAS)

Scanner (Slave) -Register :
(ScannerRegister.zip)


Im Anhang sind die Slave-Register aufgelistet. Die Register 1..9 sind zum Schreiben, die Register 10…19 zum Lesen vorgesehen.
Wichtig ist das schreiben der Parameter AB und BC (Baken-Abstand) nach einem Reset. Ohne diese Werte kommt nur X=0 und Y=0 zurück.


Der Master (Roboter) steuert den Scanner (z.B. für Initialisieren) so an:
- Register 5 (AB) = 480 schreiben 'AB=480cm
- Register 6 (BC) = 560 schreiben 'BC=560cm
- Register 1 schreiben 'Initialisieren (Motor ein / Laser ein / Orientieren)



Scanner_Initialisierung:
call Scanner_readregister(10 , 0)

if Scanner_Status.3 = 0 then
T_lob = low(ab) 'T_ab>SLAVE
T_hib = high(ab)
call Scanner_writeregister(5 , T_lob , T_hib)
T_lob = low(bc) 'T_bc>SLAVE
T_hib = high(bc)
call Scanner_writeregister(6 , T_lob , T_hib)
end if

if Turm_Status.0 = 0 then
call Scanner_writeregister(2 , 70 , 0) 'Turm Geschw.
waitms 500 ' !!! Sicherheit !!!
call Scanner_writeregister(3 , 1 , 0) 'Laser ein
call Scanner_writeregister(1 , 0 , 0) 'Initialisieren
end if
Return


Nun läuft der Scanner und liefert (hoffentlich) die gewünschten Daten.
Der Master (Roboter) bekommt die Werte nach dem Handshake (z.B. ATMega256 >PCint an PINK.7):


Isr_pcint2:
If pink.7 = 1 Then
call Scanner_readregister(10 , 0) 'Status
call Scanner_readregister(11 , 0) 'Pos. X
call Scanner_readregister(12 , 0) 'Pos. Y
call Scanner_readregister(14 , 0) 'Winkel2
call Scanner_readregister(16 , 0) 'Baken
call Scanner_readregister(17 , 0) 'Impulse

Winkel_gemessen_ Scanner = Winkel2 / 100
Pcifr.pcif2 = 1
End If
Return


Der Master (Roboter) steuert den Scanner (z.B. für AUS) so an:


Scanner _ausschalten:
Timeout = 0
do
call Scanner _writeregister(3 , 0 , 0) ' Scanner -Laser AUS
call Scanner _writeregister(2 , 0 , 0) ' Scanner -Motor AUS
call Scanner _readregister(10 , 0) ' Status
if Timeout > 60 then
Print "TIMEOUT Scanner ausschalten !!!"
Print " Scanner _status=" ; Scanner _status
exit do
end if
loop until Scanner _status.5 = 0 and Scanner _status.4 = 0
Return


….und wieder EIN:



Scanner _einschalten:
Timeout = 0
do
call Scanner_writeregister(2 , 70 , 0) ' Scanner -Motor EIN
waitms 500 ' !!! Sicherheit !!!
call Turm_writeregister(3 , 1 , 0) ' Scanner -Laser EIN
call Turm_readregister(10 , 0)
if Timeout > 60 then
Print "TIMEOUT Scanner einschalten !!!"
Print " Scanner_status=" ; Scanner_status
exit do
end if
loop until Scanner_status.5 = 1 and Scanner_status.4 = 1
Return


Der TWI-Code:



Sub Scanner_writeregister(byval Befehl as Byte , byval Wert_L as Byte , byval Wert_H as Byte )
I2cstart
I2cwbyte Scanner_slaveid 'D0
I2cwbyte Befehl '1….9
I2cwbyte Wert_L
I2cwbyte Wert_H
I2cstop
End Sub




Sub Scanner_readregister(byval ScannerRegister as Byte , byval ScannerWert as Byte)
I2cstart
I2cwbyte Scanner_slavewrite 'D0
I2cwbyte ScannerRegister '10……19
I2cwbyte ScannerWert 'für künftige Erweiterungen
I2cstop
I2cstart
I2cwbyte Scanner_slaveread 'D1
I2crbyte Hib , Ack
I2crbyte Lob , Nack
I2cstop
Select case ScannerRegister
case 10
Scanner_Status = Makeint(lob , Hib)
case 11
X_gemessen = Makeint(lob , Hib)
case 12
Y_gemessen = Makeint(lob , Hib)
case 13
Winkel1 = Makeint(lob , Hib)
case 14
Winkel2 = Makeint(lob , Hib)
case 15
Winkel3 = Makeint(lob , Hib)
case 16
Baken = Makeint(lob , Hib)
case 17
Baken_Impulse = Makeint(lob , Hib)
end select
End Sub


Ich hoffe, ich habe nichts vergessen...............:confused:
Damit ist das Projekt (vorerst) abgeschlossen und ich wünsche Euch viel Erfolg damit. :):):)

[AsuroPhilip]

Coole Sache ;)

Hast/Machst du ein Video?

mfG
Philip ;->

[WL]

Hast/Machst du ein Video?



Ich werde es mal versuchen...........

[thewulf00]

Noch ein Tipp zur Plausibilitätskontrolle der Baken als Reflektoren: Wenn es zuviele Reflektoren gibt, dann würde ich sukzessive einfach von der minimal möglichen Sendeleistung ansteigend solange drehen, bis er drei Baken sieht. Sobald man drei hat, wird diese Leistung beibehalten. Wenn man wieder eine verliert, wird wieder gesteigert; wenn er eine Quelle zuviel sieht, wieder bei null anfangen.
Dieser Algorithmus lässt viel Spielraum für Optimierungen und ist nur austricksbar, indem man Fremdreflektoren in der Nähe oder innerhalb des Spielfeldes platziert.

[WL]

Das wäre durchaus möglich. Allerdings gibt es ein Problem:
Es gibt keine Verbindung vom µC (nur Royerconverter) zum Sender. Nur Ein/Aus !
Es müsste eine neue Konstruktion her............

[thewulf00]

Tja, das wars dann :-)
Wie wärs mit einem dimmbaren Dunkelfilter vor dem Laser?

[WL]

Dimmbarer Dunkelfilter ???
:confused: Was das alles gibt !?
Aber auch dafür gibt es (momentan) keinen Rückkanal.
Außerdem reduziert das wohl auch die Empfangssignale der Baken.

Besser wird es sein den Ort (ermittelten Winkel) der Fremdreflexion auszufiltern. Muss ich mal drüber nachdenken............
Optimal wird das ganze aber mit aktiven Baken.

Über den Winter will ich jetzt allerdings mit der "Indoor"-Navigation weitermachen (Wi-Remote IR-Sensor).

[thewulf00]

Jop so ist es. Deshalb hatten wir bei unsrem Bot die Baken aktiv gemacht, und der Bot hatte bloß einen Empfänger.

[WL]

Coole Sache ;)

Hast/Machst du ein Video?

mfG
Philip ;->

Dein Wunsch war mir Befehl ! ;)
Hat allerdings ein wenig länger gedauert weil ich damit nicht so vertraut war (bin).

In dieser Version habe ich die Erfassung über 3 Umdrehungen gemittelt.
Das Teminal-Fenster zeigt die Roh-Daten im Scanner und ist leider nicht ganz synchron.

Dieses Fahrwerk soll dann mal den Rasen mähen.
Wie man vielleicht erkennen kann > NICHT in ungeordneten Bahnen (Zufallsprinzip).

Und nun viel Spaß beim "Fernsehen" !


http://www.youtube.com/watch?v=Mr8vJQKbPYw

[Bernhard667]

Respekt,

das sieht echt gut aus

Bernhard

[WL]

Respekt,

das sieht echt gut aus

Bernhard

Danke.
Habe das Projekt dieses Jahr noch nicht weiter verfolgt:( (so viel anderes:)).
Es steht noch der Umbau auf aktive Baken aus.

Hat von Euch jemand davon etwas verwenden können ?