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

一、问题描述

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

二、推导步骤

  设归一化5次多项式为：
$$s(u)=a_0+a_{1}u+a_2{u}^2+a_3{u}^3+a_4{u}^4+a_5{u}^5,u\in [0,1]\text{\hspace{1em}(3)}$$
  一阶导数为：
$$s^{\prime }(u)=a_1+2a_{2}u+3a_3{u}^2+4a_4{u}^3+5a_5{u}^4\text{\hspace{1em}(4)}$$
  二阶导数为：
$$s^{\prime \prime }(u)=2a_2+6a_{3}u+12a_4{u}^2+20a_5{u}^3\text{\hspace{1em}(5)}$$
  根据式(1)得：
$$\{\begin{array}{l}{a}_0=0\\ a_1=0\\ a_2=0\\ a_0+a_1+a_2+a_3+a_4+a_5=1\\ a_1+2a_2+3a_3+4a_4+5a_5=0\\ 2a_2+6a_3+12a_4+20a_5=0\end{array}\text{\hspace{1em}(6)}$$
  由上式解得：$a_0=0,a_1=0,a_2=0,a_3=10,a_4=-15,a_5=6$
  归一化5次多项式为：
$$\{\begin{array}{l}s(u)=10u^3-15u^4+6u^5\\ s^{\prime }(u)=30u^2-60u^3+30u^4\\ s^{\prime \prime }(u)=60u-180u^2+120u^3\end{array}\text{\hspace{1em}(7)}$$
  令二阶导数$s^{\prime \prime }(u)=60u-180u^2+120u^3=0$，解得$u=0,1/2,1$，由于$s^{\prime }(0)=s^{\prime }(1)=0$，故在两端点处的速度不是最大速度，此时归一化5次多项式的速度最大值为：$s_{max}^{\prime }(u)=s^{\prime }(1/2)=30(1/2{)}^2-60(1/2{)}^3+30(1/2{)}^4=1.875$。
  令三阶导数$s^{\prime \prime \prime }(u)=60-360u+360u^2=0$，解得$u=(3\pm \sqrt{3})/6$，归一化5次多项式的加速度最大值为：$s_{max}^{\prime \prime }(u)=max(|s^{\prime \prime }((3+\sqrt{3})/6)|,|s^{\prime \prime }((3-\sqrt{3})/6)|)=5.7735$。
  因为：
$$l(t)=Ls(u)\text{\hspace{1em}(8)}$$
  两边对$t$求导得：
$$v(t)=L{s}^{\prime }(u)\frac{du}{dt}=\frac{L}{T}{s}^{\prime }(u)\text{\hspace{1em}(9)}$$
  上式两边对$t$再求导得：
$$acc(t)=\frac{L}{T}{s}^{\prime \prime }(u)\frac{du}{dt}=\frac{L}{T^2}{s}^{\prime \prime }(u)\text{\hspace{1em}(10)}$$
  为满足式(2)，则对于所有$u\in [0,1]$有：
$$\{\begin{array}{l}\frac{L}{T}{s}^{\prime }(u)\le v_{lim}\\ \\ \frac{L}{T^2}{s}^{\prime \prime }(u)\le a_{lim}\end{array}\text{\hspace{1em}(11)}$$
  上式写成：
$$\{\begin{array}{l}T\ge \frac{L{s}^{\prime }(u)}{v_{lim}}\\ \\ T\ge \sqrt{\frac{L{s}^{\prime \prime }(u)}{a_{lim}}}\end{array}\text{\hspace{1em}(12)}$$

  为使上式恒成立，有：
$$T\ge max(\frac{L{s}_{max}^{\prime }(u)}{v_{lim}},\sqrt{\frac{L{s}_{max}^{\prime \prime }(u)}{a_{lim}}})=max(\frac{1.875L}{v_{lim}},\sqrt{\frac{5.7735L}{a_{lim}}})\text{\hspace{1em}(13)}$$
  $T$取等号以达到时间最短的速度规划。

三、MATLAB代码

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

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

%{
s = a0 + a1*u + a2*u^2 + a3*u^3 + a4*u^4 + a5*u^5
a0 = -(2*pe*us^5 - 2*ps*ue^5 - 10*pe*ue*us^4 + 10*ps*ue^4*us - 2*ue*us^5*ve
    + 2*ue^5*us*vs + ae*ue^2*us^5 - 2*ae*ue^3*us^4 + ae*ue^4*us^3 - as*ue^3*us^4
    + 2*as*ue^4*us^3 - as*ue^5*us^2 + 20*pe*ue^2*us^3 - 20*ps*ue^3*us^2 + 10*ue^2*us^4*ve
    - 8*ue^3*us^3*ve + 8*ue^3*us^3*vs - 10*ue^4*us^2*vs)/(2*(ue - us)^5)
a1 = (2*ue^5*vs - 2*us^5*ve + 2*ae*ue*us^5 - 2*as*ue^5*us + 10*ue*us^4*ve
    - 10*ue^4*us*vs - ae*ue^2*us^4 - 4*ae*ue^3*us^3 + 3*ae*ue^4*us^2 - 3*as*ue^2*us^4
    + 4*as*ue^3*us^3 + as*ue^4*us^2 + 60*pe*ue^2*us^2 - 60*ps*ue^2*us^2 + 16*ue^2*us^3*ve
    - 24*ue^3*us^2*ve + 24*ue^2*us^3*vs - 16*ue^3*us^2*vs)/(2*(ue - us)^5)
a2 = -(ae*us^5 - as*ue^5 + 4*ae*ue*us^4 + 3*ae*ue^4*us - 3*as*ue*us^4 - 4*as*ue^4*us
    + 60*pe*ue*us^2 + 60*pe*ue^2*us - 60*ps*ue*us^2 - 60*ps*ue^2*us + 36*ue*us^3*ve
    - 24*ue^3*us*ve + 24*ue*us^3*vs - 36*ue^3*us*vs - 8*ae*ue^2*us^3 + 8*as*ue^3*us^2
    - 12*ue^2*us^2*ve + 12*ue^2*us^2*vs)/(2*(ue - us)^5)
a3 = (ae*ue^4 + 3*ae*us^4 - 3*as*ue^4 - as*us^4 + 20*pe*ue^2 + 20*pe*us^2 - 20*ps*ue^2
    - 20*ps*us^2 - 8*ue^3*ve - 12*ue^3*vs + 12*us^3*ve + 8*us^3*vs + 4*ae*ue^3*us - 4*as*ue*us^3
    + 28*ue*us^2*ve - 32*ue^2*us*ve + 32*ue*us^2*vs - 28*ue^2*us*vs - 8*ae*ue^2*us^2
    + 8*as*ue^2*us^2 + 80*pe*ue*us - 80*ps*ue*us)/(2*(ue - us)^5)
a4 = -(30*pe*ue + 30*pe*us - 30*ps*ue - 30*ps*us + 2*ae*ue^3 + 3*ae*us^3 - 3*as*ue^3
    - 2*as*us^3 - 14*ue^2*ve - 16*ue^2*vs + 16*us^2*ve + 14*us^2*vs - 4*ae*ue*us^2
    - ae*ue^2*us + as*ue*us^2 + 4*as*ue^2*us - 2*ue*us*ve + 2*ue*us*vs)/(2*(ue - us)^5)
a5 = (12*pe - 12*ps - 6*ue*ve - 6*ue*vs + 6*us*ve + 6*us*vs + ae*ue^2 + ae*us^2
    - as*ue^2 - as*us^2 - 2*ae*ue*us + 2*as*ue*us)/(2*(ue - us)^5)
    %}
    
%% 归一化5次多项式
us = 0;
ue = 1;
ps = 0;
pe = 1;
vs = 0;
ve = 0;
as = 0;
ae = 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 + a(5) * u.^4 + a(6) * u.^5;
v = a(2) + 2 * a(3) * u + 3 * a(4) * u.^2 + 4 * a(5) * u.^3 + 5 * a(6) * u.^4;
acc = 2 * a(3) + 6 * a(4) * u + 12 * a(5) * u.^2 + 20 * a(6) * u.^3;
figure(1)
set(gcf, 'color', 'w')
subplot(3, 1, 1)
plot(u, s)
hold on
plot([0 1], [1 1], 'r--')
plot([1 1], [0 1], 'r--')
title('归一化5次多项式位置曲线')
xlabel('u')
ylabel('s(u)')
box off

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

subplot(3, 1, 3)
plot(u, acc)
hold on
plot((3-sqrt(3))/6, 5.7735, 'ro')
plot([0 (3-sqrt(3))/6], [5.7735 5.7735], 'r--')
plot([(3-sqrt(3))/6 (3-sqrt(3))/6], [0 5.7735], 'r--')
plot((3+sqrt(3))/6, -5.7735, 'ro')
plot([0 (3+sqrt(3))/6], [-5.7735 -5.7735], 'r--')
plot([(3+sqrt(3))/6 (3+sqrt(3))/6], [0 -5.7735], 'r--')
plot([0 1], [0 0], 'g--')
title('归一化5次多项式加速度曲线')
xlabel('u')
ylabel('s''''(u)')
box off
    
%% 归一化5次多项式实例化
L = 100;        %mm
vlim = 200;     %mm/s
alim = 1500;    %mm/s^2
dt = 0.001;     %s
T = max([(1.875 * L) / vlim, sqrt(5.7735 * L / alim)]);
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 + a(5) * u.^4 + a(6) * u.^5;
v = a(2) + 2 * a(3) * u + 3 * a(4) * u.^2 + 4 * a(5) * u.^3 + 5 * a(6) * u.^4;
a = 2 * a(3) + 6 * a(4) * u + 12 * a(5) * u.^2 + 20 * a(6) * u.^3;
pos = L * s;
vel = (L / T) * v;
acc = (L / T^2) * a;
figure(2)
set(gcf, 'color', 'w')
subplot(3, 1, 1)
plot(t, pos)
hold on
plot([0 T], [L L], 'r--')
plot([T T], [0 L], 'r--')
title('5次多项式位置曲线')
xlabel('t/s')
ylabel('pos/mm')
box off

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

subplot(3, 1, 3)
plot(t, acc)
hold on
plot((3-sqrt(3))/6*T, 5.7735*(L / T^2), 'ro')
plot([0 (3-sqrt(3))/6*T], [5.7735*(L / T^2) 5.7735*(L / T^2)], 'r--')
plot([(3-sqrt(3))/6*T (3-sqrt(3))/6*T], [0 5.7735*(L / T^2)], 'r--')
plot((3+sqrt(3))/6*T, -5.7735*(L / T^2), 'ro')
plot([0 (3+sqrt(3))/6*T], [-5.7735*(L / T^2) -5.7735*(L / T^2)], 'r--')
plot([(3+sqrt(3))/6*T (3+sqrt(3))/6*T], [0 -5.7735*(L / T^2)], 'r--')
plot([0 T], [0 0], 'g--')
title('5次多项式加速度曲线')
xlabel('t/s')
ylabel('acc/ mm/s^2')
box off