Carro Robô Autônomo de Rastreamento Facial com Arduino Nano e OpenCV

Componentes e Materiais

Componente	Quantidade
Creator Ci20 (placa computador)	1
Driver Dual H-Bridge L298 (SparkFun)	1
Bateria Li-Ion 1000mAh	1
Servos Tower Pro MG996R	2
Câmera (genérica - usei câmera de laptop velha)	1
Kit Base Robô GoPiGo (Dexter Industries)	1
Arduino Nano R3	1
Sensor Ultrassônico HC-SR04	1

Aplicativos e Serviços Online

• Arduino IDE
• OpenCV

Sobre o Projeto

É um carro que rastreia seu rosto: se eu me afasto, ele vem atrás!

Você precisa ter instalado no Ci20:

• OpenCV
• Python
• Correção de permissões porta serial

Usei distro Debian 8 com download.

Componentes utilizados:

• Câmera de laptop velha (reciclada)
• Módulo ultrassônico HC-SR04
• 2x Bateria 3.7V 4000mAh (reciclada de laptop)
• 2x Servo
• Conversor DC-DC
• Arduino Nano
• Kit base robô
• 2x L298D

Instalação no Linux (Ci20)

Atualize o sistema:

sudo apt-get update
sudo apt-get upgrade
sudo apt-get dist-upgrade

OpenCV (versão 2.4.9.1, não é a mais recente mas funciona):

sudo apt-get install libopencv-dev python-opencv

Para compilar OpenCV da fonte, este tutorial ajuda.

Instale PySerial:

sudo apt-get install python python-serial

Corrija permissões da porta serial (USB ou nativa):

sudo chown ci20:ci20 /dev/ttyUSB0  # Porta USB do Arduino
sudo chown ci20:ci20 /dev/ttyS1    # Porta nativa no Ci20

Execute o script Python (mude o 0 se tiver mais de uma fonte de vídeo):

sudo python facetrackingcar.py 0

Para melhorar performance, a imagem capturada não é exibida. Para ver, descomente estas linhas:

#cv.ShowImage("result", img)
#cv.NamedWindow("result", 1)
#cv.DestroyWindow("result")

Você precisa do arquivo haarcascade_frontalface_alt.xml no mesmo diretório do script.

Vídeo de demonstração apenas rastreamento câmera no Windows. Vídeo demo no Ci20. Feedback e melhorias são bem-vindos.

Código

faceser.py (Python - OpenCV Face Tracking)

#!/usr/bin/python
"""
Demonstração de detecção facial e objetos usando features haar-like.
Encontra faces em imagem de câmera ou stream de vídeo, mostra caixa vermelha,
centraliza webcam via dois servos para face ficar no centro da tela.
Baseado em facedetect.py nos samples do OpenCV.
"""
import sys
from optparse import OptionParser
import serial
import cv2.cv as cv
import time

# Parâmetros detecção haar
min_size = (20, 20)
image_scale = 2
haar_scale = 1.2
min_neighbors = 2
haar_flags = cv.CV_HAAR_DO_CANNY_PRUNING

max_pwm = 180
min_pwm = 1
midScreenWindow = 20  # 'erro' aceitável para centro da tela
panStepSize = 1       # graus de mudança por atualização pan
tiltStepSize = 1      # graus de mudança por atualização tilt
servoPanPosition = 90 # posição inicial pan (x)
servoTiltPosition = 45 # posição inicial tilt (y)
panGpioPin = 2        # ID servo pan no Arduino (NÃO número do pino)
tiltGpioPin = 1       # ID servo tilt no Arduino (NÃO número do pino)
var = 0

def detect_and_draw(img, cascade):
    gray = cv.CreateImage((img.width, img.height), 8, 1)
    small_img = cv.CreateImage((cv.Round(img.width / image_scale),
                                 cv.Round(img.height / image_scale)), 8, 1)
    cv.CvtColor(img, gray, cv.CV_BGR2GRAY)
    cv.Resize(gray, small_img, cv.CV_INTER_LINEAR)
    cv.EqualizeHist(small_img, small_img)
    midFace = None
    if cascade:
        t = cv.GetTickCount()
        faces = cv.HaarDetectObjects(small_img, cascade, cv.CreateMemStorage(0),
                                     haar_scale, min_neighbors, haar_flags, min_size)
        t = cv.GetTickCount() - t
        if faces:
            for ((x, y, w, h), n) in faces:
                pt1 = (int(x * image_scale), int(y * image_scale))
                pt2 = (int((x + w) * image_scale), int((y + h) * image_scale))
                cv.Rectangle(img, pt1, pt2, cv.RGB(255, 0, 0), 3, 8, 0)
                x1, x2 = pt1[0], pt2[0]
                y1, y2 = pt1[1], pt2[1]
                midFaceX = x1 + ((x2 - x1) / 2)
                midFaceY = y1 + ((y2 - y1) / 2)
                midFace = (midFaceX, midFaceY)
    return midFace

def move(servo, angle):
    """Move servo especificado para ângulo dado.
    servo: 1-4, angle: 0-180"""
    if min_pwm <= angle <= max_pwm:
        ser.write(chr(255))
        ser.write(chr(servo))
        ser.write(chr(angle))
    else:
        print("Ângulo servo deve ser inteiro entre 0 e 180.\n")

if __name__ == '__main__':
    ser = serial.Serial(port='/dev/ttyUSB0', baudrate=115200, timeout=1)
    # ser = serial.Serial(port='/dev/ttyS0', baudrate=115200, timeout=1)
    # ser = serial.Serial(port='COM14', baudrate=115200, timeout=1)

    parser = OptionParser(usage="usage: %prog [options] [camera_index]")
    parser.add_option("-c", "--cascade", action="store", dest="cascade",
                      type="str", help="Arquivo cascade Haar, default %default",
                      default="./haarcascade_frontalface_alt.xml")
    (options, args) = parser.parse_args()
    cascade = cv.Load(options.cascade)

    if len(args) != 1:
        parser.print_help()
        sys.exit(1)
    input_name = args[0]
    if input_name.isdigit():
        capture = cv.CreateCameraCapture(int(input_name))
        cv.SetCaptureProperty(capture, cv.CV_CAP_PROP_FRAME_WIDTH, 320)
        cv.SetCaptureProperty(capture, cv.CV_CAP_PROP_FRAME_HEIGHT, 240)
    else:
        print("Precisamos de entrada de câmera! Especifique índice ex: 0")
        sys.exit(0)

    # cv.NamedWindow("result", 1)
    if capture:
        frame_copy = None
        move(panGpioPin, servoPanPosition)
        move(tiltGpioPin, servoTiltPosition)
        while True:
            start = time.time()
            frame = cv.QueryFrame(capture)
            if not frame:
                cv.WaitKey(0)
                break
            if not frame_copy:
                frame_copy = cv.CreateImage((frame.width, frame.height),
                                            cv.IPL_DEPTH_8U, frame.nChannels)
            if frame.origin == cv.IPL_ORIGIN_TL:
                cv.Copy(frame, frame_copy)
            else:
                cv.Flip(frame, frame_copy, 0)

            midScreenX = frame.width / 2
            midScreenY = frame.height / 2
            midFace = detect_and_draw(frame_copy, cascade)

            if midFace is not None:
                midFaceX, midFaceY = midFace[0], midFace[1]

                # Pan: face à esquerda do centro -> move servo direita
                if midFaceX < (midScreenX - midScreenWindow):
                    servoPanPosition += panStepSize
                    print(f"{midFaceX} > {midScreenX} : Pan Right : {servoPanPosition}")
                # Face à direita -> move servo esquerda
                elif midFaceX > (midScreenX + midScreenWindow):
                    servoPanPosition -= panStepSize
                    print(f"{midFaceX} < {midScreenX} : Pan Left : {servoPanPosition}")
                else:
                    print(f"{midFaceX} ~ {midScreenX} : {servoPanPosition}")

                servoPanPosition = min(max(servoPanPosition, min_pwm), max_pwm)
                move(panGpioPin, servoPanPosition)

                # Tilt: face abaixo do centro -> abaixa tilt (se <= 90)
                if midFaceY < (midScreenY - midScreenWindow):
                    if servoTiltPosition <= 90:
                        servoTiltPosition -= tiltStepSize
                        print(f"{midFaceY} > {midScreenY} : Tilt Down : {servoTiltPosition}")
                # Face acima -> levanta tilt (se >= 1)
                elif midFaceY > (midScreenY + midScreenWindow):
                    if servoTiltPosition >= 1:
                        servoTiltPosition += tiltStepSize
                        print(f"{midFaceY} < {midScreenY} : Tilt Up : {servoTiltPosition}")
                    start = 1; end = 1
                else:
                    print(f"{midFaceY} ~ {midScreenY} : {servoTiltPosition}")

                servoTiltPosition = min(max(servoTiltPosition, min_pwm), max_pwm)
                move(tiltGpioPin, servoTiltPosition)
            else:
                end = time.time() + 0.1
                var += 0.1
                time_elapsed = int(end - start + var)
                print(f'time elapsed:\t{time_elapsed}')
                print(f'var:\t{var}')
                if time_elapsed == 20:
                    move(3, servoTiltPosition)
                    servoPanPosition = 90
                    servoTiltPosition = 45
                    var = 0
            if cv.WaitKey(1) >= 0:
                break
        # cv.DestroyWindow("result")

Car.ino (Arduino - Controle Motores e Servos)

#include 
#define MA_1 2
#define MA_2 3
#define MB_1 4
#define MB_2 5
#define MC_1 6
#define MC_2 7
#define MD_1 8
#define MD_2 9
#define SERVOX_PIN 11
#define SERVOY_PIN 10
#define trigPin 13
#define echoPin 12

Servo servoy;
Servo servox;
int x = 90, prevX;
int y = 45, prevY;
int userInput[3];
int startbyte;
int servo;
int pos;
int i;
int State = LOW;
unsigned long previousMillis = 0, val = 100;
const long interval = 100;
bool scana = false;
unsigned long currentMillis;

void setup() {
  inicializate();
  currentMillis = millis();
}

void loop() {
  if (Serial.available() > 2) {
    startbyte = Serial.read();
    if (startbyte == 255) {
      for (i = 0; i < 2; i++) {
        userInput[i] = Serial.read();
      }
      servo = userInput[0];
      pos = userInput[1];
      if (pos == 255) servo = 255;
      switch (servo) {
        case 1: servoy.write(pos); break;
        case 2: servox.write(pos); range(pos); break;
        case 3: scan(45); scan(65); break;
        default: break;
      }
    }
  }
}

void scan(int val) {
  while (Serial.available() == 0) {
    servoy.write(val);
    currentMillis = millis();
    if (currentMillis - previousMillis >= interval) {
      previousMillis = currentMillis;
      servox.write(pos);
      if (State == LOW) {
        pos += 1;
        if (pos == 130) State = HIGH;
      } else {
        pos -= 1;
        if (pos == 40) { State = LOW; break; }
      }
    }
  }
}

long distancia() {
  long duration, distance;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration / 2) / 29.1;
  return distance;
}

void range(int pos) {
  if (pos >= 80 && pos <= 100) moves();
  else if (pos >= 100 && pos <= 180) { left(); delay(10); Stop(); }
  else if (pos >= 1 && pos <= 80) { right(); delay(10); Stop(); }
}

void moves() {
  long distance, previusdistance;
  distance = distancia();
  if (val <= 80) { reverse(); delay(50); Stop(); }
  else if (val >= 140) { forward(); delay(50); Stop(); }
  if (distance >= previusdistance) val = distance - previusdistance;
  else val = previusdistance - distance;
  previusdistance = distance;
}

void inicializate() {
  Serial.begin(115200);
  pinMode(MA_1, OUTPUT); pinMode(MA_2, OUTPUT);
  pinMode(MB_1, OUTPUT); pinMode(MB_2, OUTPUT);
  pinMode(MC_1, OUTPUT); pinMode(MC_2, OUTPUT);
  pinMode(MD_1, OUTPUT); pinMode(MD_2, OUTPUT);
  pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT);
  servox.attach(SERVOX_PIN);
  servoy.attach(SERVOY_PIN);
}

void forward() {
  digitalWrite(MA_1, HIGH); digitalWrite(MA_2, LOW);
  digitalWrite(MB_1, HIGH); digitalWrite(MB_2, LOW);
  digitalWrite(MC_1, HIGH); digitalWrite(MC_2, LOW);
  digitalWrite(MD_1, HIGH); digitalWrite(MD_2, LOW);
}
void reverse() {
  digitalWrite(MA_1, LOW); digitalWrite(MA_2, HIGH);
  digitalWrite(MB_1, LOW); digitalWrite(MB_2, HIGH);
  digitalWrite(MC_1, LOW); digitalWrite(MC_2, HIGH);
  digitalWrite(MD_1, LOW); digitalWrite(MD_2, HIGH);
}
void right() {
  digitalWrite(MA_1, LOW); digitalWrite(MA_2, HIGH);
  digitalWrite(MB_1, LOW); digitalWrite(MB_2, HIGH);
  digitalWrite(MC_1, HIGH); digitalWrite(MC_2, LOW);
  digitalWrite(MD_1, HIGH); digitalWrite(MD_2, LOW);
}
void left() {
  digitalWrite(MA_1, HIGH); digitalWrite(MA_2, LOW);
  digitalWrite(MB_1, HIGH); digitalWrite(MB_2, LOW);
  digitalWrite(MC_1, LOW); digitalWrite(MC_2, HIGH);
  digitalWrite(MD_1, LOW); digitalWrite(MD_2, HIGH);
}
void Stop() {
  digitalWrite(MA_1, LOW); digitalWrite(MA_2, LOW);
  digitalWrite(MB_1, LOW); digitalWrite(MB_2, LOW);
  digitalWrite(MC_1, LOW); digitalWrite(MC_2, LOW);
  digitalWrite(MD_1, LOW); digitalWrite(MD_2, LOW);
}
void StopH() {
  digitalWrite(MA_1, HIGH); digitalWrite(MA_2, HIGH);
  digitalWrite(MB_1, HIGH); digitalWrite(MB_2, HIGH);
  digitalWrite(MC_1, HIGH); digitalWrite(MC_2, HIGH);
  digitalWrite(MD_1, HIGH); digitalWrite(MD_2, HIGH);
}

scanlinux.py (Python - Escaneia Portas Seriais Linux)

#! /usr/bin/env python
"""
Scan for serial ports. Linux specific variant that also includes USB/Serial adapters.
Part of pySerial (https://pyserial.sf.net) (C) 2009 
"""
import serial
import glob

def scan():
    """scan for available ports. return a list of device names."""
    return glob.glob('/dev/ttyS*') + glob.glob('/dev/ttyUSB*') + \
           glob.glob('/dev/ttyACM*') + glob.glob('/dev/serial/by-id/*')

if __name__ == '__main__':
    print("Found ports:")
    for name in scan():
        print(name)

Peças Personalizadas e Gabaritos

Script Python OpenCV face tracking e código Arduino.

FaceTrakingCar.rar

Esquemáticos

Usando Arduino para controlar motores e servos, pinout pode variar.