Xây dựng mô hình động học, động lực học và phương pháp điều khiển cho robot biped

LỜI CẢM ƠN Em xin chân thành cảm ơn hướng dẫn giúp đỡ thiếu Thầy PGS.TS Lê Hồi Quốc q trình nghiên cứu đề tài Em xin chân thành cảm ơn thầy cô mơn Tự Động Hố ( Khoa Điện-Điện tử ) thầy tham gia q trình giảng dạy chuyên ngành, người đặt móng vững cho nghiên cứu đề tài Xin chân thành cảm ơn ủng hộ Anh,Chị lớp Cao Học K15, đồng nghiệp Bộ Mơn KTĐKTĐ ( Khoa Cơ Khí ) Và cuối xin dành tặng Cha Mẹ… Thành phố Hồ Chí Minh ngày 10 tháng 11 năm 2006 Học viên Nguyễn Quốc Chí MỤC LỤC Lời cảm ơn Mục lục i Tóm tắt đề tài .iv Abstract .v Danh mục hình vi Danh mục bảng biểu ix CHƯƠNG TỔNG QUAN 1.1 Những nghiên cứu phát triển loại Robot 1.1.1 Robot vehicle 1.1.2 Robot không gian 1.1.3 Robot công nghiệp,Robot cá nhân Robot dịch vụ 10 1.2 Robot Humanoid 12 1.2.1 1.2.2 1.2.3 1.2.4 Lược sử Robot Humanoid chân 13 Thành tựu lĩnh vực Robot Humanoid gần 15 Thiết kế Robot Humanoid chân 17 Lĩnh vực ứng dụng Robot Humanoid chân 21 1.3 Những thách thức nghiên cứu Robot Humanoid 22 1.3.1 1.3.2 1.3.3 1.3.4 Kết cấu khí 23 Mơ hình tốn 23 Điều khiển cho robot biped .23 Giá thành 23 CHƯƠNG NHIỆM VỤ LUẬN VĂN THẠC SĨ 24 2.1 2.2 2.3 2.4 Đặt vấn đề 24 Mục đích 24 Giới hạn đề tài .25 Những nội dung nghiên cứu thưc đề tài 25 CHƯƠNG PHÂN TÍCH CÁC PHƯƠNG THỨC DI CHUYỂN 27 3.1 Giới thiệu phương thức vận động .28 3.2 Các giải pháp cho phương thức vận động 31 3.3 Robot di chuyển chân 32 3.4 Robot di chuyển chân 38 3.5 Phân tích dáng người nguyên lý di chuyển robot chân .40 3.5.1 Chu kỳ bước người 41 3.5.2 Các nguyên lý giữ thăng di chuyển Robot Biped .41 i CHƯƠNG ĐỘNG HỌC ROBOT BIPED 45 4.1 Quy ước ký hiệu .46 4.1.1 Sơ đồ nguyên lý Robot biped 46 4.1.2 Ý nghĩa ký hiệu .47 4.1.2.1 Các thông số vật lý Robot biped .47 4.1.2.2 Các ký hiệu vị trí .47 4.1.2.3 Các ký hiệu công thức .47 4.1.3 Các quy ước hệ tọa độ góc 48 4.1.3.1 Quy ước đặt hệ tọa độ 48 4.1.3.2 Quy ước chiều góc 49 4.2 Phân tích bậc tự cho Robot biped 51 4.3 Động học 53 CHƯƠNG ĐỘNG LỰC HỌC ROBOT BIPED 57 5.1 Thiết lập 58 5.1.1 Thế hệ thống Robot biped 58 5.1.2 Động hệ thống Robot biped .59 5.2 Giai đoạn Single Phase 60 5.3 Giai đoạn Double Phase .61 CHƯƠNG HOẠCH ĐỊNH DI CHUYỂN CHO ROBOT BIPED 65 6.1 Phân tích khối tâm hệ thống 66 6.1.1 6.1.2 6.1.3 6.1.4 Sơ đồ .66 Tìm tâm khối lượng chân phần thân 67 Tọa độ tâm khối lượng chân phần thân 68 Tọa độ tâm khối lượng hệ thống 68 6.2 Khảo sát toán lắc .70 6.2.1 Giới thiệu .70 6.2.2 Phương trình chuyển động lắc ngược 3D 71 6.2.3 Mơ hình lắc ngược 3D tuyến tính 72 6.3 Áp dụng toán lắc ngược 3D 74 6.3.1 Thiết kế dáng cho Robot Biped giai đoạn Single Phase 74 6.3.1.1 Ràng buộc hướng phần thân Robot 75 6.3.1.2 Ràng buộc cho khối tâm M Robot biped 76 6.3.1.3 Kết .82 6.3.2 Thiết kế dáng cho Robot Biped giai đoạn Double Phase 87 6.3.2.1 6.3.2.2 6.3.2.3 6.3.2.4 6.3.2.5 Ràng buộc hướng phần thân Robot 88 Ràng buộc góc nghiêng sang trái sang phải Robot 88 Ràng buộc cho khối tâm M Robot biped 89 Ràng buộc cho Tip_e 90 Kết .90 ii 6.4 Thiết kế dáng Robot Biped chu kỳ bước 95 6.4.1 Thiết kế dáng cho Robot biped giai đoạn Double Phase 95 6.4.1.1 6.4.1.2 6.4.1.3 6.4.1.4 6.4.1.5 Ràng buộc hướng phần thân Robot 96 Ràng buộc góc nghiêng sang trái Robot .96 Ràng buộc cho khối tâm M Robot biped 97 Ràng buộc cho Tip_e 98 Kết .99 6.4.2 Thiết kế dáng cho Robot biped giai đoạn Single Phase 104 6.4.2.1 6.4.2.2 6.4.2.3 6.4.2.4 6.4.2.5 Ràng buộc hướng phần thân Robot 104 Ràng buộc góc nghiêng sang trái Robot 104 Ràng buộc cho khối tâm M Robot biped 104 Ràng buộc cho Tip_e 106 Kết 107 CHƯƠNG PHÁC THẢO KẾT CẤU CƠ KHÍ 112 7.1 Sơ đồ nguyên lý 113 7.2 Kết cấu khí khớp .115 7.2.1 Kết cấu khí khớp 1&7 115 7.2.2 Kết cấu khí khớp hơng 115 7.2.3 Kết cấu khí khớp 117 CHƯƠNG THUẬT TOÁN ĐIỀU KHIỂN 118 8.1 Giới thiệu vấn đề cần giải 119 8.2 Thiết kế điều khiển RDC 120 8.2.1 Phương trình động lực học giả thuyết 120 8.2.2 Thiết kế điều khiển RDC 121 8.3 Xây dựng điều khiển cho single phase .123 8.3.1 Xây dựng hệ trục tọa độ tương đối qi khớp quay .123 8.3.2 Thiết lập sơ đồ khối Matlab Simulink 125 8.3.2.1 Thiết lập sơ đồ khối Matlab Simulink cho khối động lực học 125 8.3.2.2 Thiết lập sơ đồ khối cho khối điều khiển 127 8.3.2.3 Thiết lập sơ đồ khối cho tín hiệu đầu vào mong muốn 129 CHƯƠNG KẾT LUẬN & HƯỚNG PHÁT TRIỂN ĐỀ TÀI .132 9.1 Kết luận .133 9.2 Hướng phát triển đề tài 133 Tài liệu tham khảo .134 Phụ lục(in riêng) iii TĨM TẮT LUẬN VĂN Giải trí nhu cầu thiếu người nhu cầu ngày trở thành lĩnh vực ứng dụng hệ Robot thông minh giống người.Với khoa học kỹ thuật ngày phát triển, hệ Robot đời ngày tiên tiến thông minh hình thành lĩnh vực Robot giống người gọi Humanoid Robot Humanoid Robot thực đánh dấu cho bước phát triển cao kỹ thuật Robot Đề tài tập trung vào nghiên cứu lý thuyết động học, động lực học, hoạch định di chuyển phương pháp điều khiển cho Robot Biped bậc tự không gian 3D Đề tài gồm có chương: • Chương Tổng quan • Chương Nhiệm vụ đề tài • Chương Phân tích phương thức di chuyển • Chương Động học Robot Biped • Chương Động lực học Robot Biped • Chương Hoạch định di chuyển cho Robot Biped • Chương Phương pháp điều khiển • Chương Phác thảo sơ kết cấu khí • Chương Kết luận v ABSTRACT Entertainment is one of people’s essential needs This need become a new application field of robot which has some aspects and actions as human With the progress of science, many robotic generations which were generated are more and more progressive and intelligent This progress has become a new Robotic field so calls “Humanoid Robots” Humanoid robots marks for the highest progress in robotic technology nowaday This master thesis focuses on researching the theory of Kinematics, Dynamics , Motion Planning and Control Method for a degrees of freedom Robot in 3D space The thesis contains chapters • Chapter Overview • Chapter Purpose of thesis • Chapter Analysing the Motion Method • Chapter Kinematics of Biped Robot • Chapter Dynamics of Biped Robot • Chapter Motion planning for Biped Robot • Chapter Control method • Chapter Drafting preliminary mechanical design • Chapter Conclusion & future works vi DANH MỤC HÌNH VẼ HÌNH 1.1 HÌNH 1.2 HÌNH 1.3 HÌNH 1.4 HÌNH 1.5 HÌNH 1.6 HÌNH 1.7 HÌNH 1.8 HÌNH 1.9 HÌNH 1.10 HÌNH 1.11 HÌNH 1.12 HÌNH 1.13 HÌNH 1.14 HÌNH 1.15 HÌNH 1.16 HÌNH 1.17 HÌNH 1.18 HÌNH 1.19 HÌNH 1.20 HÌNH 1.21 HÌNH 1.22 HÌNH 1.23 NASA Mars Rover IFREMER ASTER IBOT advanced wheel chair(DEKA,U.S.) Chó Robot Qiro(Sony)&ASIMO(Honda) Tay máy robot không gian(Canadian Space Agency) .7 Mars Exploration Rover Robot phi hành gia Mars Exploration Rover robot arm SPDM trạm không gian(Canadian Space Agency Main Arm Small Fine Arm (Japan Aerospace Exploration Agency) Robot di chuyển bề mặt mặt trăng Các ví dụ robot lĩnh vựcRobot cơng nghiệp FANUC(trái), Mobile Robot(giữa), Robot giải trí SONY (phải) 10 Số lượng Robot công nghiệp 10.000 công nhân 11 Tỉ lệ % lĩnh vực ứng dụng robot cơng nghiệp 11 Ví dụ service robot 12 Các nghiên cứu thành công Humanoid Robot .16 Các phiên phát triển ASIMO (Honda) 18 So sánh cử động người ASIMO 19 Sự vận động khớp người 19 Mô khớp cho Robot 19 Nghiên cứu phận Humanoid Robot 21 Các Humanoid Robot giải trí 22 HÌNH 3.1 HÌNH 3.2 HÌNH 3.3 HÌNH 3.4 HÌNH 3.5 HÌNH 3.6 HÌNH 3.7 HÌNH 3.8 HÌNH 3.9 HÌNH 3.10 HÌNH 3.11 HÌNH 3.12 HÌNH 3.13 HÌNH 3.14 HÌNH 3.15 HÌNH 3.16 HÌNH 3.17 HÌNH 3.18 HÌNH 3.19 Cơ chế vận động hệ sinh vật 29 Minh họa hệ vận động di chuyển hai chân .30 Mô tả quan hệ định lượng .30 RoboTrac .30 Sự xếp chân động vật khác 32 Hai ví dụ cấu trúc chân với ba bậc tự 33 Hai ví dụ minh họa dáng bốn chân 34 Trạng thái di chuyển ổn định tĩnh 35 Robot nhảy Reibert 36 Cơ cấu chân nhảy 2D 37 Robot mô người SDR-4X II hãng SONY 38 Robot mô người P2 hãng Honda 39 Robot WABIAN-RIII Đại học Waseda, Nhật Bản 40 Robot Nhảy Flamingo 41 Chu kỳ bước người 41 Trọng tâm chu kỳ bước .41 Quỹ đạo di chuyển khối tâm chiếu mặt phẳng ngang 42 Nguyên tắc khép chân vào phía đưa chân ngồi .42 Hoạt động nguyên tắc đưa chân vịng 43 HÌNH 4.1 Sơ đồ nguyên lý Robot Biped 46 vii HÌNH 4.2 HÌNH 4.3 HÌNH 4.4 HÌNH 4.5 HÌNH 4.6 HÌNH 4.7 HÌNH 4.8 Quy ước đặt hệ tọa độ 48 Vết di chuyển chân Robot .49 Hình chiếu Robot mp xOy .49 Quy ước chiều góc 50 Quy ước góc nghiên trái nghiên phải 50 Sơ đồ nguyên lý Robot Biped 51 Sơ đồ Robot Biped mặt phẳng xOy, yOz 53 HÌNH 5.1 HÌNH 5.2 Single Phase 60 Double Phase 61 HÌNH 6.1 Phân bố khối lượng khâu 66 HÌNH 6.2 Robot Biped mp xOy 66 HÌNH 6.3 Biểu diễn tọa độ khối tâm theo Hip .69 HÌNH 6.4 Mơ hình lắc ngược tuyến tính 3D 70 HÌNH 6.4b Dạng quỹ đạo lắc 74 HÌNH 6.5 Ràng buộc θ = 75 HÌNH 6.6 Ràng buộc góc θ r = θ l 76 HÌNH 6.7 Quỹ đạo lắc mp xOz 76 HÌNH 6.8 Đường di chuyển khối tâm M 77 HÌNH 6.9 Chiều cao khối tâm M .79 HÌNH 6.10 Hoạch định quỹ đạo di chuyển khối tâm M mặt phẳng xOz 79 HÌNH 6.11 Xác định điều kiện đầu cho Tip_e 81 HÌNH 6.12 Sơ đồ khối mô giai đoạn Single Phase dùng Malab Simulink 83 HÌNH 6.13 Đồ thị góc θ1 (t ) Single Phase 84 HÌNH 6.14 Đồ thị góc θ (t ) Single Phase .84 HÌNH 6.15 Đồ thị góc θ (t ) Single Phase .85 HÌNH 6.16 HÌNH 6.17 HÌNH 6.18 HÌNH 6.19 HÌNH 6.20 HÌNH 6.21 HÌNH 6.22 HÌNH 6.23 HÌNH 6.24 HÌNH 6.25 HÌNH 6.26 Đồ thị góc θ (t ) Single Phase .85 Đồ thị góc θ (t ) Single Phase .86 Đồ thị góc θ r (t ) = θ l (t ) Single Phase 86 Dáng Double Phase 87 Robot nghiên sang trái, sang phải 88 Quỹ đạo khối tâm M .88 Điều kiện đầu cho Double Phase 89 Sơ đồ khối mô giai đoạn Double Phase dùng Malab Simulink 91 Đồ thị góc θ1 (t ) Double Phase 92 Đồ thị góc θ (t ) Double Phase 92 Đồ thị góc θ (t ) Single Phase .93 HÌNH 6.27 HÌNH 6.28 HÌNH 6.29 HÌNH 6.30 HÌNH 6.31 HÌNH 6.32 HÌNH 6.33 HÌNH 6.34 Đồ thị góc θ (t ) Double Phase 93 Đồ thị góc θ (t ) Double Phase 94 Đồ thị góc θ r (t ) = θ l (t ) Double Phase 94 Quỹ đạo khối tâm M chu kỳ 95 Double phase chu kỳ .96 Robot nghiêng sang trái 97 Sơ đồ khối simulink giai đoạn Double Phase-chu kỳ 100 Đồ thị góc θ (t ) Double Phase 101 viii HÌNH 6.35 Đồ thị góc θ (t ) Double Phase 101 HÌNH 6.36 Đồ thị góc θ (t ) Double Phase 102 HÌNH 6.37 Đồ thị góc θ (t ) Double Phase 102 HÌNH 6.38 Đồ thị góc θ1 (t ) Double Phase 103 HÌNH 6.39 Đồ thị góc θ r (t ) = θ l (t ) Double Phase 103 HÌNH 6.40 Single phase chu kỳ 104 HÌNH 6.41 Điều kiện đầu cho Tip_e 106 HÌNH 6.42 Sơ đồ khối Matlab Simulink cho giai đoạn Single phase chu kỳ 108 HÌNH 6.43 Đồ thị góc θ (t ) Single Phase 109 HÌNH 6.44 Đồ thị góc θ (t ) Single Phase 109 HÌNH 6.45 Đồ thị góc θ (t ) Single Phase 110 HÌNH 6.46 Đồ thị góc θ (t ) Single Phase 110 HÌNH 6.47 Đồ thị góc θ1 (t ) Single Phase 111 HÌNH 6.48 Đồ thị góc θ r (t ) = θ l (t ) Single Phase 111 HÌNH 7.1 HÌNH 7.2 HÌNH 7.3 HÌNH 7.4 HÌNH 7.5 HÌNH 7.6 Sơ đồ nguyên lý 113 Các động 114 Kết cấu khí khớp gối(khớp 1&7) .115 Hộp đặt động Hip .116 Kết cấu khí khớp Hip1 .116 Kết cấu khí khớp .117 HÌNH 8.1 Tọa độ tương đối qi .124 HÌNH 8.2 Khối động lực học .125 HÌNH 8.3 Chi tiết khối động lực học .125 HÌNH 8.4 Sơ đồ khối vector b .126 HÌNH 8.5 Sơ đồ khối Ma trận M 126 HÌNH 8.6 Sơ đồ khối cho cột ma trận M 127 HÌNH 8.8 Sơ đồ khối điều khiển 127 HÌNH 8.10 Chi tiết sơ đồ khối điều khiển 128 HÌNH 8.11 Sơ đồ khối Ke .128 HÌNH 8.12 Sơ đồ khối chi tiết khối tín hiệu tham chiếu 129 HÌNH 8.15 Sai lệch khớp (độ/s) 129 HÌNH 8.16 Sai lệch khớp (độ/s) 129 HÌNH 8.17 Sai lệch khớp (độ/s) 130 HÌNH 8.18 Sai lệch khớp ,5(độ/s) .130 HÌNH 8.19 Sai lệch khớp (độ/s) 130 HÌNH 8.20 Sai lệch khớp 7(độ/s) 130 HÌNH 8.21 Biểu diễn ϕ 130 ix DANH MỤC BẢNG BIỂU BẢNG 1.1 BẢNG 1.2 BẢNG 1.3 BẢNG 1.4 BẢNG 6.1 Hệ thống loại Robot di chuyển Những nghiên cứu ưu tiên lĩnh vực Robot vehicle .5 So sánh mạnh lĩnh vực Robot vehicle Thế mạnh nghiên cứu Robot không gian quốc gia Các thơng số hình học khối lượng Robot biped 78 x Chương 8: THUẬT TOÁN ĐIỀU KHIÊN 8.1 GVHD: PGS.TS LÊ HOÀI QUỐC Giới thiệu vấn đề cần giải Một vấn đề thử thách việc điều khiển trình di chuyển Robot biped phi tuyến mơ hình động lực học thông số ước lượng không mơ hình biped Mục đích luật điều khiển cần thiết kế cung cấp tín hiệu điều khiển cho vị trí khớp cùa Robot biped bám theo quỹ đạo hoạch định chương hoạch định chuyển động Nói cách khác xem mơ hình động lực học Robot biped giai đoạn single phase double phase đối tượng điều khiển cần thiết kế điều khiển với luật điều khiển thích hợp Luật điều khiển tính tốn momen cần thiết để cung cấp cho mơ hình động lực học cho thông số khớp bám theo quỹ đạo hoạch định với sai số nhỏ Vấn đề biểu diễn sau: Sau có mơ hình động lực học Robot biped với góc θ, hệ trục toạ độ tuyệt đối: ( ) M (θ )θ&& + V θ , θ& + G (θ ) = τ [8-1] Chúng ta quy chuyển động hệ toạ độ suy rộng gắn với khớp, với q chuyển động tương đối khâu thành viên: M (q)q&& + V (q, q& ) + G(q ) = τ [8-2] ⎡q ⎤ Lúc có vec tơ trạng thái ⎢ ⎥ vectơ biểu diễn trạng thái đối ⎣q& ⎦ ⎡r ⎤ tượng, biểu diễn vectơ tham chiếu tín hiệu ngõ vào ⎢ ⎥ , vec tơ ⎣r&⎦ xác định từ trình hoạch định quỹ đạo di chuyển Thực thiết kế hệ thống điều khiển vịng kín cho đối tượng, hình thành vectơ sai lệch tín hiệu ngõ vào tín hiệu hồi tiếp e(t ) Mục tiêu luật điều khiển cung cấp tín hiệu τ cho e(t ) → Để thực vấn đề thử thách này, nhiều nhà nghiên cứu sử dụng phương pháp khác Có thể kể đến Mitobe sử dụng phương pháp điều khiển theo mơmen tính tốn trước cho mơ hình single phase (1995 ) double phase (1997) Nhưng phương pháp có điểm hạn chế, lý khơng thể ước luợng cách xác thơng số vật lý mà uớc luợng gần đúng, dẫn tới sai lệch tín hiệu điều khiển Tuy nhiên luật điều khiển theo phương pháp tính tốn mơmen cho góc quay có khả đạt độ xác cao thơng số khơng ước luợng xác khơng tồn có ảnh hưởng nhỏ Trong nghiên cứu đây, nhà nghiên cứu sử dụng phương pháp điều khiển thích nghi bền vững để giải cho hệ thống phi tuyến có thông số vật lý không ước lượng cách xác Trong đó, phương pháp điều khiển trượt (sliding mode control) phương pháp thường sử dụng phuơng pháp điều khiển bền vững ứng dụng cho điều khiển dáng Robot biped để chống lại tác động từ thông số không ước luợng NGUYỄN QUỐC CHÍ 119 Chương 8: THUẬT TỐN ĐIỀU KHIÊN GVHD: PGS.TS LÊ HOÀI QUỐC xác làm cho hệ trở nên ổn định bền vững Có thể kể đến kết sau, Tzasfestas ( 1996 ) sử dụng phương pháp điều khiển trượt cho Robot biped khâu giai đoạn single phase so sánh với phương pháp điều khiển theo tải trọng tính tốn Họ xác định điều khiển theo phương pháp trượt đạt kết tốt điều kiện có thơng số chưa xác định Một thử thách thuật tốn điều khiển điều khiển biped giai đoạn Double Phase Về mặt quan sát sơ bộ, điều khiển giai đoạn rõ ràng có nhiều thuận lơi giai đoạn Single Phase Chuyển động giai đoạn Double Phase biểu diễn dạng ràng buộc holonomic Tuy nhiên, sử dụng hệ trục toạ độ tự nhiên trình điều khiển không thực tốt việc bám theo quỹ đạo hoạch định điều kiện ràng buộc bị vi phạm ( Mitobe 1997 ) Hầu hết cách tiếp cận địi hỏi ước lượng xác mơ hình động lực học hoăc đơn giản hóa mơ hình Trong việc đơn giản hóa mơ hình, bỏ qua tải trọng động lực học, tương tác thành phần nhiễu trước Như biết ước lượng xác thơng số vật lý mơ hình phức tạp khó đạt với việc tương tác phần Robot tác dụng trọng lực Bên cạnh tác dụng tải nhiễu masát khó bỏ qua Trong luận văn này, tác giả sử dụng kỹ thuật điều khiển bền vững gọi điều khiển giảm chấn bền vững-robust damping control ( RDC ) trình bày [2] Một điều khiển RDC xây dựng cho robot biped cho khơng cần phải có thơng số ước lượng Bộ điều khiền cung cấp tín hiệu điều khiển bù sai lệch dựa quỹ đạo di chuyển định trước liệu đo lường vận tốc vị trí khớp Bên cạnh hệ số điều khiển cần thiết kế cho điều chỉnh cách dễ dàng 8.2 Thiết kế điều khiển RDC 8.2.1 Phương trình động lực học giả thiết: Chúng ta nhận thấy rằng, trường hợp single phase double phase phương trình động lực học mơ tả dạng phương trình đây: •• •• M q+ C qr + F + τ d = τ R [8-3] Trong phương trình [8-3] F vectơ biểu diễn cho tác động trọng lực lực ma sát, τ d mômen đặc trưng cho tác dụng nhiễu tác dụng lên Biped Để thuận tiện cho việc giải toán đặt gỉa thiết sau Giả thiết ( Nhiễu tác dụng lên Biped bao ): Nhiễu thay đổi theo thời gian τ d phương trình động lực học tay máy bao Có thể biểu diễn tốn học sau sup τ d ≤ τ N với τ N số dương NGUYỄN QUỐC CHÍ 120 Chương 8: THUẬT TỐN ĐIỀU KHIÊN GVHD: PGS.TS LÊ HỒI QUỐC Giả thiết ( Tác dụng ảnh hưởng trọng lực lực ma sát bao): Vectơ F ⎛⎜ q, q ⎞⎟ bao F2 ⎛⎜ q, q ⎞⎟ ≤ ξ + ξ q với ξ ξ số dương • • ⎝ ⎝ ⎠ • ⎠ Với giả thiết bắt đầu thiết kế phương pháp điều khiển RDC Chúng ta cần lưu ý phương trình động lực học biến đổi với biến góc quay tương đối khớp Bộ điều khiển làm nhiệm vụ tính tốn cung cấp mômen τ r đảm bảo ổn định chuyển dịch xác cho khớp 8.2.2 Thiết kế điều khiển RDC: a) Chọn xác định hàm Lyapunov cho Biped: Xem lại phương trình [8-3], ta định nghĩa sai số bám ( tracking error ) đạo hàm sai số bám sau • e = q rd − q r • • e = q rd − q r [8-4] Chúng ta định nghĩa thêm tham số phụ dẫn xuất từ sai số bám đạo hàm sai số bám • r = e + ke với k>0 [8-5] Chúng ta viết lại phương trình động lực học [8-3] với tham số phụ r sau: • M r = −τ r + ( Mk − C )( r − ke) + F + τ d = τ r [8-6] Trong F ảnh hưởng lực ma sát trọng lực lên mơ hình Để bắt đầu thiết kế điều khiển mơ men cho mơ hình tác giả chọn hàm Lyapunov sau: V = T r Mr [8-7] Chúng ta lưu ý ma trận M ma trận xác định dương M ma trận qn tính khối lượng mơ hình ( thành phần ma trận tạo momen quán tính xung quanh trục khác khối luợng ) Hơn giới hạn góc quay robot biped nên có tính chất sau ma trận M: M I p ≤ M ( q ) ≤ M max I p [8-8] M , M max : số dương tuỳ thuộc vào đặc điểm mặt khối luợng mơ hình Ip : ma trận đơn vị p× p NGUYỄN QUỐC CHÍ 121 Chương 8: THUẬT TỐN ĐIỀU KHIÊN GVHD: PGS.TS LÊ HỒI QUỐC Từ phương trình [8-8] lấy đạo hàm vế có : • V = r T {− τ r + Mkr + (C − Mk )ke + F + τ d } [8-9] Chúng ta viết lại [8-9] sau: • V = r T {− τ r + Mkr + (C − Mk )ke + F } + r T τ d [8-10] Từ có trình biến đổi sau : • • • ⎛ •• ⎞ r T {− τ r + Mkr + (C − Mk )ke + F } ≤ − r T τ r + r ⎜ M q + k (r − ke) + C q rd + ke q + ξ + ξ q + τ N ⎟ ⎝ ⎠ T T =r τ+ rΔ ϕ [8-11] Vec tơ Δ ϕ định nghĩa sau : ΔT = ( M , C , ξ , ξ + τ N ) ⎛ • • ⎞ • ϕ T = ⎜ q rd + k (r − ke) , q rd + ke q , q ,1⎟ ⎝ ⎠ [8-12] Với biến đổi từ việc chọn hàm Lyapunov luật điều khiển, vấn đề lại thiết kế điều khiển cung cấp momen τ N cho hệ thống ổn định bền vững Chúng ta chọn luật điều khiển theo mô men sau : τ r = k pr + k r ϕ [8-13] Ở k pr ≥ , k ≥ số độ lợi điều khiển, ϕ vectơ định nghĩa [8-12] Sau chứng minh ổn định điều khiển Thay [8-13] vào [8-10] ta có : • V ≤ k pr r T r − k r T r ϕ NGUYỄN QUỐC CHÍ + r Δϕ 122 Chương 8: THUẬT TOÁN ĐIỀU KHIÊN ≤ −k r GVHD: PGS.TS LÊ HOÀI QUỐC ϕ + r Δ ϕ ⎛ Δ ≤ −k ⎜⎜ r ϕ − 2k ⎝ 2 ⎞ Δ ⎟ + ⎟ 2k ⎠ ≤ −k ( r ϕ ) + r ϕ Δ − ≤ −k r ϕ ≤ −k ( r ϕ + r ϕ Δ − ) Δ 2k Δ 4k + r ϕ Δ + Δ + Δ 2k 2 4k 2 ⎡ r ϕ Δ Δ ⎤ ⎥ ≤ − k ⎢( r ϕ ) − − k2 4k ⎥ ⎢⎣ ⎦ ⎡ ⎤ Δ r ϕ Δ ≤ −k ⎢− ( r ϕ ) − − + 2( r ϕ ) ⎥ k2 4k ⎢⎣ ⎥⎦ ⎡ ⎛ ⎤ Δ ⎞ ⎟ + 2( r ϕ )2 ⎥ ≤ −k ⎢− ⎜ r ϕ + 2k ⎟⎠ ⎢ ⎜⎝ ⎥ ⎣ ⎦ ⎡ ⎛ Δ ⎞ ⎤ ⎟ ⎥ ≤ −k ⎢2( r ϕ ) − ⎜⎜ r ϕ + ⎟ ⎥ k ⎢ ⎝ ⎠ ⎦ ⎣ ⎡ Δ ⎤⎡ Δ ⎤ ≤ −k ⎢ − r ϕ − ⎥⎢ + r ϕ + ⎥ 2k ⎦ ⎣ 2k ⎦ ⎣ ( ) ( [8-14] ) Chúng ta quan sát biểu thức [8-14], sử dụng [8-8] [8-12] ta có chuẩn Δ giá trị bao Vì áp dụng lý thuyết Lyapunov LaSalle [3] để • giải vấn đề Nếu chọn giá trị k2 thích hợp, V ≤ 0, ∀r V → ∞ • x → Và ta có tập bất biến lớn gốc tọa độ e = 0, e = quỹ đạo pha tiến gốc tọa độ cách tiệm cận toàn cục t → ∞ , nói cách khác sai số bám tiến gốc tọa độ t → ∞ Trên sở nguyên tắc thiết kế áp dụng cho mơ hình Robot Biped 8.3 Xây dựng điều khiển cho Single Phase : 8.3.1 Xậy hệ trục tọa độ tương đối qi khớp quay : NGUYỄN QUỐC CHÍ 123 Chương 8: THUẬT TỐN ĐIỀU KHIÊN GVHD: PGS.TS LÊ HỒI QUỐC Như chương khảo sát phần động học ta khảo sát góc quay tuyệt đối θ khớp để tổng hợp dáng Robot Biped Thực chất vấn đề toán điều khiển Robot Biped trường hợp điều khiển động khớp để khớp đạt góc quay θ theo mong muốn Phối hợp điều khiển động tạo thành dáng cho Robot biped Do ta chuyển tọa độ tuyệt đối θ sang tọa độ tương đối q góc sai lệch phương khâu để việc điều khiển trở nên dễ dàng Hình 8.1 Tọa độ tương đối qi Ta xây dựng tọa độ tương đối qi khâu theo Hình 8.1 qi góc quay tương đối khâu i+1 với khâu i Với cách xây dựng ta tìm mối quan hệ qi θ i sau : ⎧q1 = θ1 − θ ⎪q = θ − θ ⎪ ⎪q = θ ⎪ ⎨q = 180 − θ + θ ⎪q = θ − θ − 360 ⎪ ⎪q = −θ r ⎪ ⎩q = −θ l NGUYỄN QUỐC CHÍ [8-14] 124 Chương 8: THUẬT TỐN ĐIỀU KHIÊN GVHD: PGS.TS LÊ HỒI QUỐC ⎧θ1 = q1 + q + q3 ⎪θ = q + q ⎪ ⎪θ = q3 ⎪ ⎨θ = q − q + 180 ⎪θ = q − q − q − 180 ⎪ ⎪θ r = − q ⎪ ⎩θ l = − q [8-15] 8.3.2 Thiết lập sơ đồ khối Matlab Simulink 8.3.2.1 Thiết lập sơ đồ khối Matlab Simulink cho khối động lực học Gọi u tín hiệu điều khiển q q& góc quay vận tốc góc khớp Robot Hình 8.2 Khối động lực học Trong khối động lực học diễn tả cho phương trình động lực học của Robot M (q ) q&& = b( q, q& ,τ ) [8-16] Chi tiết khối động lực học hình : Hình 8.3 Chi tiết khối động lực học Vector b gồm phần tử mô sơ đồ sau : NGUYỄN QUỐC CHÍ 125 Chương 8: THUẬT TỐN ĐIỀU KHIÊN GVHD: PGS.TS LÊ HỒI QUỐC Hình 8.4 Sơ đồ khối vector b Trong bi xem chi tiết phụ lục Ma trận M ma trận 7x7 thực theo sơ đồ khối sau : Hình 8.5 Sơ đồ khối Ma trận M NGUYỄN QUỐC CHÍ 126 Chương 8: THUẬT TỐN ĐIỀU KHIÊN GVHD: PGS.TS LÊ HỒI QUỐC Mỗi khối Mj có dạng sau : Hình 8.6 Sơ đồ khối cho cột ma trận M Các cột lại tương tự Xem chi tiết phụ lục 8.3.2.2 Thiết lập sơ đồ khối cho khối điều khiển Sơ đồ khối điều khiển Biped Tau q Scope1 plant -K- K1 Out1 -K- qrd tau K3 q Kinematic robust_controller -KK4 Hình 8.8 Sơ đồ khối điều khiển Chi tiết sơ đồ khối khối điều khiển NGUYỄN QUỐC CHÍ 127 Chương 8: THUẬT TỐN ĐIỀU KHIÊN GVHD: PGS.TS LÊ HỒI QUỐC In1 Out1 qrd In2 -K- Norm_phi1 Scope2 K2_1 q phi2 -K- u2 Divide K2_2 Math Function -KK2_3 tau -KScope1 K2_4 du/dt e -Kr Derivative K2_5 -K- 50 K2_6 K -Kq1 K2_7 q2 Kpr_1 0.3 q3 Kpr_2 q4 Kpr_3 -Kq5 Kpr_4 0.3 q6 Kpr_5 0.3 q7 Kpr_6 1.4 Kpr_7 Hình 8.10 Chi tiết sơ đồ khối điều khiển In1 Out1 In2 Norm_1 In1 In2 In1 Out1 In1 Out1 In2 Out1 Norm_2 Norm_Frobenius du/dt In1 Out1 Derivative Norm_Frobenius1 Hình 8.11 Chi tiết sơ đồ điều khiển NGUYỄN QUỐC CHÍ 128 Chương 8: THUẬT TỐN ĐIỀU KHIÊN 8.3.2.3 GVHD: PGS.TS LÊ HOÀI QUỐC Thiết lập sơ đồ khối cho tín hiệu đầu vào mong muốn : Khối động học, tín hiệu tham chiếu lấy từ khối động học vectơ góc quay tương đối khâu q vận tốc góc quay tương ứng q& Ta thiết lập khối tín hiệu tham chiếu ( từ kết thu giải toán hoạch định chuyển động ) cho sơ đồ điều khiển Hình 8.12 Hình 8.12 Sơ đồ khối chi tiết khối tín hiệu tham chiếu Với tọa độ qi tính theo cơng thức [8-14] ta có kết trình bày bên bên với k1 = 5, k = 130, k pr = 3.4 Hình 8.15 Sai lệch khớp (độ/s) NGUYỄN QUỐC CHÍ Hình 8.16 Sai lệch khớp (độ/s) 129 Chương 8: THUẬT TOÁN ĐIỀU KHIÊN GVHD: PGS.TS LÊ HỒI QUỐC Hình 8.17 Sai lệch khớp (độ/s) Hình 8.18 Sai lệch khớp (độ/s) Hình 8.18 Sai lệch khớp (độ/s) Hình 8.19 Sai lệch khớp (độ/s) Hình 8.20 Sai số khớp (độ/s) Hình 8.21 Biểu diễn ϕ NGUYỄN QUỐC CHÍ 130 Chương 8: THUẬT TỐN ĐIỀU KHIÊN GVHD: PGS.TS LÊ HOÀI QUỐC Với chứng minh phần 8.2 mô kiểm chứng kết hợp với mơ hình động lực học cho kết tương đối tốt sai lệch khớp quay Tuy nhiên nghiên cứu cần nghiên cứu phối hợp điều khiển có tính linh hoạt mạng nơ ron, fuzzy số mơ hình điều khiển thích nghi khác Lý để phát triển mơ hình điều khiển cho Robot Biped giới hạn luận văn chưa xét tới ảnh hưởng giai đoạn tiếp xúc với mặt đất chân di chuyển không gian tiếp xúc với mặt đất ảnh huởng phản lực mặt đất tác dụng lên chân trụ q trình di chuyển Ngồi tương lai cần tương tác với số thiết bị cảm biến ( camera, loadcell ) mục tiêu cần hướng tới xây dựng Humanoid Robot với trí thơng minh nhân tạo Do mơ hình điều khiển linh hoạt thống vấn đề cần phải đặt NGUYỄN QUỐC CHÍ 131 Chương 8: KẾT LUẬN&HƯỚNG PHÁT TRIỂN GVHD: PGS.TS LÊ HOÀI QUỐC CHƯƠNG KẾT LUẬN & HƯỚNG PHÁT TRIỂN ĐỀ TÀI NGUYỄN QUỐC CHÍ 132 Chương 8: KẾT LUẬN&HƯỚNG PHÁT TRIỂN 9.1 GVHD: PGS.TS LÊ HOÀI QUỐC Kết luận Các vấn đề giải Luận văn giải vấn đề lý thuyết động học, động lực học cho Robot biped bậc tự không gian 3D • Sử dụng phần mềm tính tốn Maple để tính tốn tọa độ, ma trận qn tính, vectơ liên quan đến phương trình động lực học Robot biped hai giai đoạn Single Phase Double Phase Luận văn hoạch định dáng Robot biped sở phân tích dáng người • Thực tổng hợp khối tâm Robot biped để từ áp dụng toán lắc ngược 3D vào hoạch định dáng Robot biped hai giai đoạn Single Phase Double Phase • Sử dụng phần mềm Matlab Simulink để mô điều kiện ràng buộc, đề tài giải vấn đề xác định góc quay khớp theo thời gian, hoạch định dáng cho Robot biped • Thực mô dáng Robot mô Matlab Simulink Xây dựng phương pháp điều khiển cho Robot biped sở điều khiển RDC mô đáp ứng khớp quay Matlab Simulink 9.2 Hướng phát triển đề tài Thực khảo sát ảnh hưởng chân Robot tiếp xúc đất mà hạn hẹp thời gian mà đề tài bỏ qua Vì tượng tiếp xúc đất ảnh hưởng lớn đến dáng Robot khả cân Robot Thực nghiên cứu điều khiển bám theo góc quay cho Robot Biped theo hướng có tính linh hoạt mạng nơ ron, fuzzy số mơ hình điều khiển thích nghi khác Xây dựng mơ hình thực tế cho Robot để kiểm chứng kết lý thuyết NGUYỄN QUỐC CHÍ 133 ... tay vào thực nghiên cứu đề tài: “XÂY DỰNG MƠ HÌNH ĐỘNG HỌC, ĐỘNG LỰC HỌC VÀ PHƯƠNG PHÁP ĐIỀU KHIỂN CHO ROBOT BIPED? ?? 2.2 Mục đích Đề tài “XÂY DỰNG MƠ HÌNH ĐỘNG HỌC, ĐỘNG LỰC HỌC VÀ PHƯƠNG PHÁP ĐIỀU... 1.2 HÌNH 1.3 HÌNH 1.4 HÌNH 1.5 HÌNH 1.6 HÌNH 1.7 HÌNH 1.8 HÌNH 1.9 HÌNH 1.10 HÌNH 1.11 HÌNH 1.12 HÌNH 1.13 HÌNH 1.14 HÌNH 1.15 HÌNH 1.16 HÌNH 1.17 HÌNH 1.18 HÌNH 1.19 HÌNH 1.20 HÌNH 1.21 HÌNH... Robot thực đánh dấu cho bước phát triển cao kỹ thuật Robot Đề tài tập trung vào nghiên cứu lý thuyết động học, động lực học, hoạch định di chuyển phương pháp điều khiển cho Robot Biped bậc tự không