速度曲线规划
这一页解决什么问题
「这一段路 120 米，巡航 40 km/h」听起来是个很自然的指令，但真正落到加减速上会碰到三件麻烦事：
- 加速段要多少米？
v²/(2a)：40 km/h（11.11 m/s）、a = 1.2 m/s² 时是 51.44 m。 - 减速段要多少米？
v²/(2d)：d = 2.0 m/s² 时是 30.86 m。 - 加起来超过总距离怎么办？ 51.44 + 30.86 = 82.30 m > 60 m —— 这时 40 km/h 的曲线根本走不完。
本页给出一个纯标准库的实现：输入距离、巡航速度、加/减速度上限，规划出「加速 → 匀速 → 减速」三段曲线；距离不够时自动降峰值退化成三角形，而不是给一条物理上跑不通的曲线。然后用四条断言判断这条曲线是否可用。
自主实现：speed_profile_check
目录结构
src/ground/speed_profile_check/
├── CMakeLists.txt
├── package.xml
├── README.md
├── config/speed_profile_check.yaml # 12 个参数默认值
├── launch/main.launch # 12 个带 doc 的 arg
└── scripts/
├── main.py # roslaunch 的入口
├── speed_core.py # 纯曲线计算（不 import rospy）
└── speed_node.py # ROS 收发放这里
tests/test_speed_profile_check_local.py
公式
设巡航速度 v（m/s）、初速 v0、加速度 a、减速度 d、总距离 L：
| 段 | 时长 | 距离 | 默认工况（120 m、40 km/h、a = 1.2、d = 2.0） |
|---|---|---|---|
| 加速 | (v - v0) / a |
(v² - v0²) / (2a) |
9.26 s / 51.44 m |
| 匀速 | 匀速距离 / v |
L - 加速距离 - 减速距离 |
3.39 s / 37.70 m |
| 减速 | v / d |
v² / (2d) |
5.56 s / 30.86 m |
| 合计 | 18.21 s | 120.00 m | 平均 23.73 km/h |
距离不够时（加速距离 + 减速距离 > L）：令 L = (v² - v0²)/(2a) + v²/(2d) 反解峰值 v，匀速段长度归零，曲线变成三角形。例：L = 60 m 时峰值自动降到 34.15 km/h，总时长 12.65 s。
四条断言
| 断言 | 判据 | 不过时的含义 |
|---|---|---|
| 限速 | 峰值速度 ≤ 路段限速 | 巡航速度定高了 |
| 加速度 | 加/减速度为正且在车辆能力内 | 参数写错 |
| 距离 | 规划距离与目标距离误差 ≤ 容差 | 曲线没走完全程 |
| 时间 | 总时长 ≤ 时间预算 | 这段路太慢 |
四条互相独立：改一个参数只翻一条，便于定位。
参数表
| 参数 | 默认 | 说明 |
|---|---|---|
scene/distance |
120.0 | 目标行驶距离（m） |
scene/cruise_speed_kmh |
40.0 | 巡航速度（km/h） |
scene/start_speed_kmh |
0.0 | 初速（km/h） |
vehicle/accel |
1.2 | 加速度上限（m/s²） |
vehicle/decel |
2.0 | 减速度上限（m/s²） |
check/speed_limit_kmh |
50.0 | 路段限速（km/h） |
check/time_budget |
30.0 | 允许用时（s） |
check/tolerance |
0.5 | 距离允许误差（m） |
sim/dt |
0.05 | 采样步长（s） |
rate/publish_hz |
5.0 | 报告重发频率（Hz） |
topic/profile |
/speed/profile |
速度曲线话题 |
topic/report |
/speed/report |
报告话题 |
话题表
| 话题 | 类型 | 方向 | 说明 |
|---|---|---|---|
/speed/profile |
std_msgs/String |
发布（latched） | 速度曲线字符图 |
/speed/report |
std_msgs/String |
发布（latched） | 完整报告 |
运行
cd ~/catkin_ws9
source /opt/ros/melodic/setup.bash
source devel/setup.bash
roslaunch speed_profile_check main.launch
实际输出（默认工况）：
曲线类型：梯形（含匀速段）
巡航：峰值 40.00 km/h，初速 0.00 km/h，规划距离 120.00 m
分段：加速 9.26 s / 51.44 m，匀速 3.39 s / 37.70 m，减速 5.56 s / 30.86 m
总时长 18.21 s，平均车速 23.73 km/h
断言：4 通过 / 0 失败（共 4 条）
[PASS] 限速 峰值 40.00 km/h，限速 50.00 km/h
[PASS] 加速度 加速 1.20 m/s^2、减速 2.00 m/s^2
[PASS] 距离 规划 120.00 m，目标 120.00 m（容差 0.50 m）
[PASS] 时间 用时 18.21 s，预算 30.00 s
结论：PASSED
后面跟着速度曲线的字符图（* 曲线顶端、# 曲线以下、横轴为时间）：
*************
***#############**
**##################**
***######################*
***##########################**
**###############################*
***##################################**
***#######################################**
***############################################*
**################################################**
***####################################################**
**#########################################################*
0------------------------------------------------------18 s
反例
# 巡航速度超过限速：只有「限速」失败
roslaunch speed_profile_check main.launch cruise_speed_kmh:=25 speed_limit_kmh:=20
# 时间预算不够：只有「时间」失败
roslaunch speed_profile_check main.launch time_budget:=15
# 距离太短：自动降峰值，仍然 PASS（三角形曲线）
roslaunch speed_profile_check main.launch distance:=60
distance:=60 的峰值是 34.15 km/h —— 这说明「距离不够」不会报错，而是物理上自动让步。
离线测试
cd <仓库根目录>
python3 tests/test_speed_profile_check_local.py
期望：
通过 248 项，失败 0 项
全部通过
也可交给 pytest：python3 -m pytest tests/test_speed_profile_check_local.py -q。
与已有模块的分工
| 模块 | 关注点 |
|---|---|
twist_to_control |
速度指令 → 转角/轮速（执行量） |
braking_distance_check |
从看见到停住要几米（纵向安全） |
turning_sweep_check |
转弯时车身占多宽（横向包络） |
rostopic_cruise |
手动发指令巡航 |
| 本模块 | 一段路的速度曲线怎么规划（时间-速度） |
常见问题
为什么不做 S 型（jerk 限制）曲线？ 三角形的 jerk 是无穷大，真实控制器会自行平滑。本模块只回答「这段距离、这个限速、这段 时间能不能走完」，jerk 留给下层控制器；需要时可在加速段与匀速段之间插加速度斜坡。
为什么平均车速只有 23.73 km/h？ 起点与终点速度都是 0，加减速段占去 14.81 s（共 18.21 s），这是短途工况的正常结果。
Melodic 上报编码错误吗？
不会。speed_node.py 的 _log() 先把消息编码成 byte str 再交给 rospy.loginfo。
同一段路怎么开最快
speed_profile_check 不只校验一条曲线，还能把"同一段距离、不同巡航速度"的方案排在一起比较，直接给出推荐：
方案对比（同一段 120.00 m，限速 50.00 km/h）：
巡航 20.0 km/h -> 峰值 20.0 km/h，用时 25.30 s，平均 17.07 km/h，梯形
巡航 30.0 km/h -> 峰值 30.0 km/h，用时 19.96 s，平均 21.65 km/h，梯形
巡航 40.0 km/h -> 峰值 40.0 km/h，用时 18.21 s，平均 23.73 km/h，梯形
巡航 50.0 km/h -> 峰值 48.3 km/h，用时 17.89 s，平均 24.15 km/h，三角形
巡航 60.0 km/h -> 峰值 48.3 km/h，用时 17.89 s，平均 24.15 km/h，三角形
巡航 70.0 km/h -> 峰值 48.3 km/h，用时 17.89 s，平均 24.15 km/h，三角形
推荐：50.00 km/h（用时 17.89 s，平均 24.15 km/h，峰值 48.30 km/h 未超限）
读这张表要注意两点：
- 50、60、70 三档完全一样：120 m 的距离不足以让车跑到 50 km/h 以上，三档都退化成同一个三角形（峰值 48.30 km/h）。距离是硬约束，巡航再高也没用。
- 推荐不是"最快"而是"最快且不超限"：
recommend_plan()先筛掉峰值超限的方案，再取用时最短的；并列时取巡航更低的那条（更保守）。全部超限时返回None，表尾会提示"没有不超限的方案"。
相关函数：candidate_cruises() 生成候选阶梯、compare_plans() 批量计算、recommend_plan() 选优、format_plans() 出表；format_report(..., plans=..., speed_limit_kmh=...) 会把这一段接在"结论"之后。
大模型声明
使用大模型辅助代码编写、测试编写与文档整理，提交内容由本人负责。