基于FPGA的闭环电机控制系统设计：从VHDL实现到机器人运动控制

1. 项目概述：一个光敏机器人的“运动中枢”

在机器人项目中，让机器人“动起来”并“动得准”往往是第一个硬骨头。几年前，我和团队接手了一个光敏追踪机器人的项目，核心任务就是设计它的运动控制系统。这不仅仅是让两个轮子转起来那么简单，我们需要它能够根据摄像头“看到”的光源位置和大小，智能地调整左右电机的速度和方向，平稳、精准地奔向目标。最终，我们选择在FPGA上，用VHDL语言构建了一套完整的闭环电机控制逻辑。今天，我就把这个从算法设计到硬件实现的完整过程拆解开来，无论是正在学习数字逻辑、嵌入式系统，还是对机器人运动控制感兴趣的朋友，都能从中找到可复用的思路和踩坑经验。

简单来说，这个系统就是一个基于FPGA的电机速度与方向闭环控制器。它的输入是摄像头处理后的光源信息（位置和大小），输出是驱动两个直流电机的PWM信号。核心在于引入了速度反馈，构成闭环，让机器人的运动更稳定，抗干扰能力更强，不会因为地面摩擦不均或电池电压波动而跑偏。整个设计完全用VHDL描述，在Xilinx的Basys 3开发板上实现，最终驱动了我们的光敏机器人。

2. 核心控制策略：从“看到光”到“决定怎么跑”

在写第一行代码之前，我们必须把机器人的行为逻辑用清晰的数学关系定义下来。这就像是给机器人编写“交通规则”。

2.1 运动行为建模：双摇杆算法

我们借鉴了遥控车（RC）的双摇杆控制逻辑来设计算法，非常直观：

• 左摇杆（控制转向）：对应光源的水平位置。假设图像中心为0，左侧为负，右侧为正。
  • 光源在正前方（位置≈0）：左右电机目标速度相同，机器人直行。
  • 光源在左侧（位置为负）：右电机目标速度 > 左电机目标速度，产生右轮快、左轮慢的差速，机器人向左转。
  • 光源在右侧（位置为正）：左电机目标速度 > 右电机目标速度，机器人向右转。
• 右摇杆（控制进退与速度）：对应光源的成像大小。
  • 光源小（距离远）：目标速度为正（前进），且距离越远，速度绝对值越大，让机器人快速接近。
  • 光源大（距离近）：目标速度为负（后退），防止机器人撞上光源，距离越近，后退速度越快。

这样，我们就将两个抽象的视觉信息（位置pos、大小size）映射为了左右电机具体的目标速度V_left_goal和V_right_goal。我们设定速度范围为-255 到 +255（对应8位PWM占空比的绝对值，符号代表方向）。

一个简化的计算公式示例如下：

// 假设 pos_normalized 是归一化后的位置值 [-1, 1]，size_normalized 是归一化后的大小值 [0, 1] base_speed = (0.5 - size_normalized) * 510; // 大小控制基础速度，中间值对应停止 turn_factor = pos_normalized * 100; // 位置控制转向因子 V_left_goal = base_speed - turn_factor; // 左侧电机目标速度 V_right_goal = base_speed + turn_factor; // 右侧电机目标速度 // 最后需要将浮点数结果钳位到 [-255, 255] 的整数范围内

注意：这里的公式是原理性示意。在实际VHDL中，我们需要用定点数运算来处理，避免使用浮点单元，以节省FPGA资源。例如，可以将所有数值放大2^N倍，用整数进行乘除运算。

2.2 控制模块（Ctrl.vhd）的实现要点

这个模块是决策核心，输入是位置、大小和一个数据有效的READY信号，输出是两个9位有符号数（代表目标速度）。

• 数据有效性检查：必须使用READY信号。摄像头初始化或处理异常时，可能输出无效数据。只有READY为高时，当前的位置和大小数据才被采信，否则控制模块应输出零速度或保持上一状态，防止机器人乱跑。
• 参数化设计：将base_speed和turn_factor的计算系数（如上例中的510和100）设计为generic参数或可通过寄存器配置。这样可以在实际调试中不修改代码，仅通过调整参数就能改变机器人的“性格”（是激进还是温和）。
• 输出寄存：计算出的目标速度值必须用寄存器输出，确保每个时钟周期输出稳定，避免因组合逻辑毛刺导致电机抖动。

3. 闭环反馈：让控制更稳的“纠偏师”

只有目标速度是开环控制，机器人实际跑起来会因各种原因偏离预期。我们需要引入反馈，构成闭环。

3.1 为什么要闭环？——从开环的痛点说起

在最初的测试中，我们采用了开环控制（即直接根据目标速度输出PWM）。立刻遇到了问题：

1. 电池电压衰减：随着电池消耗，同一PWM占空比对应的电机实际转速会下降。
2. 负载变化：在不同地面上（如地毯 vs. 木地板），摩擦阻力不同，转速也不同。
3. 电机个体差异：两个“相同”的电机，其转速-占空比特性曲线也有细微差别。

结果就是，机器人无法走直线，对光源的追踪轨迹也飘忽不定。闭环控制的核心思想就是“测量-比较-纠正”。

3.2 误差计算模块（error.vhd）的设计

这个模块是闭环的核心。它接收目标速度和实测速度，输出控制增量。

• 输入/输出数据格式：目标速度和实测速度都需要转换为有符号数，以便处理正转和反转。我们选择9位有符号数（范围-256~255），其中最高位（MSB）为符号位。
• 核心算法：比例（P）控制最简单的误差计算就是比例调节：控制增量 = Kp * (目标速度 - 实测速度)。
  • Kp是比例系数，是一个需要调试的关键参数。
  • (目标速度 - 实测速度)就是速度误差。误差为正，说明实际速度太慢，需要增加PWM；误差为负，说明实际速度太快，需要减小PWM。
  • 最终输出给电机的PWM值 = 前一次PWM值 + 控制增量。这需要一个积分或累加的过程，通常在后续的PWM生成模块中实现。
• 防止积分饱和与限幅：在误差模块或后续模块中，必须对计算出的控制增量进行限幅，防止因误差长期存在导致输出值溢出到非工作区。同时，当电机达到目标速度附近时，微小的误差波动不应引起输出变化，可以设置一个误差死区（Dead Zone）。

3.3 实测速度的获取：编码器与测量模块

这是闭环的“测量”环节。我们为电机安装了增量式光电编码器（每转输出几百个脉冲）。需要设计一个速度测量模块：

• 原理：在固定时间窗口（例如10ms）内，对编码器脉冲进行计数。
• 方向判断：编码器通常输出A、B两相正交脉冲。通过判断A相和B相的相位差（90度），可以识别电机正转还是反转。
• 输出格式：该模块输出两个值：direction（1位，0正/1反）和speed（8位，0-255代表转速绝对值）。这就是反馈给误差计算模块的“实测速度”，但需要先转换成有符号数。

4. 数据格式的桥梁：有符号与无符号转换

在数字系统中，不同模块间数据格式不匹配是常见问题。我们的系统里就存在两处：

1. 反馈路径：速度测量模块输出的是无符号的方向和速度，而误差计算模块需要有符号的实测速度值。
2. 前向路径：误差计算后得到的有符号控制量，最终需要转换为无符号的PWM占空比和方向信号，才能驱动电机驱动板（如常见的H桥驱动芯片）。

4.1 无符号转有符号模块（US2S.vhd）

这个模块将来自测量模块的dir(1bit) 和spd(8bit) 合并为一个9位有符号数。

-- 伪代码逻辑 if dir = '0' then -- 正转 signed_speed <= ('0' & spd); -- 最高位补0，表示正数 else -- 反转 signed_speed <= std_logic_vector(-resize(unsigned(spd), 9)); -- 取补码表示负数 end if;

实操心得：这里要特别注意VHDL中的数据类型转换和符号扩展。直接拼接dir和spd得到的是9位无符号数，并非有符号数的补码表示。对于负数，必须手动计算其补码。使用signed和unsigned类型并配合resize函数可以更安全地完成操作。

4.2 有符号转无符号模块（S2US.vhd）

这个模块执行相反的操作，将误差计算后最终确定的9位有符号速度值，拆解为电机驱动能理解的direction信号和8位pwm_duty占空比。

-- 伪代码逻辑 if input_signed_speed(8) = '1' then -- 最高位为1，表示负数 direction <= '1'; -- 反转 pwm_duty <= std_logic_vector(unsigned(-input_signed_speed(7 downto 0))); -- 取绝对值（补码取反加一） else direction <= '0'; -- 正转 pwm_duty <= std_logic_vector(input_signed_speed(7 downto 0)); -- 直接取低8位 end if;

关键点：PWM占空比必须是无符号的正数（0-255）。因此，对于负的速度值，我们需要先取其绝对值，再将方向信号置为反转。这个“取绝对值”操作在硬件上就是一次条件取补码的电路。

5. 无光状态处理：机器人的“待机策略”

一个健壮的机器人不能只在有光时工作。当摄像头视野内没有检测到任何光源时，控制模块收到的size可能为0，且READY信号可能为低。此时，机器人应该如何行为？

5.1 无光计数器模块（nolightcounter.vhd）的逻辑

我们设计了一个状态机来控制无光时的行为：

1. 状态A：旋转寻找。一旦进入无光状态，启动一个计数器（例如，计时3秒）。在这3秒内，控制模块忽略摄像头输入，强制输出一组使机器人原地旋转（左轮正转、右轮反转，且速度较低）的目标速度。
2. 状态B：静止休眠。3秒旋转后仍未发现光，则进入休眠状态（输出零速度），并启动另一个更长的计数器（例如，计时10秒），以节省电力。
3. 状态C：循环。10秒休眠结束后，自动跳转回状态A，再次开始旋转寻找。一旦在任何状态检测到READY信号有效（发现光），立即跳出此状态机，回归正常的光追踪模式。

5.2 实现细节与防误触发

• 消抖与确认：不能因为一两帧图像丢光就立刻进入无光模式。需要实现一个“无光确认计数器”，例如连续20个时钟周期检测不到光，才判定为真正的无光状态，从而触发状态机。这能有效避免因摄像头短暂遮挡或光线闪烁导致的模式误切换。
• 参数可调：旋转时间和休眠时间应设计为可通过寄存器配置，方便在不同环境下调试（如空旷房间 vs. 复杂障碍环境）。

6. 系统集成与顶层模块设计

各个子模块完成后，需要在一个顶层模块（Top.vhd）中将它们像搭积木一样连接起来。

6.1 组件例化与信号连接

这是典型的VHDL结构化描述。你需要：

1. 声明所有子模块（Ctrl, error, US2S, S2US, nolightcounter等）为component。
2. 定义内部连接信号（signal），例如goal_speed_left,measured_speed_signed_left,error_left等。
3. 使用port map将子模块的端口与顶层模块的端口或内部信号一一对应连接。

连接关系图在脑海中（或纸上）应如下所示：

摄像头数据 (pos, size, ready) | v [Control Module] ---> goal_speed (有符号) | | | v | [Error Calculation] <--- measured_speed_signed (来自US2S) | | | v | control_output (有符号) | | v v [Nolight Counter] [S2US Converter] | | | v | (pwm_duty, direction) --> 电机驱动 | (覆盖选择信号)---------------------+

注意：无光计数器的输出（强制目标速度）需要通过一个多路选择器（MUX）与正常控制模块的输出进行选择。选择信号由无光计数器模块的状态产生。

6.2 时钟、复位与全局考虑

• 时钟域：确保所有模块使用同一个时钟和复位信号。对于Basys 3，通常使用100MHz的系统时钟。如果编码器计数频率很高，可能需要先进行同步处理。
• 复位策略：设计一个全局复位信号，能将所有寄存器、状态机和计数器初始化为确定状态。上电后，机器人应处于静止的待机状态。
• 流水线寄存器：在模块间关键路径上插入寄存器，可以提高系统最大运行频率。例如，在Control模块输出后、Error模块输入前加一级寄存器。

7. 仿真测试：在“上车”前验证逻辑

直接烧录到FPGA调试是痛苦的。必须先用仿真（Testbench）验证逻辑正确性。

7.1 模块级测试（Unit Test）

为每个关键模块编写独立的测试平台。

• 测试Control模块：模拟输入不同的pos和size组合，检查输出的goal_speed是否符合算法预期。特别测试边界情况（如pos极值、size为0）。
• 测试Error模块：输入一系列目标速度和实测速度，检查输出的误差控制增量是否与预设的Kp系数相符。
• 测试转换模块：输入有符号数和无符号数，检查转换结果是否正确，特别是正负边界（如+255, -255, -1）的转换。

7.2 系统级集成测试（Integration Test）

编写一个模拟真实场景的测试平台：

1. 模拟摄像头数据流：在Testbench中生成一个随时间变化的pos和size序列，模拟机器人从远处发现光源、逐渐靠近、光源移动的过程。
2. 模拟编码器反馈：这是一个难点。可以建立一个简单的“电机模型”：根据当前输出的PWM值，在一个延迟后，产生一个大致对应的measured_speed反馈。这能初步验证闭环是否起效。
3. 观察波形：在Vivado/ISE的仿真器中，观察关键信号（如目标速度、实测速度、误差、最终PWM输出）的波形。你应该能看到，当模拟的实测速度低于目标时，误差为正，PWM输出增加，反之亦然。

常见仿真问题：

• 信号未初始化：导致仿真开始时为U（未定义），整个逻辑链失效。务必在Testbench和设计代码中为所有信号赋予初始值。
• 时序不匹配：组合逻辑存在毛刺，或者寄存器时序不满足。在仿真中注意观察时钟边沿和数据稳定的关系。

8. 硬件实现与调试：从比特流到转动轮子

仿真通过后，就可以进行硬件部署了。

8.1 约束文件（.xdc）编写

这是将设计中的逻辑端口映射到FPGA物理引脚的关键一步。对于Basys 3：

• 时钟输入：查找手册，将系统时钟端口（如clk）约束到板载的100MHz时钟引脚（例如set_property PACKAGE_PIN W5 [get_ports clk]）。
• 电机控制输出：PWM信号和方向信号需要连接到驱动电机的PMOD接口。例如，使用PMOD HB5驱动板，你需要将4个输出（左电机PWM、方向，右电机PWM、方向）约束到PMOD JA或JB的特定引脚上。
• 编码器输入：将编码器的A、B相信号约束到另外的GPIO引脚，并设置正确的I/O标准（通常是LVCMOS33）。

8.2 电源与驱动：至关重要的硬件细节

这是我们踩过最大坑的地方，务必高度重视：

• 电机驱动板选型：Basys 3的FPGA引脚只能提供有限的电流（通常几mA），绝对不能直接驱动电机！必须使用电机驱动板（如PMOD HB5、TB6612FNG、L298N模块）。我们选用的是PMOD HB5。
• 电压与电流：仔细阅读电机和驱动板的数据手册！
  • 电机额定电压：我们的直流电机额定电压是12V。如果只给5V，它可能根本转不动或者力矩很小。
  • 驱动板供电电压：PMOD HB5的逻辑部分（接FPGA）需要3.3V或5V，但其电机驱动部分（Vmot）必须接入12V（与电机额定电压匹配）。这个12V需要独立电源提供，不能从Basys 3上取！
  • 电流要求：电机堵转时电流很大。确保你的电源（特别是12V电源）能提供足够的峰值电流。同时，驱动芯片（如HB5用的DRV8833）有最大电流限制，也要确保不被超过。
• 共地与隔离：FPGA的GND、驱动板逻辑地、电机电源地需要连接在一起，形成一个共同的参考地。但大功率的电机电源可能会引入噪声，在布线时要注意。

8.3 上电调试步骤

1. 静态测试：先不接电机，烧录程序后，用示波器或逻辑分析仪测量PWM输出引脚。通过改变模拟输入（可以暂时用拨码开关模拟pos和size），观察PWM波形和占空比是否变化正常。
2. 半动态测试：接上电机，但用手捏住轮子。观察在给出不同目标速度时，电机是否试图用力转动，并且方向是否正确。听电机的声音，正常的PWM驱动是清脆的“嘶嘶”声，如果出现沉闷的“嗡嗡”声，可能是电源不足或驱动有问题。
3. 闭环调试：让轮子空转，用手轻轻阻碍轮子，模拟负载增加。你应该能看到系统试图增加PWM以维持速度（如果编码器反馈正常工作）。调整误差计算模块中的Kp参数：Kp太小，响应慢，纠偏无力；Kp太大，容易引起振荡（轮子转速在目标值附近来回摆动）。这是一个反复调试的过程。

9. 系统联调与性能优化

当电机控制部分单独工作正常后，将其与摄像头处理部分、电源管理部分集成，构成完整的机器人。

9.1 多模块集成与时钟同步

• 数据接口同步：摄像头模块（可能在另一个FPGA或处理器中）输出的pos,size,ready信号，需要同步到电机控制模块的时钟域，避免亚稳态。通常使用两级寄存器进行同步。
• 控制频率匹配：摄像头处理帧率（如30fps）与电机控制频率（PWM频率通常几千Hz到几十kHz，控制环路频率几百Hz）可能不同。需要设计适当的缓冲或采样机制，确保电机控制使用最新且稳定的视觉数据。

9.2 从P控制到PI控制

基本的比例（P）控制可能存在静差（即最终稳定速度与目标速度有微小偏差）。为了消除静差，可以引入积分（I）环节，升级为PI控制器。

• 积分器实现：在误差计算模块中，增加一个寄存器来累积误差：integral <= integral + error。
• 积分输出：控制增量 = Kp * error + Ki * integral。
• 抗积分饱和：必须对积分器integral设置上下限，防止长时间误差累积导致输出饱和。当控制输出达到限幅值时，应停止积分。

9.3 资源优化与性能评估

• 资源查看：在Vivado综合实现后，查看资源利用率报告（LUT、FF、DSP等）。确保设计在目标FPGA（Basys 3的Artix-7）的资源范围内。
• 时序收敛：查看时序报告，确保建立时间和保持时间满足要求。如果存在时序违例，可以考虑降低时钟频率、对长路径进行流水线切割或优化关键逻辑。
• 功耗评估：简单的设计功耗通常不高，但如果你使用了大量乘法器和高速时钟，也需要关注一下。

完成所有调试后，将完整的比特流文件烧录到机器人的FPGA中。当机器人成功启动，并稳健地追着光点跑起来时，你会觉得所有那些深夜调试波形、测量电压电流的付出都是值得的。这个基于VHDL的闭环电机控制系统，不仅是一个课程项目，更是一个理解数字控制、硬件描述语言和机电一体化的绝佳范例。它教会你的，远不止那几行代码。