机器人轨迹规划——双S曲线轨迹（详细推导）

轨迹规划——双S曲线轨迹（详细推导）"/>

机器人轨迹规划——双S曲线轨迹（详细推导）

机器人轨迹

这一系列轨迹教程将主要包括以下内容：

• 机器人运动规划技术介绍

• 点到点轨迹（P2P）

  • 五次多项式插值轨迹
  • 三次多项式插值轨迹
  • 梯形速度曲线轨迹
  • 双S曲线轨迹
  • 多个自由度轨迹的时间同步

• 在线轨迹规划

  • 多项式在线轨迹规划
  • 梯形在线轨迹规划
  • 双S形在线轨迹规划
  • 非线性实时轨迹滤波

• 多点轨迹（Multi-point)

  • 三次样条曲线（cubic spline）
  • 贝赛尔曲线（Bezier Curve）
  • B样条曲线（BSpline）

• 时间最优轨迹

  • 三次样条时间最优轨迹
  • 任意路径下的时间最优轨迹
  • 时间最优停止轨迹
    • 不约束路径
    • 有路径约束

双S曲线轨迹（Trajectory with Double S Velocity Profile)

双S轨迹相比梯形轨迹，更加的平滑，梯形轨迹本质上是将一段点到点的轨迹分为三段（加速——匀速——减速）；而双S轨迹本质上是分为了七段，中间的匀速段是不变的，但是它将梯形轨迹的加速、减速段分为三段。以加速段为例，先是以最大的加加速度达到最大的加速度，然后以恒加速度进行加速，然后以最大的加加速度将加速度减到零。上面一段话看着比较晕，中文里面加加速度指的是对加速度求导，也称为跃度，对应的是英文里面的jerk。顺便整理了一下其他更高阶次的导数及其名称。

双S轨迹定义

一段典型的双S轨迹分为七段，每段的jerk是一个恒定值，要么为 j m a x j_{max} jmax​或者 j m i n j_{min} jmin​要么是0，在这里我们假设

1. 轨迹起始和结束的加速度都是0，否则计算需要考虑的状况会更多更复杂
2. j m a x j_{max} jmax​ = − j m i n -j_{min} −jmin​

那么这七段分别为：

• 加速阶段，记此段总时间为 T a T_a Ta​，速度从 v 0 v_0 v0​到 v l i m v_{lim} vlim​，分为以下三个阶段：
  • Phase1: j = j m a x j=j_{max} j=jmax​，加速度从0到 a m a x a_{max} amax​，记此段时间为 T j 1 T_{j1} Tj1​
  • Phase2: j = 0 j=0 j=0，匀加速阶段，记此段时间为 T a 1 T_{a1} Ta1​
  • Phase3: j = j m i n j=j_{min} j=jmin​，加速度从 a m a x a_{max} amax​到0，速度达到最大的速度 v l i m v_{lim} vlim​
• 匀速阶段，记此段总时间为 T v T_v Tv​，分为一个阶段：
  • Phase4: j = 0 , a = 0 , v = v l i m j=0,a=0,v=v_{lim} j=0,a=0,v=vlim​
• 减速阶段，记此段总时间为 T d T_d Td​，速度从 v l i m v_{lim} vlim​到 v 1 v_1 v1​，类似加速阶段，分为三个阶段：
  • Phase5: j = j m i n j=j_{min} j=jmin​，加速度从0到 a m i n a_{min} amin​，记此段时间为 T j 2 T_{j2} Tj2​
  • Phase6: j = 0 j=0 j=0，恒减速阶段
  • Phase7: j = j m a x j=j_{max} j=jmax​，加速度从 a m a x a_{max} amax​到0，速度达到终点速度 v 1 v_1 v1​

上面 Phase1和Phase3如果存在的话，时间肯定是相同的，因为一个是 a = 0 → a = a m a x a=0 \rightarrow a=a_{max} a=0→a=amax​，另一个是 a = a m a x → a = 0 a=a_{max}\rightarrow a=0 a=amax​→a=0，且 ∣ j m a x ∣ = ∣ j m i n ∣ |j_{max}| = |j_{min}| ∣jmax​∣=∣jmin​∣，两端的时间分别为：
a m a x − 0 j m a x ≡ 0 − a m a x − j m a x ≡ 0 − a m a x j m i n \frac {a_{max}-0}{j_{max}} \equiv \frac {0-a_{max}}{-j_{max}} \equiv \frac {0-a_{max}}{j_{min}} jmax​amax​−0​≡−jmax​0−amax​​≡jmin​0−amax​​

一般会给定以下参数的：
q 0 : 起点位置 q 1 : 终点位置 v 0 : 起点速度 v 1 ：终点速度 v m a x ：最大速度 a m a x ：最大加速度 j m a x ：最大加加速度 同时假设起始和结束的加速度 a 0 = a 1 = 0 (1) \begin{aligned} q_0 &: 起点位置\\ q_1 &: 终点位置\\ v_0 &: 起点速度\\ v_1 &：终点速度\\ v_{max} &：最大速度\\ a_{max} &：最大加速度\\ j_{max} &：最大加加速度\\ \\ 同时假设起始和结束的加速度\\ &a_0=a_1=0 \end{aligned}\tag{1} q0​q1​v0​v1​vmax​amax​jmax​同时假设起始和结束的加速度​:起点位置:终点位置:起点速度：终点速度：最大速度：最大加速度：最大加加速度a0​=a1​=0​(1)

给定以上参数，如果能够计算出 T j 1 , T j 2 T_{j1},T_{j2} Tj1​,Tj2​，那么可以计算出来其他各段的时间，在依次将jerk进行一次积分，可得加速度表达式；加速度进行一次积分，可得速度表达式；速度进行一次积分，最终可以得到位置的表达式。

同时我们需要注意到，不是每个双S轨迹都存在七段，有的时候可能只有其中的几段；极端情况下，双S轨迹可能是算不出来的，比如说 q 0 q_0 q0​和 q 1 q_1 q1​相差很小，而 v 0 v_0 v0​和 v 1 v_1 v1​相差很大，即便先以 j m a x j_{max} jmax​进行加速，然后立即以 j m i n j_{min} jmin​进行减速，速度可能达到了，但是位置可能已经超出了。在计算双S轨迹的时候，需要针对各种状况进行讨论，我们设置起始和结束的加速度为0，是为了减少讨论的分支情况，而且大多数场景下，起始和结束的加速度就是0，针对起始和结束加速度不为0的情况的计算，我们以后再讨论。

时间最优双S轨迹的计算

给定（1），要求计算一段时间最短的双S轨迹，这是最常见的。

首先我们讨论一下，jerk为0的阶段是否存在；临界情况是，以最大的jerk进行加速，然后立即以最小的jerk进行减速，也即是说没有匀速阶段，只有加速阶段（这里我们假设 v 1 > v 0 v_1 > v_0 v1​>v0​），且加速阶段也只有两个子阶段，没有恒加速阶段，即 {Phase1，Phase2}。

我们定义以下两个值：
v 1 − v 0 = 1 2 j m a x T j 1 ∗ 2 − 1 2 j m i n T j 1 ∗ 2 j m i n = − j m a x ⟹ T j 1 ∗ = ∣ v 1 − v 0 ∣ j m a x T j 1 ⋆ = a m a x j m a x T j 1 ⋆ = m i n { T j 1 ⋆ , T j 1 ∗ } (2) \begin{aligned} v_1-v_0 &= \frac {1}{2}j_{max}T_{j1\ast}^2 - \frac{1}{2} j_{min} T_{j1\ast}^2\\ j_{min} &= - j_{max} \\ \implies T_{j1\ast} &= \sqrt{\frac {|v_1-v_0|} {j_{max}}}\\ T_{j1\star} &= \frac {a_{max}}{j_{max}} \\ \\ T_{j1}^{\star} &= min \{T_{j1\star},T_{j1\ast} \} \end{aligned}\tag{2} v1​−v0​jmin​⟹Tj1∗​Tj1⋆​Tj1⋆​​=21​jmax​Tj1∗2​−21​jmin​Tj1∗2​=−jmax​=jmax​∣v1​−v0​∣​ ​=jmax​amax​​=min{Tj1⋆​,Tj1∗​}​(2)

公式（2）中的 T j 1 ∗ T_{j1\ast} Tj1∗​就是只存在两段的情况。

如果 T j 1 ⋆ = T j 1 ⋆ T_{j1}^\star = T_{j1\star} Tj1⋆​=Tj1⋆​，说明存在jerk为0的阶段；否则就是只有两段。

上面的计算中，我们只考虑了速度，没有考虑位置；实际在这两种情况下，位移都需要大于一个值才有可能计算出双S轨迹。

位移需要满足下面的情况：
h = q 1 − q 0 > { ( v 0 T j ⋆ + 1 6 j m a x T j ⋆ 3 ) + ( ( v 0 + 1 2 j m a x T j ⋆ 2 ) T j ⋆ + 1 2 ( j m a x T j ⋆ ) T j ⋆ 2 + 1 6 j m i n T j ⋆ 3 ) = T j ⋆ ( v 1 + v 0 ) ， T j ⋆ < a m a x j m a x 1 2 ( v 0 + v 1 ) [ T j ⋆ + ∣ v 1 − v 0 ∣ a m a x ] , T j ⋆ = a m a x j m a x (3) \begin{aligned} h = q_1 - q0 > \begin{cases} \left(v_0 T_{j}^\star + \frac {1}{6}j_{max}T_{j}^{\star3}\right) + \left((v_0+\frac {1}{2}j_{max}T_{j}^{\star2}) T_{j}^\star + \frac {1}{2} (j_{max}T_{j}^{\star})T_{j}^{\star2} + \frac {1}{6} j_{min} T_{j}^{\star3} \right) = T_j^\star(v_1+v_0)，& T_j^\star < \frac {a_{max}}{j_{max}}\\ \frac {1}{2}(v_0+v_1)[T_j^\star+\frac {|v_1-v_0|}{a_{max}}], & T_j^\star = \frac {a_{max}}{j_{max}} \end{cases} \end{aligned}\tag{3} h=q1​−q0>{(v0​Tj⋆​+61​jmax​Tj⋆3​)+((v0​+21​jmax​Tj⋆2​)Tj⋆​+21​(jmax​Tj⋆​)Tj⋆2​+61​jmin​Tj⋆3​)=Tj⋆​(v1​+v0​)，21​(v0​+v1​)[Tj⋆​+amax​∣v1​−v0​∣​],​Tj⋆​<jmax​amax​​Tj⋆​=jmax​amax​​​​(3)
上面的计算都是按照下面的基本公式化简出来的：
q = q 0 + v 0 t + 1 2 a 0 t 2 + 1 6 j t 3 (3-1) q = q_0 + v_0t+\frac {1}{2}a_0t^2 + \frac{1}{6}jt^3 \tag{3-1} q=q0​+v0​t+21​a0​t2+61​jt3(3-1)
其中 T j ⋆ < a m a x j m a x T_j^\star<\frac {a_{max}}{j_{max}} Tj⋆​<jmax​amax​​ 的情况较好推导，因为就只包括两段。

下面推导一下对于 T j ⋆ = a m a x j m a x T_j^\star = \frac {a_{max}}{j_{max}} Tj⋆​=jmax​amax​​ 的情况。这种情况包含三段，这三段的中每段的起始点的速度、加速度、jerk分别如下：

其中三段的运动时间长度和位移分别是：

上面表格中Phase2阶段的时间是根据 v = v 0 + 1 2 j m a x T j ⋆ 2 + a m a x T a 1 + a m a x T j ⋆ − 1 2 j m a x T j ⋆ 2 = v 1 v = v_0 + \frac {1}{2}j_{max} {T_j^\star}^2 + a_{max}T_{a1} + a_{max}T_j^\star - \frac {1}{2}j_{max}{T_j^\star}^2=v_1 v=v0​+21​jmax​Tj⋆​2+amax​Ta1​+amax​Tj⋆​−21​jmax​Tj⋆​2=v1​计算得到的。

总的位移是 δ q = δ q 1 + δ q 2 + δ q 3 \delta_q= \delta_{q1}+\delta_{q2}+\delta_{q3} δq​=δq1​+δq2​+δq3​，经过一番化简，最终会得到（3）中的第二个条件。

到现在我们已经推导出来给定约束以后，能否计算出来双S轨迹，接着就需要针对不同情况的计算方式进行推导。

最大速度是否能够达到

最大速度 v l i m = v m a x v_{lim}=v_{max} vlim​=vmax​

这种情况下再讨论下最大的加速度是否达到，即加速和减速阶段的最大加速度是否达到 a m a x , a m i n a_{max}, a_{min} amax​,amin​ 。

显然加速度越大，越容易达到最大速度，那么要达到最大加速度的条件就是加速阶段起始和结束的速度有足够的差值，否则，稍微加速就达到了最大速度，那么加速度的最大值自然是不用达到的，临界情况是在加速阶段正好达到最大加速度然后又立即以最小的加加速度将加速度减小到0，此时如果结束的速度小于 v m a x v_{max} vmax​ ，那么说明存在匀加速阶段，否则不存在匀加速阶段：
{ v 0 + a m a x 2 × 2 × a m a x j m a x > v m a x ⟹ ( v m a x − v 0 ) j m a x < a m a x 2 , a m a x 没有达到 ( 4 − 1 ) v m a x + a m i n 2 × 2 × a m i n j m i n < v 1 ⟹ ( v m a x − v 1 ) j m a x < a m a x 2 , a m i n 没有达到 ( 4 − 2 ) \begin{aligned} \begin{cases} v_0 + \frac {a_{max}}{2} \times 2 \times \frac {a_{max}}{j_{max}} > v_{max} \implies (v_{max}-v_0)j_{max} < a_{max}^2, &a_{max} 没有达到 &(4-1)\\ v_{max} + \frac {a_{min}}{2} \times 2 \times \frac {a_{min}}{j_{min}} < v_1 \implies (v_{max} - v_1)j_{max} < a_{max}^2, &a_{min}没有达到 &(4-2) \end{cases} \end{aligned} {v0​+2amax​​×2×jmax​amax​​>vmax​⟹(vmax​−v0​)jmax​<amax2​,vmax​+2amin​​×2×jmin​amin​​<v1​⟹(vmax​−v1​)jmax​<amax2​,​amax​没有达到amin​没有达到​(4−1)(4−2)​​

• 加速阶段

如果满足（4-1），最大加速度没有达到，那么可以计算出来：
v m a x = v 0 + j m a x T j 1 2 ⟹ T j 1 = v m a x − v 0 j m a x ， T a = 2 T j 1 (5-1) v_{max} = v_0 + j_{max}T_{j1}^2 \\\implies T_{j1} = \sqrt {\frac {v_{max}-v_0}{j_{max}}}， T_a = 2T_{j1} \tag{5-1} vmax​=v0​+jmax​Tj12​⟹Tj1​=jmax​vmax​−v0​​ ​，Ta​=2Tj1​(5-1)
否则的话，最大加速度达到了，加速阶段是三段：
T j 1 = a m a x j m a x v m a x = v 0 + j m a x T j 1 2 + a m a x ( T a − 2 T j 1 ) ⟹ T a = T j 1 + v m a x − v 0 a m a x (5-2) T_{j1} = \frac {a_{max}}{j_{max}}\\ v_{max} = v_0 + j_{max}T_{j1}^2 + a_{max}(T_a - 2T_{j1}) \implies T_a = T_{j1} + \frac {v_{max}-v_0}{a_{max}} \tag{5-2} Tj1​=jmax​amax​​vmax​=v0​+jmax​Tj12​+amax​(Ta​−2Tj1​)⟹Ta​=Tj1​+amax​vmax​−v0​​(5-2)

• 减速阶段

类似于加速阶段，如果满足（4-2），则a_{min}没有达到，减速阶段为两个阶段：
T j 2 = v m a x − v 1 j m a x ， T d = 2 T j 2 (5-3) T_{j2} = \sqrt {\frac{v_{max}-v_1}{j_max}}，T_d=2T_{j2} \tag{5-3} Tj2​=jm​axvmax​−v1​​ ​，Td​=2Tj2​(5-3)
否则的话，最大的减速度达到了，减速阶段是三段：
T j 2 = a m a x j m a x v 1 = v m a x + j m i n T j 2 2 + a m i n ( T d − 2 T j 2 ) ⟹ T d = T j 2 + v m a x − v 1 a m a x (5-4) T_{j2} = \frac {a_{max}}{j_{max}}\\ v_1 = v_{max} + j_{min} T_{j2}^2 + a_{min}(T_d-2T_{j2}) \implies T_d = T_{j2} + \frac {v_{max}-v_1}{a_{max}} \tag{5-4} Tj2​=jmax​amax​​v1​=vmax​+jmin​Tj22​+amin​(Td​−2Tj2​)⟹Td​=Tj2​+amax​vmax​−v1​​(5-4)

• 匀速阶段

计算出来 T j 1 , T j 2 T_{j1},T_{j2} Tj1​,Tj2​ 基本就算出来加速阶段和减速阶段的轨迹了，那么匀速阶段的时间可以计算出来：
T v = δ q 匀速 v l i m v l i m = v m a x δ q 匀速 = δ q − δ q 加速 − δ q 减速 = ( q 1 − q 0 ) − δ q 加速 − δ q 减速 δ q 加速 = T j 1 ( v 0 + v m a x ) = T a 2 ( v 0 + v m a x ) 或 δ q 加速 = 1 2 ( v 0 + v m a x ) ( T j 1 + v m a x − v 0 a m a x ) = T a 2 ( v 0 + v m a x ) δ q 减速 = T j 2 ( v m a x + v 1 ) = T d 2 ( v m a x + v 1 ) 或 δ q 减速 = 1 2 ( v m a x + v 1 ) ( T j 2 + v m a x − v 1 a m a x ) = T d 2 ( v m a x + v 1 ) ⟹ T v × v m a x = ( q 1 − q 0 ) − T a 2 ( v 0 + v m a x ) − T d 2 ( v m a x + v 1 ) ⟹ T v = q 1 − q 0 v m a x − T a 2 ( 1 + v 0 v m a x ) − T d 2 ( 1 + v 1 v m a x ) (5-5) T_v = \frac {\delta_{q匀速}}{v_{lim}}\\ v_{lim} = v_{max}\\ \delta_{q匀速} = \delta_{q} - \delta_{q加速} - \delta_{q减速} = (q_1-q_0) - \delta_{q加速} - \delta_{q减速}\\ \delta_{q加速} = T_{j1} (v_0 + v_{max}) = \frac {T_a}{2} (v_0 + v_{max}) \quad 或 \quad \delta_{q加速} = \frac {1}{2}(v_0+v_{max})\left(T_{j1}+\frac {v_{max}-v_0}{a_{max}}\right)=\frac {T_a}{2} (v_0 + v_{max})\\ \delta_{q减速} = T_{j2}(v_{max}+v_1) = \frac {T_d}{2} (v_{max}+v_1) \quad 或 \quad \delta_{q减速} = \frac {1}{2}(v_{max} + v_1)\left( T_{j2} + \frac {v_{max}-v_1}{a_{max}} \right) = \frac {T_d}{2}(v_{max}+v_1)\\ \implies T_v \times v_{max} = (q_1-q_0) - \frac {T_a}{2} (v_0 + v_{max}) - \frac {T_d}{2}(v_{max}+v_1) \\ \implies T_v = \frac {q_1-q_0}{v_{max}} - \frac {T_a}{2} (1 + \frac {v_0} {v_{max}}) - \frac {T_d}{2}(1 + \frac{v_1}{v_{max}}) \tag{5-5} Tv​=vlim​δq匀速​​vlim​=vmax​δq匀速​=δq​−δq加速​−δq减速​=(q1​−q0​)−δq加速​−δq减速​δq加速​=Tj1​(v0​+vmax​)=2Ta​​(v0​+vmax​)或δq加速​=21​(v0​+vmax​)(Tj1​+amax​vmax​−v0​​)=2Ta​​(v0​+vmax​)δq减速​=Tj2​(vmax​+v1​)=2Td​​(vmax​+v1​)或δq减速​=21​(vmax​+v1​)(Tj2​+amax​vmax​−v1​​)=2Td​​(vmax​+v1​)⟹Tv​×vmax​=(q1​−q0​)−2Ta​​(v0​+vmax​)−2Td​​(vmax​+v1​)⟹Tv​=vmax​q1​−q0​​−2Ta​​(1+vmax​v0​​)−2Td​​(1+vmax​v1​​)(5-5)

注意上面计算 δ q 加速 , δ q 加速 \delta_{q加速}, \delta_{q加速} δq加速​,δq加速​ 用到了（3）、（5-1）、(5-2)、（5-3）、（5-4）。

如果计算出来的 T v > 0 T_v > 0 Tv​>0，说明最大速度确实达到了，也就是前提条件 v l i m = v m a x v_{lim}=v_{max} vlim​=vmax​ 是正确的，否则的话，说明就没有匀速阶段，则 v l i m ≤ v m a x v_{lim} \leq v_{max} vlim​≤vmax​ ，我们接下来讨论这种情况。

最大速度 v l i m ≤ v m a x v_{lim} \leq v_{max} vlim​≤vmax​

这种情况下，不存在匀速阶段， T v = 0 T_v = 0 Tv​=0 ，只剩下加速阶段和减速阶段。

如果最大加速度和最大减速度都同时达到了，那么很容易计算出来：
T j 1 = T j 2 = T j = a m a x j m a x (6) T_{j1} = T_{j2} = T_j = \frac {a_{max}}{j_{max}} \tag{6} Tj1​=Tj2​=Tj​=jmax​amax​​(6)
同时根据位移，可以计算出来加速和减速段的时间分别为：
T a = a m a x 2 j m a x − 2 v 0 + Δ 2 a m a x (7) T_a = \frac {\frac {a_{max}^2}{j_max}-2v_0+\sqrt \Delta}{2a_{max}} \tag{7} Ta​=2amax​jm​axamax2​​−2v0​+Δ ​​(7)

T d = a m a x 2 j m a x − 2 v 1 + Δ 2 a m a x (8) T_d = \frac {\frac {a_{max}^2}{j_max}-2v_1+\sqrt \Delta}{2a_{max}} \tag{8} Td​=2amax​jm​axamax2​​−2v1​+Δ ​​(8)

其中：
Δ = a m a x 4 j m a x 2 + 2 ( v 0 2 + v 1 2 ) + a m a x ( 4 ( q 1 − q 0 ) − 2 a m a x j m a x ( v 0 + v 1 ) ) (9) \Delta = \frac {a_{max}^4}{j_{max}^2} + 2(v_0^2+v_1^2) + a_{max}\left( 4(q_1-q_0) - 2\frac {a_{max}}{j_{max}(v_0+v_1)} \right) \tag{9} Δ=jmax2​amax4​​+2(v02​+v12​)+amax​(4(q1​−q0​)−2jmax​(v0​+v1​)amax​​)(9)
公式（7）、（8）、（9）的推导比较麻烦，参考（5-5）可以得出整体的位移为
δ q 加速 + δ q 减速 = T a 2 ( v 0 + v l i m ) + T d 2 ( v 1 + v l i m ) = q 1 − q 0 (10-1) \delta_{q加速}+\delta_{q减速}=\frac {T_a}{2}(v_0+v_{lim}) + \frac {T_d}{2}(v_{1}+v_{lim}) = q_1 - q_0\tag{10-1} δq加速​+δq减速​=2Ta​​(v0​+vlim​)+2Td​​(v1​+vlim​)=q1​−q0​(10-1)
参考（3）和（6）计算Phase3终点的速度，可以得到：
v l i m = v 0 + 1 2 j m a x T j 1 2 + a m a x ( T a − 2 T j 1 ) + a m a x T j 1 − 1 2 T j 1 2 = v 0 + a m a x T a − a m a x 2 j m a x v l i m = v 1 + a m a x T d − a m a x 2 j m a x ⟹ T a − T d = v 1 − v 0 a m a x (10-2) v_{lim} = v_0 + \frac{1}{2}j_{max}T_{j1}^2+a_{max}(T_a - 2T_{j1}) + a_{max}T_{j1}-\frac{1}{2}T_{j1}^2 = v_0 + a_{max}T_a - \frac {a_{max}^2}{j_{max}} \\ v_{lim} = v_1 + a_{max}T_d - \frac {a_{max}^2}{j_{max}}\\ \implies T_a - T_d = \frac {v_1-v_0}{a_{max}} \tag{10-2} vlim​=v0​+21​jmax​Tj12​+amax​(Ta​−2Tj1​)+amax​Tj1​−21​Tj12​=v0​+amax​Ta​−jmax​amax2​​vlim​=v1​+amax​Td​−jmax​amax2​​⟹Ta​−Td​=amax​v1​−v0​​(10-2)
联立（10-1）、（10-2）可以得到（7）、（8）、（9）。

如果计算出来的 T a < 2 T j T_a < 2T_j Ta​<2Tj​ 或者 T d < 2 T j T_d < 2T_j Td​<2Tj​ 说明加速阶段或者减速阶段至少有一个最大加（减）速度没有达到。那么（6）、（7）、（8）、（9）是不能成立的。这种情况下，要计算轨迹是比较困难的，一个相对高效的计算方法是不断的减少 a m a x a_{max} amax​ （比如说让 a m a x = γ a m a x , 0 < γ < 1 a_{max} = \gamma a_{max}, \quad 0 < \gamma < 1 amax​=γamax​,0<γ<1 ），直到满足 T a ≥ 2 T j 和 T d ≥ 2 T j T_a \geq 2T_j 和 T_d \geq 2T_j Ta​≥2Tj​和Td​≥2Tj​ ，然后就可以使用（7）、（8）、（9）计算轨迹了。

轨迹的最终表达

上面计算出来 T j 1 , T a , T v , T j 2 , T d T_{j1},T_{a},T_{v},T_{j2},T_d Tj1​,Ta​,Tv​,Tj2​,Td​ 这些参数，就足够将整个轨迹分七段表示出来，当然根据情况，有的阶段是不不存在的。

假设算出来的轨迹存在七个阶段，则每个阶段的时间、速度、加速度、加加速度如下：

根据（3-1）可以依次写出每一段的轨迹表达式。