1. Giải thuật điều khiển co duỗi chân robot:
Các chân robot cần co duỗi linh hoạt để có thể co gọn lại thuận tiện đưa robot vào đường ống, đồng thời có thể duỗi dài ra để đảm bảo lực bám lên thành ống trong quá trình di chuyển. Lực ép được duy trì nhờ hệ thống lò xo nén kết hợp với hệ thống cam – con trượt đảm bảo cho lực nén lò xo đủ độ lớn yêu cầu giúp robot cứng vững khi chuyển động.
Bài toán trở thành điều khiển lực thông qua vị trí theo công thức lò xo: FK. x - K là độ cứng lò xo
CHƯƠNG 4 : HỆ THỐNG ĐIỀU KHIỂN
109
Bằng cách điều chỉnh vị trí cơ cấu cam - con trượt ta có thể điều chỉnh lực nén của lò xo.
Theo thiết kế bánh cam, độ co duỗi của chân robot được tính theo công thức chuyển đổi quay - tịnh tiến: 2 2 36 L 14 (mm) cos(11 )
Trong đó là góc quay của bánh cam o o 0 45
; L34mm63mm
Hình 4.13: Giải thuật điều khiển co duỗi chân robot.
Độ nén của lò xo được đo bởi Potentiometer, bằng cách điều khiển vị trí góc quay động cơ bánh cam bằng tín hiệu feedback Potentionmeter, hàm chuyển đổi góc quay – vị trí tịnh tiến để duy trì độ nén của lò xo đảm bảo lực bám yêu cầu.
CHƯƠNG 4 : HỆ THỐNG ĐIỀU KHIỂN
110
2. Giải thuật điều khiển động cơ bánh xe cho robot:
Robot di chuyển trong đường ống theo 2 phương án rõ ràng: chạy thẳng và queo cua. Khi chạy thẳng thì tốc độ ở mỗi bánh là như nhau, song khi gặp cua quẹo vẫn có thể vượt qua các co 90o hay các ngã rẽ ấy là tuỳ thuộc vào sự linh hoạt của robot chế tạo.
Khi gặp cua quẹo thì quãng đường di chuyển của các bánh xe không bằng nhau trong 1 thời điêm t nhất định của quá trình quẹo, điều này đòi hỏi sự thay đổi tốc độ ở mỗi bánh sao cho bánh xe khi qua co hay ngã rẽ mỗi bánh đều phải tự thay đổi vận tốc. Có 2 phương án được nghiên cứu là:
- Phương án 1: cho một số bánh chuyển động, các bánh không tham gia sẽ chịu kéo theo bởi phần bánh dẫn động trước. Cách này đơn giản tuy nhiên khi dùng thì công suất dẫn động của robot bị giảm, không đảm bảo yêu cầu hoạt động.
- Phương án 2: điều khiển vận tốc cả 6 bánh độc lập, phương án này đòi hỏi điều khiển phức tạp nhưng đảm bảo được cho công suất dẫn động.
Để đảm bảo độ linh hoạt khi qua cua quẹo cũng như kích thước nhỏ gọn yêu cầu, Robot được tập trung thiết kế theo phương án 2, 6 chân của robot được điều khiển độc lập.
Khi gặp khúc cua, robot cần định hướng điều chỉnh vận tốc các bánh xe chân 1 cách linh hoạt. Để di chuyển linh hoạt các bánh cần điều chỉnh linh hoạt cả về hướng và vận tốc. Tuy nhiên để đơn giản, ta giả sử khi đi thẳng các bánh xe di chuyển với vận tốc tối đa (duty 100%), khi vào cua, chỉ cần thay đổi tốc độ 1 hoặc 2 bánh cụ thể, giữ nguyên hướng di chuyển cũ.
Để xây dựng giải thuật cho robot tự động nhận biết góc cua, tự động điều chỉnh vận tốc là rất khó khăn. Ta xây dựng giải thuật điều khiển dựa trên Control box, việc xác định góc cua được thực hiện bởi người điều khiển thông qua Camera và thực tế đường ống; Robot sẽ dựa vào vị trí tương quan với đường ống xác định từ IMU và lệnh điều khiển để điều chỉnh vận tốc các bánh xe 1 cách phù hợp.
Để robot hoạt động chính xác cần xây dựng giải thuật điều khiển vận tốc chân robot dựa vào yêu cầu góc cua một cách linh hoạt, hình 4.14 mô tả vận tốc các bánh ứng với các góc cua đơn giản trong mặt phẳng XZ (mặt phẳng chuẩn theo IMU).
Tuy nhiên để thực hiện bài toán chuyển động qua các góc cua bất kỳ, ta xét trường hợp Robot cần qua góc cua α so với phương X như hình 4.15 dưới đây.
CHƯƠNG 4 : HỆ THỐNG ĐIỀU KHIỂN
111
Hình 4.14: Mô tả vận tốc của các bánh xe qua các góc cua đơn giản.
Để qua cua dưới góc α Robot cần 1 tác động làm xoay robot quay trục vuông góc với phương tạo bởi góc α. Robot dịch chuyển nhờ chuyển động lăn của các bánh xe trên thành ống, do tính đối xứng khi cùng các bánh xe cùng vận tốc sẽ kéo robot đi thẳng. Khi vận tốc các bánh xe khác nhau sẽ làm cho Robot bị xoay tùy theo phương chiều và độ lớn của vận tốc bánh xe. Vì vậy để cua góc α, vectơ tổng hợp của vận tốc 3 bánh phải có điểm đặt nằm trên phương tạo bởi góc α (điểm I như hình 4.15). Để đơn giản t cho 1 bánh có vận tốc bằng 0, 1 bánh có vận tốc tối đa, nhiệm vụ ở đây là điều chỉnh vận tốc bánh còn lại sao cho vectơ vận tốc có điểm đặt tại I. Bài toán được thực hiện đơn giản bằng quay tắc tổng hợp vectơ song song cùng chiều:
Bài toán chia thành 3 trường hợp, ứng với các góc cua 30o - 150o; 150o - 270o; 270o - 30o. - Trường hợp 1: góc cua 30o - 150o 2 1 3 v IA v 0; v IB Với v , v , v1 2 3 lần lượt là vận tốc bánh1, bánh 2, bánh 3.
CHƯƠNG 4 : HỆ THỐNG ĐIỀU KHIỂN 112 Bánh 1 B án h 2 B án h 3 Z X O A I B α
Hình 4.15: Mô hình tính toán vận tốc chân robot khi vào góc cua bất kỳ,
Áp dụng định lý sin trong tam giác OIB, ta có:
OI IB BO
sin(OBI) sin(IOB) sin(BIO)
o o
BOR;OBI30 ; IOB 150 ; BIO
o sin(IOB) sin(150 ) IB BO R sin sin(BIO) Tương tự cho tam giác OIA, ta có:
o o
o
sin(IOA) sin( 30 ) sin( 30 )
IA AO R R sin(180 ) sin sin(AIO) Vậy: o 2 1 o 3 v IA sin( 30 ) v 0; v IB sin(150 ) Tương tự cho các trường hợp còn lại:
CHƯƠNG 4 : HỆ THỐNG ĐIỀU KHIỂN 113 o 1 3 o 2 v sin( 150 ) v 0; v sin(270 ) - Trường hợp 3: góc cua 270o - 30o o 3 2 o 1 v sin( 270 ) v 0; v sin(30 )
Lưu đồ giải thuật điều khiển chuyển động của robot trong đường ống theo hình 4.16 sau đây:
Hình 4.16: Giải thuật điều khiển chuyển động của robot.
3. Giải thuật trên mạch điều khiển Master:
Master có nhiệm vụ các nhiệm vụ chính: - Nhận lệnh điều khiển từ control box. - Thu thập dữ liệu từ hệ thống cảm biến.
CHƯƠNG 4 : HỆ THỐNG ĐIỀU KHIỂN
114
- Gửi lệnh điều khiển xuống các Slave tương ứng thông qua chuẩn giao tiếp CAN. Hình 4.17 mô tả lưu đồ giải thuật trên Master.
Hình 4.17: Giải thuật trên Master.
4. Giải thuật trên Slave:
Slave có nhiệm vụ nhận lệnh từ Master và điều khiển động cơ tương ứng. Chia thành 2 loại Slave với chức năng khác nhau:
CHƯƠNG 4 : HỆ THỐNG ĐIỀU KHIỂN
115
- 2 Slave điều khiển vị trí cam co duỗi chân Robot. Mỗi Slave loại này tự động thu nhận dữ liệu khoảng cách từ 3 potentiometer ở 3 chân Robot ứng với mỗi cụm bánh trước, bánh sau; tự động điều khiển động cơ cam tương ứng để đảm bảo lực bám của chân Robot lên thành ống; đồng thời cũng có thể nhận lệnh từ Master để khởi động hoặc dừng hoạt động của Robot. Bắt đầu Ngắt CAN Nhận lệnh điều khiển Yes No Kiểm tra ID Đọc CAN message và điều khiển động cơ
tương ứng Yes
No
Hình 4.18: Giải thuật trên Slave.
5. Giải thuật trên control box:
Hoạt động của robot được điều khiển và giám sát thông qua camera, người điều khiển dựa trên màn hình quan sát và thiết kế đường ống thực tế để ra quyết định điều khiển cho robot. Giao tiếp giữa control box và robot có thể thực hiện qua chuẩn RS232-RS485.
Lệnh truyền đi được thực hiện bằng cách mã hóa lệnh qua mã ASCII, robot nhận mã lệnh và thực hiện đáp ứng tương ứng với mã lệnh đã nhận.
CHƯƠNG 4 : HỆ THỐNG ĐIỀU KHIỂN
116
CHƯƠNG 5: THIẾT KẾ MÔ HÌNH, MÔ PHỎNG VÀ THỬ NGHIỆM
117
CHƯƠNG 5:
THIẾT KẾ MÔ HÌNH, MÔ PHỎNG VÀ THỬ NGHIỆM I. MÔ HÌNH THIẾT KẾ MÔ PHỎNG ROBOT:
Để tài thực hiện mô hình 3D robot trên Solidworks để bố trí, kiểm tra không gian lắp ráp các thành phần của robot; đồng thời mô phỏng chuyển động của robot trong đường ống để kiểm tra hoạt động của robot, khắc phục chỉnh sửa các cơ cấu, tối ưu hoạt động của robot.
Mô hình thiết kế như sau:
1. Mô hình 3D tổng thể robot:
CHƯƠNG 5: THIẾT KẾ MÔ HÌNH, MÔ PHỎNG VÀ THỬ NGHIỆM
118
2. Cụm chi tiết chân:
Hình 5.2: mô hình Solidworks mô phỏng cụm bánh xe.
3. Cụm chi tiết cam:
CHƯƠNG 5: THIẾT KẾ MÔ HÌNH, MÔ PHỎNG VÀ THỬ NGHIỆM
119
4. Cụm đảo chiều:
Hình 5.4: mô hình Solidworks mô phỏng cụm đảo chiều.
5. Bố trí vị trí tương quan các cụm chi tiết:
Tay đánh => (bánh cam chân trước) => (bộ đảo chiều) => (bánh cam chân sau)
CHƯƠNG 5: THIẾT KẾ MÔ HÌNH, MÔ PHỎNG VÀ THỬ NGHIỆM
120
II. MẠCH ĐIỆN THỬ NGHIỆMTHỰC TẾ:
Chế tạo thử nghiệm các mạch điện theo thiết kế, để đánh giá khả năng làm việc để có thay đổi phù hợp với yêu cầu hoạt động của robot.
1. Mạch nguồn:
- Sử dụng nguồn độc lập: tiến hành thử nghiệm chạy tải trên một bộ nguồn độc lập, ta có kết quả thực tế như bảng sau:
Điện áp vào Điện áp ra Tải thử nghiệm Dòng tải Nhiệt độ IC 25V-31,5V 24V±0,5% Động cơ 100W-24V, chạy không
tải 0,43A 40
oC 25V-31,5V 24V±0,5% Động cơ 100W-24V, giữ động cơ
không quay 2,94A 75
oC 25V-31,5V 24V±0,5% Động cơ 6W-24V điều khiển chân
robot, không tải 0,07A 33
oC 25V-31,5V 24V±0,5% Động cơ 6W-24V điều khiển chân
robot, giữ động cơ không quay 0,67A 43
oC 24V±0,5% 5V Mạch Slave điều khiển động cơ
chân robot 0,03A 31
oC - Nối song song 2 bộ nguồn 24V với nhau:
Điện áp vào Điện áp ra Tải thử nghiệm Dòng tải Nhiệt độ IC 25V-31,5V 24V±0,5% Động cơ 100W-24V, chạy không
tải 0,42A 37
oC 25V-31,5V 24V±0,5% Động cơ 100W-24V, giữ động cơ
không quay 2,95A 64
oC - Kết quả thử nghiệm cho thấy, mạch nguồn hoạt động ổn định.
Hình 5.6: Mạch nguồn thử nghiệm.
Nguồn ổn áp 24V-3A Nguồn hạ áp
CHƯƠNG 5: THIẾT KẾ MÔ HÌNH, MÔ PHỎNG VÀ THỬ NGHIỆM
121
2. Mạch Slave điều khiển động cơ:
- Giao tiếp CAN: truyền nhận tốt. - Giao tiếp RS232: truyền nhận tốt. - ADC: hoạt động tốt.
- Driver điều khiển động cơ:
+ Thử nghiệm quá tải: Thử nghiệm với động cơ điều khiển động cơ chân, giữ chặt động cơ không quay, duty 100%, dòng tải 0,71A, nhiệt độ IC driver lên 45oC. Mạch vẫn hoạt động ổn định.
+ Thử nghiệm đáp ứng động cơ: thử nghiệm với động cơ KM3448A-GGM 6W-24V, encoder 256p/r; điều khiển vận tốc động cơ, gây nhiễu bằng cách dùng kìm kẹp trục động cơ. Kết quả được trả về matlab qua chuẩn truyền RS232 và biểu diễn trên đồ thị sau với thời gian lấy mẫu 0,06s:
Hình 5.7: Đồ thị đáp ứng vận tốc module điều khiển động cơ.
Từ đồ thị ta thấy vận tốc động cơ điều khiển khá ổn định, dao động trong khoảng ±2vòng/phút so với vận tốc yêu cầu, đáp ứng khá nhanh.
Kết quả thu được cho thấy mạch chạy khá ổn định. Tùy theo bài toán và tải điều khiển thực tế của robot có thể áp dụng các giải thuật điều khiển để đáp ứng tốt yêu cầu đặt ra.
CHƯƠNG 5: THIẾT KẾ MÔ HÌNH, MÔ PHỎNG VÀ THỬ NGHIỆM
122
Hình 5.8: Mạch Slave điều khiển động cơ thử nghiệm.
II. KẾT LUẬN:
1. Các vấn đề đã giải quyết:
- Nghiên cứu cải tiến đường ống cho phù hợp với hoạt động của robot. - Thiết kế robot vệ sinh đường ống phù hợp với yêu cầu đặt ra.
- Thiết kế hệ thống điều khiển, mạch điện phù hợp với yêu cầu làm việc và bố trí trong robot đã thiết kế cơ khí.
- Thử nghiệm mạch nguồn ổn áp, mạch Slave điều khiển động cơ, giao tiếp CAN hoạt động ổn định.
2. Các vấn đề cần tiếp tục nghiên cứu và giải quyết:
- Kết cấu cơ khí: robot hiện tại sử dụng kết cấu vỏ dạng tròn, vỏ chịu lực và đỡ các bộ phân trong thân robot; kết cấu này làm kích thước bề rộng của robot khá lớn, đồng thời việc lắp ráp robot cũng khó khăn hơn kết cấu dựa trên trục.
Khắc phục: Nghiên cứu kết cấu cơ khí dựa trên trục, rút gọn các chi tiết thành phần để giảm kích thước cho robot
- Nghiên cứu thêm các giải pháp vệ sinh: như truyền động đầu công tác bằng trục mềm, hay sử dụng khí nén,… MCU Module driver DRV8000 LED test Nguồn 5V-3.3V Module CAN ADC
CHƯƠNG 5: THIẾT KẾ MÔ HÌNH, MÔ PHỎNG VÀ THỬ NGHIỆM
123
- Mạch điện chưa thử nghiệm bộ cảm biến IMU, chưa thử nghiệm mạch control đọc lệnh điều khiển thông qua gamepad.
Nguyên nhân: Thời gian hạn hẹp.
Khả năng tự nghiên cứu còn kém.
- Chưa lập trình điều khiển cho robot, mạch điều khiển chỉ mới chạy không tải, cần lập trình giải thuật điều khiển đáp ứng phù hợp với những thay đổi về tải khi robot hoạt động.
Nguyên nhân: Thời gian hạn hẹp.
Chưa chế tạo được mô hình thực tế. - Chưa thiết kế lắp đặt camera quan sát.
Nguyên nhân: Thời gian hạn hẹp.
Khắc phục: bằng giải pháp thay đổi kết cấu cơ khí dựa trên trục, lắp đặt camera trên đầu trục và có thể đi dây trong lòng trục rỗng.
CHƯƠNG 5: THIẾT KẾ MÔ HÌNH, MÔ PHỎNG VÀ THỬ NGHIỆM
121
CHƯƠNG 5:
THIẾT KẾ MÔ HÌNH, MÔ PHỎNG VÀ THỬ NGHIỆM I. MÔ HÌNH THIẾT KẾ MÔ PHỎNG ROBOT:
Để tài thực hiện mô hình 3D robot trên Solidworks để bố trí, kiểm tra không gian lắp ráp các thành phần của robot; đồng thời mô phỏng chuyển động của robot trong đường ống để kiểm tra hoạt động của robot, khắc phục chỉnh sửa các cơ cấu, tối ưu hoạt động của robot.
Mô hình thiết kế như sau:
1. Mô hình 3D tổng thể robot:
CHƯƠNG 5: THIẾT KẾ MÔ HÌNH, MÔ PHỎNG VÀ THỬ NGHIỆM
122
2. Cụm chi tiết chân:
Hình 5.2: mô hình Solidworks mô phỏng cụm bánh xe.
3. Cụm chi tiết cam:
CHƯƠNG 5: THIẾT KẾ MÔ HÌNH, MÔ PHỎNG VÀ THỬ NGHIỆM
123
4. Cụm đảo chiều:
Hình 5.4: mô hình Solidworks mô phỏng cụm đảo chiều.
5. Bố trí vị trí tương quan các cụm chi tiết:
Tay đánh => (bánh cam chân trước) => (bộ đảo chiều) => (bánh cam chân sau)
CHƯƠNG 5: THIẾT KẾ MÔ HÌNH, MÔ PHỎNG VÀ THỬ NGHIỆM
124
II. MẠCH ĐIỆN THỬ NGHIỆMTHỰC TẾ:
Chế tạo thử nghiệm các mạch điện theo thiết kế, để đánh giá khả năng làm việc để có thay đổi phù hợp với yêu cầu hoạt động của robot.
1. Mạch nguồn:
- Sử dụng nguồn độc lập: tiến hành thử nghiệm chạy tải trên một bộ nguồn độc lập, ta có kết quả thực tế như bảng sau:
Điện áp vào Điện áp ra Tải thử nghiệm Dòng tải Nhiệt độ IC 25V-31,5V 24V±0,5% Động cơ 100W-24V, chạy không