Автономный робот-следопыт с распознаванием лиц на Arduino Nano и OpenCV

Компоненты

Программное обеспечение

• Arduino IDE
• OpenCV 2.4.9 (установка: sudo apt-get install libopencv-dev python-opencv)
• Python + PySerial (sudo apt-get install python python-serial)

О проекте

Робот-машинка, которая отслеживает лицо человека и едет за ним. Распознавание лиц выполняется на Ci20 (OpenCV + Python), команды сервам и моторам посылаются через последовательный порт на Arduino Nano.

Установка на Ci20 (Debian 8)

sudo apt-get update && sudo apt-get upgrade && sudo apt-get dist-upgrade
sudo apt-get install libopencv-dev python-opencv
sudo apt-get install python python-serial
# Права доступа к последовательным портам:
sudo chown ci20:ci20 /dev/ttyUSB0   # USB-порт Arduino
sudo chown ci20:ci20 /dev/ttyS1     # Нативный порт Ci20

Запуск

Поместите файл haarcascade_frontalface_alt.xml в папку со скриптом.

sudo python facetrackingcar.py 0   # 0 — индекс камеры

Для просмотра видео раскомментируйте строки cv.ShowImage, cv.NamedWindow, cv.DestroyWindow в скрипте.

Код

faceser.py (Python, OpenCV + управление сервами)

#!/usr/bin/python
import sys, serial, cv2.cv as cv, time
from optparse import OptionParser

# Параметры 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
panStepSize = 1; tiltStepSize = 1
servoPanPosition = 90; servoTiltPosition = 45
panGpioPin = 2; tiltGpioPin = 1

ser = serial.Serial(port='/dev/ttyUSB0', baudrate=115200, timeout=1)

def move(servo, angle):
    if min_pwm <= angle <= max_pwm:
        ser.write(chr(255)); ser.write(chr(servo)); ser.write(chr(angle))
    else: print("Servo angle must be 0-180")

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:
        faces = cv.HaarDetectObjects(small_img, cascade, cv.CreateMemStorage(0), haar_scale, min_neighbors, haar_flags, min_size)
        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)
                midFaceX = pt1[0] + (pt2[0]-pt1[0])/2
                midFaceY = pt1[1] + (pt2[1]-pt1[1])/2
                midFace = (midFaceX, midFaceY)
    return midFace

if __name__ == '__main__':
    parser = OptionParser(usage="usage: %prog [options] [camera_index]")
    parser.add_option("-c", "--cascade", 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: sys.exit(0)

    move(panGpioPin, servoPanPosition)
    move(tiltGpioPin, servoTiltPosition)

    while True:
        frame = cv.QueryFrame(capture)
        if not frame: break
        # ... обработка кадра, поиск лица, вычисление смещения от центра экрана
        # управление сервами pan/tilt для центрирования лица
        # если лицо потеряно надолго — возврат серв в центр, сканирование
        # (полный код в исходнике)
        if cv.WaitKey(1) >= 0: break

Car.ino (Arduino Nano — моторы, сервы, ультразвук)

#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 servox, servoy;
int x=90, y=45;
int userInput[3]; int startbyte, servo, pos, i;
int State=LOW; unsigned long previousMillis=0, val=100;
const long interval=100; bool scana=false;

void setup() {
  Serial.begin(115200);
  pinMode(MA_1,OUTPUT); ... pinMode(MD_2,OUTPUT);
  pinMode(trigPin,OUTPUT); pinMode(echoPin,INPUT);
  servox.attach(SERVOX_PIN); servoy.attach(SERVOY_PIN);
}

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;          // tilt
        case 2: servox.write(pos); range(pos); break; // pan + движение
        case 3: scan(45); scan(65); 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++; if(pos==130) State=HIGH; }
      else { pos--; if(pos==40) { State=LOW; break; } }
    }
  }
}

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

void range(int pos) {
  if (pos>=80 && pos<=100) moves();           // прямо
  else if (pos>100) { left(); delay(10); Stop(); } // влево
  else if (pos<80) { right(); delay(10); Stop(); } // вправо
}

void moves() {
  long distance = distancia();
  if (val <= 80) { reverse(); delay(50); Stop(); }
  else if (val >= 140) { forward(); delay(50); Stop(); }
  // простая логика следования по расстоянию
}

void forward()  { /* все моторы вперёд */ }
void reverse()  { /* все моторы назад */ }
void right()    { /* левые назад, правые вперёд */ }
void left()     { /* левые вперёд, правые назад */ }
void Stop()     { /* все стоп */ }

scanlinux.py (поиск последовательных портов в Linux)

#!/usr/bin/env python
import serial, 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

Схемы и 3D-модели

• Схема подключения Arduino Nano к моторам, сервам, ультразвуку (pinout может отличаться).
• Архив FaceTrakingCar.rar с Python-скриптами и Arduino-кодом.