三角函数速度规划方法介绍
在运动控制系统中，为了实现加速度连续、加加速度（跃度）有限的平滑轨迹，常采用三角函数（正弦/余弦）构造速度曲线。该方法可避免机械冲击，提高系统平稳性。
1. 参数定义
- 起始速度：v_s
- 终止速度：v_e
- 加减速总时间：T
- 最大加速度：a_max
- 最大加加速度：j_max
- 总位移：s_total
2. 方法一：对称正弦加减速（v_s = 0, v_e = v_max）
加速度函数 a(t)
a(t) = (π * v_max / (2 * T)) * sin(π * t / T)
速度函数 v(t)
v(t) = v_max * (1 - cos(π * t / T)) / 2
位移函数 s(t)
s(t) = v_max * [t / 2 - (T / π) * sin(π * t / T) / 2]
总位移（t = T）
s(T) = v_max * T / 2
3. 方法二：通用正弦速度规划（任意 v_s 和 v_e）
适用于起始和终止速度不为零的情况。
速度函数 v(t)
v(t) = v_s + (v_e - v_s) * (1 - cos(π * t / T)) / 2
加速度函数 a(t)
a(t) = (π * (v_e - v_s) / (2 * T)) * sin(π * t / T)
加加速度函数 j(t)
j(t) = (π² * (v_e - v_s) / (2 * T²)) * cos(π * t / T)
总位移 s(T)
s(T) = (v_s + v_e) * T / 2
4. 方法三：带匀速段的五段式正弦加减速
当总位移较大时，可分为：加速 → 匀速 → 减速。
设：
- 加速时间：T_a
- 匀速时间：T_c
- 减速时间：T_d
- 巡航速度：v_cruise
（1）加速段（0 ≤ t ≤ T_a）
v(t) = v_s + (v_cruise - v_s) * (1 - cos(π * t / T_a)) / 2
s_acc = (v_s + v_cruise) * T_a / 2
（2）匀速段（T_a < t ≤ T_a + T_c）
v(t) = v_cruise
s_cruise = v_cruise * T_c
（3）减速段（T_a + T_c < t ≤ T_a + T_c + T_d）
v(t) = v_cruise + (v_e - v_cruise) * (1 - cos(π * (t - T_a - T_c) / T_d)) / 2
s_dec = (v_cruise + v_e) * T_d / 2
总位移
s_total = s_acc + s_cruise + s_dec
= (v_s + v_cruise) * T_a / 2 + v_cruise * T_c + (v_cruise + v_e) * T_d / 2
5. 时间计算示例
若已知总位移 s_total 和目标速度，可通过以下步骤求解各段时间：
- 假设 T_a = T_d（对称加减速）
- 设 v_s = 0, v_e = 0, v_cruise = v_m
- 则： s_total = (0 + v_m)/2 * T_a + v_m * T_c + (v_m + 0)/2 * T_d
= v_m * T_a / 2 + v_m * T_c + v_m * T_d / 2
= v_m * (T_a / 2 + T_c + T_d / 2)
若 T_a = T_d，则： s_total = v_m * (T_a + T_c)
可据此反解 T_c 或 T_a。
6. 优点与适用场景
| 优点 | 说明 |
|---|---|
| 加速度连续 | 无突变，减少机械冲击 |
| 加加速度有界 | 避免“抖动”现象 |
| 公式简洁 | 易于编程实现 |
| 平滑性好 | 适用于高精度定位系统 |
应用场景：机器人关节运动、CNC 数控机床、3D 打印机、自动化装配线等。