3.2.1. Thiết kế hệ thống điều khiển mobie robot
Hệ thống điều khiển mobie robot gồm như hình gồm 2 module chính:
• Module truyền dữ liệu gồm 1 board Arduino và 1 chip NRF24L01+.
Chương trình xử lý ảnh sẽ gửi dữ liệu tới mobile robot qua module truyền dữ liệu.
Hình 3.1. Module truyền dữ liệu
34 ▪ 1 chip Arduino ▪ nguồn 9v ▪ 2 động cơ motor ▪ module L298 ▪ 1 module NRF24L01+.
Sau khi nhận được dữ liệu thì module L298 sẽ điều khiển 2 động cơ.
35
❖ Cách đấu nối: NRF24L01+với arduino uno r3
Cách đấu nối, các chân của chip NRF24L01+ lần lượt đc đấu với các chân của board Arduino như sau:
MISO-chân 12, SCK-chân 13, CE-chân 7, GND- GND, MOSI-chân số 11,CS-chân 8, VCC-3V3.
❖ Cách đấu nối: L298 với arduino uno
Cách đấu nối, các chân của chip L298 lần lượt đc đấu với các chân của board Arduino như sau: ENA- chân 9,IN1-chân A0, IN2-chân A1,IN3 -chân A2,IN4- chân A3, ENB- chân 10, chân 5v- Vin.
36
Hình 3.4. Bánh xe đa hướng
3.2.2. Tính toán cơ khí 3.2.2.1 Tính chọn bánh xe 3.2.2.1 Tính chọn bánh xe
Mobile robot được dẫn động bằng hai bánh chủ động và một bánh dẫn hướng, trong đó hai bánh bánh chủ động được sử dụng loại bánh xe robot V3 85mm. Vì chất lượng loại bánh này rất tốt, giá rẻ, dễ lắp ráp và ứng dụng vào thiết kế loại robot nhỏ. Bánh xe Robot 85mm thường được sử dụng kèm với khớp nối lục giác để có thể gắn với động cơ. Lốp được làm bằng cao su chất lượng cao, hệ số ma sát lớn, khả năng chịu tải rất tốt và khả năng bám đường rất tốt.
Và bánh xe dẫn hướng được lựa chọn là loại bánh xe đa hướng đường kính 40mm dày 20mm. Kết cấu giá đỡ của bánh xe đa hướng có thiết kế 4 vị trí gắn ốc để chúng ta có thể gắn ốc mà cố định với khung sườn của mô hình robot .Còn phần dưới của bánh xe được thiết kế giúp bánh xe có thể xoay, di chuyển linh động đa hướng để điều hướng cho robot . Dưới đây là ảnh của bánh xe sử dụng:
37
Hình 3.5. Sơ đồ phân tích lực
3.2.2.2 Tính chọn động cơ
Các tiêu chí lựa chọn động cơ cho Robot di động: Công suất động cơ, tốc độ, kích thước, cân nặng sao cho phù hợp với mô hình xe. Giả thiết bỏ qua sự biến dạng của bánh xe và lực cản của không khí trong quá trình di chuyển.
Các thông số của robot là:
• Khối lượng ước tính 1-2 (kg)
• Khối lượng tối đa 2 (kg)
• Đường kính bánh xe 2r=85 (mm)
• Tốc độ tối đa của xe 1,2 (m/s)
• Tỉ số bộ truyền ước tính là 1:1
Trong đó :
• M1,M2 là momen của động cơ tác động lên các bánh (N/m);
• N1,N2 là phản lực của mặt sàn (N); (adsbygoogle = window.adsbygoogle || []).push({});
• P1, P2 là trọng lực tác động vào các bánh xe (N).
• Fms1, Fms2 là lực ma sát sinh ra trong quá trình chuyển động (N);
• Fw1, Fw2 là lực ma sát sinh ra trong quá trình chuyển động (N);
38 Gỉa sử chuyển động của robot là chuyển động thẳng. Tốc độ quay của bánh xe là:
𝑛 = 60.1000𝑣
2𝜋𝑟 = 60.1000.1,2
85.π = 269,63(𝑟𝑝𝑚)
Tốc độ quay của động cơ qua bộ truyền là :
𝑛𝑑𝑐 = 269,63.1
1= 269,63(𝑟𝑝𝑚)
Phương trình cân bằng lực tác động lên một bánh: 𝐹𝑤1
⃗⃗⃗⃗⃗⃗⃗⃗ +𝐹𝑚𝑠1⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗⃗ + 𝑁1⃗⃗⃗⃗⃗⃗ +𝑃1⃗⃗⃗⃗⃗⃗ = 𝑚1. 𝑎
Với a là gia tốc của xe, khi xe chuyển động đề vận tốc không đổi với v=1,2
(m/s), a=0 (m/s2). Khi đó:
𝑁1 = 𝑚𝑔
3 =2.9,8
3 = 6,53(𝑁)
Lực kéo sinh ra do momen của bánh động lực là: 𝐹𝑤1 = 𝐹𝑀𝑆1 = 𝜇. 𝑁1 = 0,05.6,53 = 0.33(𝑁)
(Với hệ số ma sát lăn 𝜇𝑙 = 0,05 khi xe di chuyển trên sàn bê tông)
Do các con lăn được bố trí 45 độ so với bánh xe nên lực kéo sinh ra do moment của bánh động lực khi chuyển động chéo là:
𝐹𝑑𝑐 = 0,33
cos 45° = 0,47(𝑁) Lực kéo của động cơ qua bộ truyền là:
𝐹𝑑𝑐 = 0,47.1
1 = 0,47(𝑁)
Công suất cần thiết để xe di chuyển ổn định với vận tốc 1,2 (m/s) là: 𝑃1 = 𝐹𝑑𝑐. 𝑣 = 0,47.1,2 = 0,56(𝑤)
− Khi xe tăng tốc: Xe tăng tốc với gia tốc 𝑎 = 0,7 𝑚/𝑠2 , lực kéo sinh ra do momen của bánh động lực là:
𝐹𝑤1 = 𝐹𝑀𝑆1 + 𝑚1. 𝑎 = 𝜇. 𝑁1+ 𝑚1. 𝑎 = 0,8.6,53 +2
39 Với hệ số ma sát tĩnh 𝜇𝑛 = 0,8. Do các con lăn được bố trí 45 độ so với bánh xe nên lực kéo sinh ra do moment của bánh động lực khi chuyển động chéo là:
𝐹𝑑𝑐 = 5,7
cos 45° = 8,06(𝑁) (adsbygoogle = window.adsbygoogle || []).push({});
Lực kéo cần thiết của động cơ qua bộ truyền là: 𝐹𝑑𝑐 = 8,06.1
1 = 8,06(𝑁)
Momen xoắn cần thiết của động cơ để xe tăng tốc là: 𝑀1=𝐹𝑑𝑐. 𝑟 = 8,06.0,0425 = 0,342(𝑁𝑚)
Công suất cần thiết của động cơ để xe tăng tốc lên 1,2 (m/s) là: 𝑃1 = 𝐹𝑑𝑐. 𝑣 = 8,06.1,2 = 9,67(𝑤)
Từ kết quả trên suy ra momen xoắn cần thiết của động cơ là 0,342(N.m) và
công suất động cơ cần thiết là 9,67(W). Vì vậy chọn động cơDC Servo Giảm Tốc
GA25 sử dụng điện áp 12 (VDC) có công suất 13,2 (W), tốc độ không tải qua hộp giảm tốc là 320 rpm.
40
Hình 3.2.4 Động cơ sử dụng
3.2.2.3 Phân tích chân của hệ thống điều khiển và truyền nhận tín hiệu 3.2.2.4 Arduino Uno 3.2.2.4 Arduino Uno
Arduino UNO có 14 chân digital dùng để đọc hoặc xuất tín hiệu.
• 2 chân Serial: 0 (RX) và 1 (TX): dùng để gửi (transmit – TX) và nhận (receive – RX) dữ liệu TTL Serial.
• Chân PWM (~): 3, 5, 6, 9, 10, và 11: cho phép bạn xuất ra xung PWM • Chân giao tiếp SPI: 10(SS),11(MOSI), 12 (MISO), 13 (SCK). • LED 13: Khi bấm nút Reset, bạn sẽ thấy đèn này nhấp nháy để báo
hiệu. Nó được nối với chân số 13.
Arduino UNO có 6 chân analog (A0 → A5) cung cấp độ phân giải tín hiệu
10bit (0 → 210-1) để đọc giá trị điện áp trong khoảng 0V → 5V. Với
chân AREF trên board, bạn có thể để đưa vào điện áp tham chiếu khi sử dụng các
chân analog.
Đặc biệt, Arduino UNO có 2 chân A4 (SDA) và A5 (SCL) hỗ trợ giao tiếp I2C/TWI với các thiết bị khác
41
3.2.2.5 Module NRF24L01
Hình 3.8. Module NRF24L01
• Chân CE ( Pin chọn mode TX hoặc RX ) : Ngõ vào số
• Chân CSN ( Pin chọn chip SPI ) : Ngõ vào số
• Chân SCK ( Pin clock SPI ) : Ngõ vào số
• Chân MOSI ( SPI Slave Data input ) : Ngõ vào số
• Chân MISO ( SPI Slave Data output, với 3 lựa chọn ) : Ngõ ra số
• Chân IRQ ( Pin kết nối ngắt, tích cực mức thấp ) : Ngõ ra số
3.2.2.6 Module L298
Hình 3.9. Module L298 (adsbygoogle = window.adsbygoogle || []).push({});
• Chân12V Power cấp nguồn cho mạch L298 và là nguồn động lực
42
• Chân5V Power có thể dùng cấp nguồn cho Arduino, khi có Jumper
5V Enable.
• Chân GND là chân cấp MASS cho mạch, khi sử dụng khi vi điều
khiển thì cần nối GND mạch với GND của vi điều khiển.
• Chân Enable là chân cho phép ngỏ ra động cơ hoạt động hoặc
dừng. Mặc định mạch có Jumper A Enable va B Enable như hình là cho phép chạy.
• Chân IN1, IN2 điều khiển chiều và tốc độ động cơ 1 thông qua ngỏ
ra output A.
• Chân IN3, IN4 điều khiển chiều và tốc độ động cơ 2 thông qua ngỏ
ra output B.
• Chân output A, output B chân ngỏ ra động cơ 1, 2.
3.2.3. Thiết lập sơ đồ tín hiệu giao tiếp vs mobie robot
Hình 3.10. Sơ đồ tín hiệu giao tiếp với mobie robot
Chương trình xử lý ảnh trên máy tính sẽ truyền dữ liệu xuống module phát là board Arduino thông qua giao tiếp Serial. Đây là giao tiếp nối tiếp với tốc độ Baud mặc định 9600, đã được tích hợp sẵn trên Arduino.
Hai module NRF24L01 có khả năng truyền nhận hai chiều, giao tiếp qua sóng radio ở tần số 2.4 GHz
Các board Arduino kết nối với module NRF24L01 qua giao tiếp SPI. Đây là một loại giao tiếp nối tiếp đồng bộ khoảng cách ngắn thường được dùng trong hệ thống nhúng. Các bước để giao tiếp giữa board Arduino và module NRF24L01 như sau:
43
• Thiết lập chân giao tiếp
• Tạo mảng dữ liệu
• Khởi tạo địa chỉ giao tiếp
• Gán địa chỉ giao tiếp
• Thiết lập mức khuếch đại
• Thiết lập module NRF24L01 là module nhận hay phát dữ liệu +
Truyền, nhận dữ liệu.
Camera của mobie robot giao tiếp với hệ thống sử lý ảnh của robot qua kết nối wifi.
3.3.Xây dựng và đo khoảng cách từ người đến camera
3.3.1. Tính khoảng cách từ Camera đến một đối tượng với OpenCV
Trong đó (adsbygoogle = window.adsbygoogle || []).push({});
• a1 : Độ dài của người trong mẫu.
• h1 : Độ dài thật của con người.
• a2 : Độ dài của người trong ảnh cần đo khoảng cách.
• d1 : Khoảng cách từ người đến camera.
• d : Khoảng cách từ người đến camera của ảnh cần đo.
Để xác định khoảng cách từ máy ảnh của chúng tôi đến một vật thể hoặc điểm đánh dấu đã biết, chúng tôi sẽ sử dụng tính tương tự của tam giác .
44 Sự đồng dạng của tam giác diễn ra như sau: Giả sử chúng ta có một điểm đánh dấu hoặc đối tượng có chiều rộng W đã biết . Sau đó, chúng tôi đặt điểm đánh dấu này cách máy ảnh của chúng tôi một khoảng D. Chúng tôi chụp ảnh của đối tượng của chúng tôi sử dụng máy ảnh của chúng tôi và sau đó đo chiều rộng bề mặt bằng pixel P . Điều này cho phép chúng tôi tính được tiêu cự F cảm nhận được của máy ảnh:
𝐹 = (𝑃 × 𝐷)/𝑊
Ví dụ: giả sử tôi đặt một mảnh giấy tiêu chuẩn có kích thước 8,5 x 11in (theo chiều ngang; W = 11 ) D = 24 inch trước máy ảnh và chụp ảnh. Khi tôi đo chiều rộng của mảnh giấy trong hình ảnh, tôi nhận thấy rằng chiều rộng cảm nhận được của tờ giấy là P = 248 pixel .
Tiêu cự F của tôi sau đó là:
𝐹 = 248 × 24
11 = 543,45
Khi tôi tiếp tục di chuyển máy ảnh của mình cả gần và xa hơn đối tượng / điểm đánh dấu, tôi có thể áp dụng phép đồng dạng tam giác để xác định khoảng cách của đối tượng đến máy ảnh:
𝐷′ = (𝑅ộ𝑛𝑔 × 𝐹)/𝑃
Một lần nữa, để làm cho điều này cụ thể hơn, giả sử tôi di chuyển máy ảnh của mình ra xa điểm đánh dấu 3 ft (hoặc 36 inch) và chụp ảnh cùng một mảnh giấy. Thông qua xử lý hình ảnh tự động, tôi có thể xác định rằng chiều rộng cảm nhận của mảnh giấy hiện là 170 pixel . Cắm cái này vào phương trình, bây giờ chúng ta nhận được:
𝐷′ =11 × 543,45
170 = 35(𝑖𝑛𝑐ℎ)
Hoặc khoảng 36 inch, là 3 feet.
Lưu ý: Khi tôi chụp các bức ảnh cho ví dụ này, thước dây của tôi bị chùng
một chút và do đó kết quả bị lệch đi khoảng 1 inch. Hơn nữa, tôi cũng chụp các bức ảnh một cách vội vàng và không 100% trên các điểm đánh dấu chân trên
45
thước dây, điều này làm tăng thêm sai số 1 inch. Điều đó đã nói lên rằng, sự tương đồng của tam giác vẫn được giữ nguyên và bạn có thể sử dụng phương pháp này để tính toán khoảng cách từ một đối tượng hoặc điểm đánh dấu đến máy ảnh của bạn một cách khá dễ dàng.
3.4.Xây dựng giới hạn của đối tượng trong khung ảnh để robot hoạt động 3.4.1. Giới hạn trái, phải và khoảng cách của đối tượng
Hình 3.17. Giới hạn trái, phải
Khung ảnh có chiều rộng 640*480 pixel. Chúng tôi đặt giới hạn bên trái và phải cách lề 150 pixel. Đây là giới hạn của đối tượng trong khung ảnh để robot có thể di chuyển.
Chúng tôi còn xây dựng khoảng cách trong khoảng 170-220 cm.
3.4.2. Robot di chuyển từ điều kiện giới hạn
• Người nằm trong khoảng cách 170<D<=220 : robot đứng im
• Người nằm trong khoảng cách D>220 : đi thẳng
• Người nằm trong khoảng cách D<=170 : đi lùi (adsbygoogle = window.adsbygoogle || []).push({});
• Người vượt qua giới hạn phải thì: quay phải
46
3.5.Phần mềm Iriun Webcam 3.5.1. Giới thiệu 3.5.1. Giới thiệu
Iriun là cách đơn giản để biến điện thoại Android thành webcam nếu bạn đang sử dụng PC và máy tính của bạn không được trang bị webcam. Cách làm việc của Iriun chính là dùng máy ảnh điện thoại của bạn làm webcam qua mạng wifi trong nền tảng PC hoặc máy Mac.
Chúng tôi sử dụng phần mềm này để đặt điện thoại làm webcam của robot cho xử lý ảnh, dữ liệu hình ảnh được gửi về máy tính.
Iriun cần được cài đặt trên cả PC và máy điện thoại của bạn, hỗ trợ độ phân
giải hình ảnh cao. Cách sử dụng webcam như sau: khởi chạy ứng dụng Iriun Webcam trong điện thoại của bạn, khởi động Iriun Webcam Server trên PC, điện thoại sẽ được tự động kết nối với PC của bạn bằng mạng WiFi không dây và máy ảnh đã sẵn sàng để sử dụng, định cấu hình ứng dụng Windows để sử dụng Iriun Webcam làm nguồn cho âm thanh và video.
3.5.2. Các đặc điểm chính của Iriun:
- Sử dụng máy ảnh Android của bạn làm webcam cho PC. - Hỗ trợ độ phân giải hình ảnh cao.
- Hỗ trợ kết nối USB.
47
3.6.Lập trình điều khiển hệ thống 3.6.1. Chương trình trên Python 3.6.1. Chương trình trên Python
Ta thêm những thư viện cần thiết
import cv2 as cv import numpy as np
import serial # giao tiep ardunio import time
Code yolov4 để detect object ở đây ta cài đặt chỉ detect người va chỉ detect 1 người:
# nguong so sanh
CONFIDENCE_THRESHOLD = 0.4 # do tien cay NMS_THRESHOLD = 0.3
KNOWN_DISTANCE = 202 #cm khoang cach tu camera den nguoi cua anh mau
PERSON_WIDTH = 163 #cm chieu cao that cua ngươi # mau cho doi tuong
COLORS = [(255,0,0),(255,0,255),(0, 255, 255), (255, 255, 0), (0, 255, 0), (255, 0, 0)] GREEN =(0,255,0) - BLACK =(0,0,0) - YELLOW =(0,255,255) - PERPEL = (255,0,255) - WHITE = (255,255,255) - FONTS = cv.FONT_HERSHEY_COMPLEX - # doc class tu file txt
- class_names = []
- with open("classes.txt", "r") as f:
- class_names = [cname.strip() for cname in f.readlines()]
- yoloNet = cv.dnn.readNet('yolov4-tiny.weights', 'yolov4-tiny.cfg') #doc file weight
48
- #su dung gpu
- yoloNet.setPreferableBackend(cv.dnn.DNN_BACKEND_CUDA) - def object_detector(image):
- classes, scores, boxes = model.detect(image, CONFIDENCE_THRESHOLD, NMS_THRESHOLD)
- # tao tep du lieu doi tương - data_list = [] (adsbygoogle = window.adsbygoogle || []).push({});
- for (classid, score, box) in zip(classes, scores, boxes):
- if classid == 0: # id cua class nguoi la 0 chi phat hien vat the la nguoi - # mau cho doi tuong
- color = COLORS[int(classid) % len(COLORS)] -
- label = "%s : %f" % (class_names[classid[0]], score) -
- # ve hcn tren lable
- cv.rectangle(image, box, color, 2) -
- cv.putText(image, label, (box[0], box[1] - 14), FONTS, 0.5, color, 2) -
- # luu du lieu
- # 0: ten class 1: chieu cao cua nguoi trong anh, 2: vi tri ve text , 3 tao do x1 cua box, 4 toa do x2 cua box
- # person class id
- data_list.append([class_names[classid[0]], box[3], (box[0], box[1] - 2),box[0],box[2]])
- break #ta dung break de chi detect 1 vat the - return data_list
49
# ham tim tieu cu f cua may anh (f la hang so khong doi)
def focal_length_finder (measured_distance, real_width, width_in_rf): focal_length = (width_in_rf * measured_distance) / real_width
return focal_length
Hàm tìm khoảng cách từ F
# ham tim kc tu camera den nguoi
def distance_finder(focal_length, real_object_width, width_in_frame): distance = (real_object_width * focal_length) / width_in_frame return distance
Đọc 4 ảnh mẫu và f trung bình của 4 ảnh đó: - # tim chieu cao trong anh tu cac anh mau
- # ta doc anh mau va tim f trung binh cua 4 anh khac nhau co kc tu nguoi den camera lan luot(200-240-280-320) cm
- # anh mau 1 - ref_person_1 = cv.imread('anhmau/image200.png') - person_data_1 = object_detector(ref_person_1) - person_width_in_rf_1 = person_data_1[0][1] - # anh mau 2 - ref_person_2 = cv.imread('anhmau/image240.png') - person_data_2 = object_detector(ref_person_2) - person_width_in_rf_2 = person_data_2[0][1] - # anh mau 3
50 - ref_person_3 = cv.imread('anhmau/image280.png') - person_data_3 = object_detector(ref_person_3) - person_width_in_rf_3 = person_data_3[0][1] - # anh mau 4 - ref_person_4 = cv.imread('anhmau/image320.png') - person_data_4 = object_detector(ref_person_4) - person_width_in_rf_4 = person_data_4[0][1] -
- # tim tieu cu f trung binh
- focal_person_1 = focal_length_finder(KNOWN_DISTANCE, PERSON_WIDTH, person_width_in_rf_1) - focal_person_2 = focal_length_finder(KNOWN_DISTANCE+40, PERSON_WIDTH, person_width_in_rf_2) - focal_person_3 = focal_length_finder(KNOWN_DISTANCE+40, PERSON_WIDTH, person_width_in_rf_3) - focal_person_4 = focal_length_finder(KNOWN_DISTANCE+40, PERSON_WIDTH, person_width_in_rf_4) - focal_person=(focal_person_1+focal_person_2+focal_person_3+focal_ person_4)/4
Thiết lập giao tiếp Arduino và cài đặt tốc độ động cơ và hướng đi: