Im folgenden Python Skript wird gezeigt, wie Berechnungen für die Navigation eines Roboters entlang einer schwarzen Linie bereits am OpenMV Kameraboard durchgeführt und dann im Programm des KeplerOpenBOT Roboters weiterverarbeitet werden können.
Dazu werden in drei ROIs die Mittelpunkte von blobs gesucht, welche den Verlauf einer schwarzen Linie im Sichtbereich der OpenMV Cam beschreiben. Aus diesen wird mit unterschiedlicher Gewichtung der gefundenen blobs ein Abweichungswinkel berechnet, welcher beschreibt, wie weit der Roboter von der idealen Mittellinie entfernt ist. Dieser Winkel wird vom Programm des KeplerOpenBOT eingelesen und kann in der Folge verwendet werden, um die Geschwindigkeiten der beiden Motoren dahingehend zu verändern, dass der Roboter in Richtung der Linie zurückkorrigiert bzw. dieser letztendlich folgt.
Python Skript zur Berechnung des Abweichungswinkels
import pyb, sensor, image, time, math# ******************** SPI SEND INTERUPT ********************spi = pyb.SPI(2, pyb.SPI.SLAVE, polarity=0, phase=0)led_red = pyb.LED(1)led_green = pyb.LED(2)spi_list = [250, 0, 0, 0, 0, 0, 0, 0]spi_data = bytearray(spi_list)def nss_callback(line):global spi, spi_datatry:spi.send(spi_data, timeout=1000)led_green.on()led_red.off()except OSError as err:led_green.off()led_red.on()passpyb.ExtInt(pyb.Pin("P3"), pyb.ExtInt.IRQ_FALLING, pyb.Pin.PULL_UP, nss_callback)# ******************** IMAGE DETECTION ********************GRAYSCALE_THRESHOLD = [(0, 64)]ROIS = [(0, 100, 160, 20, 0.7),(0, 50, 160, 20, 0.3),(0, 0, 160, 20, 0.1)]weight_sum = 0for r in ROIS: weight_sum += r[4]sensor.reset()sensor.set_pixformat(sensor.GRAYSCALE)sensor.set_framesize(sensor.QQVGA)sensor.set_vflip(True)sensor.set_hmirror(True)sensor.skip_frames(time = 2000)sensor.set_auto_gain(False)sensor.set_auto_whitebal(False)clock = time.clock()while(True):# ***** IMAGE DETECTION CODE *****clock.tick()img = sensor.snapshot()centroid_sum = 0i = 2for r in ROIS:blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True)if blobs:largest_blob = max(blobs, key=lambda b: b.pixels())img.draw_rectangle(largest_blob.rect())img.draw_cross(largest_blob.cx(), largest_blob.cy())centroid_sum += largest_blob.cx() * r[4]spi_list[i] = largest_blob.cx()i = i + 1center_pos = (centroid_sum / weight_sum) # Determine center of line.deflection_angle = 0deflection_angle = -math.atan((center_pos-80)/60)deflection_angle = math.degrees(deflection_angle)# ***** SET and SEND VALUES over SPI to ARDUINO *****spi_list[1]=int(deflection_angle)+100spi_data = bytearray(spi_list)# print("Turn Angle: %f" % deflection_angle)# print("FPS: " + str(clock.fps()))# print(spi_list)
Erklärungen zu diesem Skript
In der Folge werden nur noch die Code-Bereiche erklärt, die in Bezug auf die vorangegangenen Code-Beispiele neu hinzukommen!
Zeile 26-30: ROIS = [ ... ]
Hier werden zusätzlich zu den Koorinaten-Angaben der ROIs auch Gewichtungen mitangegeben. Diese werden bei den Berechnungen des Abweichungswinkel herangezogen, um die Abweichung zu den gefundenen Mittelpunkten der schwarzen Linie in den betrachteten ROIs unterschiedlich stark einfließen zu lassen. Der am nächsten liegende Bereich soll mit einem Faktor 0.7, der weitest entfernte Bereich mit einem Fakto 0.1 berücksichtigt werden.
Zeile 34: for r in ROIS: weight_sum += r[4]
Die Summe aller Gewichtungen wird in der Variable weight_sum abgelegt. Somit lässt sich sehr einfach mit unterschiedlichen Gewichtungen experimenteren, in dem man diese in Zeile 26-30 ROIS=[...] ändert und sonst keine Änderungen im weiteren Code vorgenommen werden müssen.
Zeile 58: centroid_sum += largest_blob.cx() * r[4]
Für die Navigation eines Roboters wird mit einer Art Schwerpunktsberechnung die x-Koordinate des Punkts auf der Linie berechnet, zu der Roboter basierend auf den aktuellen Bilddaten hinsteuern sollte um optimal auf der Linie zu fahren. Dazu wird zunächst die x-Koordinate des Mittelpunkts eines gefundenen blobs in einer ROI mit der jeweiligen Gewichtung multipliziert und diese in der Folge innerhalb der for-Schleife aufsummiert.
Zeile 59: spi_list[i] = largest_blob.cx()
Die x-Koordinaten der Mittelpunkte der gefundenen blobs werden an die Positionen 2, 3 und 4 in die Liste von Werten geschrieben, die für die Datenübertragung zu einem KeplerOpenBOT verwendet wird.
Zeile 61: center_pos = (centroid_sum / weight_sum)
In dieser Zeile wird die x-Koordinate für den Mittelpunkt der schwarzen Linie berechnet.
Zeile 63: deflection_angle = -math.atan((center_pos-80)/60)
Nun wird mithilfe einer Winkelfunktion der Abweichungswinkel berechnet und in der Variable deflection_angle abgelegt. Die Winkelangabe ist im Gradmaß Radiant.
Zeile 64: deflection_angle = math.degrees(deflection_angle)
Die Winkelangabe in Radiant wird in einen Wert im üblichen Gradmaß zwischen 0 und 360° umgerechnet.
Zeile 67: spi_list[1]=int(deflection_angle)+100
Der Wert deflection_angle wird in die Liste mit den Werten für die Übertragung zu einem KeplerOpenBOT Roboter an die Stelle mit dem Index 1 geschrieben. Da diese Werte auch negative Zahlen annehmen können, wird für die Übertragung dieses Werts die Zahl 100 addiert. Damit ist sicher gestellt, dass der Wert bei der Übertragung sicher positiv ist, da nur positive Zahlen zwischen 0 und 254 übertragen werden können. Nach der Übertragung muss dann wieder 100 abgezogen werden, um den tatsächlichen Winkelwert zu erhalten.
Arduino Sketch zum Anzeigen des Abweichungswinkels und der Mittelpunkte der gefundenen blobs
#include "KeplerBRAIN_V4.h"uint8_t value_1;uint8_t value_2;uint8_t value_3;uint8_t value_4;uint8_t value_5;uint8_t value_6;uint8_t value_7;void setup(){KEPLERBRAIN_INIT();WRITE_LCD_TEXT(1,1," ");WRITE_LCD_TEXT(1,2," ");}void loop(){// read 8 Bytes from OpenMV BEGINdigitalWrite(SPICAM, LOW);delay(1);if(spi_cam.transfer(1) == 250){value_1 = spi_cam.transfer(0);value_2 = spi_cam.transfer(0);value_3 = spi_cam.transfer(0);value_4 = spi_cam.transfer(0);value_5 = spi_cam.transfer(0);value_6 = spi_cam.transfer(0);value_7 = spi_cam.transfer(0);}digitalWrite(SPICAM, HIGH);// read 8 Bytes from OpenMV ENDint angle = value_1 - 100;WRITE_LCD_TEXT(1, 1, "Angle: " + String(angle) + " ");WRITE_LCD_TEXT(1, 2, "x1: " + String(value_2) + " x2: " + String(value_3) + " x3: " + String(value_4));}
Erklärungen zu diesem Programmcode
Zeile 40: int angle = value1 - 100;
Da zum tatsächlichen Winkelwert vor der Übertragung die Zahl 100 dazugezählt wurde um eine positive Zahl zu erhalten, muss nach dem Empfangen 100 abgezogen werden, um den ursprünglichen Wert für den Abweichungswinkel mit dem entsprechenden Vorzeichen zu erhalten.
Zeile 42,43: WRITE_LCD_TEXT(1,1,"Angle:" ...);
Auf dem Display werden der Wert des Abweichungswinkels, wie auch die x-Koordinaten der Mittelpunkte der gefundenen blobs angezeigt. Die Leerzeichen dahinter bewirken, das etwaige Ziffern der vorigen Ausgabe gelöscht werden.