Model kontrolera PID z kamerą, belką i piłką

Środa, 31 Marzec 2021

Kamery pozwalają rejestrować obraz i dźwięk. Podczas codziennego życia nie zwracamy na nie uwagi a tak naprawdę obserwują nas na każdym kroku – na drogach, w sklepach, w pojazdach komunikacji miejskiej. Kamery mają na celu poprawić jakość naszego życia poprzez ułatwienie wykonywania pewnych czynności czy poprzez zapewnienie bezpieczeństwa. Połączenie kamer z systemami analizy obrazu daje duże możliwości.

Klasycznym przykładem zastosowania kamer są skrzyżowania uliczne. Sygnalizacji świetlnej towarzyszą specjalnie ustawione kamery umieszczone nad każdym pasem ruchu i skierowane na linie warunkowego zatrzymania. Zintegrowanie tego systemu z sygnalizacją świetlną pozwala rejestrować auta przejeżdżające na czerwonym świetle.

Zdjęcie wykonane przez kamerę podlega procesom analizy obrazu z użyciem techniki OCR (optical character recognition) – optycznego rozpoznawania znaków. Dzięki temu odczytywane są numery rejestracyjne auta zarejestrowanego na zdjęciu. Podobna technika stosowana jest do odczytywania danych z kart płatniczych, tak jak to pokazano na fotografii 1.

Fotografia 1. Użycie techniki OCR do odczytu danych z karty kredytowej

Analizę obrazu realizuje oprogramowanie. Interesującym narzędziem, które pozwala rozpocząć próby z tą techniką, jest biblioteka OpenCV (Open Source Computer Vision Library), dostępna z poziomu języka Python. Z zastosowaniem tej metody został opracowany poniższy projekt. Jego celem była wizualizacja działania kontrolera PID dla modelu belki z piłką.

Zadaniem układu jest ustawienie piłeczki w zadanym miejscu na belce tak, aby pozostała w bezruchu.

Model układu sterowania

Głównym problemem zadania było opracowanie regulatora PID, który na wyjściu będzie generował wartość odpowiadającą położeniu serwomechanizmu tak, aby piłka pozostała w bezruchu.

Rysunek 1. Model układu sterowania

Na rysunku 1 został pokazany schemat blokowy układu. Równanie opisujące regulator wygląda następująco:

gdzie:

u(t) to wartość o którą którą trzeba przestawić serwomechanizm (od pozycji 90°)
e(t) to wartość błędu w danym momencie symulacji (x_zadane – aktualny_x)

Wyliczenie poszczególnych członów regulatora w programie wykonałem w następujący sposób:

Człon proporcjonalny: k_p·e, gdzie e aktualny błąd.
Człon całkujący: k_i·całka, gdzie całka to wartość w której przechowywane są sumy iloczynów e·h (e to aktualny błąd).
Człon różniczkujący: k_d·(ePoprzedni–e)/h.

Konstrukcja

Pierwszym krokiem było przygotowanie wizualizacji 3D. Ta część zadania została wykonana z użyciem programu Sketchup. Wynik prac został pokazany na rysunku 2.

Rysunek 2. Wizualizacja modelu testowego

Następnie należało skompletować listę komponentów potrzebnych do wykonania konstrukcji, są to:

płytka Arduino Uno,
serwomechanizm (tower pro micro servo 9g),
kamera na USB,
korytko po którym będzie się poruszała piłka,
podstawa,
listewka która posłuży za uchwyt do kamery,
drut, który posłuży do połączenia ramienia serwa z korytkiem,
elementy do wykonania podpory korytka (odpowiednio ukształtowany grubszy drut),
piłka.

Po zmontowaniu wszystkich elementów konstrukcji, ostatnim krokiem jest podłączenie serwomechanizmu do płytki Arduino. Należy to wykonać według następującego schematu:

czerwony przewód serwa – GND,
brązowy przewód serwa – 5 V,
pomarańczowy przewód serwa – PIN 3.

Fotografia 2. Wygląd wykonanego prototypu

Gotowa konstrukcja została pokazana na fotografii 2.

Oprogramowanie sterujące

W pierwszej kolejności zostanie omówiony prosty szkic Arduino. Jego zadanie polega na odczytywaniu wartości liczbowych przesyłanych poprzez interfejs szeregowy i odpowiednie ustawianie osi serwomechanizmu.

Listing 1. Kod szkicu Arduino odpowiadający za sterowanie serwomechanizmem

1: #include <Servo.h>
2: Servo servo;
3: int servoPin = 3;
4: int radius = 90;
5:
6: void setup() {
7: Serial.begin(115200);
8: servo.attach(servoPin);
9: servo.write(radius);
10: }
11:
12: void loop() {
13: if (Serial.available()) {
14: radius = Serial.read();
15: servo.write(radius);
16: }
17: }

Kod programu został pokazany na listingu 1. Najpierw importowana jest biblioteka potrzebna do obsługi serwa (wiersz nr 1). W metodzie setup ustawiana jest prędkość transmisji danych i wybierany jest pin sterujący serwem oraz jego położenie początkowe odpowiadające ustawieniu 90°. W metodzie loop w wierszu nr 13 sprawdzana jest możliwość odczytu danych z interfejsu szeregowego. Jeśli została odebrana nowa wartość to kąt serwa jest ustawiany zgodnie z nią (wiersz nr 15). Przesyłane wartości zawierają się w zakresie 60...120 więc pominięto warunki sprawdzające.

Po zaprogramowaniu Arduino przyszedł czas na przygotowanie kodu w Pythonie. Jego zadanie będzie polegało na odebraniu obrazu z kamerki, obliczeniu odległości piłki od zadanej pozycji, wykonanie odpowiednich obliczeń dla regulatora PID oraz wysłanie wartości sterującej serwomechanizmem.

Analizowanie obrazu z kamerki odbywa się z użyciem biblioteki OpenCV. Żeby program rozpoznał i śledził ruch piłki musi wykonać następujące działania:

zdefiniować zakres barw,
lekko rozmazać obraz poprzez rozmycie Gaussa,
utworzyć maskę dla danego zakresu,
na podstawie stworzonej maski znaleźć kontur,
na podstawie konturu znaleźć centroid oraz narysować linię wokół obiektu.

Listing 2. Kod w języku Python odpowiadający za śledzenie ruchu piłki

1: import cv2
10: cap = cv2.VideoCapture(0)
36: lowerbound = np.array([10, 100, 120])
37: upperbound = np.array([(25, 255, 255)])
39: while (True):
40: ret, image = cap.read()
41: resized_image = cv2.resize(image, (480, 320))
42: blurred = cv2.GaussianBlur(resized_image, (11, 11), 0)
43: hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
44: mask = cv2.inRange(hsv, lowerbound, upperbound)
45: mask = cv2.erode(mask, None, iterations=2)
46: mask = cv2.dilate(mask, None, iterations=2)
47: cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)[1]
48: if len(cnts) > 0:
49: m = max(cnts, key=cv2.contourArea)
50: M = cv2.moments(m)
51: x = int(M["m10"] / M["m00"])
52: y = int(M["m01"] / M["m00"])
53: cv2.drawContours(resized_image, [m], -1, (0, 255, 0), 2)
54: cv2.circle(resized_image, (x, y), 7, (255, 255, 255), -1)
55: cv2.line(resized_image, (240, 0), (240, 320), (0, 0, 255), 2)
56: cv2.line(resized_image, (x, 0), (x, 320), (0, 255, 255), 2)
57: cv2.imshow(‘frame’, resized_image)
58: cv2.imshow(‘mask’, mask)
59: if cv2.waitKey(1) & 0xFF == ord(‘q’):
60: break

Kod programu został pokazany na listingu 2. Na początku dodawana jest biblioteka oraz inicjowana jest zmienna cap, która kontroluje kamerę (wiersz nr 10). Wartość w metodzie VideoCapture wskazuje numer kamery w systemie (gdy jest dołączonych więcej urządzeń). W linii nr 39 rozpoczyna się pętla, która zostanie przerwana dopiero po wciśnięciu na klawiaturze litery q.

W pętli najpierw odczytywana jest klatka obrazu. Zmienna ret przyjmuje wartości true/false, w zależności od tego, czy klatka została przechwycona czy nie. Zmienna image odnosi się do przechwyconej klatki. Następnie zmieniany jest rozmiar przechwyconej klatki (wiersz nr 41), potem obraz jest poddany delikatnemu rozmyciu, aby kontury nie były zbyt szczegółowe (wiersz nr 42) i zmieniany jest model przestrzeni barw z BGR na HSV (wiersz nr 43). Wiersze 44...46 tworzą maskę, która usuwa drobne niedoskonałości.

Zmienna cnts reprezentuje kontur obiektu. W wierszach 49...56 wyszukiwany jest maksymalny kontur, następnie jest znajdywany centroid, rysowany zielony kontur
piłki, rysowane białe kółko które reprezentuje środek piłki, oraz rysowane są dwie linie. Jedna dzieli ekran na pół a druga reprezentuje środek piłki (dla lepszej widoczności).

W wierszach 57 i 58 rysowane są okna wygenerowanego obrazu oraz wygenerowanej maski. Efekt działania tej części programu został pokazany na rysunku 3.

Rysunek 3. Efekt działania programu używającego bibliotekę OpenCV

Krótkiego omówienia wymaga jeszcze kod odpowiedzialny za obliczenia regulatora PID. Na listingu 3 zostały pokazane wartości początkowe układu, natomiast obliczenia znajdują się na listingu 4.

Listing 3. Wartości początkowe układu

13: position = 240
14: P = 0
15: I = 0
16: D = 0
17: kp = 0.1
18: ki = 0
19: kd = -0.04
20: h_actual = 0
21: calka = 0
22: pochodna = 0
23: e_previous = 0

Listing 4. Kod regulatora PID

92: e = (position - x)
93:
94: # calculate elapsed time
95: h_previous = h_actual
96: h_actual = time.time()
97: h = h_actual - h_previous
98: # Proportional
99: P = kp * e
100:
101: # Integral
102: calka += e * ki
103: I = ki * calka
104:
105: # Derivative
106: pochodna = (e_previous - e) / h
107: e_previous = e
108:
109: D = kd * pochodna
110:
111: radius = 90 + (P + I + D)
Strona 9 z 13
112:
112: if radius <= 60:
113: radius = 60
114: if radius >= 120:
115: radius = 120
116:
117: arduinoSerial.write(str(chr(int(radius))))
118:
119: xar.append(e)
120: yar.append(h_actual)

Na początku obliczany jest błąd e. Jest to różnica między położeniem piłki a pozycją zadaną. W wierszach 95...97 wyliczany jest czas jaki upłynął od poprzedniego pomiaru, a wiersz 99 to wyliczanie wartości członu proporcjonalnego. Wiersze 102 i 103 odpowiadają za obliczenie wartości członu całkującego, a wiersze 106 i 107 to wartość członu różniczkującego.

W linii 111 utworzona jest zmienna radius, która przechowuje wartość kąta dla położenia osi serwomechanizmu. Wartość ta jest ograniczana do zakresu 60...120°, jest to związane z budową modelu. W wierszu 117 następuje wysyłanie obliczonych wartości do Arduino.

Listing 5. Kod umożliwiający narysowanie wykresu błędu w funkcji czasu

137: plt.plot(yar, xar)
138: plt.title(“Wykres bledu od czasu dla regulatora PID pilki")
139: plt.xlabel(“Blad")
140: plt.ylabel(“Czas")
141: plt.show()
142: cap.release()
143: cv2.destroyAllWindows()

Na końcu, zapisywany jest aktualny błąd i czas w postaci listy, aby po zakończeniu symulacji utworzyć wykres błędu w czasie. Kod realizujący tę funkcję został pokazany na listingu 5, a efekt jego działania pokazuje rysunek 4.

Rysunek 4. Przykładowy wykres wartości błędu w funkcji czasu

Cały kod jest dostępny tu: https://bit.ly/3wrecGQ.

Wyniki testów

Na wejście regulatora była podawana aktualna pozycja piłki. Regulator obliczał błąd, czyli różnicę między zadaną pozycją piłki a aktualną, czas jaki upłynął od poprzedniego kroku symulacji oraz kąt o jaki powinien się obrócić serwomechanizm. Główny problem polegał na tym, żeby regulator ustawił piłkę w dokładnie zadanej pozycji. Wynikał z tego, że serwomechanizm może wykonać krok co najmniej o jeden stopień więc wartości z regulatora PID należało zaokrąglić w górę przez co sterowanie nie było zbyt precyzyjne. Dopiero po kilku próbach udało się wystartować piłkę z takiej pozycji, aby regulator ustawił ją idealnie w zadanej pozycji.

Błąd który widać na samym początku wykresu z rysunku 4 związany jest z ustawianiem piłki na pochylni. Drobne anomalie występujące pod koniec wykresu związane są z wyżej wymienionym zaokrągleniem oraz minimalnymi luzami w konstrukcji. Ostatecznie regulator spełnił swoje zadanie. W początkowej fazie bardzo szybko zareagował na duży błąd przez co bardzo szybko ustabilizował piłkę na właściwej pozycji.

Wnioski, uwagi, rekomendacje

Bardzo ważną rzeczą przy budowie konstrukcji jest eliminacja luzów, które mogą się pojawić na ramieniu łączącym serwomechanizm z pochylnią jak i na łączeniu pochylni z podporą.
Ważą podpowiedzią dla tych, którzy będą chcieli wykonać własne modyfikacje jest to, ze Arduino podczas przyjmowania danych z interfejsu szeregowego zamienia znaki na kody ASCII a przyjmuje te dane jako typ String. Dlatego podczas wysyłania danych z programu należy najpierw rzutować zmienną radius do typu całkowitego (wartości z regulatora są typu double a serwomechanizm działa na wartościach całkowitych), następnie należy ją zamienić na typ char, który potem wysyłany jest jako String.

Tabela 1. Zakresy palety HSV dostosowane do biblioteki OpenCV

	HSV	OpenCV
H	0...359°	0...179
S	0...100%	0...255
V	0...255	0...255

Maskę w bibliotece OpenCV tworzy się w przestrzeni barw HSV, dlatego należy wykonać konwersję z BGR na HSV. Biblioteka OpenCV operuje na mapowanych zakresach HSV. Tabela 1 zawiera zmapowane wartości. W wybieraniu dolnego i górnego zakresu pomocny jest rysunek 5. Wartość V można dobrać bazując na popularnych Color picker-ach dostępnych w Internecie.

Rysunek 5. Diagram ułatwiający operowanie na palecie barw HSV

Przy wyborze punktów do narysowania konturu należy wybrać te maksymalne by zniwelować ewentualne luki spowodowane oświetleniem obiektu. Gdy dany punkt obiektu jest mocno oświetlony wtedy kamera widzi go jako inny kolor. Ddy nie ma go w zdefiniowanym przez nas zakresie barw maski, to w tym miejscu pojawi się czarna plama. Wybierając minimalne punkty do narysowania konturu, spowodujemy, że ten punkt będzie znajdował poza konturami obiektu. Dla wartości kp=0,1, ki=0, kd=–0,04,
otrzymano najlepsze rezultaty.

inż. Michał Uraban
dev.michael.urban@gmail.com

Bibliografia
Dokumentacja OpenCV: https://bit.ly/39EA3kh
Poradniki jak wykrywać obiekty i je śledzić: https://bit.ly/31Iydu6, https://bit.ly/31Hh3NL
Informacje na temat przestrzeni barw HSV: https://bit.ly/3dMdE5Z
Informacje jak prawidłowo definiować zakres barw HSV: https://bit.ly/3rQYfWK