STM32 电机动态 PI 控制：从编码器反馈、增量式 PI 到 Matlab 参数拟合与 PWM 输出

前言在 STM32 智能小车、移动机器人以及其他直流电机控制系统中最基础的电机控制方式是直接改变 PWM 占空比。例如PWM 5000;PWM 增大后电机得到的驱动力通常会增大转速也会随之提高。但是直接控制 PWM 存在一个明显问题PWM 决定的是给电机多大的驱动力并不能保证电机一定达到某个确定的速度。同一个 PWM在不同情况下可能得到不同的实际转速例如电池电量充足 → 1.2 m/s 电池电压下降 → 1.0 m/s 负载增加 → 0.8 m/s 地面摩擦增大 → 0.7 m/s因此如果希望机器人能够稳定地保持某个目标速度就需要加入编码器反馈并利用 PI 或 PID 控制器不断调整 PWM。本文介绍一种进一步改进的电机控制方法动态 PI 速度控制。它在普通 PI 控制的基础上让KP和KI随电机当前速度动态变化使控制器能够适应不同的速度区间。完整控制链路如下目标速度 ↓ 编码器测量实际速度 ↓ 计算速度误差 ↓ 根据当前速度动态计算 KP、KI ↓ 增量式 PI 计算 ↓ 得到 PWM 控制量 ↓ PWM 限幅 ↓ H 桥驱动电机 ↓ 电机速度发生变化 ↓ 编码器再次反馈 └────────────────→ 下一控制周期一、动态 PI 控制整体框架一个典型的四轮移动机器人通常有四个独立电机Motor A Motor B Motor C Motor D每个电机都拥有目标速度 Target 实际速度 Encoder PI 控制器 PWM 输出因此可以分别进行速度闭环控制MOTOR_A.Motor_Pwm Incremental_PI_A( MOTOR_A.Encoder, MOTOR_A.Target ); MOTOR_B.Motor_Pwm Incremental_PI_B( MOTOR_B.Encoder, MOTOR_B.Target ); MOTOR_C.Motor_Pwm Incremental_PI_C( MOTOR_C.Encoder, MOTOR_C.Target ); MOTOR_D.Motor_Pwm Incremental_PI_D( MOTOR_D.Encoder, MOTOR_D.Target );整体结构可以表示为机器人目标运动 ↓ 运动学逆解 ↓ ┌───────────┼───────────┐ ↓ ↓ ↓ ↓ A目标速度 B目标速度 C目标速度 D目标速度 ↓ ↓ ↓ ↓ PI PI PI PI ↓ ↓ ↓ ↓ PWM_A PWM_B PWM_C PWM_D ↓ ↓ ↓ ↓ 电机A 电机B 电机C 电机D ↑ ↑ ↑ ↑ 编码器A反馈 编码器B反馈 编码器C反馈 编码器D反馈从控制系统角度来看每个轮子实际上都是一个独立的速度闭环。二、动态 PI 核心代码以下以电机 A 为例int Incremental_PI_A(float Encoder, float Target) { /* 跨控制周期保存状态 */ static float Bias; static float Pwm; static float Last_bias; /* 1. 当前速度误差 */ Bias Target - Encoder; /* 2. 根据当前速度动态计算 PI 参数 */ Velocity_KP 218.0f 179.0f * (Encoder / 3.5f) 21.0f * (Encoder * Encoder) / (3.5f * 3.5f); Velocity_KI 218.0f 179.0f * (Encoder / 3.5f) 21.0f * (Encoder * Encoder) / (3.5f * 3.5f); /* 3. 增量式 PI */ Pwm Velocity_KP * (Bias - Last_bias) Velocity_KI * Bias; /* 4. PWM 限幅 */ if(Pwm 16700) Pwm 16700; if(Pwm -16700) Pwm -16700; /* 5. 保存当前误差 */ Last_bias Bias; return (int)Pwm; }这段程序可以拆成五个步骤目标速度与实际速度比较 ↓ 计算当前误差 ↓ 动态计算 KP、KI ↓ 增量式 PI 计算 PWM ↓ PWM 限幅并输出下面依次展开。步骤 1获取目标速度与实际速度PI 控制器首先需要知道两件事情Target表示希望电机达到的目标速度以及Encoder表示编码器测量得到的实际速度例如Target 1.5 m/s Encoder 1.2 m/s则当前速度误差为Bias Target - Encoder;即代入数据e(k)1.5-1.20.3因为误差为正说明实际速度 目标速度电机需要继续加速。反过来如果Target 1.5 m/s Encoder 1.8 m/s则e(k)1.5-1.8-0.3说明实际速度 目标速度控制器需要降低输出。因此Bias 0 → 电机偏慢 Bias 0 → 电机偏快 Bias ≈ 0 → 实际速度已经接近目标速度步骤 2增量式 PI 控制1. PI 控制中的 KP 和 KIPI 控制器包含两个核心参数KP比例增益 KI积分增益2. KP 的作用KP主要决定系统对当前误差变化的响应力度。可以简单理解为误差变化大 ↓ 控制器快速调整 误差变化小 ↓ 控制器小幅调整因此 KP 主要影响响应速度 控制灵敏度 动态跟随能力KP 太小响应比较慢KP 太大容易出现超调甚至振荡3. KI 的作用KI 主要用于消除长期存在的小误差。例如目标速度 1.0 m/s 实际速度 0.95 m/s虽然只差0.05 m/s但如果这种误差持续存在控制器就应该继续进行修正。积分作用会持续累积这个误差使 PWM 逐渐变化最终尽量实现实际速度 ≈ 目标速度所以KP 主要负责“快速响应” KI 主要负责“减小稳态误差”步骤 3增量式 PI 公式程序采用Pwm Velocity_KP * (Bias - Last_bias) Velocity_KI * Bias;对应数学表达式然后这里e(k) 当前误差 e(k-1) 上一次误差 KP 比例增益 KI 积分增益 ΔPWM 本控制周期需要增加或减少的 PWM1.为什么叫“增量式”因为控制器并不是每次重新计算一个全新的 PWM。它计算的是在上一次 PWM 基础上本次需要增加多少或者减少多少。例如当前PWM 5000控制器根据误差算出ΔPWM 300那么新 PWM 5000 300 5300下一周期继续测速。假设速度已经更接近目标值ΔPWM 80则PWM 5380如此不断迭代。因此它的过程为测量 ↓ 计算误差 ↓ 计算 PWM 增量 ↓ 修改 PWM ↓ 再次测量步骤 4根据当前速度动态计算 KP 和 KI示例程序使用程序实现Velocity_KP 218.0f 179.0f * (Encoder / 3.5f) 21.0f * (Encoder * Encoder) / (3.5f * 3.5f);其中v Encoder也就是当前编码器计算出来的实际速度。三、参数变化示例假设v1.5则得到约如果v3.0则得到约因此这个参数模型表现为速度较低 ↓ KP 较小 速度提高 ↓ KP 增大需要注意的是这只是当前参数模型的变化趋势并不意味着所有电机都必须按照“速度越高KP/KI 越大”的方式调节。最终关系应该由实验结果决定。步骤 5实验数据采集例如依次测试0.5 m/s 1.0 m/s 1.5 m/s 2.0 m/s 2.5 m/s 3.0 m/s 3.5 m/s在每个速度点调节KP KI同时观察1. 上升时间 2. 超调量 3. 稳态误差 4. 振荡情况 5. 达到目标速度后的稳定程度最终可以形成类似的数据车速 vm/sKPKI0.52402501.02702801.53003152.03253502.53553903.03904303.5420450这里的数据只是用来说明回归流程。不同电机的性能 减速比 机械结构 机器人重量 轮胎摩擦 电池状态都会影响最终 PI 参数因此不能直接照搬。四、Matlab 二次回归示例假设已经获取v [0.5 1.0 1.5 2.0 2.5 3.0 3.5]; KP_values [240 270 300 325 355 390 420]; KI_values [250 280 315 350 390 430 450];使用coeffs_KP polyfit(v, KP_values, 2); coeffs_KI polyfit(v, KI_values, 2);其中polyfit(x, y, 2)最后的2表示使用二次多项式进行回归。得到的结果形式coeffs_KP [a b c]对应同理五、Matlab 绘制拟合结果除了计算系数还可以画图观察拟合效果v [0.5 1.0 1.5 2.0 2.5 3.0 3.5]; KP_values [240 270 300 325 355 390 420]; % 二次拟合 p polyfit(v, KP_values, 2); % 生成连续速度 v_fit linspace(0.5, 3.5, 100); % 根据拟合公式计算KP KP_fit polyval(p, v_fit); % 绘制实验数据 scatter(v, KP_values, filled); hold on; % 绘制拟合曲线 plot(v_fit, KP_fit); xlabel(Velocity (m/s)); ylabel(KP); title(KP-Velocity Polynomial Fitting); grid on;最终可以看到实验采样点 二次拟合曲线如果拟合曲线能够较好地接近实验数据点就可以进一步将其写入 STM32。步骤 6将回归公式部署到 STM32Matlab 得到数学模型以后真正运行机器人时并不需要 Matlab。只需要把公式写进 STM32Velocity_KP 218.0f 179.0f * (Encoder / 3.5f) 21.0f * (Encoder * Encoder) / (3.5f * 3.5f);运行过程中编码器测得当前速度 ↓ 代入数学公式 ↓ 得到当前 KP、KI ↓ 执行 PI例如Encoder 1.5计算一套 PI 参数。下一周期Encoder 2.3马上计算另一套参数。所以这里并不是每隔一段时间重新人工调一次 PI而是MCU 在运行过程中实时自动计算 PI 参数步骤 7计算 PWM获得当前KP KI以后就可以进入增量式 PIPwm Velocity_KP * (Bias - Last_bias) Velocity_KI * Bias;完整数据关系目标速度 Target │ ↓ 计算误差 ↑ │ 实际速度 Encoder │ └────→计算 KP、KI ↓ 增量式 PI ↓ PWMPI 最终输出的并不是速度而是电机需要多大的控制量。在本系统中这个控制量最终就是 PWM。步骤 8PWM 限幅PI 本质上是不断累积调整 PWM。如果误差很大理论上可能出现PWM 20000 PWM 30000 ...但实际定时器和电机驱动器都有最大输出范围。因此需要if(Pwm 16700) Pwm 16700; if(Pwm -16700) Pwm -16700;最终控制-16700 ≤ PWM ≤ 16700PWM 的正负号可以表示电机方向而绝对值表示控制力度例如5000代表一个方向。-5000代表另外一个方向。而10000相较于5000代表更大的驱动输出。步骤 9通过 H 桥驱动电机PI 得到MOTOR_A.Motor_Pwm之后这仍然只是程序中的数字。需要通过Set_Pwm();真正写入定时器比较寄存器。例如Set_Pwm( MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm );对于双输入 H 桥一般需要把一个有符号 PWM 转换成两路控制信号。例如void Set_Motor_PWM(int motor_pwm) { if(motor_pwm 0) { MOTOR_PWM1 PWM_MAX; MOTOR_PWM2 PWM_MAX motor_pwm; } else { MOTOR_PWM2 PWM_MAX; MOTOR_PWM1 PWM_MAX - motor_pwm; } }例如motor_pwm 5000可能得到PWM1 11799 PWM2 16799而motor_pwm -5000得到PWM1 16799 PWM2 11799两路 H 桥控制信号的作用关系交换因此电机电流方向发生变化最终实现正反转。完整程序框架最终整个电机动态 PI 控制程序可以概括成void Motor_Control_Task(void) { /* 1. 获取电机目标速度 */ Drive_Motor(Vx, Vy, Vz); /* 2. 编码器测速 */ Get_Velocity_Form_Encoder(); /* 3. 动态PI控制 */ MOTOR_A.Motor_Pwm Incremental_PI_A( MOTOR_A.Encoder, MOTOR_A.Target ); MOTOR_B.Motor_Pwm Incremental_PI_B( MOTOR_B.Encoder, MOTOR_B.Target ); MOTOR_C.Motor_Pwm Incremental_PI_C( MOTOR_C.Encoder, MOTOR_C.Target ); MOTOR_D.Motor_Pwm Incremental_PI_D( MOTOR_D.Encoder, MOTOR_D.Target ); /* 4. PWM限幅 */ Limit_Pwm(16700); /* 5. 输出到H桥 */ Set_Pwm( MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm ); }对应流程Drive_Motor() 运动学逆解 ↓ 获得 Target ↓ Get_Velocity_Form_Encoder() ↓ 获得 Encoder ↓ Incremental_PI() ↓ 动态 KP / KI ↓ 计算 PWM ↓ Limit_Pwm() ↓ Set_Pwm() ↓ H桥 ↓ 电机 ↓ 编码器反馈总结动态 PI 控制的核心并不复杂。普通 PI 是固定 KP 固定 KI ↓ 根据速度误差计算 PWM动态 PI 则是在 PI 前面增加了一层参数调度当前速度 v ↓ 计算 KP(v) 计算 KI(v) ↓ 增量式 PI ↓ PWM整个电机闭环可以表示为目标速度 ↓ 编码器反馈实际速度 ↓ 计算速度误差 ↓ 根据当前速度动态计算 KP、KI ↓ 增量式 PI ↓ PWM 控制量 ↓ PWM 限幅 ↓ H 桥 ↓ 电机 ↓ 编码器再次反馈其中 Matlab 主要用于开发阶段真实实验 ↓ 不同速度下调试 KP、KI ↓ 采集 (v, KP, KI) ↓ Matlab 多项式回归 ↓ 得到 KP(v)、KI(v) ↓ 写入 STM32机器人真正运行以后并不需要 Matlab 实时参与。STM32 只需要读取当前速度 ↓ 代入已经得到的函数 ↓ 实时计算 KP、KI ↓ 完成 PI 控制这种方法本质上是一种计算量较小、实现简单的增益调度策略。相比单纯固定 PI它能够让控制器根据当前工作速度选择不同的增益因此在机器人运行速度范围较大、不同速度区间动态特性差异明显时具有一定的工程应用价值。Question1:为什么需要动态 PI普通 PI 一般采用固定参数KP 300; KI 250;无论电机0.5 m/s 1.5 m/s 3.0 m/s都使用同样的参数。但真实电机的控制特性并不是完全不变的。不同速度区间可能表现出不同特征。例如低速 机械摩擦、死区影响明显 中速 系统相对稳定 高速 惯性、反电动势、负载变化影响更加明显于是固定 PI 可能产生低速表现很好高速响应慢 或者 高速表现很好低速控制过于敏感动态 PI 的思路就是也就是说速度改变 ↓ PI 参数随之改变这种控制方法实际上可以看作一种简单的增益调度 Gain Scheduling。Question2这些 KP 和 KI 参数是怎么来的动态 PI 的关键并不是KP a b*v c*v*v;而是如何找到a b c这些系数。一个比较直观的工程方法是真实电机实验 ↓ 设置不同目标速度 ↓ 人工调整 KP、KI ↓ 寻找效果较好的参数 ↓ 记录 速度 v KP KI ↓ 将数据输入 Matlab ↓ 回归分析 ↓ 得到 KP(v)、KI(v)Question3为什么使用 Matlab完成实验以后得到的是一些离散数据v 0.5 → KP 240 v 1.0 → KP 270 v 1.5 → KP 300 ...但是机器人实际运行过程中速度并不一定正好等于这些实验点。例如v 1.37 m/s此时依然需要知道应该使用多少 KP。因此需要从离散实验点得到连续数学函数Matlab 在这里主要承担回归拟合与数据分析。Question4为什么使用二次多项式回归一次函数 → 简单但只能表达线性趋势 二次函数 → 可以描述一定非线性 高阶函数 → 表达能力更强但更容易过拟合二次多项式属于拟合能力与计算复杂度之间比较合适的折中方案。同时 STM32 计算K a b*v c*v*v;非常简单对 MCU 的实时计算负担也很小。