Autonomes Gesichtsverfolgungs-Robotercar mit Arduino Nano & OpenCV

Komponenten und Materialien

Komponente	Anzahl
Creator Ci20	1
SparkFun Dual H-Bridge Motortreiber L298	1
Li-Ion-Akku 1000 mAh	1
Servos (Tower Pro MG996R)	2
Kamera (Standard)	1
Dexter Industries GoPiGo Robot Base Kit	1
Arduino Nano R3	1
Ultraschallsensor HC-SR04	1

Apps und Onlinedienste

• Arduino IDE
• OpenCV

Über dieses Projekt

Ein Auto, das euer Gesicht verfolgt – wenn ich weggehe, kommt es hinterher!

Auf dem Ci20 müsst ihr installiert haben:

• OpenCV
• Python
• Serielle Port-Berechtigungen fixen

Genutzt: Debian 8, download

Verwendete Hardware:

• Alte Laptop-Kamera (recycelt)
• Ultraschall-Modul HC-SR04
• 2× Akku 3,7 V 4000 mAh (altes Laptop)
• 2× Servo
• DC-DC-Wandler
• Arduino Nano
• Robot-Base-Kit
• 2× L298D

Installation (Terminal)

Linux aktualisieren:

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

OpenCV (Version 2.4.9.1, nicht aktuell, aber funktionsfähig):

sudo apt-get install libopencv-dev python-opencv

Zum Selbstkompilieren: Tutorial

PySerial installieren:

sudo apt-get install python python-serial

Serielle Port-Berechtigungen setzen (USB oder nativ):

sudo chown ci20:ci20 /dev/ttyUSB0   # USB-Port vom Arduino
sudo chown ci20:ci20 /dev/ttyS1     # Nativer Port im Ci20

Python-Skript starten (0 = Kamera-Index, anpassen falls mehrere Quellen):

sudo python facetrackingcar.py 0

Leistungsoptimierung: Captured Image wird nicht angezeigt. Zum Anzeigen diese Zeilen einkommentieren:

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

Datei haarcascade_frontalface_alt.xml muss im Skript-Verzeichnis liegen.

Video: Nur Tracking-Kamera unter Windows
Demo-Video auf Ci20

Feedback und Verbesserungen willkommen.

Code

faceser.py Python

Python-Skript: OpenCV Gesichtserkennung & Servo-Steuerung

#!/usr/bin/python
"""
Demo für Gesichts- und Objekterkennung mit Haar-Features.
Findet Gesichter in Kamerabild/Video, zeichnet roten Rahmen,
zentriert Webcam via zwei Servos auf Gesichtsmitte.
Basiert auf facedetect.py aus OpenCV-Samples.
"""
import sys
from optparse import OptionParser
import serial
import cv2.cv as cv
import time

# Haar-Detection Parameter
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  # tolerierter „Fehler“ zur Bildmitte
panStepSize = 1       # Grad pro Pan-Update
tiltStepSize = 1      # Grad pro Tilt-Update
servoPanPosition = 90  # Start Pan (X)
servoTiltPosition = 45 # Start Tilt (Y)
panGpioPin = 2        # Arduino Pan-Servo ID (KEIN Pin-Number!)
tiltGpioPin = 1       # Arduino Tilt-Servo ID
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)
                # Gesichtsmmittelpunkt berechnen
                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):
    """Bewegt Servo auf Winkel. 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("Servo angle must be an integer between 0 and 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="string", help="Haar cascade file, 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("We need a camera input! Specify camera index e.g. 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
                # X-Richtung
                if midFaceX < (midScreenX - midScreenWindow):
                    servoPanPosition += panStepSize
                    print(f"{midFaceX} > {midScreenX} : Pan Right : {servoPanPosition}")
                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)

                # Y-Richtung
                if midFaceY < (midScreenY - midScreenWindow):
                    if servoTiltPosition <= 90:
                        servoTiltPosition -= tiltStepSize
                        print(f"{midFaceY} > {midScreenY} : Tilt Down : {servoTiltPosition}")
                elif midFaceY > (midScreenY + midScreenWindow):
                    if servoTiltPosition >= 1:
                        servoTiltPosition += tiltStepSize
                        print(f"{midFaceY} < {midScreenY} : Tilt Up : {servoTiltPosition}")
                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

#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, servox;
int x = 90, prevX;
int y = 45, prevY;
int userInput[3];
int startbyte, servo, pos, 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

Serielle Ports unter Linux scannen

#!/usr/bin/env python
"""
Scan for serial ports. Linux-Variante inkl. USB/Serial-Adapter.
Teil von pySerial (https://pyserial.sf.net)
(C) 2009 
"""
import serial
import glob

def scan():
    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)

Custom Parts & Enclosures

Python-Skript OpenCV Face Tracking & Arduino Code

FaceTrakingCar.rar

Schaltpläne

Arduino steuert Motoren & Servos, Pinout kann abweichen.