梯形加减速算法实现

• 梯形加减速算法实现
  • 前言
  • 几种常见的加减速算法
    • 梯形加减速
    • 指数型加减速
    • S型曲线加减速
  • 步进电机梯形加减速原理
    • 背景与问题
    • 原理分析
    • 算法概念与基础方程
    • 数学模型与公式推导
      • 基本运动参数
      • 速度与时间关系
    • 位移（脉冲数）计算
    • 定时器计数与脉冲频率
  • 算法实现
    • 实现步骤
    • 示例代码
  • 优缺点分析
    • 优点
    • 缺点
  • 总结

前言

步进电机因其无需反馈就能对位置和速度进行控制，在工业自动化中应用极为广泛，但是对于速度变化较大的，如加减速频繁的设备，会出现丢步现象或力矩不足，可能产选项没有问题，其问题在于负载位置对控制系统没有反馈。产生的脉冲大于步进电机极限启动频率，是产生丢步的根源，大多在启动瞬间和停止瞬间发生，例如：瞬间以20Khz的频率启动，在20Khz瞬间停止。因为惯性等因素会产生过冲现象。导致永久性失步。因此，防止过冲和失步是开环控制系统能否正常运行的关键

本文只介绍关于梯形加减速原理和实现

几种常见的加减速算法

梯形加减速

算法简便,计算用时少、响应快、效率高,实现方便。在,变速和匀速转折点不能平滑过渡。主要应用在对升降速过程要求不高的场合

指数型加减速

克服了梯形加减速的速度不平稳问题,运动精度得到了提高,但初始加速度大,容易引起机械部件的冲击,在加减速的起点仍然存在加减速突变,限制了加速度的提高

S型曲线加减速

S曲线加减速是一种柔性程序较好的控制策略,能让电机性能得到充分的发挥,冲击振动小,但是实现过程比较复杂,计算量相对较大,并且加减速效率不高

步进电机梯形加减速原理

背景与问题

步进电机的运行依赖于控制器发送的脉冲信号，每个脉冲驱动电机旋转固定角度（步距角）。若脉冲频率过高或变化过快，电机可能因无法及时响应而堵转或丢步。例如，瞬间以 20kHz 频率启动，或从 20kHz 骤停，会因惯性导致过冲或失步。梯形加减速算法通过控制脉冲频率的线性变化，确保电机在启动和停止时逐步调整速度，从而避免上述问题

原理分析

梯形加减速的核心是使脉冲频率随时间线性变化，从而实现恒定加速度的加速和减速过程。其速度曲线如图所示：

定义：指按直线方式（从启动速度到目标速度的加减速），以一定比例进行加速/减速，加速度恒定。
整个过程是一个梯形的模型，所以以它的数学模型命名的加减速算法。这种算法是一种在加速过程和减速过程中加速度不变的匀变速控制算法，由于速度变化的曲线有折点，所以在启动、停止、匀速段中很容易产生冲击和振动

算法概念与基础方程

如果电机需要以恒定速度运动，控制器就要以恒定频率发送脉冲；如果电机需要以加减速度运动，同理，如图：

• $(t0)$ 表示脉冲发送起始时刻
• $(t1)$ 表示脉冲发送第二时刻
• $(t2)$ 表示脉冲发送第三时刻
• $(tt)$ 表示定时器的计数周期
• $(c0)$ 表示定时器从 $(t0\to t1)$ 时刻的定时器计数值
• $(c1)$ 表示定时器从 $(t1\to t2)$ 时刻的定时器计数值
• $(\delta t)$ 表示两个脉冲之间的时间间隔

以 stm32 的高级定时器 8 为例，定时器 8 的时钟频率为 168MHZ, 将分频值设置为 5，那么定时器的时钟频率则为：168/(5+1)=28MHZ, 计数 28M 次正好为一秒，周期与频率为倒数关系，所以分频值为 5 的定时器 8 的计数周期为:1/28MHZ

数学模型与公式推导

梯形加减速算法的核心在于控制脉冲频率随时间线性变化，以实现恒定加速度的加速和减速过程。以下是梯形加减速的数学模型与推导：

基本运动参数

• 初始速度 $(v_0)$: 电机启动时的速度，通常为 0 或一个较低的启动频率（脉冲/秒）
• 目标速度 $(v_m)$: 电机匀速运行时的速度（脉冲/秒）
• 加速度 $(a)$: 速度变化率，单位为脉冲/秒²，决定了加速和减速的快慢
• 加速时间 $(t_a)$: 从初始速度加速到目标速度所需的时间
• 减速时间 $(t_d)$: 从目标速度减速到停止（或最低速度）所需的时间
• 总运行时间 $(t)$: 包括加速、匀速和减速阶段的总时间
• 总脉冲数 $(N)$: 电机在整个运动过程中发出的脉冲总数，与目标位置相关

速度与时间关系

在加速阶段，速度随时间线性增加：
$$v(t)=v_0+a\cdot t,0\le t\le t_a$$

其中，加速时间：
$$t_a=\frac{v_m-v_0}{a}$$

匀速阶段，速度保持为 $(v_m)$，持续时间为 $(t_c)$。减速阶段，速度线性减少：
$$v(t)=v_m-a\cdot (t-t_a-t_c),t_a+t_c\le t\le t_a+t_c+t_d$$

减速时间：
$$t_d=\frac{v_m-v_0}{a}$$

位移（脉冲数）计算

位移（脉冲数）为速度对时间的积分。加速阶段脉冲数 $(N_a)$：
$$N_a={\int }_0^{t_a}(v_0+a\cdot t)dt=v_0\cdot t_a+\frac{1}{2}a\cdot t_a^2$$

匀速阶段脉冲数 $(N_c)$：
$$N_c=v_m\cdot t_c$$

减速阶段脉冲数 $(N_d)$（假设减速到 $(v_0)$）：
$$N_d={\int }_0^{t_d}(v_m-a\cdot t)dt=v_m\cdot t_d-\frac{1}{2}a\cdot t_d^2$$

总脉冲数：
$$N=N_a+N_c+N_d$$

若已知总脉冲数 $(N)$，可通过解方程确定 $(t_c)$：
$$t_c=\frac{N-(v_m^2-v_0^2)/a}{v_m}$$

若 $(t_c<0)$，说明总脉冲数不足以达到匀速阶段，需采用三角形加减速曲线

定时器计数与脉冲频率

脉冲频率 $(f(t))$ 与速度成正比，定时器计数周期 $(\Delta t)$ 为：
$$\Delta t=\frac{1}{f(t)}$$

定时器计数值：
$$C_n=\frac{f_{clk}}{f(t)}$$

其中，$(f_{clk})$ 为定时器时钟频率（如 28 MHz）。在加速阶段，频率随时间线性增加：
$$f(t)=v_0+a\cdot t$$

计数值动态调整为：
$$C_n=\frac{f_{clk}}{v_0+a\cdot t}$$

算法实现

实现步骤

1. 参数初始化：
   • 设置 $(v_0)$、$(v_m)$、$(a)$、$(N)$
   • 计算 $(t_a=\frac{v_m-v_0}{a})$，$(t_d=\frac{v_m-v_0}{a})$
   • 根据 $(N)$，计算 $(t_c)$。若 $(t_c<0)$，调整为三角形曲线
2. 加速阶段：
   • 按线性增加的频率发送脉冲，动态更新定时器计数值
   • 累加脉冲计数，判断是否达到 $(v_m)$
3. 匀速阶段：
   • 以恒定频率 $(v_m)$ 发送脉冲，持续 $(t_c)$
4. 减速阶段：
   • 按线性减少的频率发送脉冲，直至速度降为 $(v_0)$ 或 0
5. 边界处理：
   • 若 $(N)$ 不足以进入匀速阶段，采用三角形曲线，重新计算速度峰值
   • 考虑电机的最大频率限制，防止超频导致堵转

示例代码

以下是基于 STM32 的梯形加减速算法实现的伪代码，使用高级定时器 8：

#include "stm32f4xx.h"
#include <math.h>

// 电机和定时器参数
#define CLK_FREQ 28000000 // 定时器时钟频率 28 MHz
#define V0 100.0f         // 初始速度 (脉冲/秒)
#define VM 1000.0f        // 目标速度 (脉冲/秒)
#define ACC 2000.0f       // 加速度 (脉冲/秒²)
#define TOTAL_PULSES 5000 // 总脉冲数
#define MAX_FREQ 5000.0f  // 电机最大频率 (脉冲/秒)

volatile uint32_t pulse_count = 0; // 当前脉冲计数
volatile float current_freq = V0;  // 当前频率
volatile uint8_t phase = 0;        // 0: 加速, 1: 匀速, 2: 减速
volatile uint32_t accel_pulses;    // 加速阶段脉冲数
volatile uint32_t decel_pulses;    // 减速阶段脉冲数
volatile uint32_t const_pulses;    // 匀速阶段脉冲数
volatile float peak_freq;          // 三角形加减速峰值频率

// 初始化定时器
void init_timer(void) {
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
    
    TIM_TimeBaseInitTypeDef TIM_InitStruct;
    TIM_InitStruct.TIM_Prescaler = 5; // 分频值
    TIM_InitStruct.TIM_Period = (uint32_t)(CLK_FREQ / V0) - 1; // 初始计数周期
    TIM_InitStruct.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_InitStruct.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TIM8, &TIM_InitStruct);
    TIM_ITConfig(TIM8, TIM_IT_Update, ENABLE);
    TIM_Cmd(TIM8, ENABLE);

    // 配置 NVIC
    NVIC_InitTypeDef NVIC_InitStruct;
    NVIC_InitStruct.NVIC_IRQChannel = TIM8_UP_TIM13_IRQn;
    NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0;
    NVIC_InitStruct.NVIC_IRQChannelSubPriority = 1;
    NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStruct);

    // 计算各阶段脉冲数
    float t_a = (VM - V0) / ACC; // 加速时间 (秒)
    accel_pulses = (uint32_t)(V0 * t_a + 0.5f * ACC * t_a * t_a);
    decel_pulses = accel_pulses; // 假设对称
    const_pulses = TOTAL_PULSES - accel_pulses - decel_pulses;
    
    if (const_pulses < 0) {
        // 三角形加减速
        peak_freq = sqrtf(ACC * TOTAL_PULSES + V0 * V0);
        if (peak_freq > MAX_FREQ) peak_freq = MAX_FREQ; // 限制最大频率
        accel_pulses = TOTAL_PULSES / 2;
        decel_pulses = TOTAL_PULSES - accel_pulses;
        const_pulses = 0;
    } else {
        peak_freq = VM;
    }
}

// 定时器中断处理函数
void TIM8_UP_TIM13_IRQHandler(void) {
    if (TIM_GetITStatus(TIM8, TIM_IT_Update) != RESET) {
        TIM_ClearITPendingBit(TIM8, TIM_IT_Update);
        
        pulse_count++;
        if (pulse_count >= TOTAL_PULSES) {
            TIM_Cmd(TIM8, DISABLE); // 停止定时器
            return;
        }

        // 加速阶段
        if (phase == 0 && pulse_count < accel_pulses) {
            current_freq += ACC / current_freq; // 频率增量 Δf = a / f
            if (current_freq >= peak_freq || pulse_count >= accel_pulses) {
                current_freq = peak_freq;
                phase = 1; // 进入匀速阶段
            }
        }
        // 匀速阶段
        else if (phase == 1 && pulse_count < (accel_pulses + const_pulses)) {
            current_freq = peak_freq;
        }
        // 减速阶段
        else if (phase == 1 || phase == 2) {
            phase = 2;
            current_freq -= ACC / current_freq; // 频率减量 Δf = -a / f
            if (current_freq <= V0) {
                current_freq = V0;
            }
        }

        // 更新定时器周期
        TIM_SetAutoreload(TIM8, (uint32_t)(CLK_FREQ / current_freq) - 1);
    }
}

// GPIO 初始化（用于脉冲输出）
void init_gpio(void) {
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
    GPIO_InitTypeDef GPIO_InitStruct;
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6; // 假设使用 PC6 输出脉冲
    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
    GPIO_Init(GPIOC, &GPIO_InitStruct);
    GPIO_PinAFConfig(GPIOC, GPIO_PinSource6, GPIO_AF_TIM8);
}

int main(void) {
    init_gpio();
    init_timer();
    while (1) {
        // 主循环可添加其他控制逻辑
    }
}

优缺点分析

优点

• 实现简单：算法逻辑清晰，易于在嵌入式系统中实现
• 计算量小：适合资源受限的微控制器
• 适用广泛：满足大多数工业控制需求

缺点

• 速度不平滑：转折点处的速度突变可能引发振动
• 冲击问题：对高精度或高速设备可能不够理想

总结

梯形加减速算法以其简单高效的特点，成为步进电机控制的首选方案。通过线性变化的脉冲频率实现恒定加速度，能有效减少丢步和过冲现象。尽管在速度转折点存在冲击问题，但其计算量小、响应快的优点使其适合大多数工业场景