Điều Khiển, Giám Sát Robot Magician Ứng Dụng Vận Chuyển Sản Phẩm Trong Công Nghiệp.pdf

Untitled BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH CÔNG TRÌNH MÃ SỐ SKC 0 0 7 6 4 6 Tp Hồ Chí Minh, tháng BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐH SƯ PHẠM KỸ THUẬT TPHCM BÁO[.]

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH CƠNG TRÌNH 1*+,ầ1&8.+2$+&&$6,1+9,ầ1 ,8.+,1*,ẩ06ẩ752%270$*,&,$1 1*'1*91&+8t) y = y0 + v0*t; 50 | P a g e ptObject = draw_object(x,y,z,l,w,h); t = t + 0.1; end elseif(ss==3) t=0; end updateObject_3d(ptObject,Obline); center_object = [ptObject(1,1)+l/2; ptObject(1,2)+w/2; ptObject(1,3)+h,]; So = dist(ptPick,center_object); %update drawing updateArm_3d(oR,hRline,hRpts); % draw attractive force set(harr, 'Xdata',oR(:,1),'Ydata',oR(:,2),'Zdata',oR(:,3),'Udata',Fatt(:,1),'Vdata',Fatt(:,2),'Wdata ',Fatt(:,3)); % draw repulsive force set(hrep, 'Xdata',RepsPts(:,1),'Ydata',RepsPts(:,2),'Zdata',RepsPts(:,3),'Udata',Freps(:,1),'Vdat a',Freps(:,2),'Wdata',Freps(:,3)); set(hTitle,'String',[num2str(iteration),' of ', num2str(maxIters), ' error = ',num2str(oErr)]) drawnow if oErr < 0.001 % if current point = target point -> stop robot ss = ss+1; if(ss==4) ss=1; end break end if oErr_virtual < alpha*100 % if current point close virtual point -> set a new virtual point if Virtual_numerator < Virtual_denominator 51 | P a g e Virtual_numerator = Virtual_numerator + ; end end %map the workspace forces to joint torques tau = zeros(3,1); for ic = 1:numel(q) % torques Fatt tau = tau + Fatt(ic,:)'; end for jc = 1:numel(ptObstacles(:,1)) for ic = 1:numel(q) idx = (jc-1)*numel(q)+ic; % torques Fatt - torques Freps tau = tau + Freps(idx,:)'; end end %Gradient descent algorithm if start_gd ==1 o_temp= fkmagician(q(1),q(2),q(3)); o_now = o_temp(numel(linkLen),:); amin = min(alpha, 1/2*max(abs(o_now-oVirtual_target))); o_now = o_now+(amin*tau/norm(tau))'; q = ikmagician(o_now(1),o_now(2),o_now(3)); o_now_full(tt,:) = o_now; oVirtual_target_full(tt,:) = oVirtual_target; tt = tt + 1; end hold on 52 | P a g e plot3(o_now(1),o_now(2),o_now(3),'or','markersize',2); plot3(oVirtual_target(1),oVirtual_target(2),oVirtual_target(3),'diamond','markersize' ,10); axis([-350 350 -350 350 350]); %detect a local minimum q_recent = [q_recent(:,2:end) q]; if iteration > inLocalMinimum = (distance(q_recent(1:3,4),qVirtual_target') > eps_m && distance(q_recent(1:3,1),q_recent(1:3,2)) < eps_m && distance(q_recent(1:3,1),q_recent(1:3,3)) < eps_m && distance(q_recent(1:3,1),q_recent(1:3,4)) < eps_m); end if inLocalMinimum qtraj=jtraj_magician(q(1:3,1),qGoal(1:3,1),tv,qd0,qd1); Virtual_numerator = ceil(rand*Virtual_denominator); end end end 53 | P a g e 54 | P a g e