【机器人学】5-2.六自由度机器人轨迹规划-速度规划- S型曲线【附MATLAB代码】

鱼会上树HY

已于 2025-01-13 13:54:51 修改

阅读量2.3k

点赞数 18

分类专栏：机器人学文章标签：机器人 matlab 开发语言

于 2024-07-08 09:00:00 首次发布

本文链接：https://blog.youkuaiyun.com/y12345655/article/details/140245880

版权

机器人学专栏收录该内容

23 篇文章

订阅专栏

T型曲线中，关节的位置曲线和速度曲线连续，加速度曲线不连续且在拐角处较为粗糙不够平滑，在速度拐点处容易引起的电机和传动系统的冲击，因此该加减速曲线常用于低速、低成本的运动控制过程。同T型速度曲线相比，S形曲线更加平滑，避免了T形曲线在速度拐点的冲击，但是在相同的期望速度和加速度条件下，运动相同距离的时间要更长一点。

S型速度曲线一般包括加加速、匀加速、减加速、匀速、加减速、匀减速、减减速，一共7段，因此又被称为7段式曲线。下图是典型的S型曲线图。

MATLAB验证：

判断各种条件确定参数

function para = MySlinePara(q0, q1, v0, v1, vmax, amax, jmax)
% input 输入关节起始位置，起始点速度，最大速度，最大加速度，最大加加速度
% 得到规划参数Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin
vmin = -vmax; amin = -amax; jmin = -jmax;

% 转化得到实际的q_0、q_1、v_max、a_max
sigma = sign(q1 - q0);  
q_0 = sigma*q0;
q_1 = sigma*q1;
v_0 = v0;
v_1 = v1;
v_max = ((sigma+1)/2)*vmax + ((sigma-1)/2)*vmin;
v_min = ((sigma+1)/2)*vmin + ((sigma-1)/2)*vmax;
a_max = ((sigma+1)/2)*amax + ((sigma-1)/2)*amin;
a_min = ((sigma+1)/2)*amin + ((sigma-1)/2)*amax;
j_max = ((sigma+1)/2)*jmax + ((sigma-1)/2)*jmin;
j_min = ((sigma+1)/2)*jmin + ((sigma-1)/2)*jmax;

% 判断是否达到最大速度
if ((v_max - v_0)*j_max < a_max^2) 
    Tj1 = sqrt((v_max - v_0) / j_max); % 达不到a_max
    Ta = 2*Tj1;
    a_lima = j_max * Tj1;
else
    Tj1 = a_max / j_max; % 能够达到a_max
    Ta = Tj1 + (v_max - v_0) / a_max;
    a_lima = a_max;
end
if ((v_max - v_1)*j_max < a_max^2)
    Tj2 = sqrt((v_max - v_1) / j_max); % 达不到a_min
    Td = 2*Tj2;
    a_limd =  -j_max * Tj2;
else
    Tj2 = a_max / j_max; % 能够达到a_min
    Td = Tj2 + (v_max - v_1) / a_max;
    a_limd = -a_max;
end
% 计算匀速段时间
Tv = (q_1 - q_0)/v_max - (Ta/2)*(1 + v_0/v_max) - (Td/2)*(1 + v_1/v_max);

% 对Tv进行讨论
if (Tv > 0)
    % 达到最大速度v_max，即存在匀速阶段
    vlim = v_max;
    T = Ta + Tv + Td;
    para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
    return;
else
    % 达不到最大速度，即匀速阶段Tv=0
    % 假设最大加速度和最小加速度均能达到
    Tv = 0;
    Tj = a_max / j_max;
    Tj1 = Tj;
    Tj2 = Tj;
    delta = (a_max^4/j_max^2) + 2*(v_0^2 + v_1^2) + a_max*(4*(q_1 - q_0) - 2*(a_max/j_max)*(v_0 + v_1));
    Ta = ((power(a_max, 2)/j_max) - 2.0*v_0 + sqrt(delta)) / (2.0*a_max);
    Td = ((power(a_max, 2)/j_max) - 2.0*v_1 + sqrt(delta)) / (2.0*a_max);
    % 对Ta和Td进行讨论
    if (Ta < 0 || Td < 0)
        if (Ta < 0)
            % 没有加速段，只有减速段
            Ta = 0; Tj1 = 0;
            Td = 2*(q_1 - q_0) / (v_0 + v_1);
            Tj2 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_max*power(q_1 - q_0, 2) + power(v_1 + v_0, 2)*(v_1 - v_0)))) / (j_max*(v_1 + v_0));
            a_lima = 0;
            a_limd = -j_max*Tj2;
            vlim = v_0;
            para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
            return;
        elseif (Td < 0)
            % 没有减速段，只有加速段
            Td = 0; Tj2 = 0;
            Ta = 2*(q_1 - q_0) / (v_0 + v_1);
            Tj1 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_max*power(q_1 - q_0, 2)) - power(v_1 + v_0, 2)*(v_1 - v_0))) / (j_max*(v_1 + v_0));
            a_lima = j_max*Tj1;
            a_limd = 0;
            vlim = v_0 + a_lima*(Ta - Tj1);
            para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
            return;
        end
    elseif (Ta >= 2*Tj && Td >= 2*Tj)
        % 加速段和减速段都能达到最大加速度
        a_lima = a_max;
        a_limd = -a_max;
        vlim = v_0 + a_lima*(Ta - Tj);
        para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
        return;
    else
        % 加速和减速阶段至少有一段不能达到最大加速度
        lambda = 0.99; % 系统取0<lambda<1
        while (Ta < 2*Tj || Td < 2*Tj)
            % 循环
            a_max = lambda*a_max;
            Tv = 0;
            Tj = a_max / j_max;
            Tj1 = Tj;
            Tj2 = Tj;
            delta = (a_max^4/j_max^2) + 2*(v_0^2 + v_1^2) + a_max*(4*(q_1 - q_0) - 2*(a_max/j_max)*(v_0 + v_1));
            Ta = ((power(a_max, 2)/j_max) - 2.0*v_0 + sqrt(delta)) / (2.0*a_max);
            Td = ((power(a_max, 2)/j_max) - 2.0*v_1 + sqrt(delta)) / (2.0*a_max);
            if (Ta < 0 || Td < 0)
                if (Ta < 0)
                    % 没有加速段，只有减速段
                    Ta = 0; Tj1 = 0;
                    Td = 2*(q_1 - q_0) / (v_0 + v_1);
                    Tj2 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_max*power(q_1 - q_0, 2) + power(v_1 + v_0, 2)*(v_1 - v_0)))) / (j_max*(v_1 + v_0));
                    a_lima = 0;
                    a_limd = -j_max*Tj2;
                    vlim = v_0;
                    para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
                    return;
                elseif (Td < 0)
                    % 没有减速段，只有加速段
                    Td = 0; Tj2 = 0;
                    Ta = 2*(q_1 - q_0) / (v_0 + v_1);
                    Tj1 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_max*power(q_1 - q_0, 2)) - power(v_1 + v_0, 2)*(v_1 - v_0))) / (j_max*(v_1 + v_0));
                    a_lima = j_max*Tj1;
                    a_limd = 0;
                    vlim = v_0 + a_lima*(Ta - Tj1);
                    para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
                    return;
                end
            elseif (Ta >= 2*Tj && Td >= 2*Tj)
                % 加速段和减速段都能达到最大加速度
                a_lima = a_max;
                a_limd = -a_max;
                vlim = v_0 + a_lima*(Ta - Tj);
                para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
                return;
            end
        end
    end
end

计算位移，速度，加速度，加加速度

% 计算位移，速度，加速度，加加速度
function [time, q, qd, qdd,qddd] = compute(Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin,T)
i = 1; 
for t = 0: 0.001: T
   time(i) = 0.001*i;    
% 加速段
if (t >= 0 && t < Tj1)
    q(i) = q0 + v0*t + jmax*t^3/6;
    qd(i) = v0 + jmax*(t^2/2);
     qdd(i) = jmax*t;
     qddd(i) = jmax;
elseif (t >= Tj1 && t < Ta - Tj1)
    q(i) = q0 + v0*t +(alima/6)*(3*t^2 - 3*Tj1*t + Tj1^2);
    qd(i) = v0 + alima*(t - Tj1/2);
    qdd(i) = alima; 
    qddd(i) = 0;
elseif (t >= Ta - Tj1 && t < Ta)
    q(i) = q0 + (vlim + v0)*(Ta/2) - vlim*(Ta - t) - jmin*((Ta - t)^3/6);
    qd(i) = vlim + jmin*(power(Ta - t, 2)/2);
    qdd(i) = -jmin*(Ta - t);
    qddd(i) = jmin;
% 匀速段
elseif (t >= Ta && t < Ta + Tv)
    q(i) = q0 + (vlim + v0)*(Ta/2) + vlim*(t - Ta);
    qd(i) = vlim;
    qdd(i) = 0;
    qddd(i) = 0;
% 减速段
elseif (t >= Ta + Tv && t < T - Td + Tj2)
    q(i) = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) - jmax*(power(t - T + Td, 3)/6);
    qd(i) = vlim - jmax*(power(t - T + Td, 2)/2);
    qdd(i) = -jmax*(t - T + Td);
    qddd(i) = -jmax;
elseif (t >= T - Td + Tj2 && t < T - Tj2)
    q(i) = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) + (alimd/6)*(3*power(t - T + Td, 2) - 3*Tj2*(t - T + Td) + Tj2^2);
    qd(i) = vlim + alimd*(t - T + Td - Tj2/2);
    qdd(i) = alimd;
    qddd(i) = 0;
elseif (t >= T - Tj2 && t <= T)
    q(i) = q1 - v1*(T - t) - jmax*(power(T - t, 3)/6);
    qd(i) = v1 + jmax*(power(t - T, 2)/2);
    qdd(i) = -jmax*(T - t);
    qddd(i) = jmax;
end
i = i + 1;
end
end

测试代码

clc
clear;
% S曲线规划
% 边界条件
q0 = 0; q1 = 100; 
v0 = 0; v1 = 0;
vmax = 10; amax = 10; jmax = 30;
sigma = sign(q1 - q0);
% 得到规划参数Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin
para  = MySlinePara(q0, q1, v0, v1, vmax, amax, jmax) 
T = para(1) + para(2) + para(3);
[time, q, qd, qdd,qddd] = compute( para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16),T);
figure(1)
subplot(5,1,1)
plot(time, q, 'r', 'LineWidth', 1.5)
grid on;xlabel('time[s]');ylabel('position[mm]');
subplot(5,1,2)
plot(time, qd, 'b', 'LineWidth', 1.5)
grid on;xlabel('time[s]');ylabel('speed[mm/s]');
subplot(5,1,3)
plot(time, qdd, 'g', 'LineWidth', 1.5)
grid on;xlabel('time[s]');ylabel('acceleration[mm/s2]');
subplot(5,1,4)
plot(time, qddd, 'LineWidth', 1.5)
grid on;xlabel('time[s]');ylabel('&acceleration[mmm/s3]');

测试结果验证