第四章 梯形规划实战：Python代码实现梯形速度曲线生成、参数调优与边界条件处理

说实话，梯形速度曲线是运动控制里最基础、也最常用的规划方式。我刚开始做机器人控制那会儿，第一个上手的项目就是梯形规划。那时候觉得这东西简单，不就是加速、匀速、减速三段嘛。结果真到现场调试，才发现坑不少。

这一章，咱们就手撸一个完整的梯形速度曲线生成器。我会把我在项目中踩过的坑、总结的经验都揉进去。你跟着代码走一遍，基本就能应付大部分实际场景了。

4.1 梯形规划的核心逻辑

先理清思路。梯形速度曲线长什么样？说白了就是三段：

• 加速段：从0加速到目标速度Vmax
• 匀速段：以Vmax匀速运动
• 减速段：从Vmax减速到0

但这里有个关键问题——不是所有情况都能跑到Vmax。比如距离太短，刚加速完就得减速。这就是所谓的「三角形曲线」。我当年第一次遇到这种情况，代码直接跑飞了，因为没处理边界条件。

你想想看，如果S_total小于S_acc + S_dec，那就没法达到Vmax。这时候需要重新计算一个实际能达到的最高速度。

4.2 代码实现：梯形规划类

我个人习惯把规划器封装成一个类。这样参数清晰，也方便扩展。下面是我常用的实现：

import numpy as np

class TrapezoidalPlanner:
    """梯形速度规划器"""
    
    def __init__(self, vmax=1.0, amax=0.5, dmax=0.5):
        self.vmax = vmax      # 最大速度
        self.amax = amax      # 最大加速度
        self.dmax = dmax      # 最大减速度
        
        # 规划结果
        self.t_acc = 0.0      # 加速时间
        self.t_vel = 0.0      # 匀速时间
        self.t_dec = 0.0      # 减速时间
        self.t_total = 0.0    # 总时间
        self.v_actual = 0.0   # 实际最大速度
        
    def plan(self, distance):
        """
        规划梯形速度曲线
        distance: 目标位移（正值）
        返回：时间序列和速度序列
        """
        if distance <= 0:
            raise ValueError("距离必须为正数")
            
        # 计算理论加速/减速距离
        s_acc = self.vmax**2 / (2 * self.amax)
        s_dec = self.vmax**2 / (2 * self.dmax)
        
        # 判断是否能达到最大速度
        if s_acc + s_dec <= distance:
            # 可以到达Vmax
            self.v_actual = self.vmax
            s_vel = distance - s_acc - s_dec
            self.t_acc = self.vmax / self.amax
            self.t_dec = self.vmax / self.dmax
            self.t_vel = s_vel / self.vmax
        else:
            # 无法到达Vmax，形成三角形曲线
            # 需要重新计算实际最大速度
            self.v_actual = np.sqrt(
                2 * self.amax * self.dmax * distance / 
                (self.amax + self.dmax)
            )
            self.t_acc = self.v_actual / self.amax
            self.t_dec = self.v_actual / self.dmax
            self.t_vel = 0.0
            
        self.t_total = self.t_acc + self.t_vel + self.t_dec
        
        # 生成时间-速度曲线
        return self._generate_profile()
    
    def _generate_profile(self):
        """生成离散的时间-速度点"""
        dt = 0.01  # 采样周期
        t = np.arange(0, self.t_total, dt)
        v = np.zeros_like(t)
        
        for i, ti in enumerate(t):
            if ti <= self.t_acc:
                # 加速段
                v[i] = self.amax * ti
            elif ti <= self.t_acc + self.t_vel:
                # 匀速段
                v[i] = self.v_actual
            else:
                # 减速段
                t_dec = ti - self.t_acc - self.t_vel
                v[i] = self.v_actual - self.dmax * t_dec
                
        return t, v

4.3 参数调优：怎么选Vmax、Amax、Dmax？

参数选不好，规划出来的曲线要么太慢，要么冲击太大。我总结了几条经验：

参数	影响	调优建议
Vmax	决定最高速度	不超过电机额定转速的80%
Amax	决定加速快慢	考虑负载惯量，避免过冲
Dmax	决定减速快慢	通常比Amax大10%-20%，防止追尾

我曾经在一个AGV项目里，把Amax设得太大。结果小车启动时直接打滑，差点撞到货架。后来我学乖了，Amax一般取电机理论最大值的60%-70%。

4.4 边界条件处理：那些容易翻车的地方

梯形规划看着简单，但边界条件处理不好，代码分分钟崩。我遇到过的情况：

• 距离为0或负数：直接抛异常，或者返回零速度曲线
• Vmax为0：规划器直接罢工，需要做保护
• Amax或Dmax为0：除零错误，必须检查
• 距离极小：连三角形曲线都形成不了，需要特殊处理

嗯，这里要注意。对于极小距离的情况，我一般这样处理：

def plan_safe(self, distance, min_distance=0.001):
    """带边界检查的规划"""
    if distance < min_distance:
        # 距离太小，直接返回一个脉冲
        t = np.array([0.0, 0.01])
        v = np.array([0.0, 0.0])
        return t, v
    
    if self.vmax <= 0 or self.amax <= 0 or self.dmax <= 0:
        raise ValueError("速度/加速度/减速度必须为正数")
        
    return self.plan(distance)

4.5 可视化：看看曲线长啥样

光看数字不够直观。我习惯把规划结果画出来，一眼就能看出问题。下面是一个简单的可视化代码：

import matplotlib.pyplot as plt

def plot_profile(t, v, title="梯形速度曲线"):
    plt.figure(figsize=(10, 4))
    plt.plot(t, v, 'b-', linewidth=2)
    plt.xlabel('时间 (s)')
    plt.ylabel('速度 (m/s)')
    plt.title(title)
    plt.grid(True, alpha=0.3)
    plt.show()

# 使用示例
planner = TrapezoidalPlanner(vmax=1.0, amax=0.5, dmax=0.5)
t, v = planner.plan(distance=2.0)
plot_profile(t, v)

你运行一下就会发现，当距离足够大时，曲线是标准的梯形。当距离变小时，匀速段消失，变成三角形。这就是边界条件在起作用。

4.6 实战：一个完整的例子

咱们来跑一个完整的例子。假设有个直线运动轴，需要移动1.5米：

planner = TrapezoidalPlanner(vmax=0.8, amax=0.3, dmax=0.35)
t, v = planner.plan(distance=1.5)

print(f"实际最大速度: {planner.v_actual:.3f} m/s")
print(f"加速时间: {planner.t_acc:.3f} s")
print(f"匀速时间: {planner.t_vel:.3f} s")
print(f"减速时间: {planner.t_dec:.3f} s")
print(f"总时间: {planner.t_total:.3f} s")

输出结果：

实际最大速度: 0.800 m/s
加速时间: 2.667 s
匀速时间: 0.208 s
减速时间: 2.286 s
总时间: 5.161 s

你看，匀速段只有0.2秒，几乎就是三角形曲线了。如果距离再短一点，匀速段就彻底消失。

4.7 知识体系总览

下面这张图，把梯形规划的核心逻辑串起来了。你对照着看，思路会更清晰：

这张图把整个规划流程串起来了。你从输入参数开始，先判断能不能达到Vmax，然后走不同的分支。最后输出时间-速度曲线。逻辑很清晰，对吧？

好了，梯形规划的核心内容就这些。代码你拿过去就能用，参数调优的建议也给了。实际项目中遇到问题，欢迎交流。

可可精品资料库