Động lực học và điều khiển robot phẳng có khớp nối đàn hồi

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI LUẬN VĂN THẠC SĨ Động lực học điều khiển robot phẳng có khớp nối đàn hồi TRẦN VĂN HẢI Ngành: Kỹ thuật điện tử Giảng viên hướng dẫn: GS TSKH Nguyễn Văn Khang Viện: Cơ khí HÀ NỘI, 2020 TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI LUẬN VĂN THẠC SĨ Động lực học điều khiển robot phẳng có khớp nối đàn hồi TRẦN VĂN HẢI Ngành: Kỹ thuật điện tử Giảng viên hướng dẫn: GS TSKH Nguyễn Văn Khang Chữ ký GVHD Viện: Cơ khí HÀ NỘI, 2020 ĐỀ TÀI LUẬN VĂN Đề tài : Động lực học điều khiển robot phẳng có khớp nối đàn hồi Ngành: Kỹ thuật điện tử Học viên: Trần Văn Hải GIẢNG VIÊN HƯỚNG DẪN GS TSKH Nguyễn Văn Khang LỜI CẢM ƠN Lời đầu tiên, xin gửi lời cảm ơn chân thành sâu sắc tới GS.TSKH Nguyễn Văn Khang, người định hướng, tận tình bảo tơi q trình thực luận văn thạc sĩ Tôi xin chân thành cảm ơn thầy cô công tác Bộ môn Cơ học ứng dụng Bộ môn Kỹ thuật Cơ điện tử - Trường Đại học Bách Khoa Hà nội hướng dẫn, truyền cho kinh nghiệm quý báu nghiên cứu khoa học Tôi xin chân thành cảm ơn tới ban chủ nhiệm Khoa Cơ khí, Trường Đại học Thủy Lợi tồn thể thầy cơng tác Bộ môn Kỹ thuật Cơ điện tử, Trường Đại học Thủy Lợi tạo điều kiện thuận lợi, giúp đỡ tơi suốt q trình làm luận văn Cuối cùng, tơi xin chân thành cảm ơn đến gia đình bạn bè, người động viên ủng hộ suốt thời gian qua Tác giả luận văn Trần Văn Hải LỜI CAM ĐOAN Tôi xin cam đoan cơng trình nghiên cứu riêng tơi, kết nghiên cứu trình bày luận án trung thực, khách quan chưa công bố cơng trình khác Tác giả luận văn Trần Văn Hải MỤC LỤC MỤC LỤC i MỞ ĐẦU CHƯƠNG THIẾT LẬP PHƯƠNG TRÌNH CHUYỂN ĐỘNG CỦA ROBOT PHẲNG CÓ KHỚP NỐI ĐÀN HỒI 1.1 Phương trình lagrange loại hai robot có khớp nối đàn hồi 1.1.1 Mơ hình hóa Robot có khớp nối đàn hồi 1.1.2 Thiết lập phương trình Lagrange loại II 1.2 Phương trình lagrange loại hai robot hai khâu ba khâu phẳng có khớp nối đàn hồi 1.2.1 Robot phẳng có khâu vơí khớp đàn hồi 1.2.2 Robot phẳng khâu có khớp đàn hồi 17 CHƯƠNG ĐỘNG LỰC HỌC NGƯỢC ROBOT PHẲNG CÓ KHỚP NỐI ĐÀN HỒI 25 2.1 Bài toán động học ngược robot 25 2.1.1 Không gian thao tác khơng gian cấu hình robot 25 2.1.2 Bài toán động học ngược robot 27 2.1.2.1 Các phương pháp giải toán 28 2.1.3 Sự tồn nghiệm 28 2 Giải toán động học ngược phương pháp số 29 2.2.1 Các công thức xác định véctơ vận tốc véctơ gia tốc suy rộng 29 2.2.2 Các công thức xác định véc tơ tọa độ suy rộng 29 2.2.3 Thuật toán hiệu chỉnh gia lượng véc tơ tọa độ suy rộng 30 2.2.4 Đánh giá sai số 32 2.3 Phép nội suy xấp xỉ hàm [7] 32 2.3.1 Khái niệm nội suy 33 i 2.3.2 Phương pháp bình phương tối thiểu hay xấp xỉ trung bình phương 34 2.4 Giải toán động lực học ngược phương pháp số 41 2.4.1 Nội dung toán động lực học ngược robot có khớp nối đàn hồi 41 2.4.2 Các bước giải tốn động lực học ngược robot có khớp nối đàn hồi 42 2.5 Động lực học ngược robot phẳng hai khâu có khớp nối đàn hồi 43 2.5.1 Trường hợp quỹ đạo điểm thao tác đường thẳng 44 2.5.2 Trường hợp quỹ đạo điểm thao tác đường tròn 51 2.6 Động lực học ngược robot phẳng ba khâu có khớp nối đàn hồi 60 CHƯƠNG ĐIỀU KHIỂN ROBOT PHẲNG CÓ KHỚP NỐI ĐÀN HỒI 73 3.1 Tổng quan điều khiển bám quỹ đạo khâu thao tác 73 3.1.1 Giới thiệu chung 73 3.1.2 Bài tốn điều khiển khơng gian khớp 74 3.1.3 Bài toán điều khiển không gian thao tác 74 3.2 Điều khiển bám quỹ đạo robot có khớp nối đàn hồi 75 3.2.1 Phương pháp điều khiển dựa tuyến tính hóa phản hồi 75 3.2.2 Bộ điều khiển PD dựa động lực ngược [16] 77 3.2.3 Bộ điều phiển PD bù trọng lực [ 17] 77 KẾT LUẬN 79 TÀI LIỆU THAM KHẢO 80 ii DANH MỤC BẢNG BIỂU Bảng 1.2.1 Thông số Robot 10 Bảng 1.2.2 Thông số động học Craig robot khâu 14 Bảng 1.2.3 Tham số động lực học robot khâu 14 Bảng 1.2.4 Thông số Robot phẳng khâu có khớp nối đàn hồi 19 Bảng 1.2.5 Thông số động học Craig robot khâu 20 Bảng 1.2.6 Tham số động lực học robot khâu 20 iii DANH MỤC HÌNH VẼ Hình 1.1.1 Khớp đàn hồi Harmonic Hình 1.1.2: Robot nhẹ DLR LWR-III 7R có khớp nối đàn hồi Hình 1.1.3 Mơ hình hóa robot N khâu có khớp nối đàn hồi Hình 1.1.4 Mơ hình hóa khớp nối đàn hồi Hình 1.2.1.a Mơ hình thực tế robot phẳng có khớp nối đàn hồi Hình 1.2.1.b Robot phẳng khâu có khớp nối đàn hồi 10 Hình 1.2.2.a Robot phẳng khâu có khớp nối đàn hồi 18 Hình 1.2.2.b Mơ hình hóa Robot phẳng khâu có khớp nối đàn hồi 19 Hình 2.1.1 Tay máy khâu có khớp nối đàn hồi 27 Hình 2.5.2 Robot phẳng khâu có khớp nối đàn hồi 44 Hình 2.5.3 Đồ thị tọa độ suy rộng 𝒒𝟏𝒕, 𝒒𝟐𝒕 45 Hình 2.5.4 Cấu hình robot 46 Hình 2.5.5 Sai số hàm lượng giác xấp xỉ 𝒒𝟏𝒕 47 Hình 2.5.6 Sai số hàm lượng giác xấp xỉ 𝒒𝟐𝒕 47 Hình 2.5.7 Đồ thị hàm lượng giác 𝜽𝟏𝒕 50 Hình 2.5.8 Đồ thị hàm lượng giác 𝜽𝟐𝒕 50 Hình 2.5.9 Đồ thị mơmen 𝝉𝟏𝒕 51 Hình 2.5.10 Đồ thị mômen 𝝉𝟐𝒕 51 Hình 2.5.11 Robot phẳng khâu có khớp nối đàn hồi 52 Hình 2.5.12 Đồ thị tọa độ suy rộng 𝒒𝟏𝒕, 𝒒𝟐𝒕 54 Hình 2.5.13 Cấu hình robot 54 Hình 2.5.14 Sai số hàm lượng giác xấp xỉ 𝒒𝟏𝒕 55 Hình 2.5.15 Sai số hàm lượng giác xấp xỉ 𝒒𝟐𝒕 56 Hình 2.5.16 Đồ thị hàm lượng giác 𝜽𝟏𝒕 59 Hình 2.5.17 Đồ thị hàm lượng giác 𝜽𝟐𝒕 59 Hình 2.5.18 Đồ thị mơmen 𝝉𝟏𝒕 60 Hình 2.5.19 Đồ thị mơmen 𝝉𝟐𝒕 60 Hình 2.6.1 Robot phẳng khâu có khớp nối đàn hồi 61 Hình 2.6.2 Đồ thị tọa độ suy rộng 𝒒𝟏𝒕, 𝒒𝟐𝒕, 𝒒𝟑𝒕 63 iv Hình 2.6.3 Cấu hình robot khâu có khớp nối đàn hồi 63 Hình 2.6.4 Sai số hàm lượng giác xấp xỉ 𝒒𝟏𝒕 64 Hình 2.6.5 Sai số hàm lượng giác xấp xỉ 𝒒𝟐𝒕 65 Hình 2.6.6 Sai số hàm lượng giác xấp xỉ 𝒒𝟑𝒕 65 Hình 2.6.7 Đồ thị hàm 𝜽𝟏𝒕 69 Hình 2.6.8 Đồ thị hàm 𝜽𝟐𝒕 70 Hình 2.6.9 Đồ thị hàm 𝜽𝟑𝒕 70 Hình 2.6.10 Đồ thị mơmen 𝝉𝟏𝒕 71 Hình 2.6.11 Đồ thị mơmen 𝝉𝟐𝒕 71 Hình 2.6.12 Đồ thị mômen 𝝉𝟑𝒕 72 Hình 3.1.1 Sơ đồ hoạt động robot 73 Hình 3.1.2 Sơ đồ điều khiển khơng gian khớp 74 Hình 3.1.3 Sơ đồ điều khiển không gian thao tác 75 v ⃜ 𝑞1𝑑 = −6.9216 𝑐𝑜𝑠(2𝑡) − 0.2560 𝑠𝑖𝑛 (2𝑡) ⃜ 𝑞2d = 5.584 𝑐𝑜𝑠(2𝑡) + 5.584 𝑠𝑖𝑛(2𝑡) 𝑚11 𝑚12 𝑚13 𝑴𝑙 (𝒒) = [𝑚21 𝑚21 𝑚23 ] 𝑚31 𝑚21 𝑚31 𝑚11 = 𝐼1 + 𝐼2 + 𝐼3 + 𝑚1 𝑎12 + 𝑚2 (𝑙12 + 𝑎22 + 2𝑙1 𝑎2 𝐶2 ) + 𝑚3 (𝑙12 + 𝑙22 + 𝑎32 + 2𝑙1 𝑙2 𝐶2 + 2𝑙2 𝑎3 𝐶3 + 2𝑙1 𝑎3 𝐶23 ) 𝑚12 = 𝑚21 = 𝐼2 + 𝐼3 + 𝑚2 (𝑎22 + 𝑙1 𝑎2 𝐶2 ) + 𝑚3 (𝑙22 + 𝑎32 + 𝑙1 𝑙2 𝐶2 + 2𝑙2 𝑎3 𝐶3 + 𝑙1 𝑎3 𝐶23 ) 𝑚13 = 𝑚31 = 𝐼3 + 𝑚3 (𝑎32 + 𝑙1 𝑎3 𝐶23 + 𝑙2 𝑎3 𝐶3 ) 𝑚22 = 𝐼2 + 𝐼3 + 𝑚2 𝑎22 + 𝑚3 (𝑙22 + 𝑎32 + 2𝑙2 𝑎3 𝐶3 ) 𝑚23 = 𝑚32 = 𝑚3 (𝑎32 + 𝑙2 𝑎3 𝐶3 ) + 𝐼𝑧3 𝑚33 = 𝐼3 + 𝑚3 𝑎32 𝑚̇11 𝑴̇ = [𝑚̇21 𝑚̇31 𝑚̇12 𝑚̇22 𝑚̇32 𝑚̇13 𝑚̇23 ] 𝑚̇33 𝑚̇11 = −2𝑚2 𝑙1 𝑎2 𝑞̇ 𝑠𝑖𝑛 𝑞2 + 𝑚3 [−2𝑙1 𝑙2 𝑞̇ 𝑠𝑖𝑛 𝑞2 − 2𝑙2 𝑎3 𝑞̇ 𝑠𝑖𝑛 𝑞3 − 2𝑙1 𝑎3 (𝑞̇ + 𝑞̇ ) 𝑠𝑖𝑛(𝑞2 + 𝑞3 )] 𝑚̇12 = −2𝑚2 𝑙1 𝑎2 𝑞̇ 𝑠𝑖𝑛 𝑞2 + 𝑚3 [−𝑙1 𝑙2 𝑞̇ 𝑠𝑖𝑛 𝑞2 − 2𝑙2 𝑎3 𝑞̇ 𝑠𝑖𝑛 𝑞3 − 𝑙1 𝑎3 (𝑞̇ + 𝑞̇ ) 𝑠𝑖𝑛(𝑞2 + 𝑞3 )] 𝑚̇13 = 𝑚3 [−𝑎3 𝑙2 𝑞̇ 𝑠𝑖𝑛 𝑞3 − 𝑙1 𝑎3 (𝑞̇ + 𝑞̇ ) 𝑠𝑖𝑛(𝑞2 + 𝑞3 )] 𝑚̇21 = −2𝑚2 𝑙1 𝑎2 𝑞̇ 𝑠𝑖𝑛 𝑞2 + 𝑚3 [−𝑙1 𝑙2 𝑞̇ 𝑠𝑖𝑛 𝑞2 − 2𝑙2 𝑎3 𝑞̇ 𝑠𝑖𝑛 𝑞3 − 𝑙1 𝑎3 (𝑞̇ + 𝑞̇ ) 𝑠𝑖𝑛(𝑞2 + 𝑞3 )] 𝑚̇22 = −2𝑚3 𝑙2 𝑎3 𝑞̇ 𝑠𝑖𝑛 𝑞3 𝑚̇23 = −𝑚3 𝑙2 𝑎3 𝑞̇ 𝑠𝑖𝑛 𝑞3 𝑚̇31 = 𝑚3 [−𝑎3 𝑙2 𝑞̇ 𝑠𝑖𝑛 𝑞3 − 𝑙1 𝑎3 (𝑞̇ + 𝑞̇ ) 𝑠𝑖𝑛(𝑞2 + 𝑞3 )] 𝑚̇32 = −𝑚3 𝑙2 𝑎3 𝑞̇ 𝑠𝑖𝑛 𝑞3 𝑚̇33 = −𝑚3 𝑙2 𝑎3 𝑞̇ 𝑠𝑖𝑛 𝑞3 67 𝑚̈11 ̈ 𝑚 𝑴 = [ ̈ 21 𝑚̈31 𝑚̈12 𝑚̈22 𝑚̈32 𝑚̈13 𝑚̈23 ] 𝑚̈33 𝑚̈11 = −2𝑚2 𝑙1 𝑎2 (𝑞22 𝑐𝑜𝑠 𝑞2 + 𝑞̇ 𝑠𝑖𝑛 𝑞2 ) + 𝑚3 [ −2𝑙1 𝑙2 (𝑞̈ 𝑠𝑖𝑛 𝑞2 + 𝑞̇ 22 𝑐𝑜𝑠 𝑞2 ) − 2𝑙2 𝑎3 (𝑞̈ 𝑠𝑖𝑛 𝑞3 + 𝑞̇ 32 𝑐𝑜𝑠 𝑞3 ) ] −2𝑙1 𝑎3 (𝑞̇ + 𝑞̇ )2 𝑐𝑜𝑠(𝑞2 + 𝑞3 ) − 2𝑙1 𝑎3 (𝑞̈ + 𝑞̈ ) 𝑠𝑖𝑛(𝑞2 + 𝑞3 ) 𝑚̈12 = −2𝑚2 𝑙1 𝑎2 (𝑞22 𝑐𝑜𝑠 𝑞2 + 𝑞̇ 𝑠𝑖𝑛 𝑞2 ) + −𝑙1 𝑙2 (𝑞̈ 𝑠𝑖𝑛 𝑞2 + 𝑞̇ 22 𝑐𝑜𝑠 𝑞2 ) − 2𝑙2 𝑎3 (𝑞̈ 𝑠𝑖𝑛 𝑞3 + 𝑞̇ 32 𝑐𝑜𝑠 𝑞3 ) 𝑚3 [ ] −𝑙1 𝑎3 (𝑞̇ + 𝑞̇ )2 𝑐𝑜𝑠(𝑞2 + 𝑞3 ) − 𝑙1 𝑎3 (𝑞̈ + 𝑞̈ ) 𝑠𝑖𝑛(𝑞2 + 𝑞3 ) 𝑚̈13 = 𝑚3 [−𝑎3 𝑙2 (𝑞̈ 𝑠𝑖𝑛 𝑞3 + 𝑞̇ 32 𝑐𝑜𝑠 𝑞3 ) − 𝑙1 𝑎3 (𝑞̇ + 𝑞̇ )2 𝑐𝑜𝑠(𝑞2 + 𝑞3 ) − 𝑙1 𝑎3 (𝑞̈ + 𝑞̈ ) 𝑠𝑖𝑛(𝑞2 + 𝑞3 )] 𝑚̈21 = −2𝑚2 𝑙1 𝑎2 (𝑞22 𝑐𝑜𝑠 𝑞2 + 𝑞̇ 𝑠𝑖𝑛 𝑞2 ) −𝑙1 𝑙2 (𝑞̈ 𝑠𝑖𝑛 𝑞2 + 𝑞̇ 22 𝑐𝑜𝑠 𝑞2 ) − 2𝑙2 𝑎3 (𝑞̈ 𝑠𝑖𝑛 𝑞3 + 𝑞̇ 32 𝑐𝑜𝑠 𝑞3 ) + 𝑚3 [ ] −𝑙1 𝑎3 (𝑞̇ + 𝑞̇ )2 𝑐𝑜𝑠(𝑞2 + 𝑞3 ) − 𝑙1 𝑎3 (𝑞̈ + 𝑞̈ ) 𝑠𝑖𝑛(𝑞2 + 𝑞3 ) 𝑚̈22 = −2𝑚3 𝑙2 𝑎3 (𝑞̈ 𝑠𝑖𝑛 𝑞3 + 𝑞̇ 32 𝑐𝑜𝑠 𝑞3 ) 𝑚̈23 = −𝑚3 𝑙2 𝑎3 (𝑞̈ 𝑠𝑖𝑛 𝑞3 + 𝑞̇ 32 𝑐𝑜𝑠 𝑞3 ) 𝑚̈31 = 𝑚3 [−𝑎3 𝑙2 (𝑞̈ 𝑠𝑖𝑛 𝑞3 + 𝑞̇ 32 𝑐𝑜𝑠 𝑞3 ) − 𝑙1 𝑎3 (𝑞̇ + 𝑞̇ )2 𝑐𝑜𝑠(𝑞2 + 𝑞3 ) − 𝑙1 𝑎3 (𝑞̈ + 𝑞̈ ) 𝑠𝑖𝑛(𝑞2 + 𝑞3 )] 𝑚̈32 = −𝑚3 𝑙2 𝑎3 (𝑞̈ 𝑠𝑖𝑛 𝑞3 + 𝑞̇ 32 𝑐𝑜𝑠 𝑞3 ) 𝑚̈33 = −𝑚3 𝑙2 𝑎3 (𝑞̈ 𝑠𝑖𝑛 𝑞3 + 𝑞̇ 32 𝑐𝑜𝑠 𝑞3 ) 𝑐11 𝑪(𝒒, 𝒒̇ ) = [𝑐21 𝑐31 𝑐12 𝑐22 𝑐32 𝑐13 𝑐23 ] 𝑐33 𝑐11 = −2[𝑚2 𝑙1 𝑎2 𝑆2 + 𝑚3 𝑙1 (𝑙2 𝑆2 + 𝑎3 𝑆23 )]𝑞̇ − 2𝑚3 𝑎3 (𝑙2 𝑆3 + 𝑙1 𝑆23 )𝑞̇ 𝑐12 = −[𝑚2 𝑙1 𝑎2 𝑆2 + 𝑚3 𝑙1 (𝑙2 𝑆2 + 𝑎3 𝑆23 )]𝑞̇ − 𝑚3 𝑎3 (𝑙2 𝑆3 + 𝑙1 𝑆23 )𝑞̇ 𝑐13 = −𝑚3 𝑎3 𝑙1 𝑆23 𝑞̇ − 𝑚3 𝑎3 (𝑙2 𝑆3 + 𝑙1 𝑆23)𝑞̇ 𝑐21 = [𝑚2 𝑙1 𝑎2 𝑆2 + 𝑚3 𝑙1 (𝑙2 𝑆2 + 𝑎3 𝑆23)]𝑞̇ − [𝑚2 𝑙1 𝑎2 𝑆2 + 𝑚3 𝑙1 (𝑙2 𝑆2 + 𝑎3 𝑆23 )]𝑞̇ − 𝑚3 𝑎3 (4𝑙2 𝑆3 + 𝑙1 𝑆23 )𝑞̇ 𝑐22 = [𝑚2 𝑙1 𝑎2 𝑆2 + 𝑚3 𝑙1 (𝑙2 𝑆2 + 𝑎3 𝑆23 )]𝑞̇ − 2𝑚3 𝑎3 𝑙2 𝑆3 𝑞̇ 𝑐23 = 2𝑚3 𝑎3 𝑙1 𝑆23 𝑞̇ − 𝑚3 𝑎3 𝑙2 𝑆3 𝑞̇ 68 1 2 𝑐31 = 𝑚3 𝑎3 (𝑙2 𝑆3 + 𝑙1 𝑆23 )𝑞̇ + 𝑚3 𝑎3 (2𝑙2 𝑆3 − 𝑙1 𝑆23)𝑞̇ − 𝑚3 𝑎3 (𝑙2 𝑆3 + 𝑙1 𝑆23 )𝑞̇ 1 2 𝑐32 = 𝑚3 𝑎3 (2𝑙2 𝑆3 + 𝑙1 𝑆23 )𝑞̇ + 𝑚3 𝑎3 𝑙2 𝑆3 𝑞̇ − 𝑚3 𝑎3 𝑙2 𝑆3 𝑞̇ 1 2 𝑐33 = 𝑚3 𝑎3 (𝑙2 𝑆3 + 𝑙1 𝑆23 )𝑞̇ + 𝑚3 𝑎3 𝑙2 𝑆3 𝑞̇ 𝑚1 𝑔𝑎1 𝐶1 + 𝑚2 𝑔(𝑙1 𝐶1 + 𝑎2 𝐶12 ) + 𝑚3 𝑔(𝑙1 𝐶1 + 𝑙2 𝐶12 + 𝑎3 𝐶123 ) ] 𝒈(𝑞 ) = [ 𝑚2 𝑔𝑎2 𝐶12 + 𝑚3 𝑔(𝑙2 𝐶12 + 𝑎3 𝐶123 ) 𝑚3 𝑔𝑎3 𝐶123 (𝑚1 𝑔𝑎1 + 𝑚2 𝑔𝑙1 + 𝑚3 𝑔𝑙1 )𝐶1 + (𝑚2 𝑔𝑎2 + 𝑚3 𝑔𝑙2 )𝐶12 + 𝑚3 𝑔𝑎3 𝐶123 (𝑚2 𝑔𝑎2 + 𝑚3 𝑔𝑙2 )𝐶12 + 𝑚3 𝑔𝑎3 𝐶123 ] =[ 𝑚3 𝑔𝑎3 𝐶123 𝐾1 𝑲=[0 𝝉 = [𝜏1 𝐾2 𝜏2 𝜃1 𝐽1 ] ; 𝜽 = [𝜃2 ]; 𝑱 = [ 𝐾3 𝜃3 𝐽2 0 0] 𝐽3 𝜏 ]𝑇 Sử dụng phần mềm Maple ta vẽ đồ thị 𝜽1 (𝑡), 𝜽2 (𝑡), 𝜽3 (𝑡), 𝝉1 (𝑡), 𝝉2 (𝑡), 𝝉3 (𝑡)như sau: Hình 2.6.7 Đồ thị hàm 𝜽𝟏 (𝒕) 69 Hình 2.6.8 Đồ thị hàm 𝜽𝟐 (𝒕) Hình 2.6.9 Đồ thị hàm 𝜽𝟑 (𝒕) 70 Hình 2.6.10 Đồ thị mơmen 𝝉𝟏 (𝒕) Hình 2.6.11 Đồ thị mơmen 𝝉𝟐 (𝒕) 71 Hình 2.6.12 Đồ thị mômen 𝝉𝟑 (𝒕) 72 CHƯƠNG ĐIỀU KHIỂN ROBOT PHẲNG CĨ KHỚP NỐI ĐÀN HỒI Các tốn điều khiển chia thành nhóm: Điều khiển quanh cấu hình (vị trí) cân bằng: (𝒒, 𝜽, 𝒒̇ , 𝜽̇) = (𝒒𝑑 , 𝜽𝑑 , 𝟎, 𝟎) Điều khiển bám quỹ đạo chuyển động khâu thao tác 𝒒 = 𝒒𝑑 Điều khiển lực điều khiển lực/vị trí Trong chương này, tác giả xét tốn điều khiển bám quỹ đạo mong muốn 3.1 Tổng quan điều khiển bám quỹ đạo khâu thao tác 3.1.1 Giới thiệu chung Nhiệm vụ toán điều khiển bám quỹ đạo chuyển động khâu thao tác: Đảm bảo khâu thao tác (End-effector) chuyển động bám theo quỹ đạo cho trước không gian làm việc Cho trước quỹ đạo mong muốn 𝒙𝒅(𝒕), yêu cầu điều khiển để quỹ đạo thực tế x thỏa mãn điều kiện: ||𝒙 − 𝒙𝑑 || < 𝜀 (3.1.1) Hình 3.1.1 Sơ đồ hoạt động robot 73 Vị trí robot xác định tọa độ trạng thái q Từ x d  f  q d  ta suy cách hình thức q d  f 1  x d  Do tốn điều khiển chia thành loại: - Điều khiển không gian khớp - Điều khiển không gian thao tác 3.1.2 Bài tốn điều khiển khơng gian khớp Bài tốn điều khiển robot khơng gian khớp phân thành toán nhỏ: - Bài toán động học ngược: xd  f  qd   qd  f 1  xd  - Hệ thống điều khiển không gian khớp thiết kế đảm bảo vị trí khớp ln bám theo quỹ đạo mong muốn với sai lệch: | q  qd | Bài tốn điều khiển khơng gian khớp có ưu nhược điểm cụ thể sau: - Ưu điểm: Bộ điều khiển tác dụng trực tiếp đến hệ thống truyền động khớp - Nhược điểm: Hệ thống điều khiển khó đảm bảo độ xác khâu thao tác || x  xd || tồn sai lệch cấu dẫn động Hình 3.1.2 Sơ đồ điều khiển không gian khớp 3.1.3 Bài tốn điều khiển khơng gian thao tác Hệ thống điều khiển khơng gian thao tác có chức giữ sai số x xd tiến 0, nghĩa là: || x  xd ||   (3.1.2) 74 - Ưu điểm: Tác động trực tiếp tới biến không gian thao tác x - Nhược điểm: Trong trình điều khiển, phải đồng thời giải tốn động học ngược khối lượng tính tốn lớn, thời gian lâu Hình 3.1.3 Sơ đồ điều khiển không gian thao tác 3.2 Điều khiển bám quỹ đạo robot có khớp nối đàn hồi Trong luận văn này, tác giả chọn điều khiển robot có khớp nối đàn hồi không gian khớp Để thuận tiện q trình tính tốn, ta sử dụng số giả thuyết sau: - qd  t  có đạo hàm bậc bốn liên tục theo thời gian: qd  t   C - Tất tham số hình học khối lượng (khối lượng, mơmen qn tính, vị trí khối tâm) xác định cách xác 3.2.1 Phương pháp điều khiển dựa tuyến tính hóa phản hồi Trong chương 1, ta có phương trình vi phân chuyển động robot khâu phẳng có khớp nối đàn hồi 𝑴(𝒒)𝒒̈ + 𝑪(𝒒, 𝒒̇ )𝒒̇ + 𝒈(𝒒) + 𝑲(𝒒 − 𝜽) = 𝟎 { ̈ 𝑱𝜽 + 𝑲(𝜽 − 𝒒) = 𝝉 (3.1.1) (3.1.2) Ta thấy phương trình vi phân chuyển động thứ robot có khớp nối đàn hồi (3.1.1) không phụ thuộc vào momen đầu vào 𝝉 viết lại dạng: 𝑴(𝒒)𝒒̈ + 𝒏(𝒒, 𝒒̇ ) + 𝑲(𝒒 − 𝜽) = 𝟎 (3.1.3) Trong 𝒏(𝒒, 𝒒̇ ) = 𝑪(𝒒, 𝒒̇ )𝒒̇ + 𝒈(𝒒) (3.1.4) 75 Đạo hàm hai vế phương trình (3.1.3) ta thu được: 𝑴(𝒒)𝒒[3] + 𝑴̇(𝒒)𝒒̈ + 𝒏̇ (𝒒, 𝒒̇ ) + 𝑲(𝒒̇ − 𝜽̇) = 𝟎 (3.1.5) Tiếp tục đạo hàm hai vế (3.1.5) lần ta được: 𝑴(𝒒)𝒒[4] + 2𝑴̇(𝒒)𝒒[3] + 𝑴̈(𝒒)𝒒̈ + 𝒏̈ (𝒒, 𝒒̇ ) + 𝑲(𝒒̈ − 𝜽̈) = 𝟎 (3.1.6) Từ phương trình vi phân chuyển động động ta rút 𝜽̈ = 𝑱−1 [𝝉 − 𝑲(𝜽 − 𝒒)] (3.1.7) Thay 𝜽̈ từ (3.1.7) vào phương trình (3.1.6) ta được: 𝑴(𝒒)𝒒[4] + 𝒄(𝒒, 𝒒̇ , 𝒒̈ , 𝒒[3] ) + 𝑲𝑱−1 𝑲(𝜽 − 𝒒) = 𝑲𝑱−1𝝉 (3.1.8) Trong đó: 𝒄(𝒒, 𝒒̇ , 𝒒̈ , 𝒒[3] ) = 2𝑴̇(𝒒)𝒒[3] + (𝑴̈(𝒒) + 𝑲)𝒒̈ + 𝒏̈ (𝒒, 𝒒̇ ) (3.1.9) Từ người ta đề xuất luật điều khiển sau: 𝝉 = 𝑱𝑲−1 (𝑴(𝒒)𝒂 + 𝒄(𝒒, 𝒒̇ , 𝒒̈ , 𝒒[3] )) + 𝑲(𝜽 − 𝒒) (3.1.10) Trong đó: 𝒒[4] = 𝒂 (3.1.11) Các vectơ 𝒒̈ , 𝒒[3] xác định biểu thức: 𝒒̈ = 𝑴−1 (𝒒)[𝑲(𝜽 − 𝒒) − 𝒏(𝒒, 𝒒̇ )] (3.1.12) gia tốc khâu hàm (𝒒, 𝒒̇ , 𝜽) 𝒒[3] = 𝑴−1 (𝒒)[𝑲(𝜽̇ − 𝒒̇ ) − 𝑴̇(𝒒)𝒒̇ − 𝒏(𝒒, 𝒒̇ )] (3.1.13) gia tốc bậc ba hàm (𝒒, 𝒒̇ , 𝜽, 𝜽̇) Từ (3.1.9) ta thấy số hạng 𝒄(𝒒, 𝒒̇ , 𝒒̈ , 𝒒[3] )là hàm (𝒒, 𝒒̇ , 𝜽, 𝜽̇) Luật điều khiển (3.1.10) 𝝉 = 𝝉(𝒒, 𝜽, 𝒒̇ , 𝜽̇)có thể tính tốn theo phương pháp tuyến tính hóa phản hồi [12] Nghiên cứu ổn định toán bám quỹ đạo khâu thao tác Luật điều khiển dựa tuyến tính hóa phản hồi thực cách chọn: [4] [3] 𝒂 = 𝒒𝑑 + 𝑲3 (𝒒𝑑 − 𝒒[3] ) + 𝑲2 (𝒒̈ 𝑑 − 𝒒̈ ) + 𝑲1 (𝒒̇ 𝑑 − 𝒒) + 𝑲0 (𝒒𝑑 − 𝒒) ma trận khuếch đại đường chéo K , K1 , K , K chọn cho: 76 i  1,2, , N  𝑠 + 𝐾3𝑖 𝑠 + 𝐾2𝑖 𝑠 + 𝐾1𝑖 𝑠 + 𝐾0𝑖 Là đa thức Hurwirtz Sai số bám quỹ đạo 𝑒𝑖 = 𝑞𝑑𝑖 − 𝑞𝑖 cho hệ tọa độ khâu thỏa mãn điều kiện: [4] [3] 𝑒𝑖 + 𝐾3𝑖 𝑒𝑖 + 𝐾2𝑖 𝑒̈𝑖 + 𝐾1𝑖 𝑒̇𝑖 + 𝐾0𝑖 𝑒𝑖 = Do đó, phương trình vi phân tuyến tính ổn định theo hàm mũ (với trị riêng chọn 3.2.2 Bộ điều khiển PD dựa động lực ngược [16] Mơmen tính tốn có từ phương trình động lực ngược tính tốn: & & + K θ - q  τ d  Jθ d d d (3.2.1) Sử dụng cơng thức đoạn ta có: & &d  n  q d , q&d  θd  q d  K 1 M  q d  q  & & & & [3] θ&d  K 1 M  q d  q[4] d  c qd , qd , qd , qd   (3.2.2) Một điều khiển bám quỹ đạo đơn giản đề xuất là:   τ  τ d  K P  θ d  θ   K D θ&d  θ& (3.2.3) 3.2.3 Bộ điều phiển PD bù trọng lực [ 17] & &d Từ Trước tiên ta đưa tham số mong muốn khâu qd , q&d , q tính tốn đại lượng khác theo thơng số Thay q, θ q d , θd hệ phương trình vi phân robot ta & &d  C(q d , q&d )q& g(q d )  K (q d  θd )  M (q d )q  & &  K (θ  q )  τ Jθ d d d d  (3.3.1) τ d mơmen mong muốn Luật điều khiển bám quỹ đạo ý đến Ta viết lại phương trình (3.3.1) sau: 77 & &d  C(q d , q&d )q&d M(q)eq  C(q, q&)eq  g(q)  K (eq  e )  M(q d )q  & & & Je&   K (e  e q )  Jθd  g  q d   τ  (3.3.2) Trong đó: eq  q  q d e  θ  θd Phương trình (3.3.2) ngụ ý & &d đủ nhỏ luật điều khiển bám quỹ đạo với bù trọng lực đơn với q&d , q giản đề xuất sau: τ  Κ Pe  K De&  g  q d  (3.3.3) Một luật điều khiển xác đề xuất sau: & & τ  K Pe  K De&  g  q   Jθ d (3.3.4) Cần lưu ý hai luật điều khiển (3.3.3) (3.3.4) yêu cầu số đo vị trí vận tốc động 78 KẾT LUẬN Các kết luận văn Luận văn tập trung vào việc xây dựng mơ hình học tính tốn động lực học ngược mơ hình thu gọn robot có khớp nối đàn hồi Một số kết luận văn là: - Thiết lập phương trình vi phân chuyển động robot phẳng hai khâu ba khâu có khớp nối đàn hồi - Trình bày cách giải tốn động lực học ngược robot có khớp nối đàn hồi cách tổng quát Áp dụng phương pháp xấp xỉ hàm xấp xỉ gần chuyển động khâu hàm lượng giác đa thức Từ rút ngắn đáng kể thời gian tính tốn mà đảm bảo độ xác - Tính tốn mơ số toán động lực học ngược robot phẳng hai khâu ba khâu có khớp nối đàn hồi - Trình bày sơ lược tốn điều khiển robot có khớp nối đàn hồi Hướng nghiên cứu - Do thời gian trình độ có hạn nên tốn điều khiển robot có khớp nối đàn hồi chưa nghiên cứu kỹ Đây hướng nghiên cứu - Nghiên cứu động lực học điểu khiển mô hình phức tạp robot có khớp nối đàn hồi 79 TÀI LIỆU THAM KHẢO [1] Nguyễn Văn Khang, Cơ học kỹ thuật (in lần thứ 3), NXB Giáo dục Việt Nam, Hà Nội [2] Nguyễn Văn Khang, Động lực học hệ nhiều vật (in lần thứ 2), NXB Khoa học Kỹ thuật, Hà Nội 2017 [3] Nguyễn Văn Khang, Chu Anh Mỳ, Cơ sở robot công nghiệp, NXB Giáo dục Việt Nam [4] Nguyễn Thiện Phúc, Robot công nghiệp, NXB Khoa học Kỹ thuật, Hà Nội 2009 [5] Chu Hồng Anh, Tính tốn động lực học mơ số robot cơng nghiệp dựa chương trình ROBOTDYN cải tiến, Luận văn thạc sĩ khoa học Cơ kỹ thuật, Trường ĐH Bách khoa Hà Nội, 2010 [6] Nguyễn Quang Hoàng, Matlab 𝓈 Simulink cho kỹ sư, NXB Bách Khoa Hà Nội 2018 [7] Lê Trọng Vinh, Trần Minh Tồn, Giáo trình phương pháp tính Matlab, NXB Bách Khoa Hà Nội 2013 [8] W Schiehlen, P Eberhard, Applied Dynamics, Springer, Switzerland 2014 [9] J.Wittenburg, Dynamics of Multibody Systems (2 Edition) Berlin: Springer; 2008 [10] Shabana A.A Dynamics of multibody systems (3 Edition) Cambridge: Cambridge University Press; 2005 [11] Jalon J.G., Bayo E Kinematic and Dynamic Simulation of Multibody System/ The Real - Time Challenge Springer,Berlin 1994 [12] M.W Spong, M.Vidyasagar, Robot Dynamics and Control, John Wiley & Sons, 1989 80 [13] M W Spong, Modeling and Control of elastic joint robots, ASME-Journal of Dynamic Systems, Measurement, and Control, Vol.109, 1987, pp 310319 [14] C Canudas de Wit, B Siciliano, G Bastin, Theory of Robot Control, Springer, London 1996 [15] A.T.M Mossod, Motion and force control of flexible joint robot manipulartor, Doctor Thesis, McMaster University, Canada 1994 [16] A.De Luca, P Lucibello, A general algorthm for dynamic feedback linearization of robots with clastic joints, IEEE – International conference on Robotics and Automation, Belgium 1998,pp 504 -510 [17] S.Nicosia and P.Tomei, A PD control law for trajectory tracking of flexible joint robots, , IFAC Robot Control, Vienna, Austria, 1991 81 ... hai robot hai khâu ba khâu phẳng có khớp nối đàn hồi 1.2.1 Robot phẳng có khâu vơí khớp đàn hồi 1.2.2 Robot phẳng khâu có khớp đàn hồi 17 CHƯƠNG ĐỘNG LỰC HỌC NGƯỢC ROBOT PHẲNG... Trước động cánh tay robot đặt khớp nối đàn hồi Dựa vào giả thuyết trên, tác giả sử dụng thuật ngữ ? ?Robot có khớp nối đàn hồi? ?? để robot có khớp nối đàn hồi Tính đàn hồi khớp nối mơ hình hóa lị xo có. .. Hình 1.2.1.b Robot phẳng khâu có khớp nối đàn hồi 10 Hình 1.2.2.a Robot phẳng khâu có khớp nối đàn hồi 18 Hình 1.2.2.b Mơ hình hóa Robot phẳng khâu có khớp nối đàn hồi 19 Hình 2.1.1