Điều khiển Robot

trước của tay Robot dọc theo quỹ đạo cho ở hình 2. Thiết kế “bộ điều khiển bù trọng lực” cho Robot. Mô phỏng hệ thống với bộ điều khiển ở câu 4 khi góc quay của khớp 1 thay đổi nhảy cấp từ 0 đến 1 rad. Hình 1. Cơ cấu động học robot hai thanh nối. Hình 2. Đồ thị tốc độ. Bảng thông số của Robot: STT Đại lượng Giá trị 1 Chiều dài thanh nối1 (l1) 0,4m 2 Chiều dài thanh nối1 (l2) 0,3m 3 Khối lượng thanh nối 1 (m1) 10Kg 4 Khối lượng thanh nối 2 (m2) 5,0Kg 5 Momen quán tính khớp 1 quay quanh tâm khối (J1) 0,528 Kgm2 6 Momen quán tính khớp 2 quay quanh tâm khối (J2) 0,19 Kgm2 7 Khoảng cách từ khớp 1 đến tâm khối 1 (lg1) 0,25 m 8 Khoảng cách từ khớp 2 đến tâm khối 2 (lg2) 0,15 m 9 Hằng số momen của động cơ khớp 1, 2 (KM) 0,5 Nm/A 10 Điện trở phần ứng (r1,r2) 3 W 11 Momen lớn nhất của động cơ khớp 1 (M1max) 1,2 Nm 12 Momen lớn nhất của động cơ khớp 2 (M2max) 0,7 Nm 13 Tốc độ lớn nhất của động cơ khớp 1, 2 (wmax) 90 rad/s 14 Momen quán tính của động cơ (JĐ) 0,0004 Kgm2 15 Khối lượng tải lớn nhất (mt) 5 Kg 16 Tỉ số truyền cho cả hai khớp (i) 12 Bài giải Câu 1. Xây dựng phương trình động học thuận và ngược biểu diễn mối quan hệ giữa hệ toạ độ tay Robot (End effector) và hệ toạ độ các khớp (Joints) a. Xây dựng phương trình động học thuận Bài toán động học thuận là bài toán đi tìm các thông số về vị trí và hướng của tay robot so với khung cơ sở (gốc). Thực hiện phép biến đổi đồng nhất dựa trên hệ tọa độ thanh nối (Denavit - Hartenberg) mối quan hệ giữa hệ toạ độ tay Robot (End effector) và hệ toạ độ các khớp (Joints) được xác định như sau: (1) Với: : biểu diễn hệ toạ độ thanh thứ n so với hệ toạ độ gốc. : biểu diễn hệ toạ độ tay (điểm kẹp) so với thanh n. : được định nghĩa là ma trận chuyển đổi đồng nhất, biểu diễn mối quan hệ vị trí của một điểm trong khung i và vị trí của điểm đó trong khung thứ i-1. Từ mô hình cơ cấu động học của robot ta xây dựng được bảng Denavit - Hartenberg như sau: Thanh ai ai qi di 1 2 l1 l2 p/2 p/2 q1 q2 0 0 Theo đó ta xác định được các ma trận biến đổi A của robot như sau: Thay vào (1) ta được: (2) Với : P là điểm thuộc hệ toạ độ gắn với tay robot, có vị trí được xác định bằng vectơ cột thứ tư của . Từ (2) ta được phương trình động học thuận biểu diễn mối quan hệ giữa hệ tọa độ tay Robot và hệ tọa độ các khớp: (3) b. Xây dựng phương trình động học ngược Bài toán động học ngược là tìm vị trí các khớp khi biết vị trí của tay robot (End-effector). Ma trận biểu diễn vị trí tay robot: Theo (1) ta có: Nhân hai vế của (1) với ta có: (4) tương đương với: (5) trong đó: Cân bằng vế trái và phải của (5), ta có: (6) Với Bình phương hai vế của hai phương trình trong (6) và cộng vế ta có: (7) Đặt : ta có: (8) Thay (7) vào (6) ta được: Giải ra ta có: (9) trong đó: . Vậy phương trình động học ngược của hệ đã cho được biểu diễn dưới dạng (10) Với : Câu 2: Xây dựng quan hệ giữa tốc độ các khớp và tốc độ của tay Robot Đạo hàm hai vế phương trình động học thuận theo thời gian ta có: hay (11) Đặt: (J: ma trận Jacobien) Phương trình biểu diễn quan hệ giữa tốc độ các khớp và tốc độ của tay Robot được biểu diễn dưới dạng: (12) Với: Câu 3. Viết hàm MATLAB thực hiện các phương trình ở câu 1 và 2 và vẽ đường biểu diễn vị trí và tốc độ khớp khi tay Robot di chuyển từ vị trí [0,4:0,0] đến [0,0:0,4] theo một đường thẳng. Đồ thị tốc độ của tay Robot đã cho như hình 2 a. Chương trình tính toán động học thuận Dựa vào các công thức tính toán động học thuận (3) ta xây dựng được hàm tính toán động học thuận cho robot như sau: function xy=Thuan(l1,l2,d1,d2) % Chuyen doi gia tri goc tu do ra radian r_d1=d1*2*pi/360; r_d2=d2*2*pi/360; %Tinh toan xy(1)=l1*cos(r_d1)+l2*cos(r_d1+r_d2); % Vi tri tay theo truc x xy(2)=l1*sin(r_d1)+l2*sin(r_d1+r_d2); % Vi tri tay theo truc y xy(3)=0; % Vi tri tay theo truc z b. Chương trình tính toán động học ngược Dựa vào hệ phương trình tính động học ngược (10) ta có hàm tính toán động học ngược robot như sau: function d=Nguoc(x,y,l1,l2) k1=sqrt((x^2+y^2+l1^2+l2^2)^2-2*((x^2+y^2)^2+l1^4+l2^4)); k2=(x^2+y^2)-(l1^2+l2^2); k3=(x^2+y^2)+(l1^2-l2^2); d1=atan2(y,x)-atan2(k1,k3); %Goc theta 1 tinh theo radian. d2=atan2(k1,k2); %Goc theta 2 tinh theo radian. d(1)=d1*360/(2*pi) %Goc theta 1 tinh theo do. d(2)=d2*360/(2*pi) %Goc theta 2 tinh theo do. c. Chương trình tính tốc độ quay khớp Dựa vào phương trình biểu diễn quan hệ giữa tốc độ các khớp và tốc độ của tay robot ta xây dựng được hàm tính toán tốc độ quay của các khớp như sau: % Chuong trinh tinh toc do quay khop function vd=tocdoquay(vx,vy,d1,d2,l1,l2) % Chuyen doi d1,d2 tu do(°) sang radian() r_d1=d1*2*pi/360; r_d2=d2*2*pi/360; J=[-l1*sin(d1)-l2*sin(d1+d2), -l2*sin(d1+d2); l1*cos(d1)+l2*cos(d1+d2), l2*cos(d1+d2)] % Ma tran Jacobien v=[vx;vy]; % Vecto van toc theo truc x va y. vd=inv(J)*v; % Van toc goc cua khop 1 va 2 tinh theo radian/s d. Vẽ đường biểu diễn vị trí và tốc độ khớp khi tay Robot di chuyển từ vị trí [0,4:0,0 m] đến [0,0:0,4 m] theo một đường thẳng Dựa vào dạng đồ thị tốc độ ở hình 2 ta có quỹ đạo chuyển động của tay robot là quỹ đạo 2-1-2. Mặt khác tay Robot di chuyển từ vị trí [0,4: 0,0 m] đến [0,0: 0,4 m] theo một đường thẳng thực chất là chuyển động tịnh tiến theo cả hai trục x và y. Xét khoảng thời gian từ 0á0,25s, tay Robot di chuyển theo quỹ đạo bậc 2: Các điều kiện biên gồm: ị Xét khoảng thời gian từ 0,25á0,75s, robot di chuyển theo quỹ đạo bậc 1: Từ điều kiện biên suy ra: Xét khoảng thời gian từ 0,75á1s, robot di chuyển theo quỹ đạo bậc 2: Dựa vào các điều kiện biên có: ị Xét khoảng thời gian sau 1 giây ta có: ị Mặt khác để đảm bảo tính liên tục của quỹ đạo ta có: ị ị Vậy ta có: Thay vào các hệ số của phương trình quỹ đạo chuyển động ở trên ta được phương trình quỹ đạo chuyển động của robot như sau: Chương trình MATLAB sau thể hiện quỹ đạo chuyển động của tay robot theo vị trí và tốc độ: Quỹ đạo chuyển động của tay robot: function cdtay=cdtay(N) %N so diem quy dao can ve for i=0:N t=i/N; time(i+1)=t; if t<=0.25 x(i+1)=-1.0666*t*t+0.4; vx(i+1)=-2.1332*t; y(i+1)=1.0666*t*t; vy(i+1)=2.1332*t; elseif (t>0.25)&(t<=0.75) x(i+1)=-0.5333*(t-0.25)+0.3333; vx(i+1)=-0.5333; y(i+1)=0.5333*(t-0.25)+0.0667; vy(i+1)=0.5333; elseif(t>0.75)&(t<=1) x(i+1)=1.0666*(t-0.75)*(t-0.75)-0.5333*(t-0.75)+0.0666; vx(i+1)=2.1332*(t-0.75)-0.5333; y(i+1)=-1.0666*(t-0.75)*(t-0.75)+0.5333*(t-0.75)+0.3333; vy(i+1)=-2.1332*(t-0.75)+0.5333; else x(i+1)=0; y(i+1)=0.4; end %if end %for plot(time,[x;vx;y;vy]);%ve quy dao chuyen dong va van toc grid; xlabel('t(s)'); ylabel('s(m)&v(m/s)'); Kết quả thu được khi chạy chương trình với 200 điểm quỹ đạo được thể hiện trên hình vẽ 3. >> cdtay(200) Hình 3. Quỹ đạo chuyển động của tay robot Để xác định tốc độ và vị trí các khớp, cần tính được các giá trị x,y rời rạc của tay Robot sau đó thông qua phương trình động học ngược xác định góc quay q1 và q2. Biểu diễn q1 và q2 trên đồ thị thời gian, ta được đồ thị biểu diễn vị trí của khớp. Đạo hàm chuyển động của từng khớp theo thời gian ta sẽ được dạng đường vận tốc của khớp. Quỹ đạo chuyển động khớp: function cdkhop=cdkhop(N) %N so diem quy dao can ve l1=0.4; l2=0.3; for i=0:N t=i/N; time(i+1)=t; if t<=0.25 x=-1.0666*t*t+0.4; vx=-2.1332*t; y=1.0666*t*t; vy=2.1332*t; elseif (t>0.25)&(t<=0.75) x=-0.5333*(t-0.25)+0.3333; vx=-0.5333; y=0.5333*(t-0.25)+0.0667; vy(i+1)=0.5333; elseif(t>0.75)&(t<=1) x=1.0666*(t-0.75)*(t-0.75)-0.5333*(t-0.75)+0.0666; vx=2.1332*(t-0.75)-0.5333; y=-1.0666*(t-0.75)*(t-0.75)+0.5333*(t-0.75)+0.3333; vy=-2.1332*(t-0.75)+0.5333; else x=0; y=0.4; end %if d=Nguoc(x,y,l1,l2); theta1(i+1)=d(1); theta2(i+1)=d(2); end %for plot(time,[theta1;theta2]); grid; xlabel('t(s)'); ylabel('Goc(do)'); Chạy chương trình với số điểm là 150 ta được: >> cdkhop(150) Hình 4. Quỹ đạo chuyển động của các khớp Chương trình tính toán và vẽ đồ thị vận tốc chuyển động của khớp: function vkhop=vkhop(N) %N so diem quy dao can ve l1=0.4; l2=0.3; for i=0:N t=i/N; time(i+1)=t; if t<=0.25 x=-1.0666*t*t+0.4; vx=-2.1332*t; y=1.0666*t*t; vy=2.1332*t; elseif (t>0.25)&(t<=0.75) x=-0.5333*(t-0.25)+0.3333; vx=-0.5333; y=0.5333*(t-0.25)+0.0667; vy(i+1)=0.5333; elseif(t>0.75)&(t<=1) x=1.0666*(t-0.75)*(t-0.75)-0.5333*(t-0.75)+0.0666; vx=2.1332*(t-0.75)-0.5333; y=-1.0666*(t-0.75)*(t-0.75)+0.5333*(t-0.75)+0.3333; vy=-2.1332*(t-0.75)+0.5333; else x=0; y=0.4; end %if d=Nguoc(x,y,l1,l2); theta1(i+1)=d(1); theta2(i+1)=d(2); end %for % Lay dao ham chuyen dong khop theo goc quay cua tung khop. for i=1:N-1 omega1(i)=(theta1(i+1)-theta1(i))*N; % dw1/dt omega2(i)=(theta2(i+1)-theta2(i))*N; % dw2/dt end; omega1(N)=0.0001; omega1(N+1)=0; omega2(N)=0.0001; omega2(N+1)=0; plot(time,[omega1;omega2]); grid; xlabel('t(s)'); ylabel('Van toc goc(do/s)'); Chạy chương trình với số điểm vẽ N=150 ta được: >>vkhop(150); Hình 5. Đồ thị vận tốc chuyển động của khớp Câu 4. Thiết kế bộ điều khiển bù trọng lực cho robot a. Xây dựng phương trình động lực học Phương trình động lực học của hệ thống: Ta cần xác định được các hệ số của phương trình trên. Đối với robot Planar ta có (bỏ qua thành phần F): b. Xây dựng luật điều khiển Các hệ thống điều khiển có phản hồi nhằm tạo ra khả năng chống nhiễu tốt. Nhưng khi cần bám sát quỹ đạo với tốc độ và gia tốc lớn thì chúng không đáp ứng được. Cơ chế điều khiển có bù cho phép giảm sai số quỹ đạo. Việc xác định tín hiệu điều khiển sao cho hệ thống ổn định xung quanh điểm cân bằng dựa trên phương pháp ổn định Lyapunov. Gọi vector trạng thái của hệ thống là với e = qđ - q là sai số giữa tín hiệu mong muốn và tín hiệu thực tế. Chọn hàm Lyapunov xác định dương dạng: với mọi , Kp là ma trận chéo xác định dương. Vi phân phương trình trên theo thời gian: Từ phương trình động lực học của hệ thống: Vậy ta có: Nhìn vào phương trình trên ta chọn luật điều khiển như sau: Trong đó KD là số xác định dương, tương ứng với phần bù phi tuyến thành phần trọng trường với bộ điều khiển PD. Câu 5: Mô phỏng hệ thống với bộ điều khiển ở câu 4 khi góc quay của khớp 1 thay đổi nhảy cấp từ 0 radian đến 1 radian Mô phỏng bằng SIMULINK như hình vẽ. Hình 10. Mô phỏng hệ bằng Simulink Hình 11. Kết quả mô phỏng - đáp ứng đầu Hình 12. Kết quả mô phỏng, sai lệch tĩnh của hệ Tài liệu tham khảo [1]. Nguyễn Mạnh Tiến. “Bài giảng môn Robot cao học năm 2002”. [2]. Lorenzo Sciavicco, Bruno Sciliano. “Modeling and Control of Robot Manipulators”. Đề bài Bài giải Câu 1. Xây dựng phương trình động học thuận và ngược biểu diễn mối quan hệ giữa hệ toạ độ tay Robot (End effector) và hệ toạ độ các khớp (Joints) a. Xây dựng phương trình động học thuận b. Xây dựng phương trình động học ngược Câu 2: Xây dựng quan hệ giữa tốc độ các khớp và tốc độ của tay Robot Câu 3. Viết hàm MATLAB thực hiện các phương trình ở câu 1 và 2 và vẽ đường biểu diễn vị trí và tốc độ khớp khi tay Robot di chuyển từ vị trí [0,4:0,0] đến [0,0:0,4] theo một đường thẳng. Đồ thị tốc độ của tay Robot đã cho như hình 2 a. Chương trình tính toán động học thuận b. Chương trình tính toán động học ngược c. Chương trình tính tốc độ quay khớp d. Vẽ đường biểu diễn vị trí và tốc độ khớp khi tay Robot di chuyển từ vị trí [0,4:0,0 m] đến [0,0:0,4 m] theo một đường thẳng Câu 4. Thiết kế bộ điều khiển bù trọng lực cho robot a. Xây dựng phương trình động lực học b. Xây dựng luật điều khiển Câu 5: Mô phỏng hệ thống với bộ điều khiển ở câu 4 khi góc quay của khớp 1 thay đổi nhảy cấp từ 0 radian đến 1 radian Tài liệu tham khảo ._.