Nghiên cứu phương pháp áp dụng điều khiển cuốn chiếu cho robot hồi phục chức năng chi dưới dạng ngồinằm

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI -TRẦN HOÀNG MINH Kpmx002@gmail.com NGHIÊN CỨU PHƯƠNG PHÁP ÁP DỤNG ĐIỀU KHIỂN CUỐN CHIẾU CHO ROBOT HỒI PHỤC CHỨC NĂNG CHI DƯỚI DẠNG NGỒI/ NẰM Chuyên ngành : Hệ thống điện tử thông minh LUẬN VĂN THẠC SĨ KHOA HỌC Kỹ thuật Cơ điện tử NGƯỜI HƯỚNG DẪN KHOA HỌC : TS Nguyễn Thành Trung Hà Nội – 04/2023 ii CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc BẢN XÁC NHẬN CHỈNH SỬA LUẬN VĂN THẠC SĨ Họ tên tác giả luận văn: Trần Hoàng Minh Đề tài luận văn: Nghiên cứu phương pháp áp dụng điều khiển chiếu cho robot hồi phục chức chi dạng ngồi/ nằm Chuyên ngành: Kỹ thuật Cơ Điện tử Mã số SV: 20202809M Tác giả, Người hướng dẫn khoa học Hội đồng chấm luận văn xác nhận tác giả sửa chữa, bổ sung luận văn theo biên họp Hội đồng ngày 27/04/2023 với nội dung sau: - Chỉnh sửa lỗi chế gồm: Lỗi tả, lỗi dồn trang, trích dẫn cơng thức, văn phong sử dụng luận văn - Sửa đổi, bổ sung đồ thị kết luận chương - Sửa đổi, bổ sung kết luận chương chương - Bổ sung nội dung chương 4: cách chọn hệ số PID luận văn, giải thích phương án sửa đổi hệ số phương pháp điều khiển chiếu Ngày tháng năm Giáo viên hướng dẫn Tác giả luận văn Nguyễn Thành Trung Trần Hoàng Minh CHỦ TỊCH HỘI ĐỒNG iii iv Lời cảm ơn Lời xin bày tỏ lòng biết ơn sâu sắc tới TS Nguyễn Thành Trung – Trường Cơ Khí – Đại học Bách Khoa Hà Nội người tận tình hướng dẫn, đốc thúc nghiên cứu thực Luận văn này; Tôi xin cảm ơn thầy cô giáo Đại học Bách Khoa Hà Nội giảng dạy, trang bị kiến thức để giúp thực Luận văn này; Cuối tơi xin cảm ơn gia đình, bạn bè, người thân chia sẻ, động viên suốt trình học tập nghiên cứu Đại học Bách Khoa Hà Nội Tóm tắt nội dung luận văn Tổ chức WHO dự báo vào năm 2050 tổng số lượng người cao tuổi tăng gấp đôi, kèm theo gia tăng bệnh liên quan Để ứng phó với tình hình đó, việc phát triển loại Robot phục hồi chức nhu cầu cấp thiết Luận văn tập trung vào nghiên cứu điều khiển cho Robot phục hồi chức chi Quá trình nghiên cứu việc thu tập tài liệu mơ hình điều khiển robot phục hồi chức chi quỹ đạo đầu vào phù hợp Sau nghiên cứu vào phương pháp điều khiển Backstepping cho Robot phục hồi chức năng, dùng phần mềm Matlab/Simulink để mô quỹ đạo phản hồi cho Robot Ngoài ra, phần mềm Maple Excel sử dụng để tính tốn vẽ đồ thị đầu vào nghiên cứu HỌC VIÊN Ký ghi rõ họ tên Trần Hoàng Minh v MỤC LỤC DANH MỤC HÌNH VẼ viii DANH MỤC BẢNG x DANH MỤC TỪ VIẾT TẮT x CHƯƠNG MƠ HÌNH ROBOT PHỤC HỒI CHỨC NĂNG CHI DƯỚI 1.1 Tầm quan trọng Robot phục hồi chức 1.1.1 Đột quỵ, bệnh tương tự hệ lụy liên quan 1.1.2 Giải pháp khắc phục 1.2 Các loại Robot phục hồi chức chi 1.3 Mơ hình Robot đề xuất 1.3.1 Mơ hình Robot 1.3.2 Ưu điểm nhược điểm mơ hình chọn 1.3.3 Mơ hình Robot thực tế CHƯƠNG TỔNG QUAN CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN ROBOT CƠ SỞ LÝ THUYẾT PHƯƠNG PHÁP BACKSTEPPING 2.1 Các phương pháp điều khiển robot 2.2 Điều khiển chuyển động không gian khớp 2.2.1 Điều khiển PD bù trọng trường 10 2.2.2 Điều khiển PID 13 2.3 Lý thuyết ổn định Lyapunov 13 2.4 Lý thuyết điều khiển Backstepping (điều khiển chiếu) 18 2.5 2.6 2.4.1 Thiết kế chiếu hàm CLF cho hệ truyền thẳng 19 2.4.2 Thiết kế chiếu hàm CLF cho hệ truyền ngược 20 Phương pháp điều khiển Backstepping tổng quan cho Robot n bậc tự 22 2.5.1 Tham số phương trình động lực học 22 2.5.2 Điều khiển không gian khớp 23 2.5.3 Điều khiển Backstepping không gian làm việc 26 Kết luận chương 28 vi CHƯƠNG ÁP DỤNG PHƯƠNG PHÁP ĐIỀU KHIỂN BACKSTEPPING CHO ROBOT SLLR 29 3.1 Bài toán điều khiển cho Robot 29 3.1.1 Thông số động học Robot SLLR 30 3.1.3 Ma trận chứa lực Coriolis lực li tâm 33 3.1.4 Ma trận lực (lực trọng trường) 33 3.1.5 Ma trận lực tác động ma sát, nhiễu ngoại lực 33 3.2 Các thông số đầu vào 34 3.3 Kết tính tốn động lực học .35 3.4 Thiết kế điều khiển Matlab/Simulink 35 3.5 3.4.1 Đầu vào cho mơ hình điều khiển Robot SLLR 35 3.4.2 Mơ hình điều khiển Matlab/Simulink 38 Kết luận chương 38 CHƯƠNG KẾT QUẢ MÔ PHỎNG 39 4.1 Mô với phương pháp PID 39 4.2 Mô với phương pháp Backstepping 43 4.3 4.2.1 Điều chỉnh hệ số .43 4.2.2 Điều chỉnh chu kỳ bổ sung hệ số ma sát 48 Kết luận chương 53 CHƯƠNG KẾT LUẬN .54 5.1 Đóng góp luận văn 54 5.2 Hướng phát triển nghiên cứu tương lai 54 TÀI LIỆU THAM KHẢO 55 PHỤ LỤC Các phương pháp điều khiển Robot 57 PHỤ LỤC Kết tính tốn động lực học Robot 67 PHỤ LỤC Code khối matlab 71 vii DANH MỤC HÌNH VẼ Hình 1.1 Các loại hệ thống robot để phục hồi chức chi dưới: (a) Luyện tập dáng máy chạy bộ, (b) luyện tập dáng để chân, (c) Luyện tập dáng di động mặt đất, (d) Luyện tập dáng không di chuyển, (e) Hệ thống phục hồi mắt cá chân Hình 1.2 Mơ hình ngun lý Robot Hình 1.3 Mơ hình Solidwork Robot SLLR Hình 1.4 Mơ hình thực tế Robot SLLR Hình 2.1 Các phương pháp điều khiển Robot Hình 2.2 Sơ đồ cấu trúc điều khiển chung khơng gian khớp Hình 2.3 Sơ đồ khối phương pháp điều khiển PD bù trọng lực 12 Hình 2.4 Sơ đồ điều khiển với phương pháp PID 13 Hình 2.5 Minh họa khái niệm ổn định Lyapunov 16 Hình 2.6 Kiểm tra tính ổn định hệ  17 Hình 2.7 Đối tượng truyền thẳng qua khâu tích phân 19 Hình 2.8 Cấu trúc hệ truyền ngược 21 Hình 3.1 Mơ hình tính tốn 30 Hình 3.2 Đồ thị quỹ đạo góc cho khâu Hông, Đầu gối, Mắt cá chân () 36 Hình 3.3 Đồ thị vận tốc góc cho khâu Hông, Đầu gối, Mắt cá chân (/s) 36 Hình 3.4 Đồ thị gia tốc góc cho khâu Hơng, Đầu gối, Mắt cá chân () 37 Hình 3.5 Sơ đồ điều khiển 38 Hình 3.6 Sơ đồ khối điều khiển Matlab/Simulink 38 Hình 4.1 Mơ quỹ đạo góc Hơng PID       39 Hình 4.2 Mơ quỹ đạo góc Đầu gối PID        40 Hình 4.3 Mơ quỹ đạo góc Mắt cá chân PID        40 Hình 4.4 Sai lệch quỹ đạo góc robot SLLR sử dụng phương pháp PID        41 Hình 4.5 Sai lệch vận tốc góc robot SLLR sử dụng phương pháp PID        41 Hình 4.6 Sai lệch quỹ đạo góc Robot SLLR sử dụng Backstepping có hệ số   nhỏ        43 viii Hình 4.7 Sai lệch vận tốc góc Robot SLLR sử dụng Backstepping có hệ số   nhỏ        .44 Hình 4.8 So sánh sai lệch quỹ đạo khâu HÔNG sau điều chỉnh hệ số            Hình 4.9 So sánh sai lệch quỹ đạo khâu ĐẦU GỐI sau điều chỉnh hệ số            Hình 4.10 So sánh sai lệch quỹ đạo khâu MẮT CÁ CHÂN sau điều chỉnh hệ số            Hình 4.11 So sánh sai lệch vận tốc góc khâu HƠNG sau điều chỉnh hệ số            Hình 4.12 So sánh sai lệch vận tốc góc khâu ĐẦU GỐI sau điều chỉnh hệ số            Hình 4.13 So sánh sai lệch vận tốc góc khâu MẮT CÁ CHÂN sau điều chỉnh hệ số            Hình 4.14 Sai lệch quỹ đạo góc Robot SLLR có hệ số ma sát qua phương pháp Backstepping        49 Hình 4.15 Sai lệch vận tốc góc Robot SLLR có hệ số ma sát qua phương pháp Backstepping        49 Hình 4.16 Sai lệch quỹ đạo góc Hơng phương pháp điều khiển Backstepping PID cho robot SLLR với        50 Hình 4.17 Sai lệch vận tốc góc Hơng phương pháp điều khiển Backstepping PID cho robot SLLR với        50 Hình 4.18 Sai lệch quỹ đạo góc Đầu gối phương pháp điều khiển Backstepping PID cho robot SLLR với        .51 Hình 4.19 Sai lệch vận tốc góc Đầu gối phương pháp điều khiển Backstepping PID cho robot SLLR với        51 Hình 4.20 Sai lệch quỹ đạo góc Mắt cá chân phương pháp điều khiển Backstepping PID cho robot SLLR với        .52 Hình 4.21 Sai lệch vận tốc góc Mắt cá chân phương pháp điều khiển Backstepping PID cho robot SLLR với        .52 ix DANH MỤC BẢNG Bảng 3.1 Bảng D-H 30 DANH MỤC TỪ VIẾT TẮT SLLR Seat type Lower Limb Rehabilitation Robot CLF (Dạng robot phục hồi chức chi dạng ngồi) Hàm Lyapunov x   󰇛   󰇜   󰇛    󰇜      󰇡    󰇛    󰇜   󰇛  󰇜  󰇛󰇜󰇢 󰇡 󰇛          󰇛    󰇜   󰇛   󰇜󰇢   󰇜           󰇛    󰇜   󰇛    󰇜   󰇛  󰇡        󰇛  󰇜  󰇛󰇜󰇢 󰇡 󰇛    󰇜    󰇜        󰇛    󰇜   󰇛    󰇜󰇢;       󰇛󰇜 󰇛󰇜  󰇛󰇜 󰇛󰇜󰇜     󰇡 󰇛    󰇛󰇜  󰇛󰇜  󰇛󰇜 󰇛󰇜   󰇛󰇜󰇢 󰇡 󰇛󰇜 󰇛󰇜  󰇛󰇜 󰇛󰇜󰇜    󰇛       󰇛󰇜 󰇛󰇜  󰇛󰇜 󰇛󰇜󰇢 󰇡 󰇛 󰇛󰇜 󰇛󰇜   󰇛󰇜 󰇛󰇜󰇜 󰇛󰇜  󰇛󰇜  󰇛󰇜  󰇛󰇜      󰇛󰇜󰇢 󰇡 󰇛󰇜 󰇛󰇜  󰇛󰇜 󰇛󰇜󰇜   󰇛       󰇛󰇜  󰇛󰇜  󰇛󰇜 󰇛󰇜󰇢    󰇛      󰇡      󰇛    󰇜   󰇛    󰇜 󰇜         󰇛  󰇜  󰇛󰇜󰇢 󰇡 󰇛    󰇜   󰇛          󰇛    󰇜  󰇛  󰇛    󰇜   󰇜󰇢 󰇡        󰇛    󰇜   󰇛    󰇜 󰇜         󰇛  󰇜  󰇛󰇜󰇢 󰇡 󰇛    󰇜   󰇛            󰇛    󰇜   󰇛  󰇜󰇢; 󰇜        󰇛󰇜 󰇛󰇜  󰇛󰇜 󰇛󰇜󰇜     󰇡 󰇛      󰇛󰇜  󰇛󰇜  󰇛󰇜 󰇛󰇜󰇢 󰇛󰇜 󰇛󰇜   󰇡 󰇛 󰇛󰇜 󰇛󰇜󰇜 󰇛󰇜  󰇛󰇜  󰇛󰇜  󰇛󰇜       󰇛󰇜  󰇛󰇜󰇢 󰇛    󰇜        󰇡   󰇛    󰇜     󰇛    󰇜  󰇛       󰇛    󰇜   󰇛    󰇜  󰇜󰇢  󰇡    󰇛    󰇜   󰇛  󰇜󰇢  ;     68   󰇛    󰇜  󰇛    󰇜     󰇡     󰇛    󰇜  󰇛  󰇜󰇢 󰇡 󰇛    󰇜           󰇛    󰇜   󰇛    󰇜󰇢 󰇡  󰇛            󰇛    󰇜     󰇛    󰇜   󰇛  󰇜       󰇛    󰇜   󰇛   󰇜     󰇛 󰇜󰇢 󰇡       󰇜󰇢  ;   󰇛    󰇜   󰇛    󰇜      󰇡    󰇛    󰇜  󰇛  󰇜  󰇛󰇜󰇢 󰇡 󰇛          󰇛    󰇜   󰇛    󰇜󰇢   󰇜          󰇛   󰇜   󰇛    󰇜   󰇛  󰇡        󰇛  󰇜  󰇛󰇜󰇢 󰇡 󰇛    󰇜    󰇜        󰇛    󰇜   󰇛    󰇜󰇢  ;          󰇛    󰇜   󰇛    󰇜      󰇡    󰇛    󰇜  󰇛  󰇜󰇢 󰇡 󰇛    󰇜           󰇛    󰇜   󰇛    󰇜󰇢 󰇡  󰇛             󰇛   󰇜   󰇛    󰇜 󰇜           󰇛 󰇜󰇢 󰇡 󰇛    󰇜   󰇛    󰇜       󰇛    󰇜󰇢  ;        󰇛    󰇜   󰇛    󰇜      󰇡        󰇛    󰇜󰇢 󰇛    󰇜   󰇛    󰇡        󰇛    󰇜󰇢   󰇜     ;     Ma trận chứa lực Coriolis lực li tâm  󰇛󰇜 󰇗  󰇛󰇜 󰇗 󰇗    69   ;  󰇛    󰇜 󰇛󰇜  󰇛      󰇛   󰇛󰇜 󰇜󰇛   󰇜󰇛    󰇜    󰇛    󰇜󰇛 󰇛󰇜󰇜 󰇛    󰇜 󰇛    󰇜󰇛 󰇛󰇜      󰇛󰇜󰇜 󰇛    󰇜 󰇛 󰇛󰇜 󰇛  󰇜        󰇜󰇛  󰇛󰇜 󰇛  󰇜    󰇛󰇜 󰇜  󰇜 ;   󰇛  󰇜    󰇛    󰇜 󰇛󰇛󰇛  󰇜 󰇛󰇜󰇜 󰇛    󰇜 󰇛󰇛  󰇜 󰇛  󰇜    󰇛󰇜󰇜 󰇛    󰇜 󰇛     󰇜     󰇛 󰇛  󰇜 󰇛 󰇛󰇜 󰇜 󰇛  󰇜 󰇛󰇜󰇜 󰇛    󰇜 󰇛 󰇛  󰇜      󰇜 󰇛 󰇛  󰇜 󰇛 󰇛󰇜   󰇛󰇜󰇜 󰇛    󰇛  󰇜  󰇜󰇜; Ma trận lực (lực trọng trường)  󰇛󰇜    󰇛󰇜;         󰇛  󰇜;        󰇛    󰇜;       70 PHỤ LỤC Code khối matlab Hàm điều khiển u (Khối Control Backstepping hình 3.6) function u = luatdieukhien(t) d1 = 0.412; d2 =0.413; d3= 0.255; a3=0.090; m1=9.173; m2=2.597; m3=0.41; m4=10; g=9.81; Fz=20*g; %Dau vao e = t(1:3); de = t(4:6); muy= t(7); gama= t(8); q= t(9:11); dq= t(12:14); % Ma tran khoi luong cua he m11=m1*((1/4)*d1^2*sin(q(1))^2+(1/4)*d1^2*cos(q(1))^2)+.705+m2*((-(1/2* (-cos(q(1))*sin(q(2))-sin(q(1))*cos(q(2))))*d2-sin(q(1))*d2*cos(q(2))-cos(q(1))* d2*sin(q(2))-d1*sin(q(1)))^2+(-(1/2*(cos(q(1))*cos(q(2))-sin(q(1))*sin(q(2))))* d2+cos(q(1))*d2*cos(q(2))-sin(q(1))*d2*sin(q(2))+d1*cos(q(1)))^2)+m3*(((1/2)* sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))d2* sin(q(1)+q(2))-d1*sin(q(1)))^2+(-(1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))* d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3))+d2*cos(q(1)+q(2))+d1*cos(q(1)))^2); m12 =m2*((-(1/2*(-cos(q(1))*sin(q(2))-sin(q(1))*cos(q(2))))*d2-sin(q(1))*d2* cos(q(2))-cos(q(1))*d2*sin(q(2))-d1*sin(q(1)))*(-(1/2*(-cos(q(1))*sin(q(2))-sin(q(1))* cos(q(2))))*d2-sin(q(1))*d2*cos(q(2))-cos(q(1))*d2*sin(q(2)))+(-(1/2*(cos(q(1))* cos(q(2))-sin(q(1))*sin(q(2))))*d2+cos(q(1))*d2*cos(q(2))-sin(q(1))*d2*sin(q(2))+d1* cos(q(1)))*(-(1/2*(cos(q(1))*cos(q(2))-sin(q(1))*sin(q(2))))*d2+cos(q(1))*d2* cos(q(2))-sin(q(1))*d2*sin(q(2))))+.195+m3*(((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)* cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))-d2*sin(q(1)+q(2))-d1* sin(q(1)))*((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)* sin(q(1)+q(2)+q(3))-d2*sin(q(1)+q(2)))+(-(1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)* sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3))+d2*cos(q(1)+q(2))+d1* cos(q(1)))*((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3 ))+d2*cos(q(1)+q(2)))); m13 =m3*(((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)* 71 sin(q(1)+q(2)+q(3))-d2*sin(q(1)+q(2))-d1*sin(q(1)))*((1/2)*sin(q(1)+q(2)+q(3))* a3+(1/2)*cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3)))+(-(1/2)* cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3))+d2* cos(q(1)+q(2))+d1*cos(q(1)))*((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3 ))))+0.5e-2; m21 =m2*((-(1/2*(-cos(q(1))*sin(q(2))-sin(q(1))*cos(q(2))))*d2-sin(q(1))*d2*cos(q(2))cos(q(1))* d2*sin(q(2))-d1*sin(q(1)))*(-(1/2*(-cos(q(1))*sin(q(2))-sin(q(1))*cos(q(2))))*d2-sin(q(1))* d2*cos(q(2))-cos(q(1))*d2*sin(q(2)))+(-(1/2*(cos(q(1))*cos(q(2))-sin(q(1))*sin(q(2))))* d2+cos(q(1))*d2*cos(q(2))-sin(q(1))*d2*sin(q(2))+d1*cos(q(1)))*(-(1/2*(cos(q(1))* cos(q(2))-sin(q(1))*sin(q(2))))*d2+cos(q(1))*d2*cos(q(2))-sin(q(1))*d2*sin(q(2))))+.195+m3* (((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))-d2* sin(q(1)+q(2))-d1*sin(q(1)))*((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))* d3-sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))-d2*sin(q(1)+q(2)))+(-(1/2)*cos(q(1)+q(2)+q(3))* a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3))+d2*cos(q(1)+q(2))+d1* cos(q(1)))*((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3 ))+d2*cos(q(1)+q(2)))); m22 =m2*((-(1/2*(-cos(q(1))*sin(q(2))-sin(q(1))*cos(q(2))))*d2-cos(q(1))*d2*sin(q(2))sin(q(1))* d2*cos(q(2)))^2+(-(1/2*(cos(q(1))*cos(q(2))-sin(q(1))*sin(q(2))))*d2-sin(q(1))*d2* sin(q(2))+cos(q(1))*d2*cos(q(2)))^2)+.195+m3*(((1/2)*sin(q(1)+q(2)+q(3))* a3+(1/2)*cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))d2*sin(q(1)+q(2)))^2+(-(1/2)* cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3))+d2* cos(q(1)+q(2)))^2); m23 =m3*(((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))-d2* sin(q(1)+q(2)))*((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3sqrt(a3^2+d3^2)* sin(q(1)+q(2)+q(3)))+((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)* cos(q(1)+q(2)+q(3))+d2*cos(q(1)+q(2)))*((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3 ))))+0.5e-2; 72 m31 =m3*(((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))-d2* sin(q(1)+q(2))-d1*sin(q(1)))*((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3sqrt(a3^2+d3^2)* sin(q(1)+q(2)+q(3)))+((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)* cos(q(1)+q(2)+q(3))+d2*cos(q(1)+q(2))+d1*cos(q(1)))*((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3 ))))+0.5e-2; m32 =m3*(((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))-d2* sin(q(1)+q(2)))*((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3sqrt(a3^2+d3^2)* sin(q(1)+q(2)+q(3)))+((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)* cos(q(1)+q(2)+q(3))+d2*cos(q(1)+q(2)))*((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3 ))))+0.5e-2; m33 =m3*(((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)* sin(q(1)+q(2)+q(3)))^2+((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3 )))^2)+0.5e-2; M = [m11, m12, m13; m21, m22, m23; m31, m32, m33]; % Ma tran Coriolis*dq c1 = 0; c2 = -(1/2)*d1*(-2*m3*(sin(q(1)+q(2)+q(3))*cos(q(1))-sin(q(1))*cos(q(1)+q(2)+q(3)))* (dq(1)+dq(2)+dq(3))*sqrt(a3^2+d3^2)-m3*(dq(1)+dq(2)+dq(3))*(a3*sin(q(1))-d3*cos(q(1)))* cos(q(1)+q(2)+q(3))+m3*(dq(1)+dq(2)+dq(3))*(a3*cos(q(1))+sin(q(1))*d3)*sin(q(1)+q(2)+q(3))(2* (dq(1)+dq(2)))*(cos(q(1))*sin(q(1)+q(2))*m3sin(q(1))*cos(q(1)+q(2))*m3+(1/2)*sin(q(2))*m2)*d2)*dq(1); c3 = (1/2*(dq(1)+dq(2)+dq(3)))*m3*(((2*d2*(dq(1)+dq(2))*sin(q(1)+q(2))+2*d1*sin(q(1))*dq(1))* cos(q(1)+q(2)+q(3))(2*(d2*(dq(1)+dq(2))*cos(q(1)+q(2))+d1*cos(q(1))*dq(1)))*sin(q(1)+q(2)+q(3)))* sqrt(a3^2+d3^2)+(d2*d3*(dq(1)+dq(2))*cos(q(1)+q(2))-a3*d2*(dq(1)+dq(2))*sin(q(1)+q(2))d1*dq(1)* (a3*sin(q(1))-d3*cos(q(1))))*cos(q(1)+q(2)+q(3))+(a3*d2*(dq(1)+dq(2))*cos(q(1)+q(2))+d2* d3*(dq(1)+dq(2))*sin(q(1)+q(2))+d1*dq(1)*(a3*cos(q(1))+sin(q(1))*d3))*sin(q(1)+q(2)+q(3))); C=[c1;c2;c3]; 73 %Ma tran luc co the g1 =-(1/2)*m1*g*d1*cos(q(1)); g2 =-(1/2)*m2*g*d2*cos(q(1)+q(2)); g3 =-(1/2)*m3*g*cos(q(1)+q(2)+q(3))*a3; G = [g1; g2; g3]; %Ma tran luc khong the Q1=0; Q2=0; Q3=0; Q=[Q1;Q2;Q3]; %Ma tran he so Mgama=[15*gama 0; 5*gama 0; 0 gama]; Mmuy= [15*muy 0; 5*muy 0; 0 muy]; %Luat dieu khien u=M*Mmuy*[de(1);de(2);de(3)]+Mgama*M*([de(1);de(2);de(3)]+Mmuy*[e(1);e(2);e(3)]); Phương trình vi phân chuyển động (Khối DK_Momen hình 3.6) function Tora = PTVPCD(t) d1 = 0.412; d2 =0.413; d3= 0.255; a3=0.090; m1=9.173; m2=2.597; m3=0.41; m4=10; g=9.81; Fz=20*g; %Dau vao q = t(1:3); dq = t(4:6); u = t(7:9); ddq= t(10:12); He= t(13); Hde= t(14); e = t(15:17); de = t(18:20); 74 % Ma tran khoi luong cua he m11=m1*((1/4)*d1^2*sin(q(1))^2+(1/4)*d1^2*cos(q(1))^2)+.705+m2*((-(1/2* (-cos(q(1))*sin(q(2))-sin(q(1))*cos(q(2))))*d2-sin(q(1))*d2*cos(q(2))-cos(q(1))* d2*sin(q(2))-d1*sin(q(1)))^2+(-(1/2*(cos(q(1))*cos(q(2))-sin(q(1))*sin(q(2))))* d2+cos(q(1))*d2*cos(q(2))-sin(q(1))*d2*sin(q(2))+d1*cos(q(1)))^2)+m3*(((1/2)* sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))d2* sin(q(1)+q(2))-d1*sin(q(1)))^2+(-(1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))* d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3))+d2*cos(q(1)+q(2))+d1*cos(q(1)))^2); m12 =m2*((-(1/2*(-cos(q(1))*sin(q(2))-sin(q(1))*cos(q(2))))*d2-sin(q(1))*d2* cos(q(2))-cos(q(1))*d2*sin(q(2))-d1*sin(q(1)))*(-(1/2*(-cos(q(1))*sin(q(2))-sin(q(1))* cos(q(2))))*d2-sin(q(1))*d2*cos(q(2))-cos(q(1))*d2*sin(q(2)))+(-(1/2*(cos(q(1))* cos(q(2))-sin(q(1))*sin(q(2))))*d2+cos(q(1))*d2*cos(q(2))-sin(q(1))*d2*sin(q(2))+d1* cos(q(1)))*(-(1/2*(cos(q(1))*cos(q(2))-sin(q(1))*sin(q(2))))*d2+cos(q(1))*d2* cos(q(2))-sin(q(1))*d2*sin(q(2))))+.195+m3*(((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)* cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))-d2*sin(q(1)+q(2))-d1* sin(q(1)))*((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)* sin(q(1)+q(2)+q(3))-d2*sin(q(1)+q(2)))+(-(1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)* sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3))+d2*cos(q(1)+q(2))+d1* cos(q(1)))*((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3 ))+d2*cos(q(1)+q(2)))); m13 =m3*(((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)* sin(q(1)+q(2)+q(3))-d2*sin(q(1)+q(2))-d1*sin(q(1)))*((1/2)*sin(q(1)+q(2)+q(3))* a3+(1/2)*cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3)))+(-(1/2)* cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3))+d2* cos(q(1)+q(2))+d1*cos(q(1)))*((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3 ))))+0.5e-2; m21 =m2*((-(1/2*(-cos(q(1))*sin(q(2))-sin(q(1))*cos(q(2))))*d2-sin(q(1))*d2*cos(q(2))cos(q(1))* d2*sin(q(2))-d1*sin(q(1)))*(-(1/2*(-cos(q(1))*sin(q(2))-sin(q(1))*cos(q(2))))*d2-sin(q(1))* d2*cos(q(2))-cos(q(1))*d2*sin(q(2)))+(-(1/2*(cos(q(1))*cos(q(2))-sin(q(1))*sin(q(2))))* d2+cos(q(1))*d2*cos(q(2))-sin(q(1))*d2*sin(q(2))+d1*cos(q(1)))*(-(1/2*(cos(q(1))* cos(q(2))-sin(q(1))*sin(q(2))))*d2+cos(q(1))*d2*cos(q(2))-sin(q(1))*d2*sin(q(2))))+.195+m3* (((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))-d2* sin(q(1)+q(2))-d1*sin(q(1)))*((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))* d3-sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))-d2*sin(q(1)+q(2)))+(-(1/2)*cos(q(1)+q(2)+q(3))* a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3))+d2*cos(q(1)+q(2))+d1* cos(q(1)))*((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3 ))+d2*cos(q(1)+q(2)))); 75 m22 =m2*((-(1/2*(-cos(q(1))*sin(q(2))-sin(q(1))*cos(q(2))))*d2-cos(q(1))*d2*sin(q(2))sin(q(1))* d2*cos(q(2)))^2+(-(1/2*(cos(q(1))*cos(q(2))-sin(q(1))*sin(q(2))))*d2-sin(q(1))*d2* sin(q(2))+cos(q(1))*d2*cos(q(2)))^2)+.195+m3*(((1/2)*sin(q(1)+q(2)+q(3))* a3+(1/2)*cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))-d2*sin(q(1)+q(2)))^2+((1/2)* cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3))+d2* cos(q(1)+q(2)))^2); m23 =m3*(((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))-d2* sin(q(1)+q(2)))*((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)* sin(q(1)+q(2)+q(3)))+((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)* cos(q(1)+q(2)+q(3))+d2*cos(q(1)+q(2)))*((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3 ))))+0.5e-2; m31 =m3*(((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))-d2* sin(q(1)+q(2))-d1*sin(q(1)))*((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3sqrt(a3^2+d3^2)* sin(q(1)+q(2)+q(3)))+((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)* cos(q(1)+q(2)+q(3))+d2*cos(q(1)+q(2))+d1*cos(q(1)))*((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3 ))))+0.5e-2; m32 =m3*(((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3sqrt(a3^2+d3^2)*sin(q(1)+q(2)+q(3))-d2* sin(q(1)+q(2)))*((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)* sin(q(1)+q(2)+q(3)))+((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)* cos(q(1)+q(2)+q(3))+d2*cos(q(1)+q(2)))*((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3 ))))+0.5e-2; m33 =m3*(((1/2)*sin(q(1)+q(2)+q(3))*a3+(1/2)*cos(q(1)+q(2)+q(3))*d3-sqrt(a3^2+d3^2)* sin(q(1)+q(2)+q(3)))^2+((1/2)*cos(q(1)+q(2)+q(3))*a3+(1/2)*sin(q(1)+q(2)+q(3))*d3+sqrt(a3^2+d3^2)*cos(q(1)+q(2)+q(3 )))^2)+0.5e-2; M = [m11, m12, m13; m21, m22, m23; m31, m32, m33]; % Ma tran Coriolis*dq 76 c1 = 0; c2 = -(1/2)*d1*(-2*m3*(sin(q(1)+q(2)+q(3))*cos(q(1))-sin(q(1))*cos(q(1)+q(2)+q(3)))* (dq(1)+dq(2)+dq(3))*sqrt(a3^2+d3^2)-m3*(dq(1)+dq(2)+dq(3))*(a3*sin(q(1))-d3*cos(q(1)))* cos(q(1)+q(2)+q(3))+m3*(dq(1)+dq(2)+dq(3))*(a3*cos(q(1))+sin(q(1))*d3)*sin(q(1)+q(2)+q(3))(2* (dq(1)+dq(2)))*(cos(q(1))*sin(q(1)+q(2))*m3sin(q(1))*cos(q(1)+q(2))*m3+(1/2)*sin(q(2))*m2)*d2)*dq(1); c3 = (1/2*(dq(1)+dq(2)+dq(3)))*m3*(((2*d2*(dq(1)+dq(2))*sin(q(1)+q(2))+2*d1*sin(q(1))*dq(1))* cos(q(1)+q(2)+q(3))(2*(d2*(dq(1)+dq(2))*cos(q(1)+q(2))+d1*cos(q(1))*dq(1)))*sin(q(1)+q(2)+q(3)))* sqrt(a3^2+d3^2)+(d2*d3*(dq(1)+dq(2))*cos(q(1)+q(2))-a3*d2*(dq(1)+dq(2))*sin(q(1)+q(2))d1*dq(1)* (a3*sin(q(1))-d3*cos(q(1))))*cos(q(1)+q(2)+q(3))+(a3*d2*(dq(1)+dq(2))*cos(q(1)+q(2))+d2* d3*(dq(1)+dq(2))*sin(q(1)+q(2))+d1*dq(1)*(a3*cos(q(1))+sin(q(1))*d3))*sin(q(1)+q(2)+q(3))); C=[c1;c2;c3]; %Ma tran luc co the g1 =-(1/2)*m1*g*d1*cos(q(1)); g2 =-(1/2)*m2*g*d2*cos(q(1)+q(2)); g3 =-(1/2)*m3*g*cos(q(1)+q(2)+q(3))*a3; G = [g1; g2; g3]; %Ma tran luc khong the Q1=0; Q2=0; Q3=0; Q=[Q1;Q2;Q3]; %Ma tran ma sat Fd = [1 0; 0; 0 1]; Fs = [1; 1; 1]; Td = [10; 10; 10] %Ma tran H H= C+G+Fd*[q(1);q(2);q(3)]+Fs+Td; %Ma tran he so if e(1)