【机器人学】5-1.六自由度机器人轨迹规划-速度规划-（T）梯型曲线【附MATLAB代码】

摘要：本文重点介绍机器人轨迹规划中的梯型速度曲线轨迹规划的理论知识以及MATLAB实现。

前言

目前常用的轨迹绘算法有：三次多项式，五次多项式，抛物线轨迹，（T）梯型曲线，以及S型曲线。

但是，三次多项式，五次多项式以及抛物线轨迹，都存在没有最大速度和最大加速度限制的问题。

显然机械臂的控制是需要速度以及加速度限制的，所以这三种放法显得不太适用，而梯形曲线和S型曲线就不会存在这种问题。

注意：这里的轨迹规划中的梯型曲线和S型曲线不是让机械臂的末端执行器走一个梯形或者S型，而是基于机器人的关节空间（即机器人的速度空间）。目的是使机器人关节从旋转到，其关节的速度曲线是一个梯型或S型。

而让机器人的末端执行器走出一个具体的形状是基于笛卡尔空间的轨迹规划（后续会介绍）。

正文

梯形曲线在电机控制中经常被用到，因为其速度曲线形状为梯形，因此被称为梯形速度曲线。

T型速度曲线一般包括匀加速、匀速、匀减速三个过程。

一般情况下，用户给定起始速度、终止速度、加速度、减速度、最大速度以及起始时刻和终止时刻的位移参数，需要计算出加速段、匀速段以及减速段对应的时间Ta、Tv、Td，然后再根据位移、速度以及加速度公式计算轨迹。

    

MATLAB仿真验证

（T）梯形曲线

function [time, q, qd, qdd] = Tline(q0,q1,v0,v1,vmax,amax)
% input q0,q1,起始关节角度
% input vo,v1,起始关节速度
% inputvmax,amax 起始关节加速度
% output time,q,qd,qdd
 
v_temp = sqrt((2.0*amax*(q1-q0) + (v1^2 + v0^2)) / 2);
 
if(v_temp<vmax)
    vlin = v_temp;
else
    vlin = vmax;
end
 
Ta = (vlin-v0)/amax;
Sa = v0*Ta+amax*Ta^2/2;
 
Tv = (q1-q0-(vlin^2-v0^2)/(2*amax)-(v1^2-vlin^2)/(2*-amax))/vlin;
Sv = vlin*Tv;
 
Td = (vlin-v1)/amax;
Sd = vlin*Td - amax*Td^2/2;
 
T = Ta + Tv +Td;
%步长
td = 0.01;
k = 1;
for t = 0:td:T
    if(t >= 0 && t < Ta)
        time(k) = td *k;
        q(k) = q0 + v0*t + amax*t^2/2;
        qd(k) = v0 + amax*t;
        qdd(k) = amax;
    elseif(t >= Ta && t < Ta+Tv)
        time(k) = td *k;
        q(k) = q0 + Sa + vlin*(t - Ta);
        qd(k) = vlin;
        qdd(k) = 0;
    elseif(t >= Ta+Tv && t <= T)
        time(k) = td *k;
        q(k) = q0 + Sa + Sv + vlin*(t - Ta - Tv) - amax*power(t - Ta - Tv, 2)/2;
        qd(k) = vlin - amax*(t - Ta - Tv);
        qdd(k) = -amax;
    end
    k = k + 1;
end
end

    测试代码

clc
clear;
q0 = 0; q1 =360;
v0 = 0; v1 =0;
vmax = 50; amax = 10;
[time, q, qd, qdd] = Tline(q0,q1,v0,v1,vmax,amax);
figure(1)
subplot(311)
plot(time,q,'r','LineWidth',1.5);
grid on;xlabel('time[s]');ylabel('position[mm]');
subplot(312)
plot(time,qd,'b','LineWidth',1.5);
grid on;xlabel('time[s]');ylabel('speed[mm/s]');
subplot(313)
plot(time,qdd,'g','LineWidth',1.5);
grid on;xlabel('time[s]');ylabel('acceleration[mm/s2]');

     测试结果

        第一条线为关节的位置曲线，第二条为关节的速度曲线，第三条为关节的加速度曲线

当关节运动得很短时，机械臂无法进入最大速度区段，整个过程只有加速段和减速段两个阶段，没有匀速段。

设置 q0 = 0; q1 =120;得到结果。

        可以看到，关节的位置曲线和速度曲线连续，加速度曲线不连续。并且速度曲线在拐角较为粗糙不够平滑，下一章的S型曲线将解决这个问题。

        机器人关节为6自由度，因此规划函数将调用6次才能完成机械臂的一次运动。

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