Sau khi thiết kế các chi tiết cơ khí của robot, nhóm tiến hành lắp ráp hoàn chỉnh phần khung robot hai bánh tự cân bằng.
Động cơ bước được gá vào loại giá đỡ động cơ bước tương ứng để đảm bảo khi vận hành không gây rung lắc ảnh hưởng đến cân bằng của xe. Phần trục động cơ được lắp thêm khớp nối lục giác để gắn với bánh xe (Hình 4.22).
Hình 4.22 Lắp ráp động cơ bước vào giá đỡ
Sau khi hoàn thiện phần lắp động cơ với giá đỡ động cơ, nhóm tiến hành lắp giá động cơ với tấm đỡ (Hình 4.23).
Hình 4.24 Chế tạo phần hộp pin
Hình 4.26 Chế tạo tầng hai của robot
Hình 4.28 Robot hai bánh tự cân bằng hoàn chỉnh
Kết quả chạy thử nghiệm
Bảng 4-5 Kết quả chạy thử nghiệm
STT Nội dung Kết quả Ghi chú
1 Tốc độ tối đa 1.6 m/s 2 Tốc độ Robot ổn định 0.45 m/s 3 Góc nghiêng lớn nhất xe có thể tự cân bằng 22 0
4 Khối lượng vật có thể mang 150g
5
Độ nghiêng lớn nhất của dốc mà
Robot có thể đi 17 0
6
Độ nghiêng của dốc mà Robot
có thể đi ổn định 4 0
7 Thời gian hoạt động 1h10p
Đến khi xe mất cân bằng
8
Phạm vi điều khiển qua
bluetooth 20m
Không có vật cản
9
Bề rộng đường tối thiểu để
Robot xoay 1800 28cm 10 Góc lệch Robot khi đi thẳng 40 11 Phạm vi Robot theo vật 3m
12
Khoảng cách an toàn Robot theo
vật 0.5m
Nhận xét:
Trong quá trình chạy thử nghiệm robot trên hai bề mặt có độ ma sát khác nhau là sàn gạch hoa (bề mặt nhẵn – hệ số ma sát nhỏ) và sàn bê tông (bề mặt nhám – hệ số ma sát lớn), nhóm nhận thấy tốc độ di chuyển trên sàn gạch hoa nhanh hơn so với sàn bê tông nhưng khả năng cân bằng trên sàn bê tông lại tốt hơn so với sàn gạch hoa. Điều này hoàn toàn phù hợp với lý thuyết robot sẽ cân bằng tốt ở những nơi có ma sát cao và di chuyển nhanh hơn ở những nơi có ma sát nhỏ hơn.
Kết luận:
Trong chương 4, nhóm đã chế tạo được hệ thống cơ khí đúng trọng tâm cho robot hai bánh tự cân bằng. Bên cạnh đó, nhóm tính toán sức bền cho hệ thống cơ khí để đảm bảo an toàn khi hoạt động. Thiết kế được mạch điều khiển cho robot hoạt động ổn định và điều khiển các thông số phù hợp với hệ thống. Chạy thử nghiệm hệ thống trên các bề mặt khác nhau để quan sát sự ổn định của robot trên các bề mặt đó.
KẾT LUẬN
Kết quả đạt được
Thiết lập mô hình toán học, hàm trạng thái của mô hình và mô phỏng thành công mô hình trên Matlab Simulink.
Thiết kế, chế tạo thành công robot hai bánh tự cân bằng.
Các thành phần cơ khí đảm bảo đủ độ bền trong khi hoạt động.
Các đặc tính đầu ra (góc nghiêng, vị trí) của hệ hoạt động ổn định thông qua việc điều chỉnh các thông số của bộ điều khiển PID.
Đánh giá
Do đây lần đầu tham gia vào quá trình thiết kế, chế tạo nên phần tính toán hệ thống cơ khí chưa được tối ưu.
Phần mềm điểu khiển tuy đã có khả năng dự trù được một vài lỗi nhưng vẫn chưa đầy đủ và hầu hết vẫn phải khắc phục thủ công.
Tính thẩm mĩ của robot chưa hoàn hảo.
Năng suất của hệ thống không được cao.
Hạn chế và hướng giải quyết
Đồ án đã điều khiển được các thông số về góc nghiêng của thân xe và vị trí của xe khá ổn định trên địa hình phẳng. Nhưng bên cạnh đó còn thông số cần điều khiển để hệ thống hoạt động ổn định đó là góc xoay của xe, lọc nhiễu tín hiệu, khả năng tránh vật cản. Do đó, để hoàn thiện hơn đồ án cần:
Tối ưu hóa khối lượng, thuật toán để mô hình có thể hoạt động linh hoạt, dễ dàng điều khiển, tiết kiệm năng lượng.
Thiết kế thêm bộ lọc nhiễu tín hiệu.
Có thể gắn thêm Camera và GPS để robot có thể định vị, ghi hình, xử lý ảnh và tự hoạt động trong không gian lớn.
TÀI LIỆU THAM KHẢO
[1] Nguyễn Văn Trường, Nguyễn Xuân Thuận, Nguyễn Anh Tú, Bài giảng học phần Robot di động, Đại học Công nghiệp Hà Nội.
[2] Rich Chi Ooi, Balancing a Two-Wheeled Autonomous Robot, Final Year Thesis, 2003.
[3] Lê Ngọc Duy, Giáo trình cảm biến và hệ thống đo, 2019.
[4] Nguyễn Văn Đồng Hải, Điều khiển PID xe hai bánh tự cân bằng.
https://www.youtube.com/watch?v=kqfubY-UUhc.
[5] Phạm Thanh Khuyên, Arduino cho người bắt đầu, 2021.
https://www.youtube.com/watch?v=8uIownT95aQ&list=PL80KixHnvz1 DMNv9w5Ng8ZMV1_LqfRlkH.
PHỤ LỤC
Phụ lục 1. Chương trình mô phỏng hệ thống trên phần mềm Matlab
km=0.022; % he so momen
ke=0.4; % he so back emf cua dong co
R=1.7; % dien tro dong co
r=0.0325; % ban kinh xe
Mp=1; % khoi luong robot
Mw=0.03; % khoi luong banh xe
Ip=0.0012; % momen quan tinh than robot
Iw=0.000016; % momen quan tinh banh xe
l=0.05; % khoang cach tu tam banh xe toi tam than xe
g=9.81; % gia toc trong truong
teta_init=0; % goc nghieng ban dau
teta_dot_init=0; % van toc goc nghieng ban dau
x_init=0; % vi tri ban dau
x_dot_init=0; % van toc xe ban dau
beta = 2*Mw+2*Iw/r^2+Mp; alpha = Ip*beta+2*Mp*l^2*(Mw+Iw/r^2); A = [0 1 0 0; 0 (2*km*ke*(Mp*l*r-Ip-Mp*l^2))/(R*r^2*alpha) (Mp^2*g*l^2)/alpha 0; 0 0 0 1; 0 (2*km*ke*(r*beta-Mp*l))/(R*r^2*alpha) (Mp*g*l*beta)/alpha 0] B = [0; (2*km*(Ip+Mp*l^2-Mp*l*r))/(R*r*alpha); 0; (2*km*(Mp*l-r*beta))/R*r*alpha] C = [1 0 0 0; 0 0 1 0] D = [0; 0]
P = [B A*B A^2*B A^3*B]; %Khao sat tinh dieu khien
rank(P);
L = [C; C*A; C*A^2; C*A^3]; %Khao sat tinh quan sat
rank(L);
[num,den] = ss2tf(A,B,C,D) %Tinh ham truyen
HTXE1 = tf(num(1,:),den) HTXE2 = tf(num(2,:),den) pzmap (A,B,C,D)
Phụ lục 2. Chương trình điều khiển
Code nạp cho Arduino Uno
const int trig = 8; // chân Trig(HC-SR04) - chân 8(Arduino Uno) const int echo = 7; // chân Echo(HC-SR04) - chân 7(Arduino Uno) const int control_1 = 11; // chân 11(Arduino Uno)-chân 11(Arduino Nano) const int control_2 = 12; // chân 12(Arduino Uno)-chân 12(Arduino Nano) const int control_3 = 13; // chân 13(Arduino Uno)-chân 13(Arduino Nano) **************************************************************
// HÀM SETUP void setup(){
Serial.begin(9600); // giao tiếp Serial với baudrate 9600 pinMode(trig, OUTPUT); // chân trig sẽ phát tín hiệu pinMode(echo, INPUT); // chân echo sẽ nhận tín hiệu pinMode(control_1, OUTPUT); // thiết đặt chân control_1 là OUTPUT pinMode(control_2, OUTPUT); // thiết đặt chân control_2 là OUTPUT pinMode(control_3, OUTPUT);} // thiết đặt chân control_3 là OUTPUT **************************************************************
// HÀM LOOP void loop(){
unsigned long duration; // biến đo thời gian int distance; // biến lưu khoảng cách
/* Phát xung từ chân trig */
digitalWrite(trig, 0); // tắt chân trig delayMicroseconds(2); // dừng 2 microSeconds digitalWrite(trig, 1); // phát xung từ chân trig delayMicroseconds(5); // xung có độ dài 5 microSeconds digitalWrite(trig, 0); // tắt chân trig
/* Tính toán thời gian */ duration = pulseIn(echo, HIGH); // đo độ rộng xung HIGH ở chân echo distance = float (duration / 2 / 29.412); // tính khoảng cách đến vật if (distance > 50)digitalWrite(control_1, HIGH);
// nếu vật cản ở xa hơn 50 cm else digitalWrite(control_1, LOW); // nếu vật cản ở gần hơn 50 cm if (30 < distance <= 50)digitalWrite(control_2, HIGH);
// nếu vật cản ở trong khoảng từ 30-50 cm else digitalWrite(control_2, LOW);
// nếu vật cản không ở trong khoảng từ 30-50 cm if (distance <= 30)digitalWrite(control_3, HIGH);
// nếu vật cản ở gần hơn 30 cm else digitalWrite(control_3, LOW); // nếu vật cản ở xa hơn 30 cm
delay(200);} // dừng 0.2 giây
************************************************************** **************************************************************
Code nạp cho Arduino nano
#include "stmpu6050.h" //khai báo thư viện stmpu6050.h SMPU6050 mpu6050; //đặt tên cho SMPU6050
# define Enable 8 // chân Enable(A4988) - chân digital 8(Arduino) # define Step_3 7 // chân Step_3(A4988) - chân digital 7(Arduino) # define Step_2 6 // chân Step_2(A4988) - chân digital 6(Arduino) # define Step_1 5 // chân Step_1(A4988) - chân digital 5(Arduino) # define Dir_3 4 // chân Dir_3(A4988) - chân digital 4(Arduino) # define Dir_2 3 // chân Dir_2(A4988) - chân digital 3(Arduino) # define Dir_1 2 // chân Dir_1(A4988) - chân digital 2(Arduino)
# define MS2 10 // MS2(A4988) - chân digital 10(Arduino) const int Receive_1 = 11; // chân 11(Arduino Uno)-chân 11(Arduino Nano) const int Receive_2 = 12; // chân 12(Arduino Uno)-chân 12(Arduino Nano) const int Receive_3 = 13; // chân 13(Arduino Uno)-chân 13(Arduino Nano) int Receive_1_State = 0; // biến trạng thái báo robot đang ở gần int Receive_2_State = 0;
// biến trạng thái báo robot đang ở khoảng cách an toàn int Receive_3_State = 0; // biến trạng thái báo robot đang ở xa
int y = 0; // khai báo biến y
************************************************************** //KHAI BÁO CÁC BIẾN
int8_t Dir_M1, Dir_M2, Dir_M3; //Biến xác định chiều quay động cơ volatile int Count_timer1, Count_timer2, Count_timer3;
//đếm các lần TIMER xuất hiện trong chu kỳ xung STEP int16_t Count_TOP1,Count_BOT1,Count_TOP2,Count_BOT2, Count_TOP3, Count_BOT3;
//vị trí cuối của phần cao và cuối phần thấp trong 1 chu kỳ xung STEP float Input_L, Input_R, Output, I_L, I_R, Input_lastL, Input_lastR, Output_L, Output_R, M_L, M_R, Motor_L, Motor_R;
float Kp = 2; float Ki = 0.2; float Kd = 1; float Offset = 3; float Vgo = 0; float Vgo_L = 0; float Vgo_R = 0;
char Bluetooth;
unsigned long loop_timer;
************************************************************** //HÀM KHAI BÁO CÁC CHÂN ARDUINO NANO
void pin_INI() {
pinMode(Enable, OUTPUT); //thiết đặt chân Enable là OUTPUT pinMode(Step_1, OUTPUT); //thiết đặt chân Step_1 là OUTPUT pinMode(Step_2, OUTPUT); //thiết đặt chân Step_2 là OUTPUT pinMode(Step_3, OUTPUT); //thiết đặt chân Step_3 là OUTPUT pinMode(Dir_1, OUTPUT); //thiết đặt chân Dir_1 là OUTPUT pinMode(Dir_2, OUTPUT); //thiết đặt chân Dir_2 là OUTPUT pinMode(Dir_3, OUTPUT); //thiết đặt chân Dir_3 là OUTPUT pinMode(MS2, OUTPUT); //thiết đặt chân MS2 là OUTPUT digitalWrite(Enable, LOW); //bật động cơ digitalWrite(MS2, HIGH); //chọn chế độ 1/4 step pinMode(Receive_1, INPUT); //thiết đặt chân Receive_1 là INPUT pinMode(Receive_2, INPUT); //thiết đặt chân Receive_2 là INPUT pinMode(Receive_3, INPUT); } //thiết đặt chân Receive_3 là INPUT **************************************************************
//HÀM KHAI BÁO TIMER2 void timer_INI() {
TCCR2A = 0; //thanh ghi TCCR2A được đặt thành 0 TCCR2B = 0; //thanh ghi TCCR2B được đặt thành 0 TCCR2B |= (1 << CS21);
//Đặt bit CS21 trong thanh ghi TCCRB để đặt tần số bộ timer là 16MHz/8 OCR2A = 39; //Thanh ghi so sánh được đặt thành 39 = 20us
TCCR2A |= (1 << WGM21); //Chế độ CTC bộ đếm được xóa về 0 khi giá trị bộ đếm (TCNT0) khớp với OCR0A
TIMSK2 |= (1 << OCIE2A); }
//Đặt bit cho phép ngắt OCIE2A trong thanh ghi TIMSK2 **************************************************************
// CHƯƠNG TRÌNH NGẮT CỦA TIMER2 ISR(TIMER2_COMPA_vect) {
//tạo xung STEP cho MOTOR1
if (Dir_M1 != 0) { //nếu MOTOR quay Count_timer1++;
if (Count_timer1 <= Count_TOP1)PORTD |= 0b00100000;
//nếu là nhịp nằm trong phần cao trong xung STEP else PORTD &= 0b11011111;
//nếu là nhịp nằm trong phần thấp của xung STEP if (Count_timer1 > Count_BOT1)Count_timer1 = 0;}
//nếu là nhịp cuối của 1 xung STEP //tạo xung STEP cho MOTOR2
if (Dir_M2 != 0) { Count_timer2++;
if (Count_timer2 <= Count_TOP2)PORTD |= 0b01000000; else PORTD &= 0b10111111;
if (Count_timer2 > Count_BOT2)Count_timer2 = 0; } //tạo xung STEP cho MOTOR3
if (Dir_M3 != 0) { Count_timer3++;
else PORTD &= 0b01111111; if (Count_timer3 > Count_BOT3)Count_timer3 = 0;}} ************************************************************** // HÀM TỐC ĐỘ DI CHUYỂN MOTOR1 void Speed_M1(int16_t x) { if (x < 0) { Dir_M1 = -1; PORTD &= 0b11111011;} else if (x > 0) { Dir_M1 = 1; PORTD |= 0b00000100;} else Dir_M1 = 0; Count_BOT1 = abs(x); Count_TOP1 = Count_BOT1 / 2;} ************************************************************** // HÀM TỐC ĐỘ DI CHUYỂN MOTOR TRÁI
void Speed_L(int16_t x) { if (x < 0) { Dir_M2 = -1; PORTD &= 0b11110111;} else if (x > 0) { Dir_M2 = 1; PORTD |= 0b00001000;} else Dir_M2 = 0; Count_BOT2 = abs(x); Count_TOP2 = Count_BOT2 / 2;}
************************************************************** //HÀM TỐC ĐỘ DI CHUYỂN MOTOR PHẢI
void Speed_R(int16_t x) { if (x < 0) { Dir_M3 = -1; PORTD &= 0b11101111;} else if (x > 0) { Dir_M3 = 1; PORTD |= 0b00010000;} else Dir_M3 = 0; Count_BOT3 = abs(x); Count_TOP3 = Count_BOT3 / 2;} ************************************************************** //HÀM SETUP void setup() {
mpu6050.init(0x68); //địa chỉ I2C của MPU 6050 Serial.begin(9600); //khai báo Serial pin_INI(); //khai báo PIN Arduino đấu nối 3 DRIVER A4988 timer_INI(); //khai báo TIMER2
delay(500); //dừng 0.5 giây
loop_timer = micros() + 4000; }
************************************************************** //HÀM LOOP
void loop() {
Receive_1_State = digitalRead(Receive_1); //đọc trạng thái chân Receive_1 Receive_2_State = digitalRead(Receive_2); //đọc trạng thái chân Receive_2
Receive_3_State = digitalRead(Receive_3); //đọc trạng thái chân Receive_3 float AngleY = mpu6050.getYAngle(); //đọc giá trị góc nghiêng if (Serial.available() > 0) { //nếu kết nối bluetooth Bluetooth = Serial.read();} //đọc giá trị điều khiển từ bluetooth if (Bluetooth == 'g') { //robot tiến if (Vgo < 3.5) Vgo += 0.5;
if ((Output_L || Output_R) > 250)Vgo -= 0.01; Vgo_L = Vgo_R = 0;
y = 0;}
else if (Bluetooth == 'b') { //robot lùi if (Vgo > - 3.5) Vgo -= 0.5;
if ((Output_L || Output_R) < - 250)Vgo += 0.01; Vgo_L = Vgo_R = 0;
y = 0;}
else if (Bluetooth == 'l') { //robot quay trái if (Vgo_L > -0.2)Vgo_L -= 0.01;
if (Vgo_R < 0.2)Vgo_R += 0.01; y = 0;}
else if (Bluetooth == 'r') { //robot quay phải if (Vgo_L < 0.2)Vgo_L += 0.01;
if (Vgo_R > - 0.2)Vgo_R -= 0.01; y = 0;}
else if (Bluetooth == 's') { //robot dừng Vgo = Vgo_R = Vgo_L = 0;
Input_lastL = Input_lastR; I_L = I_R;
y = 0;}
else if (Bluetooth == 'a') { y = 1;}
if (y == 1) { //chế độ robot bám vật cản if ( Receive_1_State == HIGH) { //nếu cách vật xa hơn 50cm if (Vgo < 1.5) Vgo += 0.2; //robot tiến if ((Output_L || Output_R) > 100)Vgo -= 0.01;}
else if (Receive_2_State == HIGH) { //nếu ở trong khoảng 30-50cm Vgo = Vgo_R = Vgo_L = 0;} //robot dừng else if (Receive_3_State == HIGH) { //nếu cách vật gần hơn 30cm if (Vgo > - 2.5) Vgo -= 0.3; //robot lùi if ((Output_L || Output_R) < - 150)Vgo += 0.01;}}
//Dùng PID cho MOTOR_L Input_L = AngleY + Offset - Vgo - Vgo_L;
I_L += Input_L;
I_L = constrain(I_L, -400, 400);
Output_L = Kp * Input_L + I_L*Ki + Kd * (Input_L - Input_lastL);
Input_lastL = Input_L; //Lưu làm độ lệch trước cho vòng loop sau //Khống chế OUTPUT theo sự phi tuyến MOTOR_L if (Output_L > -5 && Output_L < 5)Output_L = 0;
Output_L = constrain(Output_L, -400, 400);
//Dùng PID cho MOTOR_R Input_R = AngleY + Offset - Vgo - Vgo_R;
I_R += Input_R;
Output_R = Kp * Input_R + I_R*Ki + Kd * (Input_R - Input_lastR); Input_lastR = Input_R;
if (Output_R > -5 && Output_R < 5)Output_R = 0; Output_R = constrain(Output_R, -400, 400);
//Khắc phục sự phi tuyến của MOTOR_L if (Output_L > 0)M_L = 405 - (1 / (Output_L + 9)) * 5500;
else if (Output_L < 0) M_L = -405 - (1 / (Output_L - 9)) * 5500; else M_L = 0;
//Khắc phục sự phi tuyến của MOTOR_R if (Output_R > 0)M_R = 405 - (1 / (Output_R + 9)) * 5500;
else if (Output_R < 0)M_R = -405 - (1 / (Output_R - 9)) * 5500; else M_R = 0;
//Làm ngược giá trị truyền vào hàm Speed_L() if (M_L > 0)Motor_L = 400 - M_L;
else if (M_L < 0)Motor_L = -400 - M_L; else Motor_L = 0;
//Làm ngược giá trị truyền vào hàm Speed_R() if (M_R > 0)Motor_R = 400 - M_R;
else if (M_R < 0)Motor_R = -400 - M_R; else Motor_R = 0;
Speed_L(Motor_L); //cho robot chạy Speed_R(Motor_R); //cho robot chạy while (loop_timer > micros());
Phụ lục 4: Kết quả thử nghiệm STT Thời gian (s) Quãng đường (m) Độ lệch (m) Góc lệch ( 0 ) Vận tốc trung bình (m/s) Ghi chú 1 13.8 4,9 0,31 3,6 0,36 Di chuy ển tr ên sà n b ê tông , có gi ó 2 15,8 4,9 0,45 5,2 0,31 3 12,8 4,9 0,41 4,8 0,38 4 16 4,9 0,61 7,1 0,31 5 11,66 4,9 0,58 6,8 0,42 6 6,31 2,5 0,225 5,1 0,4 7 7,43 2,5 0,19 4,3 0,34 8 7,3 2,5 0,15 3,4 0,34 9 4,45 2,5 0,16 3,7 0,56 10 7,6 2,5 0,13 3 0,33 11 15,9 5 0,37 4,2 0,31 Di ch uy ển tr ên s àn g ạch đ á h oa , k h ông c ó g ió 12 12,35 5 0,37 4,2 0,4 13 9,23 5 0,42 4,8 0,54 14 12,44 5 0,24 2,7 0,4 15 10,8 5 0,37 4,2 0,46 16 4,4 2,5 0,125 2,9 0,57 17 4,02 2,5 0,13 3 0,62 18 6,4 2,5 0,11 2,5 0,39 19 6,4 2,5 0,13 3 0,39 20 6,5 2,5 0,1 2,3 0,38 Trung bình 4,04 0,45
STT Quãng đường (m) Thời gian (s) Vmax (m/s) 1 2,9 4,1 1,41 2 1,8 2,4 1,5 3 3,7 5,1 1,45 4 4,3 5,3 1,62 5 1,25 1,44 1,74 6 3,25 3,9 1,67 7 3,7 4,8 1,54 8 3,3 4,1 1,61 9 5,5 6,4 1,72 10 4,2 4,8 1,75 Trung bình 1,6