三次多项式插值轨迹

三次多项式

位置表达式

q(t)=k0+k1t+k2t2+k3t3q(t)=k_0+k_1t+k_2t^2+k_3t^3

速度表达式

q˙(t)=k1+2k2t+3k3t2\dot{q}(t)=k_1+2k_2t+3k_3t^2

加速度表达式

q¨(t)=2k2+6k3t\ddot{q}(t)=2k_2+6k_3t

其中 k0∼k3k_0\sim k_3 是未知系数

计算

给定起始和结束的位置，速度和轨迹的时间，可以构造出如下几个方程

q(t0)=q0q(t1)=q1q˙(t0)=q˙0q˙(t1)=q˙1t1−t0=Tq(t_0)=q_0\\q(t_1)=q_1\\\dot{q}(t_0)=\dot{q}_0\\\dot{q}(t_1)=\dot{q}_1\\t_1-t_0=T

假设初始时间 t0t_0 为 00 ，则上述方程写作矩阵格式为

[100001001TT2T3012T3T2][k0k1k2k3]=[q0q0˙q1q1˙]\begin{bmatrix}1&0&0&0\\0&1&0&0\\1&T&T^2&T^3\\0&1&2T&3T^2\end{bmatrix}\begin{bmatrix}k_0\\k_1\\k_2\\k_3\end{bmatrix}=\begin{bmatrix}q_0\\\dot{q_0}\\q_1\\\dot{q_1}\end{bmatrix}

求解得到如下结果

{k0=q0k1=q˙0k2=−3q0+3q1−2Tq˙0−Tq˙1T2k3=2q0−2q1+Tq˙0+Tq˙1T3\left\{\begin{aligned}&k_0=q_0\\&k_1=\dot{q}_0\\&k_2=\frac{-3q_0+3q_1-2T\dot{q}_0-T\dot{q}_1}{T^2}\\&k_3=\frac{2q_0-2q_1+T\dot{q}_0+T\dot{q}_1}{T^3}\end{aligned}\right.

最后可以得到任意时间的位置速度和加速度信息

q(t)=k0+k1t+k2t2+k3t3q˙(t)=k1+2k2t+3k3t2q¨(t)=2k2+6k3tq(t)=k_0+k_1t+k_2t^2+k_3t^3\\\dot{q}(t)=k_1+2k_2t+3k_3t^2\\\ddot{q}(t)=2k_2+6k_3t

优点

  • 轨迹平滑，速度和加速度连续
  • 计算简单，易于实现

缺点

  • 只能保证起点和终点的速度和位置约束，无法直接控制加速度
  • 对于复杂轨迹，可能需要分段使用三次多项式

五次多项式插值轨迹

五次多项式

位置表达式

q(t)=k0+k1t+k2t2+k3t3+k4t4+k5t5q(t)=k_0+k_1t+k_2t^2+k_3t^3+k_4t^4+k_5t^5

速度表达式

q˙(t)=k1+2k2t+3k3t2+4k4t3+5k5t4\dot{q}(t)=k_1+2k_2t+3k_3t^2+4k_4t^3+5k_5t^4

加速度表达式

q¨(t)=2k2+6k3t+12k4t2+20k5t3\ddot{q}(t)=2k_2+6k_3t+12k_4t^2+20k_5t^3

其中 k0∼k5k_0\sim k_5 是未知系数

计算

给定起始和结束的位置，速度和加速度以及轨迹的时间 TT ，可以构造如下七个方程

q(t0)=q0q(t1)=q1q˙(t0)=q˙0q˙(t1)=q˙1q¨(t0)=q¨0q¨(t1)=q¨1t1−t0=Tq(t_0)=q_0\\q(t_1)=q_1\\\dot{q}(t_0)=\dot{q}_0\\\dot{q}(t_1)=\dot{q}_1\\\ddot{q}(t_0)=\ddot{q}_0\\\ddot{q}(t_1)=\ddot{q}_1\\t_1-t_0=T

假设初始时间 t0t_0 为 00 ，则上述方程可以表示为

[1000000100000020001TT2T3T4T5012T3T24T35T40026T12T220T3][k0k1k2k3k4k5]=[q0q0˙q0¨q1q1˙q1¨]\begin{bmatrix}1&0&0&0&0&0\\0&1&0&0&0&0\\0&0&2&0&0&0\\1&T&T^2&T^3&T^4&T^5\\0&1&2T&3T^2&4T^3&5T^4\\0&0&2&6T&12T^2&20T^3\end{bmatrix}\begin{bmatrix}k_0\\k_1\\k_2\\k_3\\k_4\\k_5\end{bmatrix}=\begin{bmatrix}q_0\\\dot{q_0}\\\ddot{q_0}\\q_1\\\dot{q_1}\\\ddot{q_1}\end{bmatrix}

求解即可得到系数矩阵，如下

{k0=q0k1=q˙0k2=q¨02k3=−20q0+20q1−12Tq˙0−8Tq˙1−3T2q¨0+T2q¨12T3k4=30q0−30q1+16Tq˙0+14Tq˙1+3T2q¨0−2T2q¨12T4k5=−12q0+12q1−6Tq˙0−6Tq˙1−T2q¨0+T2q¨12T5\left\{\begin{aligned}&k_0=q_0\\&k_1=\dot{q}_0\\&k_2=\frac{\ddot{q}_0}{2}\\&k_3=\frac{-20q_0+20q_1-12T\dot{q}_0-8T\dot{q}_1-3T^2\ddot{q}_0+T^2\ddot{q}_1}{2T^3}\\&k_4=\frac{30q_0-30q_1+16T\dot{q}_0+14T\dot{q}_1+3T^2\ddot{q}_0-2T^2\ddot{q}_1}{2T^4}\\&k_5=\frac{-12q_0+12q_1-6T\dot{q}_0-6T\dot{q}_1-T^2\ddot{q}_0+T^2\ddot{q}_1}{2T^5}\end{aligned}\right.

代入可以得到任意时刻的位置速度和加速度

q(t)=k0+k1t+k2t2+k3t3+k4t4+k5t5q˙(t)=k1+2k2t+3k3t2+4k4t3+5k5t4q¨(t)=2k2+6k3t+12k4t2+20k5t3q(t)=k_0+k_1t+k_2t^2+k_3t^3+k_4t^4+k_5t^5\\\dot{q}(t)=k_1+2k_2t+3k_3t^2+4k_4t^3+5k_5t^4\\\ddot{q}(t)=2k_2+6k_3t+12k_4t^2+20k_5t^3

优点

  • 轨迹平滑，位置、速度和加速度均连续
  • 可以精确控制起点和终点的加速度
  • 适用于高精度和高动态要求的场景

缺点

  • 计算复杂度较高
  • 对于非常复杂的轨迹，可能需要分段使用五次多项式

梯形速度曲线轨迹

梯形速度曲线轨迹规划是工业控制领域内应用最为广泛的速度控制策略之一，标准形式的梯形速度规划包含匀加速，匀速和匀减速三个阶段，在变速过程中，加速度始终是人为设定的固定值。

  • 匀加速：加速度是恒定的正值，速度随着时间线性增加
  • 匀速：加速度为 0，速度大小恒定
  • 匀减速：加速度是恒定的负值，速度随着时间线性减小

需要定义一些曲线轨迹的参数

  • 初始位置 q0q_0 ，速度 v0v_0
  • 结束位置 q1q_1 ，速度 v1v_1
  • 最大加速度 aaa_{a} ，最大减速度 ada_d
  • 运行过程中期望最大速度 vfv_{f}
  • 运行过程中实际最大速度 vmaxv_{max}
  • 匀加速段时间 TaT_a ，匀减速段时间 TdT_d

轨迹设计

  1. 首先根据需要运行的距离来计算运行过程中是否具有匀速段，需要计算当只有匀加速和匀减速段时，达到最大速度时运行的距离 vf2−v022aa+v12−vf22ad\frac{v_{f}^2-v_0^2}{2a_a}+\frac{v_{1}^2-v_{f}^2}{2a_d} ，如果这个距离不大于期望距离 ∣q1−q0∣\vert q_1-q_0\vert ，那就没有匀速段，否则就存在匀速段
  2. 对于有匀速段的，计算各个段的位移和时间，注意此时实际最大速度达到了期望最大速度
    • 加速段： Ta=vf−v0aa,xa=vf2−v022aaT_a=\frac{v_{f}-v_0}{a_a},x_a=\frac{v_{f}^2-v_0^2}{2a_a}
    • 匀速段： Tv=∣q1−q0∣−(vf2−v022aa+v12−vf22ad)vf,xv=∣q1−q0∣−(vf2−v022aa+v12−vf22ad)T_v=\frac{\vert q_1-q_0\vert-(\frac{v_{f}^2-v_0^2}{2a_a}+\frac{v_{1}^2-v_{f}^2}{2a_d})}{v_{f}},x_v=\vert q_1-q_0\vert-(\frac{v_{f}^2-v_0^2}{2a_a}+\frac{v_{1}^2-v_{f}^2}{2a_d})
    • 减速段： Td=v1−vfad,xd=v12−vf22adT_d=\frac{v_1-v_{f}}{a_d},x_d=\frac{v_{1}^2-v_{f}^2}{2a_d}
  3. 对于没有匀速段的，各个段的位移和时间，此时最大速度并未达到期望最大速度 vmax=−(aa−ad)(adv02−aav12+2aaad∣q1−q0∣)aa−adv_{max}=\frac{\sqrt{-(a_a-a_d)(a_dv_0^2-a_av_1^2+2a_aa_d\vert q_1-q_0\vert)}}{a_a-a_d}
    • 加速段： Ta=vmax−v0aa,xa=v0Ta+12aaTa2T_a=\frac{v_{max}-v_0}{a_a},x_a=v_0T_a+\frac{1}{2}a_aT_a^2
    • 减速段： Td=v1−vmaxad,xd=∣q1−q0∣−xaT_d=\frac{v_1-v_{max}}{a_d},x_d=\vert q_1-q_0\vert-x_a

结果

  • 有匀速段

    {t∈[0,Ta]{q(t)=q0+v0t+12aat2q˙(t)=aatq¨(t)=aat∈[Ta,Ta+Tv]{q(t)=vf2−v022aa+vf(t−Ta)q˙(t)=vfq¨(t)=0t∈[Ta+Tv,Ta+Tv+Td]{q(t)=vf2−v022aa+vf(t−Ta)+12ad(t−Ta−Tv)2q˙(t)=vf+ad(t−Ta−Tv)q¨(t)=ad\left\{\begin{aligned}&t\in[0,T_a]\left\{\begin{aligned}&q(t)=q_0+v_0t+\frac{1}{2}a_at^2\\&\dot{q}(t)=a_at\\&\ddot{q}(t)=a_a\end{aligned}\right.\\&t\in[T_a,T_a+T_v]\left\{\begin{aligned}&q(t)=\frac{v_{f}^2-v_0^2}{2a_a}+v_f(t-T_a)\\&\dot{q}(t)=v_f\\&\ddot{q}(t)=0\end{aligned}\right.\\&t\in[T_a+T_v,T_a+T_v+T_d]\left\{\begin{aligned}&q(t)=\frac{v_{f}^2-v_0^2}{2a_a}+v_f(t-T_a)+\frac{1}{2}a_d(t-T_a-T_v)^2\\&\dot{q}(t)=v_f+a_d(t-T_a-T_v)\\&\ddot{q}(t)=a_d\end{aligned}\right.\end{aligned}\right.

  • 无匀速段

    {t∈[0,Ta]{q(t)=q0+v0t+12aat2q˙(t)=aatq¨(t)=aat∈[Ta,Ta+Td]{q(t)=v0Ta+12aaTa2+vmax(t−Ta)+12ad(t−Ta)2q˙(t)=vmax+ad(t−Ta)q¨(t)=ad\left\{\begin{aligned}&t\in[0,T_a]\left\{\begin{aligned}&q(t)=q_0+v_0t+\frac{1}{2}a_at^2\\&\dot{q}(t)=a_at\\&\ddot{q}(t)=a_a\end{aligned}\right.\\&t\in[T_a,T_a+T_d]\left\{\begin{aligned}&q(t)=v_0T_a+\frac{1}{2}a_aT_a^2+v_{max}(t-T_a)+\frac{1}{2}a_d(t-T_a)^2\\&\dot{q}(t)=v_{max}+a_d(t-T_a)\\&\ddot{q}(t)=a_d\end{aligned}\right.\end{aligned}\right.

优点

  • 计算简单，易于实现
  • 运动过程平滑，适合大多数工业应用
  • 可以通过调整加速度和最大速度来优化运动性能

缺点

  • 加速度突变可能导致机械振动
  • 对于高动态要求的场景，可能需要更复杂的轨迹规划方法（如S形速度曲线）

双 S 形速度曲线轨迹

双S形速度曲线轨迹，也被称为七段式轨迹或铃型规划（Bell），是一种由七段加加速度恒定的轨迹组成的平滑速度曲线。与梯形速度曲线相比，双S形速度曲线轨迹更加平滑，因为它将梯形轨迹的加速和减速段进一步细分为三段，从而避免了加速度的不连续变化

双 S 形速度曲线轨迹分为七个部分

  1. 加速段初期：加速度从 0 线性增加到最大加速度 aaa_a ，加加速度保持恒定 JJ ，时间为 T1T_1
  2. 加速段中期：加速度保持在最大加速度 aaa_a ，时间为 T2T_2
  3. 加速段末期：加速度从最大加速度线性减小到 0，加加速度保持恒定 −J-J ，时间为 T3T_3
  4. 匀速段：速度保持不变 vmv_m ，加速度保持为 0，加加速度保持为 0，时间为 T4T_4
  5. 减速段初期：加速度从 0 线性减小到最大减速度 ada_d ，加加速度保持恒定 −J-J ，时间为 T5T_5
  6. 减速段中期：加速度保持在最大减速度 ada_d ，时间为 T6T_6
  7. 减速段末期：加速度从最大减速度线性增加到 0，加加速度保持恒定 JJ ，时间为 T7T_7

曲线轨迹设计

  1. 判断是否有加速段中期：当只有加速段初期和末期时计算得到的最大速度 v=aa2Jv=\frac{a_a^2}{J}
    • 如果 v<vmv<v_m 则有加速段中期
      • 加速段初期时间 T1=aaJT_1=\frac{a_a}{J} ，距离为 x1=aa36J2x_1=\frac{a_a^3}{6J^2}
      • 加速段中期时间 T2=vm−vaaT_2=\frac{v_m-v}{a_a} ，距离为 x2=JT12T2+aaT222x_2=\frac{JT_1^2T_2+a_aT_2^2}{2}
      • 加速段末期时间 T3=aaJT_3=\frac{a_a}{J} ，距离为 x3=(JT122+aaT2)T3+aaT322−JT336x_3=(\frac{JT_1^2}{2}+a_aT_2)T_3+\frac{a_aT_3^2}{2}-\frac{JT_3^3}{6}
      • 则整个加速段的路程为 xa=x1+x2+x3x_a=x_1+x_2+x_3
    • 如果 v≥vmv\geq v_m 则无加速段中期，在中间达到最大加速度为 am=Jvma_m=\sqrt{Jv_m}
      • 加速段初期时间 T1=amJT_1=\frac{a_m}{J} ，距离为 x1=am36J2x_1=\frac{a_m^3}{6J^2}
      • 加速段末期时间 T3=amJT_3=\frac{a_m}{J} ，距离为 x3=JT12T32+aaT322−JT336x_3=\frac{JT_1^2T_3}{2}+\frac{a_aT_3^2}{2}-\frac{JT_3^3}{6}
      • 则整个加速段的路程为 xa=x1+x3x_a=x_1+x_3
  2. 判断是否有减速段中期，与加速段一致，计算减速的最大速度为 v=ad2Jv=\frac{a_d^2}{J}
    • 如果 v<vmv<v_m 则有减速段中期
      • 减速段初期时间 T5=adJT_5=\frac{a_d}{J} ，距离为 x5=vmT5−JT536x_5=v_mT_5-\frac{JT_5^3}{6}
      • 减速段中期时间 T6=vm−vadT_6=\frac{v_m-v}{a_d} ，距离为 x6=(vm−JT522)T6+adT622x_6=(v_m-\frac{JT_5^2}{2})T_6+\frac{a_dT_6^2}{2}
      • 减速段末期时间 T7=adJT_7=\frac{a_d}{J} ，距离为 x7=(vm−JT522+adT6)T7+adT722+JT736x_7=(v_m-\frac{JT_5^2}{2}+a_dT_6)T_7+\frac{a_dT_7^2}{2}+\frac{JT_7^3}{6}
      • 则整个减速段的路程为 xd=x5+x6+x7x_d=x_5+x_6+x_7
    • 如果 v≥vmv\geq v_m 则无加速段中期，在中间达到最大加速度为 am=−Jvma_m=-\sqrt{Jv_m}
      • 减速段初期时间 T5=−amJT_5=-\frac{a_m}{J} ，距离为 x5=vmT5−JT536x_5=v_mT_5-\frac{JT_5^3}{6}
      • 减速段末期时间 T7=−amJT_7=-\frac{a_m}{J} ，距离为 x7=(vm−JT522)T7+amT722+JT736x_7=(v_m-\frac{JT_5^2}{2})T_7+\frac{a_mT_7^2}{2}+\frac{JT_7^3}{6}
      • 则整个减速段的路程为 xd=x5+x7x_d=x_5+x_7
  3. 判断是否有匀速段：计算加速段和减速段走过的距离 x=xa+xdx=x_a+x_d
    • 如果期望距离 xf≤xx_f\leq x ，如果有加减速段中期的话，去掉加减速段中期，重新计算路程 xx ，再次判断。如果 xf≤xx_f\leq x 那么就没有匀速段
      • 根据上述可知 xf=xa+xdx_f=x_a+x_d ，此时加速段距离为 xa=5am36J2x_a=\frac{5a_m^3}{6J^2} ，减速段距离为 xd=2am3J2x_d=\frac{2a_m^3}{J^2} （注意此处的 am=Jvma_m=\sqrt{Jv_m} ）
      • 计算出最大的加速度和减速度为 am=3J2xf43a_m= \sqrt[3]{\frac{3J^2x_f}{4}} ，带入之后就可以计算出各个阶段的时间
    • 如果期望距离 xf>xx_f>x 那么就有匀速段
      • 匀速段距离为 xm=xf−xx_m=x_f-x
      • 匀速段时间为 T4=xmvmT_4=\frac{x_m}{v_m}

结果

对于完整流程的双 S 形曲线轨迹，公式如下

​{0≤t<t1{j(t)=Ja(t)=a0+Jtv(t)=v0+a0t+Jt22s(t)=s0+v0t+a0t22+Jt36v1=JT122s1=JT136t1≤t<t2{j(t)=0a(t)=aav(t)=v1+aa(t−t1)s(t)=s1+v1(t−t1)+aa(t−t1)22v2=v1+aaT2s2=s1+v1T2+aaT222t2≤t<t3{j(t)=−Ja(t)=aa−J(t−t2)v(t)=v2+aa(t−t2)−J(t−t2)22s(t)=s2+v2(t−t2)+aa(t−t2)22−J(t−t2)36v3=v2+aaT3−JT322s3=s2+v2T3+aaT322−JT336t3≤t<t4{j(t)=0a(t)=0v(t)=v3s(t)=s3+v3(t−t3)v4=v3s4=s3+v3T4t4≤t<t5{j(t)=−Ja(t)=−J(t−t4)v(t)=v4−J(t−t4)22s(t)=s4+v4(t−t4)−J(t−t4)36v5=v4−JT522s5=s4+v4T5−JT536t5≤t<t6{j(t)=0a(t)=adv(t)=v5+ad(t−t5)s(t)=s5+v5(t−t5)+ad(t−t5)22v6=v5+adT6s6=s5+v5T6+adT622t6≤t<t7{j(t)=Ja(t)=ad+J(t−t6)v(t)=v6+ad(t−t6)+J(t−t6)22s(t)=s6+v6(t−t6)+ad(t−t6)22+J(t−t6)36v7=v6+adT7+JT722s7=s6+v6T7+adT722+JT736​\left\{\begin{aligned}&0\leq t<t_1\left\{\begin{aligned}&j(t)=J\\&a(t)=a_0+Jt\\&v(t)=v_0+a_0t+\frac{Jt^2}{2}\\&s(t)=s_0+v_0t+\frac{a_0t^2}{2}+\frac{Jt^3}{6}\\&v_1=\frac{JT_1^2}{2}\\&s_1=\frac{JT_1^3}{6}\end{aligned}\right.\\&t_1\leq t<t_2\left\{\begin{aligned}&j(t)=0\\&a(t)=a_a\\&v(t)=v_1+a_a(t-t_1)\\&s(t)=s_1+v_1(t-t_1)+\frac{a_a(t-t_1)^2}{2}\\&v_2=v_1+a_aT_2\\&s_2=s_1+v_1T_2+\frac{a_aT_2^2}{2}\end{aligned}\right.\\&t_2\leq t<t_3\left\{\begin{aligned}&j(t)=-J\\&a(t)=a_a-J(t-t_2)\\&v(t)=v_2+a_a(t-t_2)-\frac{J(t-t_2)^2}{2}\\&s(t)=s_2+v_2(t-t_2)+\frac{a_a(t-t_2)^2}{2}-\frac{J(t-t_2)^3}{6}\\&v_3=v_2+a_aT_3-\frac{JT_3^2}{2}\\&s_3=s_2+v_2T_3+\frac{a_aT_3^2}{2}-\frac{JT_3^3}{6}\end{aligned}\right.\\&t_3\leq t<t_4\left\{\begin{aligned}&j(t)=0\\&a(t)=0\\&v(t)=v_3\\&s(t)=s_3+v_3(t-t_3)\\&v_4=v_3\\&s_4=s_3+v_3T_4\end{aligned}\right.\\&t_4\leq t<t_5\left\{\begin{aligned}&j(t)=-J\\&a(t)=-J(t-t_4)\\&v(t)=v_4-\frac{J(t - t_4)^2}{2}\\&s(t)=s_4+v_4(t-t_4)-\frac{J(t-t_4)^3}{6}\\&v_5=v_4-\frac{JT_5^2}{2}\\&s_5=s_4+v_4T_5-\frac{JT_5^3}{6}\end{aligned}\right.\\&t_5\leq t<t_6\left\{\begin{aligned}&j(t)=0\\&a(t)=a_d\\&v(t)=v_5+a_d(t-t_5)\\&s(t)=s_5+v_5(t-t_5)+\frac{a_d(t-t_5)^2}{2}\\&v_6=v_5+a_dT_6\\&s_6=s_5+v_5T_6+\frac{a_dT_6^2}{2}\end{aligned}\right.\\&t_6\leq t<t_7\left\{\begin{aligned}&j(t)=J\\&a(t)=a_d+J(t-t_6)\\&v(t)=v_6+a_d(t-t_6)+\frac{J(t - t_6)^2}{2}\\&s(t)=s_6+v_6(t-t_6)+\frac{a_d(t-t_6)^2}{2}+\frac{J(t-t_6)^3}{6}\\&v_7=v_6+a_dT_7+\frac{JT_7^2}{2}\\&s_7=s_6+v_6T_7+\frac{a_dT_7^2}{2}+\frac{JT_7^3}{6}\end{aligned}\right.\end{aligned}\right.

需要注意的是，我们一般在使用过程中会将初始速度和加速度，最终速度和加速度设置为 0

优点

  • 平滑性：减少机械冲击，延长设备寿命
  • 精确性：提高运动控制的精度
  • 适应性：适用于多种复杂运动场景