Введение
Лирика
Добрый день. Мотивированный многочисленными постами на Хабре о самодельных роботах решил сделать и что-нибудь свое более менее стоящее и интересное.
Вообще роботами я увлекаюсь давно, но до нормального проекта руки не доходили, в основном только игрался. Немного подумав, придумал свой проект, поискал детали, нарисовал наброски, пофантазировал на тему будущих возможностей робота. Детали заказал не небезызвестном сайте, и пока детали преодолевают путь из поднебесной решил реализовать один из модулей будущего робота из того что есть под рукой. Вернее даже не реализовать сам модуль, а собрать прототип и написать софт, чтобы потом не отвлекаться на написание программы, да и тем более пока идут все детали есть море свободного времени, а желание что-либо сделать, не дает покоя.
Под рукой у меня оказалась платка Arduino Diecimila, несколько сервоприводов, веб-камера, джойстик и ультразвуковой дальномер. Соответственно сразу возникло желание сделать «компьютерное зрение» на основе веб-камеры, с возможностью как автономной работы, так и ручного управления (джойстиком).
Что меня сподвигло написать эту статью?
Порывшись в интернете, я в основном находил всякий мусор, невнятные вопросы на форумах, отрывки из статей, немного отдаленных от потребностей. В общем и целом я не нашел хорошей, полноценной статьи, которая бы от начала и до конца описывала создание двигающейся веб-камеры, с примерами кода, а уж тем более совмещенные с дальномером и джойстиком.
Тогда решено было ничего больше не искать, так как времени на обработку статей и собирание во едино всей информации уходить стало больше, чем если делать все с нуля самому, тем более, что большинство статей уже давно устарело.
Задача ведь тривиальная, посылать информацию с джойстика на Arduino, которая на определенный угол будет поворачивать 2 сервопривода с прикрепленной веб-камерой, и по необходимости считывать информацию с дальномера, отсылая ее в SerialPort.
Обдумав все еще раз, решил приступить к созданию данного прототипа самостоятельно. Поехали!
Основная часть
Сборка прототипа
Прототип был создан в течение 5 минут. Внешний вид прототипа не интересует вообще, основная его цель — отработка программной части до приезда деталей для робота.
А сделал я его из первой попавшейся баночки из под каких-то витаминов, двух сервоприводов, веб-камеры, скрепки, изоленты и клеевого пистолета. Получилось следующее:
Сборка завершена, сервоприводы и ультразвуковой дальномер подключены к Arduino, Arduino к ПК, приступаем к программированию Arduino.
Программируем Arduino
Тут все казалось очень просто, так как джойстик подключается к ПК, основная обработка видео тоже будет на ПК, то Arduino займется лишь приемом и обработкой информации с ПК и управлением сервоприводами. Поэтому нам надо лишь читать Serial Port, обрабатывать каким-то образом поступающую информацию и как-то на нее реагировать.
Забегая немного вперед сразу скажу, тут и произошла ошибка, к которой мне пришлось вернуться уже после написания программы на C#. Ошибка была вот в чем — я, наивный и полный энтузиазма, написал программку которая разбирает поступающую в Serial Port строку примерно следующего вида «90:90» на две части, соответственно первая часть это градусы по координате X, вторая часть Y. При помощи монитора порта все было оттестировано и работало прекрасно, но когда была написана программа для управления с джойстика, при усиленной атаке порта строками с изменяющимися значениями, Arduino просто не успевала считывать все последовательно, поэтому зачастую строки превращались в «0:909», ":9090" и тому подобное.
Соответственно сервоприводы сходили с ума и принимали все положения, кроме тех, что нужны нам.
Поэтому, не долго думая, я пришел к выводу что нам нужен символ начала строки и символ конца строки. Опять же, не долго думая, символом начала строки был выбран первый символ латинского алфавита — «a», концом строки последний — «z», а символы начала значений осей «x» и «y» соответственно. Итого входная строка принимала следующий вид: «ax90y90z».
Все бы хорошо, если бы не дальномер. Дальномер ультразвуковой, расстояние он определяет на ура, но есть несколько нюансов. Во-первых, если угол между дальномером и стеной острее 45 градусов (плюс-минус), то звук отражается от стены по касательной, и значение, не соответствует действительности. Во-вторых довольно большой угол испускания сигнала, около 30 градусов(по мануалу), а замеряется расстояние до ближайшего объекта, благо что сигнал от объектов к которым датчик находится под углом, отражается в другую сторону, и мы получаем более менее реальное расстояние по прямой, но помехи все же бывают, и довольно часто. Поэтому я дописал еще одну функцию, которая берет n замеров расстояния, складывает их и делит на кол-во, выставил n=10, так помехи стали более сглажены и менее заметны.
Код на Arduino был тут же переписан и принял следующий вид:
#include <Servo.h> #include <String.h> /* Тут реализован алгоритм приема строки строка должна быть вида ax180y180z Где a - символ начала строки x - символ начала координат x y - символ начала координат y z - символ конца строки */ String str_X=""; String str_Y=""; int XY_Flag=0; // 1 = X, 2 = Y Servo X_Servo; Servo Y_Servo; const int distancePin = 12; const int distancePin2 = 11; void setup() { Serial.begin(115200); X_Servo.attach(7); Y_Servo.attach(8); } void loop() { delay(50); if(Serial.available()>0) //считываем значения из порта { int inChar=Serial.read(); //считываем байт if(inChar == 97) { // Если это начало строки while(Serial.available()>0) { inChar=Serial.read(); //считываем байт if(inChar==120){ // x XY_Flag=1; continue; } if(inChar==121){ // y XY_Flag=2; continue; } if(inChar==122){ // z (конец строки) XY_Flag=0; } if(XY_Flag==0) break; // Если конец строки, то досрочный выход из цикла if(XY_Flag==1) str_X +=(char)inChar; //если X, то пишем в X if(XY_Flag==2) str_Y +=(char)inChar; //Если Y, то пишем в Y } if(XY_Flag==0) // Если был конец строки, то выполняем... { servo(str_X.toInt(), str_Y.toInt()); str_X=""; str_Y=""; //очищаем переменные Serial.println("d" + String(trueDistance()) + "z"); } } } } void servo(int x, int y){ //говорим сервоприводам сколько градусов им нужно взять :) X_Servo.write(x); Y_Servo.write(y); } long trueDistance() //считываем датчик n раз и возвращаем среднее значение { int n=10; long _value=0; for(int i =0; i<n; i++) _value += distance(); return _value/n; } long distance() //считываем показания ультразвукового дальномера { long duration, cm; pinMode(distancePin, OUTPUT); digitalWrite(distancePin, LOW); delayMicroseconds(2); digitalWrite(distancePin, HIGH); delayMicroseconds(10); digitalWrite(distancePin, LOW); pinMode(distancePin, INPUT); duration = pulseIn(distancePin, HIGH); cm = microsecondsToCentimeters(duration); return cm; } long microsecondsToCentimeters(long microseconds) //переводим микросекунды в сантиметры { return microseconds / 29 / 2; }
Проблема с неправильным разбором координат исчезла на совсем, 100 из 100 испытаний пройдены успешно.
Основная управляющая программа (C#)
По началу хотел писать все на C++ под Qt, но в последствии все же пришлось писать на C#, ну да ладно.
Что хотелось получить:
1. Распознавание лиц людей.
2. Слежение за лицом человека.
3. Ручное управление с помощью джойстика.
4. Определение расстояния до объекта.
Для распознавания лиц и вывода изображения с веб-камеры, без всяких вопросов, была выбрана библиотека OpenCV, а вернее ее оболочка для C# — Emgu CV.
Для считывания положения джойстика по началу использовалась библиотека Microsoft.DirectX.DirectInput, которая мне жутко не понравилась, и я применил библиотеку SharpDX, притом довольно успешно.
Что требовалось от программы:
1. Захватывать изображение с веб-камеры и выводить его на экран.
2. Распознавать лица на изображении, обводить их и получать координаты лица на изображении.
3. Формировать строку вида «ax90y90z» и отправлять ее в Serial Port для управления сервоприводами.
4. Считывать значения положения джойстика.
5. Считывать показания с дальномера.
Сформулировав задачи, приступаем к программированию.
Библиотечка SharpDX позволяет нам находить подключенный джойстик и получать с него значения осей (от 0 до 65535), нажатие и отпускание клавиш джойстика. Сервоприводы могут поворачиваться от 0 до 180 градусов, соответственно нужно преобразовывать значения осей джойстика от 0 до 180. Я просто поделил возвращаемое значение на 363, и получил на выходе значения от 0 до 180. Далее написал функцию которая формирует строку положения сервоприводов и отправляет ее в порт.
Вывод изображения и распознавание лиц написаны с использованием OpenCV и ничего сложного не представляют (для нас).
Дальше поинтереснее, имея под рукой дальномер, конечно же захотелось сделать радар, и построить хоть какую-то приблизительную картину местности.
Повторив тригонометрию и вектора, написал процедуру, которая вычисляет координаты точки относительно нашего дальномера с камерой по углу поворота сервопривода и расстоянию до объекта, и рисует полученные результаты в PictureBox, по кнопке запускаю процедуру в потоке, все работает, но все же из за рельефа комнаты получаются довольно большие помехи, но примерное очертание совпадает с действительностью. Пытался сглаживать данные с датчика, выбирая лишь пиковые значения и рисуя между ними отрезки, в принципе получилось не плохо, но решил отказаться от этого, так как часто пиковыми значениями становятся именно помехи.
Код (на всякий случай с подробными комментариями, по возможности):
Capture myCapture; private bool captureInProgress = false; string _distance = "0"; string coords; int X_joy = 90; int Y_joy = 90; SerialPort _serialPort = new SerialPort(); Image<Bgr, Byte> image; DirectInput directInput; Guid joystickGuid; Joystick joystick; Thread th; private int GRAD_TURN_X = 2; private int GRAD_TURN_Y = 2; private void GetVideo(object sender, EventArgs e) { myCapture.FlipHorizontal = true; image = myCapture.QueryFrame(); try { // Image<Gray, Byte> gray = image.Convert<Gray, Byte>().Canny(100, 60); // CamImageBoxGray.Image = gray; } catch { } /*детектор лиц */ if (FaceCheck.Checked) { List<System.Drawing.Rectangle> faces = new List<System.Drawing.Rectangle>(); DetectFace.Detect(image, "haarcascade_frontalface_default.xml", "haarcascade_eye.xml", faces); foreach (System.Drawing.Rectangle face in faces) { image.Draw(face, new Bgr(System.Drawing.Color.Red), 2); int faceX = face.X + face.Width / 2; int faceY = face.Y + face.Height / 2; if ((faceX - 320 > 120) || (faceX - 320 < -120)) //Чем дальше от центра изображения лицо, тем быстрее двигаем камеру GRAD_TURN_X = 4; else if ((faceX - 320 > 80) || (faceX - 320 < -80)) GRAD_TURN_X = 3; else GRAD_TURN_X = 2; if ((faceY - 240 > 120) || (faceY - 240 < -120)) GRAD_TURN_Y = 4; else if ((faceY - 240 > 80) || (faceY - 240 < -80)) GRAD_TURN_Y = 3; else GRAD_TURN_Y = 2; label7.Text = faceX.ToString(); label8.Text = faceY.ToString(); if (!JoyCheck.Checked) { if (faceX > 370) X_joy += GRAD_TURN_X; else if (faceX < 290) X_joy -= GRAD_TURN_X; if (faceY > 270) Y_joy -= GRAD_TURN_Y; else if (faceY < 210) Y_joy += GRAD_TURN_Y; serialPortWrite(X_joy, Y_joy); } } } /*=============*/ System.Drawing.Rectangle rect1 = new System.Drawing.Rectangle(305, 240, 30, 1); System.Drawing.Rectangle rect2 = new System.Drawing.Rectangle(320, 225, 1, 30); System.Drawing.Rectangle rect3 = new System.Drawing.Rectangle(0, 0, 640, 22); image.Draw(rect1, new Bgr(System.Drawing.Color.Yellow), 1); image.Draw(rect2, new Bgr(System.Drawing.Color.Yellow), 1); image.Draw(rect3, new Bgr(System.Drawing.Color.Black), 22); MCvFont f = new MCvFont(FONT.CV_FONT_HERSHEY_TRIPLEX, 0.9, 0.9); image.Draw("Distance: " + _distance + " cm", ref f, new System.Drawing.Point(0, 30), new Bgr(0, 255, 255)); CamImageBox.Image = image; if (JoyCheck.Checked) { th = new Thread(joy); // ручное управление, запускаем в потоке th.Start(); } label1.Text = X_joy.ToString(); label2.Text = Y_joy.ToString(); label3.Text = coords; } private void ReleaseData() { if (myCapture != null) myCapture.Dispose(); } public Form1() { InitializeComponent(); } private void serialPortWrite(int X, int Y) //отсылаем ардуине координаты и читаем из порта дистанцию { try { coords = "ax" + X + "y" + Y + "z"; _serialPort.Write(coords); _distance = _serialPort.ReadLine(); if (_distance[0] == 'd') if (_distance[_distance.Length - 2] == 'z') { _distance = _distance.Remove(_distance.LastIndexOf('z')).Replace('d', ' '); } else _distance = "0"; else _distance = "0"; } catch { } } private void joy() //ручное управление джойстиком { joystick.Poll(); var datas = joystick.GetBufferedData(); foreach (var state in datas) { if (state.Offset.ToString() == "X") X_joy = 180 - (state.Value / 363); else if (state.Offset.ToString() == "Y") Y_joy = state.Value / 363; } serialPortWrite(X_joy, Y_joy); } private void Form1_Load(object sender, EventArgs e) { if (myCapture == null) { try { myCapture = new Capture(); } catch (NullReferenceException excpt) { MessageBox.Show(excpt.Message); } } if (myCapture != null) { if (captureInProgress) { Application.Idle -= GetVideo; } else { Application.Idle += GetVideo; } captureInProgress = !captureInProgress; } _serialPort.PortName = "COM3"; _serialPort.BaudRate = 115200; if (_serialPort.IsOpen) _serialPort.Close(); if (!_serialPort.IsOpen) _serialPort.Open(); directInput = new DirectInput(); joystickGuid = Guid.Empty; foreach (var deviceInstance in directInput.GetDevices(DeviceType.Gamepad, DeviceEnumerationFlags.AllDevices)) joystickGuid = deviceInstance.InstanceGuid; if (joystickGuid == Guid.Empty) foreach (var deviceInstance in directInput.GetDevices(DeviceType.Joystick, DeviceEnumerationFlags.AllDevices)) joystickGuid = deviceInstance.InstanceGuid; joystick = new Joystick(directInput, joystickGuid); joystick.Properties.BufferSize = 128; joystick.Acquire(); } private void JoyCheck_CheckedChanged(object sender, EventArgs e) { if (FaceCheck.Checked) FaceCheck.Checked = !JoyCheck.Checked; } private void FaceCheck_CheckedChanged(object sender, EventArgs e) { if (JoyCheck.Checked) JoyCheck.Checked = !FaceCheck.Checked; } private void RadarPaint() { Bitmap map = new Bitmap(pictureBox1.Size.Width, pictureBox1.Size.Height); Graphics g = Graphics.FromImage(map); var p = new Pen(System.Drawing.Color.Black, 2); System.Drawing.Point p1 = new System.Drawing.Point(); System.Drawing.Point p2 = new System.Drawing.Point(); System.Drawing.Point p3 = new System.Drawing.Point(); System.Drawing.Point p4 = new System.Drawing.Point(); p1.X = pictureBox1.Size.Width/2 ; //начало координат переводим в удобное нам место p1.Y = pictureBox1.Size.Height; //посередине pictureBox'a внизу for (int i = 0; i < 181; i++) { serialPortWrite(i, 90); p2.X = Convert.ToInt32(Math.Ceiling(320 + int.Parse(_distance) * Math.Cos(i * Math.PI / 180))); //считаем координаты точки p2.Y = Convert.ToInt32(Math.Ceiling(480 - int.Parse(_distance) * Math.Sin(i * Math.PI / 180))); if (i > 0) g.DrawLine(p, p2, p3); if (i % 18 == 0) { p4 = p2; p4.Y -= 50; g.DrawString(_distance, new Font("Arial", 18), new SolidBrush(System.Drawing.Color.Red), p4); } p3.X = p2.X; p3.Y = p2.Y; g.DrawLine(p, p1, p2); try { pictureBox1.Image = map; } catch (Exception e) { MessageBox.Show(e.Message); } } } private void button1_Click(object sender, EventArgs e) { if (FaceCheck.Checked || JoyCheck.Checked) { FaceCheck.Checked = false; JoyCheck.Checked = false; } Thread t = new Thread(RadarPaint); t.Start(); }
class DetectFace { public static void Detect(Image<Bgr, Byte> image, String faceFileName, String eyeFileName, List<Rectangle> faces) { CascadeClassifier face = new CascadeClassifier(faceFileName); // CascadeClassifier eye = new CascadeClassifier(eyeFileName); Image<Gray, Byte> gray = image.Convert<Gray, Byte>(); gray._EqualizeHist(); Rectangle[] facesDetected = face.DetectMultiScale( gray, 1.1, 5, new Size(70, 70), Size.Empty); faces.AddRange(facesDetected); } }
В итоге получаем все, что хотели. Компьютер распознает лица и автоматически следит за ними. ручное управление джойстиком работает на ура. Радар, хоть и не совсем точно, но работает. Основные функции модуля зрения робота отработаны и остается лишь дорабатывать и усовершенствовать их.
Видео
Вот, что получилось по завершении.
Заключение
Итоги
Оказалось все довольно просто. Цель достигнута, прототип готов. Есть над чем работать и заняться в свободное время, ожидая посылку с компонентами для робота.
Планы на будущее
Следующим шагом будет построение колесной платформы для робота, настройка удаленного управления (WiFi, 3G)., навешивание датчиков (температура, давление и прочее), синтез речи. В хотелках так же имеются планы по поводу механической руки.
Думаю, если будет интерес к данной статье и ее продолжению, то оно обязательно последует! Исправления и критика приветствуются!
Спасибо за внимание!
ссылка на оригинал статьи http://habrahabr.ru/post/198102/
Добавить комментарий