六轴机器人轨迹规划之三段S曲线加减速直线插补

1.轨迹规划的定义

轨迹规划(trajectory planning)是运动规划(motion planning)研究的主要内容。运动规划指的是运动插补，在起始点和终止点之间插入中间点序列，实现沿着轨迹的平稳运动。运动控制包含路径规划(path planning)和轨迹规划，路径规划是规划位置，在起终点之间经过的路径点，轨迹规划是规划时间，将路径点与时间相对应。

对于我们的六轴机器人而言轨迹规划可以分为：关节空间轨迹规划和笛卡尔空间轨迹规划。关节空间轨迹规划是把机器人的关节变量变换成跟时间的函数，然后对角速度和角加速度进行约束。笛卡尔空间轨迹规划是把机器人末端在笛卡尔空间的位移、速度和加速度变换成跟时间的函数关系。

2.数学基础

S型规划的“S”单指加速阶段的速度轮廓，整个S规划分为7个阶段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。其中 加加速、匀加速、减加速 三个阶段的曲线合在一起像英文字母S。（绿色曲线的前半部分，蓝色的是加速度，红色为加加速度）：



本问先介绍三段S曲线加减速插补法，将轨迹分成三段，相较于普通的直线插补，该方法在靠近路径起点和终点的地方增加一段抛物线作为过渡曲线，由于抛物线的一阶导数是一次函数，二阶导数为常，可使得直线插补轨迹上的位移和速度变化都是平滑且连续的。具体的规划方法如下：

以横坐标为例：

设起始坐标为x1x1，终止坐标为x2x2，则增量△x=x2−x1△x=x2−x1

x=x1+λ△xx=x1+λ△x

其中λλ函数变化如下图所示，将插补分为加速阶段、匀速阶段、减速阶段，令两段加速度相等且时间相等（t1=1−t2t1=1−t2）。



设匀速阶段速度为vv，加减速大小为aa，加减速所需的时间为TaTa，加减速阶段位移LbLb

{Ta=vaLa=12aT2a{Ta=vaLa=12aTa2

整段轨迹的总位移L和所需的总时间T为

{L=x2−x1T=2Ta+L−2Lav{L=x2−x1T=2Ta+L−2Lav

对抛物线过渡阶段的位移、时间、加速度做归一化处理：

⎧⎩⎨⎪⎪⎪⎪⎪⎪Lλ=LaLTλ=TaTaλ=2LλT2λ{Lλ=LaLTλ=TaTaλ=2LλTλ2

可以求得λλ的分段函数如下

λ=⎧⎩⎨⎪⎪⎪⎪12aλt2,(0≤t≤T−λ)12aλTλ2+aλTλ(t−Tλ),(Tλ≤t≤1−Tλ)1−12aλ(t−1)2,(1−Tλ≤t≤1)λ={12aλt2,(0≤t≤T−λ)12aλTλ2+aλTλ(t−Tλ),(Tλ≤t≤1−Tλ)1−12aλ(t−1)2,(1−Tλ≤t≤1)

λλ的一阶导

λ˙=⎧⎩⎨⎪⎪aλt,(0≤t≤T−λ)aλTλ,(Tλ≤t≤1−Tλ)aλ(1−t),(1−Tλ≤t≤1)λ˙={aλt,(0≤t≤T−λ)aλTλ,(Tλ≤t≤1−Tλ)aλ(1−t),(1−Tλ≤t≤1)

λλ的二阶导

λ¨=⎧⎩⎨⎪⎪aλ,(0≤t≤T−λ)0,(Tλ≤t≤1−Tλ)−aλ,(1−Tλ≤t≤1)λ¨={aλ,(0≤t≤T−λ)0,(Tλ≤t≤1−Tλ)−aλ,(1−Tλ≤t≤1)

3.matlab代码实现

指定点的位置，每一段的速度，加速度，就能完成插值。

clc;
clear;
%初始条件
x_arry=[0,12,24,36];
v_arry=[4,4,4];
a_arry=[4,4,4];
weiyi=[x_arry(1)];sudu=[0];jiasudu=[a_arry(1)];shijian=[0];timeall=0;
for i=1:1:length(x_arry)-1;
%清空
l1=[];dl1=[];ddl1=[];
%计算加减速段的时间和位移
Ta=v_arry(i)/a_arry(i);
La=a_arry(i)*(Ta^2)/2;
%计算整段轨迹的总位移和时间
L=x_arry(i+1)-x_arry(i);
T=2*Ta+(L-2*La)/v_arry(i);
%归一化处理
Lr=La/L;
Tr=Ta/T;
ar=(2*Lr)/(Tr^2);
%计算出lambda的表达式
for t=0:0.001:1
if t<=Tr;%加速度阶段
p1=0.5*ar*t^2;
dp1=ar*t;
ddp1=ar;
l1=[l1,p1];dl1=[dl1,dp1];ddl1=[ddl1,ddp1];
elseif t>Tr && t<1-Tr;%匀速阶段
p1=0.5*ar*Tr^2+ar*Tr*(t-Tr);
dp1=ar*Tr;
ddp1=0;
l1=[l1,p1];dl1=[dl1,dp1];ddl1=[ddl1,ddp1];
elseif t>=1-Tr && t<=1;%减速度阶段
p1=1-0.5*ar*(t-1)^2;
dp1=ar*(1-t);
ddp1=-ar;
l1=[l1,p1];dl1=[dl1,dp1];ddl1=[ddl1,ddp1];
end
end
%反归一化
w=l1*L+x_arry(i);%位移
s=dl1*(L/T);%速度
j=ddl1*(L/T^2);%加速度
%时间
time=[timeall:0.001*T:timeall+T];
timeall=timeall+T;
%连接每一段轨迹
weiyi=[weiyi,w(2:end)];
sudu=[sudu,s(2:end)];
jiasudu=[jiasudu,j(2:end)];
shijian=[shijian,time(2:end)];
end
subplot(3,1,1),plot(shijian,weiyi,'r');xlabel('t'),ylabel('position');grid on;
subplot(3,1,2),plot(shijian,sudu,'b');xlabel('t'),ylabel('velocity');grid on;
subplot(3,1,3),plot(shijian,jiasudu,'g');xlabel('t'),ylabel('accelerate');grid on;

PS:三段s曲线加减速插值能满足机器人对加工的速度控制都是有要求的，但是能够看出加速度依然存在跳变的情况，在对机器平稳性要求高的地方不使用，下次将会为大家介绍五段s曲线加减速插值法，能够使得加速度也是光滑曲线，能提升机器的平稳运行。