嵌入式全向机器人混合控制：模糊自适应PI与LQR的工程实践

1. 项目概述：嵌入式系统如何驯服“全向漂移”的机器人

在机器人竞技场或是繁忙的仓储物流线上，你可能会看到一种机器人，它们不像汽车那样需要先“摆正车头”才能转向，而是可以像冰面上的圆盘一样，瞬间向任意方向滑行，同时自身还能旋转。这就是全向移动机器人，它的核心秘密藏在四个特殊设计的“麦克纳姆轮”或“全向轮”里。然而，这种近乎“违反直觉”的自由移动能力，也给它的“大脑”——运动控制系统——带来了巨大的挑战：如何精确、稳定且高效地协调四个轮子的转速，让这个多自由度的家伙乖乖沿着我们设定的复杂路径前进，而不是原地打转或“漂移”出界？

这正是我们这次要深入探讨的核心。传统的PID控制器在面对全向机器人这种强耦合、非线性的系统时，往往力不从心，尤其是在高速、变向的运动中，参数难以自适应调整，容易产生超调或振荡。而单纯依赖复杂的现代控制理论（如LQR），又对嵌入式平台的实时计算能力提出了苛刻要求。本文分享的，正是我们在RoboCup小型组足球机器人平台上，将模糊自适应PI控制器与线性二次型调节器（LQR）结合，并在嵌入式系统中实现的一套混合控制方案。这套方案的精髓在于：让“模糊逻辑”这位经验丰富的“老司机”在线动态调整PI控制器的参数，以应对外部扰动和模型不确定性；同时，让LQR这位“最优规划师”在底层为四个电机计算出最节能、最平滑的控制电压。最终目标，是让机器人在追求极限速度的比赛中，依然能紧紧“咬住”预设的轨迹，误差控制在厘米级以内。

如果你正在为移动机器人的“指哪不打哪”而头疼，或者对如何将先进控制算法落地到资源受限的嵌入式芯片上充满兴趣，那么接下来的内容，或许能给你带来一些切实可行的思路和避坑指南。

2. 系统整体架构与设计思路拆解

在动手写代码、调参数之前，我们必须先像建筑师一样，勾勒出整个控制系统的蓝图。对于全向移动机器人，尤其是参与RoboCup这种高速、对抗性比赛的机器人，其系统设计必须兼顾实时性、可靠性、可扩展性和低功耗。

2.1 硬件架构：从芯片到车轮的神经脉络

一套典型的全向移动机器人嵌入式硬件系统，可以看作一个精简的“自动驾驶”单元。其核心架构通常遵循“感知-决策-执行”的闭环。

1. 中央处理单元（CPU/MCU）：这是系统的大脑。在早期的机器人或对成本极其敏感的应用中，可能会选用高性能的微控制器（MCU），如STM32F4/F7系列或ESP32，它们集成度高、功耗低，且具备丰富的定时器和通信外设，非常适合产生多路PWM信号驱动电机。而在对计算能力要求更高的场景（如需要运行视觉SLAM），则会采用微处理器（MPU），如树莓派CM4、NVIDIA Jetson Nano等，它们能运行完整的Linux系统，便于部署复杂的算法。在我们的项目中，为了平衡实时控制与上层路径规划，采用了主从处理器架构：一个MPU（如RK3568）运行Linux，处理视觉数据、决策和模糊-PI高层规划；一个高性能MCU（如STM32H7）专用于实时电机控制和解算LQR算法。

2. 执行机构：电机与驱动

• 电机选型：无刷直流电机（BLDC）因其高功率密度、高效率和高转速，成为竞技机器人的首选。关键参数包括：KV值（转速/电压）、额定扭矩、峰值电流。需要根据机器人的总重、目标加速度和轮径进行详细计算。一个经验公式是：单个电机峰值扭矩 ≈ (机器人总质量 × 目标加速度 × 轮半径) / (轮子数量 × 传动效率)。算出来的扭矩再结合目标轮速，才能选定合适的电机。
• 电机驱动：通常选用集成MOSFET和驱动逻辑的BLDC驱动芯片，如DRV8323、TMC4671等。它们通过接收MCU产生的PWM信号和方向信号，输出三相电流驱动电机。特别注意：驱动芯片的电流采样和过流保护功能至关重要，能防止电机堵转时烧毁电路。

3. 感知反馈：机器人的“眼睛”和“本体感觉”

• 编码器：这是实现精准速度闭环的基石。每个电机轴后端都安装有一个高精度增量式编码器（如1000线/转）。编码器信号通过MCU的定时器编码器接口读取，实时反馈电机的实际转速。这里有个坑：编码器线数并非越高越好，线数越高，在高速时对MCU中断处理能力的挑战越大，可能导致丢步。需要根据电机最高转速和MCU处理能力折中选择。
• 惯性测量单元（IMU）：通常包含三轴陀螺仪和三轴加速度计，用于测量机器人的姿态角（偏航角Yaw）和加速度。虽然我们的高层控制器依赖外部视觉定位，但IMU提供的角速度信息对于底层LQR控制器抑制机器人的旋转抖动、提高动态稳定性有奇效。常用芯片如MPU6050、ICM-20948。
• 通信模块：机器人需要与上位机（团队服务器）进行低延迟、高可靠的无线通信。在RoboCup SSL中，常用2.4GHz或5.8GHz的自定义协议或商用模块（如基于NRF24L01+的增强方案）。关键点：通信协议必须设计有重传和校验机制，且数据包结构要紧凑，通常只传输目标位置、速度、动作指令等关键信息，而非原始图像数据。

4. 电源管理：机器人由锂电池供电（如4S锂聚合物电池）。需要一个高效的DC-DC降压模块为MCU、MPU、传感器等提供稳定的3.3V、5V、12V电压。重要经验：电机驱动电路必须与数字控制电路进行电源隔离或采用星型接地，否则电机启停时产生的大电流瞬变会严重干扰数字电路的稳定性，导致MCU复位或传感器读数异常。

2.2 软件架构：让硬件“活”起来的灵魂

软件架构的设计直接决定了系统的实时性和可维护性。我们摒弃了简单的“超级循环”（Super Loop），采用了基于实时操作系统（RTOS）的分层模块化设计。

1. 操作系统层：在MCU端，我们移植了FreeRTOS或RT-Thread这类轻量级RTOS。它为任务调度、中断管理、进程间通信提供了可靠的基础。我们将关键任务划分为不同优先级： *最高优先级：电机控制中断服务程序（ISR）。以固定频率（如1kHz）执行，读取编码器值，运行LQR控制算法，更新PWM占空比。这个中断的周期性必须严格保证，任何延迟都会直接影响控制性能。 *高优先级：通信处理任务。负责解析来自无线模块的数据包，并将解析后的目标速度、位置指令通过消息队列发送给控制任务。 *中优先级：状态监控任务。周期性读取电池电压、电机温度、IMU数据，并进行故障诊断（如过温、欠压）。 *低优先级：日志记录或调试信息发送任务。

2. 应用层模块：*通信协议栈：实现自定义的二进制应用层协议，包含帧头、机器人ID、指令类型、数据载荷、CRC校验等字段。确保数据包的完整性和正确性。 *运动学解算模块：这是全向移动机器人的“指挥中心”。它接收来自上层的[vx, vy, ω]指令（全局坐标系下的前进速度、横向速度、旋转角速度），并利用运动学逆解公式，实时计算出四个轮子各自的目标转速[ω1, ω2, ω3, ω4]。公式基于机器人的几何结构（轮子安装角度、距离中心的位置）。 *控制器模块：核心算法所在地。包含LQR控制器实现和参数集。它接收运动学解算出的目标轮速和编码器反馈的实际轮速，计算控制量（通常是PWM占空比或电压指令）。 *滤波器模块：在编码器反馈和IMU数据进入控制器前，必须进行滤波以消除噪声。对于速度信号，一阶低通滤波或卡尔曼滤波是常见选择。切记：滤波器会引入相位滞后，参数需要谨慎调整，否则会影响系统稳定性。

3. 上位机（团队服务器）算法：在性能更强的上位机（通常是场外的电脑）上，运行着基于视觉的全局定位、决策AI，以及我们本文的重点——模糊自适应PI路径跟踪控制器。它根据视觉系统反馈的机器人实际位置与目标路径的偏差，动态调整发给机器人的速度指令。

这样的软硬件协同设计，确保了从“想到”到“做到”的链路最短、延迟最低，为后续实现高性能控制打下了坚实基础。

3. 核心控制策略：模糊PI与LQR的混合之道

理解了系统架构，我们进入最核心的部分：控制算法。为什么选择“模糊PI + LQR”这种混合策略？这源于对全向移动机器人控制难点的深刻认识。

3.1 问题根源：全向机器人的控制挑战

全向机器人的运动模型是一个多输入多输出（MIMO）、强耦合的系统。四个轮子的转速相互影响，任何一个轮子的打滑或响应延迟，都会导致整体运动偏离预期。此外，机器人运行的地面摩擦系数可能变化（木地板、地毯、瓷砖），负载也可能动态变化（如抓取物体）。这些外部扰动和模型不确定性，使得传统的固定参数PI控制器表现不佳：

• P（比例）过大：响应快，但容易超调、振荡，在高速接近目标点时可能引发剧烈抖动。
• I（积分）过大：能消除静差，但会降低系统响应速度，并可能产生积分饱和，导致控制滞后。

我们需要一个能“智能”调整PI参数的机制，这就是模糊逻辑控制的用武之地。

3.2 高层控制器：模糊自适应PI路径跟踪器

模糊控制不依赖于精确的数学模型，而是模仿人类专家的经验，用“如果...那么...”的规则进行推理。在我们的路径跟踪器中，模糊控制器的作用是根据当前位置误差e和误差变化率ec，在线实时调整PI控制器的比例系数Kp和积分系数Ki。

1. 模糊化：我们将输入变量e（距离误差）和ec（误差变化率，即误差的微分）的论域（例如，e: [-10cm, 10cm], ec: [-5cm/s, 5cm/s]）划分为若干个模糊集合，如：{负大(NB), 负中(NM), 负小(NS), 零(ZO), 正小(PS), 正中(PM), 正大(PB)}。每个集合对应一个隶属度函数（常用三角形或梯形函数）。例如，“误差为正小(PS)”这个陈述，有一个介于0到1之间的“真实度”。

2. 规则库：这是模糊控制器的“大脑”，存储了专家经验。规则形式为：IF e is A AND ec is B THEN Kp is C, Ki is D。 例如：

• IF e is PB AND ec is ZO THEN Kp is PB, Ki is ZO(误差很大且没变化，需要很强的比例作用快速减小误差，积分作用暂时不需要)
• IF e is PS AND ec is NS THEN Kp is PM, Ki is PS(误差为正小但正在减小，保持中等比例作用，加入小积分作用以消除残余误差)
• IF e is ZO AND ec is ZO THEN Kp is ZO, Ki is ZO(误差很小且稳定，保持当前输出，微调即可)

我们通常需要为Kp和Ki分别制定一个规则表（如7x7=49条规则）。

3. 推理与解模糊化：根据当前输入的精确值e和ec，通过隶属度函数计算出它们属于各模糊集合的隶属度。然后，根据规则库，所有被激活的规则会贡献其结论。最后，通过“重心法”等解模糊化方法，将输出的模糊集合Kp和Ki转化为精确的数值，用于更新PI控制器。

实操心得：模糊规则表的制定是调参的关键，也是最耗时的部分。初期可以基于经典Ziegler-Nichols法整定出的PI参数作为中值，然后根据机器人实际跑动现象（如过冲、收敛慢）来微调规则。一个有效的技巧是：先调Kp的规则，让系统响应快速且无剧烈振荡；再调Ki的规则，用于精细消除静差。

3.3 底层控制器：离散时间LQR速度调节器

如果说模糊PI是决定“应该以多快的速度、多大的劲头去追目标”的指挥官，那么LQR就是确保“四个轮子精准执行转速命令”的工兵。LQR是一种最优控制方法，其目标是找到一个控制律u = -Kx，在满足系统动力学x' = Ax + Bu的前提下，最小化一个二次型性能指标J = ∫(x^T Q x + u^T R u) dt。

1. 状态空间模型建立：对于单个电机的速度控制，我们可以建立一个简化的二阶系统状态空间模型。状态变量x可以选为[θ, ω]^T，即电机转角和角速度。输入u是电机的驱动电压。矩阵A和B可以通过电机机电方程（包含电枢电阻、电感、反电动势常数、扭矩常数、转动惯量等）线性化得到。对于四个电机，我们可以将其视为四个独立的单输入单输出（SISO）系统，或者建立一个耦合的MIMO模型（更精确但更复杂）。在嵌入式平台资源有限的情况下，通常采用解耦的SISO模型，并通过上层运动学来协调。

2. 权重矩阵Q和R的选取：这是LQR设计的艺术所在，直接决定了控制器的“性格”。

• Q矩阵：惩罚状态误差。如果我们更关注速度跟踪的精度，那么对应角速度ω的状态权重就应该设置得大一些。例如，Q = diag([0.1, 10])表示对速度误差的惩罚远大于对位置误差的惩罚。
• R矩阵：惩罚控制输入（电压）的大小。增大R的值，控制器会倾向于使用更“柔和”的控制力，节省能量，但响应可能变慢；减小R，则控制更“激进”，响应快但可能耗能且易饱和。调试技巧：通常采用“Bryson法则”初选：将Q和R设为对角阵，其对角线元素取为对应状态或输入允许最大值的平方的倒数。然后通过仿真和实际测试，围绕这个初值进行微调。一个重要的原则是：先保证稳定性，再优化性能。可以先将R设得较大，确保系统稳定不振荡，再逐步减小R、增大Q中速度项的权重，来提升响应速度。

3. 离散化与在线计算：连续时间的LQR问题需要求解代数Riccati方程得到增益矩阵K。在嵌入式系统上，我们通常使用其离散时间版本。我们需要将连续的A, B矩阵用零阶保持法进行离散化，得到A_d, B_d。然后离线求解离散时间Riccati方程，得到最优反馈增益矩阵K。这个K是常数矩阵，在MCU上只需要进行简单的矩阵乘法运算u = -K * x即可，计算负担很小，非常适合在1kHz的中断中执行。

混合策略的工作流程：

1. 上位机根据全局路径和视觉反馈，计算当前位置与目标点的误差e和ec。
2. 模糊推理器根据e和ec，动态输出一组Kp,Ki。
3. 自适应PI控制器根据新的Kp,Ki和误差，计算出一个理想的速度指令[vx, vy, ω]，并通过无线发送给机器人。
4. 机器人MCU收到速度指令后，通过运动学逆解算出四个轮子的目标转速ω_ref_i。
5. 对于每个轮子，LQR控制器读取编码器反馈的实际��速ω_act_i，状态误差x = [θ_err, ω_err]，计算最优控制电压u_i。
6. 电机驱动根据u_i输出相应占空比的PWM，驱动电机。
7. 编码器持续反馈，形成闭环。

这套混合架构，巧妙地将模糊逻辑的适应性与LQR的最优性结合起来，上层应对大范围、非线性的路径跟踪问题，下层确保局部速度环的快速、平稳、节能。

4. 嵌入式实现与参数整定实战

理论再完美，不能落地也是空中楼阁。接下来，我们深入到嵌入式MCU的代码层面，看看如何将上述算法实现，并分享关键的参数整定和调试经验。

4.1 LQR控制器的C语言实现

在资源受限的MCU上实现LQR，关键在于效率。我们通常离线计算好增益矩阵K，在程序中以常量数组形式存储。

// 假设我们为每个电机设计了一个二阶系统的LQR，状态为[角度误差(rad), 角速度误差(rad/s)] // 离线计算得到的增益矩阵 K = [k1, k2] #define K1 2.5f #define K2 0.8f // 电机控制结构体 typedef struct { float target_speed; // 目标转速 (rad/s) float actual_speed; // 实际转速 (来自编码器滤波后) float speed_error; // 速度误差 float angle_error; // 角度误差（由速度误差积分得到，注意抗饱和） float control_voltage; // 计算出的控制电压 (V) float max_voltage; // 电机最大允许电压 (V) } MotorController_t; void LQR_Update(MotorController_t *motor) { // 1. 更新状态：这里角度误差用速度误差的积分来近似表示位置跟踪 // 注意：在实际中，我们可能更关注速度跟踪，所以角度误差的权重可能很低或为零 motor->angle_error += motor->speed_error * CONTROL_PERIOD; // CONTROL_PERIOD为控制周期，如0.001s // 积分抗饱和处理 if(motor->angle_error > INTEGRAL_LIMIT) motor->angle_error = INTEGRAL_LIMIT; if(motor->angle_error < -INTEGRAL_LIMIT) motor->angle_error = -INTEGRAL_LIMIT; // 2. LQR控制律计算: u = -K * x = - (K1 * angle_error + K2 * speed_error) float voltage = -(K1 * motor->angle_error + K2 * motor->speed_error); // 3. 输出限幅，防止电压指令超过驱动器范围 if(voltage > motor->max_voltage) voltage = motor->max_voltage; if(voltage < -motor->max_voltage) voltage = -motor->max_voltage; motor->control_voltage = voltage; // 4. 将电压转换为PWM占空比 (假设驱动器支持电压指令模式) // 如果驱动器是直接PWM控制，则需要一个电压-占空比标定关系: duty = (voltage / Battery_Voltage) * PWM_MAX uint16_t pwm_duty = (uint16_t)((voltage / BATTERY_NOMINAL_VOLTAGE) * PWM_MAX_VALUE); Set_PWM_Duty(motor->channel, pwm_duty); }

关键细节：

• 控制周期：CONTROL_PERIOD必须严格恒定。最好使用MCU硬件定时器触发中断来调用此函数。
• 积分抗饱和：这是防止“windup”现象的关键。当误差持续很大时（如电机被卡住），积分项会累积到巨大值，一旦障碍解除，系统会因巨大的积分量而剧烈超调。设置INTEGRAL_LIMIT能有效避免。
• 电压-PWM映射：需要根据电机驱动器的特性进行标定。有些驱动器是直接接收电压指令（通过DAC或通信），有些则需要用PWM占空比来模拟电压。后者需要考虑电池电压的实时变化，进行动态补偿。

4.2 模糊PI参数自整定模块实现

模糊推理在上位机（性能更强的计算机）上实现更为方便。我们可以使用Python的scikit-fuzzy库或MATLAB Fuzzy Logic Toolbox快速原型化。但在最终部署时，为了降低通信延迟和依赖，也可以将模糊推理器简化为一个查询表，烧录到MCU中。

步骤：

1. 离线生成查询表：在PC上，使用模糊逻辑设计工具，针对输入e和ec的整个论域，以一定的分辨率（如e和ec各取21个点），计算出对应的Kp和Ki输出值，形成两张二维表格。
2. 嵌入式查表实现：

// 模糊规则查询表 (简化示例，实际分辨率更高) const float Kp_Table[7][7] = { /* ... 根据规则库计算出的数值 ... */ }; const float Ki_Table[7][7] = { /* ... 根据规则库计算出的数值 ... */ }; // 输入e和ec的量化等级 (假设已归一化到索引范围0-6) int e_index = quantize(e, e_min, e_max, 7); int ec_index = quantize(ec, ec_min, ec_max, 7); // 查表获取当前Kp, Ki float current_Kp = Kp_Table[e_index][ec_index]; float current_Ki = Ki_Table[e_index][ec_index]; // 更新PI控制器参数 PI_SetParams(¤t_pi_controller, current_Kp, current_Ki);

quantize函数负责将连续的误差值映射到离散的表格索引。这种方法牺牲了一点精度，但换来了极高的执行速度，非常适合嵌入式实时控制。

4.3 参数整定流程与“黑箱”调试法

调参是机器人控制中最具“玄学”色彩，但也最体现工程师经验的环节。对于“模糊PI+LQR”这套混合系统，建议采用从内到外、先仿真后实测、先独立后联合的调试流程。

第一阶段：底层单电机速度环LQR调试（脱离机器人本体）

1. 开环测试：给电机一个固定的PWM，测量其稳态转速。改变PWM，绘制PWM-转速曲线，检查电机和驱动器的基本工作是否正常，线性度如何。
2. 模型辨识（可选但推荐）：对电机施加阶跃电压信号，记录其转速响应曲线。利用系统辨识工具（如MATLAB的System Identification Toolbox）估算出电机的传递函数，从而得到状态空间模型的A, B矩阵初值。这比纯粹猜想要科学得多。
3. 仿真确定LQR的Q, R：在MATLAB/Simulink或Python中，使用辨识出的模型，设计LQR控制器。通过调整Q和R，在仿真中观察阶跃响应的超调量、调节时间、控制输入大小。目标是得到一个快速无超调（或极小超调）的响应。
4. 实际闭环测试：将仿真得到的LQR增益K应用到真实的电机上。给定一个目标速度阶跃信号，观察实际响应。重点观察：
   • 是否振荡？振荡说明增益太高，需要增大R（惩罚控制量）或减小Q中速度项的权重。
   • 响应是否太慢？响应慢说明增益太低，需要减小R或增大Q。
   • 稳态是否有误差？LQR本身不包含积分环节，对恒定扰动（如摩擦）可能存在静差。这就是为什么我们在状态中引入了“角度误差”（即速度的积分），虽然它物理意义不完全是位置，但起到了类似积分的作用。如果静差明显，可以尝试稍微增大Q中“角度误差”项的权重。

第二阶段：上层模糊PI路径跟踪调试（在机器人平台上）

1. 固定PI参数调试：先暂时关闭模糊自适应，为PI控制器设置一组固定的、保守的Kp和Ki（Ki可以暂时为0）。让机器人跟踪一条简单的直线路径。
2. 观察现象，制定模糊规则：
   • 现象：机器人总是冲过目标点，然后来回振荡。
   • 分析：接近目标时，误差e变小，但误差变化率ec（负值）的绝对值还很大，说明减速太晚。
   • 规则制定：当e为PS（正小）且ec为NB（负大）时，说明正在快速接近目标但即将越过。此时应该大幅减小Kp（甚至给一个很小的值），并给一点Ki来最终消除静差。对应的输出Kp可以设为NS（负小）或ZO（零）。
3. 逐步复杂化路径：从直线到正方形，再到“8”字形或圆形。每更换一种路径，观察在拐点、曲线处的跟踪表现，相应地调整模糊规则表中对应区域的输出值。
4. 开启模糊自适应：将调试好的模糊查询表激活。对比开启前后，机器人跟踪复杂轨迹的平滑度和精度是否有显著提升。

黑箱调试经验：

• 数据记录是关键：一定要让机器人实时通过无线串口将关键数据（如目标速度、实际速度、控制电压、Kp、Ki变化值）发送到电脑上，用绘图软件（如Python的Matplotlib）实时显示。眼睛看机器人的运动是模糊的，但看数据曲线是精确的。
• 先调“粗壮”，再调“精细”：先保证机器人在各种指令下不翻车、不打转（系统稳定），再慢慢优化跟踪精度。
• 理解每个参数的实际物理意义：Kp大了机器人会“急躁”，Ki大了会“迟钝”，LQR中的Q/R比值决定了“能耗”和“性能”的权衡。调参时，要能预测改变某个参数后，机器人大概会有什么样的行为变化。

5. 实验验证、问题排查与性能优化

任何控制系统都需要经过实验的检验。我们搭建了一个标准的RoboCup SSL小型足球场测试环境，使用Overhead视觉系统进行全局定位，来验证我们混合控制器的性能。

5.1 实验设置与对比基准

我们设计了以下对比实验：

1. 控制器A：传统固定参数PI控制器（作为性能基准）。
2. 控制器B：仅使用LQR作为速度环控制器，上层使用固定参数PI做路径跟踪。
3. 控制器C（本文方案）：模糊自适应PI + LQR。

测试轨迹：

• 轨迹1（直线冲刺与急停）：测试系统的阶跃响应和制动性能。
• 轨迹2（正方形）：测试在90度拐角处的跟踪精度和超调。
• 轨迹3（圆形）：测试连续曲线跟踪的平滑度和稳态误差。
• 轨迹4（带扰动的“8”字形）：在路径上随机放置轻微障碍（如薄海绵垫），模拟地面摩擦突变，测试系统的鲁棒性。

评价指标：

• 最大跟踪误差（Max Tracking Error）：整个过程中，机器入中心点与目标路径之间的最大欧氏距离。
• 均方根误差（RMSE）：衡量整体跟踪精度。
• 到达时间（Time to Target）：完成特定轨迹所需的时间。
• 能量消耗（Energy Consumption）：通过电池管理系统记录的总功耗。

5.2 典型问题排查实录

在实际调试中，我们遇到了几乎所有机器人工程师都会踩的坑：

问题1：电机发出“滋滋”声，机器人剧烈高频抖动。

• 可能原因1（最常见）：控制频率过高或过低，与机械系统固有频率耦合产生共振。电机驱动PWM频率通常在10kHz-20kHz，而速度环控制频率建议在500Hz-2kHz。我们最初设为5kHz，结果与机械结构共振。
• 解决方案：将控制频率降至1kHz，抖动立刻消失。教训：控制频率不是越高越好，需要与系统时间常数匹配。
• 可能原因2：LQR或PI的微分增益（或等效的K2）过大，引入了高频噪声放大。编码器信号中的毛刺被过度响应。
• 解决方案：加强对编码器速度值的滤波（如一阶低通滤波，截止频率设为控制带宽的5-10倍），同时适当降低LQR中对应速度误差的反馈增益。

问题2：机器人走直线时，总是缓慢地向一侧偏航。

• 可能原因1：四个轮子的机械安装不完全对称，或轮胎磨损程度不同，导致即使给相同的PWM，实际转速也不同。
• 解决方案：引入轮径校准和摩擦补偿。让机器人原地旋转，记录每个轮子编码器计数，反算实际轮径比例系数。在运动学正/逆解中，乘以这个校准矩阵。
• 可能原因2：IMU的偏航角零漂。虽然我们主要用视觉定位，但底层LQR可能使用了IMU的角速度进行阻尼控制。IMU的零漂会导致一个持续的微小角速度误差积分。
• 解决方案：上电后，让机器人静止2秒，采集IMU数据取平均作为零偏，在后续读数中减去。或使用陀螺仪和加速度计融合的姿态解算算法（如Mahony滤波）来抑制漂移。

问题3：在高速下进行直角转弯时，机器人外侧轮子打滑，轨迹变成“圆角”。

• 可能原因：上层路径规划器给出的速度指令变化过快，超过了电机和轮胎的物理极限。虽然底层LQR能很好地跟踪速度指令，但指令本身就不合理。
• 解决方案：在运动学解算模块之前，加入速度和加速度限幅器。根据电机最大转速、扭矩以及地面摩擦系数，计算出机器人本体系下的最大线加速度和角加速度。对上层发出的[vx, vy, ω]指令进行梯度限制，使其变化率不超过物理极限。这是实现“平滑”运动的关键一步，论文中也提到了这一点。

问题4：无线通信偶尔丢包，导致机器人“卡顿”或“抽搐”。

• 可能原因：通信中断导致目标指令丢失，控制器使用旧值或零值，产生突变。
• 解决方案：
  1. 协议层面：增加数据包序列号和简单的应答重传机制（在实时控制中，重传过期数据不如丢弃）。
  2. 控制层面：在嵌入式端实现一个“看门狗”或“指令保持”功能。如果超过一定时间（如50ms）未收到新指令，则让机器人进入缓停模式（将目标速度平滑地衰减至零），而不是急停或保持旧指令。

5.3 性能优化技巧

在系统稳定工作后，还可以从以下几个方面进行深度优化：

1. 内存与计算优化：

   • 使用查表法代替在线模糊计算：如前所述，这是嵌入式实现的黄金法则。
   • 定点数运算：如果MCU没有硬件FPU（浮点运算单元），将所有浮点运算转换为定点数运算（如Q格式）可以极大提升速度。对于STM32F4等有FPU的芯片，则不必。
   • 优化矩阵运算：LQR中的矩阵乘法通常是小型矩阵（2x2, 4x4），可以手动展开循环，避免调用通用的矩阵库。

2. 通信优化：

   • 压缩数据包：将float类型的数据（如位置、速度）转换为int16_t进行传输，在上位机和下位机约定好缩放比例。
   • 预测算法：在上位机，可以根据当前运动状态，预测未来几毫秒的位置，并发送一组“预测指令包”。下位机在短暂通信中断时，可以使用预测值进行插值，保持运动平滑。

3. 自适应与学习：

   • 离线参数自整定：录制多组机器人运行不同轨迹的数据（输入指令、输出响应）。利用MATLAB的系统辨识或简单的优化算法（如梯度下降），反向推出一组更优的LQR权重Q, R。
   • 在线微调：可以为模糊规则表的输出值设置一个微调系数，允许机器人在运行过程中，根据长期跟踪误差的统计量（如误差的均值、方差），缓慢地自适应调整规则表的输出强度。这需要谨慎设计，避免系统因过度学习而不稳定。

通过以上系统的设计、实现、调试和优化，我们最终让机器人实现了厘米级的轨迹跟踪精度，即使在高速和存在扰动的情况下，也能表现出优异的稳定性和鲁棒性。这套“模糊PI（上层决策与适应）+ LQR（底层执行与优化）”的框架，不仅适用于足球机器人，也为任何需要高精度、高动态运动控制的轮式机器人平台提供了一个坚实可靠的解决方案。