Подражая белому кролику

Робот, который следовал за белым кроликом в нашем предыдущем проекте, очень захотел научиться так же ловко, как робот-кролик, двигаться вдоль черной линии. Мы решили ему в этом помочь, написав для него программу на Python, использующую камеру и машинное зрение с использованием библиотеки OpenCV.

Чтобы несколько усложнить задачу, мы решили построить дорожный знак и положить на линию условного "лежачего полицейского".

Дорожный знак было решено построить на основе платы Arduino и сервопривода, а управлять им - используя произвольный ИК-пульт дистанционного управления.

Код программы для дорожного знака выглядит следующим образом:

#define ServoPin 5 
#include <Servo.h>
#include <IRremote.h> 

Servo servo; 
int receiverPin = 11; 
IRrecv irrecv(receiverPin);  
decode_results results;

void setup() {
  Serial.begin(9600);
  irrecv.enableIRIn();    
}

void loop() {
  if (irrecv.decode(&results)) { 
    Serial.println(results.value, HEX); 
    if(results.value == 0xFF609F){ 
      servo.attach(ServoPin);
      servo.write(180);
      delay(1000);
      servo.detach();
    }
    if(results.value == 0xFFE01F){ 
      servo.attach(ServoPin);
      servo.write(0);
      delay(1000);
      servo.detach();
    }
    irrecv.resume();
  }
}

Конструкция робота не претерпела изменений по сравнению с предыдущим проектом, разве что камеру развернули под иным углом, чтобы она видела одновременно и линию перед роботом и пространство рядом с дорогой.

Увидев красный знак, робот должен остановиться и ожидать пока ему будет разрешено двигаться дальше с помощью зеленого знака. "Лежачим полицейским" будет перпендикулярная черной линии синяя полоса. Перед ней робот должен будет снизить скорость.

Алгоритм работы нашего робота таков:

• Запускаем независимый процесс по непрерывному чтению данных с камеры в объект в памяти
• Запускаем независимый процесс по управлению двигателями c использованием ПИД-регулятора
• Запускаем основной процесс, в котором:
• Берем очередной кадр из объекта в памяти, и переводим в цветовое пространство HSV
• Отрезаем от кадра нижнюю четверть кадра и, используя функцию вычисления моментов cv2.moments, вычисляем горизонтальное смещение линии относительно центра
• Передаем данные о смещении относительно линии процессу ПИД-регулятора
• Отрезаем нижнюю половину кадра и анализируем количество "синих" точек в нем. Если их количество превышает заданную границу - сообщаем процессу ПИД-регулятора о снижении скорости движения
• Анализируем количество "красных" точек во всем кадре. Если их количество превышает заданную границу - сообщаем процессу ПИД-регулятора о нулевой скорости движения

Робот видит примерно такую картинку:

Линию в нижней четверти кадра робот видит так:

А вот так он выделяет в кадре красный знак:

Исходный код программы робота:

#!/usr/bin/env python3

import threading
import rpyc
import numpy as np
from imutils.video import WebcamVideoStream
import cv2, time

conn = rpyc.classic.connect('192.168.32.209')
ev3 = conn.modules['ev3dev.ev3']

btn = ev3.Button()

mA = ev3.LargeMotor('outA')
mD = ev3.LargeMotor('outB')

P = 0.4
D = 1.0
K = 0
speed = 300
speed_go = 0
max = 0
x = 0
es = 0
dArea = 0
size = 0

cap = WebcamVideoStream(src=0).start()
for i in range(10): frame = cap.read()
hsv = cv2.cvtColor(frame[:][360:], cv2.COLOR_BGR2HSV)
frame_gray = cv2.inRange(hsv, (0, 0, 0), (255, 170, 80))
cv2.imwrite('/var/www/html/camera_bot_line.png', frame_gray)

def motor_control():
    global x, D, P, frame_gray, speed, es, speed_go
    while True:
        if(speed_go < speed): speed_go += 50
        if(speed_go > speed): speed_go -= 50

        if(speed_go < 150):
            P = 0.15
            D = 0.2
        else:
            P = 0.4
            D = 1.0

        if(speed_go < 5): 
            P = 0
            D = 0
        else:
            P = 0.4
            D = 1.0

        speedA = speed_go - ((x*P) + D*(x-es))
        speedD = speed_go + ((x*P) + D*(x-es))
        if(speedA > 900): speedA = 900
        if(speedA < -900): speedA = -900
        if(speedD > 900): speedD = 900
        if(speedD < -900): speedD = -900
        mA.run_forever(speed_sp=speedA)
        mD.run_forever(speed_sp=speedD)
        es = x

t1 = threading.Thread(target=motor_control)
t1.daemon = True
t1.start()

while True:
    frame = cap.read()
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    frame_gray = cv2.inRange(hsv[:][360:], (0, 0, 0), (255, 170, 80))
    frame_blue = cv2.inRange(hsv[:][240:], (90, 120, 60), (135, 250, 250))
    frame_red = cv2.inRange(hsv, (150, 100, 60), (255, 250, 250))

    if(np.sum(frame_blue) > 30000):
        speed = 100
    elif(np.sum(frame_red) > 80000):
        speed = 0
    else:
        speed = 300

    moments = cv2.moments(frame_gray, 1)
    dM01 = moments['m01']
    dM10 = moments['m10']
    dArea = moments['m00']
    if(dArea != 0):
        x = 320 - int(dM10 / dArea)
        y = 80 - int(dM01 / dArea)

    if(btn.backspace):
        speed = 0
        speed_go = 0
        x = 0
        break

mA.stop(stop_action="brake")
mD.stop(stop_action="brake")