三段S曲线速度规划模型

该文档描述了算法的数学模型、约束条件以及求解流程。文档中的符号和公式与代码中的计算逻辑相对应，详细说明了如何根据输入参数（如轨迹弧长 $S$、总飞行时间$T$、进入速度 $v_{In}$、期望速度 $v_{Des}$、最小/最大速度 $v_{min}, v_{max}$、最大加速度 $a_{Max} $和最大加加速度 $j_{Max}$）计算出整个轨迹在仿真步长 $dt$ 下的速度、加速度和加加速度曲线，同时在后段尽可能保持接近期望速度飞行，并通过二分查找调整 $v_{Des}$ 以匹配目标弧长。

1. 引言

本模型旨在为无人机轨迹规划提供一条平滑的 S 曲线速度规划方案。给定目标轨迹弧长 $S$ 与总飞行时间 $T$，输入参数包括进入速度 $v_{in}$ 与期望速度 $v_{des}$（代码中记为 vIn 与 vDes），同时受到速度上下限 $v_{min}, v_{max}$ 以及动态约束：最大加速度 $a_{max}$ 和最大加加速度 $j_{max}$ 的限制。整个模型通过先计算变速（过渡）阶段的 S 曲线段，再补充恒速段（定速阶段），最后利用二分查找方法调整期望速度以匹配目标弧长，从而生成一组离散时间点上的速度、加速度和加加速度数据。

在线计算器链接

C++代码库： https://github.com/flitai/Velocity_Plan/

2. 模型参数与符号定义

• 输入参数：

  • $S$ : 轨迹弧长（米）
  • $T$ : 总飞行时间（秒）
  • $v_{in}$ : 进入速度（米/秒）
  • $v_{des}$ : 期望速度（米/秒）
  • $v_{min}$ : 最小允许速度（米/秒）
  • $v_{max}$ : 最大允许速度（米/秒）
  • $a_{max}$ : 最大加速度（米/秒²）
  • $j_{max}$ : 最大加加速度（米/秒³）
  • $dt$ : 仿真步长（秒）

• 其他符号：

  • $\Delta v = v_{des} - v_{in}$
  • $s = \text{sgn}(\Delta v)$，正表示加速，负表示减速。

3. S 曲线变速段设计

为实现平滑过渡，S 曲线变速段分为两种情况，根据 $|\Delta v|$ 与 $\frac{a_{max}^2}{j_{max}}$ 的关系区分为【梯形 S 曲线】与【三角形 S 曲线】。

3.1 梯形 S 曲线

当

采用梯形 S 曲线，其分为三个子段：

1. 上升阶段（加加速阶段）：

   • 时间区间：$t \in [0, T_j]$，其中
     $$ T_j = \frac{a_{max}}{j_{max}}. $$
   • 动态表达式： $$ \begin{aligned} j(t) &= s\, j_{max},\\ a(t) &= s\, j_{max}\, t,\\ v(t) &= v_{in} + \frac{s\, j_{max}\, t^2}{2}. \end{aligned} $$

2. 恒定加速阶段：

   • 时间区间：$t \in (T_j,\, T_j + T_a]$，其中
     $$ T_a = \frac{|\Delta v|}{a_{max}} - T_j. $$
   • 动态表达式： $$ \begin{aligned} j(t) &= 0,\\ a(t) &= s\, a_{max},\\ v(t) &= v_{in} + \frac{s\, a_{max}^2}{2j_{max}} + s\, a_{max}(t - T_j). \end{aligned} $$

3. 下降阶段（减加速阶段）：

   • 时间区间：$t \in (T_j+T_a,\, T_{conv}]$，其中
     $$ T_{conv} = 2T_j + T_a. $$
   • 定义局部时间 $\tau = t - (T_j+T_a)$，动态表达式： $$ \begin{aligned} j(t) &= -s\, j_{max},\\ a(t) &= s\, a_{max} - s\, j_{max}\, \tau,\\ v(t) &= v_{in} + \frac{s\, a_{max}^2}{2j_{max}} + s\, a_{max}\, T_a + s\Big(a_{max}\tau - \frac{j_{max}\, \tau^2}{2}\Big). \end{aligned} $$

3.2 三角形 S 曲线

当

采用三角形 S 曲线，其分为两个子段：

1. 上升阶段：

   • 时间区间：$t \in [0, T']$，其中
     $$ T' = \sqrt{\frac{|\Delta v|}{j_{max}}}. $$
   • 动态表达式： $$ \begin{aligned} j(t) &= s\, j_{max},\\ a(t) &= s\, j_{max}\, t,\\ v(t) &= v_{in} + \frac{s\, j_{max}\, t^2}{2}. \end{aligned} $$

2. 下降阶段：

   • 时间区间：$t \in (T',\, 2T']$，设 $\tau = t - T'$，动态表达式： $$ \begin{aligned} j(t) &= -s\, j_{max},\\ a(t) &= s\, j_{max}(T' - \tau),\\ v(t) &= v_{in} + \frac{s\, j_{max}\, T'^2}{2} + s\Big(j_{max}\, T'\,\tau - \frac{j_{max}\,\tau^2}{2}\Big). \end{aligned} $$
   • 此时，总变速段时间为
     $$ T_{conv} = 2T'. $$

4. 定速阶段

在变速阶段（$t \le T_{conv}$）结束后，进入定速阶段，设定：

定速阶段的位移为

5. 总位移匹配与 vDes 调整

整个飞行的总位移为：

其中

• $S_{conv} = \int_{0}^{T_{conv}} v(t)\, dt$（变速段的累计位移），
• $S_{const} = v_{des}\,(T - T_{conv})$。

目标是满足

由于直接根据初始参数计算得到的 $S_{total}$ 可能与目标 $S$ 不符，代码中采用二分查找法在允许范围 $[v_{min}, v_{max}]$ 内调整期望速度 $v_{des}$，使得修正后的 $v_{des}$ 满足

其中 $\epsilon$ 为容忍误差（例如 0.001 米）。

同时，通过比例因子

记录调整幅度，以便反馈用户。

6. 离散化与仿真步骤

1. 时间离散化：
   将总飞行时间 $T$ 以步长 $dt$ 离散为 $N = \lceil T/dt \rceil$ 个时间点。

2. 按阶段计算：

   • 对于 $t \le T_{conv}$ 的点，依据所选 S 曲线（梯形或三角形）分别计算对应的 $v(t)$、$a(t)$ 与 $j(t)$，同时累加位移 $S_{conv}$。
   • 对于 $t > T_{conv}$ 的点，设 $v(t) = v_{des}$（调整后的值），位移按 $v_{des} \times dt$ 累加。

3. 数据输出：
   生成离散的时间序列数据，格式为
   

   $$ \{t_k,\, v(t_k),\, a(t_k),\, j(t_k)\},\quad k=0,1,\ldots, N, $$

   用于后续图形显示和进一步分析。

7. 模型求解流程

1. 初始计算：
   根据输入参数计算 $\Delta v$ 和符号 $s$。判断 $|\Delta v|$ 是否满足梯形 S 曲线条件，从而计算对应的 $T_j, T_a$（或 $T'$），进而确定变速阶段时间 $T_{conv}$。

2. 积分求位移：
   对变速段进行数值积分获得 $S_{conv}$，再计算定速段位移 $S_{const}$，从而得出 $S_{total}$。

3. 二分查找调整 $v_{des}$：
   若 $S_{total}$ 与目标 $S$ 存在误差，则在 $[v_{min}, v_{max}]$ 范围内采用二分查找调整 $v_{des}$，直到满足误差要求。

4. 生成输出数据：
   依据调整后的 $v_{des}$ 与计算结果生成全程的速度、加速度与加加速度数据，并输出曲线信息与统计指标。

8. 总结

本 S 曲线速度规划模型在满足总飞行时间 $T$ 与轨迹弧长 $S$ 的严格约束下，通过以下步骤实现：

• 变速段设计： 根据 $v_{in}$ 与 $v_{des}$ 之间的差值，选择梯形或三角形 S 曲线生成平滑的加速/减速过渡，计算出变速段时间 $T_{conv}$ 与累计位移 $S_{conv}$。
• 定速段补充： 变速结束后，采用定速 $v_{des}$ 飞行，确保后段尽可能长以接近期望速度。
• 迭代优化： 采用二分查找调整期望速度 $v_{des}$（同时记录比例因子），以确保总体位移匹配目标 $S$。
• 离散仿真： 按照步长 $dt$ 离散生成整段轨迹的速度、加速度和加加速度数据。

该模型不仅实现了速度规划的平滑性和安全性，同时在后段尽可能保持接近飞出速度飞行，从而满足特定飞行性能的要求。