Điều khiển robot tự hành bằng số ngón tay

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN CƠ KHÍ BÀI TẬP LỚN ĐIỀU KHIỂN ROBOT TỰ HÀNH ĐỀ TÀI Điều khiển chuyển động và góc quay của xe tự hành theo số ngón tay Giảng viên TS Bùi Đình Bá Sinh viên thực hiện.

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

VIỆN CƠ KHÍ

BÀI TẬP LỚN ĐIỀU KHIỂN ROBOT TỰ HÀNH

ĐỀ TÀI:

Điều khiển chuyển động và góc quay của xe tự hành theo số ngón tay

Hà Nội 8/2021

Bùi Thế Sáng 20170883 CK.CĐT 05 – K62 Hoàng Nghĩa Lê Quân 20170866 CK.CĐT 05 – K62

MỤC LỤC

CHƯƠNG 1: TỔNG QUAN VỀ ĐỀ TÀI 1.1 Giới thiệu đề tài

- Robot tự hành ( mobile robot ) là một hướng phát triển của lĩnh vực robotic

mà trọng tâm là nghiên cứu chuyển động của robot trong một không gian nhất định Robot tự hành thực sự là một lĩnh vực riêng biệt kể từ cuối những năm 1960 bằng dự án Shakey tại SRI Báo cáo của N.J.Nilsson "A Mobile Automation: An Application of Artificial Intelligence Techniques" tại IJCAI

1969 đã đưa ra các yếu tố "nhận thức", "lập bản đồ", "lập kế hoạch đường đi" và các khái niệm về kiến trúc điều khiển Thập niên 1980 bùng nổ với các dự án về mobile robot và ngay lập tức đã cho thấy việc cần thiết để xét đến các thuộc tính của môi trường thực tế, vấn đề được bắt gặp trong các dự

án thiếu thực tế, thường cho rằng robot chỉ là sản phẩm của trí tuệ nhân tạo

- Trong vài năm trở lại đây robot tự hành đã phát triển vô cùng nhanh chóng Hàng loạt các nhà sản xuất oto và công ty công nghệ lớn đều nghiên cứu tính năng tự hành như: Tesla, Uber, Toyota, Waymo, Apple Ford, Daimler, PWM, Honda, Google…

Có thể thấy những lợi ích mà xe tự hành mang lại chính là tương lai của ngành ô tô

- Ngày nay với sự phát triển của máy tính và vi xử lý cũng như là các loại cảm biến đã giúp cho robot tự hành phát triển không ngừng Các tính năng của robot tự hành rất đa dạng : robot có thể di chuyển trong môi trường độc hại, địa hình phức tạp, có thể vẽ lại bản đồ của môi trường và truyền về bộ điều khiển trung tâm Robot có thể được điều khiển bằng vi xử lý hoặc máy tính nhúng; một hệ thống robot có thể được kiểm soát bởi một máy tính trung tâm qua mạng không dây Việc tương tác với môi trường bên ngoài có thể được thực hiện nhờ vào các cảm biến siêu âm, hồng ngoại hoặc camera Việc định hướng robot trong không gian được thực hiện bằng GPS hoặc la bàn số

- Trong đề tài này nhóm thực hiện điều khiển robot thông qua việc nhận diện hình ảnh bằng camera và gửi tín hiệu điều khiển của từng hình ảnh đến robot

từ máy tính thông qua mạng không dây

1.2 Công cụ thiết kế

1.2.1 Module ESP32

Module ESP32-WROOM-32

Sơ đồ ngoại vi module ESP32-WROOM-32

- ESP32-WROOM-32 là một module với nhiều tính năng cải tiến hơn các module dòng ESP8266 khi hỗ trợ thêm các tính năng Bluetooth và Bluetooth Low Energy (BLE) bên cạnh tính năng WiFi Sản phẩm sử dụng chip

ESP32-D0WDQ6 với 2 CPU có thể được điều khiển độc lập với tần số xung clock lên đến 240 MHz

- Module hỗ trợ các chuẩn giao tiếp SPI, UART, I2C và I2S và có khả năng kết nối với nhiều ngoại vi như các cảm biến, các bộ khuếch đại, thẻ nhớ (SD card),…

Thông số kĩ thuật

• Kích thước: 18 mm x 20 mm x 3 mm

• CPU: Xtensa Dual-Core 32-bit LX6 với tần số hoạt động lên đến 240 MHz

• Bộ nhớ trong:

 448 KBytes ROM cho booting và các tính năng của lõi chip

 520 KBytes SRAM trên chip dùng cho dữ liệu và các lệnh instruction

 8 KBytes SRAM trong RTC (gọi là RTC SLOW Memory) để truy xuất bởi các bộ co-processor

 8 KBytes SRAM trong RTC (gọi là RTC FAST Memory) dùng cho lữu

dữ liệu, truy xuất bởi CPU khi RTC đang boot từ chế độ Deep-sleep

 1 Kbit EFUSE, với 256 bit cho hệ thống (địa chỉ MAC và cấu hình chip), 768 còn lại cho ứng dụng người dùng, gồm cả mã hóa bộ nhớ Flash và định ID cho chip

• Kết nối WiFi:

 Wi-Fi: 802.11 b/g/n/e/i

 Bluetooth: BR/EDR phiên bản v4.2 và BLE

• Ethernet MAC hỗ trợ chuẩn: DMA và IEEE 1588

• Bus hỗ trợ mang CAN 2.0

• Giao tiếp ngoại vi:

 Bộ chuyển đổi ADC 12 bit, 16 kênh

 Bộ chuyển đổi 8-bits DAC: 2 kênh

 10 chân để giao tiếp với cảm biến chạm (touch sensor)

 IR (TX/RX)

 Ngõ ra PWM cho điều khiển Motor

 LED PWM: 16 kênh

 Cảm biến Hall

 Cảm biến nhiệt độ

 4 X SPI

 2 X I²S

 2 X I²C

 3 X UART

• Nhiệt độ hoat động ổn định: -40C đến 85C

• Điện áp hoạt động: 2.2-3.6V

• Dòng tiêu thụ ổn định: 80mA

Ứng dụng

• Module được dùng nhiều trong các ứng dụng thu thập dữ liệu và điều khiển thiết bị qua WiFi, Bluetooth

• Sử dụng cho các ứng dụng tiết kiệm năng lượng, điều khiển mạng lưới cảm biến, mã hóa hoặc xử lí tiếng nói, xử lí Analog-Digital trong các ứng dụng phát nhạc, hoặc vói các file MP3…

• Module cũng có thể dùng cho các thiết bị điện tử đeo tay như đồng hồ thông minh…

1.2.2 Động cơ servo sg90s

- Được điều khiển bằng cách điều chế độ rộng xung: Động cơ được điều khiển bằng tín hiệu PWM với chu kì 20ms (50Hz) Độ rộng xung thay đổi từ 1 ms đến 2 ms để có điều khiển góc quay động cơ – biến đổi từ 0 đến 180 độ

- Có 3 chân kết nối:

• Đỏ: Nguồn 5V

• Nâu: GND

• Cam: Điều khiển bằng PWM

CHƯƠNG 2: THIẾT KẾ ROBOT VÀ THUẬT TOÁN NHẬN DIỆN SỐ NGÓN

TAY ĐIỀU KHIỂN ROBOT 2.1 Thiết kế mô hình Robot

Sơ đồ nối dây mô hình Robot

- Tuy nhiên, do thiếu động cơ và L298N nên nhóm sử dụng 2 động cơ servo để điều khiển:

• 0: servo 2 quay 0 độ

• 1: servo 2 quay 90 độ

• 2: servo 2 quay 180 độ

• 4: servo 1 quay 180 độ

• 5: servo 1 quay 0 độ

• 3: servo 1 quay 90 độ

2.2 Xây dựng thuật toán nhận diện số ngón tay

- Các bước đếm ngón tay

• Bước 1: Nhận diện bàn tay:

 Sử dụng camera quay lại bàn tay trên nền trắng để loại bỏ nhiễu không mong muốn

 Sử dụng thư viện opencv để nhận diện bàn tay Cách nhận dạng: nhận dạng màu da người:

 Các hàm sử dụng:

o cvtColor: chuyển ảnh từ định dạng RGB sang định dạng HSV

o Sử dụng hàm inRange để giới hạn ngưỡng trong ảnh HSV và tìm ra ảnh nhị phân cho bàn tay

• Bước 2: Đếm số ngón tay:

 Tìm biên ngoài của hình dạng tay Sử dụng findContour() để tìm biên dạng ngoài của bàn tay Sau đó sử dụng lệnh ApproPoly để xấp xỉ hình dạng bàn tay

 Ý tưởng xác định số ngón tay: Các ngón tay là phần lồi lên của bàn tay

Và giữa 2 ngón sẽ có khoảng trống Từ các điểm lồi và khoảng trống, ta

có thể xác định được số ng

 convexHull(): Tìm các điểm lồi lên của bàn tay

 convexityDefect(): Tìm các phần khuyết của biên dạng khi so sánh với các điểm lồi tìm được ở lệnh convexHull

 Một khu vực khuyết giữa 2 ngón tay được biểu thị bằng ba điểm: điểm xanh lá, điểm xanh dương, điểm màu đỏ

• Bước 3 : truyền dữ liệu đến ESP 32

 Nếu số ngón tay đếm được không đổi sau khi xử lý 30 khung hình của video thì gửi dữ liệu đến esp32

 Gửi dữ liệu qua cổng serial port trên máy tính:

o CreatfileA(): Hàm tạo cổng kết nối đến cổng COM( cổng kết nối với esp32)

o WriteFile(): Gửi dữ liệu đến cổng COM kết nối với ESP32

o Chuẩn kết nối: Serial port profile

 Baudrate:115200

 Bytesize: 4

 Stopbit: 1

 Dữ liệu được gửi dưới dạng một chuỗi kí tự string

o Esp32 nhận dữ liệu từ máy tính

 Sử dụng thư viện BluetoothSeria.h và Arduino Ide để lập trình

o Sử dụng thư viện Esp32servo.h để điều khiển servo

 Điều khiển động cơ servo bằng chân D5

CHƯƠNG 3: CHƯƠNG TRÌNH LẬP TRÌNH CHO ROBOT

3.1 Chương trình xử lý ảnh

#include <iostream>

#include <Windows.h>

#include <opencv2/core.hpp>

#include <opencv2/imgproc.hpp>

#include <opencv2/imgcodecs.hpp>

#include <opencv2/videoio.hpp>

#include <opencv2/highgui.hpp>

usingnamespace std;

usingnamespace cv;

void PrintCommState(DCB dcb)

{

// In ra các giá trị cấu trúc DCB

wprintf( TEXT("\nBaudRate = %d, \nByteSize = %d, \nParity = %d,

\nStopBits = %d\n"),

dcb.BaudRate,

dcb.ByteSize,

dcb.Parity,

dcb.StopBits);

}

// Hàm tính cosin của góc từ ba điểm

double cosine1(Point p1,Point p2, Point pmid)

{

double dx1 = p1.x - pmid.x;

double dy1 = p1.y - pmid.y;

double dx2 = p2.x - pmid.x;

double dy2 = p2.y - pmid.y;

return (dx1 * dx2 + dy1 * dy2) / (sqrt(dx1 * dx1 + dy1 * dy1) + sqrt(dx2 * dx2 + dy2 * dy2));

}

int main()

{

/// <summary>

/// Tạo cổng serial

/// </summary>

/// <returns></returns>

HANDLE hSerial = nullptr;

{

cout << "Nhap cong COM ket noi: ";

string COM;

cin >> COM;

if (COM == "n") break;

string pc = "\\\\.\\COM6";

hSerial = CreateFileA(COM.c_str(),

GENERIC_READ | GENERIC_WRITE,

0,

0,

OPEN_EXISTING,

FILE_ATTRIBUTE_NORMAL,

0);

if (hSerial == INVALID_HANDLE_VALUE) {

if (GetLastError() == ERROR_FILE_NOT_FOUND) {

//Báo lỗi và mã lỗi

int d = (int)GetLastError();

cout << "serial not found - error code:" << d << endl;

}

//Báo lỗi không xác định

cout << " loi khong xac dinh" << endl;

}

} while (hSerial == INVALID_HANDLE_VALUE);

/// <summary>

/// Cấu hình kết nối Serial

/// </summary>

/// <returns></returns>

DCB dcb = { 0 };

dcb.DCBlength = sizeof(DCB);

if (!GetCommState(hSerial, &dcb))

{

cout << "loi dcb : -" << endl;

}

dcb.BaudRate = CBR_115200; //tốc độ truyền 115200bit

dcb.ByteSize = 4; //độ dài dữ liệu: 4 byte

dcb.StopBits = ONESTOPBIT; //Một bit dừng

dcb.Parity = NOPARITY;

if (!SetCommState(hSerial, &dcb))

{

cout << "khoi tao dcb that bai: -" << endl;

}

PrintCommState(dcb); // In ra thông tin cấu hình đường truyền

DWORD dwBytesRead = 0;

//Đường dẫn file

string path = "tay3.mp4";

Mat img;

//Đọc file video

VideoCapture cap(path);

Mat img_roi,mask,blur;

Mat kernel = getStructuringElement(MORPH_RECT, Size(1, 1), Point(-1, -1));

int current;

int sumfps = cap.get(CAP_PROP_FRAME_COUNT);

std::cout << "tong so khung hinh: " << sumfps << endl;

int old=0,second=0;

while (1)

{

cap >> img; // Đọc ảnh từ video

int count = 0; // Biến lưu số ngón tay

resize(img, img, Size(img.cols / 2, img.rows / 2));

img_roi = img.clone();

GaussianBlur(img_roi, img_roi, Size(7, 7), 1, 1);

// Chuyển ảnh sang định dạng HSV

cvtColor(img_roi, img_roi, COLOR_RGB2HSV_FULL);

vector<vector<Point>> contour;

vector<Vec4i> hier;

Scalar lower(144, 0, 0);

Scalar upper(255, 255, 255);

// Giới hạn các giá trị của ảnh để có ảnh nhị phân của bàn tay

inRange(img_roi, lower, upper, mask);

morphologyEx(mask, mask, MORPH_OPEN, kernel);

// Tìm biên dạng bàn tay

findContours(mask, contour, hier, RETR_EXTERNAL, CHAIN_APPROX_NONE);

if (contour.size() > 0)

vector<vector<Point>> appro(contour.size());

vector<vector<int>> hull(contour.size());

vector<vector<Point>> hullPoint(contour.size());

vector<vector<Vec4i>> defects(contour.size());

vector<vector<Point>> defectPoint(contour.size());

for (size_t i = 0; i < contour.size(); i++)

{

// Xấp xỉ hình dạng bàn tay thành đa giác approxPolyDP(contour[i], appro[i], 0.02 * arcLength(contour[i], true),

true);

if (contourArea(appro[i]) > 500)

{

// Tìm các điểm lồi lên convexHull(appro[i], hullPoint[i], true, true);

// Nối các điểm lồi lên cv::drawContours(img, hullPoint, i, Scalar(0, 0, 250));

// Tìm chỉ số của điểm lồi lên trong tập hợp điểm appro

convexHull(appro[i], hull[i], true,false);

if (hull[i].size() != 0)

{

// Tìm cá điểm khuyết của bàn tay convexityDefects(appro[i], hull[i], defects[i]);

if (defects[i].size() == 0) continue;

for (size_t k = 0; k < defects[i].size(); k++)

{

if (defects[i][k][3] > 30 * 256) {

int p_start = defects[i][k][0];

int p_end = defects[i][k][1];

int p_far = defects[i][k][2];

defectPoint[i].push_back(appro[i][p_far]);

// Tính cosin của góc tạo từ khu vực khuyết

if (cosine1(appro[i][p_start ], appro[i][p_end ], appro[i]

[p_far])>0)

count++;

}

}

if (count == 0 && defectPoint[i].size() >= 1) count++;

elseif (count > 0) count++;

//Neu so ngon tay duoc giu nguyen 30 khung hinh thi gui du lieu den esp32

if (old == count)

{

second++;// Đếm số khung hình nếu số ngón giữ nguyên }

else {

old = count; second = 0; // Nếu số ngón tay thay đổi, đếm số khung hình lại từ đầu }

if (second==30) // Nếu số khung hình đếm được bằng 30 thì gửi tín hiệu đến esp32 {

second = 0; string szBuff = to_string(old); //gui du lieu, neu khong gui duoc thi dung chuong trinh while (!WriteFile(hSerial, &szBuff[0], 4, &dwBytesRead, NULL)) { exit(-1); } cout << "send count -" << szBuff << endl; waitKey(500); }

}

}

}

std::cout << " so ngon tay: " << count << endl; //Ghi số ngón tay vào ảnh cv::putText(img, to_string(count), Point(50, 70), FONT_HERSHEY_SCRIPT_SIMPLEX, 3, Scalar(255, 100, 100), 1);

}

cv::imshow("mask", mask);

cv::imshow("img", img);

current = cap.get(CAP_PROP_POS_FRAMES);

if (current == sumfps)

{

cap.set(CAP_PROP_POS_FRAMES, 0); //Tạo vòng lặp cho video

}

if (waitKey(1) == 's')

{

waitKey(0);

} elseif (waitKey(1)=='q') break;

}

std::cout<<backend_name<<" " << "Hello World!\n";

}

3.2 Chương trình điều khiển ESP32

#include "BluetoothSerial.h"

#include <ESP32Servo.h>

#if !defined(CONFIG_BT_ENABLED) || !

defined(CONFIG_BLUEDROID_ENABLED)

#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it

#endif

BluetoothSerial SerialBT;

Servo myservo;

int servoPin=5;

int pinServo2=18;

String data;

int len;

int al=90;

void setup() {

SerialBT.begin("ESP32test"); //tên Bluetooth

pinMode(2, OUTPUT); //đèn báo nhận dữ liệu

myservo.setPeriodHertz(50); // servo hoạt động với tần số 50Hz

myservo.attach(servoPin); //Khai báo chân diều khiển servo

servo2.setPeriodHertz(50); // tần số 50hz servo

servo2.attach(pinServo2); //Khai báo chân diều khiển servo

void loop() {

while (SerialBT.available()>0)

{

digitalWrite(2,HIGH); //Đèn báo nhận dữ liệu data=SerialBT.readString(); //đọc dữ liệu từ máy tính al=(int)data.toInt(); //Chuyển sang kiểu dữ liệu int digitalWrite(2,LOW); //Đèn tắt khi nhận xong dữ liệu SerialBT.flush();

}

while (SerialBT.available()==0) //Không có tín hiệu gửi

{

switch (al)

case 0:{ //servo 2 quay 0 độ

{

myservo.write(0);

break;

}

case 1: //servo 2 quay 90 độ

{

myservo.write(90);

break;

}

case 2: //servo 2 quay 180 độ

{

myservo.write(180);

break;

}

case 3: //servo 1 quay 0 độ

{

servo2.write(0);

break;

}

case 4: //servo 1 quay 90 độ

{

servo2.write(90);

break;

}

case 5: //servo 1 quay 180 độ

{

servo2.write(180);

break;

}

}

}