- 微信
- 微博
  
  分享文章到微博
- 复制链接
  
  复制链接到剪贴板

轨迹规划方法

yd_256119014 发表于 2023/10/02 20:15:45 2023/10/02

【摘要】目录题目一三次多项式五次多项式三段二次函数/梯形速分布最小时间法/七段S形速度曲线子函数测试函数题目二公式推导机器人轨迹规划——双S曲线轨迹（详细推导）_boldyoungster的博客-CSDN博客题目一已知 q0=10o ， qf=90o ，v0 = 0，vf = 0 求：1）用三次多项式规划轨迹 2）增加加速度约束 a0 = 0，af = 0，用五次多项式规划轨迹 3）增加约束vm...

公式推导

机器人轨迹规划——双S曲线轨迹（详细推导）_boldyoungster的博客-CSDN博客

题目一

已知 q0=10o ， qf=90o ，v0 = 0，vf = 0

求：1）用三次多项式规划轨迹

2）增加加速度约束 a0 = 0，af = 0，用五次多项式规划轨迹

3）增加约束vmax=10，amax=1，用三段二次函数规划轨迹

4）增加约束 jmax =0.1，用最小时间法规划轨迹

画出关节位置、速度和加速度随时间的变化函数曲线。

三次多项式

参考：MATLAB机械臂轨迹规划：在关节空间中生成多项式轨迹以及 jtraj函数（内容超详细讲解）_matlab多点轨迹规划_Onino_plus的博客-CSDN博客

%  https://blog.csdn.net/qq_40969179/article/details/124841559
%% a)三次项多项式
clear;close all;clc;
figure('name','三次多项式');
q3_s = 10; q3_f = 90;%；起始点start角度值为10，终止点final角度值为90
t3_s=0; t3_f = 1;%；整个运动时间t=1s
v3_s = 0;v3_f = 0;%已知起始点和终止点的角速度为0
%计算系数
a0_3 = q3_s;
a1_3 = 0;
a2_3 = (3/t3_f^2)*(q3_f - q3_s);
a3_3 = (-2/t3_f^3)*(q3_f - q3_s);
j = 1;
for tc = 0: 0.01: 1
   q_3(j) = a0_3 + a1_3*tc + a2_3*tc^2 + a3_3*tc^3; %角度变化函数
   qd_3(j) = a1_3 + 2*a2_3*tc + 3*a3_3*tc^2;%角速度
   qdd_3(j) = 2*a2_3 + 6*a3_3*tc;%角加速度
   qddd_3(j) = 6*a3_3;%加速度变化率
   j = j + 1;
end
subplot(4,1,1),plot([0:0.01:1], q_3,'r'),xlabel('时间(t)'),ylabel('角度(°)');grid on;
subplot(4,1,2),plot([0:0.01:1], qd_3,'b'),xlabel('时间(t)'),ylabel('速度(°/s)');grid on;
subplot(4,1,3),plot([0:0.01:1], qdd_3,'g'),xlabel('时间(t)'),ylabel('加速度(°/s^2)');grid on;
subplot(4,1,4),plot([0:0.01:1], qddd_3,'k'),xlabel('时间(t)'),ylabel('加速度变化率');grid on;

五次多项式

参考：MATLAB机械臂轨迹规划：在关节空间中生成多项式轨迹以及 jtraj函数（内容超详细讲解）_matlab多点轨迹规划_Onino_plus的博客-CSDN博客

%% b五次多项式
figure('name','五次多项式');
a_array1 = 0;a_array2 = 0;%起止加速度值
t_array1=0;t_array2=1;%起止时间值
q5_s=10;q5_f=90;%起止角度值
v_array1=0;v_array2=0;%起止速度值

T=t_array2-t_array1;
a0=q5_s;
a1=v_array1;
a2=a_array1/2;
a3=(20*q5_f-20*q5_s-(8*v_array2+12*v_array1)*T-(3*a_array1-a_array2)*T^2)/(2*T^3);
a4=(30*q5_s-30*q5_f+(14*v_array2+16*v_array1)*T+(3*a_array1-2*a_array2)*T^2)/(2*T^4);
a5=(12*q5_f-12*q5_s-(6*v_array2+6*v_array1)*T-(a_array1-a_array2)*T^2)/(2*T^5);%计算五次多项式系数 

tc=t_array1:0.01:t_array2;
q=a0+a1*tc+a2*tc.^2+a3*tc.^3+a4*tc.^4+a5*tc.^5;
v=a1+2*a2*tc+3*a3*tc.^2+4*a4*tc.^3+5*a5*tc.^4;
a=2*a2+6*a3*tc+12*a4*tc.^2+20*a5*tc.^3;%位置，速度，加速度函数的计算
ad = 6*a3 + 24*a4*tc + 60*a5*tc.^2;
subplot(4,1,1),plot(tc,q,'r'),xlabel('时间(t)'),ylabel('位置（°）');hold on;grid on;
%plot(t_array1,q_array1,'*','color','k');plot(t_array2,q_array2,'*','color','b');
subplot(4,1,2),plot(tc,v,'b'),xlabel('时间(t)'),ylabel('速度（°/S）');hold on;grid on;
%plot(t_array1,v_array1,'*','color','k'),plot(t_array2,v_array2,'*','color','b');
subplot(4,1,3),plot(tc,a,'g'),xlabel('时间(t)'),ylabel('加速度(°/S^2)');hold on;grid on;
%plot(t_array1,a_array1,'*','color','k'),plot(t_array2,a_array2,'*','color','b');
subplot(4,1,4),plot(tc,ad,'k'),xlabel('时间(t)'),ylabel('加速度变化率');hold on;grid on;

三段二次函数/梯形速分布

参考：轨迹规划 - 梯形速度分布_梯形加减速轨迹规划_equalsai的博客-CSDN博客


clear;close all;clc;
figure('name','三段二次函数');
q_start = 10; % 起始位置
q_end = 90; % 终止位置
v_max = 10; % 最大速度
a_max = 1; % 最大加速度
%确定最大速度
vf=sqrt(a_max*abs(q_end-q_start));
if(vf>v_max)
    v_max=v_max;
    
    t_1 = v_max/a_max; % 加速阶段时间
    t_3 = t_1; % 减速阶段时间
    t_2 = (abs(q_end-q_start)-a_max *t_1^2)/v_max; % 匀速阶段时间
    T_tot = 2*t_1+t_2; % 总运动时间
else
    v_max=vf;
    T_tot = 2*v_max/a_max; % 总运动时间
    t_1 = v_max/a_max ;% 加速阶段时间
    t_3 = t_1; % 减速阶段时间
    t_2 = 0; % 匀速阶段时间
end


% 计算各阶段的位移和速度
q_1 = q_start + 0.5*a_max*t_1^2;
q_2 = q_1 + v_max*t_2;
q_3 = q_2 + 0.5*a_max*t_3^2;

v_1 = a_max*t_1;
v_2 = v_max;
v_3 = a_max*t_3;

% 计算三段二次函数系数
a0_1 = q_start;
a1_1 = 0;
a2_1 = 0.5*a_max;

a0_2 = q_1;

a1_2 = v_max;
a2_2 = 0;

a0_3 = q_2;

a1_3 = v_max;
a2_3 = -0.5*a_max;



% 生成轨迹
t_1_arr = linspace(0,t_1,100);
q_1_arr = a0_1 + a1_1*t_1_arr + a2_1*t_1_arr.^2;
v_1_arr = a1_1 + 2*a2_1*t_1_arr;
a_1_arr = 2*a2_1*ones(size(t_1_arr));
ad_1_arr = zeros(size(t_1_arr));

t_2_arr = linspace(t_1,t_1+t_2,100);
q_2_arr = a0_2 + a1_2*(t_2_arr-t_1) + a2_2*(t_2_arr-t_1).^2;
v_2_arr = a1_2*ones(size(t_2_arr));
a_2_arr = zeros(size(t_2_arr));
ad_2_arr = zeros(size(t_2_arr));

t_3_arr = linspace(t_1+t_2,T_tot,100);
q_3_arr = a0_3 + a1_3*(t_3_arr-t_1-t_2) + a2_3*(t_3_arr-t_1-t_2).^2;
v_3_arr = a1_3 + 2*a2_3*(t_3_arr-t_1-t_2);
a_3_arr = 2*a2_3*ones(size(t_3_arr));
ad_3_arr = zeros(size(t_3_arr));

% 绘制轨迹图
subplot(4,1,1),plot([t_1_arr t_2_arr t_3_arr], [q_1_arr q_2_arr q_3_arr],'r'),xlabel('时间(t)'),ylabel('位置（°）');hold on;grid on;
subplot(4,1,2),plot([t_1_arr t_2_arr t_3_arr], [v_1_arr v_2_arr v_3_arr],'b'),xlabel('时间(t)'),ylabel('速度（°/S）');hold on;grid on;
subplot(4,1,3),plot([t_1_arr t_2_arr t_3_arr], [a_1_arr a_2_arr a_3_arr],'g'),xlabel('时间(t)'),ylabel('加速度(°/S^2)');hold on;grid on;
subplot(4,1,4),plot([t_1_arr t_2_arr t_3_arr], [ad_1_arr ad_2_arr ad_3_arr],'k'),xlabel('时间(t)'),ylabel('加速度变化率');hold on;grid on;

最小时间法/七段S形速度曲线

参考：S型速度规划_Galaxy_Robot的博客-CSDN博客

子函数

%{
CalcFun.m
S型速度规划的算法子函数
%}
%%
%子函数声明，可供外部文件调用
function fun=CalcFun
fun.displacement=@displacement;
fun.velocity=@velocity;
fun.acceleration=@acceleration;
fun.jerk=@jerk;
fun.CalcSProfile=@CalcSProfile;
fun.InitialParam=@InitialParam;
end
%%
%子函数定义
%{
根据式(3.30a)-(3.30g)计算任意时刻的位移
%}
function q=displacement(t,Param)
q0=Param.q0;
q1=Param.q1;
v0=Param.v0;
v1=Param.v1;
vlim=Param.vlim;
alima=Param.alima;
alimd=Param.alimd;
jmax=Param.jmax;
Tj1=Param.Tj1;
Ta=Param.Ta;
Tv=Param.Tv;
Tj2=Param.Tj2;
Td=Param.Td;
T=Param.T;
jmin=Param.jmin;
if(t>=0 && t<Tj1)
    q=q0+v0*t+jmax*t^3/6;
elseif(t>=Tj1 && t<Ta-Tj1)   
    q=q0+v0*t+alima/6*(3*t^2-3*Tj1*t+Tj1^2);
elseif(t>=Ta-Tj1 && t<Ta)
    q=q0+(vlim+v0)*Ta/2-vlim*(Ta-t)-jmin*(Ta-t)^3/6;
elseif(t>=Ta && t<Ta+Tv)
    q=q0+(vlim+v0)*Ta/2+vlim*(t-Ta);
elseif(t>=T-Td && t<T-Td+Tj2)
    q=q1-(vlim+v1)*Td/2+vlim*(t-T+Td)-jmax*(t-T+Td)^3/6;
elseif(t>=T-Td+Tj2 && t<T-Tj2)
    q=q1-(vlim+v1)*Td/2+vlim*(t-T+Td)+alimd/6*(3*(t-T+Td)^2-3*Tj2*(t-T+Td)+Tj2^2);
else
    q=q1-v1*(T-t)-jmax*(T-t)^3/6;
end
%计算（3.33）
    q=Param.sigma*q;
end


%{
根据式(3.30a)-(3.30g)计算任意时刻的速度
%}
function dq=velocity(t,Param)

v0=Param.v0;
v1=Param.v1;
vlim=Param.vlim;
alima=Param.alima;
alimd=Param.alimd;
jmax=Param.jmax;
Tj1=Param.Tj1;
Ta=Param.Ta;
Tv=Param.Tv;
Tj2=Param.Tj2;
Td=Param.Td;
T=Param.T;
jmin=Param.jmin;
if(t>=0 && t<Tj1)
    dq=v0+jmax*t^2/2;
elseif(t>=Tj1 && t<Ta-Tj1)   
    dq=v0+alima*(t-Tj1/2);
elseif(t>=Ta-Tj1 && t<Ta)
    dq=vlim+jmin*(Ta-t)^2/2;
elseif(t>=Ta && t<Ta+Tv)
    dq=vlim;
elseif(t>=T-Td && t<T-Td+Tj2)
    dq=vlim-jmax*(t-T+Td)^2/2;
elseif(t>=T-Td+Tj2 && t<T-Tj2)
    dq=vlim+alimd*(t-T+Td-Tj2/2);
else
    dq=v1+jmax*(T-t)^2/2;
end
%计算（3.33）
dq=Param.sigma*dq;
end

%{
根据式(3.30a)-(3.30g)计算任意时刻的加速度
%}
function ddq=acceleration(t,Param)

alima=Param.alima;
alimd=Param.alimd;
jmax=Param.jmax;
Tj1=Param.Tj1;
Ta=Param.Ta;
Tv=Param.Tv;
Tj2=Param.Tj2;
Td=Param.Td;
T=Param.T;
jmin=Param.jmin;
if(t>=0 && t<Tj1)
    ddq=jmax*t;
elseif(t>=Tj1 && t<Ta-Tj1)   
    ddq=alima;
elseif(t>=Ta-Tj1 && t<Ta)
    ddq=-jmin*(Ta-t);
elseif(t>=Ta && t<Ta+Tv)
    ddq=0;
elseif(t>=T-Td && t<T-Td+Tj2)
   ddq=-jmax*(t-T+Td);
elseif(t>=T-Td+Tj2 && t<T-Tj2)
    ddq=alimd;
else
    ddq=-jmax*(T-t);
end
%计算（3.33）
ddq=Param.sigma*ddq;
end

%{
根据式(3.30a)-(3.30g)计算任意时刻的加加速度
%}
function jt=jerk(t,Param)

jmax=Param.jmax;
Tj1=Param.Tj1;
Ta=Param.Ta;
Tv=Param.Tv;
Tj2=Param.Tj2;
Td=Param.Td;
T=Param.T;
jmin=Param.jmin;
if(t>=0 && t<Tj1)
    jt=jmax;
elseif(t>=Tj1 && t<Ta-Tj1)   
    jt=0;
elseif(t>=Ta-Tj1 && t<Ta)
    jt=jmin;
elseif(t>=Ta && t<Ta+Tv)
    jt=0;
elseif(t>=T-Td && t<T-Td+Tj2)
   jt=jmin;
elseif(t>=T-Td+Tj2 && t<T-Tj2)
    jt=0;
else
    jt=jmax;
end
%计算（3.33）
jt=Param.sigma*jt;
end

%{
（1）参数合法性检查
（2）初始化参数结构体
%}
function IniParam=InitialParam(q0,q1,v0,v1,v_max,a_max,j_max)
%参数检查，绝对值小于1.0E-8的数值认为是0.
if(abs(v_max)<1.0E-8)
    disp('参数错误。最大速度不能为0！程序退出。');
    return;
end
if(abs(a_max)<1.0E-8)
    disp('参数错误。最大加速度不能为0！程序退出。');
    return;
end
if(abs(j_max)<1.0E-8)
    disp('参数错误。最大加加速度不能为0！程序退出。');
    return;
end
if(abs(q1-q0)<1.0E-8)
    disp('参数错误。输入位移不能为0！程序退出。');
    return;
end
%%
%定义并初始化输入参数的结构体
if(q1-q0>0)
    sigma=1;
else
    sigma=-1;
end
v_min=-v_max;
j_min=-j_max;
a_min=-a_max;
%计算（3.31），
hq0=sigma*q0;
hq1=sigma*q1;
hv0=sigma*v0;
hv1=sigma*v1;
%计算（3.32）
hv_max=(sigma+1)/2*v_max+(sigma-1)/2*v_min;
hv_min=(sigma+1)/2*v_min+(sigma-1)/2*v_max;
ha_max=(sigma+1)/2*a_max+(sigma-1)/2*a_min;
ha_min=(sigma+1)/2*a_min+(sigma-1)/2*a_max;
hj_max=(sigma+1)/2*j_max+(sigma-1)/2*j_min;
hj_min=(sigma+1)/2*j_min+(sigma-1)/2*j_max;
IniParam=struct(...
'q0',{hq0},...
'q1',{hq1},...
'v0',{hv0},...
'v1',{hv1},...
'vmax',{hv_max},...
'vmin',{hv_min},...
'amax',{ha_max},...
'amin',{ha_min},...
'jmax',{hj_max},...
'jmin',{hj_min},...
'sigma',{sigma}...
);
end

%{
根据输入参数进行S型速度规划，输出结果Param
%}
function Param=CalcSProfile(q0,q1,v0,v1,v_max,a_max,j_max)

%初始化参数
InParam=InitialParam(q0,q1,v0,v1,v_max,a_max,j_max);
%定义并初始化存储规划结果的结构体
Param=struct(...
 'Tj1',{0},'Ta',{0},'Tv',{0},'Tj2',{0},'Td',{0},'T',{0},...
 'q0',{0},...
 'q1',{0},...
'v0',{0},...
'v1',{0},...
'vlim',{0},...
'alima',{0},...
'alimd',{0},...
'jmax',{0},...
'jmin',{0},...
'sigma',{InParam.sigma}...
);
Param.q0=InParam.q0;
Param.q1=InParam.q1;
Param.v0=InParam.v0;
Param.v1=InParam.v1;
Param.jmax=InParam.jmax;
Param.jmin=InParam.jmin;
%%
%计算（3.17）和（3.18）
T1=sqrt(abs(InParam.v1-InParam.v0)/InParam.jmax);
T2=InParam.amax/InParam.jmax;
Tjs=min(T1,T2);
if(T1<=T2)
    Dq=InParam.q1-InParam.q0;
    if(Dq<Tjs*(InParam.v0+InParam.v1))
        disp('位移过小，不存在满足始末速度的轨迹！程序退出。');
        return;
    end
else
    Dq=InParam.q1-InParam.q0;
    if(Dq<0.5*(InParam.v0+InParam.v1)*(Tjs+abs(InParam.v1-InParam.v0)/InParam.amax))
        disp('位移过小，不存在满足始末速度的轨迹！程序退出。');
        return;
    end
end
%%
%输入参数正确误（即轨迹存在），分类讨论
if((InParam.vmax-InParam.v0)*InParam.jmax<InParam.amax^2)
    %(3.19)满足,amax不能达到
    Tj1=sqrt((InParam.vmax-InParam.v0)/InParam.jmax);
    Ta=2*Tj1;
    Param.alima=InParam.jmax*Tj1;
else
    %(3.19)不满足，amax能达到
    Tj1=InParam.amax/InParam.jmax;
    Ta=Tj1+(InParam.vmax-InParam.v0)/InParam.amax;
    Param.alima=InParam.amax;
end

if((InParam.vmax-InParam.v1)*InParam.jmax<InParam.amax^2)
    %(3.20)满足,amin不能达到
    Tj2=sqrt((InParam.vmax-InParam.v1)/InParam.jmax);
    Td=2*Tj2;
    Param.alimd=-InParam.jmax*Tj2;
else
     %(3.20)不满足,amin能达到
    Tj2=InParam.amax/InParam.jmax;
    Td=Tj2+(InParam.vmax-InParam.v1)/InParam.amax;
    Param.alimd=-InParam.amax;
end
 
%计算（3.25）
Tv=(InParam.q1-InParam.q0)/InParam.vmax-Ta/2*(1+InParam.v0/InParam.vmax)-...
    Td/2*(1+InParam.v1/InParam.vmax);
if(Tv>0)
    %case1,最大速度能达到
    Param.vlim=InParam.vmax;
    Param.Tj1=Tj1;
    Param.Ta=Ta;
    Param.Tj2=Tj2;
    Param.Td=Td; 
    Param.T=Ta+Tv+Td;
    Param.Tv=Tv;
    return;   
else
   % case2,最大速度不能达到
   Tv=0;
   Param.Tv=Tv;
   %计算（3.26a）,(3.27),（3.26b）,（3.26c）
   Tj=InParam.amax/InParam.jmax;
   Tj1=Tj;
   Tj2=Tj;
   Delta=InParam.amax^4/InParam.jmax^2+2*(InParam.v0^2+InParam.v1^2)+InParam.amax*...
       (4*(InParam.q1-InParam.q0)-2*InParam.amax/InParam.jmax*(InParam.v0+InParam.v1));
   Ta=(InParam.amax^2/InParam.jmax-2*InParam.v0+sqrt(Delta))/(2*InParam.amax);
   Td=(InParam.amax^2/InParam.jmax-2*InParam.v1+sqrt(Delta))/(2*InParam.amax);
   if(Ta>2*Tj && Td>2*Tj)
       %加速段和减速段都能达到最大加速度
       Param.Tj1=Tj1;
       Param.Tj2=Tj2;
       Param.Ta=Ta;
       Param.Td=Td;
       Param.T=Ta+Tv+Td;
       Param.alima=InParam.amax;
       Param.alimd=-InParam.amax;
       Param.vlim=InParam.v0+(Ta-Tj1)*Param.alima;
       return;
   else
       %至少有一段不能达到最大加速度
       gamma=0.99;
       amax=InParam.amax;
       %逐渐减小最大加速度约束
       while(Ta<2*Tj || Td<2*Tj)
           if(Ta>0 && Td>0)
               amax=gamma*amax;
                %循环计算（3.26a）,(3.27),（3.26b）,（3.26c）
               Tj=amax/InParam.jmax;
               Tj1=Tj;
               Tj2=Tj;
               Delta=amax^4/InParam.jmax^2+2*(InParam.v0^2+InParam.v1^2)+amax*...
                   (4*(InParam.q1-InParam.q0)-2*amax/InParam.jmax*(InParam.v0+InParam.v1));
               Ta=(amax^2/InParam.jmax-2*InParam.v0+sqrt(Delta))/(2*amax);
               Td=(amax^2/InParam.jmax-2*InParam.v1+sqrt(Delta))/(2*amax);          
           else
               %出现Ta或Td小于0
               if(Ta<=0)
                   Ta=0;
                   Tj1=0;
                   %计算（3.28a）
                   Td=2*(InParam.q1-InParam.q0)/(InParam.v0+InParam.v1);
                   %计算（3.28b）
                   num=InParam.jmax*(InParam.q1-InParam.q0)-...
                       sqrt(InParam.jmax*(InParam.jmax*(InParam.q1-InParam.q0)^2+...
                       (InParam.v1+InParam.v0)^2*(InParam.v1-InParam.v0)));
                   den=InParam.jmax*(InParam.v1+InParam.v0);
                   Tj2=num/den;
               elseif(Td<=0)
                   Td=0;
                   Tj2=0;
                   %计算（3.29a）
                   Ta=2*(InParam.q1-InParam.q0)/(InParam.v0+InParam.v1);
                   %计算（3.29b）
                    num=InParam.jmax*(InParam.q1-InParam.q0)-...
                       sqrt(InParam.jmax*(InParam.jmax*(InParam.q1-InParam.q0)^2-...
                       (InParam.v1+InParam.v0)^2*(InParam.v1-InParam.v0)));
                   den=InParam.jmax*(InParam.v1+InParam.v0);
                   Tj1=num/den;
               end 
               Param.Tj1=Tj1;
               Param.Tj2=Tj2;
               Param.Ta=Ta;
               Param.Td=Td;
               Param.T=Ta+Tv+Td;
               Param.alima=InParam.jmax*Tj1;
               Param.alimd=-InParam.jmax*Tj2;
               Param.vlim=InParam.v0+(Ta-Tj1)* Param.alima;
               return;             
           end   
       end
       Param.Tj1=Tj1;
       Param.Tj2=Tj2;
       Param.Ta=Ta;
       Param.Td=Td;
       Param.T=Ta+Tv+Td;
       Param.alima=InParam.jmax*Tj1;
       Param.alimd=-InParam.jmax*Tj2;
       Param.vlim=InParam.v0+(Ta-Tj1)* Param.alima;
       return;    
   end    
end

end

测试函数

%{
%SProfileTest.m
S型速度规划测试程序，2019.02.12，Brian
（1）输入轨迹的边界条件及约束条件；
（2）绘制速度规划的结果。
%}
clc
clear
close('all')
%%
%定义轨迹边界条件及约束条件
q0=10;
q1=0;
v0=-7;
v1=0;
v_max=10;
a_max=10;
j_max=30;
%%
%调用算法子函数，进行速度规划，得到规划结果
fun=CalcFun();
Param=fun.CalcSProfile(q0,q1,v0,v1,v_max,a_max,j_max);
%%
%根据规划结果，调用算法子函数计算轨迹
Ts=0.001;
i=0;
for t=0:Ts:Param.T
    i=i+1; 
    time(i)=i*Ts;
    dis(i)=fun.displacement(t,Param);
    vel(i)=fun.velocity(t,Param);
    acc(i)=fun.acceleration(t,Param);
    jerk(i)=fun.jerk(t,Param);
end
%%
%绘图
figure
%位置
subplot(4,1,1)
plot(time,dis,'r','LineWidth',1.5);
grid on
ylabel('position')
%速度
subplot(4,1,2)
plot(time,vel,'b','LineWidth',1.5);
grid on
hold on
ylabel('velocity')
v_ref1=v_max*ones(1,length(time));
v_ref2=-v_max*ones(1,length(time));
plot(time,v_ref1,'b--','LineWidth',1.5);
plot(time,v_ref2,'b--','LineWidth',1.5);
%加速度
subplot(4,1,3)
plot(time,acc,'g','LineWidth',1.5);
grid on
hold on
ylabel('acceleration')
a_ref1=a_max*ones(1,length(time));
a_ref2=-a_max*ones(1,length(time));
plot(time,a_ref1,'g--','LineWidth',1.5);
plot(time,a_ref2,'g--','LineWidth',1.5);
%加加速度
subplot(4,1,4)
plot(time,jerk,'m','LineWidth',1.5)
grid on
hold on
ylabel('jerk')
j_ref1=j_max*ones(1,length(time));
j_ref2=-j_max*ones(1,length(time));
plot(time,j_ref1,'m--','LineWidth',1.5);
plot(time,j_ref2,'m--','LineWidth',1.5);
%结束

题目二

7.13 编写Matlab程序。使用平面两杆操作臂沿着习题7.8的路径运动

（起始点𝜽𝟎 = 𝟓. 𝟎 ° ，中间点𝜽𝒗 = 𝟏𝟓. 𝟎 ° ，终止点𝜽𝒈 = −𝟏𝟎. 𝟎 ° ，每段曲线

均为三次多项式，持续时间均为2.0s，计算方式参照例7.2）使用动力学

公式（如下，具体计算过程见6.7节）计算所需要施加的力矩随时间变化

的规律，求出力矩的最大值以及发生在路径的何处。

%%两段带有中间点的三项多项式(连接点的速度和加速度相等)
clear;close all;clc;
% theta(t)_1 = a10 + a11*t1 + a12*t1^2 + a13*t1^3
% theta(t)_2 = a20 + a21*t2 + a22*t2^2 + a23*t2^3
theta_s_ = 5; theta_c_ = 15; theta_f_ = -10;%开始、中间、结尾角度值
tc = 2; t3c_f = 4;%中间、结束时间
theta_s_d_ = 0; theta_s_dd_ = 0; 
theta_f_d_ = 0; theta_f_dd_ = 0;
%计算系数值
a10 = theta_s_;
a11 = 0;
a12 = (12*theta_c_ - 3*theta_f_ - 9*theta_s_) / (4*tc^2);
a13 = (-8*theta_c_ + 3*theta_f_ + 5*theta_s_) / (4*tc^3);
a20 = theta_c_; 
a21 = (3*theta_f_ - 3*theta_s_) / (4*tc);
a22 = (-12*theta_c_ + 6*theta_f_ + 6*theta_s_) / (4*tc^2);
a23 = (8*theta_c_ - 5*theta_f_ - 3*theta_s_) / (4*tc^3);
s = 1;
for T = 0: 0.02: 2
    theta_1(s) = a10 + a11*T + a12*T^2 + a13*T^3;
    theta_d_1(s) = a11 + 2*a12*T + 3*a13*T^2;
    theta_dd_1(s) = 2*a12 + 6*a13*T;
    theta_ddd_1(s) = 6*a13;
    s = s + 1;
end
s = 1;
for T = 0: 0.02: 4
    theta_2(s) = a20 + a21*T + a22*T^2 + a23*T^3;
    theta_d_2(s) = a21 + 2*a22*T + 3*a23*T^2;
    theta_dd_2(s) = 2*a22 + 6*a23*T;
    theta_ddd_2(s) = 6*a23;
    s = s + 1;
end
% 去掉首尾值
theta_ = [theta_1, theta_2(2: 101)];
theta_d_ = [theta_d_1, theta_d_2(2: 101)];
theta_dd_ = [theta_dd_1, theta_dd_2(2: 101)];
theta_ddd_ = [theta_ddd_1, theta_ddd_2(2: 101)];

subplot(4, 1, 1);plot([0:0.02:4], theta_,'r'),xlabel('时间(t)'),ylabel('角度(°)');grid on;
title('带中间点三次多项式');
subplot(4, 1, 2);plot([0:0.02:4], theta_d_,'b'),xlabel('时间(t)'),ylabel('速度(°/s)');grid on;
subplot(4, 1, 3);plot([0:0.02:4], theta_dd_,'g'),xlabel('时间(t)'),ylabel('加速度(°/s^2)');grid on;
subplot(4, 1, 4);plot([0:0.02:4], theta_ddd_,'m'),xlabel('时间(t)'),ylabel('加速度变化率');grid on;

%取重力加速度g为10，令两个速度，加速度相等
g=10;
p1=theta_*pi/180;p2=theta_*pi/180;
v1=theta_d_*pi/180;v2=theta_d_*pi/180;
a1=theta_dd_*pi/180;a2=theta_dd_*pi/180;

A1= a2+cos(p2).*(2*a1+a2)+4*a1-sin(p2).*v2.^2-2*sin(p2).*v1.*v2+g*cos(p1+p2)+3*g*cos(p1);%力矩1
A2= cos(p2).*a1+sin(p2).*v1.^2+g*cos(p1+p2)+a1+a2;%力矩2
figure;
subplot(2, 1, 1);plot([0:0.02:4], A1,'r'),xlabel('时间(t)'),ylabel('力矩1(N·m))');grid on;hold on;
title('力矩');
T=[0:0.02:4];
[~, ind1] = max(abs(A1));
plot( T(ind1),A1(ind1), 'ro');
fprintf('力矩1最大时theta度数为 %f\n ',theta_(ind1))

subplot(2, 1, 2);plot([0:0.02:4], A2,'g'),xlabel('时间(t)'),ylabel('力矩2(N·m)');grid on;hold on;
T=[0:0.02:4];
[~, ind2] = max(abs(A2));
plot( T(ind2),A2(ind2), 'ro');
fprintf('力矩2最大时theta度数为 %f\n ',theta_(ind2))

点赞
收藏
关注作者

0/1000

抱歉，系统识别当前为高风险访问，暂不支持该操作

全部回复

上滑加载中

设置昵称

在此一键设置昵称，即可参与社区互动！

*长度不超过10个汉字或20个英文字符，设置后3个月内不可修改。

确认取消

加入云驻计划，成为创作者

华为云周边好礼
免费体验产品
特殊身份标识
线下官方门票
内部专家零距离
与10000+优质创作者共同成长

立即加入

轨迹规划方法

题目一

三次多项式

五次多项式

三段二次函数/梯形速分布

最小时间法/七段S形速度曲线

子函数

测试函数

题目二

全部回复

设置昵称

关于作者

目录

加入云驻计划，成为创作者

推荐阅读

相关产品