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

一、问题描述

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

二、推导步骤

  设归一化7次多项式为：
$$s(u)=a_0+a_{1}u+a_2{u}^2+a_3{u}^3+a_4{u}^4+a_5{u}^5+a_6{u}^6+a_7{u}^7,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+6a_6{u}^5+7a_7{u}^6\text{\hspace{1em}(4)}$$
  二阶导数为：
$$s^{\prime \prime }(u)=2a_2+6a_{3}u+12a_4{u}^2+20a_5{u}^3+30a_6{u}^4+42a_7{u}^5\text{\hspace{1em}(5)}$$
  三阶导数为：
$$s^{\prime \prime \prime }(u)=6a_3+24a_{4}u+60a_5{u}^2+120a_6{u}^3+210a_7{u}^4\text{\hspace{1em}(6)}$$
  根据式(1)得：
$$\{\begin{array}{l}{a}_0=0\\ a_1=0\\ a_2=0\\ a_3=0\\ a_0+a_1+a_2+a_3+a_4+a_5+a_6+a_7=1\\ a_1+2a_2+3a_3+4a_4+5a_5+6a_6+7a_7=0\\ 2a_2+6a_3+12a_4+20a_5+30a_6+42a_7=0\\ 6a_3+24a_4+60a_5+120a_6+210a_7=0\end{array}\text{\hspace{1em}(7)}$$
  由上式解得：$a_0=0,a_1=0,a_2=0,a_3=0,a_4=35,a_5=-84,a_6=70,a_7=-20$
  归一化7次多项式为：
$$\{\begin{array}{l}s(u)=35u^4-84u^5+70u^6-20u^7\\ s^{\prime }(u)=140u^3-420u^4+420u^5-140u^6\\ s^{\prime \prime }(u)=420u^2-1680u^3+2100u^4-840u^5\\ s^{\prime \prime \prime }(u)=840u-5040u^2+8400u^3-4200u^4\end{array}\text{\hspace{1em}(8)}$$
  令二阶导数$s^{\prime \prime }(u)=420u^2-1680u^3+2100u^4-840u^5=0$，解得$u=0,0,1/2,1,1$，由于$s^{\prime }(0)=s^{\prime }(1)=0$，故在两端点处的速度不是最大速度，此时归一化7次多项式的速度最大值为：$s_{max}^{\prime }(u)=s^{\prime }(1/2)=140(1/2{)}^3-420(1/2{)}^4+420(1/2{)}^5-140(1/2{)}^6=2.1875$。
  令三阶导数$s^{\prime \prime \prime }(u)=840u-5040u^2+8400u^3-4200u^4=0$，解得$u=0,0.2764,0.7236,1$，由于$s^{\prime \prime }(0)=s^{\prime \prime }(1)=0$，故在两端点处的加速度不是最大加速度，归一化7次多项式的加速度最大值为：$s_{max}^{\prime \prime }(u)=max(|s^{\prime \prime }(0.2764)|,|s^{\prime \prime }(0.7236)|)=7.5132$。
  令四阶导数$s^{\prime \prime \prime \prime }(u)=840-10080u+25200u^2-16800u^3=0$，解得$u=0.1127,0.5,0.8873$。归一化7次多项式的加加速度最大值为：$s_{max}^{\prime \prime \prime }(u)=max(|s^{\prime \prime \prime }(0.1127)|,|s^{\prime \prime \prime }(0.5)|,|s^{\prime \prime \prime }(0.8873)|)=52.5$。
  因为：
$$l(t)=Ls(u)\text{\hspace{1em}(9)}$$
  两边对$t$求导得：
$$v(t)=L{s}^{\prime }(u)\frac{du}{dt}=\frac{L}{T}{s}^{\prime }(u)\text{\hspace{1em}(10)}$$
  上式两边对$t$再求导得：
$$acc(t)=\frac{L}{T}{s}^{\prime \prime }(u)\frac{du}{dt}=\frac{L}{T^2}{s}^{\prime \prime }(u)\text{\hspace{1em}(11)}$$
  上式两边对$t$再求导得：
$$jerk(t)=\frac{L}{T^2}{s}^{\prime \prime \prime }(u)\frac{du}{dt}=\frac{L}{T^3}{s}^{\prime \prime \prime }(u)\text{\hspace{1em}(12)}$$
  为满足式(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}(13)}$$
  上式写成：
$$\{\begin{array}{l}T\ge \frac{L{s}^{\prime }(u)}{v_{lim}}\\ \\ T\ge \sqrt{\frac{L{s}^{\prime \prime }(u)}{a_{lim}}}\\ \\ T\ge \sqrt[3]{\frac{L{s}^{\prime \prime \prime }(u)}{j_{lim}}}\end{array}\text{\hspace{1em}(14)}$$

  为使上式恒成立，有：
$$\begin{gathered} T\ge max(\frac{L{s}_{max}^{\prime }(u)}{v_{lim}},\sqrt{\frac{L{s}_{max}^{\prime \prime }(u)}{a_{lim}}},\sqrt[3]{\frac{L{s}_{max}^{\prime \prime \prime }(u)}{j_{lim}}}) \\ =max(\frac{2.1875L}{v_{lim}},\sqrt{\frac{7.5132L}{a_{lim}}},\sqrt[3]{\frac{52.5L}{j_{lim}}})\text{\hspace{1em}(15)} \end{gathered}$$
  $T$取等号以达到时间最短的速度规划。

三、MATLAB代码

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

%% 通用7次多项式
a = [1, us, us^2, us^3, us^4, us^5, us^6, us^7
    0, 1, 2*us, 3*us^2, 4*us^3, 5*us^4, 6*us^5, 7*us^6 
    0, 0, 2, 6*us, 12*us^2, 20*us^3, 30*us^4, 42*us^5 
    0, 0, 0, 6, 24*us, 60*us^2, 120*us^3, 210*us^4
    1, ue, ue^2, ue^3, ue^4, ue^5, ue^6, ue^7
    0, 1, 2*ue, 3*ue^2, 4*ue^3, 5*ue^4, 6*ue^5, 7*ue^6 
    0, 0, 2, 6*ue, 12*ue^2, 20*ue^3, 30*ue^4, 42*ue^5 
    0, 0, 0, 6, 24*ue, 60*ue^2, 120*ue^3, 210*ue^4] \ [ps; vs; as; js; pe; ve; ae; je];
a = [simplify(a(1)), simplify(a(2)), simplify(a(3)), simplify(a(4)), ...
    simplify(a(5)), simplify(a(6)), simplify(a(7)), simplify(a(8))]'

%% 归一化7次多项式
us = 0;
ue = 1;
ps = 0;
pe = 1;
vs = 0;
ve = 0;
as = 0;
ae = 0;
js = 0;
je = 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 + a(7) * u.^6 + a(8) * u.^7;
v = a(2) + 2 * a(3) * u + 3 * a(4) * u.^2 + 4 * a(5) * u.^3 + 5 * a(6) * u.^4 + 6 * a(7) * u.^5 + 7 * a(8) * u.^6;
acc = 2 * a(3) + 6 * a(4) * u + 12 * a(5) * u.^2 + 20 * a(6) * u.^3 + 30 * a(7) * u.^4 + 42 * a(8) * u.^5;
jerk = 6 * a(4) + 24 * a(5) * u + 60 * a(6) * u.^2 + 120 * a(7) * u.^3 + 210 * a(8) * u.^4;

figure(1)
set(gcf, 'color', 'w')
subplot(4, 1, 1)
plot(u, s)
hold on
plot([0 1], [1 1], 'r--')
plot([1 1], [0 1], 'r--')
title('归一化7次多项式位置曲线')
xlabel('u')
ylabel('s(u)')
box off

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

subplot(4, 1, 3)
plot(u, acc)
hold on
plot(0.2764, 7.5132, 'ro')
plot([0 0.2764], [7.5132 7.5132], 'r--')
plot([0.2764 0.2764], [0 7.5132], 'r--')
plot(0.7236, -7.5132, 'ro')
plot([0 0.7236], [-7.5132 -7.5132], 'r--')
plot([0.7236 0.7236], [0 -7.5132], 'r--')
plot([0 1], [0 0], 'g--')
title('归一化7次多项式加速度曲线')
xlabel('u')
ylabel('s''''(u)')
box off

subplot(4, 1, 4)
plot(u, jerk)
hold on
plot(0.5, -52.5, 'ro')
plot([0 0.5], [-52.5 -52.5], 'r--')
plot([0.5 0.5], [0 -52.5], 'r--')
title('归一化7次多项式加加速度曲线')
plot([0 1], [0 0], 'g--')
xlabel('u')
ylabel('s''''''(u)')
box off
    
%% 归一化7次多项式实例化
L = 100;        %mm
vlim = 200;     %mm/s
alim = 1500;    %mm/s^2
jlim = 15000;    %mm/s^3
dt = 0.001;     %s
T = max([(2.1875 * L) / vlim, sqrt(7.5132 * L / alim), (52.5 * L / jlim)^(1/3)]);
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 + a(7) * u.^6 + a(8) * u.^7;
v = a(2) + 2 * a(3) * u + 3 * a(4) * u.^2 + 4 * a(5) * u.^3 + 5 * a(6) * u.^4 + 6 * a(7) * u.^5 + 7 * a(8) * u.^6;
acc = 2 * a(3) + 6 * a(4) * u + 12 * a(5) * u.^2 + 20 * a(6) * u.^3 + 30 * a(7) * u.^4 + 42 * a(8) * u.^5;
jerk = 6 * a(4) + 24 * a(5) * u + 60 * a(6) * u.^2 + 120 * a(7) * u.^3 + 210 * a(8) * u.^4;

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

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

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

subplot(4, 1, 4)
plot(t, jerk)
hold on
plot(0.5*T, -52.5*(L / T^3), 'ro')
plot([0 0.5*T], [-52.5*(L / T^3) -52.5*(L / T^3)], 'r--')
plot([0.5*T 0.5*T], [0 -52.5*(L / T^3)], 'r--')
plot([0 T], [0 0], 'g--')
title('7次多项式加加速度曲线')
xlabel('t/s')
ylabel('jerk/ mm/s^3')
box off