Liste der Anhänge anzeigen (Anzahl: 2)
Laser-Positionsscanner
Hallo Kreative,
wie bereits hier https://www.roboternetz.de/community...immung-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...-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
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
Code:
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
Code:
K1winkel2 = Beta * 2 : K2winkel2 = Alpha * 2 'Thaleskreis !
Thales_4.pdf
Code:
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
Code:
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 :
Code:
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) ..............
Liste der Anhänge anzeigen (Anzahl: 2)
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) :
Anhang 20015
Fragen hierzu?
Als nächstes dann die konkrete Schaltung.............
\\:D/
Liste der Anhänge anzeigen (Anzahl: 5)
Die Lichtschranke:
Anhang 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.
Anhang 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.
Anhang 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°).
Anhang 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.
Liste der Anhänge anzeigen (Anzahl: 1)
....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........................
Liste der Anhänge anzeigen (Anzahl: 1)
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.).
Code:
'--------------------------------------------------------------------------
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
Code:
'-------------------------------------------------------------------------------
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.)
