воскресенье, 6 мая 2018 г.

Длинный хвост питона - часть 1

Последние полтора года наша команда изучает Python и почти все наши проекты, использующие LEGO Mindstorms EV3 в качестве платформы, были разработаны с использованием этого языка программирования. Изучение Python и, параллельно, ОС Linux, велось не случайно, а имело далеко идущие планы. Дело в том, что Питон должен стать "переходным мостиком", протянув свой длинный хвост между LEGO-роботами и более серьезной робототехникой, в которой компонентов от LEGO должно становиться все меньше и меньше.


Задача, которую мы начнем решать в рамках этого проекта - создание модели автономно управляемого транспортного средства, использующего машинное зрение. В первой части проекта, модель автомобиля, оснащенная бортовым одноплатным компьютером и камерой, должна будет двигаться по черной линии (трассе) с кратковременной остановкой на заданном расстоянии перед знаком "стоп".
Продвинутый читатель возможно обратит внимание, что решаемая задача в чем-то схожа с описанной в регламенте состязания ВРО "Автотранспортные интеллектуальные робототехнические системы" и будет несомненно прав. Однако следует заметить, что мы не готовимся к соревнованиям, мы решаем интересные задачи в рамках самообразования. Использовать или нет опубликованный нами код в соревновательной плоскости - дело вашей совести.

Конструкция модели автомобиля, которую мы будем использовать, во многом повторяет модель LEGO Technic #9398, имеет полноприводное шасси и рулевой сервомотор.


В проекте мы снова будем использовать Raspberry Pi, а вот в качестве контроллера двигателей, роль которого в прошлом проекте "Следуй за белым кроликом" выполнял LEGO Mindstroms EV3, применим Arduino с установленным Adafruit Motor Shield. C LEGO Technic двигателями постоянного тока все понятно, они штатно подключаюся к клеммам M1 и M2 шилда и управляются функциями изменения скорости и направления вращения.


А вот с LEGO сервомотором пришлось поэкспериментировать. Как оказалось, управляется он посредством ШИМ по двум средним проводам, крайние же обеспечивают питание. Соответственно подключить его можно точно также как обычный DC-мотор к, например, клеммам M3, однако необходимо дополнительно обеспечить ему питание 9В, которое можно взять с клемм питания шилда PWR_EXT. Для управления углом поворота сервомотора используется обычная ШИМ-модуляция, соответственно встроенные в библиотеку Adafruit Motor Shield функции для управления скоростью и направлением вращения DC-мотора начинают управлять углом поворота сервомотора.


Как вы уже наверное успели заметить на фото выше - мы используем новую камеру - это камера Sony Playstation Eye. Ее основные преимущества - высокая частота кадров (до 180 fps), достаточно широкий, изменяемый физическим регулятором угол зрения и отличная поддержка со стороны Linux.

Основные моменты, которые мы проработаем в первой части проекта:
  • Организуем передачу данных между Raspberry Pi и Arduino, используя помехозащищенный "протокол", основанный на передаче текстовых строк
  • Настроим многопоточную обработку данных для разумного распределения вычислительных ресурсов
  • Запустим веб-трансляцию обработанного видеопотока для визуального мониторинга прохождения трассы
  • Научимся распознавать один знак - "Стоп", расположенный справа от дороги и остановку на заданном расстоянии до него.
Для взаимодействия Raspberry Pi и Arduino мы используем стандартное serial-соединение поверх USB и соответствующие библиотеки с обоих сторон. Так как нам нужно передавать различные данные - скорость и направление вращения ходового мотора и угол поворота сервомотора, то просто передавать число через serial не выйдет - принимающая сторона должна понимать что она получает. Мы организовали передачу пакета данных в виде текстовой строки в формате "L90F255", где:
  • первый символ L/R - указывает направление поворота рулевого сервомотора
  • следующие два символа - угол поворота рулевого сервомотора в данном направлении, в диапазоне 0..90
  • четвертый символ - F/B - направление вращение ходового мотора (вперед-назад)
  • следующие три символа - скорость вращения ходовых моторов в данном направлении, в диапазоне 0..255.
Последний переданный пакет (строка) запоминается, и новый пакет, чтобы не забивать канал передачи данных, передается только при изменении углов/скоростей, либо раз в секунду подтверждается повторением, если последние переданные угол и скорость все еще актуальны. Необходимость в подтверждении связана с реализацией на принимающей стороне автоматического отключения двигателей, если за последние 3 секунды не приходило никаких "правильных" пакетов. Это очень удобно - можно прерывать программу на Raspberry Pi не опасаясь что Arduino продолжит крутить двигателями в соответствии с последней переданной командой.


На стороне Raspberry Pi у нас выполняется целый ряд задач, каждая из которых должна работать "в своем темпе", не тормозя работу остальных:
  • взаимодействие с Arduino
  • чтение данных с камеры в памяти
  • анализ нижней части кадра, поиск линии, формирование отклонения от линии
  • ПИД регулирование (ошибка - отклонение от линии, управляющее воздействие - угол поворота рулевого сервомотора)
  • анализ правой части кадра, поиск знака "стоп"
  • трансляция видеопотока, формирующегося из нижней части кадров при поиске линии
Для параллельного выполнения задач в программе на стороне Raspberry Pi используется модуль threading.


Веб-трансляция видеопотока, формирующегося в ходе анализа положения линии относительно модели автомобиля, происходит с использованием модуля flask. Конечно, использование wi-fi дает некоторую задержку при трансляции, но при невысокой скорости движения это не так важно. Гораздо важнее иметь возможность "видеть как робот", понимать на каком участке трассы какие есть проблемы с распознаванием линии (засветка, радиусы поворота и т.п.).

Для поиска знака "стоп" и остановки перед ним на заданном расстоянии мы будем использовать более умный алгоритм, чем используемый ранее в проекте "Подражая белому кролику". он состоит в следующем:
  • отрезаем правую часть кадра (знак должен быть только справа от дороги)
  • накладываем цветовую маску, отбрасывая все не красные объекты
  • ищем в оставшихся в кадре объектах эллипсы встроенной функцией OpenCV
  • если найден эллипс с диаметром, лежащем в заданном диапазоне (знак на заданном расстоянии) - остановка на 10 секунд, после чего следующие 10 секунд движения алгоритм поиска знака не выполняется


Исходный код программы для Raspberry Pi:

#!/usr/bin/env python3

from flask import Flask, render_template, Response
import serial, time
import cv2
import threading
import numpy as np
from imutils.video import WebcamVideoStream

port = "/dev/ttyACM0"
ser = serial.Serial(port , 115200, timeout = 1)
time.sleep(5)

speed_go = 0
porog = 500
speed = 155
K = 2
see_red = 0
x = 0
out_old = 0
i_main = 0
i_mail2arduino = 0
i_see_red = 0
i_camera2inet = 0
time_main = time.time()
time_mail2arduino = time.time()
time_see_red = time.time()
time_camera2inet = time.time()
fps_main = 0
fps_mail2arduino = 0
fps_see_red = 0
fps_camera2inet = 0
pixel_ellips = 0

cap = WebcamVideoStream(src=0).start()
for i in range(5): frame = cap.read()
frame = cap.read()
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
frame_gray = cv2.inRange(hsv[420:,:, :], (0, 0, 0), (255, 255, 100))
frame_red = cv2.inRange(hsv[:, 320:, :], (0, 150, 30), (10, 255, 80))

cv2.imwrite('/var/www/html/dgip_gray360.png', frame_gray)
cv2.imwrite('/var/www/html/dgip_frame_all.png', frame)

def mail2arduino_pr1():
    print("Start arduino thread")
    global x, out_old, speed_go, speed, i_mail2arduino, fps_mail2arduino, time_mail2arduino
    while 1:
        local_x = x
        if(local_x >= 0): out = "L"
        else: out = "R"
        local_x = abs(int(local_x*K))

        if(local_x > 90): local_x = 90
        if(local_x < 10): out += "0" + str(local_x)
        else: out += str(local_x)

        if(speed >= 100): out += "F" + str(speed)
        elif(speed >= 10): out += "F0" + str(speed)
        else: out += "F00" + str(speed)

        if(out != out_old):
            ser.write(out.encode())
            time_out = time.time()

        if(time.time() - time_out > 2):
            ser.write("0000000".encode())
            time_out = time.time()
        out_old = out
        i_mail2arduino += 1
        if(time.time() - time_mail2arduino > 1):
            time_mail2arduino = time.time()
            fps_mail2arduino = i_mail2arduino
            i_mail2arduino = 0

def image2jpeg(image):
    ret, jpeg = cv2.imencode('.jpg', image)
    return jpeg.tobytes()

def camera2inet_pr2():
    print("Start inet thread")
    global frame_gray, frame, frame_red, i_camera2inet, time_camera2inet,  fps_camera2inet
    global fps_main, fps_see_red, fps_mail2arduino, fps_camera2inet, pixel_ellips, see_red
    app = Flask(__name__)

    @app.route('/')
    def index():
        return render_template('index.html')

    def gen_gray():
        while True:
            cv2.putText(frame_gray,"fps_main: "+str(fps_main), (0,10), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255,255,255), 1)
            cv2.putText(frame_gray,"fps_see_red: "+str(fps_see_red), (0,20), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255,255,255), 1)
            cv2.putText(frame_gray,"fps_mail2arduino: "+str(fps_mail2arduino), (0,30), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255,255,255), 1)
            cv2.putText(frame_gray,"fps_camera2inet: "+str(fps_camera2inet), (0,40), cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255,255,255), 1)

            frameinet = image2jpeg(frame_gray)
            yield (b'--frame\r\n'
                    b'Content-Type: image/jpeg\r\n\r\n' + frameinet + b'\r\n\r\n')
    def gen_red():
        while True:
            cv2.putText(frame_red,"red_see: "+str(see_red), (0,10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255,255,255), 1)
            cv2.putText(frame_red,"red_pixel: "+str(pixel_ellips), (0,20), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255,255,255), 1)
            frameinet = image2jpeg(frame_red)
            yield (b'--frame\r\n'
                    b'Content-Type: image/jpeg\r\n\r\n' + frameinet + b'\r\n\r\n')

    @app.route('/video_line')
    def video_line():
        return Response(gen_gray(),
                        mimetype='multipart/x-mixed-replace; boundary=frame')
    @app.route('/video_red')
    def video_red():
        return Response(gen_red(),
                        mimetype='multipart/x-mixed-replace; boundary=frame')
    time.sleep(0.15)
    i_camera2inet += 1
    if(time.time() - time_camera2inet > 1):
        time_camera2inet = time.time()
        fps_camera2inet = i_camera2inet
        i_camera2inet = 0

    app.run(host='0.0.0.0', debug=False,threaded=True)

def see_red_pr3():
    print("Start see red thread")
    global K, pixel_ellips, see_red, hsv, frame_red, i_see_red, time_see_red, fps_see_red
    print(frame_red.dtype)
    time_last_see_red = time.time()-20
    search = True
    while 1:
        frame_red = cv2.inRange(hsv[:, 320:, :], (0, 120, 30), (10, 255, 255))
        if(search):
            frame_copy = frame_red.copy()
            _, con, hierarchy = cv2.findContours(frame_copy, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
            for i in con:
                if len(i)>10:
                    ellipse = cv2.fitEllipse(i)
                    _, y1 = ellipse[0]
                    _, y2 = ellipse[1]
                    pixel_ellips = abs(y2-y1)*0.15 + 0.85 * pixel_ellips
                    if(pixel_ellips > 200):
                        pixel_ellips = 0
                        see_red = 1
                        search = False
                        time_last_see_red = time.time()
                    else: see_red = 0
                    cv2.ellipse(frame_red, ellipse, (255,0,0), 2)
                    break
                else:
                    see_red = 0
        if(time.time() - time_last_see_red <= 10):
            see_red = 1
        if(time.time() - time_last_see_red > 10):
            see_red = 0
            K = 0.1
        if(time.time() - time_last_see_red >= 12):
            K = 1
        if(time.time() - time_last_see_red >= 20):
            search = True

        i_see_red += 1
        if(time.time() - time_see_red > 1):
            time_see_red = time.time()
            fps_see_red = i_see_red
            i_see_red = 0


pr1 = threading.Thread(target=mail2arduino_pr1)
pr1.daemon = True
pr1.start()
pr2 = threading.Thread(target=camera2inet_pr2)
pr2.daemon = True
pr2.start()
pr3 = threading.Thread(target=see_red_pr3)
pr3.daemon = True
pr3.start()

while 1:
    frame = cap.read()
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    frame_gray = cv2.inRange(hsv[420:,:, :], (0, 0, 0), (255, 255, 80))
    if(np.sum(frame_gray) > porog and see_red != 1):
        speed = 255
        moments = cv2.moments(frame_gray, 1)
        dM01 = moments['m01']
        dM10 = moments['m10']
        dArea = moments['m00']

        x = 320 - int(dM10 / dArea)
        y = 120 - int(dM01 / dArea)
    else:
        speed = 0
    i_main += 1
    if(time.time() - time_main > 1):
        time_main = time.time()
        fps_main = i_main
        i_main = 0

Исходный код программы для Arduino:

#include <AFMotor.h>

char in_command_array[7]; 
int napr = 0;
int ugol = 0;
int error = 0;
int speed_ = 0;

AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);

void setup() {
  Serial.begin(115200);           

  motor1.setSpeed(200);
  motor1.run(RELEASE);
  motor2.setSpeed(200);
  motor2.run(RELEASE);
  motor3.setSpeed(0);
  motor3.run(FORWARD);
  delay(5000);
}

void loop() {
  String inString = "";
  String ServoNapr = "";
  String ServoUgol = "";
  String MotorNapr = "";
  String MotorSpeed = "";
  char in_command_array[7];
  
  while (Serial.available()) { 
    in_command_array[0] = Serial.read();
    if (ServoNapr == 'L' || ServoNapr == 'R') { 
      inString = "";
      for (int i=0;i<3;i++) {
        delay(1);
        char inChar = Serial.read(); 
        if (isDigit(inChar)) in_command_array[i+1] = (char)inChar; 
      }  
      int ServoUgol = inString.toInt();

    in_command_array[3] = Serial.read(); 
    inString = "";
    for (int i=0;i<3;i++) {
      delay(1);
      char inChar = Serial.read(); 
      if (isDigit(inChar)) in_command_array[i+4] = (char)inChar; 
    }  
    int MotorSpeed = inString.toInt();

    Serial.println(in_command_array);
    }
  }  

    if(in_command_array[0] == 76) { napr = -1; }
    else if(in_command_array[0] == 82) { napr = 1; }
    else if(in_command_array[0] == 48) { napr = 0; }
    else { error++; }

    Serial.println();
    Serial.print("napr = ");
    Serial.println(napr); 
    Serial.print("error = ");
    Serial.println(error); 

    int ugol_num1 = in_command_array[1] - 48;
    int ugol_num2 = in_command_array[2] - 48;

    Serial.print("ugol_num1 = ");
    Serial.println(ugol_num1); 
    Serial.print("ugol_num2 = ");
    Serial.println(ugol_num2); 

    if(ugol_num1 == 0) { ugol = ugol_num2; }
    else if(ugol_num1 != 0) { ugol = ugol_num1*10 + ugol_num2; }
    else { error++; }

    Serial.print("ugol = ");
    Serial.println(ugol);
    Serial.print("error = ");
    Serial.println(error);

    int speed_num1 = in_command_array[4] - 48;
    int speed_num2 = in_command_array[5] - 48;
    int speed_num3 = in_command_array[6] - 48;

    Serial.print("speed_num1 = ");
    Serial.println(speed_num1); 
    Serial.print("speed_num2 = ");
    Serial.println(speed_num2); 
    Serial.print("speed_num3 = ");
    Serial.println(speed_num3);

    if(speed_num1 == 0 && speed_num2 == 0) { speed_ = speed_num3; }
    else if(speed_num1 == 0) { speed_ = ugol_num2*10 + speed_num3; }
    else if(speed_num1 != 0) { speed_ = speed_num1*100 + speed_num2*10 + speed_num3; }
    else { error++; }

    Serial.print("speed = ");
    Serial.println(speed_);
    Serial.print("error = ");
    Serial.println(error);

    if(speed_ >= 255) {
      speed_ = speed_ - 255;
      motor1.run(FORWARD);
      motor1.setSpeed(speed_);
      motor2.run(FORWARD);
      motor2.setSpeed(speed_);
    }
    else {
      motor1.run(BACKWARD);
      motor1.setSpeed(speed_);
      motor2.run(BACKWARD);
      motor2.setSpeed(speed_);
    }

    if(speed_ == 255) {
      motor1.run(RELEASE);
      motor2.run(RELEASE);
    }

    if(napr == 1) { motor3.run(FORWARD); }
    else if(napr == -1) { motor3.run(BACKWARD); }

    motor3.setSpeed(ugol*2.83);
}

суббота, 31 марта 2018 г.

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

Робот, который следовал за белым кроликом в нашем предыдущем проекте, очень захотел научиться так же ловко, как робот-кролик, двигаться вдоль черной линии. Мы решили ему в этом помочь, написав для него программу на 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")


суббота, 10 марта 2018 г.

Следуй за белым кроликом

Машинное зрение использует достаточно ресурсоемкие алгоритмы и, хотя ev3dev на платформе LEGO Mindstorms EV3 и позволяет использовать веб-камеру с библиотеками OpenCV, производительность этой связки оставляет желать лучшего. С целью получения большей вычислительной мощности на борту, но в тоже время не отказываясь от удобных элементов прототипирования, входящих в наборы LEGO, мы решили использовать Raspberry Pi, который будет осуществлять все расчеты, оставляя блоку EV3 задачу низкоуровнего управления двигателями.


В проекте "Следуй за белым кроликом" мы создадим робота, который будет обнаруживать заданный объект и следовать за ним на определенной дистанции, подстраиваясь под скорость его движения. В качестве "кролика", мы будем использовать другого робота, следующего по линии и везущего на себе контрастный предмет, за которым и будет следовать разрабатываемый в рамках проекта робот.


В проекте мы используем Raspberry Pi второй версии. 4 ядра на борту дают возможность распределить по ним задачи:
1) получение потока данных с камеры
2) постобработка изображения и выделение в кадрах искомого объекта, анализ его размера и положения
3) управление двигателями посредством двух ПИД-регуляторов.


Для связи между Raspberry Pi и EV3 можно использовать как проводное подключение через USB (настроив сетевой LAN-интерфейс), так и беспроводную связь через WiFi/Bluetooth. Мы используем пару WiFi-адаптеров, один в EV3, другой в Raspberry Pi. Питание Raspberry Pi получает от повербанка, который робот также везет на себе. Для "удаленного" управления двигателями мы используем библиотеку RPyC (Remote Python Call).


Алгоритм работы робота следующий:
1) Получаем единичный кадр с камеры
2) Анализируем цвет объекта и размер объекта, расположенного по центру кадра, именно за ним мы и будем следовать в дальнейшем
3) запускаем процесс чтения данных с камеры, получая объект в памяти, содержащий в каждый момент времени актуальную картинку
4) циклично анализируем кадр и пытаемся найти на нем искомый объект. Если объект найден - определяем его горизонтальное смещение и изменение размера относительно эталонного
5) передаем процессу с ПИД-регуляторами данные о горизонтальном смещении и отклонении в размере
6) ПИД регуляторы вырабатывают управляющие воздействия, которое суммируются и результат передается подсистеме управления двигателями.


Исходный код робота, следующего за белым кроликом выглядит так:

#!/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.2
speed = 0
max = 0
x = 0
dArea = 0
size = 0

cap = WebcamVideoStream(src=0).start()
for i in range(10): frame = cap.read()

hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
H = hsv[320,240, 0]
S = hsv[320,240, 1]
V = hsv[320,240, 2]
Lo = np.array([H-35, S-50, V-75])
Hi = np.array([H+35, S+50, V+75])

frame_gray = cv2.inRange(hsv, Lo, Hi)
cv2.imwrite('/var/www/html/kube_search.png', frame_gray)

def motor_control():
    global dArea, x, P, size, frame_gray
    while True:
        if(np.sum(frame_gray) > 10000):
            speedA = size*3 - (x*P)
            speedD = size*3 + (x*P)

            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)
        else:
            mA.stop(stop_action="brake")
            mD.stop(stop_action="brake")

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, Lo, Hi)
    if(np.sum(frame_gray) > 10000):

        moments = cv2.moments(frame_gray, 1)
        dM01 = moments['m01']
        dM10 = moments['m10']
        dArea = moments['m00']

        if(max==0): max = dArea
        size = 45 - (dArea / max * 100)

        if(dArea != 0):
            x = 320 - int(dM10 / dArea)
            y = 240 - int(dM01 / dArea)

        if(x>-10 and x < 10): x = 0
        if(size>-5 and size < 5): size = 0

    if(btn.backspace):
        size = 0
        x = 0
        break

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

Самое популярное