FreeRTOS 控制任务设计 (2)--- 运动学逆解 + PID 闭环 + PWM 驱动全流程实现

🎬 渡水无言 ：个人主页渡水无言

❄专栏传送门 ： 《linux专栏》《嵌入式linux驱动开发》《linux系统移植专栏》

❄专栏传送门 ： 《freertos专栏》 《STM32 HAL库专栏》《linux裸机开发专栏》

❄专栏传送门 ：《产品测评专栏》

⭐️流水不争先，争的是滔滔不绝

📚博主简介：第二十届中国研究生电子设计竞赛全国二等奖 |国家奖学金 | 省级三好学生

| 省级优秀毕业生获得者 | csdn新星杯TOP18 | 半导纵横专栏博主 | 211在读研究生

在这里主要分享自己学习的linux嵌入式领域知识；有分享错误或者不足的地方欢迎大佬指导，也欢迎各位大佬互相三连

​

目录

前言

一、运动学逆解

[二、PID 控制简介](#二、PID 控制简介)

[三、PID 代码实现](#三、PID 代码实现)

四、PWM输出

五、整体控制流程

总结

---

前言

在上一节中，我们完成了控制任务的两级模式设计与任务通知交互，实现了从 "指令触发" 到 "闭环运行" 的平滑切换。在每个10ms的控制周期内，真正让底盘动起来的，是运动学逆解、PID 控制与 PWM 输出这三个关键环节。

这三者构成了控制闭环的核心：运动学逆解负责 "算目标"，PID 控制负责 "追目标"，硬件驱动负责 "执行目标"，三者分工明确、解耦清晰，即使后续更换底盘结构或驱动方案，也只需修改对应模块，不影响整体控制框架。

---

一、运动学逆解

本项目的底盘采用两轮差速驱动结构，左右两侧各有一个独立驱动轮，通过控制左右轮的转速不同，实现前进、后退以及转向等运动形式。

上位控制模块（如导航或遥控模块）并不会直接给出左右轮的速度，而是以更直观的方式给出机器人 X 轴速度、Y 轴速度和角速度。因此，在进入电机控制之前，需要通过运动学逆解，将整体运动指令转换为左右轮各自的目标速度。

在本系统中，上层给出的运动指令包含两个量（差速模型，没有 Y 轴速度）：

线速度 vx_mmps：表示机器人沿自身前进方向的速度，单位为毫米每秒（mm/s）；

角速度 wz_mradps：表示机器人绕自身中心垂直轴旋转的角速度，单位为毫弧度每秒（mrad/s）。

差速底盘的运动学模型基于一个直观的物理关系：

当机器人直线行驶时，左右轮速度相等；

当机器人发生转向时，外侧轮速度更快，内侧轮速度更慢，二者之间的速度差由角速度和轮距共同决定。

在理想情况下，在代码实现中，首先将角速度从毫弧度每秒转换为弧度每秒，这是因为后续计算采用的是国际单位制。随后根据底盘的轮距参数 WHEEL_BASE_MM 计算半轮距 half_base，并利用如下关系得到左右轮目标速度：

左轮目标速度 = 线速度 − 角速度 × 半轮距。

右轮目标速度 = 线速度 + 角速度 × 半轮距。

/* ==================================================
 * 差速逆解 (单位: mm/s, mrad/s)
 * 输入:
 *   vx_mmps  : 线速度 mm/s
 *   wz_mradps: 角速度 mrad/s
 * 输出:
 *   vL_mmps/vR_mmps: 左右轮目标线速度 mm/s
 * ================================================== */
static void diff_inverse_kinematics_mmps_mradps(int16_t vx_mmps, int16_t wz_mradps, float* vL, float* vR)
{
    // 将角速度从 mrad/s 转换为 rad/s
    float wz_rad = (float)wz_mradps / 1000.0f;
    // 计算底盘半轮距
    float half_base = WHEEL_BASE_MM * 0.5f;

    // 计算左右轮目标速度
    float vL_val = (float)vx_mmps - wz_rad * half_base;
    float vR_val = (float)vx_mmps + wz_rad * half_base;

    // 限制轮速在最大范围内，防止超出电机性能
    vL_val = clampf(vL_val, -MAX_WHEEL_SPEED_MMPS, MAX_WHEEL_SPEED_MMPS);
    vR_val = clampf(vR_val, -MAX_WHEEL_SPEED_MMPS, MAX_WHEEL_SPEED_MMPS);

    *vL = vL_val;
    *vR = vR_val;
}

说明：运动学逆解过程只涉及几何关系和速度分配，不关心电机如何实现这些速度，也不涉及 PID 控制、电流控制或 PWM 输出等细节。后续如果底盘结构或参数发生变化（例如轮距改变），只需修改运动学相关参数即可，不影响整体控制框架。

二、PID 控制简介

在差速底盘控制中，运动学逆解只能计算出左右轮的期望速度，而电机本身并不能直接接收 "速度" 这一抽象指令。电机真正能接收和执行的，是 PWM（脉宽调制）信号。因此，必须通过一套控制算法，将 "目标速度" 转换为 "合适的 PWM 输出"，并在执行过程中不断修正偏差，保证实际速度尽可能贴近目标速度。本项目中，这一任务由PID 控制器 + PWM 输出机制共同完成。

PID 控制器的组成与含义

PID 控制器由比例（P）、积分（I）和微分（D）三部分组成，本项目中每个轮子各自使用一个独立的 PID 控制器，互不干扰。
**比例项（P）：**反映当前误差的大小。当目标速度与测量速度之间存在偏差时，比例项会立即产生一个与误差成正比的输出，误差越大，输出修正越强。比例项的作用是快速响应。

局限： 但单独使用比例控制往往会留下稳态误差。
**积分项（I）：**用于累计历史误差。当系统长期存在小误差时，积分项会逐步累加并放大其影响，推动输出继续调整，直到误差被消除。积分项的引入可以有效消除稳态误差。

局限： 但如果不加限制，容易产生 "积分饱和" 问题，导致系统响应变慢甚至振荡，因此代码中通过i_limit对积分项进行限制。
**微分项（D）：**用于反映误差变化的趋势，即误差是 "在变大还是变小"。它相当于对未来的预测，可以在误差变化过快时提前抑制输出，减少震荡和超调。当前代码中微分系数暂设为 0，便于调试和稳定系统，后续可按需开启。

PID 计算

利用公式 Output=Kp​×err+Ki​×err_acc+Kd​×err_dot 计算出 PWM 修正量

err：误差。

err_acc:误差累积。

err_dot:误差变化率。

综合来看，PID 的输出是三项之和：比例负责 "快"，积分负责 "准"，微分负责 "稳"。

三、PID 代码实现

/* ==================================================
 * PID控制器初始化
 * ================================================== */
static void PID_Init(PID_t* pid, float kp, float ki, float kd, float i_limit, float out_limit)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->i_acc = 0.0f;
    pid->prev_err = 0.0f;
    pid->i_limit = i_limit;
    pid->out_limit = out_limit;
}

/* ==================================================
 * PID更新计算
 * 公式: Output = Kp*err + Ki*err_acc + Kd*(err-prev_err)/dt
 * ================================================== */
static int16_t PID_Update(PID_t* pid, float ref, float meas, float dt)
{
    // 计算误差
    float err = ref - meas;

    // 比例项
    float p_out = pid->kp * err;

    // 积分项（带限幅）：误差累积 * Ki
    pid->i_acc += pid->ki * err * dt;
    pid->i_acc = clampf(pid->i_acc, -pid->i_limit, pid->i_limit);

    // 微分项（可选）：误差变化率 * Kd
    float d_out = 0.0f;
    if (pid->kd != 0.0f && dt > 0.0f) {
        d_out = pid->kd * (err - pid->prev_err) / dt;
    }
    pid->prev_err = err;

    // 总输出 + 限幅
    float out = p_out + pid->i_acc + d_out;
    out = clampf(out, -pid->out_limit, pid->out_limit);

    // 转换为int16_t PWM值
    return clampi16((int16_t)lroundf(out), PWM_MIN, PWM_MAX);
}

四、PWM输出

PWM（Pulse Width Modulation，脉宽调制）本质上是一种用开关模拟连续调节的方法。它输出的不是平滑的模拟电压，而是周期性的高低电平信号，电机因惯性和电感的特性，最终表现出的是一个平均效果。

PWM 最重要的两个概念：
频率（Frequency）： 表示 PWM 高低电平重复的速度，如 20kHz 表示 1 秒重复 20000 次。
**占空比（Duty Cycle）：**决定高电平占一个周期的比例，占空比越大，平均电压越高，电机转速越快。

具体的PWM介绍可以参考此篇博客：

STM32定时器进阶：输出比较与PWM信号生成，从原理到实例一篇就够---STM32 HAL库专栏_stm32 锯齿波与比较器 pwm-CSDN博客

本项目中，电机执行器模块负责将控制模块的 "逻辑 PWM 指令" 转换为真实硬件可执行的控制信号，并提供轮速反馈能力，对外提供 4 个核心接口：

Motor_Init()：初始化电机执行器，包括启动 PWM 输出、初始化方向控制，并将输出清零（停机）。

Motor_ApplyPwm(pwmL, pwmR)：接收左右轮的逻辑 PWM 指令（范围 - 1000~1000），完成限幅、方向控制和 PWM 输出，是电机真正 "执行动作" 的入口。

MotorSim_Tick(pwmL, pwmR, dt_s)：用于仿真轮速反馈，根据 PWM 指令模拟电机动态响应，生成测量速度。

Motor_GetWheelSpeedMmps(vL, vR)：返回当前测得的左右轮速度（单位 mm/s），当前版本默认来自仿真模型，后续可替换为真实编码器测量值。

电机方向控制

电机方向控制采用典型的两线制驱动方式，对应 TB6612、L298 等常用电机驱动方案，本项目映射关系如下：

左轮：PB4 = IN1，PB5 = IN2

右轮：PB6 = IN1，PB7 = IN2

方向逻辑由motor_apply_dir_2pin()完成：

pwm > 0：正转，IN1=1, IN2=0
pwm < 0：反转，IN1=0, IN2=1
pwm = 0：停机

五、整体控制流程

在每一个控制周期内，系统会按如下顺序完成 PID 与 PWM 控制流程,如下图所示：

这一过程不断重复，形成稳定的速度闭环控制。

---

总结

本文完整实现了机器人底盘控制的核心执行闭环 ：通过运动学逆解 将上层速度指令转换为左右轮目标轮速，通过PID 控制器 实现精准的速度闭环调节，再通过PWM 输出驱动电机完成最终执行。