Autonome Gezichtsvolgende Robot Auto met Arduino Nano & OpenCV

Onderdelen en benodigdheden

Apps en online diensten

• Arduino IDE
• OpenCV

Over dit project

Het is een auto die je gezicht volgt, als ik weg ben, komt hij!

Je moet dit geïnstalleerd hebben (Ci20):

• OpenCV
• Python
• Seriële poort fix permissies

Ik gebruikte de distributie Debian 8 install & download

Ik gebruikte:

• Oude laptopcamera (gerecycled)
• Ultrasonische afstandsmodule HC - SR04
• 2x Batterij 3.7v 4000mah (gerecycled oude laptop)
• 2x Servo
• DC-DC converter
• Arduino nano
• Robot base kit
• 2x L298d

Voor installatie, gebruik de volgende commando's in de terminal.

Linux updaten

sudo apt-get update # Haalt de lijst met beschikbare updates op
sudo apt-get upgrade # Upgrade strikt de huidige pakketten
sudo apt-get dist-upgrade # Installeert updates (nieuwe)

OpenCV

Het is versie 2.4.9.1, niet up-to-date, maar het werkt prima.

sudo apt-get install libopencv-dev python-opencv

Als je OpenCV vanuit de bron wilt compileren, vond ik deze tutorial.

PySerial installeren

sudo apt-get install python python-serial

Seriële poort permissies wijzigen

sudo chown ci20:ci20 /dev/ttyUSB0 # Seriële poort USB van Arduino
sudo chown ci20:ci20 /dev/ttyS1   # Seriële poort native in Ci20

Python-script draaien

(verander de nul als je meer dan één videobron hebt, dit selecteert de bron)

sudo python facetrackingcar.py 0

Om de prestaties iets te verbeteren, wordt de vastgelegde afbeelding niet weergegeven. Als je wilt zien, moet je deze regels zoeken en uncommenten:

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

Je moet het haarcascade_frontalface_alt.xml bestand in dezelfde map hebben als het script.

Video voor alleen tracking-camera in Windows

Demovideo op Ci20

Feedback en verbeteringen zijn welkom.

Code

faceser.py

Python

Python-script OpenCV gezichtsvolging

#!/usr/bin/python
"""
Dit programma is een demonstratie voor gezichts- en objectdetectie
met haar-achtige kenmerken. Het programma vindt gezichten in een
camera-afbeelding of videostream en toont een rode box eromheen,
en centreert de webcam via twee servos zodat het gezicht in het
midden van het scherm zit.
Gebaseerd op facedetect.py in de OpenCV samples directory
"""
import sys
from optparse import OptionParser
import serial
import cv2.cv as cv
import time

# Parameters voor haar-detectie
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  # acceptabele 'fout' voor het midden van het scherm
panStepSize = 1       # graad verandering per pan-update
tiltStepSize = 1      # graad verandering per tilt-update
servoPanPosition = 90  # initiële pan-positie x
servoTiltPosition = 45 # initiële tilt-positie y
panGpioPin = 2        # arduino pan servo ID (GEEN pin-nummer)
tiltGpioPin = 1       # arduino tilt servo ID (GEEN pin-nummer)
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)
    # Kleur naar grijswaarden
    cv.CvtColor(img, gray, cv.CV_BGR2GRAY)
    # Schalen voor snellere verwerking
    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:
                # Input was geresized, dus schaal bounding box
                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)
                
                # Bereken midden van gezicht
                x1 = pt1[0]
                x2 = pt2[0]
                y1 = pt1[1]
                y2 = pt2[1]
                midFaceX = x1 + ((x2 - x1) / 2)
                midFaceY = y1 + ((y2 - y1) / 2)
                midFace = (midFaceX, midFaceY)
    
    return midFace

def move(servo, angle):
    """Beweeg de gespecificeerde servo naar de gegeven hoek.
    Argumenten:
    servo: het servo-nummer om te bevelen, integer 1-4
    angle: de gewenste servo-hoek, integer 0-180
    """
    if (min_pwm <= angle <= max_pwm):
        ser.write(chr(255))
        ser.write(chr(servo))
        ser.write(chr(angle))
    else:
        print "Servo-hoek moet een integer zijn tussen 0 en 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)
    
    # Parse command line opties, setup Haar classifier
    parser = OptionParser(usage="usage: %prog [options] [camera_index]")
    parser.add_option("-c", "--cascade", action="store", dest="cascade",
                      type="str", help="Haar cascade bestand, 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 hebben een camera-input nodig! Specificeer camera-index bijv. 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 = midFace[0]
                midFaceY = midFace[1]
                
                # Check of X-component van gezicht links van midden is
                if (midFaceX < (midScreenX - midScreenWindow)):
                    servoPanPosition += panStepSize
                    print str(midFaceX) + " > " + str(midScreenX) + " : Pan Right : " + str(servoPanPosition)
                # Check of X-component van gezicht rechts van midden is
                elif (midFaceX > (midScreenX + midScreenWindow)):
                    servoPanPosition -= panStepSize
                    print str(midFaceX) + " < " + str(midScreenX) + " : Pan Left : " + str(servoPanPosition)
                else:
                    print str(midFaceX) + " ~ " + str(midScreenX) + " : " + str(servoPanPosition)
                
                servoPanPosition = min(servoPanPosition, max_pwm)
                servoPanPosition = max(servoPanPosition, min_pwm)
                move(panGpioPin, servoPanPosition)
                
                # Check of Y-component van gezicht onder midden is
                if (midFaceY < (midScreenY - midScreenWindow)):
                    if (servoTiltPosition <= 90):
                        servoTiltPosition -= tiltStepSize
                        print str(midFaceY) + " > " + str(midScreenY) + " : Tilt Down : " + str(servoTiltPosition)
                # Check of Y-component van gezicht boven midden is
                elif (midFaceY > (midScreenY + midScreenWindow)):
                    if (servoTiltPosition >= 1):
                        servoTiltPosition += tiltStepSize
                        print str(midFaceY) + " < " + str(midScreenY) + " : Tilt Up : " + str(servoTiltPosition)
                    start = 1
                    end = 1
                else:
                    print str(midFaceY) + " ~ " + str(midScreenY) + " : " + str(servoTiltPosition)
                
                servoTiltPosition = min(servoTiltPosition, max_pwm)
                servoTiltPosition = max(servoTiltPosition, min_pwm)
                move(tiltGpioPin, servoTiltPosition)
            else:
                end = time.time() + 0.1
                var += 0.1
                time_elapsed = int(end - start + var)
                print 'time elapsed:\t{}'.format(time_elapsed)
                print 'var:\t{}'.format(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

// Gebruikersinput voor servo en positie
Servo servoy;
Servo servox;
int x = 90, prevX;
int y = 45, prevY;
int userInput[3]; // ruwe input van seriële buffer, 3 bytes
int startbyte;    // startbyte, begin met lezen input
int servo;        // welke servo te pulseren?
int pos;          // servo-hoek 0-180
int i;            // iterator
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() {
  // Wacht op seriële input (min 3 bytes in buffer)
  if (Serial.available() > 2) {
    // Lees eerste byte
    startbyte = Serial.read();
    // Als het echt de startbyte is (255)...
    if (startbyte == 255) {
      // ... dan krijg de volgende twee bytes
      for (i = 0; i < 2; i++) {
        userInput[i] = Serial.read();
      }
      // Eerste byte = welke servo te bewegen?
      servo = userInput[0];
      // Tweede byte = welke positie?
      pos = userInput[1];
      
      // Pakket foutcontrole en herstel
      if (pos == 255) {
        servo = 255;
      }
      
      // Wijs nieuwe positie toe aan juiste servo
      switch (servo) {
        case 1:
          servoy.write(pos); // beweeg servo1 naar '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);
    unsigned long 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 if (distance <= previusdistance) {
    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

Scan Linux seriële poorten

#! /usr/bin/env python
"""
Scan voor seriële poorten. Linux-specifieke variant die ook
USB/Seriële adapters omvat. Deel van pySerial (https://pyserial.sf.net)
(C) 2009 
"""
import serial
import glob

def scan():
    """Scan voor beschikbare poorten. Retourneer een lijst met apparaatnamen."""
    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

Aangepaste onderdelen en behuizingen

Python-script OpenCV gezichtsvolging en Arduino-code

Schema's

Arduino gebruiken om motoren en servos te besturen, pin-out kan variëren.