六轴机器人轨迹规划之五段位置s曲线插补

1.原理
五段s曲线相较于三段s曲线而言加速度也是连续变化的，能适用于平稳性要求更高的场合。分为加加速、加减速、匀速、减加速、减减速这五段。
设除匀速段以为，其余四段的时间相等都为 $T_a$ ，总时间为 $T$ ，匀速段速度为 $v_s$ ，四个变速段斜率大小都为 $A$ ，整段轨迹的总位移 $L$ 、加加速段位移 $L_1$ 、加减速段位移 $L_2$

⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ T a = v s A - - \sqrt L 1 = 1 6 A T 3 a L 2 = 5 6 A T 3 a T = 4 T a + L - 2 L 1 - 2 L 2 v s

$\begin{eqnarray*} \left\{ \begin{array}{l} T_a=\sqrt{\frac{v_s}{A}}\\ L_1=\frac{1}{6}AT_a^3\\ L_2=\frac{5}{6}AT_a^3\\ T=4T_a+\frac{L-2L_1-2L_2}{v_s} \end{array}\right. \end{eqnarray*}$

则加速度分段函数为

a = ⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ A t, (0 \leq t \leq T a) - A (t - 2 T a), (T a \leq t \leq 2 T a) 0, (2 T a \leq t \leq T - 2 T a) - A [t - (T - 2 T a)], (T - 2 T a \leq t \leq T - T a) A (t - T), (T - T a \leq t \leq T)

$\begin{eqnarray*} a= \left\{ \begin{array}{l} At,(0\leq{t}\leq{T_a})\\ -A(t-2T_a),({T_a}\leq{t}\leq2{T_a})\\ 0,({2T_a}\leq{t}\leq{T-2{T_a}})\\ -A[t-(T-2T_a)],(T-2{T_a}\leq{t}\leq{T-{T_a}})\\ A(t-T),(T-{T_a}\leq{t}\leq{T}) \end{array}\right. \end{eqnarray*}$
对加速度积分可得

v = ⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ 1 2 A t 2, (0 \leq t \leq T a) - 1 2 A (t - 2 T a) 2 + A T 2 a, (T a \leq t \leq 2 T a) v s, (2 T a \leq t \leq T - 2 T a) - 1 2 A （ t - T + 2 T a) 2 + A T 2 a, (T - 2 T a \leq t \leq T - T a) 1 2 A (t - T) 2, (T - T a \leq t \leq T)

$\begin{eqnarray*} v=\left\{ \begin{array}{l} \frac{1}{2}At^2,(0\leq{t}\leq{T_a})\\ -\frac{1}{2}A(t-2T_a)^2+AT_a^2,({T_a}\leq{t}\leq2{T_a})\\ v_s,({2T_a}\leq{t}\leq{T-2{T_a}})\\ -\frac{1}{2}A（t-T+2T_a)^2+AT_a^2,(T-2{T_a}\leq{t}\leq{T-{T_a}})\\ \frac{1}{2}A(t-T)^2,(T-{T_a}\leq{t}\leq{T}) \end{array}\right. \end{eqnarray*}$
对速度积分可得到位移s的分段函数

s = ⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ 1 6 A t 3, (0 \leq t \leq T a) - 1 6 A (t - 2 T a) 3 + A T 2 a t - A T 3 a, (T a \leq t \leq 2 T a) A T 2 a t - A T 3 a, (2 T a \leq t \leq T - 2 T a) - 1 6 A (t - T + 2 T a) 3 + A T 2 a t - A T 2 a, (T - 2 T a \leq t \leq T - T a) 1 6 A (t - T) 3 - 2 A T 3 a + A T 2 b T, (T - T a \leq t \leq T)

$\begin{eqnarray*} s= \left\{ \begin{array}{l} \frac{1}{6}At^3,(0\leq{t}\leq{T_a})\\ -\frac{1}{6}A(t-2T_a)^3+AT_a^2t-AT_a^3,({T_a}\leq{t}\leq2{T_a})\\ AT_a^2t-AT_a^3,({2T_a}\leq{t}\leq{T-2{T_a}})\\ -\frac{1}{6}A(t-T+2T_a)^3+AT_a^2t-AT_a^2,(T-2{T_a}\leq{t}\leq{T-{T_a}})\\ \frac{1}{6}A(t-T)^3-2AT_a^3+AT_b^2T,(T-{T_a}\leq{t}\leq{T}) \end{array}\right. \end{eqnarray*}$
3.matlab代码实现
指定位置、速度、斜率

clc; clear; %初始条件 x_arry=[0,10,20,30]; v_arry=[2,2,2]; A_arry=[3,3,3]; weiyi=[x_arry(1)];sudu=[0];shijian=[0];timeall=0;jiasudu=[0] for i=1:1:length(x_arry)-1; %清空 a=[];v=[];s=[]; %计算加减速段的时间和位移 L=x_arry(i+1)-x_arry(i); A=A_arry(i); vs=v_arry(i); Ta=sqrt(vs/A); L1=A*(Ta^3)/6; L2=A*(Ta^3)*(5/6); %计算整段轨迹的总位移 T=4*Ta+(L-2*L1-2*L2)/vs; for t=0:0.001:T if t<=Ta;%加加速度阶段 ad=A*t; vd=0.5*A*t^2; sd=(1/6)*A*t^3; a=[a,ad];v=[v,vd];s=[s,sd]; elseif t>Ta && t<=2*Ta;%加减速阶段 ad=-A*(t-2*Ta); vd=-0.5*A*(t-2*Ta)^2+A*Ta^2; sd=-(1/6)*A*(t-2*Ta)^3+A*Ta^2*t-A*Ta^3; a=[a,ad];v=[v,vd];s=[s,sd]; elseif t>2*Ta && t<=T-2*Ta;%匀速阶段 ad=0; vd=vs; sd=A*Ta^2*t-A*Ta^3; a=[a,ad];v=[v,vd];s=[s,sd]; elseif t>T-2*Ta && t<=T-Ta;%减加度阶段 ad=-A*(t-(T-2*Ta)); vd=-0.5*A*(t-T+2*Ta)^2+A*Ta^2; sd=-(1/6)*A*(t-T+2*Ta)^3+A*Ta^2*t-A*Ta^3; a=[a,ad];v=[v,vd];s=[s,sd]; elseif t>T-Ta && t<=T;%减减阶段 ad=A*(t-T); vd=0.5*A*(t-T)^2; sd=(1/6)*A*(t-T)^3-2*A*Ta^3+A*Ta^2*T; a=[a,ad];v=[v,vd];s=[s,sd]; end end %时间 time=[timeall:0.001:timeall+T]; timeall=timeall+T; %连接每一段轨迹 weiyi=[weiyi,s(2:end)+x_arry(i)]; sudu=[sudu,v(2:end)]; jiasudu=[jiasudu,a(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;