BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - - - - - - - - - - -o0o- - - - - - - - - - - TRẦN THỊ HỒNG GẤM THIẾT KẾ ROBOT TRỢ GIÚP PHẪU THUẬT TIM VÀ CAN THIỆP ĐỘNG MẠCH VÀNH LUẬN VĂN THẠC SĨ KHOA HỌC CHUYÊN NGÀNH: TOÁN TIN Hà Nội - 2015 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - - - - - - - - - - -o0o- - - - - - - - - - - TRẦN THỊ HỒNG GẤM THIẾT KẾ ROBOT TRỢ GIÚP PHẪU THUẬT TIM VÀ CAN THIỆP ĐỘNG MẠCH VÀNH Chuyên ngành: Toán Tin LUẬN VĂN THẠC SĨ KHOA HỌC NGÀNH: TOÁN TIN NGƯỜI HƯỚNG DẪN KHOA HỌC GS TSKH LÊ HÙNG SƠN Hà Nội - 2015 Mục lục Lời cảm ơn iii Lời mở đầu iv Danh mục hình vẽ vii Tổng quan robot công nghiệp 1.1 Khái niệm robot công nghiệp 1 1.1.1 Sơ lược khái niệm robot 1.1.2 Các khái niệm robot 1.2 Phương trình động học robot 1.2.1 Sơ lược 1.2.2 Bộ thông số Denavit - Hartenburg(DH) 10 1.2.3 Đặc trưng ma trận A 12 1.2.4 Thiết lập hệ phương trình động học 14 1.2.5 Giải hệ phương trình động học 15 1.2.6 Quỹ đạo chuyển động 22 Thiết kế robot trợ giúp phẫu thuật tim động mạch vành 26 i 2.1 Robot phẫu thuật 27 2.1.1 Khái niệm robot phẫu thuật 27 2.1.2 Thiết kế tay máy robot C-arm 29 Mô tay máy đơn giản phần mềm Easy-Rob 48 3.1 Bảng thông số kích thước robot Elbow 49 3.2 Hình ảnh mô 50 3.3 Lập trình điều khiển khớp robot với lệnh PTP EasyRob Tài liệu tham khảo 52 56 ii Lời cảm ơn Đầu tiên, xin bày tỏ lòng biết ơn chân thành sâu sắc tới GS.TSKH Lê Hùng Sơn, người tận tình hướng dẫn, bảo để luận văn hoàn thành, giúp tăng trưởng niềm đam mê nghiên cứu khoa học Tôi xin chân thành cảm ơn Viện Toán ứng dụng Tin học, Viện Đào tạo Sau Đại học, trường Đại học Bách khoa Hà Nội, tạo điều kiện thuận lợi cho trình học tập nghiên cứu trường Tôi xin cảm ơn dạy dỗ, bảo quan tâm thầy cô Viện Toán ứng dụng Tin học suốt thời gian theo học nghiên cứu Cuối cùng, muốn gửi lời cảm ơn chân thành tới gia đình bạn bè đồng nghiệp, người động viên khích lệ giúp hoàn thành luận văn Xin chân thành cảm ơn Học viên: Trần Thị Hồng Gấm Lớp: 12BTT.KH iii Lời mở đầu Trong sống đại ngày nay, robot ngành kỹ thuật cao phát triển mạnh mẽ ứng dụng rộng rãi giới, đầu tư phát triển nước ta Robot sử dụng nhiều lĩnh vực sản xuất, nghiên cứu khoa học, giải trí, phục vụ người Đặc biệt nay, lĩnh vực y tế áp dụng mạnh mẽ việc sử dụng robot cho việc hỗ trợ việc phẫu thuật, phục hồi sức khỏe thay phận thể Áp dụng robot vào phẫu thuật giúp người bớt đau đớn hơn, giúp bác sỹ thực ca mổ với độ tỉ mỉ cao mà không bị mệt mỏi Trong luận văn dựa sở robot tìm hiểu robot y tế để đến xây dựng cánh tay robot mặt lý thuyêt cấu trúc giải toán động học tay máy Trong phẫu thuật thông thường chia tay máy với tay máy có chức khác Chúng gắn với tách rời Trong luận văn xây dựng tay máy tách rời, ta cần thiết kế xây dựng tay máy Với mô hình hoạt động ta thay công việc kíp phẫu thuật gồm nhiều bác sĩ nhân viên y hệ thống tay máy, đặt điều khiển bác sĩ tổng huy iv Nội dung Luận văn trình bày ba chương Cụ thể: Chương 1: Tổng quan robot công nghiệp Chương dành để nghiên cứu khái niệm cấu trúc robot công nghiệp Cách xác định thông số Denavit- Hartenburg, thành phần quan trọng trình xây dựng hệ phương trình động học thuận cho robot.Và cách giải hệ phương trình động học ngược phục vụ cho việc lập trình robot chạy theo quỹ đạo định trước Chương 2: Thiết kế robot trợ giúp phẫu thuật tim động mạch vành Mục đích chương giới thiệu robot phẫu thuật Nghiên cứu tay máy robot phục vụ cho trình phẫu thuật tim động mạch vành, vẽ kết cấu tay máy từ xác định hệ tọa độ thông số Denavit - Hartenburg để thiết lập hệ phương trình động học chi tiết cho tay máy Từ giải hệ phương trình động học tìm giá trị θi , di, cho tay máy Chương 3: Mô tay máy đơn giản phần mềm EasyRob Trong chương mô tay máy Elbow 3D lập trình PTP chuyển động cho tay máy v Luận văn hoàn thành Viện Toán ứng dụng Tin học, trường Đại học Bách khoa Hà Nội, hướng dẫn GS.TSKH Lê Hùng Sơn Mặc dù cố gắng, song luận văn không tránh khỏi thiếu sót Rất mong nhận góp ý thầy cô bạn Xin chân thành cảm ơn Hà Nội, ngày 30 tháng 03 năm 2015 vi Danh mục hình vẽ Hình 1.1 Các tọa độ suy rộng robot Hình 1.2 Quy tắc bàn tay phải Hình 1.3 Biểu diễn vùng làm việc robot Hình 1.4 Hệ tọa độ gắn khâu chấp hành cuối(bàn tay) Hình 1.5 Chiều dài góc xoắn khâu Hình 1.6 Các thông số khâu: θ, d, a, α Hình 1.7 Phép quay Euler Hình 1.8 Tính liên tục quỹ đạo Hình 2.1 Hệ thống robot da Vinci Hình 2.2 Thiết kế tay máy hệ tọa độ robot phẫu thuật Hình 3.1 Robot Elbow Hình 3.2 Toàn cảnh làm việc Easy-Rob Hình 3.3 Robot Elbow 3D Easy-Rob Bảng 2.1 Bộ thông số Denavit - Hartenburg Bảng 3.1 Bộ thông số Denavit - Hartenburg Elbow vii Chương Tổng quan robot công nghiệp 1.1 Khái niệm robot công nghiệp 1.1.1 Sơ lược khái niệm robot • Khái niệm: Robot công nghiệp bao gồm hai thành phần khí điện tử Thành phần khí cấu tạo khâu gắn liền với khớp điều khiển theo chương trình lập sẵn • Hình dạng: Tùy vào mục đích khác mà robot có hình dạng khác • Ưu điểm: Làm việc "chán", làm việc môi trường độc hại, thực công việc đòi hỏi tinh tế tỉ mỉ Robot đại nghe, nói, nhìn nhờ thiết bị cảm ứng • Hoạt động: điều khiển giám sát người cài đặt sẵn chương trình làm việc Từ ta có: cos(φ − θ1) = C42 1− r Cuối ta có : θ1 = arctan(ay , ax ) − arctan C4 , ax + ay C42 1− (ax + ay 2)2 So sánh phần tử hàng cột hàng cột ta có:   −S4 C5−7 = S1 nx − C1 ny (2.26) (2.27)  −S S 5−7 = S1 Ox − C1 Oy Ta có: θ5 − θ7 = arctan(S1 Ox − C1Oy , S1nx − C1 ny ) θ7 = θ5 − arctan(S1 Ox − C1Oy , S1nx − C1 ny ) (2.28) Tiếp tục so sánh phần tử hàng cột hàng cột ta có:   −S2+3S4 = az (2.29)  −C S = C a − S a 2+3 x y Ta có: θ2 + θ3 = arctan(az , C1ax − S1 ay ) Vậy: θ2 = arctan(az , C1ax − S1ay ) − θ3 Từ (2.26) ta có: S4 = C4 − (S1ax − C1 ay )2) S1 ax − C1ay 43 (2.30) Suy ra: θ4 = arctan − (S1ax − C1ay )2), S1 ax − C1ay (2.31) Tiếp tục đồng phần tử hàng cột ta có: C2+3d4 − C2+3C5d6 + S2+3C4S5 d6 + a2 S2 = pz Suy nghiệm: a2 = pz − C2+3d4 − C2+3C5d6 + S2+3C4 S5d6 S2 (2.32) • Từ phương trình (2.18) ta có:  n O C S 0 C S2 −a2  x x  1     −S2 C2 0   0 0 ny Oy −1 −1   A2 A1 T =      0   S1 C1 0 nz Oz    0 0 0   f (n) f21(O) f21(a) f21(p)  21    f22(n) f22(O) f22(a) f22(p)  =   f23(n) f23(O) f23(a) f23(p)   0   Trong đó: f21(a) = az S2 + C2(C1ax + S1 ay ) f22(a) = az C2 − S2 (C1ax + S1 ay ) f23(p) = −C1py + S1 px 44 ax −p.n    ay −p.O   az −p.a   Vế phải biểu thức (2.18) biểu thức (2.13) So sánh phần tử hàng cột hàng cột ta có:   −C3C4 = f21(a) = az S2 + C2(C1ax + S1 ay ) (2.33)  −S S = f 2(a) = a C − S (C a + S a ) z 2 x y Suy ra: C4 az C2 − S2(C1ax + S1 ay ) S3 = C3 S4 az S2 + C2(C1ax + S1 ay ) az C2 − S2(C1ax + S1 ay ) S1 ax − C1ay = − (S1ax − C1 ay )2) az S2 + C2(C1ax + S1 ay ) Vậy ta có: θ3 = arctan ((S1 ax − C1ay )(az C2 − S2(C1ax + S1ay )), (az S2 + C2 (C1ax + S1ay )) − (S1ax − C1 ay (2.34) )2 Tiếp tục so sánh phần tử hàng cột ta có: −d6S4 S5 = f23(p) = −C1py + S1 px d6 = C p y − S1 p x S4 S5 (2.35) • Từ phương trình (2.19) ta có:  f (n) f31(O) f31(a) f31(p)   31   f32(n) f32(O) f32(a) f32(p) −1 −2 −1  A3 A A1 T =    f33(n) f33(O) f33(a) f33(p)   0  Trong đó: f33(a) = −S3(a2 + az S2 + C2(C1px + S1py )) + C3 (C2pz − S2(C1px + S1 py )) = −a2 S3 − az S2 S3 + Pz C2C3 − px C1S2+3 − py S1S2+3 45 Vế phải biểu thức (2.19) là:   CC CS −S4 C4S5 d6   5−7 5−7    S4 C5−7 S4 S5−7 C4 S4 S5 d       S5−7 −C5−7 d4 − C5d6    0 Cân phần tử hàng cột ta có: d4 − C5d6 = −a2 S3 − az S2S3 + Pz C2 C3 − px C1 S2+3 − py S1 S2+3 (2.36) Vậy ta có nghiệm: d4 = C5d6 − a2 S3 − az S2 S3 + Pz C2C3 − px C1S2+3 − py S1 S2+3 (2.37) • Từ phương trình (2.20) ta có:   f (n) f41(O) f41(a) f41(p)  41      f (n) f (O) f (a) f (p) 42 42 42 42 −1 −2 −1   T = A A A A−1   f43(n) f43(O) f43(a) f43(p)   0 Trong đó: f41(p) = S4 (C1py − S1px ) + C4 (−C3(a2 + pz S2 + C2(C1px + S1 py )) + S3 (C2pz − S2 (C1px + S1 py ))) f42(p) = −D4 − S3 (a2 + pz S2 + C2(C1px + S1py )) + C3 (C2pz − S2(C1px + S1 py )) 46 Vế phải biểu thức (2.20) là:  C S5−7  5−7   S5−7 −C5−7    0  0 S5 d    −C5d6      Đồng phần tử hàng cột hàng cột ta có:   S5d6 = f41(p) Ta có nghiệm: (2.38) (2.39)  −C d = f (p) 42 θ5 = arctan(−f41 (p), f42(p)) Vậy ta có tập nghiệm hệ phương trình sau:   C42 C4  θ = arctan(a , a ) − arctan − ,  y x ax +ay (ax +ay )2        θ2 = arctan(az , C1ax − S1 ay ) − θ3        θ3 = arctan ((S1 ax − C1ay )(az C2 − S2(C1ax + S1ay )),         (az S2 + C2(C1ax + S1ay )) − (S1ax − C1 ay )2       θ4 = arctan − (S1 ax − C1ay )2 ), S1ax − C1 ay    θ5 = arctan(−f41 (p), f42(p))        θ7 = θ5 − arctan(S1 Ox − C1Oy , S1nx − C1ny )         d4 = C5d6 − a2 S3 − az S2S3 + Pz C2C3 − px C1 S2+3 − py S1 S2+3       C p −S p  d6 = Sy4 S51 x       a = pz −C2+3 d4 −C2+3 C5 d6 +S2+3 C4 S5 d6 S2 47 (2.40) Chương Mô tay máy đơn giản phần mềm Easy-Rob Mô kỹ thuật đại, áp dụng nhiều lĩnh vực nghiên cứu sản xuất Khi nghiên cứu điều khiển robot, ta thực điều khiển trực tiếp robot điều khiển mô Điều khiển mô dùng mô hình tính toán động học động lực học robot kết hợp với phương pháp đồ họa máy vi tính để mô tả kết cấu hoạt động cánh tay robot nghiên cứu mô hoạt động robot máy tính giúp cho nhà thiết kế nhanh chóng lựa chọn phương án hình- động học robot, kiểm tra khả hoạt động robot hình, kiểm tra phối hợp robot với thiết bị khác dây chuyền Điều có ý nghĩa trình thiết kế chế tạo robot bố trí dây chuyền sản xuất Qua mô người thiết kế đánh giá tương đối đầy đủ khả làm việc phương án thiết kế mà không cần chế thử Nó xem phương tiện đối thoại, hiệu chỉnh thiết kế theo yêu 48 cầu đa dạng người sử dụng Phương pháp lập trình mô giúp người thiết kế chọn quỹ đạo công nghệ hợp lý robot trình làm việc với đối tượng cụ thể hay phối hợp thiết bị khác công đoạn sản xuất tự động hóa Hiện có nhiều phần mềm công nghiệp phần mềm nghiên cứu khác để mô robot, phạm vi ứng dụng giá thành chúng khác Ở sử dụng phần mềm Easy- Rob để mô chuyển động cánh tay robot Elbow 3.1 Bảng thông số kích thước robot Elbow Robot Elbow robot có bậc tự do, khớp toàn khớp quay: Hình 3.1: Robot Elbow Bảng thông số DH Elbow: 49 Khâu θi αi di θ1 900 d1 θ2 a2 θ3 00 a3 θ4 −900 a4 θ5 900 0 θ6 900 d6 Bảng 3.1: Bộ thông số Denavit - Hartenburg Elbow Ta mô robot Elbow với thông số kích thước sau: a2 = 0.4m a3 = 0.3m a4 = 0.25m d1 = 0.4m d6 = 0.15m 3.2 Hình ảnh mô Sau nhập thông số vào phần mềm vẽ khâu ta có hình ảnh tay máy Elbow sau: 50 Hình 3.2: Toàn cảnh làm việc Easy-Rob Các hệ tọa độ gắn vào tay máy minh họa 3D hình vẽ: Hình 3.3: Robot Elbow 3D Easy-Rob 51 3.3 Lập trình điều khiển khớp robot với lệnh PTP Easy-Rob Sử dụng PTP ta nhập tọa độ điểm muốn đến khâu cuối tay máy ta biểu diễn đường tay máy trước ta có bảng lập trình tay máy sau: PROGRAMFILE ! prgfln C:\Users\24h\Downloads\EASY_ROB\EASY_ROB\proj\luanvan.prg PTP_AX 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000 PTP_AX -40.5000 0.0000 0.0000 -68.5000 72.5000 -74.0000 PTP_AX -40.5000 0.0000 0.0000 -68.5000 72.5000 -74.0000 PTP_AX 34.0000 0.0000 0.0000 -68.5000 72.5000 -74.0000 PTP_AX -101.0000 0.0000 0.0000 -34.5000 72.5000 -160.5000 PTP_AX -64.5001 47.0000 -67.0000 2.5000 72.5000 -10.5000 PTP_AX 56 PTP_AX 30.0000 0 -30.0000 0 5.9000 89.5000 -130.000 ENDPROGRAMFILE PROGRAMFILE ! prgfln C:\Users\24h\Downloads\EASY_ROB\EASY_ROB\proj\luanvan2.prg PTP_AX 45.5000 -30.0000 -0.0000 19.5000 18.5002 -1.0000 PTP_AX 45.5000 -30.0000 -0.0000 19.5000 18.5002 -1.0000 PTP_AX 13.5000 -9.5000 -48.5000 48.5000 18.5002 74.0000 PTP_AX 4.5000 18.5000 -69.4999 39.0000 35.5002 74.0000 PTP_AX 4.5000 18.5000 -69.4999 39.0000 35.5002 74.0000 PTP_AX 48.5000 18.5000 -69.4999 48.0000 35.5002 74.0000 PTP_AX -64.0000 18.5000 -36.9999 -69.5000 10.0002 -24.0000 PTP_AX 2.5000 18.5000 -36.9999 -21.0000 131.0002 -52.0000 PTP_AX 2.5000 18.5000 -36.9999 65.5000 131.0002 -52.0000 PTP_AX -40.5000 18.5000 -36.9999 -29.5000 74.0001 -52.0000 PTP_AX -18.4999 18.5000 -32.9999 -29.5000 74.0001 -52.0000 PTP_AX 0 0 0 52 ENDPROGRAMFILE PROGRAMFILE ! prgfln C:\Users\24h\Downloads\EASY_ROB\EASY_ROB\proj\luanvan3.prg PTP_AX -18.4999 18.5000 -32.9999 -29.5000 74.0001 -52.0000 PTP_AX -53.4999 8.5000 -12.9999 -29.5000 74.0001 -52.0000 PTP_AX -53.4999 8.5000 -12.9999 -52.5000 127.0001 -52.0000 PTP_AX -77.9999 8.5000 -19.9999 -52.5000 127.0001 -52.0000 PTP_AX -48.4999 -6.5000 -19.9999 -5.5000 127.0001 -52.0000 PTP_AX -48.4999 -6.5000 -19.9999 -7.0000 30.5002 -52.0000 PTP_AX -22.4999 4.5000 -39.4999 18.0000 30.5002 -52.0000 PTP_AX -22.4999 4.5000 -39.4999 18.5000 163.5002 -52.0000 PTP_AX -48.4999 8.0000 -39.4999 -16.0000 119.5003 -150.4999 PTP_AX -48.4999 8.0000 -39.4999 68.5000 114.5005 -150.4999 PTP_AX 28.5000 -11.5000 -57.4999 68.5000 114.5005 -150.4999 PTP_AX -19.0000 6.5000 -39.9999 33.9999 114.5005 -150.4999 PTP_AX -75.5000 6.5000 -39.9999 16.4999 60.0006 -150.4999 PTP_AX -45.5000 26.5000 -39.9999 17.4999 158.5004 -150.4999 PTP_AX 0 0 0 ENDPROGRAMFILE 53 Kết luận chung Luận văn nghiên cứu thiết kế giải toán cho robot phẫu thuật Cụ thể, luận văn thực công việc sau: i) Giới thiệu tổng quan robot công nghiệp Các khái niệm khâu, khớp, vùng làm việc robot Cách xác định thông số thiết lập phương trình động học cho robot ii) Giới thiệu robot phẫu thuật Tìm hiểu cấu trúc robot phẫu thuật từ thiết kế hình dạng robot phẫu thuật từ xây dựng hệ tọa độ thông số DH cho robot Xây dựng hệ phương trình động học thuận giải bải toán nghiệm cho hệ phương trình động học ngược iii) Xây dựng chương trình mô tay máy robot dạng 3D để thấy rõ đường robot có tọa độ định trước Khi việc lập trình robot theo đường có sẵn dẽ dàng với mô cho trước Mặc dù cố gắng song hạn chế thời gian, kiến thức kinh nghiệm nên luận văn không tránh khỏi thiếu sót Tác giả mong nhận quan tâm đóng góp ý kiến thầy cô 54 bạn để luận văn hoàn thiện Trân trọng cảm ơn Hà Nội, ngày 30 tháng 03 năm 2015 55 Tài liệu tham khảo A Tài liệu Tiếng Việt [1] Phạm Đăng Phước (2001), Giáo trình Robot công nghiệp, Đại học bách khoa Đà Nẵng [2] Trần Thế San TS Nguyễn Tiến Dũng (2003), Cơ sở nghiên cứu sáng tạo robot, Nhà xuất thống kê [3] Nguyễn Thiện Phúc (2011), Robot công nghiệp, Nhà xuất khoa học kỹ thuật [4] Nguyễn Quang Minh (2012), Nghiên cứu nguyên lý toán học cho hoạt động nhiều tay máy công nghiệp ứng dụng, Luận văn thạc sỹ kỹ thuật, Đại học bách khoa Hà Nội B Tài liệu Tiếng Anh [5] Diana C.W Friedman (2008), Kinematic and Dynamic Analysis of a Surgical Robot Positioning Arm, Master of Science in Mechanical Engineering, University of Washington 56 [6] Gerkey, B and Matatric, M (2002), IEE Transactions on Robotics and Automation, Soid! Auction methods for multi-robot coodination, 18(5): 758 - 768 57 ... trình robot chạy theo quỹ đạo định trước Chương 2: Thiết kế robot trợ giúp phẫu thuật tim động mạch vành Mục đích chương giới thiệu robot phẫu thuật Nghiên cứu tay máy robot phục vụ cho trình phẫu. .. 1.2.4 Thiết lập hệ phương trình động học 14 1.2.5 Giải hệ phương trình động học 15 1.2.6 Quỹ đạo chuyển động 22 Thiết kế robot trợ giúp phẫu thuật tim động mạch vành. ..BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - - - - - - - - - - -o0o- - - - - - - - - - - TRẦN THỊ HỒNG GẤM THIẾT KẾ ROBOT TRỢ GIÚP PHẪU THUẬT TIM VÀ CAN THIỆP ĐỘNG MẠCH VÀNH Chuyên