X = [px py pz αβ γ]T
l i= R*bi p a
3.3.2.4 Khối điểm đặt trƣớc:
Theo yêu cầu đặt ra là điều khiển End Effector đến vị trí mong muốn có toạ độ [0 0 1.6] với góc lệch của hệ toạ độ mặt bàn gá phôi so với hệ toạ độ mặt nền là 300tương ứng là 0.523 rad theo phương z thì mơ hình của khối điểm đặt trước là:
Hình 3.21- Sơ đồ khối bộ điều khiển PID
* M_file dữ liệu điều khiển:
% Cac toa do
B1=[0.2415;-0.0647;1.273];% Toa do dau tren cua Upper leg 1 B2=[0.2415;0.0647;1.273];% Toa do dau tren cua Upper leg 2 B3=[-0.0647;0.2415;1.273];% Toa do dau tren cua Upper leg 3 B4=[-0.1768;0.;1.273];% Toa do dau tren cua Upper leg 4
B5=[-0.1768;-0.1768;1.273];% Toa do dau tren cua Upper leg 5 B6=[-0.0647;-0.2415;1.273];% Toa do dau tren cua Upper leg 6
body_pts=[B1 B2 B3 B4 B5 B6];% Ma tran toa do dau tren cua cac chan
A1=[0.7071;-0.7071;0];% Toa do dau duoi cua Lower leg 1 A2=[0.7071;0.7071;0];% Toa do dau duoi cua Lower leg 2 A3=[0.2588;0.9659;0];% Toa do dau duoi cua Lower leg 3 A4=[-0.9659;0.2588;0];% Toa do dau duoi cua Lower leg 4 A5=[-0.9659;-0.2588;0];% Toa do dau duoi cua Lower leg 5 A6=[0.2588;-0.9659;0];% Toa do dau duoi cua Lower leg 6
body_base=[A1 A2 A3 A4 A5 A6]% Ma tran toa do dau duoi cua cac chan
l_n=[1.273 1.273 1.273 1.273 1.273 1.273];% Chieu dai binh thuong cua cac chan
Kp=100; Ki=500; Kd=20;
Với dữ liệu điều khiển đưa vào sơ đồ Simulink hình 3.14 thì vị trí điểm tác động cuối chuyển động như hình 3.23
Số hóa bởi Trung tâm Học liệu – Đại học Thái Nguyên http://www.lrc-tnu.edu.vn
Nhận xét: Với các thông số đặt vào như dữ liệu bộ điều khiển đã đưa trong M-file thì khi điểm tác động cuối tiến đến vị trí điểm đặt trước, sai lệch vị trí của các khớp trượt (khớp chủ động) của các chân Hexapod tiến dần về 0
KẾT LUẬN
Ba chương luận văn đã trình bày là kết quả của việc nghiên cứu khai thác bộ công cụ Simmechanics của Matlab Simulink. Trên cơ sở đó xây dựng được sơ đồ mơ hình hố hồn chỉnh Robot song song loại Hexapod. Và khảo sát hoạt động của Robot thơng qua mơ hình hồn chỉnh đó. Bước đầu xây dựng được sơ đồ Simulink mơ tả phương trình động học ngược, tìm ra sai số giữa độ dài các chân thực tế và độ dài tính tốn đặt vào. Sai số này được đưa vào bộ điều khiển. Lấy tín hiệu đầu ra của bộ điều khiển này đưa tới điều khiển khớp chủ động của cơ cấu để hệ thống đi theo một quỹ đạo nào đó đã định trước. Tuy nhiên bộ điều khiển ở đây mới chỉ dừng lại ở bộ điều khiển kinh điển PID mà chưa đi sâu nghiên cứu vào các hệ điều khiển cụ thể.