Matlab机器人工具箱（3-2）：五自由度机械臂（轨迹规划）

轨迹规划可以分为关节空间的轨迹规划和笛卡尔空间的轨迹规划

• 关节空间规划：用时小，计算量少
  [q,qt,qtt]=jtraj(q1,q2,t);
  返回关节的位置、速度、加速度
  使用五次多项式插值

• 笛卡尔空间规划：对于直线、圆形等末端轨迹形状要求严格的场合
  Ts=ctraj(T1,T2,length(t));
  返回末端位姿
  使用带抛物线过渡的直线规划（对位姿矩阵的3个位置量）

• 所用函数：
  p = transl(Ts);
  轨迹的位移部分

01 关节空间轨迹规划

绘制图为：

1. 机械臂位置及其运动过程
2. 关节 位置&速度&加速度 随时间变化曲线
3. 末端执行器轨迹(x-y视图)
4. 带轨迹标记的运动过程

% RoboticToolbox v10
% 关节空间轨迹规划
clc
 clear
%模型导入
    mdl_5dof    
    du = pi/180;   

%% 运动学轨迹
    % 改
    t=[0:0.1:8];%8秒完成轨迹，步长0.05
    
    %产生位姿矩阵法1：直接给出关节角度
    T1 = bot.fkine([20 50 -30 -25 -10]*du);%生成一个位姿
    T2 = bot.fkine([70 10 -60 -50 30]*du);%生成一个位姿
    
    %产生位姿矩阵法2：描述位置
    %T1 = transl(0.2,0.2,0.2)*trotx(pi/4);%位移*旋转,创建齐次变换
    %T2 = transl(0.2,-0.1,0.1)*trotx(pi/2);
    %T1 = transl(300)*trotz(pi/4);%位移*旋转,创建齐次变换
    %T2 = transl(200)*trotz(pi/2);    
    
    %关节角度
    q1 = bot.ikine(T1,'mask',[1 1 1 1 0 1]); %如果是[1 1 1 1 1 0]，则最后一个关节角度一直是0
    q2 = bot.ikine(T2,'mask',[1 1 1 1 0 1],'q0',q1); 
    
    %关节空间运动规划
    [q,qt,qtt]=jtraj(q1,q2,t);
   
    %笛卡尔运动规划
    %Ts=ctraj(T1,T2,length(t));
    
    bot.plot(q)%绘制轨迹
 %% 画图
    figure('name','关节随时间变化')
    subplot(3, 1, 1);
    plot(t, q,'LineWidth',1.5) %绘制关节角随时间的变化
    grid on;
    xlabel('时间(s)');ylabel('关节角度(rad)')
    set(gca,'YLim',[-3 2]);
    set(gca,'YTick',[-3,-2:2:2]);%设置要显示坐标刻度
    legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside')

    subplot(3,1,2);
    plot(t, qt,'LineWidth',1.5) %绘制关节角速度随时间的变化
    grid on;
    xlabel('时间(s)');ylabel('角速度(rad/s)')
    set(gca,'YLim',[-0.3 0.3]);
    set(gca,'YTick',[-0.3:0.15:0.3]);%设置要显示坐标刻度
    legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside')

    subplot(3, 1, 3);
    plot(t, qtt,'LineWidth',1.5) %绘制关节角加速度随时间的变化，如图2
    grid on;
    xlabel('时间(s)');ylabel('角加速度(rad/s^2)')
    set(gca,'YLim',[-0.12 0.12]);
    set(gca,'YTick',[-0.12:0.06:0.12]);%设置要显示坐标刻度
    legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside')

%%    
    %末端执行器轨迹(x-y视图)
    figure('name','末端执行器轨迹(x-y视图)')
    T = bot.fkine(q); %得到笛卡尔轨迹
    p = transl(T);%轨迹的位移部分
    plot(p(:,1),p(:,2),'LineWidth',1.5) %绘制xy平面内的末端轨迹，如图3
    xlabel('X轴(mm)');ylabel('Y轴(mm)')
    %title('末端执行器轨迹(x-y视图)')
    
    
    %带轨迹标记的运动过程
    figure('name','带轨迹标记的运动过程')
    plot3(p(:,1),p(:,2),p(:,3),'LineWidth',3)
    bot.plot(q)
    

02 笛卡尔空间轨迹规划

绘制的图形分别为

1. 机械臂位置及其运动过程
2. 关节转角随时间变化
3. 末端轨迹 x-y视图
4. 末端轨迹 三维视图
5. 各关节 速度&加速度 变化曲线
6. 末端 x、y、z轴位置的变化曲线
7. 末端 x、y、z轴 速度&加速度 的变化曲线

 clc
 clear
%模型导入
    mdl_5dof    
    du = pi/180;
    ra = 180/pi;

%% 运动学轨迹
    t=[0:0.05:2];%两秒完成轨迹，步长0.05
      
    %产生位姿矩阵
    T1 = bot.fkine([-70 60 -50  10  30]*du);%10
    T2 = bot.fkine([-1.1278    0.2226    0.9737    -0.6219    0.1415]);

    %笛卡尔运动规划
    Ts=ctraj(T1,T2,length(t));
     
    for i=1:41
       q(i,:) = bot.ikine(Ts(i),'mask',[1 1 1 1 0 1]);
    end
    

    bot.plot(q)%绘制轨迹
%%
figure('name','关节转角')
	plot(t,q,'LineWidth',2) %绘制关节角随时间的变化
    grid on;
    xlabel('时间(s)');ylabel('关节角度(rad)')
    legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside')
%%
figure('name','末端执行器轨迹(x-y视图)')
	p = transl(Ts);%轨迹的位移部分
	plot(p(:,1),p(:,2),'LineWidth',1.5) %绘制xy平面内的末端轨迹，如图3
	xlabel('X轴(mm)');ylabel('Y轴(mm)')
	% title('末端执行器轨迹(x-y视图)')
	grid on;
%%
figure('name','T0-T1直线轨迹（三维）')
	plot2(p);
	xlabel('x轴/mm');ylabel('y轴/mm');zlabel('z轴/mm');
	grid on;
%%
figure('name','v&a')
	% 速度
	subplot(2,1,1)
		delt_t=t(2)-t(1);
		for i=1:length(t)-1
		    v(i,:)=(q(i+1,:)-q(i,:))/delt_t;
		end
		plot(t,[[0 0 0 0 0];v],'LineWidth',2)%绘制轨迹
		grid on;
		xlabel('时间(s)');ylabel('关节角速度(rad/s)')
		legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside')
	
	% 加速度
	subplot(2,1,2)
			delt_t=t(2)-t(1);
			a(1,:)=v(1)/delt_t;
			for i=1:length(v)-1
			    a(i+1,:)=(v(i+1,:)-v(i,:))/delt_t;
			end
			plot(t(1:40),a,'LineWidth',2)%绘制轨迹
			grid on;
			xlabel('时间(s)');ylabel('关节角加速度(rad/s_2)')
			legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside')

%%
%%

figure('name','cart_p')
% x、y、z轴 末端执行器加速度
	for i=1:length(t)
	    p_c(i,:)=Ts(i).t;
	end
	subplot(3,1,1)
	plot(t,p_c(:,1),'LineWidth',2)%绘制轨迹
	xlabel('时间(s)');ylabel('X轴位置(m/s)')
	subplot(3,1,2)
	plot(t,p_c(:,2),'LineWidth',2)%绘制轨迹
	xlabel('时间(s)');ylabel('Y轴位置(m/s)')
	subplot(3,1,3)
	plot(t,p_c(:,3),'LineWidth',2)%绘制轨迹
	xlabel('时间(s)');ylabel('Z轴位置(m/s)')

figure('name','cart——v&a')
	% figure('name','cart_pv')
	% x、y、z轴 末端执行器速度
	subplot(2,1,1)
		delt_t=t(2)-t(1);
		for i=1:length(t)-1
		    v_c(i,:)=(p_c(i+1,:)-p_c(i,:))/delt_t;
		end
		plot(t,[[0 0 0];v_c],'LineWidth',2)%绘制轨迹
		grid on;
		xlabel('时间(s)');ylabel('速度(m/s)')
		legend('p_x','p_y','p_z','location','northeastoutside')

	% figure('name','cart_pa')
	% x、y、z轴 末端执行器加速度
	subplot(2,1,2)
		delt_t=t(2)-t(1);
		a_c(1,:)=v_c(1,:)/delt_t;
		for i=1:length(v_c)-1
		    a_c(i+1,:)=(v_c(i+1,:)-v_c(i,:))/delt_t;
		end
		plot(t(1:40),a_c,'LineWidth',2)%绘制轨迹
		grid on;
		xlabel('时间(s)');ylabel('加速度(mm/s_2)')
		legend('p_x','p_y','p_z','location','northeastoutside')

03 五次多项式轨迹规划自己写的函数

function[q,qd,qdd]=FiveInterpolation(q0,qf,t,qd0,qdf,qdd0,qddf)
    if length(t)==1
        time_seq=(0:t-1)'/(t-1);
        time_duration=1;
    else
        t_max=max(t);
        t_min=min(t);
        time_duration=t_max-t_min;
        time_seq=(t-t_min)/(time_duration);
        time_seq=time_seq(:);
    end
    q0=q0(:);
    qf=qf(:);
    if length(q0)~=length(qf)
        error('输入矢量长度不一致')
    end
    if nargin==3
        qd0=zeros(length(q0),1);
        qdf=qd0;
        qdd0=zeros(length(q0),1);
        qddf=qdd0;
    elseif nargin==5
        qdd0=zeros(length(q0),1);
        qddf=qdd0;
%         if (1ength(qd0)~=length(q0))||(1ength(qdO)~=length(qdf))
%             error('输入矢量长度不一致')
%         end
        qd0=qd0(:);
        qdf=qdf(:);
    elseif nargin==7
        qdd0=qdd0(:);
        qddf=qddf(:);
%         if(1ength(qdd0)~=length(q0))||(1ength(qdd0)~=length(qddf))
%             error('输入矢量长度不一致')
%         end
    else
        error('输人参数数目不对')
    end
 
    F=q0;E=qd0;D=qdd0/2;
    templ=qf-D-E-F;
    temp2=qdf-2*D-E;
    temp3=qddf/2-D;
    A=-2*(temp2-3*templ)+(temp3-temp2);
    B=temp2-3*templ-2*A;
    C=templ-B-A;
    time_matrix=[time_seq.^5 time_seq.^4 time_seq.^3 time_seq.^2 time_seq ones(size(time_seq))];%时间矩阵
    coeff=[A B C D E F]';%系数矩阵
    q=time_matrix*coeff;
    if nargout>=2
        coeff=[zeros(size(A)) 5*A 4*B 3*C 2*D E]';
        q=time_matrix*coeff/time_duration;
    end
    if nargout>=3
        coeff=[zeros(size(A)) zeros(size(A)) 20*A 12*B 6*C 2*D]';
        qdd=time_matrix*coeff/time_duration^2;
    end
end

04 demo：七次多项式插值-关节轨迹规划

参考：Matlab Robotics ToolBox 实战 – 七次多项式取放轨迹规划

% v10
% 七次插值轨迹规划

close all;
clc;
mdl_puma560
t0 = 0;%开始时刻
t1 = 2;%提升结束时刻
t2 = t1 + 4;%平移结束时刻
tm = t2 + 3;%下降结束时刻

t0_1 = 0:0.2:2;%上升时间
t1_2 = 0:0.5:4;%平移时间
t2_x = 0:0.3:3;%下降时间

aim0 = [0,-0.5,-0.5];%取货点
aim1 = [0,-0.5,0.2];%提升点
aim2 = [-0.5,0.5,0.2];%下落点
aimx = [-0.5,0.5,-0.5];%存货点
    

T0 = transl(aim0);
T1 = transl(aim1);
T2 = transl(aim2);
Tx = transl(aimx);

theta0 = p560.ikine6s(T0,'rdf');%左臂、手肘朝下、手腕翻转(旋转180度)
theta1 = p560.ikine6s(T1,'rdf');
theta2 = p560.ikine6s(T2,'rdf');
thetax = p560.ikine6s(Tx,'rdf');

%初始条件
theta0_ = [0 0 0 0 0 0];%初始位置速度
theta0__ = [0 0 0 0 0 0];%初始位置加速度
thetax_ = [0 0 0 0 0 0];%目标位置速度
thetax__ = [0 0 0 0 0 0];%目标位置加速度

Theta = [theta0' theta0_' theta0__' theta1' theta2' thetax' thetax_' thetax__']';
    
M = [1     0    0      0       0        0        0        0
     0     1    0      0       0        0        0        0
     0     0    2      0       0        0        0        0
     1     t1   t1^2   t1^3    t1^4     t1^5     t1^6     t1^7
     1     t2   t2^2   t2^3    t2^4     t2^5     t2^6     t2^7
     1     tm   tm^2   tm^3    tm^4     tm^5     tm^6     tm^7
     0     1    2*tm   3*tm^2  4*tm^3   5*tm^4   6*tm^5   7*tm^6
     0     0    2      6*tm    12*tm^2  20*tm^3  30*tm^4  42*tm^5];
 
 C = M^-1 * Theta;%第i列对应第i个关节的其次多项式系数
 
 %计算关节各函数
 tmietick = 0.1;
 T = 0: tmietick:9;
 %角度
 Q = [ones(int16(9/tmietick)+1,1)   T'    (T.^2)'   (T.^3)'   (T.^4)'   (T.^5)'   (T.^6)'  (T.^7)']*C;
 %速度
 Qv =[zeros(int16(9/tmietick)+1,1)  ones(int16(9/tmietick)+1,1) 2* T' 3*(T.^2)' 4*(T.^3)' 5*(T.^4)' 6*(T.^5)' 7*(T.^6)']*C;
 %加速度
 Qa =[zeros(int16(9/tmietick)+1,1)  zeros(int16(9/tmietick)+1,1) 2*ones(int16(9/tmietick)+1,1) 6*T' 12*(T.^2)' 20*(T.^3)' 30*(T.^4)' 42*(T.^5)']*C;

 %正运动学分析
 Txy=p560.fkine(Q);
 %画轨迹
 Tjtraj1=transl(Txy);
 x = Tjtraj1(:,1);
 y = Tjtraj1(:,2);
 z = Tjtraj1(:,3);
 figure
 waitforbuttonpress;
 plot3(x,y,z,'b');%轨迹图像
 hold on;
 %画出四个过程点
[x0,y0,z0]  = ellipsoid(aim0(1),aim0(2),aim0(3),0.05,0.05,0.05);
[x1,y1,z1]  = ellipsoid(aim1(1),aim1(2),aim1(3),0.05,0.05,0.05);
[x2,y2,z2]  = ellipsoid(aim2(1),aim2(2),aim2(3),0.05,0.05,0.05);
[xx,yx,zx]  = ellipsoid(aimx(1),aimx(2),aimx(3),0.05,0.05,0.05);
surf(x0,y0,z0) %画起始点
surf(x1,y1,z1) %画提升点
surf(x2,y2,z2) %画下降点
surf(xx,yx,zx) %画目标点
hold on;
%画轨迹图
p560.plot(Q);
%画关节位置、速度、加速度曲线
figure
subplot(3,1,1);
% plot(T,Q(:,1));
plot(T,Q);
title('关节位移');
xlabel('时间t/s');
ylabel('位移s/rad');
legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside' );
str=[ '\leftarrow' '(' num2str(t1) ',' num2str(theta1(1)) ')'];
text(t1,theta1(1),cellstr(str));
str=[ '\leftarrow' '(' num2str(t2) ',' num2str(theta2(1)) ')'];
text(t2,theta2(1),cellstr(str));
grid on;
subplot(3,1,2);
plot(T,Qv);
title('关节速度');
xlabel('时间t/s');
ylabel('速度v/(rad/s)');
legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside' );
grid on;
subplot(3,1,3);
plot(T,Qa);
title('关节加速度');
xlabel('时间t/s');
ylabel('加速度a/(rad/s^2)');
legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside' );
grid on;

05 智能算法+插补

为更好的解决实际问题，可以在多项式插值的基础上，结合 遗传算法，粒子群算法等智能算法

可以参考硕士论文《六自由度机器人运动控制及轨迹规划研究_韩冲》

参考：

• https://max.book118.com/html/2017/1221/145290217.shtm