SCARA 机器人点到点轨迹规划（满足各轴同时启停）
1. 求解起始与目标关节角（逆运动学）
设起始位姿为 T_s，目标位姿为 T_f，通过逆运动学求得：
θ_s = IK(T_s)
θ_f = IK(T_f)
其中：
- θ_s = θ₁,ₛ, θ₂,ₛ, θ₃,ₛ, d₄,ₛᵀ
- θ_f = θ₁,𝒇, θ₂,𝒇, θ₃,𝒇, d₄,𝒇ᵀ
前三个为旋转关节（rad），第四个为平移关节（m）
2. 计算各关节位移
Δθ₁ = |θ₁,𝒇 - θ₁,ₛ|
Δθ₂ = |θ₂,𝒇 - θ₂,ₛ|
Δθ₃ = |θ₃,𝒇 - θ₃,ₛ|
Δd₄ = |d₄,𝒇 - d₄,ₛ|
3. 计算各关节最小可行时间
对每个关节 i，给定 v_max,i（最大速度），a_max,i（最大加速度）。
判断是否有匀速段：
如果：Δq_i ≥ (2·v_max,i²) / a_max,i
则使用梯形速度剖面：t_f,i = (Δq_i / v_max,i) + (v_max,i / a_max,i)
否则（三角速度剖面）：
v_actual,i = √( (a_max,i · Δq_i) / 2 )
t_f,i = 2 · √( (2 · Δq_i) / a_max,i )
对关节 1~3 使用 Δθ_i，对关节 4 使用 Δd₄
4. 确定统一总时间（实现同起同停）
t_f = max(t_f,₁, t_f,₂, t_f,₃, t_f,₄)
此 t_f 将作为 所有关节的共同运动时间。
5. 生成同步轨迹（五次多项式插值）
对每个关节 i，在区间 t ∈ 0, t_f 上使用五次多项式：
q_i(t) = a₀ + a₁·t + a₂·t² + a₃·t³ + a₄·t⁴ + a₅·t⁵
边界条件（静止到静止）：
- q_i(0) = q_i,s
- q_i(t_f) = q_i,f
- q̇_i(0) = 0
- q̇_i(t_f) = 0
- q̈_i(0) = 0
- q̈_i(t_f) = 0
解得系数：
a₀ = q_i,s
a₁ = 0
a₂ = 0
a₃ = (10·(q_i,f - q_i,s)) / t_f³
a₄ = (-15·(q_i,f - q_i,s)) / t_f⁴
a₅ = (6·(q_i,f - q_i,s)) / t_f⁵
或使用无量纲化形式（推荐）：
令归一化时间：τ = t / t_f
则：q_i(t) = q_i,s + (q_i,f - q_i,s) · (10·τ³ - 15·τ⁴ + 6·τ⁵)
该式保证所有关节在 t = 0 启动，在 t = t_f 同时停止，且轨迹平滑（C² 连续）。
总结
- ✅ 同起同停 ：通过
t_f = max(...)统一时间。 - ✅ 平滑性：五次多项式保证速度、加速度连续。
- ✅ 可实现性 ：每条轨迹均满足该关节的速度/加速度约束（因为
t_f ≥ t_f,i，实际速度 ≤ v_max,i）。