VelocityProfile::SCurve

cpkg add VelocityProfile::SCurve

文档

SCurve

SCurve 提供带初末速度、初末加速度约束的单轴 S 曲线速度规划实现，适用于嵌入式底盘、关节或直线模组等对速度平滑性敏感的场景。

规划器会在给定：

起点位置 / 速度 / 加速度；
终点位置 / 速度 / 加速度；
最大速度 / 最大加速度 / 最大加加速度；

的前提下，自动求解一条满足约束的 S 曲线，并根据实际路程决定：

是否需要先消除与目标方向相反的初始加速度；
是否可以达到最大速度；
中间是否存在匀速段；
如何在末端满足目标速度与目标加速度。

轨迹组成

当前实现会把整条轨迹拆成四个逻辑阶段：

起点预处理：处理非零初始加速度，尤其是与目标方向相反的情况；
主加速段：从等效起始状态加速到峰值速度；
匀速段：若总位移足够，则在峰值速度处保持巡航；
末端逆过程：把终点约束转换为时间反演后的减速段。

这种拆分方式既保留了 S 曲线的平滑性，也让“带初末状态约束”的情况更容易求解和维护。

代码结构

src/s_curve.hpp：对外接口、数据成员和阶段划分说明；
src/s_curve.cpp：求解流程、峰值速度判定、二分搜索与采样实现；
simulation/s_curve.py：Python 版仿真模型；
simulation/plot_display.py：单轴位移/速度/加速度曲线可视化；
simulation/plot_chassis_display.py：底盘场景下的世界坐标系与车体坐标系验证。

使用方式

典型使用流程如下：

构造 SCurveProfile::Config 并设置速度、加速度、加加速度上限；
传入起终点状态构造 SCurveProfile；
先检查 success() 是否为 true；
通过 getTotalTime() 获取总时长；
在控制循环中调用 CalcX(t)、CalcV(t)、CalcA(t) 获取期望轨迹状态。

如果构造失败，通常意味着：

初末状态本身已经超出全局约束；
两端预处理已经消耗了全部位移，导致无可行解；
在给定约束下无法同时满足起终点状态和总位移。

验证建议

在将本曲线实际应用到嵌入式系统之前，建议先使用仿真脚本确认轨迹是否符合预期：

cd SCurve/simulation && uv sync
cd SCurve/simulation && uv run python plot_display.py
cd SCurve/simulation && uv run python plot_chassis_display.py

建议重点检查：

起点与终点的 x、v、a 是否准确收敛；
曲线是否满足 max|v|、max|a| 和 max|jerk|；
对于底盘应用，body frame 下是否仍满足控制器的动态约束。

注意

该模块面向轨迹生成，不直接处理控制器跟踪误差；
约束参数过激进时，真实系统可能因执行器饱和而无法稳定跟踪；
在真实硬件部署前，请先完成仿真验证并结合采样周期检查控制离散化误差。