mbot2 Linienverfolger und Stoppen bei rot

[haiflosse]

Hallo!
Ich habe mit python (mblock) einen code für den mbot2 erstellt, wo anhand der rgb Sensoren der mbot2 einer Schwarzen Linie folgen soll und bei rot er kurz stehen bleiben soll. Ich habe dazu den beigelegten Plan verwendet wo in der schwarzen Linie Markierungen von rot, gelb, grün, blau vorhanden sind.
Wenn ich nur das Auslesen der Farben verwende, werden die Farben yellow, green, blue, red recht gut erkannt, aber wenn ich dies im Linienverfolger einbaue, wird rot kaum erkannt, bzw. recht spät oder es passiert oft dass rot auch erkannt wird, wenn er auf der schwarzen Linie fährt.

Hier mein Code für den Linienverfolger

import cyberpi, mbot2, mbuild, time
float_m1_start = 20
float_m2_start = 20
int_sollwert = 50
float_p = 0.5
while True:
    str_s1 = mbuild.quad_rgb_sensor.get_color_sta(1)
    str_s2 = mbuild.quad_rgb_sensor.get_color_sta(2)
    str_s3 = mbuild.quad_rgb_sensor.get_color_sta(3)
    str_s4 = mbuild.quad_rgb_sensor.get_color_sta(4)
    if str_s1 == "red" or str_s2 == "red" or str_s3 == "red" or str_s4=="red":
        mbot2.EM_stop()
        time.sleep(2)
        mbot2.EM_set_speed(20, 1)
        mbot2.EM_set_speed(-20, 2)
        time.sleep(0.5)
    else:
        int_s2 = mbuild.quad_rgb_sensor.get_gray(2)
        int_fehler = int_sollwert - int_s2
        float_m1 = float_m1_start + int_fehler * float_p
        float_m2 = float_m2_start - int_fehler * float_p
        mbot2.EM_set_speed(float_m1,1)
        mbot2.EM_set_speed(-float_m2,2)

Hier mein Code nur mit rgb der recht gut funktioniert

import cyberpi, mbuild
while True:
    str_s1 = mbuild.quad_rgb_sensor.get_color_sta(1)
    str_s2 = mbuild.quad_rgb_sensor.get_color_sta(2)
    str_s3 = mbuild.quad_rgb_sensor.get_color_sta(3)
    str_s4 = mbuild.quad_rgb_sensor.get_color_sta(4)
    str_ausgabe = "Farbwerte\n\n"
    str_ausgabe = str_ausgabe + "S1: " + str_s1 + "\n"
    str_ausgabe = str_ausgabe + "S2: " + str_s2 + "\n"
    str_ausgabe = str_ausgabe + "S3: " + str_s3 + "\n"
    str_ausgabe = str_ausgabe + "S4: " + str_s4 + "\n"
    cyberpi.display.show_label(str_ausgabe,16,0,0)

Vielleicht kann mir jemand weiterhelfen.
Danke

 

[Cool Master]

wird rot kaum erkannt, bzw. recht spät oder es passiert oft dass rot auch erkannt wird, wenn er auf der schwarzen Linie fährt.

Hast du mal probiert die Geschwindigkeit runter zu setzen? Des Weiteren gibt es keine Funktion wir get_color_red oder ähnlich? Damit könnte man in der Theorie ein HEX Wert vorgeben und rot berechnen lassen das hilft ggf. auch etwas bei der rot/schwarz Erkennung.

 

[Bunkeropfer]

bin zwar kein mbot-Kenner, aber habe an der Schule bei mir die Lego Spike Plattform am Laufen welche sich vermutlich ähnlich verhält. Gerade beim Linienfolgen und weiteren "Farbaktionen" gibt es oft Probleme bzw dass eine Seite nicht so läuft wie gedacht. Erfahrungen hier nur aus der Lego-Welt und hoffentlich übertragbar

• Versuche einmal, das Linienfolgen und das Roterkennen zu trennen, also S2 = Linie, S1,3,4=ROT. dann ist die Gefahr für ein ungewolltes Rot an der Linienkante erst einmal weg
• wie zuvor schon geschrieben wurde, mit den RGB-Werten arbeiten und sich selber das ROT bzw. GRAU berechnen. Dann kann man für ROT einen passenden Threshold setzen (und mit dem komplementären Grünwert absichern). Also R~G ==> es muss ein Grau sein, R>>G ==> ist wohl Rot.
• wie verhält sich der mbot, wenn er nicht bei Rot halten soll, sondern zB bei Gelb/Grün/Blau? Evtl. spielt hier auch die Beleuchtung mit eine Rolle?
• ist das Rot der Matte auch das Rot, auf welches der Sensor ideal reagiert? (z.B. gerade bei Gelb sind die menschlichen Farbeindrücke und gemessenen RGB-Werte sehr unterschiedlich, evtl. auch hier). Kann man die Farbwerte kalibrieren?
• bei (alten) Legosensoren (EV3-Basis) machte es einen Unterschied, ob man Farbwerte oder Grauwerte ausgeben lässt - der Sensor musste intern umschalten und hat damit eine Totzeit, in welcher nichts gemessen wird. kA ob/wie das hier ist
• sehe gerade diese mgl. Matte. Können denn da alle Sensoren jemals auf die rote Farbstrecke kommen oder nur max. zwei? Und wenn der Sensor nur zur Hälfte auf der Matte ist (was ja das Ziel des Linienfolgers ist), gibt es dann ein Rot oder doch eher weiß? --> Sensorausgaben prüfen und nur die triggern lassen, die überhaupt die Chance dazu haben
• die Rot-Prüfung und den Linienfolger nicht innerhalb derselben While-Schleife mit IF prüfen. Eher etwas der Form "While not RED...Linienfolger". Entsprechend dann eine gemeinsame Runloop drumherum machen. (liegt aber mglw. auch daran, dass ich den Linienfolger als festes Modul habe, welches ich nur de/aktivieren lasse und nicht immer im Code haben will)

Soweit die ersten spontanen Ideen. Hilft hoffentlich auf die ein oder andere Weise

 

[qualle]

Hi @haiflosse ...

Zuerst ein wenig Theorie. Der "cyberpi.quad_rgb_sensor" besitzt bereits eine vorprogrammierte Linienerkennung. Du brauchst dich also nicht selber um die einzelnen RGB-Werte kümmern...

Funktionsweise des Quad-RGB-Sensors verstehen

Der Sensor besitzt vier nebeneinander liegende Messpunkte (L2, L1, R1, R2). Wenn Sie den Sensor auf den Erkennungsmodus „Linie“ einstellen, gibt der Sensor einen Statuswert zwischen 0 und 15 zurück. Dieser Wert ist die Summe der Binärwerte aller vier Sensoren (wobei eine erkannte schwarze Linie als 1 gewertet wird):

• Wert 0: Keine Linie erkannt (Roboter hat die Linie komplett verloren).
• Wert 6: Die beiden mittleren Sensoren (L1 und R1) erkennen schwarz. Der Roboter ist perfekt zentriert.
• Wert kleiner als 4: Die Linie gerät nach rechts (der Roboter muss nach rechts korrigieren).
• Wert ist ein Vielfaches von 4 (z. B. 4, 8, 12): Die Linie gerät nach links (der Roboter muss nach links korrigieren).

Und jetzt, wie man das mit CyberPi, mBot2 und Python umsetzen könnte:

import cyberpi
import time

# Sensor initialisieren und auf Linienfolge-Modus einstellen
cyberpi.quad_rgb_sensor_1.set_line_track_mode()

# Basisgeschwindigkeit festlegen
base_speed = 30

while True:
    # Holt die relative Position zur Linie (-100 bis 100)
    # 0 bedeutet: Der Roboter steht perfekt zentriert auf der Linie
    offset = cyberpi.quad_rgb_sensor_1.get_line_track_offset()
    
    # Prüfen, ob die Linie komplett verloren wurde
    if cyberpi.quad_rgb_sensor_1.get_line_status() == 0:
        cyberpi.mbot2.EM_stop() # Sicherungsstopp
        continue

    # Ein einfacher proportionaler Faktor (P-Regler) für weichere Lenkung
    # Erhöhen Sie den Wert (z.B. 0.4), falls er zu träge reagiert
    correction = offset * 0.3
    
    # Berechnung der Geschwindigkeit für den linken und rechten Motor
    left_motor_speed = base_speed + correction
    right_motor_speed = base_speed - correction
    
    # Motoren mit den berechneten Werten ansteuern
    cyberpi.mbot2.drive_motor(left_motor_speed, right_motor_speed)
    
    # Ganz kurze Pause, um den Prozessor nicht zu überlasten
    time.sleep(0.01)

Oder auch in Block- (Tree-) Form (das ist der neuste Shice):

Wiederhole fortlaufend:
├── Wenn [Quad-RGB-Sensor [1] Linienstatus] = [6] dann:
│     └── [Chassis] fährt vorwärts mit [40] % Geschwindigkeit
│
├── Sonst wenn [Quad-RGB-Sensor [1] Linienstatus] < [4] dann:
│     └── [Chassis] steuert nach [rechts] mit [30] % Geschwindigkeit
│
├── Sonst wenn ([Quad-RGB-Sensor [1] Linienstatus] mod) = [0] dann:
│     └── [Chassis] steuert nach [links] mit [30] % Geschwindigkeit
│
└── Sonst:
      └── [Chassis] stoppt die Bewegung

Leider (oder verständlicherweise) kann ich das in der Web-IDE (https://ide.mblock.cc/) weder testen noch simulieren (also ist lediglich ein "Dry/Blind Run" möglich...)

Du müsstest das Programm also einmal auf den physischen Roboter übertragen und ausprobieren, ob tatsächlich genau einer schwarzen Linie gefolgt wird.

Falls du die Linienerkennung aber doch "manuell" programmieren möchtest, melde dich hier noch mal - dann rechnen wir mal Farben um.

Ergänzung (Freitag um 00:55)

Mir ist gerade noch etwas aufgefallen. Womöglich bräuchtest du zusätzlich noch eine Entprellung (Debounce). Das erkläre ich am einfachsten an einem Beispiel:

Wenn dein Roboter in einer Links- oder Rechtsdrehung war und jetzt die genaue Linie wiedergefunden hat, dann muss er leicht nach rechts oder links gegensteuern, um so wieder in einer exakten Ausrichtung zur Linie zu sein.

Andernfalls könnte er die Linie sofort wieder verlassen (bzw. überfahren) und müsste seine Ausrichtung unnötig oft korrigieren. Es entstünde ein Zickzackkurs (statt einer weichen Kurve).

Das Debounce erreichst du, indem du dir den letzten Rotationswert merkst, mal -0.5 rechnest, und anschließend kurzzeitig in die entsprechende Richtung rotierst.

 

[qualle]

Rückmeldung?

Wenn du das doch lieber manuell machen möchtest, die Ähnlichkeit zu Schwarz kann über die euklidische Distanz berechnet werden, der max. Abstand wäre demnach sqrt(r²+g²+b²) (wobei r=b=g=255) ~= 441.67:

import math

def similarity_to_black(r: int, g: int, b: int) -> float:
    """
    Berechnet die Ähnlichkeit einer RGB-Farbe zu Schwarz (0, 0, 0).
    
    Gibt einen Wert zwischen 0.0 und 100.0 Prozent zurück (both incl.)
    """
    
    if not all(0 <= c <= 255 for c in (r, g, b)):
        raise ValueError("Die RGB-Werte müssen zwischen 0 und 255 liegen.")
    
    max_distance = math.sqrt(255**2 + 255**2 + 255**2)
    
    current_distance = math.sqrt(r**2 + g**2 + b**2)
    
    similarity = (1 - (current_distance / max_distance)) * 100
    
    return round(similarity, 2)

# Beispiele:
print(f"Schwarz (0, 0, 0): {similarity_to_black(0, 0, 0)}%")          # 100.0%
print(f"Weiß (255, 255, 255): {similarity_to_black(255, 255, 255)}%") # 0.0%
print(f"Dunkelgrau (50, 50, 50): {similarity_to_black(50, 50, 50)}%") # 80.39%
print(f"Reines Rot (255, 0, 0): {similarity_to_black(255, 0, 0)}%")   # 42.26%

Ein anderer Ansatz wäre, zunächst in das HSV-Farbraummodell zu konvertieren, da der V-Wert die Helligkeit bereits direkt beschreibt.

 

[qualle]

Hatte etwas Zeit... Ohne die integrierte Linienerkennung (des CyberPi mC) müsste man die Schwarz-Intensitätswerte der vier Sensoren nehmen und geschickt miteinander verrechnen:

def deviation_degrees_from_black_line(
    r2: float, r1: float, l1: float, l2: float
) -> float:
    if not all(0 <= p <= 100 for p in (r2, r1, l1, l2)):
        raise ValueError("Die Wert müssen zwischen 0.0 und 100.0 Prozent liegen.")
    return round(r2 * -0.175 + r1 * -0.0875 + l1 * 0.0875 + l2 * 0.175, 1)

Man erhält einen Wert zwischen ungefähr -25 und +25. 0 bedeutet dabei, man steht genau auf der schwarzen Linie. Ein Wert kleiner 0 bedeutet, die Linie ist ungefähr so viel Grad links von mir, und ein Wert größer 0, die Linie ist ungefähr so viel Grad rechts von mir.

Da ich ja so einen Roboter nicht habe, habe ich mir zum Testen eine kleine Simulation geschrieben:

import math


def similarity_to_black(r: int, g: int, b: int) -> float:
    """
    Berechnet die Ähnlichkeit einer RGB-Farbe zu Schwarz (0, 0, 0).

    Gibt einen Wert zwischen 0.0 und 100.0 Prozent zurück (both incl.)
    """

    if not all(0 <= c <= 255 for c in (r, g, b)):
        raise ValueError("Die RGB-Werte müssen zwischen 0 und 255 liegen.")

    max_distance = math.sqrt(255**2 + 255**2 + 255**2)

    current_distance = math.sqrt(r**2 + g**2 + b**2)

    similarity = (1 - (current_distance / max_distance)) * 100

    return round(similarity, 2)


def deviation_degrees_from_black_line(
    r2: float, r1: float, l1: float, l2: float
) -> float:
    if not all(0 <= p <= 100 for p in (r2, r1, l1, l2)):
        raise ValueError("Die Wert müssen zwischen 0.0 und 100.0 Prozent liegen.")
    return round(r2 * -0.175 + r1 * -0.0875 + l1 * 0.0875 + l2 * 0.175, 1)


def inbetween(minv, val, maxv):
    return min(maxv, max(minv, val))


def get_sensor_value(i: float, step: float, treshold: float) -> float:
    max = 255.0 / treshold
    if i <= treshold:
        return 255.0 - round(inbetween(0.0, max * i, 255.0), 1)
    return round(inbetween(0.0, max * (i - treshold), 255.0), 1)


def simulate():
    n = 30
    step = 15
    treshold = 7.5

    def get_black_similarity(sensor_index: int) -> float:
        val = get_sensor_value(sensor_index, step, treshold)
        return val, similarity_to_black(val, val, val)

    for i in range(0, n + 1):
        d = round(i * (100 / n) - 50, 1)

        v1, r2 = get_black_similarity(i - 20)
        v2, r1 = get_black_similarity(i - 10)
        v3, l1 = get_black_similarity(i - 5)
        v4, l2 = get_black_similarity(i)

        deviatation = deviation_degrees_from_black_line(r2, r1, l1, l2)

        print(
            f"{i:>2} {d:>5.1f} -> "
            f"{v4:>5.1f} {v3:>5.1f} {v2:>5.1f} {v1:>5.1f} -> "
            f"{l2:>5.1f} {l1:>5.1f} {r1:>5.1f} {r2:>5.1f} -> "
            f"{deviatation:>5.1f}"
        )


# Simulation:
simulate()

Die Idee ist folgende: Der virtuelle Roboter steht nahe einer schwarzen Linie und dreht sich einmal gegen den Uhrzeigersinn um 90 Grad von rechts nach links. Dabei steht er etwa nach 45 Grad genau auf der Linie und verlässt sie danach wieder. Es entsteht eine interessante Tabelle, aus der die Richtungswerte abgelesen werden können:

 0 -50.0 -> 255.0 255.0 255.0 255.0 ->   0.0   0.0   0.0   0.0 ->   0.0
 1 -46.7 -> 221.0 255.0 255.0 255.0 ->  13.3   0.0   0.0   0.0 ->   2.3
 2 -43.3 -> 187.0 255.0 255.0 255.0 ->  26.7   0.0   0.0   0.0 ->   4.7
 3 -40.0 -> 153.0 255.0 255.0 255.0 ->  40.0   0.0   0.0   0.0 ->   7.0
 4 -36.7 -> 119.0 255.0 255.0 255.0 ->  53.3   0.0   0.0   0.0 ->   9.3
 5 -33.3 ->  85.0 255.0 255.0 255.0 ->  66.7   0.0   0.0   0.0 ->  11.7
 6 -30.0 ->  51.0 221.0 255.0 255.0 ->  80.0  13.3   0.0   0.0 ->  15.2
 7 -26.7 ->  17.0 187.0 255.0 255.0 ->  93.3  26.7   0.0   0.0 ->  18.7
 8 -23.3 ->  17.0 153.0 255.0 255.0 ->  93.3  40.0   0.0   0.0 ->  19.8
 9 -20.0 ->  51.0 119.0 255.0 255.0 ->  80.0  53.3   0.0   0.0 ->  18.7
10 -16.7 ->  85.0  85.0 255.0 255.0 ->  66.7  66.7   0.0   0.0 ->  17.5
11 -13.3 -> 119.0  51.0 221.0 255.0 ->  53.3  80.0  13.3   0.0 ->  15.2
12 -10.0 -> 153.0  17.0 187.0 255.0 ->  40.0  93.3  26.7   0.0 ->  12.8
13  -6.7 -> 187.0  17.0 153.0 255.0 ->  26.7  93.3  40.0   0.0 ->   9.3
14  -3.3 -> 221.0  51.0 119.0 255.0 ->  13.3  80.0  53.3   0.0 ->   4.7
15   0.0 -> 255.0  85.0  85.0 255.0 ->   0.0  66.7  66.7   0.0 ->   0.0
16   3.3 -> 255.0 119.0  51.0 255.0 ->   0.0  53.3  80.0   0.0 ->  -2.3
17   6.7 -> 255.0 153.0  17.0 255.0 ->   0.0  40.0  93.3   0.0 ->  -4.7
18  10.0 -> 255.0 187.0  17.0 255.0 ->   0.0  26.7  93.3   0.0 ->  -5.8
19  13.3 -> 255.0 221.0  51.0 255.0 ->   0.0  13.3  80.0   0.0 ->  -5.8
20  16.7 -> 255.0 255.0  85.0 255.0 ->   0.0   0.0  66.7   0.0 ->  -5.8
21  20.0 -> 255.0 255.0 119.0 221.0 ->   0.0   0.0  53.3  13.3 ->  -7.0
22  23.3 -> 255.0 255.0 153.0 187.0 ->   0.0   0.0  40.0  26.7 ->  -8.2
23  26.7 -> 255.0 255.0 187.0 153.0 ->   0.0   0.0  26.7  40.0 ->  -9.3
24  30.0 -> 255.0 255.0 221.0 119.0 ->   0.0   0.0  13.3  53.3 -> -10.5
25  33.3 -> 255.0 255.0 255.0  85.0 ->   0.0   0.0   0.0  66.7 -> -11.7
26  36.7 -> 255.0 255.0 255.0  51.0 ->   0.0   0.0   0.0  80.0 -> -14.0
27  40.0 -> 255.0 255.0 255.0  17.0 ->   0.0   0.0   0.0  93.3 -> -16.3
28  43.3 -> 255.0 255.0 255.0  17.0 ->   0.0   0.0   0.0  93.3 -> -16.3
29  46.7 -> 255.0 255.0 255.0  51.0 ->   0.0   0.0   0.0  80.0 -> -14.0
30  50.0 -> 255.0 255.0 255.0  85.0 ->   0.0   0.0   0.0  66.7 -> -11.7

Natürlich musste ich dabei ein paar Annahmen treffen. Die genaue Wertkalibrierung geht nur anhand der Sensoranordnung des echten Roboters.

 

[qualle]

Man erhält einen Wert zwischen ungefähr -25 und +25. 0 bedeutet dabei, man steht genau auf der schwarzen Linie. Ein Wert kleiner 0 bedeutet, die Linie ist ungefähr so viel Grad links von mir, und ein Wert größer 0, die Linie ist ungefähr so viel Grad rechts von mir.

Sorry verschrieben, - bedeutet rechts und + bedeutet links. Das müsste eigentlich noch umgedreht werden.

Ende des Monologs.