Nghiên cứu, thiết kế và chế tạo chân giả chủ động

ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ ĐỒ ÁN TỐT NGHIỆP ĐỀ TÀI: NGHIÊN CỨU THIẾT KẾ VÀ CHẾ TẠO CHÂN GIẢ CHỦ ĐỘNG Người hướng dẫn: Sinh viên thực hiện: TS LÊ HOÀI NAM NGƠ HỒNG TRUNG NGƠ THIỆN ĐỨC Số thẻ sinh viên : 101120375 101120286 Lớp: 12CDT1 Đà Nẵng, 2017 TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ TĨM TẮT ĐỒ ÁN TỐT NGHIỆP Tên đề tài: Họ tên SV: Nghiên cứu, thiết kế chế tạo chân giả chủ động Ngơ Hồng Trung Mã SV: 101120375 Ngơ Thiện Đức 101120286 Lớp: 12CDT1 – 12CDT2 GV hướng dẫn: TS Lê Hoài Nam GV duyệt: PGS TS Trần Xuân Tùy Nội dung làm bao gồm vấn đề sau: Nhu cầu thực tế đề tài: Từ mong muốn chế tạo loại chân giả chủ động có giá thành thấp cho người khuyết tật Việt Nam, chân giả chủ động Việt Nam chưa phổ biến, giá thành loại chân giả chủ động Việt Nam giới cao, Hoa kỳ chân giả chủ động có giá lên tới 20.000 – 50.000 USD Ngồi ra, chân giả cịn sử dụng cho mục đích giảng dạy phương thức truyền xử lý liệu, xây dựng thử nghiệm điều khiển khác Phạm vi nghiên cứu đề tài tốt nghiệp: ✓ Nghiên cứu, thiết kế, chế tạo phần khí chân giả ✓ Xây dựng hệ thống cảm biến chuyển động chân người ✓ ✓ ✓ ✓ Xử lý liệu xây dựng thư viện quỹ đạo chân người Mơ hình hóa hàm truyền chân giả Xây dựng điều khiển thích nghi cho chân giả Lập trình vào vi điều khiển dsPIC33 arduino promini Nội dung đề tài thực hiện: ✓ Số trang thuyết minh: 75 trang ✓ Số vẽ: A0 ✓ Mơ hình: Kết đạt được: • Phần lý thuyết tìm hiểu ✓ Xử lý liệu excel MATLAB ✓ Thiết kế mơ hình 3D Solidwork ✓ Chế tạo mẫu thử phương pháp in 3D ✓ Thiết kế mạch Altium ✓ Mơ hình hóa hệ thống MATLAB ✓ Lý thuyết điều khiển thích nghi ✓ Lý thuyết cảm biến MPU6050, vi điều khiển Arduino, vi điều khiển dsPIC33 • Đã tính tốn, thiết kế phần ✓ Tính tốn, thiết kế phần khí, khoảng cách động học chân giả ✓ Tính tốn, xây dựng cơng thức quy đổi từ góc phân chân giả thành vị trí đai ốc vít me ✓ Tính tốn, thiết kế mơ hình hệ thống chân giả ✓ tính tốn, thiết kế mạch điều khiển chân giả ✓ Tính tốn, thiết kế hệ thống cảm biến chuyển động ✓ Tính tốn, xây dựng quỹ đạo chân người ✓ Tính tốn, thiết kế điều khiển thích nghi chân giả • Có mơ hình kèm CD Đĩa CD gồm file thuyết minh đồ án, tóm tắt, tờ nhiệm vụ , video mơ q trình lắp ráp chân giả,các video mơ hoạt động chân giả, video thử nghiệm chân giả người thật, hai báo tiếng Việt tiếng Anh nhóm chân giả Đà Nẵng, ngày tháng Sinh viên thực năm 2017 ĐẠI HỌC ĐÀ NẴNG CỘNG HỊA XÃ HƠI CHỦ NGHĨA VIỆT NAM TRƯỜNG ĐẠI HỌC BÁCH KHOA Độc lập - Tự - Hạnh phúc KHOA ………………………………………… NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP TT Họ tên sinh viên Số thẻ SV Lớp Ngành Ngơ Hồng Trung 101120375 12CDT2 Cơ điện tử Ngô Thiện Đức 101120286 12CDT1 Cơ điện tử Tên đề tài đồ án: Nghiên cứu, thiết kế chế tạo chân giả chủ động Đề tài thuộc diện: ☐ Có ký kết thỏa thuận sở hữu trí tuệ kết thực Các số liệu liệu ban đầu: - Dữ liệu quỹ đạo chân người thật Bảng liệu thô hệ thống chân giả Vật liệu: Nhựa, kim loại (thép, inox) Khối lượng thô chân giả: 3kg Nội dung phần thuyết minh tính tốn: a Phần chung: TT Họ tên sinh viên Ngơ Hồng Trung Ngơ Thiện Đức Ngơ Hồng Trung Ngơ Thiện Đức Ngơ Hồng Trung Ngơ Thiện Đức Ngơ Hồng Trung Ngơ Thiện Đức Nội dung Thu thập liệu quỹ đạo chân Xây dựng sơ đồ động học chân giả Nghiên cứu, thiết kế sơ đồ điều khiển Nghiên cứu, thiết kế thuật toán điều khiển b Phần riêng: TT Họ tên sinh viên Nội dung Ngơ Hồng Trung Nghiên cứu, thiết kế, chế tạo khí nửa chân giả Nghiên cứu, xử lý liệu quỹ đạo chân Nghiên cứu, thiết kế điều khiển thích nghi PI Ngơ Thiện Đức Nghiên cứu, thiết kế, chế tạo khí nửa chân giả Nghiên cứu, thiết kế, chế tạo mạch điều khiển Nghiên cứu, thiết kế việc xử lý liệu từ cảm biến Các vẽ, đồ thị (ghi rõ loại kích thước vẽ): a Phần chung: TT Họ tên sinh viên Ngơ Hồng Trung Ngơ Thiện Đức Ngơ Hồng Trung Ngơ Thiện Đức Nội dung Bản vẽ sơ đồ động điều khiển Poster chân giả thông minh b Phần riêng: TT Họ tên sinh viên Ngơ Hồng Trung Nội dung Bản vẽ lắp nửa chân Bản vẽ lưu đồ thuật tốn Ngơ Thiện Đức Bản vẽ lắp nửa chân Bản vẽ nguyên lý mạch điều khiển Họ tên người hướng dẫn: Phần/ Nội dung: Lê Hoài Nam Nghiên cứu, thiết kế, chế tạo chân giả Ngày giao nhiệm vụ đồ án: Ngày hoàn thành đồ án: …… /……./201… …… /……./201… Đà Nẵng, ngày tháng năm 201 Trưởng Bộ mơn……………………… Người hướng dẫn LỜI NĨI ĐẦU Ngày với phát triển không ngừng lĩnh vực, ngành Cơ khí nói chung ngành Cơ điện tử nói riêng ngành phát triển mạnh với nhiều thay đổi kỹ thuật, công nghệ chất lượng Để đạt điều địi hỏi người cán kỹ thuật ngồi trình độ chun mơn cịn cần phải có tư sáng tạo, sâu nghiên cứu để tận dung hết khả Qua năm học khoa Cơ khí – Trường Đại học Bách khoa – Đại học Đà Nẵng, giúp đỡ tận tình Thầy, Cơ giáo nỗ lực thân, chúng em tích lũy cho số kiến thức để tham gia vào đội ngũ kỹ sư Cơ điện tử tương lai Và thước đo kiến thức đồ án tốt nghiệp Đó thực thử thách lớn sinh viên chúng em chưa giải khối lượng công việc lớn Hoàn thành đồ án tốt nghiệp lần thử thách với cơng việc tính tốn phức tạp, gặp nhiều vướng mắc khó khăn Tuy nhiên hướng dẫn tận tình Thầy giáo hướng dẫn giúp chúng em hoàn thành đồ án Nhưng với kiến thức hạn hẹp mình, đồng thời chưa có kinh nghiệm tính tốn thi công thực tế, nên đồ án thể khơng tránh khỏi sai sót Chúng em kính mong tiếp tục bảo Thầy, Cơ để em hồn thiện kiến thức Xin cảm ơn gia đình, người thân bạn bè động viên giúp đỡ chúng em suốt trình thực đồ án Cuối cùng, chúng em xin chân thành cảm ơn Thầy, Cô giáo khoa Cơ khí – Trường Đại học Bách khoa – Đại học Đà Nẵng, đặc biệt Thầy, Cô trực tiếp hướng dẫn chúng em đề tài tốt nghiệp Đà Nẵng, ngày… tháng… năm 2017 Sinh viên CAM ĐOAN Chúng em xin cam đoan đồ án tuân thủ tốt quy định liêm học thuật: • Khơng bịa đặt, đưa thơng tin sai lệch so với nguồn trích dẫn; • Khơng ngụy tạo số liệu q trình khảo sát, thí nghiệm, thực hành, thực tập hoạt động học thuật khác; • Khơng sử dụng hình thức gian dối việc trình bày, thể hoạt động học thuật kết từ trình học thuật mình; • Không đạo văn, sử dụng từ ngữ, cách diễn đạt người khác thể mình, trình bày, chép, dịch đoạn, nêu ý tưởng người khác mà khơng có trích dẫn • Khơng tự đạo văn, sử dụng lại thông tin nghiên cứu mà khơng có trích dẫn phân mảnh thơng tin kết nghiên cứu để cơng bố nhiều ấn phẩm Sinh viên thực Sinh viên thực Ngơ Hồng Trung Ngơ Thiện Đức MỤC LỤC MỞ ĐẦU Chương 1: TỔNG QUAN VỀ CHÂN GIẢ 1.1 Định nghĩa phân loại chân giả 1.1.1 Định nghĩa 1.1.2 Phân loại 1.2 Tổng quan chân giả chủ động gối Chương 2: THIẾT KẾ CƠ KHÍ 2.1 Sơ đồ động học chân giả 2.2 Lý thuyết động thụ động 2.2.1 Khái niệm 2.2.2 Lý thuyết động thụ động 2.3 Khớp gối “Remotion knee” 2.3.1 Cấu tạo 2.3.2 Nguyên lý hoạt động khớp gối “Remotion knee” 2.4 Cấu tạo bàn chân giả 2.5 Các công thức động học chân giả Chương 3: THIẾT KẾ ĐIỆN TỬ 12 3.1 Sơ đồ khối mạch điều khiển 12 3.2 Giới thiệu vi điều khiển dsPIC33F Arduino 13 3.2.1 Giới thiệu vi điều khiển dsPIC33FJ32MC302 13 3.2.2 Điều kiện làm việc: 13 3.2.3 Khối xử lý trung tâm CPU: 13 3.2.4 Timer/Capture/Compare: 13 3.2.5 Điều khiển ngắt 13 3.2.6 Bộ chuyển đổi tương tự số ADC 13 3.2.7 Các cổng vào I/O ngoại vi: 14 3.2.8 Bộ xử lý tín hiệu số 14 3.2.9 Bộ điều khiển động cơ: 14 3.3 Giới thiệu Board Arduino Pro Mini: 14 3.4 Giới thiệu cảm biến đo góc nghiêng MPU6050 15 3.5 Thông số kỹ thuật 15 3.6 Giới thiệu cảm biến gia tốc Accelerometer 16 3.7 Giới thiệu quay hồi chuyển Gyroscope 20 3.8 Xử lý liệu đọc từ cảm biến MPU6050 22 3.9 Đọc xử lý tín hiệu cảm biến 23 Giới thiệu Altium PCB chân giả 29 3.10 3.10.1 Giới thiệu phần mềm Altium 29 3.10.2 Giới thiệu PCB chân giả 30 Chương 4: THIẾT KẾ BỘ ĐIỀU KHIỂN 33 4.1 Giới thiệu MATLAB thiết kế control system designer 33 4.1.1 Giới thiệu MATLAB 33 4.1.2 Bộ thiết kế “control system designer” 35 4.2 Phương pháp ước lượng thơng số bình phương bé mơ hình hệ thống 40 4.2.1 Ước lượng thơng số hệ thống phương pháp bình phương bé 40 4.2.2 Mơ hình hệ thống sử dụng cho chân giả 41 4.3 Bộ điều khiển thích nghi gián tiếp tự chỉnh PI 42 4.3.1 Các khái niệm 42 4.3.2 Bộ điều khiển thích nghi gián tiếp PI 43 Chương 5: PHƯƠNG PHÁP XÂY DỰNG QUỸ ĐẠO CHÂN GIẢ 47 5.1 Cảm biến theo dõi chuyển động khớp 47 NGHIÊN CỨU THIẾT KẾ CHẾ TẠO CHÂN GIẢ CHỦ ĐỘNG VN   =0  =       ( k )  y ( k ) −  T ( k )  =   k =1 N N N k =1 k =1     ( k ) y ( k ) =   ( k )  T ( k ) −1 N  N    =   ( k )  T ( k )   ( k ) y ( k )  k =1   k =1   4.2.2 Mơ hình hệ thống sử dụng cho chân giả Ở đây, chân giả truyền động nhờ động cơ, mô hình sử dụng cho chân giả mơ hình sử dụng cho động có tải Chúng tơi sử dụng mơ hình phổ biến sau: G( z) = Y ( z) b z + b2 = U ( z ) z + a1 z + a2 Chúng tơi sử dụng mơ hình gần hàm truyền hệ thống quy dạng phân thức gồm tử số mẫu số đa thức theo z Tuy nhiên, đáp ứng tương ứng với cực nằm xa trục ảo suy giảm nhanh Do xấp xỉ hệ bậc cao hệ bậc hai với cặp cực gần trục ảo Và, thực tế, hệ thống hợp thức chặt, nghĩa bậc tử nhỏ bậc mẫu Như vậy, hàm truyền mơ tả gần tồn hệ thống MIMO, động Với liệu có thời gian lấy mẫu T=0.001(s), ta dùng phương pháp bình phương bé để ước lượng thông số hệ thống trên, lập trình MATLAB: clear all data = load('data.txt'); %load data from "data.txt" file T = 0.001; % T of discrete system u = data(:,1); %load vector (collum 1) to u y = data(:,2); %load vector (collum 2) to y N = size(data,1); %size of data (according to collum - u) phi = nan(4,N); %initialing the first value for phi sum1 = zeros(4,4); %initialing the first value for sum1 sum2 = zeros(4,1); %initialing the first value for sum2 %calculating value of phi using LEAST SQUARES TRANSFER QUADRATIC function for k = 3:N SVTH: NGƠ HỒNG TRUNG, NGƠ THIỆN ĐỨC GVHD: TS LÊ HOÀI NAM 41 NGHIÊN CỨU THIẾT KẾ CHẾ TẠO CHÂN GIẢ CHỦ ĐỘNG phi(:,k) = [-y(k-1) -y(k-2) u(k-1) u(k-2)]'; sum1 = sum1 + phi(:,k)*transpose(phi(:,k)); sum2 = sum2 +phi(:,k)*y(k); end theta = inv(sum1)*sum2 G = tf([theta(3,1) theta(4,1)],[1 theta(1,1) theta(2,1)],T) Ta có mơ hình phần khớp gối: G( z) = Y ( z) −0.2621z + 0.2648 = , T=0.001(s) U ( z ) z − 0.1143z − 0.4274 Tương tự cho phần mắt cá chân 4.3 Bộ điều khiển thích nghi gián tiếp tự chỉnh PI 4.3.1 Các khái niệm Thích nghi q trình thay đổi thông số cấu trúc hay tác động điều khiển sở lượng thơng tin có q trình làm việc với mục đích đạt trạng thái định, thường tối ưu thiếu lượng thông tin ban đầu điều khiện làm việc thay đổi Điều khiển thích nghi khác với điều khiển có hồi tiếp chỗ điều khiển thích nghi có thêm vịng hồi tiếp, vịng hồi tiếp làm đầu vào cho q trình tính tốn để thay đổi thơng số điều khiển Riêng việc thay đổi này, ta thay đổi trực tiếp thông số điều khiển, thay đổi thông số hệ thống, dẫn đến cập nhập lại thông số điều khiển Phương pháp đầu gọi điều khiển thích nghi trực tiếp, phương pháp thứ hai gọi thích nghi gián tiếp Trong thực tế, phân điều khiển thích nghi thành ba nhóm lớn: điều khiển thích nghi theo mơ hình chuẩn, điều khiển thích nghi tự chỉnh điều khiển thích nghi hoạch định độ lợi Điều khiển thích nghi theo mơ hình chuẩn điều khiển theo mơ hình chuẩn cộng với vịng hồi tiếp thích nghi, tức ép hệ thống hoạt động cho quan hệ đầu vào đầu bám theo hệ thống chuẩn cho trước Loại điều khiển thường dùng cho hệ thống mà mơ hình hệ thống thông số hệ thống biết trước Điều khiển thích nghi tự chỉnh điều khiển có hai bước chính, thứ nhận dạng, tính tốn thơng số mơ hình, thứ hai tính tốn luật điều khiển Cả hai bước thực cách tự động Có hai loại chính, trực tiếp, hai gián tiếp SVTH: NGƠ HỒNG TRUNG, NGƠ THIỆN ĐỨC GVHD: TS LÊ HỒI NAM 42 NGHIÊN CỨU THIẾT KẾ CHẾ TẠO CHÂN GIẢ CHỦ ĐỘNG Loại thích nghi gián tiếp loại thích nghi phải cập nhập lại thông số hệ thống Loại phù hợp cho hệ thống biết trước mơ hình hệ thống Điều khiển thích nghi hoạch định độ lợi điều khiển biến chọn làm biến hoạch định độ lợi, từ thiết lập quy luật cập nhập thông số biến Loại điều khiển khơng có phương pháp chung, lớp đối tượng đặc thù cần có cách xem xét riêng Việc chọn biến hoạch định độ lợi khơng có phương pháp rõ ràng, chủ yếu dựa vào thực nghiệm Chân giả hệ thống phức tạp, có mơ hình hệ thống biết trước, loại mơ hình dùng cho động điện chiều Dựa đặc điểm trên, nhóm định chọn phương pháp sử dụng điều khiển thích nghi tự chỉnh gián tiếp 4.3.2 Bộ điều khiển thích nghi gián tiếp PI Đối tượng điều khiển hệ thống chân giả hai động dẫn động khớp gối khớp mắt cá chân thơng qua cấu vít me – đai ốc tương ứng Để mơ hình hóa động có tải, hàm truyền bậc hai có dạng sau sử dụng [9]: GV ( z ) = Y ( z) b z + b2 = U ( z ) z + a1 z + a2 (4.1) đó: Y(z), U(z) ảnh Laplace vận tốc di chuyển dọc vít me đai ốc giá trị xung điều khiển PWM driver động Như trình bày trên, trình di chuyển người dùng, ngoại lực tác động khác lên kết cấu khí nên vận tốc di chuyển đai ốc không cố định dù với giá trị PWM Nói cách khác, thông số a1, a2, b1, b2 mô hình hàm truyền hệ thống ln thay đổi Do đó, để điều khiển hệ thống xác cần ước lượng trực tuyến thông số dựa liệu thu thập Để đơn giản hóa việc ký hiệu khơng tính tổng qt, đặt Ù q(k ) = éëa1 (k ) a2 (k ) b1 (k ) b2 (k )ù û vector thông số hàm truyền động khớp gối (hoặc động khớp mắt cá chân) thời điểm k Ù T q(0) = éëa1 (0) a2 (0) b1 (0) b2 (0)ù û vector thông số hàm truyền tương ứng thời điểm ban đầu (người dùng đứng thẳng, chân giả song song với chân cịn lại) Từ (5) ta có: T y (k ) = - a1 y (k - 1)- a2 y (k - 2)+ b1.u (k - 1)+ b2 u (k - 2) SVTH: NGƠ HỒNG TRUNG, NGƠ THIỆN ĐỨC (4.2) GVHD: TS LÊ HOÀI NAM 43 NGHIÊN CỨU THIẾT KẾ CHẾ TẠO CHÂN GIẢ CHỦ ĐỘNG Trong đó: y(k), u(k) giá trị vận tốc di chuyển dọc trục vít me đai ốc giá trị PWM thời điểm k Vector thông số hàm truyền động khớp gối (hoặc động khớp mắt cá chân) cập nhập thơng qua thủ tục đệ quy dựa phương pháp bình phương sai số tối thiểu sau [7][8] (4.3) Ù ìï Ù ïï q (k ) = q (k - 1)+ L (k ).e (k ) ïï Ù ïï e (k ) = y (k )- j T (k ).q (k - 1) ïï ïï P (k - 1).j (k ) í L (k ) = ïï l + j T (k )P (k - 1).j (k ) ïï ïï T é ù ïï P (k ) = êP (k - 1)- P (k - 1)j (k )j (k )P (k - 1)ú ïï l êêë l + j T (k )P (k - 1)j (k ) ú ú û ỵï (4.4) (4.5) (4.6) đó: (4.7) ìï e (k ) = y (k )+ a (k - 1) y (k - 1)+ a (k - 1) y (k - 2) ïï ïï í - b1 (k - 1).u (k - 1)- b2 (k - 1).u (k - 2) ïï ïï j (k ) = é- y (k - 1) - y (k - 2) u (k - 1) u (k - 2)ùT ë û ïỵ (4.8) Từ thơng số mơ hình hàm truyền hệ thống ước lượng trực tuyến, hệ số điều khiển liên tục cập nhật để đảm bảo chất lượng điều khiển yêu cầu Thư viện quỹ đạo Bộ tạo điểm đặt Bộ điều chỉnh tự chỉnh Bộ so sánh Điều chỉnh thông số Bộ điều khiển PID Ước lượng trực tuyến Hệ thống chân giả Cảm biến Hình 4.10 Cấu trúc điều khiển PID tự chỉnh gián tiếp Từ (4.1) hàm truyền hệ hở thể quan hệ vị trí tương đối đai ốc (tức giá trị x1 x2 hình 3a, 3b) giá trị PWM mô tả sau: SVTH: NGƠ HỒNG TRUNG, NGƠ THIỆN ĐỨC GVHD: TS LÊ HOÀI NAM 44 NGHIÊN CỨU THIẾT KẾ CHẾ TẠO CHÂN GIẢ CHỦ ĐỘNG GP (z ) = b1 z + b2 T t x z + a1 z + a2 z - (4.9) với T thời gian lấy mẫu, tx chiều dài bước vít me Để điều khiển vị trí đai ốc, điều khiển PI thích hợp đơn giản [8] với hàm truyền sau sử dụng [7][8]: K I T z + z- GC (z ) = K P + (4.10) Phương trình đặc tính hệ kín có điều chỉnh là: 1+ GC (z).GP (z) = (4.11) Thay (4.9), (4.10) vào (4.11) ta nhận được: (2.K P + T K I ) z + éë2(a1 - 1)K P + T (a1 + 1)K I ù ûz + éë2(a2 - a1 )K P + T (a2 + a1 )K I - 2.T t x b1 ù ûz (4.12) + a2 T K I - 2.a2 K P - 2.T t x b2 = Hệ hở (4.9) có dạng hợp thức chặt bậc có điểm cực z = Theo [7], với hệ bậc cao có nhiều hai cực, đáp ứng tương ứng với cực nằm xa trục ảo suy giảm nhanh Do xấp xỉ hệ bậc cao hệ bậc hai với cặp cực hai cực nằm gần trục ảo Trong nghiên cứu này, chất lượng điều khiển thể qua tiêu sau [7]: ▪ Đáp ứng độ: theo tiêu chuẩn ITAE, hệ thống có mẫu số bậc nên lấy ξ = 0,707 ▪ Thời gian xác lập: theo tiêu chuẩn 5%, với thời gian xác lập mong muốn 1s: t= 3 £ Þ w ³ ; 4.243 n x x.wn Chọn ωn = (rad/s) Theo (4.12) phương trình đặc tính có dạng hàm bậc Hơn nữa, phương trình phải có cặp cực phức ứng với ξ = 0,707 ωn = nên phân tích thành: (z + a)(z + 2xw z + w ) = Û (z + a)(z + 2´ 0.707´ z + ) = Û z + (7.07 + a) z + 2(7.07a + 25) z + 25a = 2 n n (4.13) Bằng cách đồng hệ số phương trình đặc tính (4.12) (4.13) trên, ta nhận hệ phương trình: SVTH: NGƠ HỒNG TRUNG, NGƠ THIỆN ĐỨC GVHD: TS LÊ HỒI NAM 45 NGHIÊN CỨU THIẾT KẾ CHẾ TẠO CHÂN GIẢ CHỦ ĐỘNG ìï 2(a1 - 1).K P + T (a1 + 1).K I = (7.07 + a )(2.K P + T K I ) ïï ïï 2(a - a ).K + T (a + a ).K - 2.T t b P I x ïí ïï = (25 + 7.07a)(2.K P + T K I ) ïï ïï - 2.a2 K P + T a2 K I - 2.T t x b2 = 25.a.(2.K P + T K I ) ợ (4.14) (4.15) (4.16) Ly 7,07ì(18) - (19) 25×(18) - (20) ta có hệ phương trình: ìï (16.14´ a1 - 2a2 - 64.14).K P ïï ïï + T (6.07 ´ a - a - 17.93).K = - 2.T t b I x ïí ïï (50a1 + 2a2 - 403.5).K P ïï ïï + T (50a1 - a2 - 126.75).K I = - 2.T t xb2 ỵ (4.17) (4.18) Kí hiệu: é(16.14´ a1 - 2a2 - 64.14) T (6.07 ´ a1 - a2 - 17.93)ù ú A = êê T (50a1 - a2 - 126.75) ûúú êë(50a1 + 2a2 - 403.5) éK P ù é- 2.T t x b1 ù ê ú ê ú X = B= êK I ú ê- 2.T t x b2 ú ; ë û ë û Ta nhận hệ phương trình A.X = B (23) éK ù X = ê P ú= A- 1.B êK I ú ë û (24) Do đó: Như vậy, ứng với thông số hàm truyền hệ thống (khớp gối, khớp mắt cá chân) cập nhật liên tục, hệ số điều khiển PI tương ứng tính tốn liên tục theo phương trình (24) Chương đề cập đến phương pháp xây dựng quỹ đạo chân giả giải thích liên quan đến bắt buộc phải có thư viện quỹ đạo chân giả SVTH: NGƠ HỒNG TRUNG, NGƠ THIỆN ĐỨC GVHD: TS LÊ HOÀI NAM 46 NGHIÊN CỨU THIẾT KẾ CHẾ TẠO CHÂN GIẢ CHỦ ĐỘNG Chương 5: PHƯƠNG PHÁP XÂY DỰNG QUỸ ĐẠO CHÂN GIẢ Khác với cấu robot, việc tải trọng thay đổi phạm vị rộng, chân giả phải đạt trạng thái hình dáng hình học chân người thường, tức phận chân giả bao gồm đùi, bắp chân bàn chân phải đạt trạng thái tương ứng giống chân người di chuyển Vậy nên, việc tạo thư viện quỹ đạo chân bắt buộc để chân giả hoạt động mô theo chân người thật 5.1 Cảm biến theo dõi chuyển động khớp Trong thực tế, để theo dõi chuyển động xoay tương đối khớp (khớp gối, khớp mắt cá chân), có vài cách để thực hiện: dùng encoder để xác định góc quay tương đối; dùng điện trở xoay (hoặc tụ điện xoay), cảm biến lực để xác định thông số phụ, từ tính góc xoay tương đối …Tuy nhiên, trường hợp này, dùng cảm biến MPU6050 (cảm biến tích hợp quay hồi chuyển gia tốc kế) số lý quan trọng đề cập sau Thứ nhất, sử dụng cảm biến để lấy thơng số đơn góc quay tương đối vai trị cảm biến encoder, loại điện trở, tụ điện xoay cảm biến lực … so với cảm biến MPU6050 (có thể dùng cảm biến tính để thay cảm biến này) nhau, mối quan hệ góc xoay tương đối khớp gối, khớp mắt cá chân với bắp đùi (phần chân lại người khuyết tật) không đơn mối quan hệ đơn ánh, nghĩa ứng với góc bắp đùi có nhiều giá trị góc xoay tương đối khớp gối, khớp mắt cá chân Ngoài ra, việc sử dụng MPU6050 dễ dàng giúp ta lấy thông số trực tiếp phải thông qua thông số phụ Thứ hai, lắp vào chân người bình thường để lấy thơng số lắp vào chân giả để vận hành, MPU6050 tỏ vượt trội mặt dễ dàng tháo lắp, sai số lắp đặt dễ loại bỏ việc bù góc Thêm vào đó, cảm biến MPU6050 có độ xác cao so với trở xoay tụ xoay thơng thường, khơng cần xác dịnh điểm đầu tính góc xoay so với encoder, rẻ nhiều so với dùng cảm biến lực Hơn nữa, cảm biến khơng dùng phương pháp tiếp xúc nên độ xác không giảm theo thời gian, cần chỉnh lần lúc thiết lập chân giả Và cuối cùng, việc mua cảm biến dễ dàng, cho dù sản xuất đơn lẻ sản xuất hàng loạt thực SVTH: NGƠ HỒNG TRUNG, NGƠ THIỆN ĐỨC GVHD: TS LÊ HOÀI NAM 47 NGHIÊN CỨU THIẾT KẾ CHẾ TẠO CHÂN GIẢ CHỦ ĐỘNG 5.2 Phương pháp xây dựng quỹ đạo chân giả Sơ đồ trình lấy liệu để xây dựng quỹ đạo chân giả: Góc đùi Góc bắp chân Góc bàn chân MPU6050 MPU6050 MPU6050 I2C I2C I2C Arduino Pro mini Arduino Pro mini Arduino Pro mini SOFT UART Arduino pro mini SOFT UART SOFT UART COM to USB CPU (arduino IDE software) Mối quan hệ góc Thư viện quỹ đạo Hình 5.1 Phương pháp xây dựng quỹ đạo chân giả Trong suốt q trình, Arduino pro mini giao tiếp với MPU6050 có chứa lọc Kalman, giúp loại bỏ nhiễu không mong muốn Từ liệu thơ có được, ta thực việc ta thực việc dùng excel phương pháp bình phương bé để có mối quan hệ góc bắp đùi, góc bắp chân góc bàn chân (góc quy đổi): SVTH: NGƠ HỒNG TRUNG, NGƠ THIỆN ĐỨC GVHD: TS LÊ HOÀI NAM 48 NGHIÊN CỨU THIẾT KẾ CHẾ TẠO CHÂN GIẢ CHỦ ĐỘNG Hình 5.2 Đường cong quỹ đạo chân giả Từ đây, ta chia chuyển động chân thành pha, ứng với điều kiện sau: Bảng 5.1: Pha chuyển động góc đùi tăng góc đùi giảm bắp chân góc bpc giảm góc bpc tăng góc đùi =< -98 góc đùi >=-97 bàn chân góc bpc giảm góc bpc tăng góc đùi =< -98 góc đùi >=-97 pha th1 th2 th3 th4 Ghi chú: góc bpc: góc bắp chân Từ ta xây dựng thư viện quỹ đạo chân, lập trình dsPIC: //*********************************** //goc duoc tinh theo cac cong thuc sau: //theta_thigh = raw_theta_thigh + (-90 - theta_thigh_complement); //theta_calf = raw_theta_calf + (-90 - theta_calf_complement); //theta_foot = raw_theta_foot + (-90 - theta_foot_complement); //complement la phan goc hieu chinh (do truc tiep dung thang nguoi) //tu goc tren quy doi goc raw_theta (goc hoi tiep ve tu MPU6050) hoac nguoc lai //********************************************************************* *********** SVTH: NGƠ HỒNG TRUNG, NGƠ THIỆN ĐỨC GVHD: TS LÊ HOÀI NAM 49 NGHIÊN CỨU THIẾT KẾ CHẾ TẠO CHÂN GIẢ CHỦ ĐỘNG //********************insert those function in your interrupt timer or your main program //identify_topography() //calculate_parameters() //********************************************************************* ************ float dt = 0.001; float omega_thigh=0.0; // van toc goc bap dui float omega_calf=0.0; // van toc goc bap chan float omega_foot=0.0; // van toc goc ban chan float theta_thigh_tempt = 0.0,theta_calf_tempt = ; float theta_thigh_complement = -80.0; float theta_calf_complement = -80.0; float theta_foot_complement = -8.35; float theta2_request = 0.0, theta1_request = 0.0; float theta_thigh=0.0,raw_theta_thigh=0;//dui float theta_calf=0.0,raw_theta_calf=0;//bap float theta_foot=0.0,raw_theta_foot=0;//ban chan float error_calf=0.0,error_foot=0.0; float theta_calf_request = 0.0, theta_foot_request = 0; float x_calf_request=0.0,x_foot_request=0.0; float x_calf = 0.0, x_foot = 0.0; float theta2 = 0, theta1 = 0; //***********cach xac dinh dia hinh **************** // thay doi dia hinh, nguoi su dung can dung yen thoi gian (5 -10 s) // neu goc ban chan thay doi < thi nhay vao ham xac dinh dia hinh /*int identify_topography() { if (abs(raw_theta_foot - theta_foot_complement) >= 10) return 1; // dia hinh doc if (abs(raw_theta_foot - theta_foot_complement) < 10 ) { SVTH: NGƠ HỒNG TRUNG, NGƠ THIỆN ĐỨC GVHD: TS LÊ HOÀI NAM 50 NGHIÊN CỨU THIẾT KẾ CHẾ TẠO CHÂN GIẢ CHỦ ĐỘNG if((raw_theta_thigh + (-90 - theta_thigh_complement)) > -79) return 2; //dia hinh cau thang else return 3; //dia hinh phang } } */ void calculate_parameters(){ omega_thigh = (theta_thigh - theta_thigh_tempt)/dt; omega_calf = (theta_calf - theta_calf_tempt)/dt; theta_thigh_tempt = theta_thigh; theta_calf_tempt = theta_calf; theta_thigh = (raw_theta_thigh - theta_thigh_complement) - 90; theta_calf = (raw_theta_calf - theta_calf_complement) - 90; theta_foot = (raw_theta_foot - theta_foot_complement) - 90; theta2 = 180 + theta_thigh - theta_calf; //(degree) theta1 = 360 + 90 + theta_calf + theta_foot; //(degree) x_calf = 1.0425*theta2 - 123.78; // x_calf (mm) x_foot = 0.592*theta1 - 142.5; //(mm) } //***********************calculate x from angel *************** //x_calf_request = 1.0425*theta2 - 123.78; % x_calf_request (mm) //x_foot_request = 0.5753*theta1 - 29.352; %x_foot_request (mm) //**********************flat_topography********************************* void position_calf_request() { theta_calf = (raw_theta_calf - theta_calf_complement) - 90; float x = theta_thigh; //x la goc bap dui float y = 0.0; if(omega_thigh > 0) { if(omega_calf < 0) y = -0.3833*x*x - 82.667*x - 4509.8; else y = 0.0222*x*x + 5.1221*x + 193.2; } SVTH: NGƠ HỒNG TRUNG, NGƠ THIỆN ĐỨC GVHD: TS LÊ HOÀI NAM 51 NGHIÊN CỨU THIẾT KẾ CHẾ TẠO CHÂN GIẢ CHỦ ĐỘNG else { if(x 0) { if(omega_calf < 0) y = 0.1333*x*x + 32.167*x + 1779.8; else y = -0.0023*x*x*x - 0.7238*x*x - 74.565*x - 2639.9; } else { if(x