4. CHƢƠNG IV ĐIỀU KHIỂN VÀ THIẾT KẾ MẠCH ĐIỀU KHIỂN
4.2 ĐIỀU KHIỂN ĐỊNH HƢỚNG
Chuyển động của robot rắn theo phƣơng trình
sin( ( 1) ) ( 1 )
i t i i n
(4.1)
Có tham số điều khiển hƣớng là γ. Khi γ=0, robot chạy thẳng. Khi γ>0, robot rẽ trái theo đƣờng tròn và ngƣợc lại γ <0 robot sẽ rẽ phải.
Khi tăng độ lớn của γ sẽ làm giảm bán kính của đƣờng tròn, hay nói cách khác thì càng cao, robot sẽ rẽ càng gấp. Nhƣ vậy, việc chọn hợp lý sẽ là sự kết hợp của tốc độ rẽ và độ ổn định của robot.
Ta sử dụng thuật toán Fuzzy để điều khiển hƣớng do bộ điều khiển Fuzzy có thể sử dụng các biến ngôn ngữ để điều khiển hệ thống phức tạp với cấu trúc bộ điều khiển đơn giản
Tín hiệu đƣa vào bộ điều khiển sẽ là độ sai lệch giữa hƣớng di chuyển của robot và hƣớng mong muốn đến mục tiêu
Hình 4.4 : Mô hình xác định hƣớng của robot Hƣớng của robot đƣợc định nghĩa bởi công thứ
1 1 n i i n (4.2) Hƣớng mong muốn * là hƣớng từ trọng tâm robot đến mục tiêu
Độ lệch hƣớng giữa hƣớng mong và hƣớng của robot là *
Để thực hiện giải thuật điều khiển điều khiển định hƣớng cho robot rắn, ta coi mục tiêu sẽ đƣợc phát một nguồn sáng về phía robot. Cảm biến định hƣớng đƣợc sử dụng là quang điện trở. Dùng để xác định góc lệch của robot với mục tiêu. Quang điện trở (LDR) là một cảm biến điện trở nhạy sáng làm bằng vật liệu bán dẫn Cadmium Sulfide (CdS). Bình thƣờng, trở kháng của LDR là rất cao khoảng 1.000.000 Ω , khi đƣợc chiếu sáng với ánh sáng thì trở kháng giảm rất nhiều.
Hình 4.5 : Mạch quang điện trở
Khi mức độ ánh sáng thấp điện trở của LDR là cao. Điều này làm các làm các transistors không dẫn. Do đó LED không sáng.
Tuy nhiên, khi có ánh sáng chiếu lên bề mặt LDR nó phân cực cho transistror đầu tiên dẫn, và do đó transistor thứ 2 dẫn, đèn LED sẽ phát sáng.
Một biến trở đặt sẵn có thể điều chỉnh để tăng hoặc giảm trở kháng của mạch (LDR), trong cách này, nó có thể làm cho độ nhạy của mạch thay đổi
Chọn nguồn sáng là một bóng đèn Rạng Đông Compact 40W đặt cách robot 2 m.Độ rọi sáng của bóng đèn vào khoảng 500Lux. Khi robot hoạt động trong điều kiện bình thƣờng, với các bố trí cảm biến nằm ngang, độ rọi sáng vào hai cảm biến do ánh sáng môi trƣờng tạo ra ở vào khoảng 50Lux. Nhƣ vậy, điện trở của LDR sẽ biến thiên trong khoảng 1÷10 kΩ ( 1 lux là độ rọi có đƣợc của một bề mặt có diện tích 1 mét vuông có thông lƣợng chiếu sang 1 lumen )
Một cặp LDR sẽ đóng vai trò cảm biến ánh sáng để định hƣớng cho robot. Đặt 2 LDRs định vị trí chúng 60° theo hƣớng của khâu đầu để xác định góc tƣơng đối của khâu với nguồn sang thông qua tính hiệu của cƣờng độ sáng. Cặp LDR sẽ cung cấp vị trí nguồn sáng về mạch điều khiển và tạo thành một vòng hồi tiếp. Điểm quan trọng ở đây là ta sử dụng giá trị sai lệch giữa LDR trái và LDR phải, và giảm đƣợc nhiễu do ánh sáng bên ngoài.
Hình 4.7 : Bố trí cảm biến định hƣớng
4.2.1.1Mờ Hóa
Giá trị đầu vào của tập mờ là góc lệch của robot rắn với mục tiêu. Giá trị đầu ra là hệ số là hệ số định hƣớng trong phƣơng trình chuyển động của robot rắn. Sử dụng bộ ADC 10 bit của vi điều khiển, với cảm biến LDR 12mm đã đƣợc chọn, điện áp chân Analog đạt đƣợc trong khoảng 1÷5V. Tín hiệu cảm biến đƣợc đƣa về sẽ nằm trong khoảng [100 1000] và tín hiệu sai lệch (LDR trái – LDR phải) nằm trong khoảng [-900 900].
4.2.1.2Hệ luật mờ
Nếu 0 thì 0 (mục tiêu lệch bên trái – robot rẻ trái) Nếu 0 thì 0 (mục tiêu không lệch – robot đi thẳng) Nếu 0 thì 0 (mục tiêu lệch bên phải – robot rẻ phải)
Tín hiệu vào biến thiên trong khoảng [-900 900 ], ta cho tín hiệu ra biến thiên trong khoảng [- 600 600 ] .
Sử dụng phƣơng pháp suy diễn MAX – MIN để tạo thành quan hệ mờ và phƣơng pháp MAMDANI để kết hợp các luật hợp thành.
Hình 4.8 : Sơ đồ bộ điều khiển Fuzzy
Hình 4.10 : Sơ đồ Membership Function output Trọng số của từng luật đƣợc tính theo suy diễn Min – Max
1 min( ( ) & ( )) w Ne Ne (4.3) 2 min( ( ) & ( )) w Ze Ze (4.4) 3 min( ( ) & ( )) w Po Po (4.5) Kết quả là đầu ra γ đƣợc tính theo công thức
1 2 3
( ) max(w w w, , )
(4.6)
4.2.1.3Giải mờ
Sử dụng phƣơng pháp trọng tâm (centroid) để giải mờ, tích phân số đƣợc tính gần đúng bằng công thức Simpson
* ( ). ( ) B B y ydy y y dy (4.7) Hình 4.11: Phƣơng pháp giải mờ theo tọa độ trọng tâm
Bộ ADC của vi xử lý sẽ xác định độ sai lệch giữa 2 cảm biến LDR trái và LDR phải.Góc γ đƣợc xác định theo thuật toán Fuzzy và tính góc quay của mỗi module theo phƣơng trình chuyển động. Từ đó chuyển thành xung để kích cho các động cơ servo quay theo mong muốn
4.3 THIẾT KẾ MẠCH ĐIỆN CHO ROBOT RẮN 4.3.1 Mạch Nguồn cho vi điều khiển trên robot 4.3.1 Mạch Nguồn cho vi điều khiển trên robot
Sử dụng loại pin sạc 860mA nguồn 7.4V
Hình 4.13 : Pin sài cho mô hình robot
Tất cả các module của robot điều sử dụng 1 nguồn chung từ pin . Riêng vi điều khiển dùng IC LM7805 để chuyển thành điện áp 5V cho mạch từ pin .
Hình 4.14: Mạch nguồn cho vi điều khiển trên robot
4.3.2 Mạch cảm biến xác định hƣớng
Các cảm biến sẽ đƣợc mắc nối tiếp với một biến trở 5KΩ làm cầu chia áp cho chân Analog AN0 và AN1 của vi điều khiển. Sự biến thiên cƣờng độ sáng làm biến thiên điện áp vào chân Analog. Giá trị điện trở của LDR biến thiên từ 1÷10KΩ nên ta đặt giá trị biến trở khoảng 5KΩ. Điện áp vào chân Analog sẽ biến thiên từ 1÷5V
Hình 4.15 : Sơ đồ mạch định hƣớng LDR
4.3.3 Mạch vi điều khiển
Sử dụng vi điều khiển PIC18F4431 để điều khiển robot rắn.Với các chân RB0-RB4 đƣợc nối với chân tính hiệu của động cơ RC servo. Và hai chân Analog AN0 và AN1 đƣợc nối với cảm biến quang (LDR).
4.3.4 Moduel các động cơ và Led báo tín hiệu của động cơ
Hình 4.17: Module nối tới các động cơ
4.4 LƢU ĐỒ ĐIỀU KHIỂN ROBOT RẮN
Vi điều khiển sẽ đọc tín hiệu Analog từ hai kênh AN0 và AN1 từ đây tính đƣợc góc lệch giữa robot và mục tiêu.
Dùng Fuzzy để tính các góc γ và tính các góc cho từng động cơ.Sau đó chuyển các giá trị góc thành các giá trị xung và xuất ra điều khiển các động cơ điều khiển robot chuyển động tới mục tiêu
5. CHƢƠNG V : MÔ PHỎNG VÀ THỰC NGHIỆM 5.1 CÁC THÔNG SỐ MÔ PHỎNG 5.1 CÁC THÔNG SỐ MÔ PHỎNG
Số khâu n=6 Khối lƣợng của khâu m=0,4 (kg)
Chiều dài khâu l=0,24m Hệ số Kp của servo Kp= 100 Hệ số α=π/6 (rad) Hệ số Ki của servo Ki = 10 Góc lệch pha β=π/3(rad) Hệ số Kd của servo Kd = 10 Vận tốc góc ω=1(rad/s) Moment quán tính I=ml2/3 (kg.m2)
5.2 MÔ PHỎNG VỚI SIM MECHANICS 5.2.1 Xây dựng mô hình mô phỏng 5.2.1 Xây dựng mô hình mô phỏng
Mỗi khâu đƣợc nối với nhau bởi một khớp quay có gắn động cơ servo, robot có ba bậc tự do chuyển động so với đất gồm hai bậc tịnh tiến theo x, y và một bậc quay theo z.
Mỗi khâu sẽ chịu lực tác động của lực ma sát (khối Friction) . Mô hình lực ma sát nhớt nhƣ sau
Hình 5.2 : Mô hình lực ma sát
Hình 5.3 : Mô hình động cơ Servo
5.2.2 Xây dựng mô hình mục tiêu
Ta xây dựng 2 mục tiêu có tọa độ khác nhau để làm mục tiêu cho robot rắn di chuyển.Đầu tiên robot sẽ di chuyển đến mục tiêu thứ nhất và sau đó sẽ tiếp đến mục tiêu thứ hai
Hình 5.4 : Mô hình 2 mục tiêu
5.2.3 Bộ điều khiển của robot
Bộ điều khiển trung tâm sẽ quyết định hình dạng của robot tùy theo mục đích điều khiển (điều khiển hƣớng để bám mục tiêu, hoặc điều khiển tốc độ).Tùy vào mục đích điều khiển bộ điều khiển sẽ cho ra các giá trị để động cơ servo thực thi. Động cơ servo sẽ nhận tín hiệu góc tham chiếu từ bộ điều khiển để cấp moment vào các khớp quay. Do đó hình dạng robot sẽ chạy theo mong muốn của bộ điều khiển.
Ta xây dựng bộ điều khiển Fuzzy để robot di chuyển bám theo mục tiêu
Tín hiệu đƣa vào bộ điều khiển sẽ là độ sai lệch giữa hƣớng di chuyển của robot và hƣớng mong muốn đến mục tiêu .
Tín hiệu vào biến thiên trong khoảng [-π/2 π/2 ], ta cho tín hiệu γ biến thiên trong khoảng [- 1 1 ] .
Hình 5.6 : Membership Function input
Hình 5.8 : Bộ Điều khiển Fuzzy trung tâm và các tín hiệu điều khiển
5.2.4 Kết quả mô phỏng
Cho robot di chuyển đến mục tiêu thứ nhất có tọa độ [17 17] và mục tiêu thứ 2 có tọa độ [20 25] Kết quả robot đi qua cả 2 mục tiêu
Chuẩn bị qua mục tiêu thứ nhất Qua mục tiêu thứ nhất
Chuẩn bị qua mục tiêu thứ 2 Qua mục tiêu thứ 2
Hình 5.10 : Quá trình di chuyển qua 2 mục tiêu của robot
Việc điều khiển định hƣớng bằng Fuzzy Logic đã thực hiện đƣợc. Tuy nhiên,tọa độ trọng tâm khi kết thúc mô phỏng vẫn còn lệch so với vị trí của mục tiêu. Cần cải thiện để robot di chuyển chính xác hơn
5.3 THỰC NGHIỆM 5.3.1 Xây dựng mô hình
Mạch Điều khiển robot
Hình 5.12 : Mạch điều khiển robot Một đốt của robot
Hình 5.14 : Một đốt thực tế của robot rắn Toàn bộ robot
Hình 5.16 : Mô hình thực tế của robot rắn
5.3.2 Kết Quả
Mô hình thực nghiệm đã kiểm tra đƣợc thuật toán điều khiển động cơ và điều khiển robot theo dạng hình sin
6. CHƢƠNG VI : KẾT LUẬN 6.1 KẾT QUẢ ĐẠT ĐƢỢC
- Tính toán động học và động lực học cho robot rẳn ở dạng tổng quát - Viết chƣơng trình tính toán động lực học cho robot rắn
- Mô phỏng chuyển động của robot
- Điều khiển robot bám mục tiêu vơi bộ điều khiển Fuzzy - Thiết kế robot rắn chuyển động
- Thiết kế robot rắn chuyển động bám mục tiêu - Xây dựng mô hình robot rắn
- Thiết kế mạch điều khiển cho robot rắn
6.2 KẾT QUẢ CHƢA ĐẠT ĐƢỢC
- Phần cứng chính xác với thiết kế
- Giải thuật Fuzzy dƣới vi điều khiển chƣa chính xác nhƣ mô phỏng - Cảm biến định hƣớng góc còn hạn chế [-60 60]
6.3 HƢỚNG PHÁT TRIỂN
- Mô phỏng và thiết kế robot rắn có thể di chuyển dƣới nƣớc
- Mô phỏng và thiết kế robot rắn có thể chuyển động trong không gian 3D
- Sử dụng các cảm biến khác nhƣ GPS, Camera để định vị, giám xác robot và xác định mục tiêu .
TÀI LIỆU THAM KHẢO
[1] S.A. Fjerdingen et al, Adaptive Snake Robot Locomotion: A Benchmarking Facility for Experiments, H. Bruyninckx et al. (Eds.): European Robotics Symposium 2008, STAR 44, pp. 13–22, 2008.
[2] The MathWorks, Inc, Fuzzy Logic Toolbox™ User’s Guide
[3] William R. Hutchison, Betsy J. Constantine, Johann Borenstein, and Jerry Pratt Development of Control for a Serpentine Robot”
[4] Toshio TAKAYAMA, Shigeo HIROSE “Amphibious 3D Active Cord Mechanism HELIX with Helical Swimming Motion”
[5] ENDO, Keiji TOGAWA and Shigeo HIROSE “Study on Gen Self-contained and Terrain
Active Cord Mechanism Adaptive”
[6] Brad Oraw & Jeremy Tinder “Proposal for a Modular Snake Robot”
[7] Kevin J. Dowling; Advised by William L. Whittaker “Limbless Locomotion: Learning to Crawl with a Snake Robot”
[8] P. Liljebäck, Ø. Stavdahl and K. Y. Pettersen, Modular Pneumatic Snake Robot: 3D Modelling, Implementation and Control, Proceedings of 16th IFAC World Congress,
Prague,Czech Republic (Jul. 2005). M. Saito, M. Fukaya and T. Iwasaki, “Serpentine locomotion with robotic snakes,” IEEE Contr. Syst. Mag. 22(1), 64–81 (Feb. 2002).
[9] SHIGEO HIROSE AND HIROYA YAMADA “Snake-Like Robots - Machine Design of Biologically Inspired Robots ”
[10] Bài giảng “Máy và hệ thống thông minh ” PGS.TS Từ Diệp Công Thành - ĐHBK TP HCM
PHỤ LỤC A
CHƢƠNG TRÌNH MATLAB TÍNH ĐỘNG LỰC HỌC CHO ROBOT clc; clear all; echo off; format long; global invM; global N1; global N2; global N3; global N4; global N5; global N6; global Q1; global Q2; global Q3; global Q4; global Q5; global Q6; global f1; global f2; global f3; global f4; global f5; global f6; global FF; global Ff; global FF_AV delta_t = 0.01;
i_end_2 = 12000; % total time %Thong so tinh luc ma sat
g=9.98;
mit= 0.1; % he so muy(u) theo phuong tiep tuyen t
min= 0.5;
% mit= 0.05; % he so muy(u) theo phuong tiep tuyen t % min= 0.56;
% Khai bao cac thong so lien quan den hinh dang hinh hoc cua ca
i1 = 7.68*10^3; m1 = 0.4;
l1 = 0.21; a1 = 0.105;
k1 = 0.5; c1 = 10^-3; i2 = 7.68*10^3; m2 = 0.4; l2 = 0.24; a2 = 0.12; k2 = 0.5; c2 = 10^-3; i3 = 7.68*10^3; m3 = 0.4; l3 = 0.24; a3 = 0.12; k3 = 0.5; c3 = 10^-3; i4 = 7.68*10^3; m4 = 0.4; l4 = 0.24; a4 = 0.12; k4 = 0.5; c4 = 10^-3; i5 = 7.68*10^3; m5 = 0.2; l5 = 0.24; a5 = 0.12; k5 = 0.5; c5 = 10^-3; i6 = 7.68*10^3; m6 = 0.24; l6 = 0.24; a6 = 0.12;
sampling = 0.01;%thoi gian lay mau
DtoR = pi/180;% he so chuyen sang radian % Input functions data
%thong so tin hieu dieu khien dong co
A1 = pi/2; A2 = A1; A3 = A1; A4 = A1; A5 = A1; A6 = A1;
f1 = 1.5; f2 = f1; f3 = f1; f4 = f1; f5 = f1; f6 = f1; beta=60; beta1=60; beta2=60; beta3=60 ; beta4=60; beta5=60; beta6=60; % he so de tinh trong SVD SVD_Max = 0.0355; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % khai bao cac luc Ma Sat ban dau,gia su van toc ban dau duong nen % Theo Phuong x Fx1 = 0; Fx2 = 0; Fx3 = 0; Fx4 = 0; Fx5 = 0; Fx6 = 0; % Theo Phuong y Fy1 = 0; Fy2 = 0; Fy3 = 0; Fy4 = 0; Fy5 = 0; Fy6 = 0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% NN = []; FF = []; P = []; DD = []; tor1 = [];
FF =0; % Khai bao vector cua luc day FF de cong don gia tri tai tung thoi diem
P = 0; tor_01 = 0;
invm = zeros(6,6);
x0 = [0 0 0 0 0 0 0 0 0 0 0 0]; % Gia tri ban dau cho cac goc: x0(1): theta1, x0(2): theta1_dot, x0(3): theta2, x0(4): theta2_dot, x0(5): theta3, x0(6): theta3_dot,x0(7):theta4,x0(8):theta4_dot, x0(9):theta5, x0(10):theta5_dot, x0(11):theta6, x0(12):theta6_dot
theta2_begin = (beta*DtoR)/(2*pi*f2); theta2_begin_step = theta2_begin/delta_t; if theta2_begin_step <= 0.5 theta2_begin_time = 1; else theta2_begin_integer = fix(theta2_begin_step);
theta2_begin_real = theta2_begin_step - theta2_begin_integer; if theta2_begin_real >= 0.5
theta2_begin_real = 1; else
theta2_begin_real = 0; end
theta2_begin_time = (theta2_begin_integer + theta2_begin_real);
end
for i=1:i_end_2 i
s_t = (i-1)*delta_t; e_t = i*delta_t;
tspan = [s_t e_t];%s_t start time va e_t la end time
x_tmp = size(x0,1); x = x0(x_tmp,:);
% Khai bai cac tin hieu (moment xoan)dieu khien dau vao cho dong co RC -
x(1) = A1*(sin(2*pi*f1*s_t*DtoR)); x(3) = A2*(sin((2*pi*f2*s_t*DtoR - beta2*DtoR))); x(5) = A3*(sin((2*pi*f3*s_t*DtoR - 2*beta3*DtoR))); x(7) = A4*(sin((2*pi*f4*s_t*DtoR - 3*beta4*DtoR))); x(9) = A5*(sin((2*pi*f5*s_t*DtoR - 4*beta5*DtoR))); x(11) = A6*(sin((2*pi*f6*s_t*DtoR - 5*beta6*DtoR))); T1 = 0.1; T2 = T1; T3 = T1; T4 = T1; T5 = T1; T6 = T1;
% KHai bao bo dong hoc cua robot ca
m11 = i1+m1*a1^2; m12 = 0; m13 = 0; m14 = 0; m15 = 0; m16 = 0; m21 = m2*a2*l1*cos(x(3)-x(1)); m22 = i2+m2*a2^2; m23 = 0;
m24 = 0; m25 = 0; m26 = 0; m31 = m3*a3*l1*cos(x(5)-x(1)); m32 = m3*a3*l2*cos(x(5)-x(3)); m33 = i3+m3*a3^2; m34 = 0; m35 = 0; m36 = 0; m41 = m4*a4*l1*cos(x(7)-x(1)); m42 = m4*a4*l2*cos(x(7)-x(3)); m43 = m4*a4*l3*cos(x(7)-x(5)); m44 = i4+m4*a4^2; m45 = 0; m46 = 0; m51 = m5*a5*l1*cos(x(9)-x(1)); m52 = m5*a5*l2*cos(x(9)-x(3)); m53 = m5*a5*l3*cos(x(9)-x(5)); m54 = m5*a5*l4*cos(x(9)-x(7)); m55 = i5+m5*a5^2; m56 = 0; m61 = m6*a6*l1*cos(x(11)-x(1)); m62 = m6*a6*l2*cos(x(11)-x(3)); m63 = m6*a6*l3*cos(x(11)-x(5)); m64 = m6*a6*l4*cos(x(11)-x(7)); m65 = m6*a6*l5*cos(x(11)-x(9)); m66 = i6+m6*a6^2;
% Update the value of tt value
tt1=l1*[c1*x(2)-c1*x(4)+k1*(x(1)-x(3))]; tt2=l2*[c1*x(2)+(c1+c2)*x(4)-c2*x(6)+(k1+k2)*x(3)-k1*x(1)-k2*x(5)]; tt3=l3*[c2*x(4)+(c2+c3)*x(6)-c2*x(8)+(k2+k3)*x(5)-k1*x(3)-k2*x(7)];