Luận văn cao học: Động lực học điều khiển robot dạng mạch hở LỜI MỞ ĐẦU Robot thiết kế để thực chức thay người, thao tác tương tự người thực tác vụ hỗ trợ người Chúng ta điều khiển chúng Mục đích luận văn để phát triển framework cho phép tính toán điều khiển lớp robot có cấu trúc mạch hở Ngày nay, với phát triển nhanh chóng nghiên cứu robot, hàng loạt mẫu robot phát triển, phục vụ người nhiều lĩnh vực với kết cấu đa dạng phong phú Thế nhưng, xét khía cạnh học phân chia robot thành ba dạng Thứ dạng hệ có cấu trúc mạch hở, mà đại diện robot dạng tay máy, dạng thứ hai robot có cấu trúc mạch kín, tiêu biểu robot song song Vấn đề tính toán động lực học điều khiển cho hai loại robot có nhiều điểm khác biệt Còn dạng thứ ba, loại robot chuyển đổi qua lại hai dạng trên, từ dạng hở sang dạng kín ngược lại, robot di động có chân, chẳng hạn robot dáng người Trong trình lại robot, tùy vào thời điểm, ví dụ hai chân tiếp xúc, robot có dạng hệ kín, có chân tiếp đất, robot lại có dạng hệ hở Trong phạm vi luận văn này, phát triển phương pháp cho phép thực việc điều khiển cho robot loại thứ – dạng mạch hở Để thực việc điều khiển cho robot dạng thứ hai, cách thức phát triển thực cắt liên kết để chuyển robot từ dạng mạch kín thành dạng mạch hở, thay vết cắt cặp phản lực liên kết Việc lựa chọn điểm cắt thực dựa lý thuyết đồ thị để đảm bảo chọn điểm cắt tốt nhất, thuận lợi cho việc tính toán Và để thực việc điều khiển cho dạng thứ ba, thực ý tưởng dùng ngắt chuyển đổi chương trình điều khiển theo trạng thái robot Tức robot dạng hệ kín, sử dụng chương trình điều khiển cho hệ kín, chuyển sang hệ hở, sử dụng chương trình điều khiển hệ hở Frameworks phát triển nhằm mục đích cho phép điều khiển thời gian thực, tập trung chủ yếu vào giải thuật có chi phí tính toán thấp, viết tảng Matlab để thuận lợi cho việc thử nghiệm kiểm tra Và tới đây, hi vọng thử nghiệm kết tính toán cho mô hình robot cụ thể có kết cấu phức tạp – robot dáng người - để kiểm nghiệm tính đắn giải thuật đưa Do hạn chế thời gian trình độ hiểu biết nên chắn không tránh khỏi thiếu xót trình thực luận văn tốt nghiệp Vì mong nhận nhận xét, đánh giá quý thầy cô bạn để luận văn hoàn thiện Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 1  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở LỜI CẢM ƠN Trong suốt năm năm học đại học hai năm học cao học trường Đại học Bách Khoa Hà Nội, có hội theo đuổi nghiên cứu chủ đề hấp dẫn đời mình, Robotics Thời gian học đại học cho hội tìm hiểu robot di động robot tay máy, đây, trình nghiên cứu cao học, có điều kiện để tìm hiểu sâu mô hình robot lý thú phức tạp, robot dáng người Việc nghiên cứu robot dáng người có liên quan đến nhiều lĩnh vực khoa học, robotics, nghiên cứu vận động có chân (cơ - sinh), lập kế hoạch thực thi điều khiển theo mức ưu tiên, điều khiển phi tuyến… Tôi học nhiều điều lĩnh vực kết hợp lại để hoàn thành mục tiêu nghiên cứu Việc viết hoàn thành luận văn nỗ lực cá nhân mạnh mẽ, lẽ dự định nghiên cứu ban đầu không hoàn toàn giống với kết thu Nó đòi hỏi kiên nhẫn phải chấp nhận thực nghiệm mong đợi Trong trình thực việc nghiên cứu, không lần gặp thất bại sai hướng, thất bại dòng suối nhỏ, hợp lại để giúp đạt thành công, hoàn thành mục đích nghiên cứu đặt ban đầu Và nhận rằng, để thành công, không dựa nỗ lực cá nhân, mà cần có hợp tác với giáo sư, nghiên cứu sinh, sinh viên người có niềm ham thích đôi với vấn đề quan tâm Tôi muốn gửi lời cảm ơn chân thành đến người giúp hoàn thành luận văn Trước tiên muốn cảm ơn thầy Nguyễn Văn Khang, giáo viên hướng dẫn tôi, người truyền thụ cho nhiều kiến thức động lực học điều khiển, cho nhiều lời khuyên vô hữu ích lúc dường bế tắc Thầy người cho phương pháp nghiên cứu, cách tìm hiểu giải vấn đề Đồng thời, muốn gửi lời cảm ơn tới thầy Phan Bùi Khôi, giáo viên hướng dẫn thời đại học, người cho hội để thiết kế, chế tạo thực robot, từ học nhiều kinh nghiệm thực tế vô có giá trị Thầy người cho nhiều lời khuyển chân tình sống công việc nghiên cứu Bên cạnh đó, muốn gửi lời cảm ơn đền bạn bè tôi, người có niềm ham mê nghiên cứu robot với Cám ơn Lê Đức Đạt, người bạn đọc thảo tôi, trao đổi, cho nhiều lời khuyên hữu ích người giúp tìm nhiều báo giá trị Cám ơn Nguyễn Quốc Nam, người thường xuyên trao đổi với thiết kế robot dạy nhiều vấn đề xử lý ảnh, vấn đề quan tâm Cám ơn Nguyễn Hải Nguyên, người bạn phòng lớp cao học với tôi, người động viên, giúp đỡ cố gắng đề hoàn thành luận văn Ngoài ra, muốn gửi lời cám ơn tới người bên suốt thời gian học cao học Tôi muốn cảm ơn mẹ tôi, người động viên học cao học, bên tôi, giúp đỡ suốt thời gian qua hiểu tôi, chia sẻ với khó khăn Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 2  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở Tôi muốn cám ơn bố tôi, người cố vấn giúp đưa định quan trọng mình, bố thường xuyên phải làm việc xa nhà Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 3  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở MỤC LỤC LỜI MỞ ĐẦU 1  LỜI CẢM ƠN 2  CHƯƠNG .7  GIỚI THIỆU 7  Các vấn đề động học động lực học hệ nhiều vật 7  Tóm tắt chương 7  CHƯƠNG .9  NHỮNG VẤN ĐỀ CƠ BẢN CỦA ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC HỆ NHIỀU VẬT 9  Xây dựng mô hình .9  1.1 Cấu trúc liệu biểu diễn cho vật rắn 9  1.2 Quy tắc thiết lập quan hệ tọa độ hệ 12  1.3 Lý thuyết đồ thị ứng dụng toán hệ nhiều vật 13  1.3.1 Cách thiết lập liệu cho toán đồ thị 13  1.3.2 Tìm kiếm ưu tiên độ sâu 16  1.3.3 Ví dụ áp dụng với mô hình robot dáng người 21  1.3.4 Tìm kiếm ưu tiên theo chiều rộng 24  Bài toán động học 26  2.1 Bài toán động học cho hệ dạng chuỗi nối tiếp 26  2.1.1 Bài toán động học thuận 26  2.1.2 Bài toán động học ngược 28  2.1.2.1 Đặt toán công thức 28  2.1.2.2 Thuật toán xác định q = q ( t0 ) 29  2.1.2.3 Thuật toán xác định q k +1 = q ( tk +1 ) (k=0, 1, , n* - 1) 30  2.1.3 Các giải thuật bổ sung 31  2.1.3.1 Xác định vận tốc góc gia tốc góc vật rắn .31  2.1.3.2 Xác định vận tốc khối tâm vật rắn: .33  2.1.3.3 Giải thuật tính Jacobian J(q) .34  2.1.3.4 Giải thuật tính ma trận J&( q ) 37  2.1.3.5 Một số toán mở rộng ứng dụng trường hợp robot dư dẫn động 39  2.1.3.6 Vấn đề giới hạn khớp toán động học ngược 40  2.2 Bài toán động học cho hệ dạng 41  2.2.1 Bài toán động học thuận 42  2.2.2 Bài toán động học ngược 44  Bài toán động lực học 46  3.1 Bài toán động lực học ngược 46  3.1.1 Bài toán động lực học ngược cho hệ dạng chuỗi nối tiếp .46  3.1.1.1 Công thức xác định vận tốc góc gia tốc góc 46  Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 4  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở 3.1.1.2 Công thức xác định gia tốc khối tâm vật rắn 46  3.1.1.3 Công thức xác định lực 47  3.1.1.4 Công thức xác định mô men: 48  3.1.1.5 Giải thuật Newton – Euler Recusive 49  3.1.1.6 Các ví dụ áp dụng .51  3.1.2 Bài toán động lực học ngược cho hệ dạng chuỗi nối tiếp với trường hợp có nhân tử lagrange liên kết .54  3.1.2.1 Công thức xác định lực .54  3.1.2.2 Công thức xác định mô men: 55  3.1.3 Bài toán động lực học ngược cho hệ dạng 60  3.2 Bài toán động lực học thuận cho hệ hở 62  3.2.1 Giải thuật tính N ( q,q&) 63  3.2.2 Giải thuật tính M ( q ) 63  3.2.3 Giải thuật để tính ma trận M(q) 63  CHƯƠNG 3: 68  KIẾN TRÚC ĐIỀU KHIỂN VÀ ỨNG DỤNG .68  Kiến trúc điều khiển cho hệ .68  KẾT LUẬN 72  Tóm lược kết đóng góp 72  Các nghiên cứu tương lai 72  Tài liệu tham khảo 74  PHỤ LỤC .79  Phụ lục 1: .79  Phụ lục 2: .82  Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 5  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở MỤC LỤC HÌNH ẢNH Hình 1: Mô hình vật rắn cấu trúc nhập liệu 10  Hình 2: Sơ đồ cách thức cấu hình hệ dạng chuỗi nỗi tiếp 12  Hình 3: Đồ thị cấu trúc liên kết hệ nhiều vật 14  Hình 4: Lưu trữ liệu đồ thị ma trận kề dạng tam giác 14  Hình 5: Lữu trữ liệu cấu trúc kề 15  Hình 6: Phân tích nhập liệu cho hệ nhiều vật 16  Hình 7: Lữu trữ liệu cấu trúc kề mở rộng 16  Hình 8: Trình tự duyện qua đỉnh cạnh đồng thời xác định điểm cắt đồ thị giải thuật DFS 18  Hình 9: Tóm lược ý nghĩa DFS 19  Hình 10: Mô hình robot 35 DOFs sử dụng luận văn .21  Hình 11: Mô hình hóa robot sang dạng đồ thị .22  Hình 12: Giải thuật duyệt theo chiều rộng 25  Hình 13: Sơ đồ phân tầng 25  Hình 14: Sơ đồ phương pháp số giải toán động học ngược 31  Hình 15: Phép biến đổi từ hệ tọa độ tuyệt đối sang hệ tọa độ tương đối gắn với vật rắn 32  Hình 16: Dùng DFS để phân tích cấu trúc robot dáng người thành chuối nối tiếp .43  Hình 17: DFS cho mô hình robot dáng người .45  Hình 18: Tính phân cấp(2 mức) viêc tính toán động học/ động lực học cho hệ nhiều vật phương pháp đệ quy 60  MỤC LỤC BẢNG BIỂU Bảng 1: Ký hiệu ý nghĩa tham số biểu diễn vật rắn hệ nhiều vật 10  Bảng 2: Kết sau hàm DFS cấu trúc liên kết 23  Bảng 3:giải thuật truy hồi tính động học thuận robot dạng chuỗi .27  Bảng 4: Giải thuật tính J(q) 37  Bảng 5: Giải thuật tính J&( q ) .38  Bảng 6: Quy tắc ký hiệu cho hệ có cấu trục dạng 42  Bảng 7: kết phân tách hệ dạng thành chuỗi nối tiếp .45  Bảng 8: Giải thuật RNE tính động lực học cho hệ dạng chuỗi nối tiếp .49  Bảng 9: giải thuật tính động lực học hệ dạng chuỗi nối tiếp có ngoại lực/moment tác dụng 58  Bảng 10: Giải thuật tính ma trận M ( q ) 66  Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 6  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở CHƯƠNG GIỚI THIỆU Các vấn đề động học động lực học hệ nhiều vật Nền tảng học robotics đến từ Động lực học hệ nhiều vật Các nghiên cứu động học, động lực học điều khiển áp dụng cho robot dạng tay máy phát triển mạnh năm 80 kỷ trước, sau đó, với phát triển nhanh chóng khoa học máy tính, hàng loạt phương pháp số phát triển, cho phép giải toán động lực học cho hệ nhiều vật có cấu trúc phức tạp Hầu hết phương pháp tính nhanh dựa phương thức tính đệ quy Stepanenko cộng (Stepanenko Y, 1976) lần thiết lập phương thức đệ quy Newton Euler cho chuỗi không gian dạng hở Trong công thức họ, động học khâu biểu thị khung cố định Công thức Newton Euler Recursive đề xuất Luh cộng (Luh JYS, 1980) sử dụng khung toạ độ động dường trở thành giải thuật hiệu cho toán động lực học ngược, cho cấu hình thông thường tay máy dạng nối tiếp với bậc tự (DOFs) Chi phí tính toán phương pháp tỷ lệ tuyến tính với số lượng vật rắn có hệ, tức độ phức tạp tính toán cỡ O(n), với n số lượng vật rắn có hệ Hollerbach (Hollerbach, 1980) đưa dạng đệ quy với cách tiếp cận theo dạng Lagrange, từ đưa giải thuật khác có độ phức tạp cỡ O(n) (WM, 1982) coi tương đương hiệu tính toán so với phương pháp NE Sự phát triển giải thuật hiệu tính toán động lực học ngược nhắc lại Walker Orin [50] sử dụng sở cho việc đưa giải thuật tính toán động lực học thuận Đó giải thuật cỡ O(n3) gọi composite inertia method Jain (Jain, 1991) đề xuất giải thuật động lực học thuận cho hoàn hảo cho hệ nhiều vật có cấu trúc nối tiếp có chi phí tính toán cỡ O(n) Featherstone (FeatherStone, 1987) phát triển giải thuật đệ quy đầy đủ cỡ O(n) cho toán động lực học thuận hệ dạng chuỗi hở sử dụng kí hiệu vector không gian Trong đó, sử dụng kí hiệu twist wrench vector 6x1 để đơn giản hóa việc phân tích động lực vật rắn Bằng cách đưa moment quán tính vật rắn có khớp, ông ta đưa công thức động lực học thuận cỡ O(n), đó, gia tốc khớp tính toán mà không cần phải thực phép tính nghịch đảo ma trận khối lượng Công thức cỡ O(n) Featherstone có tác dụng lớn việc phát triển giải thuật động lực học hệ dạng , hệ dạng kín tay máy (Rodriguez G, 1991) Gần đây, Saha (Saha, 1999) phát triển giải thuật cỡ O(n) cho động lực học thuận ngược hệ nhiều vật dạng nối tiếp sử dụng ma trận DeNOC Trong luận văn này, dựa ý tưởng giải thuật đệ quy Newton – Euler để thực việc tính toán động lực học cho hệ nhiều vật dạng chuỗi nối tiếp dạng Và áp dụng kết cho mô hình robot dáng người Tóm tắt chương Luận văn tổ chức sau Trong chương 2, đưa giải thuật dùng việc nhận dạng hệ nhiều vật, sử dụng giải thuật lý thuyết đồ thị Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 7  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở Tiếp đó, xây dựng giải thuật đệ quy để thực việc tính toán động học, động lực học hệ nhiều vật dạng chuỗi nối tiếp dạng Đây sở tảng để áp dụng cho chương Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 8  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở CHƯƠNG NHỮNG VẤN ĐỀ CƠ BẢN CỦA ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC HỆ NHIỀU VẬT Xây dựng mô hình Xây dựng mô hình cách biểu diễn liên kết vật rắn hệ nhiều vật để nhằm đạt thuận lợi định việc tính toán Trong động lực học hệ nhiều vật, sử dụng nhiều nguyên tắc thiết lập hệ tọa độ Denavit – Hartenberg thiết lập hệ tọa độ Cray Ưu điểm phương pháp liệu đầu vào tối thiểu đảm bảo đủ thông tin Về chất, phương pháp thiết lập hệ tọa độ có chung mục đích đề tìm ma trận chuyển từ hệ tọa độ gắn với vật rắn sang hệ tọa độ gắn với vật rắn khác hệ Trong luận văn mình, đưa phương pháp khác để thiết lập phép chuyển đổi hệ tọa độ vật rắn, phương pháp thiết lập có ưu điểm thuận lợi cho việc chuyển đổi liệu từ phần mềm thiết kế Solidworks, Inventor sang mô hình tính toán matlab 1.1 Cấu trúc liệu biểu diễn cho vật rắn Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 9  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở τ Q2( k ) FP1( k ) FP2( k ) τ Q1( k ) P2( k ) (k ) JCon2 Jtype1( k ) z P1( #k rP2 JCon1( k ) Jtype2( k ) z z (k ) k) rP1( CSJ1( ) x k k) y rJ1( k ) rJ 2( x k rPnP( k) k CSGk rJ nJ( kk) ( ) JConnJ k k CSJ nJ( kk) z y k) y PnP( k) CSJ 2( ) x k y ( ) JtypenJ k k x FPnP( kk ) (k ) τ QnQ k Hình 1: Mô hình vật rắn cấu trúc nhập liệu Cách thức cấu hình cho hệ dựa dạng liệu vật rắn, với thông tin cần thiết mô tả hình theo bảng sau: Bảng 1: Ký hiệu ý nghĩa tham số biểu diễn vật rắn hệ nhiều vật Dạng liệu chung #k CGk Chỉ số vật rắn thứ (k) hệ Hệ tọa độ gắn với khối tâm vật rắn #k Do Matlab không hỗ trợ số 0, nên sử dụng làm vật rắn đánh số mk Khối lượng vật rắn #k I Ck = [3 × 3] Tensor quán tính vật rắn #k lấy so với hệ tọa độ gắn với khối tâm CGk Dạng liệu khớp C(Jki ) = [3 × 3], i = nJ k Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Hệ tọa độ gắn với khớp thứ (i) vật rắn #k tính so với hệ tọa độ gắn với khối tâm vật rắn #k Ở có số quy tắc thiết lập hệ tọa độ gắn với Trang 10  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở CHƯƠNG 3: KIẾN TRÚC ĐIỀU KHIỂN VÀ ỨNG DỤNG Trong hai chương 2, đề xuất vấn đề lý thuyết sở sử dụng để điều khiển robot dạng chuỗi robot dạng Chương thứ ba việc ứng dụng lý thuyết vào chương trình tính toán cho số mô hình robot cụ thể Việc tính toán xây dựng giải thuật điều khiển thực nhờ công cụ Matlab Kiến trúc điều khiển cho hệ Cartesian Space PID Controller Trajectory planning Joint Space Inverse Kinematic Mechanical System qr , q r q, q − e, e desired trajectory ( qr , q r , q  r ) Topology ∫ System of Chains ie u = K i ie + K p e + K d e u q, q , q  , q  r Computed Torque Control τ Forward Dynamics q, q , q  Simulation Hình 1: Sơ đồ kiến trúc điều khiển Kiến trúc điều khiển sử dụng sơ đồ khối mô tả chức thành phần chương trình tính toán điều khiển Trước hết khối thiết lập quỹ đạo không gian đề Khối thực việc thiết lập quỹ đạo cho điểm tác vụ robot, dựa theo yêu cầu công việc, đảm bảo tính cân trình di chuyển… Tiếp theo khối hệ (system), khối khối liệu hình học robot Nó lưu trữ thông tin khâu hệ, với cấu trúc lưu trữ đề cập mục chương 2, Bao gồm thông tin khối lượng, chiều dài, moment quán tính, vị trí trọng tâm, hệ tọa độ gắn với khớp… Tiếp khối Phân tích đồ thị (Topology), thực chức nhận dạng hệ nhiều vật đầu vào, từ phân tách thành hệ dạng chuỗi nối tiếp, để dễ dàng áp dụng tính toán động học, động lực học cho hệ dạng chuỗi nối tiếp có cho hệ Từ thông tin quỹ đạo không gian thao tác, kết hợp với chuỗi nhánh sau phân tích đồ thị, chương trình thực việc tính toán động học ngược, để từ tìm thông số khớp Các thông số đóng vai trò làm mục tiêu điều khiển (hay giá trị tham chiếu) Tiếp đó, điều khiển PID sử dụng phương pháp điều khiển tuyến tính hóa động lực học ngược tính toán lực/moment cần thiết để đưa đến điều khiển động khớp Sau có lực/moment điều khiển, áp dụng cho mô hình thực, sử dụng sensor để đo giá trị tọa độ, vận tốc, gia tốc khớp thực tế Sau sử dụng chúng làm thông tin mô máy tính Phương án thứ hai, áp dụng cho mô hình ảo (mô hình hệ máy tính), sử dụng tính toán động lực học thuận (forward dynamics) để tính đáp ứng đầu tương tự Trong luận văn này, sử dụng phương án thứ hai, hi vọng tương lai gần, chế Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 68  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở tạo thực robot dáng người, kiểm nghiệm xác lý thuyết Hình 2: Sơ đồ simulink Sơ đồ simulink cụ thể hóa sơ đồ nguyên lý điều khiển hình 4.1 Kết mô cho trường hợp điều khiển nửa robot dáng người: Hình 3.3: Kết mô điều khiển PID cho mô hình nửa robot dáng người với hai tay bám theo hai quỹ đạo đường thẳng cho trước Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 69  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở Lúc ban đầu, để chuyển từ vị trí thời trạng thái đứng yên hai bàn tay đến vị trí điểm đầu thao tác, điểm pivot hai bàn tay bị lệch so với quỹ đạo lớn Nhưng đến tiếp cận với quỹ đạo, điểm pivot hai bàn tay dần vào ổn định bám theo quỹ đạo để đến đích Một ưu điểm cách xây dựng mô chương trình cho phép lựa chọn điểm thân robot để làm điểm pivot điều khiển Lấy ví dụ, ta chọn điểm pivot khối tâm vật rắn nằm phần hông robot để điều khiển theo quỹ đạo định trước: Hình 3.4 Điều khiển điểm điểm pivot nằm hông robot bám theo quỹ đạo định trước Dựa việc cho phép điều khiển điểm pivot robot, chương trình mở khả cho phép robot điều khiển theo dáng theo mức ưu tiên (sẽ tập trung nghiên cứu giai đoạn tiếp theo) Ở thử nghiệm trường hợp thực nhiều tác vụ với nhiều điểm pivot: hai tay robot phần hông, điểm pivot bám theo quỹ đạo định trước: Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 70  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở Hình 3.5 Điều khiển đồng thời nhiều điểm pivot thân robot Hình 3.6 Điều khiển đồng thời phần hông bàn chân trái để thực bước Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 71  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở KẾT LUẬN Trong suốt luận văn này, phát triển lý thuyết cho tính toán động học, động lực học điều khiển hệ nhiều vật, ứng dụng vào robot dáng người Trong chương cuối này, thảo luận số điểm mốc nghiên cứu đưa kết luận đáng lưu ý Tóm lược kết đóng góp Nhìn lại, nêu số sở cho chương trình thực việc điều khiển hệ robot dạng mạch hở Trước hết xây dựng giải thuật tính toán động học, động lực học cho mô hình hệ vật rắn dạng chuỗi đơn phương pháp đệ quy, dựa tảng học nguyên lý Newton – Euler Tiếp đó, mở rộng cho hệ nhiều vật có cấu trúc dạng mạch hở dựa ý tưởng phân tách hệ dạng thành nhiều hệ dạng chuỗi đơn kết nối với Trong luận văn này, xây dựng nên giải thuật áp dụng để thực việc phân tách đó, dựa lý thuyết đồ thị - giải thuật duyệt đồ thị theo chiều sâu Chương trình đưa cách thức nhập liệu cho hệ thống để thuận tiện cho người dùng dễ dàng sử dụng, đồng thời thuận tiện cho chương trình nhận dạng, phân tích điều khiển hệ học đưa vào Trong phần điều khiển, chương trình sử dụng điều khiển PID, cho kết khả quan Ở đây, mô hình robot nhắm tới để điều khiển dạng với nhiều nhánh, nên việc phát triển hệ điều khiển với nhiều điểm pivot phát triển Chương trình cho phép lựa chọn điểm vật rắn hệ làm điểm pivot điều khiển điểm pivot bám theo quỹ đạo mong muốn Hơn thế, lựa chọn đồng thời nhiều điểm pivot robot điều khiển chúng đồng thời bám theo quỹ đạo mong muốn Frameworks tính toán điều khiển robot dáng người phát triển luận văn thiết kế để chạy thời gian thực Sử dụng giải thuật với tốc độ tính toán nhanh đề cập chương 2, với chi phí tính toán cỡ O(n) để tính đại lượng động học O(n2) để tính đại lượng động lực Do đòi hỏi tính toán thời gian thực, việc tính toán đòi hỏi phải xử lý nhanh Với hệ có số bậc tự 10, máy tính cá nhân hoàn toàn đáp ứng yêu cầu tính toán thời gian thực Tuy nhiên, với hệ có số bậc tự lớn, ví dụ robot sử dụng để minh họa luận văn này, có số lượng bậc tự lớn 30, việc tính toán thường bị chậm yêu cầu hàng chục lần Các nghiên cứu tương lai Trong tương lai gần, dự kiến áp dụng thử nghiệm chương trình cho mô hình robot dáng người thực, mô hình hệ nhiều vật có cấu trúc phức tạp Dựa mô tả robot dáng người hệ chịu dẫn động với 6DOFs thụ động gắn vào base nó, phát triển biểu diễn tác vụ động học động lực học robot liên kết tiếp xúc với Chúng hi vọng tương lai gần, áp dụng kết chương trình tính cho robot dáng người thực để kiểm nghiệm đắn giải thuật tính toán động Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 72  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở học, động lực học, giải thuật điều khiển sử dụng cho mô hình ảo máy tính Đồng thời, hướng tới việc phát triển tính toán song song để tăng cường tốc độ tính toán, phát triển chương trình tính toán hệ điều hành thời gian thực… Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 73  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở Tài liệu tham khảo A, J (1991) Unified formulation of dynamics for serial rigid multibody systems Journal of Guidance, Control and Dynamics , 531-542 Anderson, F and M Pandy (2001) Static and dynamic optimization solutions for gait are practically equivalent Journal of Biomechanics 34, 153–161 Angeles, J (2003) Fundamentals Of Robotic Mechanical Systems Theory Methods And Algorithms 2Nd Edition Springer Arai, H and O Khatib (1994, 81-84) Experiments with dynamic skills In Proceedings of The Japan-USA Symposium on Flexible Automation, Kobe, Japan Arai, H and S Tachi (1991, April) Dynamic control of a manipulator with passive joints in an operational coordinate space In Proceedings of the IEEE International Conference on Robotics and Automation, Sacramento, USA Arbib, M (1981) Perceptual Structures and Distributed Motor Control In: Brooks VB (ed) Handbook of physiology (Section 2: The nervous system, Vol IL Motor control, Part I) American Physiological Society, pp 1449-1480 Arimoto, S (2008) Control Theory of Multi-fingered Hands Springer Bachar, Y (2004) Developing Controllers for Biped Humanoid Locomotion Master of Science: University of Edinburgh Bae DS, H E (1987) A recursive formulation for constrained mechanical system dynamics: Part II Closed loop systems Int J of Mechanics of Structures and Machines , 481-506 Bae DS, H E (1987) A recursive formulation for constrained mechanical system dynamics:Part I Open systems Int J of Mechanics of Structures and Machines , 359-382 Baerlocher, P and R Boulic (1998, October) Task-priority formulation for the kinematic control of highly redundant articulated structures In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Victoria, Canada Baginski, B (1998) Motion Planning for Manipulators with Many Degrees of Freedom - The BB Method Munich: Doctoral Thesis Brock, O and O Khatib (2002) Elastic strips: A framework for motion generation in human environments International Journal of Robotics Research 21(12), 1031–1052 Brock, O., O Khatib, and S Viji (2002) Task-consistent obstacle avoidance and motion behavior for mobile manipulation In Proceedings of the IEEE International Conference on Robotics and Automation, Washingtion, USA, pp 388–393 10 11 12 13 14 15 16 17 18 Bruno Siciliano, O K (2008) Springer Handbook of Robotics Springer Buckley, C (1986) The application of continuum methods to path planning Ph D thesis, Stanford University, Stanford, USA Chang, K and O Khatib (2000, April) Operational space dynamics: Effcient algorithms for modeling and control of branching mechanisms In Proceedings of the IEEE International Conference on Robotics and Automation Cole, A., J Hauser, and S Sastry (1989) Kinematics and control of multifingered hands with rolling contact IEEE Transaction on Automation and Control 34 (4), 398–404 Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 74  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 Delp, S and J Loan (2000) A computational framework for simulating and analyzing human and animal movement IEEE Computational Science and Engineering 2(5), 46–55 Dimity M Gorinevsky, A M (1997) Force control of robotic systems CRC Press Edsinger, A L (2007) Robot Manipulation in Human Environments Doctor of Philosophy in Electrical Engineering and Computer Science Featherstone, R (1987) Robot Dynamics Algorithms Norwell, USA: Kluwer Academic Publishers Featherstone, R., S Thiebaut, and O Khatib (1999, May) A general contact model for dynamically-decouled force/motion control In Proceedings of the IEEE International Conference on Robotics and Automation, Detroit, USA Fichter, E and E McDowell (1980) A nover design for a robot arm In Advancements of Computer Technology, pp 250–256 ASME Frank L.Lewis, Darren M.Dawson, Chaouki T.Abdallah,( 2004) Robot Manipulator Control Theory and Practice, Second Edition – Marcel Dekker Fujimoto, Y and A Kawamura (1996) Proposal of biped walking control based on robust hybrid position/foce control In Proceedings of the IEEE International Conference on Robotics and Automation, Volume 4, pp 2724–2730 Hanafusa, H., T Yoshikawa, and Y Nakamura (1981) Analysis and control of articulated robot with redundancy In Proceedings of IFAC Symposium on Robot Control, Volume 4, pp 1927–1932 Harada, K., H Hirukawa, F Kanehiro, K Fujiwara, K Kaneko, S Kajita, and M Nakamura (2004, September) Dynamical balance of a humanoid robot grasping an environment In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Sendai, Japan, pp 1167–1173 Harada, K., S Kajita, K Kaneko, and H Hirukawa (2004, November) An analytical method on real-time gait planning for a humanoid robot In Proceeding of the IEEE/RSJ International Conference on Humanoid Robots, Los Angeles, USA, pp Hauser, K (2008) MOTION PLANNING FOR LEGGED AND HUMANOID ROBOTS DISSERTATION: STANFORD UNIVERSITY Himanshu Chaudhary, S K (2009) Dynamics and Balancing of Multibody Systems Springer Hirai,K., M Hirose, Y Haikawa, and T Takenaka (1998) The development of Honda humanoid robot In Proceedings of the IEEE International Conference on Robotics and Automation, Volume 2, Leuven, Belgium, pp 1321–1326 Hofmann, A G (2006) Robust Execution of Bipedal Walking Tasks From Biomechanical Principles Doctoral Thesis: Massachusetts Institute of Technology Hogan, N (1987, March-April) Stable execution of contact tasks using impedance control In Proceedings of the IEEE International Conference on Robotics and Automation, Raleigh, USA, pp 1047–1054 Hollerbach (1980) A recursive lagragian formulation of manipulator dynamics and a comparative study of dynamics formulation complexity IEEE trans Systematic man Cybernetics , 730-736 Kajita, S., F Kanehiro, K Kaneko, K Fujiwara, K Harada, K Yokoi, and H Hirukawa (2003a, September) Biped walking pattern generation by using preview control of zero-moment point In Proceedings of the IEEE International Conference on Robotics and Automation, Taipei, Taiwan, pp 14–19 Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 75  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 Kajita, S., F Kanehiro, K Kaneko, K Fujiwara, K Harada, K Yokoi, and H Hirukawa (2003b, October) Resolved momentum control: Humanoid motion planning based on the linear and angular momentum In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Las Vegas, USA, pp 1644–1650 Kanehiro, F and H Hirukawa (2001, September) Online self-collision checking for humanoids In Proceedings of the 19th Annual Conference of the Robotics Society of Japan, Tokyo, Japan Kaynov, D (2008) Open motion control architecture for humanoid robots Ph D Theis: Universidad Carlos III de Madrid Khang, N V (2005) Động lực học hệ nhiều vật NXB Khoa học kỹ thuật Khatib, O (1986) Real-time obstacle avoidance for manipulators and mobile robots International Journal of Robotics Research 5(1), 90–8 Khatib, O (1987) A unified approach for motion and force control of robot manipulators: The operational space formulation International Journal of Robotics Research 3(1), 43–53 Khatib, O and J Maitre (1978, September) Dynamic control of manipulators operating in a complex environment In Proceedings of RoManSy’78, 3rd CISMIFToMM Symposium, Udine, Italy, pp 267–282 Khatib, O., L Sentis, J Park, and J Warren (2004, March) Whole body dynamic behavior and control of human-like robots International Journal of Humanoid Robotics 1(1), 29–43 Kress-Gazit, H (2008) TRANSFORMING HIGH LEVEL TASKS TO LOW LEVEL CONTROLLERS University of Pennsylvania: Doctoral Thesis Kröger, T (2010) On-Line Trajectory Generation in Robotic Systems_Basic Concepts for Instantaneous Reactions to Unforeseen (Sensor) Events Springer Kuffner, J., K Nishiwaki, S Kagami, M Inaba, and H Inoue (2003, October) Motion planning for humanoid robots In Proceedings of the International Symposium of Robotics Research, Siena, Italy Kwon, S., W Chung, Y Youm, and M Kim (1991, October) Self-collision avoidance for n-link redundant manipulators In Proceedings of the IEEE International Conference on System, Man and Cybernetics, Charlottesville, USA, pp 937–942 Lionel Birglen, T L (2008) Underactuated Robotic Hands Springer Luh JYS, W M (1980) On-Line computational scheme for mechanical manipulators ASME Journal of Dynamic Systems, Measurement and Control , 69-76 M.OKADA, T T (2003) Humanoid Robot Mechanisms for Responsive Mobility Proceeding of the 2nd International Symposium on Adaptive Motion of Animals and Machines Maciejewski, A and C Klein (1985) Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments International Journal of Robotics Research 4(3), 109–117 MartijnWisse (2004) Essentials of dynamic walking: Analysis and design of twolegged robots Moravec, H (1980) Obstacle Avoidance and Navigation in the Real World by a Seeing Robot Rover Ph D thesis, Stanford University, Stanford, USA Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 76  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở 55 56 57 Nakamura, Y (1991) Advanced Robotics Redundancy and Optimization AddisonWesley Nakamura, Y., H Hanafusa, and T Yoshikawa (1987) Task-priority based control of robot manipulators International Journal of Robotics Research 6(2), 3–15 NAKAOKA, S (2005) LEARNING FROM OBSERVATION PARADIGM:LEG TASK MODELS FOR REPRODUCING HUMAN DANCE MOTIONS ON BIPED HUMANOID ROBOTS University of Tokyo: Doctral Thesis 58 Petrovskaya, A., J Park, and O Khatib (2007, April) Probabilistic estimation of whole body contacts for multi-contact robot control In Proceedings of the IEEE International Conference on Robotics and Automation, Rome, Italy 59 Pons, J L Wearable Robots:Biomechatronic Exoskeletons CSIC, Madrid, Spain: John Wiley & Sons Quinlan, S (1994) Real-time Modification of Collision-free Paths Ph D thesis, Stanford University, Stanford, USA R, F (1987) Robot dynamics algorithms, Kluwer Academic Publishers R Kelly, V S (2005) Control of robot manipulators in joint space Springer Rodriguez G, J A.-D (1991) A spatial operator algebra for manipulator modeling and control Int Journal of Robotics Research , 371-381 Roth, B., J Rastegar, and V Sheinmann (1973) On the design of computer controlled manipulators In First CISM IFToMM Symposium, pp 93–113 Russakov, J., O Khatib, and S Rock (1995, May) Extended operational space formulation for serial-toparallel chain (branching) manipulators In Proceedings of the IEEE International Conference on Robotics and Automation, Nagoya, Japan, pp 1056–1061 Saavedra, M R (2008) Stable locomotion of humanoid robots based on mass concentrated model Ph D Thesis: Universidad Carlos III de Madrid Sadao Kawamura, M S (2006) Advances in Robot Control - From Everyday Physics to Human-Like Movements Springer Sandin, P E (2003) Robot Mechanisms And Mechanical Devices Illustrated McGraw-Hill Sentis, L (2007) SYNTHESIS AND CONTROL OF WHOLE-BODY BEHAVIORS IN HUMANOID SYSTEMS Doctoral Thesis: stanford university Sentis, L and O Khatib (2005b, December) Synthesis of whole-body behaviors through hierarchical control of behavioral primitives International Journal of Humanoid Robotics 2(4), 505–518 Shabana, A A (2005) Dynamics of Multibody Systems_3_ed Cambridge University Press Siciliano, B and J Slotine (1991, June) A general framework for managing multiple tasks in highly redundant robotic systems In Proceedings of the IEEE International Conference on Advanced Robotics, Pisa, Italy, pp 1211–1216 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 Stepanenko Y, V M (1976) Dynamics of articulated open-chain active mechanisms Mathematical Biosciences 28 , 137-170 Sugihara, T (2004) Mobility Enhancement Control of Humanoid Robot based on Reaction Force Manipulation viaWhole Body Motion Ph D thesis, University of Tokyo, Tokyo, Japan Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 77  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở 75 76 77 78 79 80 Umetami, Y and K Yoshida (1989, June) Resolved motion rate control of space manipulators with generalized Jacobian matrix IEEE Transactions on Robotics and Automation 5(3) Valavanis, B S (1998) Control Problems in Robotics and Automation Springer Vukobratovic, M and B Borovac (2004) Zero-moment point thirty five years of its life International Journal of Humanoid Robotics 1(1), 157–173 W Khalil, E D (2002) Modeling, identification and control of robots Hermes Penton Yokoyama, K., H Handa, T Isozumi, Y Fukase, K Kaneko, F Kanehiro, Y Kawai, F Tomita, and H Hirukawa (2003, September) Cooperative works by a human and a humanoid robot In Proceedings of the IEEE International Conference on Robotics and Automation, Taipei, Taiwan, pp 2985–2991 Zonfrilli, F (2004) Theoretical and Experimental Issues in Biped Walking Control Based on Passive Dynamics Ph.D Thesis: Universitµa degli Studi di Roma Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 78  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở PHỤ LỤC Phụ lục 1: Mã nguồn chương trình Maple: > restart: > with(linalg): with(LinearAlgebra): > E2T:=proc(x::Vector) local y,cx,cy,cz,sx,sy,sz,rx,ry,rz; cx:=cos(x(1)): sx:=sin(x(1)): cy:=cos(x(2)): sy:=sin(x(2)): cz:=cos(x(3)): sz:=sin(x(3)): rx:=Matrix(3,3,[1,0,0,0,cx,-sx,0,sx,cx]): ry:=Matrix(3,3,[cy,0,sy,0,1,0,-sy,0,cy]): rz:=Matrix(3,3,[cz,-sz,0,sz,cz,0,0,0,1]): y:=Multiply(rx,Multiply(ry,rz)): return y: end proc: > wave:=proc(a::Vector) local wa: wa:=Matrix(3,3,[0,-a(3),a(2),a(3),0,-a(1),-a(2),a(1),0]): return wa: end proc: > # input data for robot configuration m:=: rJ[1][1]:=Vector([0,0,0]): rJ[1][2]:=Vector([0,0,0]): CSJ[1][1]:=E2T(Vector([0,0,0])): CSJ[1][2]:=E2T(Vector([0,Pi/2,0])): Jtype[1]:=Vector([1]): IC[1]:=Matrix(3,3,[0.4e-2, 0, 0, 0, 0.3e-2, 0, 0, 0, 0.1e-2]): m(1):=2: rJ[2][1]:=Vector([0,-a1,0]): rJ[2][2]:=Vector([0,0,0]): CSJ[2][1]:=E2T(Vector([0,0,0])): CSJ[2][2]:=E2T(Vector([0,0,0])): Jtype[2]:=Vector([1,1]): IC[2]:=ZeroMatrix(3): #Matrix(3,3,[0, 0, 0, 0, 0, 0, 0, 0, I1z]): m(2):=m1: rJ[3][1]:=Vector([0,-a2,0]): rJ[3][2]:=Vector([0,0,0]): CSJ[3][1]:=E2T(Vector([0,0,0])): CSJ[3][2]:=E2T(Vector([0,0,0])): Jtype[3]:=Vector([1,1]): Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 79  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở IC[3]:=ZeroMatrix(3): #Matrix(3,3,[0, 0, 0, 0, 0, 0, 0, 0, I2z]): m(3):=m2: FEXT:=Vector([Px,0,Py,0,0,0]): #q:=Vector([seq(theta[i],i=1 n1)]): #dq:=Vector([seq(dtheta[i],i=1 n1)]): #ddq:=Vector([seq(ddtheta[i],i=1 n1)]): > # Newton - Euler Recusive RNE := proc (rJ, CSJ, Jtype, IC, m, FEXT, q, dq, ddq) local n1, w, dw, z0, dv, A, T, i, z1, TR, dTR, CSP, f, n, dT, F, N, mC, tau, z2; n1 := RowDimension(m)-1; alias(seq(C[i] = cos(q[i]), i = n1), seq(S[i] = sin(q[i]), i = n1), C[12] = cos(q[1]+q[2]), C[123] = cos(q[1]+q[2]+q[3]), C[23] = cos(q[2]+q[3]), S[23] = sin(q[2]+q[3])); w[1] := ZeroVector(3); dw[1] := ZeroVector(3); z0 := Vector([0, 0, 1]); dv[1] := Vector([0, 0, g]); A[1] := IdentityMatrix(3); T[1] := IdentityMatrix(3); for i from to n1 if Jtype[i](1) = then T[i+1] := Multiply(CSJ[i][2], Transpose(CSJ[i+1][1])); A[i+1] := Multiply(A[i], T[i+1]); z1 := Multiply(CSJ[i+1][1], z0); w[i+1] := Multiply(Transpose(T[i+1]), w[i]); dw[i+1] := Multiply(Transpose(T[i+1]), dw[i]); dv[i+1] := Multiply(Transpose(T[i+1]), dv[i])+Multiply(Multiply(wave(w[i+1]), wave(w[i+1]))+wave(dw[i+1]), Multiply(Transpose(T[i+1]), rJ[i][2])+z1*q[i]-rJ[i+1][1])+2*Multiply(wave(w[i+1]), z1)*dq[i]+z1*ddq[i] else TR := Matrix(3, 3, [cos(q[i]), -sin(q[i]), 0, sin(q[i]), cos(q[i]), 0, 0, 0, 1]); dTR := Matrix(3, 3, [-sin(q[i]), -cos(q[i]), 0, cos(q[i]), -sin(q[i]), 0, 0, 0, 0])*dq[i]; T[i+1] := Multiply(Multiply(CSJ[i][2], TR), Transpose(CSJ[i+1][1])); dT := Multiply(Multiply(CSJ[i][2], dTR), Transpose(CSJ[i+1][1])); A[i+1] := Multiply(A[i], T[i+1]); z1 := Multiply(CSJ[i+1][1], z0); w[i+1] := Multiply(Transpose(T[i+1]), w[i])+z1*dq[i]; dw[i+1] := Multiply(Transpose(dT), w[i])+Multiply(Transpose(T[i+1]), dw[i])+z1*ddq[i]; Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 80  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở dv[i+1] := Multiply(Transpose(T[i+1]), dv[i]+Multiply(Multiply(wave(w[i]), wave(w[i]))+wave(dw[i]), rJ[i][2]))-Multiply(Multiply(wave(w[i+1]), wave(w[i+1]))+wave(dw[i+1]), rJ[i+1][1]); end if; end do; #Backward recusive CSP := Multiply(A[n1+1], CSJ[n1+1][2]); f[n1+1] := Multiply(Transpose(CSP), SubVector(FEXT, 3)); n[n1+1] := Multiply(Transpose(CSP), SubVector(FEXT, 6)); F := Multiply(CSJ[n1+1][2], f[n1+1]); N := Multiply(CSJ[n1+1][2], n[n1+1]); f[n1] := m(n1+1)*dv[n1+1]+F; mC := Multiply(IC[n1+1], dw[n1+1])+Multiply(Multiply(wave(w[n1+1]), IC[n1+1]), w[n1+1]); n[n1] := mC+N-Multiply(wave(rJ[n1+1][1]), f[n1])+Multiply(wave(rJ[n1+1][2]), F); tau[n1] := Multiply(Multiply(Transpose(z0), Transpose(CSJ[n1][1])), Jtype[n1+1](1)*n[n1]+(1Jtype[n1+1](1))*f[n1]); for i from n1-1 by -1 to F := Multiply(T[i+2], f[i+1]); N := Multiply(T[i+2], n[i+1]); mC := Multiply(IC[i+1], dw[i+1])+Multiply(Multiply(wave(w[i+1]), IC[i+1]), w[i+1]); z2 := Multiply(CSJ[i+1][2], z0); f[i] := m(i+1)*dv[i+1]+F; n[i] := mC+N-Multiply(wave(rJ[i+1][1]), f[i])+Multiply(wave(rJ[i+1][2])+(1Jtype[i+1](2))*wave(z2)*q[i], F); tau[i] := Multiply(Multiply(Transpose(z0), Transpose(CSJ[i+1][1])), Jtype[i+1](1)*n[i]+(1Jtype[i+1](1))*f[i]) end do; return tau: end proc: > tau := RNE(rJ, CSJ, Jtype, IC, m, FEXT, q, dq, ddq): > tau := simplify(tau, {S[1]^2+C[1]^2 = 1, S[2]^2+C[2]^2 = 1, C[1]*C[2]-S[1]*S[2] = C[12], S[1]*C[2]+C[1]*S[2] = S[12]}): > tau[1] := collect(tau[1], {Px, Py, ddq[1], ddq[2], dq[1], dq[2]}); > tau[2] := collect(tau[2], {Px, Py, ddq[1], ddq[2], dq[1], dq[2]}); Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 81  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở Phụ lục 2: Chương trình tính cho hệ robot dáng người, chương trình tính lớn, nên để CD chương trình kèm với luận văn Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 82  ... cao học: Động lực học điều khiển robot dạng mạch hở CHƯƠNG GIỚI THIỆU Các vấn đề động học động lực học hệ nhiều vật Nền tảng học robotics đến từ Động lực học hệ nhiều vật Các nghiên cứu động học, ...Luận văn cao học: Động lực học điều khiển robot dạng mạch hở LỜI CẢM ƠN Trong suốt năm năm học đại học hai năm học cao học trường Đại học Bách Khoa Hà Nội, có hội theo đuổi nghiên... nr2(nn ) Học viên: Nguyễn Mạnh Tiến  Lớp: Cơ học kỹ thuật 2008 - 2010 Trang 27  Luận văn cao học: Động lực học điều khiển robot dạng mạch hở 2.1.2 Bài toán động học ngược Bài toán động học ngược