点到点轨迹规划——自适应S曲线

点到点轨迹规划的S曲线，已知起始位置、终止位置、最大速度、最大加速度、总的运动时间、这5个参数，自动计算出运动规划曲线（若输入的参数不合适，代码可以自行计算出合适参数）

%% 自适应S曲线
clc;
clear;
close all;
%%
N = 500;
%% 1
ThetaStart = 0;
ThetaEnd = 90;
VTheta = 90;    %1
ATheta = 135;   %1.5
Tf = 1.8;
%% 2
% ThetaStart = 90;
% ThetaEnd = 00;
% VTheta = 99;    %1.1
% ATheta = 180;   %2
% Tf = 1.8;
%% J<0
% ThetaStart = 90;
% ThetaEnd = 00;
% VTheta = 1.8 * （ThetaEnd - ThetaStart）;
% ATheta = 3 * （ThetaEnd - ThetaStart）;
% Tf = 0.1;
%% t4<0
% ThetaStart = 90;
% ThetaEnd = 00;
% VTheta = 5 * （ThetaEnd - ThetaStart）;
% ATheta = 2 * （ThetaEnd - ThetaStart）;
% Tf = 3;
%% t2<0
% ThetaStart = 90;
% ThetaEnd = 00;
% VTheta = 5 * （ThetaEnd - ThetaStart）;
% ATheta = 40 * （ThetaEnd - ThetaStart）;
% Tf = 3;
%%
v = VTheta/（ThetaEnd - ThetaStart）;
a = ATheta/（ThetaEnd - ThetaStart）;
v = abs（v）;
a = abs（a）;
% %J<0
% Tf = 0.1;
% v = 1.8;
% a = 3;
% Tf = 1.2556;
% v = 1.5831;
% a = 3;
%t4<0
% Tf = 3;
% v = 1.8;
% a = 2;
% %t2<0
% Tf = 3;
% v = 5;
% a = 40;
display（Tf‘原始Tf:‘）;
display（v‘原始v:‘）;
display（a‘原始a:‘）;

Theta = zeros（1N）;
s = zeros（1N）;
sd = zeros（1N）;
sdd = zeros（1N）;

[TFVAJT] = SCurvePara（Tf v a）;
display（J ‘新J:‘）;
display（TF‘新Tf:‘）;
display（V‘新v:‘）;
display（A ‘新da:‘）;

display（TF-Tf‘新dTf:‘）;
display（V-v‘新dv:‘）;
display（A-a ‘新da:‘）;

t=linspace（0TFN）;
dt = t（2） - t（1）;
for i = 1:N
    if i == N
        a = a;
    end
    s（i） = SCurveScaling（t（i）VAJTTF）;
    Theta（i） = ThetaStart + s（i） * （ThetaEnd - ThetaStart）;
    if i>1
        sd（i-1） = （s（i） - s（i-1）） / dt;
    end
    if i>2
        sdd（i-2） = （sd（i-1） - sd（i-2）） / dt;
    end
end

figure;
plot（tTheta）
legend（‘Theta‘）;
xlabel（‘t‘）;
ylabel（‘theta‘）;
figure;
plot（ts）
legend（‘s‘）;
xlabel（‘t‘）;
ylabel（‘s‘）;
figure;
plot（tsd）;
legend（‘s的一阶导数‘）;
xlabel（‘t‘）;
ylabel（‘s的一阶导数‘）;
figure
plot（tsdd）;
legend（‘s的二阶导数‘）;
xlabel（‘t‘）;
ylabel（‘s的二阶导数‘）

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1928  2018-09-01 13:18  SCurve\JointTrajectorySCurve.m

     文件        647  2018-07-11 15:22  SCurve\SCurvePara.m

     文件       2032  2018-07-11 14:37  SCurve\SCurveScaling.m

     目录          0  2018-07-23 11:33  SCurve

----------- ---------  ---------- -----  ----

                 4607                    4