Na projekt składały się komponenty użyte przez Koło Naukowe Robotyków PW w dronie wystawionym na zawody Droniada Challenge 2024. Są to: sterowanie chwytakiem, sterowanie ruchem kamery za pomocą serwomotoru oraz algorytm na bazie biblioteki OpenCV użyty do analizy obrazu na potrzeby zawodów.
1. Płytka Raspberry Pi 4 Model B
2. USB typu C
3. Serwomotor
4. Mostek H
5. Silnik DC + zasilanie
6. Przewody do połączenia komponentów
7.Model uchwytu na kamerę
Sterowanie kamerą:
Sterowanie kamerą odbywa się za pomocą serwomotora, który umożliwia obrót o jednym stopniu swobody. W ramach projektu wykonano także uchwyt na kamerę. Napisano również komendy (które mogą być później użyte na dalszym etapie prac nad np. autonomicznym pozycjonowaniem kamery) umożliwiające sterowanie ruchem serwomotora. Do komend można łatwo odnosić się przez wywołanie klasy, gdyż ten jak i reszta kodów do projektu jest napisana w języku Python
Podane komendy umożliwiają:
-Ruch do góry o kąt jednostkowy
-Obrót do dołu o kąt jednostkowy
-Ustawienie kamery pod kątem wybranym przez użytkownika
-Ustawienie kamery w skrajnym dolnym położeniu
-Testowanie poprawnego działania komend
Do budowy układu wystarczy serwomotor, płytka Raspberry Pi, kilka przewodów oraz, jeśli jest taka potrzeba, zewnętrzne źródło zasilania dla serwomotoru. Należy sprawdzić w dokumentacji używanego serwa, czy napięcie oferowane przez płytkę Raspberry Pi wystarczy do zasilenia serwomotoru.
Sterowanie chwytakiem:
Do budowy układu potrzeba silnika DC, płytki Raspberry Pi 4 Model B, mostka H oraz źródła zasilania dla silnika DC. Sam kod został również napisany w Pythonie i umożliwia on, za pomocą wywołania klasy, ustawienie chwytaka w dwóch położeniach: zacisku oraz braku zacisku. Również tutaj napisane komendy zostały potem użyte do algorytmu łapiącego piłki.
Algorytm do wykrywania piłek:
Algorytm został napisany głównie za pomocą biblioteki OpenCV. Jest to jedno z najczęściej używanych narzędzi do obróbki i analizy obrazu w obszarze wizji komputerowej. Użyto również biblioteki NumPy, która ułatwia działania na dużych, wielowymiarowych macierzach i tabelach.
Aby lepiej zrozumieć, czym motywowane były niektóre elementy działania algorytmu, należy odnieść się krótko do regulaminu zawodów... Celem konkurencji jest zebranie 9 kolorowych piłek tenisowych ustawionych na białych platformach przez autonomicznego drona. Same rozstawienie platform jest znane przed rozpoczęciem konkurencji i tworzą one kwadrat 3x3, ale nie są znane kolory piłek na danych platformach, dlatego też nasz algorytm powinien umieć je rozpoznawać, gdyż kolor piłki definiuje, czy należy ją zebrać w danym momencie.
Działanie algorytmu sprowadza się do prostej procedury. Na początku wykrywa on same białe pola, po czym analizuje on obszar każdej białej platformy z osobna i na podstawie najczęściej występowanego koloru (oprócz białego) klasyfikuje on kolor piłki. Finalnie algorytm zwraca nam tablicę 3x3, z której możemy wyczytać jaki kolor ma nasza piłka oraz pewien parametr "Confidence". Reprezentuje on procent analizowanego obszaru jaki zajmuje wykryty kolor. Nie jest to, więc żaden parametr wyliczony na podstawie np. rachunku prawdopodobieństwa, ale może dać nam pewne przypuszczenia, co do trafności klasyfikacji.
*Wszystkie kody napisano z pomocą AI*
###kod do kamery###
from gpiozero import AngularServo
from time import sleep
from gpiozero import Device
from gpiozero.pins.pigpio import PiGPIOFactory
Device.pin_factory = PiGPIOFactory()
class camera_servo:
def __init__(self):
self.servo = AngularServo(18, min_angle=-90, max_angle=90, min_pulse_width=0.0005, max_pulse_width=0.0025)
def go_up(self): #x - aktualny kąt
self.servo.angle -= 0.5
return self.servo.angle
def go_down(self, x): #x - aktualny kąt
self.servo.angle += 0.5
print("aktualny kąt:" + str(self.servo.angle))
return self.servo.angle
def look_down(self):
self.servo.angle = 90
print("aktualny kąt:" + str(self.servo.angle))
return self.servo.angle
def set_angle(self, x):
self.servo.angle = x
print("aktualny kąt:" + str(self.servo.angle))
return self.servo.angle
def test(self):
while (True):
self.look_down()
sleep(2)
servo = camera_servo()
while(1):
servo.test()
###kod do grippera###
from gpiozero import DigitalOutputDevice
from gpiozero import Device
import RPi.GPIO as GPIO
from time import sleep
class Gripper:
pin1=17
pin2=27
def __init__(self):
self.gripped=False
GPIO.setmode(GPIO.BCM)
def grip(self):
GPIO.output(self.pin1, GPIO.HIGH)
GPIO.output(self.pin2, GPIO.LOW)
self.gripped=True
def no_grip(self):
GPIO.output(self.pin1, GPIO.LOW)
GPIO.output(self.pin2, GPIO.HIGH)
self.gripped=False
def test(self):
while(1):
self.grip()
self.no_grip()
##kod do wykrywania piłek##
import cv2
import threading
from collections import Counter
import numpy as np
def get_platforms(image):
# Convert to grayscale
gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
# Thresholding to get binary image and detect white areas (platforms)
_, binary_image = cv2.threshold(gray_image, 200, 255, cv2.THRESH_BINARY)
# Find contours
contours, _ = cv2.findContours(binary_image, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
area_threshold = 30 # Adjust this value based on your requirements
# Filter contours by area
filtered_contours = [contour for contour in contours if cv2.contourArea(contour) >= area_threshold]
# Calculate the bounding rectangles, which gives the boundaries of the platforms
bounding_boxes = [cv2.boundingRect(contour) for contour in filtered_contours]
return bounding_boxes, len(bounding_boxes)
def classify_color(color):
max_comp = color.index(max(color))
diff = max(color) - min(color)
if color[max_comp] < 60:
return 'black'
if min(color) > 200:
return 'empty'
if diff < 10:
return 'unknown'
if max_comp == 0:
return 'red'
if max_comp == 1:
return 'green'
if max_comp == 2:
return 'blue'
return 'unknown'
def get_color_histogram(image, exclude_white=True):
# Reshape the image to be a list of pixels
pixels = image.reshape(-1, 3)
# Convert to a list of tuples
pixels = [tuple(pixel) for pixel in pixels]
if exclude_white:
# Remove white pixels (thresholds can be adjusted)
pixels = [pixel for pixel in pixels if not (pixel[0] > 200 and pixel[1] > 200 and pixel[2] > 200)]
# Count the occurrences of each color
color_counts = Counter(pixels)
return color_counts
def classify_colors(image, bounding_boxes, center_crop_factor=0.5):
color_classes = []
confidences = []
for x, y, w, h in bounding_boxes:
# Calculate the center crop dimensions
center_w, center_h = int(w * center_crop_factor), int(h * center_crop_factor)
center_x, center_y = x + w // 2 - center_w // 2, y + h // 2 - center_h // 2
# Ensure the center region is within bounds
center_x = max(0, center_x)
center_y = max(0, center_y)
center_w = min(center_w, image.shape[1] - center_x)
center_h = min(center_h, image.shape[0] - center_y)
# Extract the center region
center_roi = image[center_y:center_y+center_h, center_x:center_x+center_w]
color_histogram = get_color_histogram(center_roi)
if not color_histogram:
color_classes.append('Empty')
confidences.append(0.0)
continue
# Classify the color based on the histogram
colors, counts = zip(*color_histogram.items())
max_count = max(counts)
total_pixels = sum(counts)
confidence = max_count / total_pixels
color = colors[np.argmax(counts)]
color_class = classify_color(color)
color_classes.append(color_class)
confidences.append(confidence)
return color_classes, confidences
def find_colors(image_path, visualize=True):
image = cv2.imread(image_path)
if visualize:
image_copy = image.copy()
bounding_boxes, num = get_platforms(image)
print(num, bounding_boxes)
colors, confidences = classify_colors(image, bounding_boxes)
if visualize:
titles = [f'{color} (Confidence: {conf:.2f})' for color, conf in zip(colors, confidences)]
# show_regions_of_interest(image_copy, bounding_boxes, titles=titles)
return colors, confidences
def preprocess_image(image):
resized_image = cv2.resize(image, (1000, 1000))
return resized_image
class BallFinder:
_instance = None
_lock = threading.Lock()
grid_size = (3, 3)
image_size = (1000, 1000)
def __new__(cls, *args, **kwargs):
with cls._lock:
if cls._instance is None:
cls._instance = super(BallFinder, cls).__new__(cls)
cls._instance.init()
return cls._instance
def init(self):
self.grid_dict = {i: None for i in range(9)}
self.grid = [[{'color': None, 'confidence': 0.0} for _ in range(self.grid_size[1])] for _ in range(self.grid_size[0])]
self.is_finished = False
def process_image(self, image_path) -> bool:
image = cv2.imread(image_path)
bounding_boxes, num = get_platforms(image)
if num !=9:
return False
colors, confidences = classify_colors(image, bounding_boxes)
counts = {'blue': 0, 'green': 0, 'red': 0, 'black': 0, 'empty': 0, 'unknown': 0}
for color in colors:
counts[color.lower()] += 1
if counts['blue'] == 1 and counts['red'] == 1 and counts['black'] == 1:
for i, color in enumerate(colors):
self.grid_dict[i] = color
self.grid[i // 3][i % 3] = {'color': color, 'confidence': confidences[i]}
self.is_finished = True
return True
else:
return False
if __name__ == '__main__':
finder = BallFinder()
image_path = "sciezka_do_zdjecia/nazwa_zdjecia.jpg"
finder.process_image(image_path)
if finder.is_finished:
print(finder.grid)
print(finder.grid[2][2]['color'])