4、T型规划位移公式推导：基于时间与速度的位移积分计算
好，咱们接着聊T型速度规划。上一节我们把速度曲线拆成了三段：匀加速、匀速、匀减速。那问题来了——我怎么知道电机到底走了多远？
说白了，位移就是速度对时间的积分。你想想看，速度曲线下面的面积，就是位移。这个思路很直观，但真正推导的时候，有几个细节我得跟你讲清楚。
4.1 从速度函数到位移函数
我们先写出分段的速度函数。假设总时间被分成三段：
- 加速段：时间从0到t₁，速度从0线性增加到V_max
- 匀速段：时间从t₁到t₂，速度保持V_max不变
- 减速段：时间从t₂到T，速度从V_max线性减到0
写成数学表达式就是：
v(t) =
a * t, 0 ≤ t < t₁
V_max, t₁ ≤ t < t₂
V_max - a * (t - t₂), t₂ ≤ t ≤ T嗯，这里要注意：加速和减速的加速度大小相等，方向相反。我在项目中遇到过有人把加减速的加速度设成不一样，结果算出来的位移对不上，电机最后停的位置总是偏的。
4.2 分段积分求位移
位移公式很简单：
S = ∫ v(t) dt
我们一段一段来算。
加速段位移（S₁）
从0到t₁，速度是线性增长的：
S₁ = ∫₀ᵗ¹ a·t dt = (1/2) · a · t₁²因为t₁时刻速度刚好达到V_max，所以t₁ = V_max / a，代入得：
S₁ = (1/2) · a · (V_max / a)² = V_max² / (2a)这个公式很眼熟吧？其实就是高中物理的匀加速位移公式。我个人习惯把这个结果记成「三角形面积」——底是t₁，高是V_max，面积就是一半。
匀速段位移（S₂）
匀速段最简单，速度恒定，时间长度是(t₂ - t₁)：
S₂ = V_max · (t₂ - t₁)就是矩形面积，没什么好说的。
减速段位移（S₃）
减速段和加速段对称，速度从V_max减到0：
S₃ = ∫ₜ₂ᵀ [V_max - a·(t - t₂)] dt算出来结果和加速段一样：
S₃ = V_max² / (2a)为什么？因为减速段的速度曲线也是一个三角形，只不过倒过来了。面积一样大。
4.3 总位移公式
三段加起来：
S_total = S₁ + S₂ + S₃
= V_max²/(2a) + V_max·(t₂ - t₁) + V_max²/(2a)
= V_max²/a + V_max·(t₂ - t₁)这个公式很简洁。但实际项目中，我们往往不知道t₁和t₂，只知道总时间T和加速度a。那怎么办？
其实t₁和t₂可以用已知量表示：
- t₁ = V_max / a
- t₂ = T - V_max / a
代入总位移公式：
S_total = V_max²/a + V_max·(T - 2·V_max/a)
= V_max·T - V_max²/a这就是T型规划最常用的位移公式。我建议你把这个公式记下来，调试的时候经常要用到。
核心公式：
S = V_max · T - V_max² / a
其中T是总运动时间，a是加速度大小。
4.4 实际应用中的避坑指南
公式推导完了，但实际用起来有几个坑我得提醒你。
我曾经踩过的坑：
- 加速度单位搞混：加速度用mm/s²还是m/s²？我有一回在代码里用了mm，但位置单位是m，结果算出来的位移差了1000倍。电机直接冲过了限位开关。
- 忽略加减速对称性：上面的推导假设加速和减速的加速度大小相等。如果不等，公式要重新推导。我建议你一开始就保持对称，等系统稳定了再考虑非对称的情况。
- 浮点数精度问题：嵌入式平台上浮点数运算有误差。特别是V_max²/a这种除法，如果a很小，结果可能溢出。我习惯先把单位换算好，尽量用整数运算。
4.5 代码实现示例
最后，给一个简单的C代码实现。这个函数根据目标位移、最大速度和加速度，计算出T型规划的总时间：
// 计算T型规划总时间
// 输入：目标位移S，最大速度V_max，加速度a
// 输出：总时间T
float calc_t_plan_time(float S, float V_max, float a)
{
float T;
float t_acc = V_max / a; // 加速段时间
float S_acc = V_max * t_acc / 2; // 加速段位移
// 检查是否能达到最大速度
if (2 * S_acc >= S) {
// 达不到最大速度，三角形速度曲线
// 此时 S = a * t²，解出 t = sqrt(S / a)
T = 2 * sqrtf(S / a);
} else {
// 能达到最大速度，梯形速度曲线
float S_const = S - 2 * S_acc; // 匀速段位移
float t_const = S_const / V_max; // 匀速段时间
T = 2 * t_acc + t_const;
}
return T;
}小技巧：代码里我判断了「是否能达到最大速度」的情况。如果目标位移太小，电机刚加速完就要减速，根本跑不到V_max。这时候速度曲线是三角形，不是梯形。很多新手会忽略这个边界条件，导致算出来的时间偏大。
好了，T型规划的位移公式推导就到这里。下一节我们聊聊S型规划——那个曲线更平滑，但公式也更复杂。做好准备。