归一化3次多项式速度规划

一、问题描述

  给定插补总长度$L$L，速度限制$v_{lim}$vlim​，计算总运动时间$T$T，规划归一化3次多项式$s(u)$s(u) ($u=t/T$u=t/T为归一化变量)，使其满足以下条件：
$\begin{cases} s(0)=0 \\ s'(0)=0 \\ s(1)=1 \\ s'(1)=0 \tag 1 \end{cases}$⎩⎪⎪⎪⎨⎪⎪⎪⎧​s(0)=0s′(0)=0s(1)=1s′(1)=0​(1)
  并使得，3次多项式轨迹$l(t)$l(t)的最大速度 $v_{max}$vmax​满足：
$v_{max}\leq v_{lim} \tag 2$vmax​≤vlim​(2)

二、推导步骤

  设归一化3次多项式为：
$s(u)=a_0+a_1u+a_2u^2+a_3u^3,u\in[0,1]\tag 3$s(u)=a0​+a1​u+a2​u2+a3​u3,u∈[0,1](3)
  一阶导数为：
$s'(u)=a_1+2a_2u+3a_3u^2\tag 4$s′(u)=a1​+2a2​u+3a3​u2(4)
  根据式(1)得：
$\begin{cases} a_0=0 \\ a_1=0 \\ a_0+a_1+a_2+a_3=1 \\ a_1+2a_2+3a_3=0 \\ \tag 5 \end{cases}$⎩⎪⎪⎪⎨⎪⎪⎪⎧​a0​=0a1​=0a0​+a1​+a2​+a3​=1a1​+2a2​+3a3​=0​(5)
  由上式解得：$a_0=0,a_1=0,a_2=3,a_3=-2$a0​=0,a1​=0,a2​=3,a3​=−2
  归一化3次多项式为：
$\begin{cases} s(u)=3u^2-2u^3 \\ s'(u)=6u-6u^2 \\ \tag 6 \end{cases}${s(u)=3u2−2u3s′(u)=6u−6u2​(6)
  令二阶导数$s''(u)=6-12u=0$s′′(u)=6−12u=0，解得$u=1/2$u=1/2，此时归一化3次多项式的速度最大值为$s_{max}'(u)=s'(1/2)=6(1/2)-6(1/2)^2=1.5$smax′​(u)=s′(1/2)=6(1/2)−6(1/2)2=1.5。
  因为：
$l(t)=Ls(u)\tag 7$l(t)=Ls(u)(7)
  两边对$t$t求导得：
$v(t)=Ls'(u)\frac{du}{dt}=\frac{L}{T}s'(u)\tag 8$v(t)=Ls′(u)dtdu​=TL​s′(u)(8)
  为满足式(2)，则对于所有$u\in[0,1]$u∈[0,1]有：
$\frac{L}{T}s'(u)\leq v_{lim} \tag 9$TL​s′(u)≤vlim​(9)
  上式写成：
$T\geq\frac{Ls'(u)}{v_{lim}} \tag {10}$T≥vlim​Ls′(u)​(10)
  为使上式恒成立，有：
$T\geq \frac{Ls_{max}'(u)}{v_{lim}}= \frac{1.5L}{v_{lim}}\tag {11}$T≥vlim​Lsmax′​(u)​=vlim​1.5L​(11)
  $T$T取等号以达到时间最短的速度规划。

三、MATLAB代码

clc;
clear;
close all;
syms us ue ps pe vs ve real

%% 通用3次多项式
a = [1, us, us^2, us^3
   0, 1, 2*us, 3*us^2
   1, ue, ue^2, ue^3
   0, 1, 2*ue, 3*ue^2] \ [ps; vs; pe; ve];
a = [simplify(a(1)), simplify(a(2)), simplify(a(3)), simplify(a(4))]'

%{
s = a0 + a1*u + a2*u^2 + a3*u^3
a0 = -(pe*us^3 - ps*ue^3 - 3*pe*ue*us^2 + 3*ps*ue^2*us - ue*us^3*ve
    + ue^3*us*vs + ue^2*us^2*ve - ue^2*us^2*vs)/(ue - us)^3
a1 = (ue^3*vs - us^3*ve - ue*us^2*ve + 2*ue^2*us*ve - 2*ue*us^2*vs
    + ue^2*us*vs - 6*pe*ue*us + 6*ps*ue*us)/(ue - us)^3
a2 = (3*pe*ue + 3*pe*us - 3*ps*ue - 3*ps*us - ue^2*ve - 2*ue^2*vs
    + 2*us^2*ve + us^2*vs - ue*us*ve + ue*us*vs)/(ue - us)^3
a3 = -(2*pe - 2*ps - ue*ve - ue*vs + us*ve + us*vs)/(ue - us)^3
%}

%% 归一化3次多项式
us = 0;
ue = 1;
ps = 0;
pe = 1;
vs = 0;
ve = 0;
a = eval(a)

deltaU = 0.01;
u = 0 : deltaU : 1;
s = a(1) + a(2) * u + a(3) * u.^2 + a(4) * u.^3;
v = a(2) + 2 * a(3) * u + 3 * a(4) * u.^2;
figure(1)
set(gcf, 'color', 'w')
subplot(2, 1, 1)
plot(u, s)
hold on
plot([0 1], [1 1], 'r--')
plot([1 1], [0 1], 'r--')
title('归一化3次多项式位置曲线')
xlabel('u')
ylabel('s(u)')
box off

subplot(2, 1, 2)
plot(u, v)
hold on
plot(0.5, 1.5, 'ro')
plot([0 0.5], [1.5 1.5], 'r--')
plot([0.5 0.5], [0 1.5], 'r--')
title('归一化3次多项式速度曲线')
xlabel('u')
ylabel('s''(u)')
box off

%% 归一化3次多项式实例化
L = 100;        %mm
vlim = 200;     %mm/s
dt = 0.001;     %s
T = (1.5 * L) / vlim;
t = 0 : dt : T;
u = t / T;
if abs(t(end) - T) > 1.0e-8
   t = [t, T];
   u = [u, 1];
end
s = a(1) + a(2) * u + a(3) * u.^2 + a(4) * u.^3;
v = a(2) + 2 * a(3) * u + 3 * a(4) * u.^2;
pos = L * s;
vel = (L / T) * v;

figure(2)
set(gcf, 'color', 'w')
subplot(2, 1, 1)
plot(t, pos)
hold on
plot([0 T], [L L], 'r--')
plot([T T], [0 L], 'r--')
title('3次多项式位置曲线')
xlabel('t/s')
ylabel('pos/mm')
box off

subplot(2, 1, 2)
plot(t, vel)
hold on
plot(0.5*T, 1.5*L/T, 'ro')
plot([0 0.5*T], [1.5*L/T 1.5*L/T], 'r--')
plot([0.5*T 0.5*T], [0 1.5*L/T], 'r--')
title('3次多项式速度曲线')
xlabel('t/s')
ylabel('vel/ mm/s')
box off