Hình trên cùng trong hình 2.15 thể hiện sự bố trí của 2 cảm biến kênh A và B lệch pha nhau. Khi cảm biến A bắt đầu bị che thì cảm biến B hoàn toàn nhận được hồng ngoại xuyên qua, và ngược lại. Hình thấp là dạng xung ngõ ra trên 2 kênh. Xét trường hợp motor quay cùng chiều kim đồng hồ, tín hiệu “đi” từ trái sang phải. Lúc này tín hiệu A chuyển từ mức cao xuống thấp (cạnh xuống) thì kênh B đang ở mức thấp. Ngược lại, nếu động cơ quay ngược chiều kim đồng hồ, tín hiệu “đi” từ phải qua trái. Lúc này, tại cạnh xuống của kênh A thì kênh B đang ở mức cao. Như vậy, bằng cách phối hợp 2 kênh A và B chúng ta không những xác định được góc quay (thông qua số xung) mà còn biết được chiều quay của động cơ (thông qua mức của kênh B ở cạnh xuống của kênh A).
Cuối cùng là sử dụng ngắt ngoài: đây là phương pháp dễ nhưng chính xác để đọc encoder và cũng là phương pháp được dùng trong luận văn này. Ý tưởng của phương pháp rất đơn giản, chúng ta nối kênh A của encoder với 1 ngắt ngoài
(INT2 chẳng hạn) và kênh B với một chân nào đó bất kỳ (không phải chân ngắt). Cứ mỗi lần ngắt ngoài xảy ra, tức có 1 xung xuất hiện trên ở kênh A thì trình phục vụ ngắt ngoài tự động được gọi. Trong trình phục vụ ngắt này chúng ta kiểm tra mức của kênh B, tùy theo mức của kênh B chúng ta sẽ tăng biến đếm xung lên 1 hoặc giảm đi 1. Tuy nhiên, cần phải tính toán rất cẩn thận khi sử dụng phương pháp này. Ví dụ trường hợp encoder có độ phân giải 2000 xung/vòng, motor quay với vận tốc 100 vòng/s thì tần số xung trên kênh A của encode là 2000x100=200KHz, nghĩa là cứ mỗi 5 us ngắt ngoài xảy ra một lần.
2.7. Lựa chọn cảm biến
Để robot hoạt động ổn định, chính xác ngoài hệ thống camera trước và sau được truyền về máy tính, robot cần hệ thống cảm biến, robot tiến, lùi, quay trái, phải với các góc 450, 900, 1800 thông qua cảm biến la bàn số và hệ thống encoder gắn trên động cơ.
Trong khuôn khổ đề tài, lựa chọn cảm biến la bàn số QMC5883L có kích thước nhỏ gọn sử dụng giao tiếp I2C, được dùng để đo từ trường của trái đất nhằm xác định phương hướng với độ chính xác lên đến 1 hoặc 2 độ, cảm biến có cách đo riêng biệt cho từng trục và có thể kết hợp lại để tính toán 3D.
Cảm biến la bàn số QMC5883L còn có thể dùng để đo từ trường thô hoặc các nguồn từ trường mạnh hơn gần nó, cảm biến có thể cảm nhận được nguồn từ trường xung quanh nó như của nam châm hoặc điện trường, khi phát hiện được từ trường từ bên ngoài, nó có thể xác định được khoảng cách tương đối hoặc chiều đến vật phát ra từ trường đó.
Thông số kỹ thuật:
Nguồn cung cấp: 3÷5 VDC
Chuẩn truyền thông: Standard IIC communication protocol
Dải đo: ± 1.3 ÷ 8 Gauss
Điện áp điều chỉnh mức thấp nhất: 3.3V
Kích thước (D x R x C): 14.35 mm x 13.16 mm x 3.40 mm Kết nối đến Arduino:
Arduino GND -> QMC5883L GND
Arduino 3.3V -> QMC5883L VCC
Arduino A4 (SDA) -> QMC5883L SDA
Arduino A5 (SCL) -> QMC5883L SCL
Hình 2.19. Cảm biến la bàn số QMC5883L
La bàn chỉ xác định hướng, muốn xác định tọa độ của robot cần có 2 hướng, 2 hướng này giao nhau tại 1 điểm đó là tọa độ của robot.
Robot hoạt động bằng la bàn thường kèm theo encoder để xác định tọa độ của nó. Muốn robot hoạt động, người ta phải lập bản đồ hoạt động. Thí dụ robot đi từ điểm A đến điểm B, giữa A và B có chướng ngại, robot cần phải tránh.
Trước tiên phải vẽ bản đồ hoạt động. Từ A đến B có hướng thí dụ là 330 độ Nam, từ A đến chướng ngại là 1m, bề dài chướng ngại là 1m. Từ chướng ngại đến B là 2m. Bánh xe robot có chu vi là 0,1 m.
Như vậy robot bắt đầu hoạt động từ điểm A đi về hướng 330 độ Nam, encoder gắn trên trục bánh xe đếm được 10 vòng là phải tránh chướng ngại. Robot quẹo phải tức là ở hướng: 330 + 90 = 420 - 360 = 60 độ Bắc. Cứ thế robot sẽ tiếp tục đi thẳng 10 encoder rồi quẹo trái, đi thẳng, quẹo trái, đi thẳng, quẹo phải đi thẳng đến mục tiêu.
Chương 3
KẾT QUẢ VÀ THỬ NGHIỆM 3.1. Kết quả
Hình 3.1. Bản vẽ mạch in hai lớp
Bo mạch được thiết kế bằng phần mềm Orcad, với mạch in hai lớp, bao gồm arduino điều khiển các khối: Khối điều khiển Motor 1, Motor 2 , Motor 3, Motor 4, Motor 5, Motor 6.
Sau khi đã gia công, lắp ráp hoàn chỉnh các bo mạch, các bo mạch được lắp vào trong các hộp đựng mạch của Robot Rhex, đấu nối các dây dẫn liên quan, đạt được phần cứng điều khiển động cơ hoàn chỉnh.
3.2. Thử nghiệm
3.2.1. Thử nghiệm các thao tác vòng trái, vòng phải với các góc độ khác nhau nhau
Hàm truyền đạt của động cơ DC khi robot thực hiện thao tác vòng trái, vòng phải:
(3.1)
Xác định thông số bộ điều khiển
Bộ điều khiển PID ( KD =0 )
KP = 0,6
TN = 0,5Tcrit = 0,021
TV = 0,125Tcrit = 0,00525
KI = 1318,29
KD = KP.TV = 0,145
Hình 3.2. Đồ thị hiển thị tín hiệu góc quay
Khi Robot đứng (không di chuyển tiến lùi), người sử dụng có thể điều khiển Robot vòng trái, vòng phải với các góc 45 độ, 90 độ, 180 độ. Trong thực tế, các thao tác này thường được sử dụng khi Robot cần phải rẽ hướng tại các địa hình phức tạp (không thể vừa di chuyển tiến, lùi vừa rẽ hướng được).
+ Đánh giá kết quả:
a) Robot đang vòng phải. b) Robot đang vòng trái Hình 3.3. Thử nghiệm cho Robot tại chỗ vòng phải, vòng trái.
t(s) v(cm/s)
- Ưu điểm: Do có phương pháp di chuyển khác với khi di chuyển thông thường, nên quá trình vòng phải, vòng trái cơ bản được linh hoạt; Robot được chuyển hướng ngay lập tức theo hướng đã điều khiển, đặc biệt có thể rẽ vuông góc. Vị trí của Robot sau khi rẽ hướng hầu như không thay đổi so với trước khi rẽ
- Hạn chế: Góc rẽ chưa được chính xác, do phụ thuộc vào địa hình nên chân Robot có thể bị vướng làm sai góc rẽ (thông thường là góc rẽ < góc cần rẽ).
3.2.2. Thử nghiệm Robot di chuyển tại địa hình bằng phẳng
Cho KI = KD = 0, KP = 0,5
Hình 3.4. Đồ thị hiển thị tín hiệu di chuyển bằng phẳng
+ Thử nghiệm tại địa hình tương đối bằng phẳng. Khi di chuyển trên đường bằng phẳng, Robot sử dụng phương pháp di chuyển thông thường (từng cặp 3 chân sẽ thay nhau chuyển động).
Hình 3.5. Thử nghiệm Robot di chuyển trên đường bằng. v (cm/s)
Đây là phương pháp di chuyển chủ yếu, được sử dụng nhiều nhất. Để đảm bảo Robot không bị lệch hướng thì tốc độ, thời điểm chạm đất của các chân phải đồng bộ với nhau.
- Ưu điểm: Việc di chuyển (tiến, lùi; thực hiện vòng trái, vòng phải trong khi tiến) được Robot thực hiên tương đối tốt. Các chân của Robot đồng bộ với nhau, Robot không bị giật khi chuyển bước. Khi đứng lại Robot trở về đúng trạng thái yêu cầu (trạng thái sẵn sàng di chuyển cho bước tiếp theo). Việc điều khiển tiến, lùi, vòng trái, vòng phải, tạm dừng có hiệu quả tức thì (khi kết thúc bước đang thực hiện thì Robot sẽ chuyển trạng thái yêu cầu).
Khi di chuyển trên đường bằng, thì tốc độ của Robot có thể được đặt ở mức 1 đến mức 9.
- Hạn chế: Khi đặt tốc độ di chuyển ở mức 9 (mức cao nhất) thì Robot có cảm giác bị giật (do tốc độ nhanh, các cảm biến không cập nhật kịp). [1]
3.2.3. Thử nghiệm Robot lên, xuống bậc cao
Cho KP = KI = 2; KD = 0,01 ta có đồ thị sau:
Hình 3.6. Đồ thị hiển thị tín hiệu lên xuống bậc cao t(s) v (cm/s)
Theo thiết kế, Robot có khả năng lên, xuống bậc thẳng đứng có chiều cao < 2 lần đường kính chân của Robot. Với đường kính chân là 20 cm, do vậy nhóm nghiên cứu đề tài đã thử nghiệm bậc cao 35 cm.
- Ưu điểm: Đối với các bậc cao < 40 cm thì Robot có khả năng leo lên, leo xuống tốt; đồng thời Robot không bị lật. Các bộ cảm biến có phản ứng tốt, khi Robot bắt đầu bước chân lên bậc (tăng tốc độ để tăng lực đẩy); đồng thời Robot tự điều chỉnh để đảm bảo thăng bằng khi lên bậc và xuống bậc. Khi đã hoàn thành việc lên, xuống bậc thì Robot lại trở về trạng thái di chuyển bình thường.
- Hạn chế: Khi Pin yếu hoặc tốc độ đặt ở mức thấp thì việc leo bậc sẽ gặp khó khăn (Robot sẽ phải dừng lại để tăng tốc nhiều lần). Khi leo bậc mà góc chạm của Robot với bề thành đứng của bậc thấp (< 30 độ) thì Robot có thể bị lệch hướng. [1]
KẾT LUẬN 1. Kết quả nghiên cứu của đề tài
Về cơ sở lý thuyết:
- Phân tích và lựa chọn được phương án thiết kế hợp lý.
- Phân tích và tính toán được các thông số kỹ thuật cần thiết.
- Sử dụng được phương pháp điều khiển tối ưu hóa cho robot.
Về mặt thực nghiệm:
- Chế tạo thành công hệ thống điều khiển động cơ cho robot.
Áp dụng thực tiễn:
- Với kết quả đạt được có thể ứng dụng đề tài vào việc điều khiển robot từ xa bằng sóng vô tuyến và thu được các hình ảnh trực quan từ camera trên robot về máy tính.
- Đề tài góp phần làm phát triển thêm các hình thức mô hình, mô phỏng cho các robot địa hình chuyên dụng có khả năng thay thế hoạt động của con người trong các khu vực nguy hiểm.
- Đề tài góp phần phục vụ cho công việc học tập, nghiên cứu của sinh viên các ngành cơ khí, tự động hóa…
2. Hướng phát triển của đề tài:
Đề tài có tính mở, có nhiều vấn đề có thể tiếp tục triển khai nghiên cứu
phát triển đề tài như sau:
- Hoàn thiện kết cấu cơ khí để robot có thể hoạt động hiệu quả ở các địa hình phức tạp hơn.
- Hướng đến phát triển việc điều khiển robot ở khoảng cách xa hơn qua đường truyền Internet.
- Định vị cho robot [13,15].
- Trang bị cho robot các cảm biến đo nhiệt độ, độ ẩm, độ phóng xạ… để gửi tín hiệu các trạng thái ở vùng hoạt động của robot về máy tính, làm tăng thêm khả năng ứng dụng cho robot.
- Trang bị thêm tay máy gắn trên robot để người điều khiển thực hiện các thao tác mong muốn từ xa.
- Xây dựng, hoàn thiện khả năng làm việc nhóm của robot tạo thế mạnh nổi bật so với các loại robot khác…
TÀI LIỆU THAM KHẢO Tiếng Việt
1. Trung tá, TS Vũ Văn Tâm (2018), Nghiên cứu thiết kế và chế tạo robot
chuyên dùng phục vụ lực lượng công an nhân dân, trường Đại học Kỹ thuật –
Hậu cần CAND.
2. Đặng Công Huy Minh (2013), Thiết kế và chế tạo robot vượt địa hình phức
tạp, luận văn thạc sĩ trường Đại học Đà Nẵng.
3. Phạm Quang Huy, Lê CảnhTrung, Lập trình điều khiển với Arduino, Nhà xuất
bản khoa học kỹ thuật.
4. TS Lê Mỹ Hà, KS Phạm Quang Huy, Lập trình IoT với Arduino, Nhà xuất
bản Thanh niên.
5. Phạm Quang Huy, Lê Cảnh Trung, Bài tập thực hành Arduino - Lập trình
điều khiển với Arduino, Nhà xuất bản khoa học kỹ thuật.
6. TS. Trương Đình Nhơn, KS Phạm Quang Huy, Hướng dẫn sử dụng Arduino,
Nhà xuất bản Thanh niên.
Tiếng Anh
7. Kevin C. Galloway, Galen Clark Haynes, B. Deniz Ilhan, Aaron M. Johnson,
Ryan Knopf (2010), “X-RHex: A Highly Mobile Hexapedal Robot for
SensoriMotor Tasks”, University of Pennsylvania Scholarly Commons
Department of Electrical & Systems Engineering.
8. Joel D.Weingarten, Gabriel A. D. Lopes, Martin Buehler, Richard E. Grof, Daniel E. Koditschek (2004), ”Automated Gait Adaptation for Legged
Robots”,Reprinted from Proceedings of the 2004 IEEE International Conference
9. Z. Moore and M. Buehler (2001),“Stable Stair Climbing in a Simple Hexapod
Robot”, Ambulatory Robotic Lab, Centre for Intelligent Machines, McGill
University.
10. Joel D.Weingarten, Gabriel A. D. Lopes, Martin Buehler, Richard E. Grof, Daniel E. Koditschek (2004), ”Automated Gait Adaptation for Legged Robots”, Reprinted from Proceedings of the 2004 IEEE International Conference on Robotics and Automation (ICR 2004), Volume 3, pages 2153-2158.
11. Uluc Saranli, Daniel E. Koditschek (2003), “Back Flips with a Hexapedal
Robot”, Reprinted from Proceedings of the IEEE International Conference on
Robotics and Automation, Volume 3, 2003, pages 22092215.
12. Sarjoun Skaff, Alfred A. Rizzi, Howie Choset, Pei-Chun Lin (2005), “A
Context-Based State Estimation Technique for Hybrid Systems”, Proceedings of
the 2005 IEEE international Conference on Robottics and Automation Barcelona, Spain.
13. Tran, D. T., Luu, M. H., Nguyen, T. L., Nguyen, D. D., & Nguyen, P. T. (2007). Land-Vehicle MEMS INS/GPS Positioning During GPS Signal Blockage Periods. Journal of Science, Vietnam National University, Hanoi, 23(4), 243-251.
14. Tran, D. T., Luu, M. H., Nguyen, T. L., Nguyen, P. T., & Huynh, H. T. (2007). Performance Improvement of MEMS-Based Sensor Applying in Inertial Navigation Systems. Posts, Telematics & Information Technology Journal, 2, 19-24.
15. Duc-Tan, T., Fortier, P., & Huynh, H. T. (2011). Design, simulation, and performance analysis of an INS/GPS system using parallel kalman filters structure. REV Journal on Electronics and Communications, 1(2)
PHỤ LỤC
Mã nguồn chương trình
include "PinChangeInt.h"
#include <PID_v1.h> // Thanks to Brett Beauregard for his nice PID library
#define encodPinA1 2 // Quadrature encoder A pin
#define encodPinB1 8 // Quadrature encoder B pin
#define M1 9 // PWM outputs to L298N H-Bridge motor driver module
#define M2 10
double kp = 5 , ki = 1 , kd = 0.01 ,input = 0, output = 0, setpoint = 0; // modify kp, ki and kd for optimal performance
long temp;
volatile long encoderPos = 0;
PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT); // if motor will only run at full speed try 'REVERSE' instead of 'DIRECT'
void setup() {
pinMode(encodPinA1, INPUT_PULLUP); // quadrature encoder input A
pinMode(encodPinB1, INPUT_PULLUP); // quadrature encoder input B
TCCR1B = TCCR1B & 0b11111000 | 1; // set 31KHz PWM to prevent motor noise myPID.SetMode(AUTOMATIC); myPID.SetSampleTime(1); myPID.SetOutputLimits(-255, 255); } void loop() {
temp += analogRead(0); // increment position target with potentiometer value (speed), potmeter connected to A0
if (temp < 0) { // in case of overflow
encoderPos = 0;
temp = 0;
}
setpoint = temp / 500; // modify division to fit motor and encoder characteristics
input = encoderPos ; // data from encoder
myPID.Compute(); // calculate new output
pwmOut(output); // drive L298N H-Bridge module
}
if (out > 0) {
analogWrite(M1, out); // drive motor CW
analogWrite(M2, 0);
}
else {
analogWrite(M1, 0);
analogWrite(M2, abs(out)); // drive motor CCW
}
}
void encoder() { // pulse and direction, direct port reading to save cycles
if (PINB & 0b00000001) encoderPos++; // if
(digitalRead(encodPinB1)==HIGH) count ++;
else encoderPos--; // if (digitalRead(encodPinB1)==LOW) count --;
}
POSITION CONTROL
// PID motor position control.
// Thanks to Brett Beauregard for his nice PID library
#include <PinChangeInt.h>
#define encodPinA1 2 // Quadrature encoder A pin
#define encodPinB1 8 // Quadrature encoder B pin
#define M1 9 // PWM outputs to L298N H-Bridge motor driver module
#define M2 10
double kp = 5 , ki = 1 , kd = 0.01; // modify for optimal performance
double input = 0, output = 0, setpoint = 0;
long temp;
volatile long encoderPos = 0;
PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT); // if motor will only run at full speed try 'REVERSE' instead of 'DIRECT'
void setup() {
pinMode(encodPinA1, INPUT_PULLUP); // quadrature encoder input A
pinMode(encodPinB1, INPUT_PULLUP); // quadrature encoder input B
TCCR1B = TCCR1B & 0b11111000 | 1; // set 31KHz PWM to prevent motor noise
myPID.SetMode(AUTOMATIC);
myPID.SetSampleTime(1);
myPID.SetOutputLimits(-255, 255);
Serial.begin (115200); // for debugging
}
void loop() {
setpoint = analogRead(0) / 5; // modify to fit motor and encoder characteristics, potmeter connected to A0
input = encoderPos ; // data from encoder