Đang tải... (xem toàn văn)
Luật không gian trường thế nhân tạo: Cánh tay máy được coi là di chuyển trong một trường lực. Trong đó, vị trí cần, đi tới (điểm đích) phát ra một trường lực hút cánh tay máy, còn vật cản (chướng ngại vật) sẽ phát ra một trường lực đẩy cánh tay máy ra xa(The manipulator moves in a field of forces. The position to breached is an attractive pole for end effector and obstacles are repulsive surfaces for the manipulator parts)
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN ĐIỆN BỘ MÔN TỰ ĐỘNG HOÁ CÔNG NGHIỆP ====o0o==== ĐỒ ÁN TỐT NGHIỆP ĐỀ TÀI: ĐIỀU KHIỂN THỜI GIAN THỰC TRÁNH VA CHẠM VỚI VẬT CẢN CHO CÁNH TAY ROBOT CÔNG NGHIỆP Hà nội, 12-2015 Nhiệm Vụ NHIỆM VỤ Lời Cam Đoan LỜI CAM ĐOAN Mục Lục MỤC LỤC LỜI MỞ ĐẦU Error! Bookmark not defined MỤC LỤC HÌNH VẼ CHƯƠNG I: ROBOT VÀ MÔ HÌNH TOÁN HỌC CỦA ROBOT 1.1 Động học vị trí robot 1.1.1 Biểu diễn ma trận 1.1.2 Phép biến đổi 10 1.2 Động học thuận robot 13 1.2.1 Tham số nối khớp 13 1.2.2 Phép biểu diễn Danevit-Hartenberg 13 1.2.3 Quan hệ hai khung tọa độ i i-1 14 1.3 Phương trình động lực học robot 15 1.3.1 Phương trình Lagrange 15 1.3.2 Phương trình động lực học cho cánh tay robot 17 CHƯƠNG II: XÂY DỰNG LUẬT ĐIỀU KHIỂN ROBOT TRÁNH VẬT CẢN 19 2.1 Luật không gian trường nhân tạo 19 2.1.1 Lực hút tới điểm đích 20 2.1.2 Lực đẩy nhân tạo từ bề mặt vật cản (FIRAS) 21 2.1.3 Tổng lực tác dụng vào chất điểm 22 2.2 Lực hút tác dụng vào cánh tay robot 23 2.3 Lực đẩy tác dụng vào cánh tay robot 23 2.3.1 Tính toán khoảng cách gần từ điểm tới vật cản 24 2.3.2 Lực đẩy từ vật cản tác dụng vào phần tử robot 29 2.3.3 Lực đẩy từ vật cản tác dụng vào cánh tay robot 29 CHƯƠNG III: XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN TRÊN ROBOT BẬC TỰ DO 33 3.1 Xây dựng đối tượng robot 33 3.1.1 Đối tượng điều khiển 33 3.1.2 Xây dựng mô hình đối tượng điều khiển (Robot bậc tự do) 34 3.2 Xây dựng mô hình điều khiển cho robot bậc tự 39 Mục Lục 3.2.1 Tính toán lực tác dụng vào cánh tay robot thông qua luật trường nhân tạo 39 3.2.2 Mô men điều khiển tác động vào khớp cánh tay robot 43 CHƯƠNG 4: XÂY DỰNG MÔ HÌNH MÔ PHỎNG BẰNG MATLAB 50 4.1 Mô hình đối tượng mô (Robot bậc tự do) 50 4.2 Mô hình điều khiển tránh va chạm vật cản cho đối tượng xây dựng 52 4.2.1 Mô trường hợp vật cản 56 4.2.2 Mô trường hợp có vật cản 58 4.3 So sánh với trường hợp điều khiển robot theo quỹ đạo thiết kế 60 Hình Vẽ HÌNH VẼ Hình 1.1 Tọa độ điểm khung tọa độ Đề-Các Hình 1.2 Biểu diễn khung tọa độ p khung tọa độ chuẩn 10 Hình 1.3 Biểu diễn khung tọa độ tịnh tiến 11 Hình 1.4 Biểu diễn khung tọa độ quay đơn theo trục z 12 Hình 1.5 mô tả mối liên kết nối cánh tay robot 14 Hình 2.1 Điểm đích 19 Hình 2.2 Vật cản O 19 Hình 2.3 Trường nhân tạo 20 Hình 2.4 Khoảng cách từ điểm tới đường tròn 25 Hình 2.5 Khoảng cách từ điểm tới đường thẳng 26 Hình 2.6 Khoảng cách ngắn từ điểm tới tứ giác 27 Hình 2.7 Khoảng cách gần cánh tay robot vật cản hình tròn 30 Hình 2.8 Khoảng cách gần từ đoạn thẳng tới 31 liên kết cánh tay robot 31 Hình 3.1 Mô hình robot bậc tự xét 33 Hình 3.2 Mô hình robot bậc tự 34 Hình 3.3 Mô hình robot hoạt động môi trường 39 có vật cản hình tròn điểm đích P 39 Hình 3.4 Mô hình robot hoạt động môi trường có vật cản hình tròn 40 Hình 3.5 Ma trận Jacobian tính toán cho lực hút điểm đích 45 Hình 3.6 Ma trận Jacobian tính toán cho lực đẩy vào liên kết 47 vật cản 47 Hình 3.7 Ma trận Jacobian tính toán cho lực đẩy vào liên kết 48 vật cản 48 Hình 4.1 Sơ đồ cấu trúc robot bậc tự 50 Hình 4.2 Mô hình điều khiển tránh va chạm cho robot 53 Hình 4.3 Quá trình chuyển động cánh tay robot từ vị trí ban đầu tới điểm đích có tọa độ [0.2,0.3] 56 Hình 4.4 Đồ thị mô tả trình góc quay hai khớp 57 Hình 4.5 Mô tả thay đổi mô men điều khiển 57 Hình 4.6 Quá trình chuyển động tránh vật cản cánh tay robot 58 Hình Vẽ Hình 4.7 Đồ thị mô tả trình góc quay hai khớp tránh vật cản 59 Hình 4.8 Mô tả thay đổi mô men điều khiển tránh vật cản 60 Hình 4.9 So sánh Quá trình tránh vật cản robot theo quỹ đạo 61 tránh vật cản 61 Hình 4.10 So sánh thay đổi góc khớp hai trường hợp 62 Hình 4.11 So sánh thay đổi mô men tác động hai trường hợp 62 Lời Nói Đầu LỜI NÓI ĐẦU Chương 1: Robot mô hình toán học robot CHƯƠNG I: ROBOT VÀ MÔ HÌNH TOÁN HỌC CỦA ROBOT 1.1 Động học vị trí robot 1.1.1 Biểu diễn ma trận Ma trận sử dụng để biểu diễn điểm, vector khung tọa độ không gian a)Biểu diễn vector không gian Hình 1.1 Tọa độ điểm khung tọa độ Đề-Các [4],[6] Một vector không gian xác định ba hình chiếu vector ba trục khung tọa độ chuẩn (khung tọa độ Đề-Các) ̅ Trong đó: ( ) (1.1) ( ) (1.2) – hình chiếu điểm p trục x,y,z – hình chiếu vector r trục x,y,z Vector ̅ điểm p biểu diễn dạng ma trận: ̅ [ ] [ [ ] ̅ [ ] ] (1.3) (1.4) Chương 1: Robot mô hình toán học robot Trong robot, điểm biểu diễn ma trận cột với thành phần thứ tư, dùng để mô tả ma trận vector điểm, dùng để mô tả ma trận vector vector b)Biểu diễn khung tọa độ không gian Hình 1.2 Biểu diễn khung tọa độ p khung tọa độ chuẩn [5] Một khung tọa độ biểu diễn thành phần, bao gồm thành phần biểu diễn hướng khung tọa độ thành phần để biểu diễn gốc tọa độ khung tọa độ xét [2] (1.5) [ ] 1.1.2 Phép biến đổi Phép biến đổi dùng để mô tả chuyển động khung tọa độ hay vector không gian tương đối so với khung tọa độ chuẩn (khung tọa độ gốc) Có dạng phép biến đổi [2]: - Phép biến đổi tịnh tiến Phép biển đổi quay Phép biến đổi kết hợp phép biến đổi tịnh tiến phép biến đổi quay a)Phép biến đổi tịnh tiến Khi khung tọa độ (có thể bao gồm đối tượng) di chuyển tịnh tiến không gian làm việc, hướng khung tọa độ không thay đổi trình chuyển động Khi phép biến đổi tịnh tiến biểu diễn ma trân vuông sau: 10 Chương 3: Xây dựng thuật toán điềuk hiển robot bậc tự Phi2 = in (4); V1 = -m2*ls1*0.5*ls2*sin(Phi2)*(dPhi2^2+2*dPhi1*dPhi2); V2 = m2*ls1*0.5*ls2*sin(Phi2)*(dPhi1^2); out = [V1;V2]; Ở liệt kê thông số robot sử dụng công thức, đầu vào tính toán thành phần V bao gồm vận tốc góc liên kết vậy, đầu vào thành phần V vector bao gồm giá trị tương ứng với giá trị tốc độ hai khớp Đầu hàm thành phần lực Coriolis lực hướng tâm cánh tay robot V Hàm mô tả thành phần mô men quán tính khớp: function out = RobotHFunc(in) m1 = 2.5; m2 = 1.5; ls1 = 0.25; ls2 = 0.15; g = 9.81; J1 = m1*ls1/12; J2 = m2*ls2/12; Phi2 = in(2); H11 = m1*(0.5*ls1)^2+J1+m2*(ls1^2+(0.5*ls2)^2+… 2*ls1*0.5*ls2*cos(Phi2))+J2; H12 = m2*((0.5*ls2)^2+ls1*0.5*ls2*cos(Phi2))+J2; H21 = m2*(ls2^2+ls1*ls2*cos(Phi2))+J2; H22 = m2*(0.5*ls2)^2+J2; out = [H11 H12;H21 H22]; out = out^(-1) Trong có thông số robot bao gồm khối lượng độ dài liên kết thông số gia tốc trọng trường Ngoài có thêm thông số mô men quán tính hai liên kết trục qua tầm khối liên kết Đầu hàm thành phần nghịch đảo H đứng theo công thức xây dựng (công thức 3.19) 4.2 Mô hình điều khiển tránh va chạm vật cản cho đối tượng xây dựng Mô hình điều khiển robot xây dựng điều khiển thời gian thực trực tiếp mô men vào robot ta coi trình điều khiển cấu động khâu tỉ lệ Vì mô hình điều khiển cho robot có dạng: 52 Chương 3: Xây dựng thuật toán điềuk hiển robot bậc tự Hình 4.2 Mô hình điều khiển tránh va chạm cho robot Bộ điều khiển nhận giá trị góc tốc độ góc hai khớp từ đối tượng, sau tính toán giá trị robot không gian làm việc từ đưa vào tính toán mô men điều khiển tránh vật cản cho robot đưa vào đối tượng Hàm xây dựng điều khiển xây dựng theo công thức xây dựng điều khiển tránh vật cản xây dựng mục 3.2 Qua ta viết hàm điều khiển cho robot Matlab dạng: function out = RobotMomentFunc(in) %gia tri su dung m1 = 2.5; m2 = 1.5; g = 9.81; %vi tri dich txd = [0.2 0.3 0]; %2 diem vat can m1,m2 tm1 = [0 0.35 0]; %duong tron tam la m1 r1=0.05; %tam anh huong cua truong can km = 0.05; %qua trinh tinh toan khong gian lam viec b1 = in(1); b2 = in(2); db1 = in(3); db2 = in(4); ls1 = 0.25; ls2 = 0.15; zx1=ls1*cos(b1); zy1=ls1*sin(b1); zx2=zx1+ls2*cos(b1+b2); zy2=zy1+ls2*sin(b1+b2); %vi tri ban dau ts0 = [0 0]; ts1 = [zx1 zy1 0]; ts2 = [zx2 zy2 0]; 53 Chương 3: Xây dựng thuật toán điềuk hiển robot bậc tự %khoang cach toi khop goc k0 = sqrt((tm1(1)-0).^2+(tm1(2)-0).^2)-r1; %khoang cach toi khop k1 = sqrt((tm1(1)-zx1).^2+(tm1(2)-zy1).^2)-r1; %khoang cach toi khop k11 = sqrt((tm1(1)-zx2).^2+(tm1(2)-zy2).^2)-r1; %toa moi cua cac khop ts1 = [zx1 zy1 0]; ts2 = [zx2 zy2 0]; %khoang cach ngan nhat toi s0s1 ls1 = 0.25; lamdas1 = ((tm1(1)-0)*(zx1-0)+(tm1(2)-0)*… (zy1-0))/((ls1).^2); if (lamdas1=1) k1m = k1; ts1m = ts1; elseif (0[...]... nhất của cánh tay robot tới vật cản Từ đó, với việc tránh va chạm giữa hai điểm gần nhau nhất của vật cản và cánh tay robot ta sẽ tránh được sự va chạm giữa cánh tay robot và vật cản 29 Chương 2: Xây dựng luật điều khiển robot tránh vật cản Để xét lực đẩy từ vật cản vào cánh tay robot ta xét lực đẩy từ vật cản vào một liên kết của cánh tay robot Một liên kết của cánh tay robot được định nghĩa là một... quyết khác cho vấn đề này Ta nhận thấy, mục tiêu của việc xây dựng lực đẩy từ vật cản tác dụng lên cánh tay robot nhằm tránh sự va chạm giữa cánh tay robot với vật cản, vì vậy chúng ta chỉ cẩn tìm một điểm nằm trên cánh tay robot gần nhất tới vật cản và xây dựng lực đẩy sao cho tại bất kì thời điểm nào điểm gần nhất của cánh tay robot cũng không va chạm với vật cản, khi đó toàn bộ cánh tay robot cũng... THUẬT TOÁN ĐIỀU KHIỂN TRÊN ROBOT 2 BẬC TỰ DO 3.1 Xây dựng đối tượng robot 3.1.1 Đối tượng điều khiển Trước khi bắt đầu vào công việc tính toán bộ điều khiển tránh vật cản cho robot, điều đầu tiên chúng ta cần làm đó là xác định đối tượng điều khiển cho bộ điều khiển của mình Như đã được đề cập từ đầu đề tài, phương pháp tránh vật cản được xây dựng ở đây sẽ được áp dụng cho cánh tay máy công nghiệp và cụ... dụng cho vật cản ta sẽ xác định được lực đẩy từ vật cản tác động vào cánh tay robot giúp cánh tay robot tránh va chạm với vật cản Như đã được xây dựng ở trên với một chất điểm, ta biết được khi một chất điểm di chuyển trong môi trường phức tạp, nhờ luật trường thế nhân tạo ta sẽ xác định được lực đẩy từ vật cản tác dụng vào chất điểm giúp làm cho chất điểm hướng đi xa ra khỏi vật cản ( hay tránh vật cản. .. của cánh tay robot Tuy nhiên, như đã nói ở đầu mục, nếu phải tính toán lực đẩy vào từng phần tử của robot sẽ tạo gánh nặng cho bộ xử lí của cánh tay robot đồng thời tạo một khoảng thời gian trễ trong quá trình điều khiển cánh tay robot Vì vậy, tương tự như phương pháp xác định điểm gần nhất của vật cản với một phần tử của cánh tay robot, ở mục này ta cũng sẽ xác định điểm gần nhất của cánh tay robot. .. Chính vì vậy công thức trên còn có cách biến đổi khác: ̅ ̅ ̅ (1.39) ̅ (1.40) Hay: ̅ ̅ 18 (1.41) Chương 2: Xây dựng luật điều khiển robot tránh vật cản CHƯƠNG II: XÂY DỰNG LUẬT ĐIỀU KHIỂN ROBOT TRÁNH VẬT CẢN 2.1 Luật không gian trường thế nhân tạo Phương pháp này được xây dựng để giải quyết bài toán chống va chạm trong điều khiển cánh tay máy và robot di động Nguyên lí của phương pháp “Không gian trường... độ của điểm M gần nhất tới vật cản, [ ] là tọa độ của điểm gần nhất từ vật cản tới cánh tay robot Khi đó ta chỉ cần xác định tọa độ điểm gần nhất M nằm trên cánh tay robot tới vật cản là ta có thể xác định được vector lực đẩy từ vật cản tác dụng vào cánh tay robot 2.3.1 Tính toán khoảng cách gần nhất từ một điểm tới một vật cản Ở phần trên ta mới chỉ xét tới tránh né một vật cản đơn O của một chất điểm,... (tránh va chạm với vật cản) 2.3.2 Lực đẩy từ vật cản tác dụng vào một phần tử trên robot Như ở mục 2.3.1 trên ta đã xác định được khoảng cách gần nhất từ một điểm cần tránh va chạm (một phần tử của cánh tay robot) là , đồng thời ta cũng xác định [ ] được tọa độ của điểm gần nhất nằm trên vật cản với phần tử đang xét Từ đó ta có thể xác định được lực đẩy từ vật cản tới phần tử đang xét tương đương với. .. Chương 2: Xây dựng luật điều khiển robot tránh vật cản Khi đó lực đẩy từ vật cản hình tròn vào liên kết cánh tay robot sẽ có dạng: ( ) { ( ) (2.30) Trong đó vector chỉ hướng của lực có dạng: [ ] (2.31) b)Lực đẩy từ một vật cản hình đa giác (tứ giác) vào một liên kết của cánh tay robot Như đã làm ở mục 3.1 để xét được lực đẩy từ một vật cản hình tứ giác vào một liên kết của cánh tay robot, đầu tiên ta sẽ... vào đầu cánh tay nên công thức tính lực tác dụng vào đầu cánh tay tương tự như công thức tính lực tác dụng vào một điểm Vì vậy công thức tính lực hút tác dụng vào đầu cánh tay sẽ giống như công thức 2.12 ( ) ( ) ̇ 2.3 Lực đẩy tác dụng vào cánh tay robot Ở mục này ta sẽ phân tích, xây dựng lực đẩy từ vật cản tác dụng vào cánh tay robot khi robot được đặt trong một môi trường phức tạp có nhiều vật cản Khi