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

一、问题描述

  给定插补总长度$L$，速度限制$v_{lim}$，计算总运动时间$T$，规划归一化3次多项式$s(u)$ ($u=t/T$为归一化变量)，使其满足以下条件：
$$\{\begin{array}{l}s(0)=0\\ s^{\prime }(0)=0\\ s(1)=1\\ s^{\prime }(1)=0\end{array}\text{\hspace{1em}(1)}$$
  并使得，3次多项式轨迹$l(t)$的最大速度 $v_{max}$满足：
$$v_{max}\le v_{lim}\text{\hspace{1em}(2)}$$

二、推导步骤

  设归一化3次多项式为：
$$s(u)=a_0+a_{1}u+a_2{u}^2+a_3{u}^3,u\in [0,1]\text{\hspace{1em}(3)}$$
  一阶导数为：
$$s^{\prime }(u)=a_1+2a_{2}u+3a_3{u}^2\text{\hspace{1em}(4)}$$
  根据式(1)得：
$$\{\begin{array}{l}{a}_0=0\\ a_1=0\\ a_0+a_1+a_2+a_3=1\\ a_1+2a_2+3a_3=0\end{array}\text{\hspace{1em}(5)}$$
  由上式解得：$a_0=0,a_1=0,a_2=3,a_3=-2$
  归一化3次多项式为：
$$\{\begin{array}{l}s(u)=3u^2-2u^3\\ s^{\prime }(u)=6u-6u^2\end{array}\text{\hspace{1em}(6)}$$
  令二阶导数$s^{\prime \prime }(u)=6-12u=0$，解得$u=1/2$，此时归一化3次多项式的速度最大值为$s_{max}^{\prime }(u)=s^{\prime }(1/2)=6(1/2)-6(1/2{)}^2=1.5$。
  因为：
$$l(t)=Ls(u)\text{\hspace{1em}(7)}$$
  两边对$t$求导得：
$$v(t)=L{s}^{\prime }(u)\frac{du}{dt}=\frac{L}{T}{s}^{\prime }(u)\text{\hspace{1em}(8)}$$
  为满足式(2)，则对于所有$u\in [0,1]$有：
$$\frac{L}{T}{s}^{\prime }(u)\le v_{lim}\text{\hspace{1em}(9)}$$
  上式写成：
$$T\ge \frac{L{s}^{\prime }(u)}{v_{lim}}\text{\hspace{1em}(10)}$$
  为使上式恒成立，有：
$$T\ge \frac{L{s}_{max}^{\prime }(u)}{v_{lim}}=\frac{1.5L}{v_{lim}}\text{\hspace{1em}(11)}$$
  $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