Phát triển thuật toán thích nghi điều khiển đồng thời quỹ đạo và lực tương tác của tay máy robot sử dụng bộ quan sát vận tốclực

BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG VIỆN KHOA HỌC VÀ CÔNG NGHỆ QUÂN SỰ ĐÀO MINH TUẤN PHÁT TRIỂN THUẬT TỐN THÍCH NGHI ĐIỀU KHIỂN ĐỒNG THỜI QUỸ ĐẠO VÀ LỰC TƯƠNG TÁC CỦA TAY MÁY ROBOT SỬ DỤNG BỘ QUAN SÁT VẬN TỐC/LỰC Chuyên ngành: Kỹ thuật điều khiển tự động hóa Mã số: 52 02 16 LUẬN ÁN TIẾN SĨ KỸ THUẬT HÀ NỘI - 2019 BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHỊNG VIỆN KHOA HỌC VÀ CƠNG NGHỆ QN SỰ ĐÀO MINH TUẤN PHÁT TRIỂN THUẬT TỐN THÍCH NGHI ĐIỀU KHIỂN ĐỒNG THỜI QUỸ ĐẠO VÀ LỰC TƯƠNG TÁC CỦA TAY MÁY ROBOT SỬ DỤNG BỘ QUAN SÁT VẬN TỐC/LỰC Chuyên ngành: Kỹ thuật điều khiển tự động hóa Mã số: 52 02 16 LUẬN ÁN TIẾN SĨ KỸ THUẬT NGƯỜI HƯỚNG DẪN KHOA HỌC: PGS TS Trần Đức Thuận TS Nguyễn Hữu Thung HÀ NỘI - 2019 i LỜI CAM ĐOAN Tôi xin cam đoan cơng trình nghiên cứu riêng tơi Các số liệu, kết nêu luận án hoàn tồn trung thực chưa cơng bố cơng trình khoa học khác, liệu tham khảo trích dẫn đầy đủ Tác giả luận án Đào Minh Tuấn ii LỜI CẢM ƠN Trước tiên, tơi bày tỏ kính trọng xin gửi lời cảm ơn sâu sắc tới tập thể giáo viên hướng dẫn, PGS.TS Trần Đức Thuận TS Nguyễn Hữu Thung ln ln nhiệt tình bảo ln động viên để tơi hồn thành luận án Tiếp theo, tơi gửi lời cảm ơn tới Phịng Đào tạo-Viện Khoa học Công nghệ quân sự, Viện Tự động hóa KTQS cán cơng tác viện giúp đỡ tơi q trình học tập, nghiên cứu khoa học có ý kiến đóng góp quý báu nội dung, bố cục luận án Tôi xin gửi lời cảm ơn đến tập thể thầy cô nơi công tác khoa Điện-Điện tử trường đại học SPKT Hưng Yên động viên chân thành san sẻ công việc chuyên môn để yên tâm thực nội dung luận án Tơi xin cảm ơn nhà nghiên cứu phịng Tối ưu Điều khiển-Viện toán học-Viện hàn lâm khoa học công nghệ Việt Nam giúp đỡ nhiều q trình nghiên cứu Cuối cùng, tơi muốn gửi lời cảm ơn tới gia đình tơi, bố mẹ, anh chị em đặc biệt người vợ thân yêu dành cho tơi tình u tin tưởng để tơi có động lực tâm thực thành công luận án TÁC GIẢ iii MỤC LỤC Nội dung Trang DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT v DANH MỤC CÁC BẢNG vii DANH MỤC CÁC HÌNH VẼ vii MỞ ĐẦU CHƯƠNG 1: CƠ SỞ ĐIỀU KHIỂN TAY MÁY ROBOT 11 1.1 Động lực học tay máy Robot 11 1.1.1 Mơ hình tốn học tay máy Robot 11 1.1.2 Các đặc tính thành phần động lực học tay máy Robot 12 1.2 Tổng quan điều khiển tay máy Robot 15 1.2.1 Điều khiển quỹ đạo tay máy Robot 16 1.2.2 Điều khiển lực tay máy Robot 25 1.3 Tổng quan quan sát 30 1.3.1 Bộ quan sát Luenberger 31 1.3.2 Quan sát lọc Kalman 34 1.3.3 Bộ quan sát tốc độ cao 38 1.4 Kết luận chương 40 CHƯƠNG 2: XÂY DỰNG BỘ QUAN SÁT VẬN TỐC/LỰC CHO TAY MÁY ROBOT 43 2.1 Cơ sở ước lượng trạng thái hệ động học lực không xác định 43 2.1.1 Ước lượng trạng thái cho hệ động lực học bậc n 45 2.1.2 Xây dựng quan sát để ước lượng trạng thái cho hệ động lực học có tác động nhiễu đo lường 51 2.2 Xây dựng quan sát vận tốc/lực cho tay máy Robot 57 2.2.1 Ràng buộc chuyển động tay máy Robot với môi trường 58 2.2.2 Xây dựng quan sát vận tốc/lực cho tay máy Robot 62 2.2.3 Sự hội tụ quan sát vận tốc/lực 65 2.3 Kết luận chương 69 CHƯƠNG 3: TỔNG HỢP THUẬT TỐN ĐIỀU KHIỂN THÍCH NGHI VỊ TRÍ VÀ LỰC CHO TAY MÁY ROBOT SỬ DỤNG BỘ QUAN SÁT 70 3.1 Cơ sở điều khiển thích nghi cho cánh tay robbot 71 3.1.1 Điều khiển thích nghi không gian khớp 71 3.1.2 Điều khiển thích nghi khơng gian Descartes 77 3.2 Tổng hợp thuật tốn điều khiển thích nghi vị trí lực sử dụng quan sát vận tốc/lực 78 3.2.1 Tổng hợp thuật toán điều khiển 79 3.2.2 Phân tích hội tụ sai lệch vị trí lực 91 iv 3.3 Kết luận chương 97 CHƯƠNG 4: MƠ PHỎNG THUẬT TỐN ĐIỀU KHIỂN THÍCH NGHI VỊ TRÍ VÀ LỰC SỬ DỤNG BỘ QUAN SÁT VẬN TỐC/LỰC 99 4.1 Xây dựng mơ hình tốn học tham số mô 99 4.1.1 Mô hình tốn học tay máy Robot A465 99 4.1.2 Các tham số mô 102 4.2 Sơ đồ mô 104 4.3 Kết mô nhận xét 106 4.3.1 Trường hợp khơng có thay đổi tham số động lực học khơng có tác động nhiễu đo lường 106 4.3.2 Trường hợp có tác động nhiễu đo lường 110 4.3.3 Trường hợp có thay đổi tham số động lực học 115 4.4 Kết luận chương 121 KẾT LUẬN 123 DANH MỤC CƠNG TRÌNH KHOA HỌC ĐÃ ĐƯỢC CÔNG BỐ 125 PHỤ LỤC Phụ lục 1: Động lực học tay máy Robot A465R Phụ lục 2: Các thành phần mô hình tốn học tay máy Robot A465R Phụ lục 3: Động học tay máy Robot sử dụng quan sát Phụ lục 4: Thiết kế quỹ đạo điểm tác động cuối tay máy Robot Phụ lục 5: Tham số quan sát điều khiển thích nghi vị/lực Phụ lục 6: Code chương trình mơ v DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT H q  Ma trận quán tính V  q, q  Véc tơ ly tâm tương hỗ g  q  Véc tơ thành phần trọng trường D q  Véc tơ ma sát Dv  d g  Hệ số ma sát nhớt Véc tơ mô men đặt vào khớp Thành phần nhiễu Gia tốc trọng trường, Ma trận hướng hệ tọa độ gắn lên khâu tác động cuối so với hệ tọa độ gốc Ma trận Jacobi Robot J q  S L wk vk xˆ k zk Pk  t Ma trận chọn điều khiển lai vị trí/lực Hệ số khuếch đại quan sát Nhiễu trình lọc Kalmam Nhiễu đo lường lọc Kalmam Ước lượng trạng thái hậu nghiệm bước k Giá trị đo lường bước k Hiệp phương sai sai lệch hậu nghiệm lọc Kalman Sai lệch ước lượng trạng thái Nhiễu đo lường d t p    Véc tơ tham số động lực học tay máy Robot Véc tơ tham số ước lượng động lực học tay máy Robot pˆ  (x) Ràng buộc chuyển động điểm tác động cuối tay máy Robot J  (q) Gradian ràng buộc chuyển động không gian khớp J x  q  Gradian ràng buộc chuyển động không gian Descartes I nn Qq  Ma trận đơn vị Ma trận chiếu theo phương tiếp tuyến P q  Ma trận chiếu theo phương pháp tuyến  q r Toán tử Lagrange điểm tương tác Biến tham chiếu vi  L  M0  i Tj Hệ số khuếch đại sai lệch lực Hệ số khuếch đại sai lệch vị trí Hệ số cập nhập tham số động lực học Hệ số giảm chấn Sai lệch đáp ứng lực Ma trận biến đổi đồng xác định vị trí hướng hệ tọa độ  j hệ tọa độ i x Vị trí khâu tác động cuối hệ trục tọa độ 0 Độ lớn lực ma sát điểm tác động cuối tay máy Robot   x  x với môi trường làm việc Ma trận hồi quy  q,  q  Y  q, q,  i i   p  n Hệ số quan sát li i   3 mi i   3 I i i   3 AFE APFC APFC-VFO DOF FC FMS GPI Observer HGO IR PC PD Controller RRR NGPI Observer slvtul VFO SPO Chiều dài nối thứ i  m  Khối lượng nối thứ i kg  Mơ men qn tính nối thứ i trọng tâm Ci  kg.m  Bộ lước lượng lực thích nghi (Adaptive force estimator) Bộ điều khiển thích nghi vị trí lực (Adaptive Position and Force Controller) Bộ điều khiển thích nghi vị trí lực sử dụng quan sát vận tốc/lực (Adaptive Position and Force Controller using Velocity/Force Observer) Bậc tự (Degrees Of Freedom) Bộ điều khiển lực (Force Controller) Hệ thống sản xuất linh hoạt (Flexible Manufacturing System) Bộ quan sát tích phân tỷ lệ tổng quát (Generalized Proportional Integral Observer) Bộ quan sát hệ số cao (Hight Gain Observer) Robot công nghiệp (Industrial Robot) Bộ điều khiển vị trí (Position Controller) Bộ điều khiển PD (Proportion Derivative Controller) Rotation, Rotation, Rotation Bộ quan sát tích phân tỷ lệ tổng quát (New Generalized Proportional Integral Observer) Sai lệch vận tốc ước lượng Bộ quan sát vận tốc lực (Velocity Force Observer) Bộ quan sát nhiễu trượt (Sliding Perturbation Observer) vii DANH MỤC CÁC BẢNG Trang Bảng Các tham số động lực học tay máy Robot A465R 101 Bảng Giá trị thay đổi tham số động lực học 114 DANH MỤC CÁC HÌNH VẼ Trang Hình 1.1 Sơ đồ phương pháp điều khiển tay máy Robot 15 Hình 1.2 Sơ đồ khối điều khiển PD bù trọng trường khơng gian khớp 18 Hình 1.3 Điều khiển PD bù trọng trường không gian làm việc 19 Hình 1.4 Sơ đồ điều khiển PID cho tay máy Robot 21 Hình 1.5 Sơ đồ khối điều khiển động lực học đảo tay máy Robot 23 Hình 1.6 Sơ đồ điều khiển động lực học đảo khơng gian làm việc 24 Hình 1.7 Điều khiển độ cứng tích cực 25 Hình 1.8 Sơ đồ phương pháp điều khiển trở kháng 27 Hình 1.9 Sơ đồ phương pháp điều khiển lai vị trí/lực 29 Hình 1.10 Sơ đồ khối quan sát Luenberger 33 Hình 1.11 Chu trình lọc Kalman 37 Hình 1.12 Sơ đồ tổng quan trình thực lọc Kalman 38 Hình 2.1 Sơ đồ quan sát vận tốc/lực cho tay máy Robot 57 Hình 2.2 Khơng gian trực giao điểm tác động cuối 59 Hình 3.1.Sơ đồ điều khiển thích nghi tay máy Robot 76 Hình 3.2 Sơ đồ điều khiển thích nghi vị trí lực sử dụng quan sát 78 Hình 3.3 Ràng buộc điểm tác động cuối khơng gian Descartes 79 viii Hình 3.4 Phép chiếu véc tơ q lên mặt phẳng vuông góc vớt J T  q  khơng gian khớp 82 Hình 3.5 Sơ đồ điều khiển thích nghi sử dụng quan sát vận tốc/lực 84 Hình 3.6 Lưu đồ thuật toán điều khiển 86 Hình 3.7 Khơng gian hình học q q d 92 Hình 4.1 Chuyển động khớp tay máy Robot A465 99 Hình 4.2 Hệ tọa độ gán tay máy Robot 100 Hình 4.3 Mơ tả hình học tay máy Robot A465R 100 Hình 4.4 Các tham số chuyển động tay máy Robot A465R 101 Hình 4.5 Mơ tả chuyển động điểm tác động cuối 103 Hình 4.6 Sơ đồ khối mơ tả hệ thống điều khiển Simulink 104 Hình 4.7 Khối động lực học Robot ràng buộc môi trường 104 Hình 4.8 Khối tạo nhiễu đo lường 105 Hình 4.9 Khối điều khiển 105 Hình 4.10 Đáp ứng vị trí theo trục x 106 Hình 4.11 Đáp ứng vị trí theo trục y 106 Hình 4.12 Đáp ứng vị trí theo góc  106 Hình 4.13 Sai lệch vị trí theo trục x 107 Hình 4.14 Sai lệch vị trí theo trục y 107 Hình 4.15 Sai lệch vị trí theo góc  107 Hình 4.16 Lực đặt, đáp ứng lực thực tế lực đặt 108 Hình 4.17 Lực đặt, đáp ứng lực thực tế lực ước lượng 108 Hình 4.18 Giá trị thực ước lượng vận tốc khớp 108 P-6 Phụ lục 5: Tham số quan sát điều khiển thích nghi vị/lực 10 0   200 0      K d   10  ,   45 , L   200  ,   0.01      0 10   200     Phụ lục 6: Code chương trình mô clc; clear all; global p1 p2 p3 p4 p5 p6 p7 p8 p9 p10 p11 p12 p13 p14 p15 global alpha beta l1 l2 l3 global a b global T %% Thoi gian tric mau T = 0.001; %% tham so Denavit-Harenberg l1 = 0.305; l2 = 0.33; l3 = 0.13; %% tham so dong co(gan tren cac khop quay robot) global Dn Dk go r1 = 100; r2 = 100; r3 = 100; Ka1 = 0.1876896; Ka2 = Ka1; Ka3 = 0.053; Kb1 = 0.14229; Kb2 = 0.14229; Kb3 = 0.0534; Ra1 = 0.84; Ra2 = 0.84; Ra3 = 2.7; fm1 = 9.70996e-5; fm2 = 9.70996e-5; fm3 = 0.02234; Jm1 = 9.0376e-5; Jm2 = 9.0376e-5; Jm3 = 4.9e-6; Dj = diag([Jm1 Jm2 Jm3]); Df = diag([fm1+(Ka1*Kb1)/Ra1 fm2+(Ka2*Kb2)/Ra2 fm3+(Ka3*Kb3)/Ra3]); Dn = diag([1/(r1^2) 1/(r2^2) 1/(r3^2)]); Dk = diag([Ka1/(Ra1*r1) Ka2/(Ra2*r2) Ka3/(Ra3*r3)]); %% Tham so cua robot m1 = 28.5; m2 = 16.6; m3 = 1; I1 = 0.85; I2 = 0.7; I3 = 0.18; lc1 = 0.14; lc2 = 0.14; lc3 = 0.07; Im1 = Jm1*r1^2; Im2 = Jm2*r2^2; Im3 = Jm3*r3^2; d1 = 2.6; d2 = 2.5; d3 = 1.5; df1 = fm1+(Ka1*Kb1)/Ra1; df2 = fm2+(Ka2*Kb2)/Ra2; df3 = fm3+(Ka3*Kb3)/Ra3; go = 9.8; fc1 = 2.5; fc2 = 2.5; fc3 = 3.0; p1 = m1*lc1^2 + m2*(l1^2+lc2^2) + m3*(l1^2+l2^2+lc3^2) + I1 + I2 + I3; p2 = m2*lc2^2 + m3*(l2^2+lc3^2) + I2 + I3; p3 = m3*lc3^2 + I3; p4 = m2*l1*lc2 + m3*l1*l2; p5 = m3*l1*lc3; p6 = m3*l2*lc3; p7 = d1; p8 = d2; p9 = d3; p10 = (m1*lc1 + (m2+m3)*l1); p11 = (m2*lc2 + m3*l2); p12 = m3*lc3; P-7 p13 = fc1; p14 = fc2; p15 = fc3; %% Tham so mat phang rang buoc chuyen dong canh tay robot alpha = 68*pi/180; beta = 0.35; polab = poly([-0.000001 -0.000001]); a = polab(3); b = polab(2); %% Cac dieu kien ban dau global q1i q2i q3i di di = 0.4; df = 0; x1i = beta + di*cos(alpha); x2i = di*sin(alpha); x3i = alpha - pi/2; xc1 = x1i - l3*cos(x3i); xc2 = x2i - l3*sin(x3i); D = (xc1*xc1 + xc2*xc2 -l1*l1 -l2*l2)/(2*l1*l2); q2i = atan2(-sqrt(1-D*D),D); q1i = atan2(xc2,xc1)-atan2(l2*sin(q2i),l1+l2*cos(q2i)); q3i = x3i - q1i -q2i; %% Cac tham so khac %Momen lon nhat cua co cau chap hanh dua global parmax parmax = Dn\Dk*[4;4;6]; % Tham so thiet ke quy dao chuyen dong robot(quy dao bac 5) global a0 a1 a2 a3 a4 a5 x1f x2f tf x1f = beta + df*cos(alpha); x2f = df*sin(alpha); tf = 6; a0 = [x1i;x2i]; a1 = [0;0]; a2 = [0;0]; Cf = [tf*tf*tf*tf*tf,tf*tf*tf*tf,tf*tf*tf; 5*tf*tf*tf*tf, 4*tf*tf*tf, 3*tf*tf; 20*tf*tf*tf, 12*tf*tf, 6*tf]; ax1 = Cf\[(x1f-x1i);0;0]; ax2 = Cf\[(x2f-x2i);0;0]; a5 = [ax1(1);ax2(1)]; a4 = [ax1(2);ax2(2)]; a3 = [ax1(3);ax2(3)]; %Tham so bo dieu khien(PID) global e_1 qd_1 qrp_1 Kp Kd Ki ei e_1 = [0;0;0]; ei = [0;0;0]; qrp_1 = [0;0;0]; qd_1 = [q1i;q2i;q3i]; Kp = diag([5000,20000,1000]); % Kd = diag([100,100,100]); Kd = diag([20,20,4]); %voi truong hop co nhieu Ki = diag([10000,10000,1000]); %Cac tham so luc dat global DeltaF lambdadf xif lambdadi kt amsif lambdadi = 12; kt = 2; amsif = 15; lambdadf = 50; P-8 DeltaF = 0; xif = 45; %tham so thich nghi global al bt Kv Gamma th Kv = 1*diag([10,10,2]); al = diag(2*[200,200,200]); bt =0.01; Gamma = diag([0.001,0.001,0.001 ,0.0001,0.0001,0.0001 ,10,10,10 ,.1,.1,.1 ,0*1,0*1,0*1]); th = zeros(15,1); % Tham so GPI global eta wn eta = diag(0.9*[1.0,1.0,1.0]); wn = diag(0.25*[4,4,4]); global lam0 lam1 lam2 lam3 eg1 eg2 zg1 zg2 global lamf0 lamf1 lamf2 lamf3 egf zgf1 zgf2 zgf3 polamb = poly([-80 -80 -80 -80]); lam0 = polamb(5); lam1 = polamb(4); lam2 = polamb(3); lam3 = polamb(2); eg1 = [0;0;0]; eg2 = [0;0;0]; zg1 = [0;0;0]; zg2 = [0;0;0]; polamb = poly([-5 -5 -5 -5]); lamf0 = polamb(5); lamf1 = polamb(4); lamf2 = polamb(3); lamf3 = polamb(2); egf = 0; zgf1 = 0; zgf2 = 0; zgf3 = 0; global xg zi Deltax Deltaz KD lambdag Jm1_1 DeltaFb xg = [x1i;x2i;x3i]; zi = [0;0;0]; lambdag = 0; Deltax = diag([5,5,5]); Deltaz = diag([100,100,100]); KD = diag([50,50,100]); Jm1_1 = [-l1*sin(q1i)-l2*sin(q1i+q2i)-l3*sin(q1i+q2i+q3i), l2*sin(q1i+q2i)-l3*sin(q1i+q2i+q3i), -l3*sin(q1i+q2i+q3i); l1*cos(q1i)+l2*cos(q1i+q2i)+l3*cos(q1i+q2i+q3i), l2*cos(q1i+q2i)+l3*cos(q1i+q2i+q3i), l3*cos(q1i+q2i+q3i); 1,1,1]; DeltaFb = 0; global eta1 eta2 eta3 sigma KP Kgamma Kbeta sini k1 xb_1 eta1 = 0.0001; eta2 = 50; eta3 = 0.001; sigma = [0;0;0]; Kgamma = diag([0.001,0.001,0.01]); Kbeta = 0.1; KP = 20*diag([100,200,10]); k1 = 0.1; JphixT = [-sin(alpha);cos(alpha);0]; %Plano Px = JphixT/(JphixT'*JphixT)*JphixT'; P-9 Qx = eye(3) - Px; xd = [a0(1);a0(2);pi-alpha;]; sini = Qx*(xg - xd); xb_1 = xg - xd; global x1g x2g x3g x4g x5g x6g xgp x7g x8g x9g global epsilonx epsilony epsilonphi w1f w2f w3f ff x1g = x1i + 0.0005; x2g = x2i - 0.0005; x3g = x3i + 0.01*pi/180; x4g = 0; x5g = 0; x6g = 0; x7g = 0; x8g = 0; x9g = 0; epsilonx = 0.9;%0.042; epsilony = 0.9; %0.055 epsilonphi = 0.9; %0.067 w1f = 0; w2f = 0; w3f = 0; ff = 1/sqrt(T); xgp = [0;0;0]; global ks1 ks2 ks3 M = 1; ks1 = 2*M^(1/3); ks2 = 1.5*sqrt(M); ks3 = 1.1*M; global mu1 mu2 mu3 mu1 = 100;%100; mu2 = 100;%100; mu3 = 50;%50; global lamd0 lamd1 lamd2 lamd3 lamd4 s1g s2g z1g z2g z3g z1gf s1g = [0;0;0]; s2g = [0;0;0]; z1g = [0;0;0]; z2g = [0;0;0]; z3g = [0;0;0]; z1gf = [0;0;0]; polambd = poly([-120 -120 -120 -120 -120]); lamd0 = eye(3)*polambd(6); lamd1 = eye(3)*polambd(5); lamd2 = eye(3)*polambd(4); lamd3 = eye(3)*polambd(3); lamd4 = eye(3)*polambd(2); global epsilon epsilon = 0.02; % Tham so bo loc hoi quy global yf lamfilt taufilt Pt yf = zeros(3,15); taufilt = [0;0;0]; lamfilt = 1; Pt = diag([10, 10, 10]); % Lua chon phuong phap dieu khien va chon loai bo quan sat global tipoCtrl tipoObserv conObserv movLibre movLibre = 0; % Dieu khien chuyen dong cua robot voi rang buoc va dieu khien luc conObserv = 1; %0:Quan sat voi vong lap ho, 1:Quan sat voi vong lap kin P-10 tipoCtrl = 2; %1:PID, 2:Thich nghi Slovetin_Le, 3:GPI, 4:ArteagaRivera2010 5: Adaptable tipoObserv = 5; %1:ArteagaRivera2010, 2:Truot bac 2, 3: Truot bac 3, 4:GSTO, 5:GPI %% Chương trình tính nhân tử Lagrang  function [lambda] = fuerzaPlano(u) global p1 p2 p3 p4 p5 p6 p7 p8 p9 p10 p11 p12 p13 p14 p15 global a b alpha beta l1 l2 l3 go global movLibre %% tin hieu dau vao tau = u(1:3); q1 = u(4); q2 = u(5); q3 = u(6); dq1 = u(7); dq2 = u(8); dq3 = u(9); qp = [dq1;dq2;dq3]; %% Dinh nghia cac ky hieu c1 = cos(q1); c2 = cos(q2); c3 = cos(q3); s1 = sin(q1); s2 = sin(q2); s3 = sin(q3); c12 = cos(q1+q2); c123 = cos(q1+q2+q3); c23 = cos(q2+q3); s12 = sin(q1+q2); s23 = sin(q2+q3); s123 = sin(q1+q2+q3); %% Dong luc hoc robot % Ma tran quan tinh H h11 = p1 + 2*c2*p4 + 2*p5*c23 + 2*p6*c3; h12 = p4*c2 + p5*c23 + 2*p6*c3 + p2; h13 = p5*c23 + p6*c3 + p3; h22 = p2 + 2*p6*c3; h23 = p3 + p6*c3; h33 = p3; H = [h11 h12 h13; h12 h22 h23; h13 h23 h33]; % Vec to phan luc ly tam va coriolis C11 = -p4*s2*dq2 - p5*s23*(dq2 + dq3) - p6*s3*dq3; C12 = -p4*s2*(dq1+dq2) - p5*s23*(dq1+dq2+dq3)-p6*s3*dq3; C13 = -p5*s23*(dq1 + dq2 + dq3) - p6*s3*(dq1 + 2*dq2 + dq3); C21 = -p4*s2*(dq1+dq2) - p5*s23*(dq1+dq2+dq3) - p6*s3*dq3; C22 = -p6*s3*dq3; C23 = -p6*s3*(dq1 + dq2+ dq3); C31 = p5*s23*dq1 + p6*s3*(dq1 + dq2); C32 = p6*s3*(dq1 + dq2); C33 = 0; C = [C11 C12 C13; C21 C22 C23; C31 C32 C33]; % He so ma sat nhot D = diag([p7 p8 p9]); % Vec to phan luc truong g1 = go*(p10*c1 + p11*c12 + p12*c123); g2 = go*(p11*c12 + p12*c123); P-11 g3 = go*(p12*c123); g = [g1;g2;g3]; Nqqp = C*qp + D*qp + g; Jphix = [-sin(alpha),cos(alpha),0]; % Dao ham rieng mat phang theo bien x % Ma tran Jacobi J = [-l1*s1-l2*s12-l3*s123, -l2*s12-l3*s123, -l3*s123; l1*c1+l2*c12+l3*c123, l2*c12+l3*c123, l3*c123; 1,1,1]; Jp = [-l1*c1*dq1-l2*c12*(dq1+dq2)-l3*c123*(dq1+dq2+dq3), l2*c12*(dq1+dq2)-l3*c123*(dq1+dq2+dq3),-l3*c123*(dq1+dq2+dq3); -l1*s1*dq1-l2*s12*(dq1+dq2)-l3*s123*(dq1+dq2+dq3), -l2*s12*(dq1+dq2)l3*s123*(dq1+dq2+dq3),-l3*s123*(dq1+dq2+dq3); 0,0,0]; % Dao ham cua Jphix Jphixp = [0,0,0]; % Toa diem tac dong cuoi xet mat phang rang buoc voi cac bien la x % va y(tuong ung voi bien x1 x2) x1 = l1*c1+l2*c12+l3*c123; x2 = l1*s1+l2*s12+l3*s123; xp = J*qp; x1p = xp(1); x2p = xp(2); % phuong trinh mat phang rang buoc chuyen dong robot robot(rang buoc holonomic) phidex = cos(alpha)*x2 - sin(alpha)*(x1-beta); phidexp = cos(alpha)*x2p - sin(alpha)*x1p; phidexpp = -a*phidex -b*phidexp; % Cac gia tri gan de tinh nhan tu Lagrange_multiplier(lambda) Ma1 = Jphix*J/H*J'*Jphix'; Ma2 = Jphix*J/H; sumat = phidexpp - Ma2*(tau - Nqqp) - Jphix*Jp*qp - Jphixp*J*qp; % Gan co "movlibre", neu movlibre=1 thi chuyen dong la tu do, nghia la % % lambda=0 if(movLibre==1) lambda = 0; else lambda = Ma1\sumat; end %% Chương trình tính lực tương tác điểm tác động cuối với môi trường J   T function [JphiTL] = fuerzaSup (u) global alpha l1 l2 l3 q1 = u(1); q2 = u(2); q3 = u(3); lambda = u(4); %% Dinh nghia cac ky hieu c1 = cos(q1); c2 = cos(q2); c3 = cos(q3); s1 = sin(q1); s2 = sin(q2); s3 = sin(q3); P-12 c12 = cos(q1+q2); c123 = cos(q1+q2+q3); s12 = sin(q1+q2); s123 = sin(q1+q2+q3); JphixT = [-sin(alpha);cos(alpha);0]; %Dao ham rieng mat phang theo bien x J = [-l1*s1-l2*s12-l3*s123, -l2*s12-l3*s123, -l3*s123; l1*c1+l2*c12+l3*c123, l2*c12+l3*c123, l3*c123; 1,1,1]; JphiT = J'*JphixT; JphiTL = JphiT*lambda; %% Chương trình động lực học robot function [qpp] = dinamica(u) global p1 p2 p3 p4 p5 p6 p7 p8 p9 p10 p11 p12 p13 p14 p15 go %% Cac tin hieu dau vao tau = u(1:3); q1 = u(4); q2 = u(5); q3 = u(6); dq1 = u(7); dq2 = u(8); dq3 = u(9); qp = [dq1;dq2;dq3]; %% Mơ hình %% Dinh nghia cac ky hieu c1 = cos(q1); c2 = cos(q2); s2 = sin(q2); c3 = cos(q3); s3 = sin(q3); c12 = cos(q1+q2); c23 = cos(q2+q3); s23 = sin(q2+q3); c123 = cos(q1+q2+q3); %% dong_luc_hoc % Ma tran phan quan tinh h11 = p1 + 2*c2*p4 + 2*p5*c23 + 2*p6*c3; h12 = p4*c2 + p5*c23 + 2*p6*c3 + p2; h13 = p5*c23 + p6*c3 + p3; h22 = p2 + 2*p6*c3; h23 = p3 + p6*c3; h33 = p3; H = [h11 h12 h13; h12 h22 h23; h13 h23 h33]; %Vector phan luc ly tam va coriolis C11 = -p4*s2*dq2 - p5*s23*(dq2 + dq3) - p6*s3*dq3; C12 = -p4*s2*(dq1+dq2) - p5*s23*(dq1+dq2+dq3)-p6*s3*dq3; C13 = -p5*s23*(dq1 + dq2 + dq3) - p6*s3*(dq1 + 2*dq2 + dq3); C21 = -p4*s2*(dq1+dq2) - p5*s23*(dq1+dq2+dq3) - p6*s3*dq3; C22 = -p6*s3*dq3; C23 = -p6*s3*(dq1 + dq2+ dq3); C31 = p5*s23*dq1 + p6*s3*(dq1 + dq2); C32 = p6*s3*(dq1 + dq2); C33 = 0; C = [C11 C12 C13; C21 C22 C23; C31 C32 C33]; % He so ma sat nhot D = diag([p7 p8 p9]); % Vector luc truong P-13 g1 = go*(p10*c1 + p11*c12 + p12*c123); g2 = go*(p11*c12 + p12*c123); g3 = go*(p12*c123); g = [g1;g2;g3]; % He so ma sat khô fc = [p13*tanh(1000*qp(1));p14*tanh(1000*qp(2));p15*tanh(1000*qp(3))]; %% Gia toc goc cac khop qpp = H\(tau - C*qp - D*qp - g - 0*fc); %% Chương trình tính vị trí điểm tác động cuối cánh tay robot function [x] = directa(u) global alpha beta l1 l2 l3 q1 = u(1); q2 = u(2); q3 = u(3); %% Dinh nghia cac ky hieu c1 = cos(q1); c2 = cos(q2); c3 = cos(q3); s1 = sin(q1); s2 = sin(q2); s3 = sin(q3); c12 = cos(q1+q2); c123 = cos(q1+q2+q3); c23 = cos(q2+q3); s12 = sin(q1+q2); s23 = sin(q2+q3); s123 = sin(q1+q2+q3); %% Dong hoc nguoc vi tri x1 = l1*c1+l2*c12+l3*c123; x2 = l1*s1+l2*s12+l3*s123; x3 = q1 + q2 + q3; phidex = cos(alpha)*(x2)-sin(alpha)*(x1-beta); x = [x1;x2;x3;phidex]; %% Chương trình thuật tốn điều khiển quan sát function [sal] = control(u) global p1 p2 p3 p4 p5 p6 p7 p8 p9 p10 p11 p12 global alpha T e_1 qd_1 qrp_1 Kp Kd Ki ei q1i q2i q3i global a0 a1 a2 a3 a4 a5 x1f x2f tf global l1 l2 l3 global lambdadf DeltaF xif lambdadi kt amsif global al bt Kv Gamma th global eta wn global lam0 lam1 lam2 lam3 eg1 eg2 zg1 zg2 global lamf0 lamf1 lamf2 lamf3 egf zgf1 zgf2 zgf3 global xg zi Deltax Deltaz KP KD lambdag Jm1_1 DeltaFb global eta1 eta2 eta3 sigma Kgamma Kbeta sini k1 xb_1 global x1g x2g x3g x4g x5g x6g xgp x7g x8g x9g global epsilonx epsilony epsilonphi w1f w2f w3f ff global Dn Dk global tipoCtrl tipoObserv conObserv movLibre global ks1 ks2 ks3 global mu1 mu2 mu3 global lamd0 lamd1 lamd2 lamd3 lamd4 s1g s2g z1g z2g z3g z1gf P-14 global epsilon go global lamfilt taufilt Pt yf %% Tin hieu dau vao t = u(1); q1 = u(2); q2 = u(3); q3 = u(4); q1p = u(5); q2p = u(6); q3p = u(7); lambda = u(8); % if(lambda