tính toán thiết kế mô phỏng robot
BÀI TIỂN LUẬN ROBOTICS MỤC LỤC LỜI NÓI ĐẦU I. Xây dựng cấu trúc, thiết lập hệ phương trình động học của robot……… .2 a. Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg……………………… 2 b. Thiết lập hệ phương trình động học của robot……………………………… .2 II. Giải bài toán động học………………………………………………… …….3 2.1. Giải bài toán động học thuận…………… ………………………….… .3 a, Xác định vận tốc điểm tác động cuối và vận tốc góc khâu thao tác …… .3 b, Xây dựng quy luật chuyển động từng khâu từ đó vẽ quỹ đạo điểm E, vận tốc điểm E và vận tốc góc …………………………………………………………… 4 2.2. Giải bài toán động học ngược…………………………………………… .6 a, Giải bằng phương pháp giải tích…………………………………………….6 b, Xây dựng quy luật chuyển động khâu thao tác E và giải động học ngược bằng phương pháp số ………………….……………………………………… .…7 III. Tính toán tĩnh học………………………………………………………… 11 3.1. Cho lực tác động vào khâu thao tác tìm lực dẫn động………………….11 3.2. Cho lực ( mô men ) dẫn động tính lực ( mô men ) khâu thao tác tác dụng lên đối tượng…………………………………………………………………… 14 IV. Tính toán động lực học…………………………………………………… 16 4.1 Xây dựng cấu trúc động lực học và các thành phần cần thiết để viết phương trình động lực học…………………………………………………… .16 4.2 Thiết lập phương trình Lagrang của robot………………………………19 V. Luật điều khiển………………………………………………………………22 1. Hệ thống điều khiển trong không gian khớp………………………………22 2. Hệ thống điều khiển trong không gian làm việc………………………… 25 Kết Luận TÀI LIỆU THAM KHẢO …………………………………………………… .27 BÀI TIỂN LUẬN ROBOTICS LỜI NÓI ĐẦU Trong sự nghiệp công nghiệp hóa, hiện đại hóa đất nước vấn đề tự động hóa có vai trò đặc biệt quan trọng. Nhằm nâng cao năng suất dây chuyền công nghệ, nâng cao chất lượng và khả năng cạnh tranh của sản phẩm, cải thiện điều kiện lao động, nâng cao năng suất lao động…vấn đề được đặt ra là hệ thống sản xuất phải có tính linh hoạt cao.Robot công nghiệp, đặc biệt là những tay máy robot là bô phận quan trọng để tạo ra những hệ thống đó. Tay máy Robot đã có mặt trong sản xuất từ nhiều năm trước, ngày nay tay máy Robot đã dùng ở nhiều lĩnh vực sản xuất, từ những ưu điểm mà tay máy Robot hoạt động trong quá trình sản xuất, làm việc, chúng ta có thể đúc rút ra những tính năng mà con người không thể có được : khả năng làm việc ổn định, có thể làm việc trong môi trường độc hại… Do đó việc đầu tư nghiên cứu, chế tạo ra những tay máy Robot phục vụ cho công cuộc tự động hóa sản xuất là rất cần thiết cho hiện tại và tương lai. Môn học ‘ Robotics’ giúp chúng em bước đầu làm quen, tìm hiểu,học cách giải các bài toán động học thuận và động học ngược hay những bài toán tĩnh học, động lực học . đối với những mô hình tay máy Robot công ngiệp cơ bản và giúp chúng em củng cố và mở rộng kiến thức phục vụ cho hoạt động học tập, nghiên cứu cũng như công việc sau này. Trong quá trình học môn ‘Robotics’ chúng em còn nhiều điều chưa hiểu cặn kẽ cũng như những thiếu sót , mong thầy giúp đỡ chỉ bảo thêm cho chúng em . *** Cuối cùng em xin chân thành cảm ơn và chúc thầy nhiều sức khỏe, gia đình hạnh phúc, công tác tốt ! Sinh viên thực hiện: Nguyễn Văn Quảng _CNCDT1_K55 BÀI TIỂN LUẬN ROBOTICS Xây dựng cấu trúc, thiết lập hệ phương trình động học của robot RRR *Mô hình 3D của robot RRR: Ta thiết lập phương trình động học theo phương pháp ma trận Denavit Hartenberg a. Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg Khâu 0: đế ta chọn hệ tọa độ XoYoZo có trục Zo chọn trùng với khớp 1, trục Xo chọn tùy ý sao cho phù hợp nhất như hình vẽ , trục Yo chọn theo quay tắc tam diện thuận. Khâu 1: ta chọn hệ tọa độ X 1 Y 1 Z 1 có trục Z 1 trùng với khớp 2, trục X 1 ta chọn theo hướng Zo x Z 1 , trục Y 1 chọn theo quay tắc tam diện thuận. Khâu 2: ta chọn hệ tọa độ X 2 Y 2 Z 2 có trục Z 2 trùng với khớp 3,X 2 chọn theo đường vuông góc chung Z 1 và Z 2 , Y 2 chọn theo quy tắc tam diện thuận. Khâu 3: Ta chọn hẹ X 3 Y 3 Z 3 có trục Z 3 song song Z 2 X 2 chọn theo đường vuông góc chung Z 2 và Z 3 , Y 3 chọn theo quy tắn tam diện thuận. BÀI TIỂN LUẬN ROBOTICS X0 Y0 Z0 Z1 Z2 X2 Y2 Z3 Y3 X3 Y1 X1 q1 q2 q3 d1 a3 a2 Hình 2.1. Hệ tọa độ Robot RRR b. Thiết lập hệ phương trình động học của robot Từ việc chọn hệ tọa độ ta có bảng DH sau: Khâu d i θ i a i α i 1 d 1 q 1 0 π/2 2 0 q 2 a 2 0 3 0 q 3 a 3 0 0 1 A = 11 11 1 cos( ) 0 sin( ) 0 sin( ) 0 os( ) 0 0 1 0 0 0 0 1 qq q c q d , 1 2 A = 2 2 2 2 2 2 2 2 cos( ) sin( ) 0 os( ) sin( ) os( ) 0 sin( ) 0 0 1 0 0 0 0 1 q q a c q q c q a q 2 3 A = 3 3 3 3 3 3 3 3 cos( ) sin( ) 0 os( ) sin( ) os( ) 0 sin( ) 0 0 1 0 0 0 0 1 q q a c q q c q a q 0 2 A = 0 1 A . 1 2 A = 1 2 1 2 1 2 1 2 1 2 1 2 1 2 1 2 2 2 1 2 2 0 0 0 0 1 C C C S S a C C S C S S C a S C S C d a S 0 3 ()q A = 0 1 A . 1 2 A . 2 3 A = 1 23 1 23 1 1 3 23 2 2 1 23 1 23 1 1 3 23 2 2 23 23 1 2 2 3 23 ( ) ( ) 0 0 0 0 1 C C C S S C a C a C S C S S C S a C a C S C d a S a S BÀI TIỂN LUẬN ROBOTICS Với C 1 =cos(q 1 ), S 1 =sin(q 1 ), C 2 =cos(q 2 ), S 2 =sin(q 2 ), S 23 =sin(q 2 +q 3 ), C 23 =cos(q 2 +q 3 ), q=[q 1 , q 2 , q 3 ] T Mặt khác ta lại mô tả được hướng và vị trí qua ma trận sau thông qua vector p=[x E , y E, z E , α, β, η] T α, β, η là 3 góc Cardan. 0 3 ()p A = os( ) os( ) os( )sin( ) sin( ) sin( )sin( ) os( ) os( )sin( ) sin( )sin( )sin( ) os( ) os( E c c c x c c c c ) sin( ) os( ) os( )sin( ) os( ) sin( )sin( ) os( )sin( )sin( ) sin( ) os( ) os( ) os( ) 0 E E cy c c c c c c z 0 0 1 So sánh hai ma trận 0 3 ()q A , 0 3 ()p A ta thiết lập được hệ phương trình động học sau: 00 1 1 3 2 3 2 2 33 00 2 1 3 2 3 2 2 33 00 3 1 2 2 3 2 3 33 00 4 33 ( )[1,4] ( )[1,4] os( ) os( ) os( ) 0 ( )[2,4] ( )[2,4] sin( ) os( ) os( ) 0 ( )[3,4] ( )[3,4] sin( ) sin( ) 0 ( )[1,1] ( ) E E E f p q x c q a c q q a c q f p q y q a c q q a c q f p q z d a q a q q f p q AA AA AA AA 1 2 3 00 5 1 2 3 33 00 6 33 [1,1] os( ) os( ) os( ) os( ) 0 ( )[2,2] ( )[2,2] sin( )sin( )sin( ) os( ) os( ) sin( )sin( ) 0 ( )[3,3] ( )[3,3] os( ) os( ) 0 c c c q c q q f p q c c q q q f p q c c AA AA II. Giải bài toán động học 2.1. Giải bài toán động học thuận Để tính toán ta chọn d 1 =1 (m), a 2 =0.5 (m), a 3 =0.4 (m). a, Xác định vận tốc điểm tác động cuối và vận tốc góc khâu thao tác Từ phương trình động ở trên ta rút ra ( với q=[q 1 , q 2 , q 3 ] T ) 0 E r = 1 3 2 3 2 2 1 3 2 3 2 2 1 2 2 3 2 3 os( ) os( ) os( ) sin( ) os( ) os( ) sin( ) sin( ) E E E c q a c q q a c q x y q a c q q a c q z d a q a q q 0 E v = 00 ( ) ( ) EE E d q J q dt q rr J E = 1 3 23 2 2 1 3 23 2 2 3 1 23 1 3 23 2 2 1 3 23 2 2 3 1 23 3 23 2 2 3 23 ( ) ( ) ( ) ( ) 0 S a C a C C a S a S a C S C a C a C S a S a S a S S a C a C a C => 0 E v = 1 3 23 2 2 1 1 3 23 2 2 2 3 1 23 3 1 3 23 2 2 1 1 3 23 2 2 2 3 1 23 3 3 23 2 2 2 3 23 3 ( ) ( ) ( ) ( ) ( ) E E E S a C a C q C a S a S q a C S q x y C a C a C q S a S a S q a S S q z a C a C q a C q BÀI TIỂN LUẬN ROBOTICS Từ ma trận 0 3 A ta rút trân cosin chỉ hướng 0 3 R = 1 23 1 23 1 1 23 1 23 1 23 23 0 C C C S S S C S S C SC => 0 0 0 3 2 2 . T R R = 1 2 3 1 2 3 1 sin( )( ) os( )( ) q q q c q q q q b, Xây dựng quy luật chuyển động từng khâu từ đó vẽ quỹ đạo điểm E, vận tốc điểm E và vận tốc góc. Để khảo sát kết quả trên ta xây dựng quy luật chuyển động của các biến khớp q như sau 1 2 3 sin(3 ) 23 os(5 ) 3 qt qt q c t t 0 E r = Với t=0-> 2*Pi/5 ta vẽ được đồ thị điểm tác động cuối bằng phân mềm maple Hình 2.2. Đồ thị điểm tác động cuối E x = BÀI TIỂN LUẬN ROBOTICS E y = E z = 2 2 2 E E E E v x y z x , y , y 2 2 2 3 w x y z Ta dùng maple để vẽ với khoảng t=0 3*Pi Hình 2.3. Đồ thị vận tốc điểm E theo t Hình 2.4. Đồ thị vận tốc góc khâu 3 theo t Để tính toán ta sử dụng hai thủ tuc đó là hai hàm con viết bằng maple như sau: Hàm doitm có tác dụng đổi các tham số các ma trận có các thành phần là hàm theo thời gian Hàm wmo có tác dụng tính vector vân tốc góc tuyệt đối chiếu trên hệ tọa độ khâu i i BÀI TIỂN LUẬN ROBOTICS 2.2. Giải bài toán động học ngược a, Giải bằng phương pháp giải tích từ hệ phương 1 3 2 3 2 2 1 3 2 3 2 2 1 2 2 3 2 3 os( ) os( ) os( ) sin( ) os( ) os( ) sin( ) sin( ) E E E x c q a c q q a c q y q a c q q a c q z d a q a q q Từ phương trình 1, 2 trong hệ ta rút ra : 1 atan2( , ) EE q y x (2.1) 2 2 2 2 2 2 2 1 2 3 2 3 2 23 2 23 2 3 2 3 3 ( ) 2 ( ) 2 E E E x y z d a a a a C C S S a a a a C 2 2 2 2 2 1 2 3 3 23 2 32 () cos( ) 2 sin( ) 1 os ( ) E E E x y z d a a q aa q c q 3 3 3 atan2(sin( ), os( ))q q c q (2.2) Ta viết lại hệ như sau : BÀI TIỂN LUẬN ROBOTICS 2 3 3 1 2 3 3 1 2 1 3 3 2 2 3 3 2 () () E E x a a C C C a S C S z d a S C a a C S Giải phương đại số tuyến tính ta có : 2 3 3 1 3 3 1 2 2 2 2 2 2 3 2 3 3 1 1 1 3 3 2 3 3 ( ) ( 2 ) ( ) ) ( ) E E E a a C C a S C a a a a C C x y z d C a S a a C 3 3 1 1 2 3 3 3 3 1 2 3 3 () E E E E E x a S C a x a x C a z d S C z d a a C 2 3 3 1 2 2 1 3 3 1 3 3 3 1 ( ) ( ) ( ) p E E E E a a C C x a z d a z d C C x S a S z d 2 3 3 3 3 1 2 2 2 2 11 2 1 3 3 1 3 2 2 2 2 11 () cos( ) ( ) ) ( ) ( ) sin( ) ( ) ) E E E E E E E E E E E E a x a x C a z d S C q x y z d C a z d a z d C C x S q x y z d C Tính atan2 triệt tiêu được C 1 ở dưới mẫu hệ trên => 2 1 3 3 1 3 2 3 3 3 3 1 1 2 2 2 2 2 2 11 ( ) ( ) () tan 2 , ( ) ) ( ) ) E E E E E E E E E E E E a z d a z d C C x S a x a x C a z d S C qa x y z d x y z d (2.3) Từ (2.1), (2.2), (2.3) ta xác định được dưới dạng giải tích của các biến khớp liên hệ với điểm tác động cuối, mà khi xác định được các biến khớp ngược lại ta tìm được các góc cardan b, Xây dựng quy luật chuyển động khâu thao tác E và giải động học ngược bằng phương pháp số Newton-Raphson. BÀI TIỂN LUẬN ROBOTICS 1 1 3 2 3 2 2 2 1 3 2 3 2 2 3 1 2 2 3 2 3 1 2 3 os( ) os( ) os( ) 0 sin( ) os( ) os( ) 0 sin( ) sin( ) 0 0 E E E f x c q a c q q a c q f y q a c q q a c q f z d a q a q q f Ff f Bài toán khi biết được x E (t) , y E (t) , z E (t) tại mỗi thời điểm t ta sẽ tìm được vector q=[q 1 , q 2 , q 3 ] T tại mỗi thời điểm đó. Ta lấy giá trị sát giá trị đầu để tiến hành quá trình lặp Newton-Raphson Quá trình lặp dừng lại khi sai số ở lần k+1 với lần k nhỏ hơn giá trị cho phép. Dưới đây là 1 doạn chương trình viết bằng maple sử dụng phương pháp Newton-Raphson lưu kết quả tính toán ra file txt và 1 đoạn vẽ đồ thị từ file txt được viết bằng matlab Với quỹ đạo điểm tác động cuối là đường thẳng có phương trình như sau 0.6 0.4 0.3 0.5 0.4 E E E xt yt zt Đoạn chương trình viết bằng maple > > > > >