Ứng dụng Maple trong tính toán rô bốt

Robotics Ứng dụng Maple trong tính toán rô bốt 1. Xử lý file và thể hiện số liệu -Tạo 1 thư mục (folder) để làm việc -Lưu 1 file maple (ví dụ: xulyfile.mw) vào thư mục trên -Chương trình chạy sẽ tự động tạo file trong thư mục này -Dữ liệu thường lưu trữ dạng file text (.txt) dễ đọc và xử lý, số liệu lưu đơn giản dạng ma trận số, lấy dữ liệu ra dạng ma trận số và xử lý ma trận số này. 1. Xử lý file -Mở file dữ liệu, viết dữ liệu, lưu dữ liệu. -Mở file dữ liệu, đọc dữ liệu (lấy dữ liệu từ file lưu trữ) 1.1.Xử lý file văn bản Nguyên tắc: Tạo file mới (mở file)->Ghi dữ liệu lên file->Đóng file Lệnh mở file: fopen Cụ thể: HEPL fopen(name, mode) fopen(name, mode, type) Viết lên file: fprintf Cụ thể: HEPL Đóng file: close Ví dụ: >fh := fopen("xuly.txt", WRITE) >fprintf(fh, "x=%d,y=%d", 1, 2) >fclose(fh) 1.2.Xử lý file dạng số Nguyên tắc: Tạo file mới (mở file)->Ghi dữ liệu lên file->Đóng file Lệnh mở file: fopen Viết dữ liệu dạng số lên file: writedata Lệnh đọc dữ liệu: readdata Lệnh đóng file: fclose Ví dụ viết dữ liệu: >A := matrix([[1, 2, 3, 90], [6, 8, 80, 56], [1, 5, 8, 3]]) >fh := fopen("matrandulieu.txt", WRITE) >writedata(fh, A, float) >fclose(fh) Muốn mở và lưu file vào thư mục khác: fh := fopen("D:/study/fileMOI.txt", WRITE) fclose(fh) Ví dụ đọc dữ liệu: > fh := fopen("matrandulieu.txt", READ);                                       0 > datalayra := readdata(fh, float, 4);           [[1., 2., 3., 90.], [6., 8., 80., 56.], [1., 5., 8., 3.]] > datalayra;           [[1., 2., 3., 90.], [6., 8., 80., 56.], [1., 5., 8., 3.]] > datalayra[1, 1];                                      1. > fclose(fh); 2. Hiển thị số liệu dạng số 2.1 Tạo file dữ liệu Ví dụ: > matran := matrix(100, 2); > for i from 1to 100 by 1 do  matran[i, 1] := i: matran[i, 2] := i^2:  end do; > fh := fopen("thehiensolieu.txt", WRITE);                                       0 > writedata(fh, matran, integer); > fclose(fh); 2.2 Thể hiện dữ liệu - đọc file: readdata fh := fopen("thehiensolieu.txt", READ) -Lưu thành ma trận matransolieu := readdata(fh, integer, 2) -Vẽ đồ thị các điểm riêng rẽ > points := {seq([matransolieu[i, 1], matransolieu[i, 2]], i = 1 .. 100)}; > pointplot(points, color = red, thickness = 3); II. HƯỚNG DẪN GIẢI PHƯƠNG TRÌNH ĐỘNG HỌC BẰNG MAPLE 1. Khai báo thư viện sử dụng with(linalg); with(VectorCalculus); with(LinearAlgebra); with(plots); with(plottools): 2. Thiết lập phương trình động học -Bằng cách gán ma trận trạng thái khâu thao tác A theo tọa độ thao tác p=[xE,yE,zE,rotx,roty,rotz]T và ma trận trạng thái khâu thao tác A theo cấu trúc động học q=[q1,q2,zE,,qn]T. Ta được phương trình động học. -Sau đây là 1 ví dụ phương trình động học: f1:=0.28-(.61*(cos(q2)*cos(q3)-sin(q2)*sin(q3)))*cos(q4)*sin(q5)+(.61*(-cos(q2)*sin(q3)-sin(q2)*cos(q3)))*cos(q5)-.86*cos(q2)*sin(q3)-.86*sin(q2)*cos(q3)+.7*cos(q2)-xE: f2:=-0.61*(cos(q2)*sin(q3)+sin(q2)*cos(q3)))*cos(q4)*sin(q5)+(.61*(cos(q2)*cos(q3)-sin(q2)*sin(q3)))*cos(q5)-.86*sin(q2)*sin(q3)+.86*cos(q2)*cos(q3)+.7*sin(q2)-yE: f3:=-0.61*sin(q4)*sin(q5)+q1-zE: f4:=-((cos(q2)*cos(q3)-sin(q2)*sin(q3))*cos(q4)*cos(q5)+(-cos(q2)*sin(q3)-sin(q2)*cos(q3))*sin(q5))*sin(q6)-(cos(q2)*cos(q3)-sin(q2)*sin(q3))*sin(q4)*cos(q6)+cos(roty)*sin(rotz): f5:=(cos(q2)*sin(q3)+sin(q2)*cos(q3))*cos(q4)*sin(q5)-(cos(q2)*cos(q3)-sin(q2)*sin(q3))*cos(q5)+sin(rotx)*cos(roty): f6:=sin(q4)*cos(q5)*cos(q6)+cos(q4)*sin(q6)+cos(rotx)*sin(roty)*cos(rotz)-sin(rotx)*sin(rotz): f := [f1, f2, f3, f4, f5, f6]: 3. Giải bài toán thuận 3.1. Đầu vào bài toán thuận - Cho trước q=[q1,q2,q3,,qn]T (chú ý sv cho phải phù hợp với cấu hình) Ví dụ: q1:=1+0.8*sin(Pi/4*t): q2:=-0.5+0.2*sin(Pi/5*t): q3:=0.3-0.5*sin(Pi/6*t): q4:=0.2+0.3*sin(Pi/3*t): q5:=-0.4-0.3*sin(Pi/4*t): q6:=0.1+0.4*sin(Pi/5*t): q := [q1, q2, q3, q4, q5, q6]; -Biến (đầu ra bài toán thuận) unknow := [xE, yE, zE, rotx, roty, rotz]; 3.2. Cách giải - Tìm nghiệm đầu unknowinit qinit := evalf(subs(t = 0, q)); f0 := evalf(subs(t = 0, f)); unknow0 := solve(f0, unknow); assign(unknow0[10]); unknowinit := [xE, yE, zE, rotx, roty, rotz]; unassign('xE', 'yE', 'zE', 'rotx', 'roty', 'rotz'); -Dùng thuật giải NewtonRaphson để giải read "NewtonRS.txt"; L := 100: nghiemthuan := matrix(L, 6): nghiemthuan[1, 1] := unknowinit[1]: nghiemthuan[1, 2] := unknowinit[2]: nghiemthuan[1, 3] := unknowinit[3]: nghiemthuan[1, 4] := unknowinit[4]: nghiemthuan[1, 5] := unknowinit[5]: nghiemthuan[1, 6] := unknowinit[6]:  for i from 1 to L-1 by 1 do Eqts := subs(t = VectorCalculus[`*`](.1, i), f): jaco := jacobian(f, unknow): y := Newton_Raphson(Eqts, jaco, unknow, unknowinit): nghiemthuan[i+1, 1] := evalf(y[1]): nghiemthuan[i+1, 2] := evalf(y[2]): nghiemthuan[i+1, 3] := evalf(y[3]): nghiemthuan[i+1, 4] := evalf(y[4]): nghiemthuan[i+1, 5] := evalf(y[5]): nghiemthuan[i+1, 6] := evalf(y[6]): end do: -Lưu kết quả bài toán thuận (lưu thành cột” xE yE zE rotx roty rotz”)vào file  nghiembaitoanthuan.txt fh := fopen("nghiembaitoanthuan.txt", WRITE); writedata(fh, nghiemthuan, float); fclose(fh); -Đọc dữ liệu    fh := fopen("nghiembaitoanthuan.txt", READ); vethuan := readdata(fh, float, 6); fclose(fh); -Lập tọa độ điểm tác động cuối theo thời gian t    pointxE := [seq(0.1*i, vethuan[i, 1]], i = 1 .. 100)]: pointyE := [seq(0.1*i, vethuan[i, 2]], i = 1 .. 100)]: pointzE := [seq(0.1*i, vethuan[i, 3]], i = 1 .. 100)]: -Vẽ tọa độ điểm tác động cuối (thành phần tọa độ và tọa độ 3D)    point1 := pointplot(pointxE, color = red, legend = "xE"); point2 := pointplot(pointyE, color = green, legend = "yE"); point3 := pointplot(pointzE, color = blue, legend = "zE"); display({point1, point2, point3}); point3D := [seq([vethuan[i, 1], vethuan[i, 2], vethuan[i, 3]], i = 1 .. 100)]: pointplot3d(point3D, axes = normal): III. HƯỚNG DẪN TÍNH TOÁN TĨNH HỌC ROBOT BẰNG MAPLE 1.Khai báo thư viện sử dụng > restart; > with(linalg); > with(LinearAlgebra); 2.Khai báo ma trận DH Tổng quát > A := Matrix([[cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta)], [sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta)], [0, sin(alpha), cos(alpha), d], [0, 0, 0, 1]]); 3.Tính ma trận DH các khâu > A01 := subs({a = 0, d = d1, alpha = (1/2)*Pi, theta = q1}, A); > A12 := subs({a = a2, d = 0, alpha = 0, theta = q2}, A); > A23 := subs({a = a3, d = 0, alpha = 0, theta = q3}, A); > A02 := MatrixMatrixMultiply(A01, A12); > A03 := MatrixMatrixMultiply(A02, A23); 4.Tính ma trận R0i R01 := Matrix(3, 3); for i to 3 do for j to 3 do R01[i, j] := A01[i, j] end do end do; R02 := Matrix(3, 3); for i to 3 do for j to 3 do R02[i, j] := A02[i, j] end do end do; R03 := Matrix(3, 3); for i to 3 do for j to 3 do R03[i, j] := A03[i, j] end do end do; 5.Tính ma trận rii và rici rj := Vector([xj, yj, zj]); rsong := Matrix([[0, -zj, yj], [zj, 0, -xj], [-yj, xj, 0]]); r11 := subs({xj = 0, yj = -d1, zj = 0}, rj); r22 := subs({xj = -a2, yj = 0, zj = 0}, rj); r33 := subs({xj = -a3, yj = 0, zj = 0}, rj); rc11 := subs({xj = 0, yj = -(1/2)*d1, zj = 0}, rj); rc22 := subs({xj = -(1/2)*a2, yj = 0, zj = 0}, rj); rc33 := subs({xj = -(1/2)*a3, yj = 0, zj = 0}, rj); r1 := MatrixVectorMultiply(R01, r11); r2 := MatrixVectorMultiply(R02, r22); r3 := MatrixVectorMultiply(R03, r33); rc1 := MatrixVectorMultiply(R01, rc11); rc2 := MatrixVectorMultiply(R02, rc22); rc3 := MatrixVectorMultiply(R03, rc33); 6.Tính ma trận rii song và rici sóng r1song := subs({xj = r1(1, 1), yj = r1(2, 1), zj = r1(3, 1)}, rsong); r2song := subs({xj = r2(1, 1), yj = r2(2, 1), zj = r2(3, 1)}, rsong); r3song := subs({xj = r3(1, 1), yj = r3(2, 1), zj = r3(3, 1)}, rsong); rc1song := subs({xj = rc1(1, 1), yj = rc1(2, 1), zj = rc1(3, 1)}, rsong); rc2song := subs({xj = rc2(1, 1), yj = rc2(2, 1), zj = rc2(3, 1)}, rsong); rc3song := subs({xj = rc3(1, 1), yj = rc3(2, 1), zj = rc3(3, 1)}, rsong); 7.Các giá trị đầu vào F := Vector([Fx, Fy, Fz]); P1 := subs({Fx = 0, Fy = 0, Fz = -m1*g}, F); P2 := subs({Fx = 0, Fy = 0, Fz = -m2*g}, F); P3 := subs({Fx = 0, Fy = 0, Fz = -m3*g}, F); F43 := subs({Fx = fx, Fy = fy, Fz = fz}, F); M43 := subs({Fx = mx, Fy = my, Fz = mz}, F); 8.Tính lực tác dụng tại các khớp rô bốt F32 := F43-P3; M32 := M43-MatrixVectorMultiply(r3song, F32)-MatrixVectorMultiply(rc3song, P3); F21 := F32-P2; M21 := M32-MatrixVectorMultiply(r2song, F21)-MatrixVectorMultiply(rc2song, P2); F10 := F21-P1; M10 := M21-MatrixVectorMultiply(r1song, F10)-MatrixVectorMultiply(rc1song, P1); 9.Một trường hợp khảo sát cụ thể M10ve := subs({a2 = .7, a3 = .5, d1 = 1, fx = 50, fy = 70, fz = 90, g = 9.81, m1 = 5.5, m2 = 3.5, m3 = 1.5, mx = 50, my = 70, mz = 130, q1 = (1/2)*Pi*sin(Pi*t-(1/6)*Pi), q2 = (1/3)*Pi*cos(Pi*t+(1/3)*Pi), q3 = Pi+(1/4)*Pi*sin(Pi*t+(1/4)*Pi)}, M10); M21ve := subs({a2 = .7, a3 = .5, d1 = 1, fx = 50, fy = 70, fz = 90, g = 9.81, m1 = 5.5, m2 = 3.5, m3 = 1.5, mx = 50, my = 70, mz = 130, q1 = (1/2)*Pi*sin(Pi*t-(1/6)*Pi), q2 = (1/3)*Pi*cos(Pi*t+(1/3)*Pi), q3 = Pi+(1/4)*Pi*sin(Pi*t+(1/4)*Pi)}, M21); M32ve := subs({a2 = .7, a3 = .5, d1 = 1, fx = 50, fy = 70, fz = 90, g = 9.81, m1 = 5.5, m2 = 3.5, m3 = 1.5, mx = 50, my = 70, mz = 130, q1 = (1/2)*Pi*sin(Pi*t-(1/6)*Pi), q2 = (1/3)*Pi*cos(Pi*t+(1/3)*Pi), q3 = Pi+(1/4)*Pi*sin(Pi*t+(1/4)*Pi)}, M32); F10ve := subs({a2 = .7, a3 = .5, d1 = 1, fx = 50, fy = 70, fz = 90, g = 9.81, m1 = 5.5, m2 = 3.5, m3 = 1.5, mx = 50, my = 70, mz = 130, q1 = (1/2)*Pi*sin(Pi*t-(1/6)*Pi), q2 = (1/3)*Pi*cos(Pi*t+(1/3)*Pi), q3 = Pi+(1/4)*Pi*sin(Pi*t+(1/4)*Pi)}, F10); F21ve := subs({a2 = .7, a3 = .5, d1 = 1, fx = 50, fy = 70, fz = 90, g = 9.81, m1 = 5.5, m2 = 3.5, m3 = 1.5, mx = 50, my = 70, mz = 130, q1 = (1/2)*Pi*sin(Pi*t-(1/6)*Pi), q2 = (1/3)*Pi*cos(Pi*t+(1/3)*Pi), q3 = Pi+(1/4)*Pi*sin(Pi*t+(1/4)*Pi)}, F21); F32ve := subs({a2 = .7, a3 = .5, d1 = 1, fx = 50, fy = 70, fz = 90, g = 9.81, m1 = 5.5, m2 = 3.5, m3 = 1.5, mx = 50, my = 70, mz = 130, q1 = (1/2)*Pi*sin(Pi*t-(1/6)*Pi), q2 = (1/3)*Pi*cos(Pi*t+(1/3)*Pi), q3 = Pi+(1/4)*Pi*sin(Pi*t+(1/4)*Pi)}, F32); plot(M10ve, t = .1 .. 1); plot(M21ve, t = .1 .. 1); plot(M32ve, t = .1 .. 1); plot(F10ve, t = .1 .. 1); IV. VÍ DỤ VỀ TÍNH ĐỘNG LỰC HỌC ROBOT BẰNG MAPLE (3DOF) Sinh viên tham khảo luận văn về robot: https://www.dropbox.com/s/7pt18kmc7vo7jfb/Luan%20van%20master%20ve%20robot.rar R01 := Matrix(3, 3); for i to 3 do for j to 3 do R01[i, j] := A01[i, j] end do end do;  R02 := Matrix(3, 3); for i to 3 do for j to 3 do R02[i, j] := A02[i, j] end do end do;  R03 := Matrix(3, 3); for i to 3 do for j to 3 do R03[i, j] := A03[i, j] end do end do;  R12 := Matrix(3, 3); for i to 3 do for j to 3 do R12[i, j] := A12[i, j] end do end do;  R23 := Matrix(3, 3); for i to 3 do for j to 3 do R23[i, j] := A23[i, j] end do end do; rj := Vector([xj, yj, zj]); rsong := Matrix([[0, -zj, yj], [zj, 0, -xj], [-yj, xj, 0]]); p1c1 := subs({xj = -(1/2)*a1, yj = 0, zj = 0}, rj);  p2c2 := subs({xj = -(1/2)*a2, yj = 0, zj = 0}, rj);  p3c3 := subs({xj = 0, yj = 0, zj = -(1/2)*l}, rj); I11 := (1/12)*m1*a1^2*Matrix([[0, 0, 0], [0, 1, 0], [0, 0, 1]]);  I22 := (1/12)*m2*a2^2*Matrix([[0, 0, 0], [0, 1, 0], [0, 0, 1]]);  I33 := (1/12)*m3*l^2*Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 0]]); I01 := multiply(R01, I11, transpose(R01));  I02 := multiply(R02, I22, transpose(R02)); I03 := multiply(R03, I33, transpose(R03)); p1c1TN := Matrix([[p1c1(1, 1)], [p1c1(2, 1)], [p1c1(3, 1)], [1]]);  p2c2TN := Matrix([[p2c2(1, 1)], [p2c2(2, 1)], [p2c2(3, 1)], [1]]);  p3c3TN := Matrix([[p3c3(1, 1)], [p3c3(2, 1)], [p3c3(3, 1)], [1]]);  p01 := Matrix([[A01[1, 4]], [A01[2, 4]], [A01[3, 4]], [0]]);  p02 := combine(Matrix([[A02[1, 4]], [A02[2, 4]], [A02[3, 4]], [0]]));  p0c1saos := multiply(A01, p1c1TN); p0c2saos := combine(multiply(A02, p2c2TN));  p0c3saos := combine(multiply(A03, p3c3TN)); p1c2saos := evalm(p0c2saos-p01);  p2c3saos := combine(evalm(p0c3saos-p02));  p1c3saos := evalm(p0c3saos-p01); assign(q1(t), q1);  assign(q2(t), q2);  assign(d3(t), d3);  p0c1sao := Vector([p0c1saos[1, 1],  p0c1saos[2, 1], p0c1saos[3, 1]]); p0c2sao := Vector([p0c2saos[1, 1],  p0c2saos[2, 1], p0c2saos[3, 1]]);  p0c3sao := Vector([p0c3saos[1, 1],  p0c3saos[2, 1], p0c3saos[3, 1]]);  jv1 := jacobian(p0c1sao, [q1, q2, d3]);  jv2 := jacobian(p0c2sao, [q1, q2, d3]);  jv3 := jacobian(p0c3sao, [q1, q2, d3]);  unassign(q1(t));  unassign(q2(t));  unassign(d3(t)); omegasong01 := combine(multiply(map(diff, R01, t), transpose(R01)));  omegasong02 := combine(multiply(map(diff, R02, t), transpose(R02)));  omegasong03 := combine(multiply(map(diff, R03, t), transpose(R03))); omega1 := Vector([omegasong01[3, 2], omegasong01[1, 3], omegasong01[2, 1]]);  omega2 := Vector([omegasong02[3, 2], omegasong02[1, 3], omegasong02[2, 1]]);  omega3 := Vector([omegasong03[3, 2], omegasong03[1, 3], omegasong03[2, 1]]); omega1 := Vector([0, 0, q1c]); omega2 := Vector([0, 0, q1c+q2c]);  omega3 := Vector([0, 0, q1c+q2c]); jomega1 := jacobian(omega1, [q1c, q2c, q3c]);  jomega2 := jacobian(omega2, [q1c, q2c, q3c]);  jomega3 := jacobian(omega3, [q1c, q2c, q3c]); M := evalm(combine(multiply(transpose(jv1)*m1, jv1)+multiply(transpose(jomega1), I01, jomega1)+multiply(transpose(jv2)*m2, jv2)+multiply(transpose(jomega2), I02, jomega2)+multiply(transpose(jv3)*m3, jv3)+multiply(transpose(jomega3), I03, jomega3))); V1 := 0; V2 := 0; V3 := 0; qch := Vector([qc1, qc2, 0]); for j to 3 do V1 := simplify(V1+(diff(M[1, j], q1)-(1/2)*(diff(M[j, 1], q1)))*qch(1, 1)*qch(j, 1)+(diff(M[1, j], q2)-(1/2)*(diff(M[j, 2], q1)))*qch(j, 1)*qch(2, 1)+(diff(M[1, j], q3)-(1/2)*(diff(M[j, 3], q1)))*qch(j, 1)*qch(3, 1));  V2 := simplify(V2+(diff(M[2, j], q1)-(1/2)*(diff(M[j, 1], q2)))*qch(1, 1)*qch(j, 1)+(diff(M[2, j], q2)-(1/2)*(diff(M[j, 2], q2)))*qch(j, 1)*qch(2, 1)+(diff(M[2, j], q3)-(1/2)*(diff(M[j, 3], q2)))*qch(j, 1)*qch(3, 1));  V3 := simplify(V3+(diff(M[3, j], q1)-(1/2)*(diff(M[j, 1], q3)))*qch(1, 1)*qch(j, 1)+(diff(M[3, j], q2)-(1/2)*(diff(M[j, 2], q3)))*qch(j, 1)*qch(2, 1)+(diff(M[3, j], q3)-(1/2)*(diff(M[j, 3], q3)))*qch(j, 1)*qch(3, 1)) end do V. VÍ DỤ VỀ LẬP TRÌNH MÔ PHỎNG SỬ DỤNG KẾT QUẢ TÍNH TOÁN ĐỘNG HỌC MAPLE https://www.dropbox.com/s/u33n1fpw6b4m8g3/lap%20trinh%20mo%20phong.rar VI. VÍ DỤ VỀ TÍNH ĐỘNG HỌC RÔ BỐT 6 DOF BẰNG MAPLE https://www.dropbox.com/s/yhisd927800u9nw/Robot%206%20DOF.rar