l i= R*bi
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à
300 tương ứng l à 0.523 rad theo phương z t hì mơ hì nh của khối điểm đặt trước l à:
* M_fil e dữ l iệ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
Hì nh 3.24 Đồ thị sai lệch vị trí của các khớp
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 He xapod tiến dần về 0
KẾT LUẬN
Ba chương l uận văn đã trình bày l à kết quả của việc nghiên cứu khai t hác bộ công cụ Si mmechanics của Matlab Si muli nk. Trên cơ sở đó xây dựng được sơ đồ mơ hình ho á ho àn c hỉnh Ro bot song song loại Hexapo d. Và khảo sát ho ạt động của Robot thơng qua mơ hì nh ho àn chỉnh đó. Bước đầu xây dựng được sơ đồ Si mulink mơ tả phương trình động học ngược, tìm r a s ai số gi ữa độ dài các c hâ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 c hủ độ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à c hưa đi sâu nghiê n cứu vào các hệ điều khiể n cụ thể.