Thiết kế bộ nhận dạng thông số cho cánh tay robot 2 bậc tự do, bộ điều khiển

1.1 Tổng quan về đề tài Với sự phát triển của nền công nghiệp hiện nay, việc chế tạo và sử dụng robot ngày càng phổ biến và phát triển vượt bậc. Robot công nghiệp ngày càng được sử dụng rộng rãi không chỉ trong các hệ thống dây chuyền sản xuất tự động mà còn được sử dụng trong các lĩnh vực khác như: y tế, quân sự,… Do đó, việc tối ưu cũng như phát triển nâng cao các tính năng của robot đang là một yêu cầu cấp thiết hiện nay. Để tối ưu và nâng cao tính năng của robot, ta cần sử dụng các phương pháp điều khiển robot. Việc thiết kế các bộ điều khiển robot, ta thường dựa trên các mô hình robot và nó phụ thuộc vào các thông số động lực học của robot. Tuy nhiện đối với các mô hình và hệ thống điều khiển robot phức tạp liên quan đến tính phi tuyến, sô trục và số bậc của robot, môi trường làm việc, các đặc tính của tải… và các thông số của mô hình khó có thể xác định bằng cách đo đạc trực tiếp như các thông số liện quan đến quán tính và ma sát trong phương trình động lực học của robot. Vì vậy, việc xác định các thông số động lực học là yêu cầu cần thiết để xây dụng các thuật toán điều khiển hiệu quả hơn. Thuật toán nhận dang thông số của cánh tay robot gồm các vấn đề cơ bản là thu thập dữ liệu, chọn cấu trúc mô hình, ước lượng thông số, đánh giá chất lượng mô hình. Các bước này được lặp đi lặp lại nhiều lần cho đến khi mô hình đạt yêu cầu. Phương pháp nhận dạng thông số đơn giản được sử dụng để xác định thông số là phương pháp tổng bình phương tối thiểu. 1.2 Mục tiêu Mục tiêu chính của đề tài: Tìm ra được các thông số của phương trình động lực học bằng phương pháp bình phương cực tiểu Thiết kế được bộ nhận dạng thông số theo thời gian thực Kết hợp bộ điều khiển PID với nhận dạng thông số 1.3 Nội dung Nội dung của báo cáo gồm 5 phần: Tổng quan Cơ sở lý thuyết Mô phỏng tính toán nhận dạng thông số cho robot 3 bậc tự do Nhận dạng thông số trên mô hình thực tế Kết luận

Bộ môn Đồ án Robot GVHD: Trần Đức Thiện 1572Equation Chapter Section 57 BỘ GIÁO DỤC & ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO ĐỒ ÁN ROBOT -⸙∆⸙ - BÁO CÁO MÔN HỌC ĐỀ TÀI: THIẾT KẾ, NHẬN DẠNG THÔNG SỐ VÀ THIẾT KẾ BỘ ĐIỀU KHIỂN PID CHO ROBOT DOF GVHD: Trần Đức Thiện SVTH: Đinh Thanh Tùng 18151143 Nguyễn Nhật Trường 1815113 Tp Hồ Chí Minh tháng năm LỜI CẢM ƠN 1 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện Để hoàn thành báo cáo mơn học, nhóm chúng em ln ln diện giúp đỡ nhiệt tình thầy Nhờ giúp đỡ từ thầy mà nhóm chúng em cải thiện nhiều kĩ xử lí giải vấn đề học vấn đề cần có ngồi mơn học Chúng em xin chân thành cảm ơn ghi nhận sâu sắc đóng góp thầy Trần Đức Thiện 2 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện Mục lục 3 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện Mục lục bảng 4 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện Chương TỔNG QUAN 1.1 Tổng quan đề tài Với phát triển công nghiệp nay, việc chế tạo sử dụng robot ngày phổ biến phát triển vượt bậc Robot công nghiệp ngày sử dụng rộng rãi không hệ thống dây chuyền sản xuất tự động mà sử dụng lĩnh vực khác như: y tế, quân sự,… Do đó, việc tối ưu phát triển nâng cao tính robot yêu cầu cấp thiết Để tối ưu nâng cao tính robot, ta cần sử dụng phương pháp điều khiển robot Việc thiết kế điều khiển robot, ta thường dựa mơ hình robot phụ thuộc vào thông số động lực học robot Tuy nhiện mơ hình hệ thống điều khiển robot phức tạp liên quan đến tính phi tuyến, sô trục số bậc robot, môi trường làm việc, đặc tính tải… thơng số mơ hình khó xác định cách đo đạc trực tiếp thông số liện quan đến qn tính ma sát phương trình động lực học robot Vì vậy, việc xác định thông số động lực học yêu cầu cần thiết để xây dụng thuật toán điều khiển hiệu Thuật tốn nhận dang thơng số cánh tay robot gồm vấn đề thu thập liệu, chọn cấu trúc mơ hình, ước lượng thơng số, đánh giá chất lượng mơ hình Các bước lặp lặp lại nhiều lần mơ hình đạt u cầu Phương pháp nhận dạng thông số đơn giản sử dụng để xác định thơng số phương pháp tổng bình phương tối thiểu 1.2 Mục tiêu Mục tiêu đề tài: - Tìm thơng số phương trình động lực học phương pháp bình phương cực tiểu - Thiết kế nhận dạng thông số theo thời gian thực - Kết hợp điều khiển PID với nhận dạng thông số 1.3 Nội dung Nội dung báo cáo gồm phần: - Tổng quan Cơ sở lý thuyết Mơ tính tốn nhận dạng thơng số cho robot bậc tự Nhận dạng thông số mơ hình thực tế Kết luận 5 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện Chương CƠ SỞ LÝ THUYẾT 2.1 Lý thuyết robot 2.1.1 Động học robot 2.1.1.1 Đặt trục cho robot Hình 2.1 Hệ trục tọa độ robot bậc Để tìm bảng DH cho robot ta cần dựa theo quy tắc bàn tay phải cách đặt hệ trục tọa độ cho khớp sau: Khung thứ i gắn với liên kết thứ i Trục zi chọn dọc theo trục khớp thứ i Trục xi chọn theo đường vng góc chung zi zi+1 yi chọn để làm thành tọa độ bên phải Khung thứ thường chọn khớp với khung thứ biến khớp đầu Khung thứ N chọn tự do, thường chọn tự do, đảm bảo nhiều số liên kết tốt Dựa vào quy tắc ta đặt hệ trục tọa độ hình 2.1 với thơng số độ dài liên kết cho sau: Bảng 2.1 Thông số robot 2.1.1.2 L1 (cm) L2 (cm) 18 14 Bảng DH Các thông số bảng D-H xác định theo nguyên tắc: 6 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện khoảng cách từ trục Zi đến Zi+1 đo dọc theo trục Xi αi góc quay từ trục Xi-1 đến Xi đo dọc theo trục Zi di khoảng cách từ trục Zi đến Zi+1 đo dọc theo trục Zi θi góc quay từ trục Xi-1 đến Xi đo dọc theo trục Zi Từ hình nguyên tắc hình 1.1 ta lập bảng D-H cho robot Bảng 2.2 Bảng DH theo lý thuyết i ai-1 αi-1 di θi Z0Z1X0 0 θ1 X0X1Z0 Z1Z2X1 L1 0 θ2 X1X2Z1 2.1.1.3 Động học thuận Công thức tổng quát: − sθi  cθ i  sθ cα i −1  i i −1 cθi cα i −1 − sα i −1 iT =  sθi sα i −1 cθi sα i −1 cα i −1  0  −1  − sα i −1di  cα i −1di    (2 2) i −1 i T : ma trận chuyển vị nhất, biểu diễn hướng vị trí khâu i so với khâu i-1 cθi : cos(θi ) ( Với θi góc quay quanh trục Z khâu i so với khâu i −1 ) sθi : sin(θi ) ( Với θi góc quay quanh trục Z khâu i so với khâu i −1 ) di : khoảng cách trục X khâu sα i −1 i −1 cα i −1 i −1 α i −1 : s in(α i −1 ) ( Với α i −1 i so với khâu i −1 góc quay quanh trục X khâu i so với khâu ) : cos(α i −1 ) ( Với α i −1 góc quay quanh trục X khâu i so với khâu ) : khoảng cách trục Z khâu i so với khâu i −1 7 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện T : ma trận chuyển vị nhất, biểu diễn hướng vị trí khâu so với khâu  c1 s  T = 0  0 − s1 0  c1 0  0  0 1 (2 2) T : ma trận chuyển vị nhất, biểu diễn hướng vị trí khâu so với khâu c2 s  2T = 0  0 − s2 c2 0 L1  0  0  1 (2 2) Từ (1.2), (1.3) ta có ma trận chuyển vị trục tọa độ gốc trục tọa độ thứ  c12 s 0  12 T = T T = 2 0  0 − s12 c12 0 L1c1  L1s1     (2 4) Điểm đầu cuối robot DOF EE  L1c1 + L2 c12   EE3 P    P= T  =  L1s1 + L2 s12       (2 5) 8 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện 2.1.1.4 Động học nghịch Áp dụng phương pháp đại số để tính động học nghịch cho robot Từ phương trình (1.5) ta có tọa độ điểm đầu cuối:  X = L1c1 + L2c12   Y = L1s1 + L2 s12  Z =0  (2 6) Từ tọa độ x y điểm đầu cuối phương trình số (1.6) ta có L2c12 = X − L1c1 (2 7) L2 s12 = Y − L1s1 (2 8) Tổng bình phương phương trình (1.7) (1.8) ta có: L1c1 + L1s1 = X − L1c12 + Y − L2 s12 (2.9) 9 Bộ mơn Đồ án Robot Basic_01: Trong đó: GVHD: Trần Đức Thiện ac1 + bs1 = d cos α = Đặt: sin α = a a + b2 b a2 + b2 d cα c1 + sα s1 = a + b2 Suy ra: (2 10) cos(θ1 − α ) = d a + b2 (2 11) sin(θ1 − α ) = ± − d a + b2 (1 12) Suy ra:  d d θ1 = α + arctan  ± − 2 ; a +b a + b2   ÷ ÷  (2 13) Từ phương trình số (2.17) (2.18), ta lấy phương trình (2.18) chia cho phương trình (2.17) θ = arctan(Y − L1s1 , X − L1c1 ) − θ1 (2 14) Sau giải phương trình động học nghịch, ta tìm nghiệm : 10 10 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện Hình 3.15 Sai số momen quỹ đạo - Quỹ đạo 4: P=200, λ=0.98 Hình 3.16 Momen tham chiếu momen nhận dạng quỹ đạo Hình 3.17 Sai số momen quỹ đạo 35 35 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện Từ hình ta nhận thấy moment tham chiếu hai khớp thể hai đường màu đen màu đỏ momen nhận dạng hai khớp thể đường màu vàng đứt nét đường màu xanh đứt nét khớp với ước lượng phương pháp bình phương cực tiểu hồi quy Mức ngưỡng sai số moment tham chiếu momen nhận dạng 10-5 – 10-10 Bảng 3.9 So sánh kết Các thông số Giá trị tham Sai số chiếu IZZ1 0.001649491 0.245% IZZ2 0.000166021 0.052% x1 0.07999 0.113% y1 -0.00051 0.78% x2 0.0278 0.075% y2 -0.00084 0.38% Từ bảng trên, ta thấy sai số thông số thực không chệnh lệch nhiều so với thong số ước lượng phương pháp nhận dạng tham số bình phương cực tiểu hồi quy, múc ngưỡng sai số bé 1% Ngoài phương pháp nhận dạng thơng số quỹ đạo robot ảnh hưởng đến độ xác thơng số ước lượng Đối với quỹ đạo khác cần điều chỉnh ma trận P hệ số quên λ 3.4 Nhận dạng thơng số kết hợp điều khiển PID Hình 3.18 Sơ đồ khối thuật tốn nhận dạng thơng số PID Như sơ đồ trên, kết hợp điều khiển PID với nhận dạng thông số nhằm tăng hiệu nhận dạng thông số cho hệ thống thực tế Ngồi phat triển điều khiển PID tự điều chỉnh tăng hiệu cho hệ thống 36 36 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện Ta tiến hành mô hệ thống sơ đồ sau: Hình 3.19 Sơ đồ khối mơ Simulink Qũy đạo sử dụng mô lấy từ phần 3.3.2 mô 37 37 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện Bảng 3.10 Thông số nhận dạng A1 A2 0.00615117 0.0002314 0.00615117 0.0002314 0.00615112 0.00023143 0.00615112 0.00023143 A3 A4 0.37246846 0.02350768 0.37246846 0.02350768 0.37246846 0.02350768 0.37246846 0.02350768 A5 -0.00140434 -0.00140434 -0.00140433 -0.00140433 A6 A7 -0.00071030 0.00042314 -0.00071030 0.00042314 -0.00071030 0.00042314 -0.00071030 0.00042314 A8 A9 -0.00001273 -0.00001273 -0.00001279 -0.00001273 A10 0.01 0.01 0.01 0.01 A11 A12 0.01 0.01 0.01 0.01 Từ bảng ta thấy thông số nhận dạng lệch so với khơng có điều khiển PID Bảng cho thấy độ chênh lệch giá trị thông số thực với giá trị thông số ước lượng Bảng 3.11 Sai số chệnh lệch thông số thực thông số ước lượng Các thông số IZZ1 IZZ2 x1 x2 y1 y2 Giá trị tham chiếu 0.001649491 0.000166021 0.07999 0.0278 -0.00051 -0.00084 Gía trị nhận dạng 0.001525782 0.000163323 0.082750531 0.028367346 -0.00052041 -0.00085714 Sai số 7.5% 1.63% 3.5% 2% 2% 2% 38 38 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện - Quỹ đạo 1: p=200, λ= 0.98 Hình 3.20 Momen khớp quỹ đạo Hình 3.21 Giá trị setpoint giá trị ngõ quỹ đạo - Quỹ đạo 2:p=180, λ=0.98 Hình 3.22 Momen khớp quỹ đạo Hình 3.23 Giá trị setpoint giá trị ngõ quỹ đạo 39 39 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện - Quỹ đạo 3: p=300, λ=0.995 Hình 3.24 Momen khớp quỹ đạo Hình 3.25 Giá trị setpoint giá trị ngõ quỹ đạo - Quỹ đạo 4: p =200, λ = 0.995 Hình 3.26 Momen khớp quỹ đạo 40 40 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện Hình 3.27 Giá trị setpoint giá trị ngõ quỹ đạo Nhận xét: Sau mô ta nhận thấy nhận dạng thơng số có điều khiển PID chệnh lệch không đáng kể so với nhận dạng thơng số khơng só điều khiển PID Ngồi thơng số nhận dạng cịn ảnh hưởng bời quỹ đạo robot, có số quỹ đạo làm nhận dạng không hội tụ cần phải điều chỉnh lại ma trận P hệ số quên λ 41 41 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện Chương NHẬN DẠNG THƠNG SỐ TRÊN MƠ HÌNH THỰC TẾ 4.1 Sơ đồ thuật tốn nhân dạng thơng số Hình 4.28 Sơ đồ khối nhận dạng thông số 42 42 Bộ mơn Đồ án Robot GVHD: Trần Đức Thiện Hình 4.29 Thuật tốn nhận dạng thơng số Sơ đồ khối diễn tả thuật tốn nhận dạng thơng số Vi xử lý đọc tín hiệu cảm biến khoảng thời gian lấy mẫu định, sau tính tốn nhận dạng thơng số Kết thơng số lưu lại để tính cho thời gian lấy mẫu Lúc thông số cập nhật liện tục Ngồi thuật tốn ta cần quan tâm đến quỹ đạo làm việc robot Cần lựa chọn quỹ đạo thích hợp để giảm sai lệch thơng số cần nhận dạng Có quỹ đạo sử dụng phương trình Fourier, quỹ đạo bậc , … áp dụng để tìm quỹ đạo thích hợp Ta sử dụng nhận dạng thơng số để tìm thơng số động nhằm tăng hiệu cho robot 43 43 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện 4.2 Mơ hình robot thực tế 4.2.1 Bản vẽ Soliworks Hình 4.30 Mơ hình robot Solidworks Từ mơ hình robot Solidworks, nhóm em lựa chọn thiết bị phù hợp lập bảng dự trù kinh phí STT 10 11 12 Tên thiết bị Board Arduino UNO R3 Mạch cầu L298N Động GA25 có encorder Dây cắm board Cắt mica Vòng bi 608z Gá đỡ động Pu-li GT3 Pu-li GT2 8mm Trục inox Kẹp trục ngang SHF8 Kinh phí dự trù Tổng chi phí Gía 150.000 30.000 2x120.000 30.000 200.000 4x30.000 2x18.000 2x30.000 3x14.000 100.000 3x170.000 200.000 1.718.000 44 44 Bộ môn Đồ án Robot Chương GVHD: Trần Đức Thiện KẾT LUẬN 5.1 Kết đạt Sau thực đề tài nhóm em học hỏi thêm cách xây dựng mô hình động lực học robot Đồng thời hiểu áp dụng phương pháp bình phương cực tiểu để xác định tối ưu hóa thơng số phương trình động lực học robot Kết hợp thêm điều khiển PID để tăng độ xác cho robot Đã tìm thơng số cần nhận dạng mơ hình robot phương pháp bình phương cực tiểu 5.2 Hạn chế Sau hồn thành đề tài nhóm em nhận đề tài nhiều hạn chế sau: - Khơng có mơ hình thực tế để kiểm chứng lại kết tìm kiếm phương pháp bình phương tối thiểu - Chưa đưa cách xây dựng quỹ đạo tối ưu để sử dụng trình nhận dạng - Chưa sử dụng nhiều phương pháp khác để so sánh đo đạt 5.3 Hướng phát triển - Tiến hành xây dựng mơ hình robot thực tế để kiểm chứng tính khả thi phương pháp nhận dạng - Áp dụng thêm phương pháp khác để tìm kiếm so sánh thông số nhận dạng, từ chọn phương pháp hiệu - Xây dựng quỹ đạo tối ưu cho việc nhận dạng thông số - Sử dụng nhận dạng thông số để thiết kế điều khiển PID tự chỉnh 45 45 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện TÀI LIỆU THAM KHẢO [1] John J Craig, Introduction to Robotics - Third Edition [2] Sci-Hub | Dynamic Parameter Identification for a Manipulator with Joint Torque Sensors Based on an Improved Experimental Design | 10.3390/s19102248 (hkvisa.net) [3] Sci-Hub | Design, simulation, comparison and evaluation of parameter identification methods for an industrial robot | 10.1016/j.compeleceng.2016.09.004 (hkvisa.net) [4] Sci-Hub | Application of the Weighted Least Squares Parameter Estimation Method to the Robot Calibration | 10.1115/1.2919465 (hkvisa.net) [5]Sci-Hub | [IEEE 1985 24th IEEE Conference on Decision and Control Fort Lauderdale, FL, USA (1985.12.11-1985.12.13)] 1985 24th IEEE Conference on Decision and Control - Parameter identification of robot dynamics | 10.1109/CDC.1985.268838 (hkvisa.net) - 46 46 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện PHỤ LỤC • Code clc close all l0=180/1000; l1=140/1000; m1=275.36/1000; m2=84.56/1000; Izz1=0.00341148698; Izz2=0.00023144292; n = length(tor_mh.signals(1).values); M = zeros(2,12); T = zeros(2,1); g = 9.8; for i = 1:n tor1 = tor_mh.signals(1).values(i, 1); tor2 = tor_mh.signals(2).values(i, 1); the1 = datathe.signals.values(i, 1); the2 = datathe.signals.values(i, 2); dthe1 = datadthe.signals.values(i, 1); dthe2 = datadthe.signals.values(i, 2); ddthe1 = dataddthe.signals.values(i, 1); ddthe2 = dataddthe.signals.values(i, 2); H = [sign(dthe1), dthe1, 0, 0, ddthe1, ddthe1+ddthe2, cos(the1), cos(the1+the2), -sin(the1), -sin(the1+the2), (dthe2^2+2*dthe1*dthe2)*sin(the2)+(2*ddthe1+ddthe2)*cos(the2), (dthe2^2+2*dthe1*dthe2)*cos(the2)-(2*ddthe1+ddthe2)*sin(the2); , , sign(dthe2), dthe2, 0, ddthe1+ddthe2, , cos(the1+the2), -sin(the1+the2), dthe1^2*sin(the2)+ddthe1*cos(the2) , dthe1^2*cos(the2)-ddthe1*sin(the2) ]; M = [M; H]; TOR = [tor1; tor2]; T = [T; TOR]; end F = M'*M; matrixH = M; matrixT = T; p = pinv(F)*(matrixH'*matrixT); x1 = (p(7) - g*l0*m2)/(g*m1); x2 = p(8)/(g*m2); y1 = p(9)/(g*m1); y2 = p(10)/(g*m2); Izz1 = p(5) - (m1*x1^2+m1*y1^2+m2*l0^2); Izz2 = p(6) - (m2*x2^2+m2*y2^2); • Code 2: function [ipe, Pe, error, tor] = RLS(the, dthe, ddthe, in, ips, Ps) 47 47 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện lamda = 0.989; the1 = the(1); the2 = the(2); dthe1 = dthe(1); dthe2 = dthe(2); ddthe1 = ddthe(1); ddthe2 = ddthe(2); omega = [sign(dthe1), dthe1, 0, 0, ddthe1, ddthe1+ddthe2, cos(the1), cos(the1+the2), -sin(the1), -sin(the1+the2), (dthe2^2+2*dthe1*dthe2)*sin(the2)+(2*ddthe1+ddthe2)*cos(the2), (dthe2^2+2*dthe1*dthe2)*cos(the2)-(2*ddthe1+ddthe2)*sin(the2); , , sign(dthe2), dthe2, 0, ddthe1+ddthe2, , cos(the1+the2), -sin(the1+the2), dthe1^2*sin(the2)+ddthe1*cos(the2) , dthe1^2*cos(the2)-ddthe1*sin(the2) ]; L = (Ps*omega')/(lamda + omega*Ps*omega'); error = in - omega*ips; ipe = ips + L*error; Pe = (Ps - L*omega*Ps)*(1/lamda); tor = omega*ips; end 48 48 Bộ môn Đồ án Robot GVHD: Trần Đức Thiện 49 49 ... − 2m2 s2 L1 y2 + m1 x 12 + m2 x 22 + m1 y 12 + m2 y 22 + I ZZ1 + I ZZ (2. 44) M 12 = m2 x 22 + m2 y 22 + 2m2c2 L1 x2 − 2m2 s2 L1 y2 + I ZZ (2. 45) M 21 = m2 x 22 + m2 y 22 + 2m2c2 L1 x2 − 2m2 s2 L1 y2 +... gm2 ( x2 c 12 − y2 s 12 + L1c1 ) (2. 50) G21 = gm2 ( x2 c 12 − y2 s 12 ) (2. 51) 2. 2 Các thông số nhận dạng thông số động lực học robot 2. 2.1 Tổng quan phương pháp nhận dạng thông số động lực học robot. .. y2 + I ZZ (2. 46) M 22 = m2 x 22 + m2 y 22 + I ZZ (2. 47) - Ma trận V: ( ) & & V11 = − L1m2θ& 2? ?1 + θ ( y2 c2 + x2 s2 ) (2. 48) V21 = L1m2θ& ( y c2 + x2 s2 ) (2. 49) 17 17 Bộ môn Đồ án Robot - GVHD: