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

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

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

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

绘制图为：

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)

绘制的图形分别为

 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,'LineWidt

h',2)%绘制轨迹        grid on;        xlabel('时间(s)');ylabel('加速度(mm/s_2)')        legend('p_x','p_y','p_z','location','northeastoutside')

    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;    endend

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

% v10% 七次插值轨迹规划

close all;clc;mdl_puma560t0 = 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);%画关节位置、速度、加速度曲线figuresubplot(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;

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

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

参考：

打赏 0

点赞 0

收藏 1

分享