机械臂/机器人 轨迹规划 matlab

%ABB close all; clear; clc; %建立机器人模型 % % alpha:连杆扭角；a:连杆长度； theta:关节转角； d:关节距离； offset:偏移 % % theta d a alpha offset % L1=Link([0 200 200 pi/2 0 ]); %定义连杆的D-H参数 % L2=Link([pi/2 0 550 0 0 ]); % L3=Link([0 0 160 pi/2 0 ]); % L4=Link([0 600 0 pi/2 0 ]); % L5=Link([pi/2 0 0 -pi/2 0 ]); % L6=Link([0 0 0 0 0 ]); % robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','ABB'); %连接连杆，机器人取名manman % theta d a alpha offset L1=Link([0 486.5 150 -pi/2 0 ]); %定义连杆的D-H参数 L2=Link([0 0 700 0 0 ]); L3=Link([0 0 0 -pi/2 0 ]); L4=Link([0 600 0 pi/2 0 ]); L5=Link([0 0 0 -pi/2 0 ]); L6=Link([0 0 0 0 0 ]); robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','ABB'); %连接连杆，机器人取名ABB % robot.plot([0,0,0,0,0,0]); %输出机器人模型，后面的六个角为输出时的theta姿态 % robot.display(); % teach(robot); % xy=[638 1008; % 566 1127; % 549 1210; % 839 1214; % 823,1130; % 755,1004; % 638 1008;]; % figure; % plot(xy(:,1),xy(:,2)); T1=transl(100,600,0);%根据给定起始点，得到起始点位姿 T2=transl(1000,0,500);%根据给定终止点，得到终止点位姿 qz=robot.ikine(T1);%根据起始点位姿，得到起始点关节角 qr=robot.ikine(T2); [q ,qd, qdd]=jtraj(qz,qr,50); %五次多项式轨迹，得到关节角度，角速度，角加速度，50为采样点个数 grid on T=robot.fkine(q);%根据插值，得到末端执行器位姿 X=T(1,:).T; x=squeeze(X(1,4,:)); y=squeeze(X(2,4,:)); z=squeeze(X(3,4,:)); plot3(x,y,z,'k','LineWidth',2);%输出末端轨迹 hold on axis([-1000 2000 -1000 1000 -1000 1000]); robot.plot(q ); % xy=[638 1008; % 566 1127; % 549 1210; % 839 1214; % 823,1130; % 755,1004; % 638 1008;]; % figure; % plot(xy(:,1),xy(:,2)); %动画演示 % t=[0:0.02:1.5]; % T=ctraj(T1,T2,length(t)); % q=robot.ikine(T);%根据插值，得到末端执行器位姿 % theta=[0,0,0,0,0,0]; %指定的关节角 % p=robot.fkine(theta) %fkine正解函数，根据我们给定的关节角theta，求解出末端位姿p % q=robot.ikine(p) %ikine逆解函数，根据我们给定的末端位姿p，求解出关节角q