Động lực học và điều khiển robot đi hai chân Động lực học và điều khiển robot đi hai chân Động lực học và điều khiển robot đi hai chân luận văn tốt nghiệp,luận văn thạc sĩ, luận văn cao học, luận văn đại học, luận án tiến sĩ, đồ án tốt nghiệp luận văn tốt nghiệp,luận văn thạc sĩ, luận văn cao học, luận văn đại học, luận án tiến sĩ, đồ án tốt nghiệp
BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - Trịnh Quốc Trung ĐỘNG LỰC HỌC VÀ ĐIỀU KHIỂN ROBOT ĐI HAI CHÂN Chuyên ngành: Cơ điện tử LUẬN VĂN THẠC SĨ KHOA HỌC CƠ ĐIỆN TỬ NGƯỜI HƯỚNG DẪN KHOA HỌC GS.TSKH Nguyễn Văn Khang Hà Nội – 2013 LỜI CAM ĐOAN Tôi xin cam đoan luận văn thạc sỹ khoa học “Động lực học điều khiển robot hai chân” nghiên cứu hướng dẫn GS.TSKH Nguyễn Văn Khang Các số liệu luận văn có nguồn góc rõ ràng trích dẫn Nếu có gian lận xin chịu trách nhiệm Hà Nội, ngày … tháng … năm 2013 Học viên Trịnh Quốc Trung MỤC LỤC DANH MỤC HÌNH VẼ DANH MỤC BẢNG DANH SÁCH KÝ HIỆU, CHỮ VIẾT TẮT .5 LỜI NÓI ĐẦU Chương MỘT SỐ KHÁI NIỆM CƠ BẢN CỦA ROBOT HAI CHÂN 1.1 Giới thiệu robot hai chân .8 1.2 Các khái niệm robot hai chân .12 1.2.1 Các thuật ngữ 13 1.2.2 Dáng ổn định 16 1.3 Điểm moment triệt tiêu ZMP 20 1.3.1 Điểm moment triệt tiêu ZMP 21 1.3.2 Các điểm ZMP, FZMP CoP 23 Chương ĐỘNG HỌC NGƯỢC ROBOT HAI CHÂN 25 2.1 Phân tích giai đoạn chuyển động 26 2.1.1 Giai đoạn khởi động 27 2.1.2 Giai đoạn chuyển động bước ổn định 29 2.1.3 Giai đoạn dừng 31 2.1.4 Tính chu kỳ robot hai chân bước 32 2.2 Thiết kế quỹ đạo chuyển động 34 2.2.1 Pha chân trụ 34 2.2.2 Pha hai chân trụ 38 2.2.3 Các kỳ dị động học 40 2.2.4 Điều kiện tham số đảm bảo toán động học ngược giải 40 2.3 Bài toán động học ngược robot hai chân 41 2.3.1 Phát biểu toán 41 2.3.2 Phương pháp Newton – Raphson cải tiến giải tốn động học ngược 42 2.4 Mơ số động học ngược robot hai chân 45 2.5 Một phương pháp thiết kế quỹ đạo khớp robot hai chân 50 Chương ĐỘNG LỰC HỌC ROBOT HAI CHÂN .58 3.1 Phương trình Lagrange loại cho hệ nhiều vật 58 3.2 Thiết lập phương trình vi phân chuyển động robot hai chân sử dụng toạ độ khớp tuyệt đối .61 3.3 Thiết lập phương trình vi phân chuyển động robot hai chân sử dụng tọa độ khớp tương đối 73 3.4 Động lực học ngược robot hai chân 79 3.4.1 Robot hai chân pha chân trụ 80 3.4.2 Robot hai chân pha hai chân trụ 84 3.4.3 Phương pháp số giải phương trình vi phân 85 Chương ĐIỀU KHIỂN CHUYỂN ĐỘNG CỦA ROBOT HAI CHÂN TRONG PHA MỘT CHÂN TRỤ 87 4.1 Điều khiển PD .88 4.1.1 Điều khiển PD robot dạng chuỗi 88 4.1.2 Điều khiển PD cho robot hai chân 90 4.2 Điều khiển trượt .91 4.2.1 Điều khiển trượt robot dạng chuỗi 91 4.2.2 Điều khiển trượt cho robot hai chân 96 4.3 Điều khiển robot theo nguyên lý trượt sử dụng mạng nơ ron .98 4.3.1 Điều khiển robot dạng chuỗi theo nguyên lý trượt sử dụng mạng nơ ron98 4.3.2 Điều khiển robot hai chân theo nguyên lý trượt sử dụng mạng nơ ron 104 4.4 Các kết mô số 106 KẾT LUẬN .118 TÀI LIỆU THAM KHẢO .119 PHỤ LỤC 1: TÍNH TOÁN ĐỘNG LỰC HỌC 121 PHỤ LỤC 2: GIẢI BÀI TOÁN ĐỘNG LỰC HỌC NGƯỢC .125 PHỤ LỤC 3: MƠ HÌNH SIMULINK 132 DANH MỤC HÌNH VẼ Hình Robot WABIAN – trường đại học Waseda 10 Hình Các dịng sản phầm robot dáng người P1, 2, 3, Asimo Honda 11 Hình Các Robot HRP – 2, HRP – 3, HRP – 11 Hình Robot PETMAN Boston Dynamic 12 Hình Các mặt phẳng sở để nghiên cứu robot dáng người 13 Hình Hình biểu diễn chu kỳ bước 16 Hình Hình vẽ biểu diễn áp lực tổng hợp vị trí CoP 19 Hình Lực moment tác dụng bàn chân 22 Hình Quan hệ điểm ZMP, FZMP CoP 24 Hình 10 Mơ hình robot hai chân có khâu 25 Hình 11 Tính chu kỳ robot hai chân bước 33 Hình 12 Chuyển động bước robot hai chân .36 Hình 13 Chuyển động robot hai chân pha hai chân trụ 38 Hình 14 Lưu đồ thuật toán phương pháp Newton – Raphson cải tiến 45 Hình 15 Đồ thị hơng giai đoạn khởi động bước ổn định .47 Hình 16 Đồ thị chân bước giai đoạn khởi động bước ổn định .47 Hình 17 Đồ thị góc khớp robot bước .48 Hình 18 Đồ thị vận tốc góc khớp robot bước .49 Hình 19 Đồ thị gia tốc góc khớp robot bước 49 Hình 20 Mơ bước robot hai chân .50 Hình 21 Bước bắt đầu đồ thị góc 51 Hình 22 Bước ổn định đồ thị góc 52 Hình 23 Chuyển từ thiết kế quỹ đạo theo hàm bậc thành hàm sin 53 Hình 24 Đồ thị vị trí, vận tốc gia tốc góc khớp bước bắt đầu 54 Hình 25 Đồ thị vị trí, vận tốc gia tốc góc khớp bước bắt đầu 54 Hình 26 Đồ thị vị trí, vận tốc gia tốc góc khớp bước bắt đầu 55 Hình 27 Đồ thị vị trí, vận tốc gia tốc góc khớp bước bắt đầu 55 Hình 28 Đồ thị vị trí, vận tốc gia tốc góc khớp bước ổn định 56 Hình 29 Đồ thị vị trí, vận tốc gia tốc góc khớp bước ổn định 56 Hình 30 Đồ thị vị trí, vận tốc gia tốc góc khớp bước ổn định 57 Hình 31 Đồ thị vị trí, vận tốc gia tốc góc khớp bước ổn định 57 Hình 32 Hệ tọa độ khâu hệ toạ độ khớp 58 Hình 33 Mơ hình robot hai chân hai pha 61 Hình 34 Mơ hình robot hai chân tách liên kết chân .62 Hình 35 Mơ hình robot với tọa độ khớp tương đối 74 Hình 36 Đồ thị moment phát động robot hai chân bước bắt đầu 83 Hình 37 Đồ thị moment phát động robot hai chân bước ổn định 83 Hình 38 Mạng RBF ứng dụng cho điều khiển robot 101 Hình 39 Đồ thị moment phát động bước ban đầu PD 108 Hình 40 Đồ thị sai số góc khớp bước ban đầu PD .108 Hình 41 Đồ thị moment phát động bước ban đầu SL 109 Hình 42 Đồ thị sai số góc khớp bước ban đầu SL 109 Hình 43 Đồ thị moment phát động bước ban đầu NR 110 Hình 44 Đồ thị sai số góc khớp bước ban đầu NR .110 Hình 45 Đồ thị moment phát động bước ổn định PID 111 Hình 46 Đồ thị sai số góc khớp bước ổn định PID 111 Hình 47 Đồ thị moment phát động bước ổn định SL .112 Hình 48 Đồ thị sai số góc khớp bước ổn định SL 112 Hình 49 Đồ thị moment phát động bước ổn định NR .113 Hình 50 Đồ thị sai số góc khớp bước ổn định NR 113 DANH MỤC BẢNG Bảng Các ký hiệu sử dụng miêu tả kí hiệu .26 Bảng Các tham số robot hai chân kích thước nhỏ 26 Bảng Moment phát động điều khiển với độ bất định 0% 114 Bảng Moment phát động điều khiển với độ bất định 30% 114 Bảng Sai số góc khớp điều khiển với độ bất định 0% 114 Bảng Sai số góc khớp điều khiển với độ bất định 30% 114 Bảng Thời gian mô điều khiển với độ bất định 0% 115 Bảng Thời gian mô điều khiển với độ bất định 30% .115 Bảng Moment phát động điều khiển với độ bất định 0% 115 Bảng 10 Moment phát động điều khiển với độ bất định 30% .115 Bảng 11 Sai số góc khớp điều khiển với độ bất định 0% 115 Bảng 12 Sai số góc khớp điều khiển với độ bất định 30% 116 Bảng 13 Thời gian mô điều khiển với độ bất định 0% .116 Bảng 14 Thời gian mô điều khiển với độ bất định 30% .116 DANH SÁCH KÝ HIỆU, CHỮ VIẾT TẮT STT Ký hiệu Chú giải DS Hai chân trụ SP Một chân trụ SP Đa giác trụ vững DSP Pha hai chân trụ SSP Pha chân trụ CoM Khối tâm FCoM Hình chiếu khối tâm lên mặt phẳng ZMP Điểm mô men triệt tiêu CoP Tâm áp lực 10 FZMP Điểm mô men triệt tiêu ảo 16 Nguyễn Văn Khang, Động lực học hệ nhiều vật, Nhà xuất khoa học kỹ thuật, Hà Nội 2007 17 Nguyễn Văn Khang, Chu Anh Mỳ, Cơ sở robot công nghiệp, Nhà xuất giáo dục Việt Nam, Hà Nội 2010 18 Nguyễn Văn Khang, Nguyễn Quang Hoàng, Lê Đức Đạt, Trần Hồng Nam, Về thuật tốn điều khiển trượt robot dư dẫn động, Tuyển tập Hội nghị học toàn quốc lần thứ 8, Vol (2007), pp 250-259 19 Nguyễn Văn Khang, Lương Anh Tuấn, Tính toán so sánh phương pháp số giải toán động học ngược robot song song dư dẫn động, Tạp chí Tin học Điều khiển học, T.29, S.1 (2013), 3-15 20 Nguyễn Trung Tuấn, Tính tốn động học, động lực học điều khiển robot phương pháp ma trận Craig, Đồ án tốt nghiệp, Trường Đại học Bách Khoa Hà Nội, 2010 21 Nguyễn Mạnh Tiến, Động lực học điều khiển robot có cấu trúc dạng mạch hở, Luận văn Thạc sỹ khoa học, Trường Đại học Bách Khoa Hà Nội, 2011 22 Nguyễn Quang Huy, Nghiên cứu động học cân robot hai chân, Luận văn Thạc sỹ, Trường Đại học Bách Khoa Hà Nội, 2010 23 Nguyễn Văn Khang, Điều khiển robot song song, Báo cáo nghiên cứu, Trường đại học Bách khoa Hà Nội 24 Nguyễn Văn Khang, Trịnh Quốc Trung, Điều khiển chuyển động robot hai chân pha chân trụ theo nguyên lý trượt sử dụng mạng nơ ron, gửi tạp chí Tin học Điều khiển học 120 PHỤ LỤC 1: TÍNH TỐN ĐỘNG LỰC HỌC q=sym('q',[1 5]); dq=sym('dq',[1 5]); syms q1 q2 q3 q4 q5 syms dq1 dq2 dq3 dq4 dq5 syms a1 a2 a3 a4 a5 syms l1 l2 l3 l4 l5 syms m1 m2 m3 m4 m5 syms i1 i2 i3 i4 i5 syms g r1 = [a1*sin(q1); 0; a1*cos(q1)]; Jt1 = jacobian(r1,q); r2 = [l1*sin(q1)+a2*sin(q2); 0; l1*cos(q1)+a2*cos(q2)]; Jt2 = jacobian(r2,q); r3 = [l1*sin(q1)+l2*sin(q2)+a3*sin(q3); 0; l1*cos(q1)+l2*cos(q2)+a3*cos(q3)]; Jt3 = jacobian(r3,q); r4 = [l1*sin(q1)+l2*sin(q2)-(l4-a4)*sin(q4); 0; l1*cos(q1)+l2*cos(q2)-(l4-a4)*cos(q4)]; Jt4 = jacobian(r4,q); r5 = [l1*sin(q1)+l2*sin(q2)-l4*sin(q4)-(l5-a5)*sin(q5); 0; l1*cos(q1)+l2*cos(q2)-l4*cos(q4)-(l5-a5)*cos(q5)]; Jt5 = jacobian(r5,q); o1 = [0; dq1; 0]; Jr1 = jacobian(o1, dq); o2 = [0; dq2; 0]; Jr2 = jacobian(o2, dq); o3 = [0; dq3; 0]; Jr3 = jacobian(o3, dq); o4 = [0; -dq4; 0]; Jr4 = jacobian(o4, dq); o5 = [0; -dq5; 0]; Jr5 = jacobian(o5, dq); I1 = [i1 0 0; 0 0 0; 0 0 0; 0 0 0; 0 0 0]; I2 = [0 0 0; i2 0 0; 0 0 0; 0 0 0; 0 0 0]; I3 = [0 0 0; 121 0 0 I4 = [0 0 0 I5 = [0 0 0 S1 S2 S3 S4 S5 = = = = = 0 0 0 0 0 0 0 0 0; i3 0; 0 0; 0 0]; 0 0; 0 0; 0 0; i4 0; 0 0]; 0 0; 0 0; 0 0; 0 0; 0 i5]; m1*simplify(Jt1.'*Jt1); m2*simplify(Jt2.'*Jt2); m3*simplify(Jt3.'*Jt3); m4*simplify(Jt4.'*Jt4); m5*simplify(Jt5.'*Jt5); M = S1 + S2 + S3 + S4 + S5 + I1 + I2 + I3 + I4 + I5; C1 = simplify(matrixC(M,q,dq)); H1 = simplify(C1*dq.'); C2 = simplify(matrixC2(M)); H2 = simplify(C2*dq.'); %=======================================================================% function [DHList,RList,JtList,OList_0,OList_i,JrList,M,C,G,F] = ListData(file_dh,file_r,file_m,file_i,file_g,file_f,type) % Khoi tao % Type: Kieu tinh matrix C(q,dq) DHList = []; RList = []; JtList = []; OList_0 = []; OList_i = []; JrList = []; M = []; C = []; G = []; F = []; if file_dh ~= DHTb = file2sym(file_dh);% [m,~] = size(DHTb); T = cell(1,m); Tp = cell(1,m); Temp = eye(4); for i = 1:m Temp = Temp*DH(DHTb(i,:)); T{i} = simplify(Temp); Tp{i} = T{i}(1:3,1:3); end DHList = T; DHTmp = Tp; 122 T_0 = cell(1,m); T_i = cell(1,m); q = sym('q',[1 m]); dq = sym('dq', [1 m]); for i = 1:m dA = diffT(DHList{i}(1:3,1:3),q,dq); A = DHList{i}(1:3,1:3); Omega_0 = simplify(dA*A.'); T_0{i}=[-Omega_0(2,3) Omega_0(1,3) -Omega_0(1,2)]; Omega_i = simplify(A.'*dA); T_i{i}=[-Omega_i(2,3) Omega_i(1,3) -Omega_i(1,2)]; end OList_0 = T_0; OList_i = T_i; if file_r ~= RTb = file2sym(file_r); T = cell(1,m); for i = 1:m T{i} = simplify(DHList{i}*RTb(i,:).'); end RList = T; T = cell(1,m); for i = 1:m T{i} = simplify(jacobian(RList{i}(1:3),q)); end JtList = T; T = cell(1,m); for i = 1:m T{i} = jacobian(OList_0{i},dq); end JrList = T; if and(file_m ~= 0,file_i ~= 0) mTb = file2sym(file_m); iTb = file2sym(file_i); c = ones(1,m); iTb = mat2cell(iTb, 3*c); M = matrixM(JtList, JrList, mTb, iTb, DHTmp); if type == C = matrixC(M,q,dq); else C = matrixC2(M); end if file_g ~= mG = file2sym(file_g); Po = 0; for i = 1:m Po = Po + mTb(i)*mG*RList{i}(1:3); end G = jacobian(Po,q).'; if file_f ~= mF = file2sym(file_f); Rq = jacobian(DHList{m}(1:3,4),q); T = sym(zeros(m,1)); for i = 1:m T(i) = mF*Rq(:,i); 123 end F = T; disp('Matrix F is available'); else disp('file_f is invalid'); end disp('Matrix G is available'); else disp('file_g is invalid'); end disp('Matrix Jt and Jo are available'); else disp('file_m or file_i is invalid'); end disp('Matrix R is available'); else disp('file_r is invalid'); end disp('Matrix DH is available'); else disp('file_dh is invalid'); end 124 PHỤ LỤC 2: GIẢI BÀI TOÁN ĐỘNG LỰC HỌC NGƯỢC clear all; close all; mode = 2; if mode == fid_q = fopen('data_q_st.txt'); fid_dq = fopen('data_dq_st.txt'); fid_d2q = fopen('data_d2q_st.txt'); qi = fscanf(fid_q,'%f',[4 inf]); dqi = fscanf(fid_dq,'%f',[4 inf]); d2qi = fscanf(fid_d2q,'%f',[4 inf]); [~,N] = size(qi); fclose(fid_q); fclose(fid_dq); fclose(fid_d2q); elseif mode == fid_q = fopen('data_q_ssp.txt'); fid_dq = fopen('data_dq_ssp.txt'); fid_d2q = fopen('data_d2q_ssp.txt'); qi = fscanf(fid_q,'%f',[4 inf]); dqi = fscanf(fid_dq,'%f',[4 inf]); d2qi = fscanf(fid_d2q,'%f',[4 inf]); [~,N] = size(qi); fclose(fid_q); fclose(fid_dq); fclose(fid_d2q); end % Gia toc truong g = 9.81; %[m/s^2] % Khoi luong m1 = 2.23; %[kg] m2 = 5.28; %[kg] m3 = 14.79; %[kg] m4 = 5.28; %[kg] m5 = 2.23; %[kg] % Chieu dai l1 = 0.332; %[m] l2 = 0.302; %[m] l3 = 0.486; %[m] l4 = 0.302; %[m] l5 = 0.332; %[m] % Trong tam a1 = 0.189; %[m] a2 = 0.236; %[m] a3 = 0.282; %[m] a4 = 0.236; %[m] a5 = 0.189; %[m] % Moment quan tinh i1 = 3.3*10^-2; %[kgm^2] i2 = 3.3*10^-2; %[kgm^2] i3 = 3.3*10^-2; %[kgm^2] i4 = 3.3*10^-2; %[kgm^2] i5 = 3.3*10^-2; %[kgm^2] qi1 = qi(1,:); qi2 = qi(2,:); 125 qi3 = qi(3,:); qi4 = qi(4,:); dqi1 dqi2 dqi3 dqi4 = = = = d2qi1 d2qi2 d2qi3 d2qi4 dqi(1,:); dqi(2,:); dqi(3,:); dqi(4,:); = = = = d2qi(1,:); d2qi(2,:); d2qi(3,:); d2qi(4,:); syms q0 q1 q2 q3 q4 dq0 dq1 dq2 dq3 dq4 d2q0 d2q1 d2q2 d2q3 d2q4 R = [1 1 1 0 0; -1 0 0; -1 -1 0; -1 -1 0; -1 -1 1]; theta = num2cell(R.'*[q0 q1 q2 q3 q4].'); [theta1 theta2 theta3 theta4 theta5] = theta{:}; dtheta = num2cell(R.'*[dq0 dq1 dq2 dq3 dq4].'); [dtheta1 dtheta2 dtheta3 dtheta4 dtheta5] = dtheta{:}; d2theta = num2cell(R.'*[d2q0 d2q1 d2q2 d2q3 d2q4].'); [d2theta1 d2theta2 d2theta3 d2theta4 dt2heta5] = d2theta{:}; p12 = m2*a2*l1+(m3+m4+m5)*l1*l2; p13 = m3*l1*a3; p14 = m4*l1*(l4-a4)+m5*l1*l4; p15 = m5*l1*(l5-a5); p23 = m3*l2*a3; p24 = m4*l2*(l4-a4) + m5*l2*l4; p25 = m5*l2*(l5-a5); p45 = m5*l4*(l5-a5); % m(q) m11 = i1 + m1*a1^2 + (m2+m3+m4+m5)*l1^2; m12 = p12*cos(theta1-theta2); m13 = p13*cos(theta1-theta3); m14 = -p14*cos(theta1-theta4); m15 = -p15*cos(theta1-theta5); m21 m22 m23 m24 m25 = = = = = m12; i2 + m2*a2^2 + (m3+m4+m5)*l2^2; p23*cos(theta2-theta3); -p24*cos(theta2-theta4); -p25*cos(theta2-theta5); m31 = m13; m32 = m23; m33 = i3 + m3*a3^2; 126 m34 = 0; m35 = 0; m41 = m14;m42 = m24;m43 = m34; m44 = i4 + m4*(l4-a4)^2 + m5*l4^2; m45 = p45*cos(theta4-theta5); m51 = m15;m52 = m25;m53 = m35;m54 = m45; m55 = i5 + m5*(l5-a5)^2; M = [m11 m12 m13 m14 m15; m21 m22 m23 m24 m25; m31 m32 m33 m34 m35; m41 m42 m42 m44 m45; m51 m52 m53 m54 m55]; % % h(q,dq) % h1 = p12*dtheta2.^2.*sin(theta1-theta2) + p13*dtheta3.^2.*sin(theta1theta3) - p14*dtheta4.^2.*sin(theta1-theta4) p15*dtheta5.^2.*sin(theta1-theta5); h2 = -p12*dtheta1.^2.*sin(theta1-theta2) + p23*dtheta3.^2.*sin(theta2theta3) - p24*dtheta4.^2.*sin(theta2-theta4) p25*dtheta5.^2.*sin(theta2-theta5); h3 = -p13*dtheta1.^2.*sin(theta1-theta3) - p23*dtheta2.^2.*sin(theta2theta3); h4 = p14*dtheta1.^2.*sin(theta1-theta4) + p24*dtheta2.^2.*sin(theta2theta4) + p45*dtheta5.^2.*sin(theta4-theta5); h5 = p15*dtheta1.^2.*sin(theta1-theta5) + p25*dtheta2.^2.*sin(theta2theta5) - p45*dtheta4.^2.*sin(theta4-theta5); H = [h1 h2 h3 h4 h5].'; % % g(q) % g1 = -(m1*a1 + (m2+m3+m4+m5)*l1)*g*sin(theta1); g2 = -(m2*a2 + (m3+m4+m5)*l2)*g*sin(theta2); g3 = -m3*a3*g*sin(theta3); g4 = (m4*(l4-a4) + m5*l4)*g*sin(theta4); g5 = m5*(l5-a5)*g*sin(theta5); g = [g1 g2 g3 g4 g5].'; % % M(q) % Mq = num2cell(R.'*M*R); [mq11 [mq21 [mq31 [mq41 [mq51 mq12 mq22 mq32 mq42 mq52 mq13 mq23 mq33 mq43 mq53 mq14 mq24 mq34 mq44 mq54 mq15] mq25] mq35] mq45] mq55] = = = = = Mq{1,:}; Mq{2,:}; Mq{3,:}; Mq{4,:}; Mq{5,:}; Hq = num2cell(R.'*H); [hq1 hq2 hq3 hq4 hq5] = Hq{:}; gq = num2cell(R.'*g); [gq1 gq2 gq3 gq4 gq5] = gq{:}; % 127 %Runge – Kutta % h = 2*0.001; MAX = round(N/2); qi0 = zeros(MAX,1); dqi0 = zeros(MAX,1); d2qi0 = zeros(MAX,1); if mode == qi0(1) = 0; dqi0(1) = 0; elseif mode == qi0(1) = 0.265; dqi0(1) = 0; end f = [dq0,-([mq12 mq13 mq14 mq15]*[d2q1 d2q2 d2q3 d2q4].' + hq1 + gq1)/mq11]; d2qi0(1) = subs(f(2),{q0,q1,q2,q3,q4, dq0,dq1,dq2,dq3,dq4, d2q1,d2q2,d2q3,d2q4}, {qi0(1),qi1(1),qi2(1),qi3(1),qi4(1), dqi0(1),dqi1(1),dqi2(1),dqi3(1),dqi4(1), d2qi1(1),d2qi2(1),d2qi3(1),d2qi4(1)}); if mode == fid_q0_st = fopen('data_q0_st.txt','w'); fid_dq0_st = fopen('data_dq0_st.txt','w'); fid_d2q0_st = fopen('data_d2q0_st.txt','w'); fprintf(fid_q0_st,'%f\n',qi0(1)); fprintf(fid_dq0_st,'%f\n',dqi0(1)); fprintf(fid_d2q0_st,'%f\n',d2qi0(1)); elseif mode == fid_q0_ssp = fopen('data_q0_ssp.txt','w'); fid_dq0_ssp = fopen('data_dq0_ssp.txt','w'); fid_d2q0_ssp = fopen('data_d2q0_ssp.txt','w'); fprintf(fid_q0_ssp,'%f\n',qi0(1)); fprintf(fid_dq0_ssp,'%f\n',dqi0(1)); fprintf(fid_d2q0_ssp,'%f\n',d2qi0(1)); end for i=1:MAX-1 k1 = subs(f,{q0,q1,q2,q3,q4, dq0,dq1,dq2,dq3,dq4, d2q1,d2q2,d2q3,d2q4}, {qi0(i),qi1(2*i-1),qi2(2*i-1),qi3(2*i-1),qi4(2*i-1), dqi0(i),dqi1(2*i-1),dqi2(2*i-1),dqi3(2*i-1),dqi4(2*i1), d2qi1(2*i-1),d2qi2(2*i-1),d2qi3(2*i-1),d2qi4(2*i-1)}); k2 = subs(f,{q0,q1,q2,q3,q4, dq0,dq1,dq2,dq3,dq4, d2q1,d2q2,d2q3,d2q4}, {qi0(i)+h/2*k1(1),qi1(2*i),qi2(2*i),qi3(2*i),qi4(2*i), dqi0(i)+h/2*k1(2),dqi1(2*i),dqi2(2*i),dqi3(2*i),dqi4(2*i), d2qi1(2*i),d2qi2(2*i),d2qi3(2*i),d2qi4(2*i)}); k3 = subs(f,{q0,q1,q2,q3,q4, dq0,dq1,dq2,dq3,dq4, 128 d2q1,d2q2,d2q3,d2q4}, {qi0(i)+h/2*k2(1),qi1(2*i),qi2(2*i),qi3(2*i),qi4(2*i), dqi0(i)+h/2*k2(2),dqi1(2*i),dqi2(2*i),dqi3(2*i),dqi4(2*i), d2qi1(i),d2qi2(i),d2qi3(i),d2qi4(i)}); k4 = subs(f,{q0,q1,q2,q3,q4, dq0,dq1,dq2,dq3,dq4, d2q1,d2q2,d2q3,d2q4}, {qi0(i)+h*k3(1),qi1(2*i+1),qi2(2*i+1),qi3(2*i+1),qi4(2*i+1), dqi0(i)+h*k3(2),dqi1(2*i+1),dqi2(2*i+1),dqi3(2*i+1),dqi4(2*i+1), d2qi1(2*i+1),d2qi2(2*i+1),d2qi3(2*i+1),d2qi4(2*i+1)}); qi0(i+1) = qi0(i) + h/6*(k1(1)+2*k2(1)+2*k3(1)+k4(1)); dqi0(i+1) = dqi0(i) + h/6*(k1(2)+2*k2(2)+2*k3(2)+k4(2)); d2qi0(i+1) = subs(f(2),{q0,q1,q2,q3,q4, dq0,dq1,dq2,dq3,dq4, d2q1,d2q2,d2q3,d2q4}, {qi0(i+1),qi1(2*i+1),qi2(2*i+1),qi3(2*i+1),qi4(2*i+1), dqi0(i+1),dqi1(2*i+1),dqi2(2*i+1),dqi3(2*i+1),dqi4(2*i+1), d2qi1(2*i+1),d2qi2(2*i+1),d2qi3(2*i+1),d2qi4(2*i+1)}); if mode == fprintf(fid_q0_st,'%f\n',qi0(i+1)); fprintf(fid_dq0_st,'%f\n',dqi0(i+1)); fprintf(fid_d2q0_st,'%f\n',d2qi0(i+1)); elseif mode == fprintf(fid_q0_ssp,'%f\n',qi0(i+1)); fprintf(fid_dq0_ssp,'%f\n',dqi0(i+1)); fprintf(fid_d2q0_ssp,'%f\n',d2qi0(i+1)); end end if mode == fclose(fid_q0_st); fclose(fid_dq0_st); fclose(fid_d2q0_st); elseif mode == fclose(fid_q0_ssp); fclose(fid_dq0_ssp); fclose(fid_d2q0_ssp); end to = R.'*(M*R*[d2q0 d2q1 d2q2 d2q3 d2q4].' + H + g); To = zeros(MAX,4); for i = 1:MAX temp = subs(to(2:5),{q0,q1,q2,q3,q4, dq0,dq1,dq2,dq3,dq4, d2q0,d2q1,d2q2,d2q3,d2q4}, {qi0(i),qi1(2*i-1),qi2(2*i-1),qi3(2*i-1),qi4(2*i-1), dqi0(i),dqi1(2*i-1),dqi2(2*i-1),dqi3(2*i-1),dqi4(2*i-1), d2qi0(i),d2qi1(2*i-1),d2qi2(2*i-1),d2qi3(2*i-1),d2qi4(2*i1)}); To(i,1) = temp(1); To(i,2) = temp(2); 129 To(i,3) = temp(3); To(i,4) = temp(4); end figure; if mode == t = 0.:h:.5; subplot(3,1,1); plot(t,qi0,'r','LineWidth',2);grid on;set(gca,'GridLineStyle',' '); ylabel('q_0 [rad]','fontsize',12,'FontName','Times New Roman'); subplot(3,1,2); plot(t,dqi0,'r','LineWidth',2);grid on;set(gca,'GridLineStyle',' '); ylabel('dq_0 [rad/s]','fontsize',12,'FontName','Times New Roman'); subplot(3,1,3); plot(t,d2qi0,'r','LineWidth',2);grid on;set(gca,'GridLineStyle','-'); ylabel('d2q_0 [rad/s^2]','fontsize',12,'FontName','Times New Roman'); print -painters -dbmp -r300 plot_q0_st.bmp elseif mode == t = 0.5:h:1.0; subplot(3,1,1); plot(t-.5,qi0,'r','LineWidth',2);grid on;set(gca,'GridLineStyle','-'); ylabel('q_0 [rad]','fontsize',12,'FontName','Times New Roman'); subplot(3,1,2); plot(t-.5,dqi0,'r','LineWidth',2);grid on;set(gca,'GridLineStyle','-'); ylabel('dq_0 [rad/s]','fontsize',12,'FontName','Times New Roman'); subplot(3,1,3); plot(t-.5,d2qi0,'r','LineWidth',2);grid on;set(gca,'GridLineStyle','-'); ylabel('d2q_0 [rad/s^2]','fontsize',12,'FontName','Times New Roman'); print -painters -dbmp -r300 plot_q0_ssp.bmp end figure; if mode == t = 0.:h:.5; subplot(2,2,1); plot(t,To(:,1),'r','LineWidth',2);grid on;set(gca,'GridLineStyle','-'); ylabel('\tau_1 [Nm]','fontsize',12,'FontName','Times New Roman'); subplot(2,2,2); plot(t,To(:,2),'r','LineWidth',2);grid on;set(gca,'GridLineStyle','-'); ylabel('\tau_2 [Nm]','fontsize',12,'FontName','Times New Roman'); subplot(2,2,3); plot(t,To(:,3),'r','LineWidth',2);grid on;set(gca,'GridLineStyle','-'); ylabel('\tau_3 [Nm]','fontsize',12,'FontName','Times New Roman'); subplot(2,2,4); plot(t,To(:,4),'r','LineWidth',2);grid on;set(gca,'GridLineStyle','-'); ylabel('\tau_4 [Nm]','fontsize',12,'FontName','Times New Roman'); 130 print -painters -dbmp -r300 plot_t_st.bmp elseif mode == t = 0.5:h:1.0; subplot(2,2,1); plot(t-.5,To(:,1),'r','LineWidth',2);grid on;set(gca,'GridLineStyle',' '); ylabel('\tau_1 [Nm]','fontsize',12,'FontName','Times subplot(2,2,2); plot(t-.5,To(:,2),'r','LineWidth',2);grid on;set(gca,'GridLineStyle',' '); ylabel('\tau_2 [Nm]','fontsize',12,'FontName','Times subplot(2,2,3); plot(t-.5,To(:,3),'r','LineWidth',2);grid on;set(gca,'GridLineStyle',' '); ylabel('\tau_3 [Nm]','fontsize',12,'FontName','Times subplot(2,2,4); plot(t-.5,To(:,4),'r','LineWidth',2);grid on;set(gca,'GridLineStyle',' '); ylabel('\tau_4 [Nm]','fontsize',12,'FontName','Times print -painters -dbmp -r300 plot_t_ssp.bmp end 131 New Roman'); New Roman'); New Roman'); New Roman'); Mô hình Simulink điều khiển PD PHỤ LỤC 3: MƠ HÌNH SIMULINK 132 133 Mơ hình Simulink điều khiển trượt 134 Mơ hình Simulink điều khiển Nơron ... ĐI? ??U KHIỂN CHUYỂN ĐỘNG CỦA ROBOT HAI CHÂN TRONG PHA MỘT CHÂN TRỤ 87 4.1 Đi? ??u khiển PD .88 4.1.1 Đi? ??u khiển PD robot dạng chuỗi 88 4.1.2 Đi? ??u khiển PD cho robot hai. .. chương Chương 1: Một số khái niệm robot hai chân Chương 2: Động học ngược robot hai chân Chương 3: Động lực học ngược robot hai chân Chương 4: Đi? ??u khiển robot hai chân Luận văn hoàn thành hướng... tốn động lực học, động lực học ngược đi? ??u khiển robot dạng có đi? ??m thú vị, thách thức Ngồi robot hai chân loại robot có khả lại, tức liên quan tới loại mobile robot Việc nghiên cứu động lực học,