现代永磁同步电机FOC控制技术权威指南：从PID、系统辨识到LQR/MPC

前言

在机器人、新能源汽车、高端数控机床和无人机技术飞速发展的今天，永磁同步电机（PMSM）凭借其高效率、高功率密度和优越的调速性能，已成为高性能运动控制领域的绝对主力。然而，要将PMSM的潜力发挥到极致，仅仅让它“转起来”是远远不够的。如何实现丝滑的低速控制、闪电般的动态响应和精准的转矩输出，是对每一位嵌入式工程师提出的严峻挑战。

这份指南正是为此而生。我们观察到，许多工程师和学者在学习磁场定向控制（FOC）时，面临着三大核心痛点：

1. 理论与实践脱节：教科书上充斥着复杂的数学公式，但鲜有文章能讲清楚这些公式如何一步步转化为稳定、高效的嵌入式代码。
2. 知其然，不知其所以然：很多工程师使用开源库或厂商提供的例程，能够驱动电机，但对于其中的参数（如PID增益、电流环带宽）的物理意义和整定依据一知半解，一旦遇到性能瓶颈或“疑难杂症”，便束手无策，调参过程如同“玄学”。
3. 技术迭代的知识壁垒：当项目需求超出传统PID的控制能力时，如何向LQR、MPC等更现代的控制理论平滑过渡，缺乏一条清晰、可实践的学习路径。

本指南旨在彻底打破这些壁垒。我们将沿着一条精心设计的脉络，带领读者从零开始，构建一个坚不可摧的知识体系： FOC基础 -> 硬件抽象与信号链路 -> 系统辨识 -> PID深度设计 -> 前馈补偿 -> LQR/MPC进阶

我们将摒弃空洞的理论说教，每一个知识点都将围绕“在FOC中遇到了什么问题？”、“这个理论如何解决它？”以及“代码层面如何落地？”这三个核心问题展开。您将看到的不仅是公式，更是经过验证的代码、直观的图表和来自工程一线的实践经验。

如何使用本指南

• 对于初学者或希望系统学习的工程师：我们强烈建议您从第零章开始，按顺序阅读。打好硬件和基础理论的地基，是通往高性能控制的唯一捷径。
• 对于有一定经验，希望解决特定问题的工程师：您可以直接跳转到感兴趣的章节。例如，如果您对PID调参感到困惑，可以直接阅读第二章（系统辨识）和第三章（PID深度设计）。
• 对于项目经理或技术决策者：第六章的横向对比表格和技术选型路线图，将为您提供清晰、实用的决策依据。

前置知识

为了获得最佳的学习体验，我们假设您已具备：

• 基本的C语言编程能力和嵌入式开发经验。
• 对线性代数（矩阵运算）和自动控制原理（传递函数、状态空间）有初步了解。
• 一颗渴望探究事物本质、追求卓越工程技术的心。

现在，让我们一起开启这段从“能转”到“转得好”，再到“极致性能”的探索之旅。

第零章：万丈高楼平地起——硬件平台与信号链路

【忠告】 任何先进的控制算法都建立在可靠的硬件和高质量的信号之上。软件的性能上限，在项目初期就已经由硬件平台和信号链路的质量所决定。忽视本章内容，直接进入算法调试，是导致项目延期和性能不达标的最常见原因之一。

0.1 典型FOC硬件系统架构

一个典型的FOC驱动系统由以下几个核心部分组成，它们之间环环相扣，构成一个完整的闭环控制系统。

• 微控制器 (MCU)：整个系统的大脑。它负责执行FOC算法（坐标变换、PID计算等），生成PWM信号，并处理来自传感器的反馈信号。
• 预驱动器 (Gate Driver)：一个关键的中间环节。它负责将MCU输出的逻辑电平PWM信号，放大为足以驱动逆变器中功率管（MOSFET/IGBT）的电压和电流。同时，它还提供关键的保护功能，如**死区插入（Dead-time Insertion）**和短路保护。
• 三相逆变器 (Inverter)：系统的“肌肉”。由六个功率管组成，它根据预驱动器传来的开关信号，将直流母线电压（DC Bus Voltage）转换为三相交流电，驱动电机。
• 永磁同步电机 (PMSM)：控制对象和执行机构。
• 传感器 (Sensors)：系统的“感官”。
  • 电流传感器：测量至少两相的相电流，是电流环闭环控制的基础。
  • 位置/速度传感器：测量转子的精确角度和速度，是速度环闭环和坐标变换（Park变换）的基础。

0.2 核心元器件选型考量

• MCU的选择：

  • 主频与FPU：FOC算法包含大量三角函数和浮点运算。带有**硬件浮点单元（FPU）**的MCU（如Cortex-M4, M7等）是实现高性能FOC的首选。它能将浮点运算时间缩短一个数量级，使您有更多CPU周期来运行更快的控制循环或更复杂的算法。
  • ADC性能：ADC的分辨率和转换速度直接决定了电流采样的质量。一个12-bit、转换时间小于1µs的ADC是基本要求。高速、高精度的ADC能提供更平滑、噪声更低的电流反馈。
  • PWM模块：需具备互补输出模式和硬件死区插入功能。这能有效防止同一桥臂的上下管同时导通（直通），避免硬件损坏。高级PWM模块还支持与ADC的联动触发，这是实现精确同步采样的关键。

• 电流传感器的选择：

  • 采样方式：常见的有低侧采样（在相线和地之间串联采样电阻）、高侧采样（在直流母线正极和桥臂上管之间串联电阻）和相内采样。低侧采样电路简单，但无法检测到直通等故障；高侧采样更安全；相内采样（如霍尔传感器）隔离性好，但成本和体积较大。对于大多数应用，基于采样电阻的方案性价比最高。
  • 带宽与噪声：传感器的带宽必须远大于电流环的带宽（通常要求10倍以上），否则会引入相位延迟，影响环路稳定性。运放的选型同样重要，低噪声、高带宽的运放是保证信号质量的前提。

• 位置/速度传感器的选择：

  • 分辨率：编码器的分辨率直接决定了电机在低速下的平稳性。一个高分辨率的编码器（如2500线/转或更高）可以提供更精确的角度信息，从而实现更平滑的转矩控制，显著改善低速“顿挫感”。

0.3 关键信号链路的同步性

【核心痛点】 为什么我的电流采样值噪声很大，即使在电机静止时也是如此？为什么电机在低速时会“发抖”？90%的概率，是你的PWM和ADC采样没有做到精确同步。

在FOC中，我们希望测量的是相电流的平均值，而不是充满了开关纹波的瞬时值。在使用中心对齐PWM模式时，电流在一个PWM周期内的纹波是近似三角波的。

电流纹波在PWM周期的**中心（计数器峰值）和两端（计数器零点）**达到峰值。在这些点，电流的瞬时变化率di/dt最小，信号最稳定。因此，必须将ADC采样触发点精确地安排在PWM计数器达到峰值（上溢）或谷值（下溢）的瞬间，这能显著降低开关纹波引入的采样噪声，从而保证采样质量。

如何实现同步？ 现代MCU的定时器模块（Timer）通常具有触发ADC的功能。您需要将PWM定时器配置为中心对齐模式，并设置其更新事件（Update Event，即计数器下溢）或触发输出（Trigger Out）来启动ADC转换。

C伪代码示例 (以STM32为例):

// 伪代码: 配置Timer1触发ADC1

// 1. 配置TIM1为主模式，当更新事件(UEV)发生时，产生触发信号(TRGO)
TIM_MasterConfigTypeDef sMasterConfig = {0};
sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE; // 选择更新事件作为触发源
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig);

// 2. 配置ADC1为从模式，其转换由TIM1的触发信号启动
ADC_ExternalTrigConvCfgTypeDef sConfigExternalTrig = {0};
sConfigExternalTrig.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_TRGO; // 外部触发源为TIM1的TRGO
sConfigExternalTrig.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; // 在触发信号的上升沿启动
HAL_ADC_ConfigChannel(&hadc1, &sConfig);

// 3. 启动PWM和ADC
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1); // 启动互补通道
HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adc_raw_buffer, 2); // 启动ADC，使用DMA传输数据

通过这种硬件层面的联动，可以确保每一次电流采样都发生在PWM周期的最佳时刻，从根源上获得干净、稳定的电流反馈信号，为上层控制算法的成功打下坚实的基础。

第一章：FOC控制与PMSM模型基础

本章将建立FOC控制所需的最小数学模型，并阐述其核心思想。所有复杂的控制策略，都源于对这个基础模型的深刻理解。

1.1 PMSM的数学模型

我们从PMSM在自然三相（ABC）静止坐标系下的定子电压方程开始：

其中，$v_{a,b,c}$ 是三相电压，$i_{a,b,c}$ 是三相电流，$R_s$ 是定子电阻，$\psi_{a,b,c}$ 是三相磁链。这个模型是耦合的、非线性的，难以直接用于控制器设计。FOC的目标就是通过坐标变换将其解耦和线性化。

第一步：Clarke变换 (ABC -> αβ)

Clarke变换将三相时变的ABC坐标系，投影到一个正交的、静止的αβ坐标系。

变换后，三个变量变成了两个。此时的模型仍然与转子位置有关，是时变的。

第二步：Park变换 (αβ -> dq)

Park变换是FOC的精髓。它引入了转子的电角度 $\theta_e$，将静止的αβ坐标系，变换为一个与转子同步旋转的dq坐标系。d轴（Direct-axis）通常与转子磁链方向对齐，q轴（Quadrature-axis）与其正交。

最终的dq轴电压方程

经过两次变换，我们得到了在dq旋转坐标系下的PMSM电压方程。对于表贴式PMSM（SPMSM, $L_d = L_q = L_s$），其模型为：

其中：

• $v_d, v_q$：d,q轴电压
• $i_d, i_q$：d,q轴电流
• $L_s$：定子电感 ($L_d = L_q$)
• $\omega_e$：转子电角速度 ($\omega_e = N_p \cdot \omega_m$, $N_p$为极对数, $\omega_m$为机械角速度)
• $\psi_f$：永磁体磁链

这个模型是线性的、时不变的（当$\omega_e$恒定时）。所有的变量都变成了类似直流的慢变量，这为使用经典的PI控制器创造了完美的条件。

1.2 FOC的核心思想

观察dq轴电压方程，再看电磁转矩方程：

对于表贴式电机（SPMSM），$L_d = L_q$，转矩方程简化为：

核心思想就体现在这里：

1. 解耦：转矩$T_{em}$只与q轴电流$i_q$成正比，而与d轴电流$i_d$无关。这就像控制一台他励直流电机，我们可以通过控制电枢电流（相当于$i_q$）来直接控制转矩，同时保持励磁电流（相当于$i_d$）不变。
2. 磁场控制：d轴电流$i_d$主要控制转子磁场。在FOC中，最常用的策略是Id=0控制，即让$i_d$的参考值为0。这样做的好处是：
   • 效率最高：定子电流矢量完全用于产生转矩，没有产生无效磁场的无功分量，铜耗最小。
   • 线性转矩特性：转矩与$i_q$完全呈线性关系，简化了控制。
   • （在高速区，会使用弱磁控制，即注入负的$i_d$，此处暂不展开）。

而对于内嵌式电机（IPMSM），由于$L_q > L_d$，转矩方程中的 $(L_d - L_q)i_d i_q$ 项不能忽略，这一项被称为磁阻转矩。通过注入特定的负$i_d$，可以利用磁阻转矩在不增加电流的情况下获得额外的转矩输出，即“最大转矩电流比（MTPA）”控制策略。

FOC的本质，就是通过坐标变换，在旋转的dq坐标系里，把交流电机当成一台直流电机来控制。

1.3 FOC的典型级联结构

为了实现对电机速度和转矩的精确控制，FOC通常采用双闭环级联控制结构：一个外环（速度环）和一个内环（电流环）。

• 速度环（外环）：一个较慢的环路。
  • 输入：速度参考值 $\omega_{ref}$ 和从编码器反馈的速度实际值 $\omega_{fb}$。
  • 控制器：通常是一个PI控制器。
  • 输出：q轴电流的参考值 $I_{q\_ref}$。这个输出本质上是期望的转矩指令。
• 电流环（内环）：一个非常快的环路，是整个FOC性能的核心。
  • 它包含两个独立的PI控制器，分别控制$i_d$和$i_q$。
  • 输入：d轴电流参考值 $I_{d\_ref}$（通常为0）和q轴电流参考值 $I_{q\_ref}$（来自速度环的输出），以及经过坐标变换后的$i_d, i_q$反馈值。
  • 控制器：两个PI控制器。
  • 输出：d轴和q轴的电压指令 $V_{d\_ref}, V_{q\_ref}$。
• 执行：$V_{d\_ref}, V_{q\_ref}$经过逆Park变换和SVPWM模块，最终生成驱动逆变器的6路PWM信号。

这种级联结构的好处是，内环（电流环）的动态响应远快于外环（速度环），可以确保电流能够快速、精确地跟踪其参考值。这样，速度环就可以假定“我给出的转矩指令（$I_{q\_ref}$）能够被完美执行”，从而大大简化了速度环的设计。

1.4 SVPWM技术简介

SVPWM（空间矢量脉宽调制）是FOC控制链的最后一环，也是最理想的执行机构。

• 作用：它接收来自电流环输出的电压指令 $V_{d\_ref}, V_{q\_ref}$。这两个值经过逆Park变换后，会合成一个空间电压矢量 $\vec{V}_{ref}$。SVPWM的任务就是，通过巧妙地控制逆变器六个开关管在一个PWM周期内的开关时间，来合成等效于 $\vec{V}_{ref}$ 的电压输出。
• 优势：相比传统的SPWM，技术资料表明，SVPWM和三次谐波注入（THIPWM）的直流母线电压利用率可达0.907（即$1/\sqrt{3}$），相比SPWM的0.785（即$\pi/4$）提高了约15%。这意味着在相同的母线电压下，电机可以输出更高的转速和转矩。

在本指南中，我们不深入SVPWM的算法细节，而是将其视为一个标准的“函数块”：输入是期望的 $\alpha\beta$ 轴电压，输出是三相PWM的占空比。

1.5 坐标系约定与一致性

【提示】 一个新手极易出错且调试过程极其痛苦的“坑”，就是整个项目中的角度定义不一致。

FOC的正确运行，依赖于一个绝对统一的电气角度 $\theta_e$。这个角度在两个地方至关重要：

1. Park变换：将反馈电流从 $\alpha\beta$ 坐标系变换到 dq 坐标系。
2. 逆Park变换：将期望电压从 dq 坐标系变换回 $\alpha\beta$ 坐标系。

问题根源：

• 编码器零点与A相轴线未对齐：物理安装时，编码器的0度脉冲位置，与电机A相绕组的磁轴中心位置，几乎不可能完全重合。它们之间存在一个固定的电气偏移角（Electrical Offset）。
• 不同变换公式的差异：不同的教科书或论文，其Clarke/Park变换矩阵可能存在细微差异（例如，对齐q轴还是d轴），这会导致角度定义相差90度。

解决方案：初始位置角校准 在系统第一次上电时，必须执行一个校准程序来找到这个电气偏移角。一个简单有效的方法是：

1. 强制对齐：给电机d轴一个固定的直流电流（例如 $I_d = 0.1 \times I_{rated}, I_q = 0$），$I_q$设为0。这会在定子中产生一个方向确定的磁场。
2. 转子跟随：由于永磁体转子会像指南针一样，自动旋转到与定子磁场对齐的位置以求能量最低，此时转子磁链轴线（d轴）将与定子合成磁场轴线对齐。
3. 记录角度：等待电机稳定后，读取此时编码器的机械角度读数 $\theta_{m\_offset}$。
4. 计算偏移：电气偏移角 $\theta_{offset} = N_p \cdot \theta_{m\_offset}$。
5. 应用校正：在后续的FOC运算中，真实的电气角度应该是 $\theta_e = N_p \cdot \theta_{m\_raw} - \theta_{offset}$，其中$\theta_{m\_raw}$是编码器实时读数。

在整个项目中，所有使用到角度的地方，都必须使用这个经过校正的 $\theta_e$。强烈建议将角度的获取和校正封装成一个独立的函数，以保证一致性。

第二章：高性能控制的基石——系统辨识

如果说第一章我们建立了理论的“地图”，那么本章就是教会我们如何使用“GPS”来精确定位我们自己（即我们的控制对象）在这张地图上的位置。没有精确的系统模型参数，任何先进的控制算法都如同在迷雾中航行，再精妙的理论也无法转化为卓越的性能。

2.1 核心论点：为何系统辨识是不可或缺的？

让我们用一个生动的例子来回答这个问题。

想象一下调试一个高精度六轴机械臂。

• “盲调”工程师小李：他拿到机械臂，不进行任何建模，直接开始凭感觉调整速度环的PID参数。他增大P，机械臂开始震荡；他减小P，又觉得响应太慢；他加入I，希望能消除稳态误差，却在快速启停时看到了巨大的过冲和积分饱和。整个下午过去了，机械臂的性能依然差强人意，时而振动，时而迟钝，像一个“醉汉”。
• “建模”专家王工：他拿到机械臂后，第一件事不是调PID，而是进行系统辨识。他通过施加特定的转矩指令并测量关节的角加速度响应，精确地计算出了该关节的转动惯量 J 和摩擦系数 B。有了这些参数，他直接套用基于模型的PID整定公式，一次性计算出了接近最优的PID增益。整个过程只花了半小时。随后，他只需要进行微调，机械臂的响应便如“外科医生”般精准、快速且稳定。

这个例子中的天壤之别，就是系统辨识的价值所在。盲目调参是在与一个未知的、看不见的敌人作战，而基于系统辨识的参数整定，则是拿着精确的地图和装备，进行一场胜券在握的精确打击。

2.2 需要辨识的关键参数

在FOC控制中，我们主要关心以下几个核心物理参数：

2.3 辨识方法详述

2.3.0 辨识前的安全须知

【警告】 系统辨识过程会给电机注入非正常的电压或电流信号，存在潜在的失控风险。在开始任何辨识操作前，请务必遵守以下安全准则：

1. 设置严格的限制：在代码中设置严格的电流保护阈值、电压保护阈值和速度保护阈值。一旦超出，立即停止所有PWM输出。
2. 机械解耦：如果条件允许，将电机与最终的负载（如齿轮箱、传送带）断开，先辨识电机本体的参数。
3. 备好急停开关：确保您的测试平台有一个物理的急停按钮，可以随时切断电源。
4. 从低幅值开始：所有注入的测试信号，都应从一个非常小的幅值开始，确认系统稳定后再逐步增大。

2.3.1 离线与在线辨识方法论

• 离线辨识（Offline Identification）：在电机正式运行前，通过专门的测试程序，依次测量出电机的各项参数。这些参数在后续的正常运行中被视为常数。本章主要介绍离线辨识方法，因为它最常用且效果稳定。
• 在线辨识（Online Identification）：在电机正常运行时，通过特定的算法（如模型参考自适应、卡尔曼滤波器等）持续地估计和更新系统参数。这对于参数会发生变化的系统（如负载变化、电机温升导致电阻变化）非常有用，但算法复杂，实现难度高。

2.3.2 直流注入法辨识电阻 $R_s$

• 原理：基于欧姆定律 $V=IR$。当给电机施加一个直流电压时，电机静止不动（$\omega_e=0$），dq模型中的速度项为零，d轴电压方程简化为 $v_d = R_s i_d$。
• 步骤：
  1. 锁住转子：可以通过机械方式固定，或者开启一个高增益的位置环将电机锁在0度位置。
  2. 注入直流电压/电流：在FOC控制环路中，设置 $I_{q\_ref}=0$，给 $I_{d\_ref}$ 一个较小的正值（例如额定电流的5%-10%）。
  3. 等待稳定：等待d轴电流环稳定后，记录此时稳态的d轴电压指令 $V_{d\_ref}$ 和d轴电流反馈值 $I_{d\_fb}$。
  4. 计算电阻：$R_s = V_{d\_ref} / I_{d\_fb}$。为提高精度，可测量多次取平均值。
• 注意事项：电阻值受温度影响显著。此方法测出的是冷态电阻，电机运行发热后电阻会增大。

2.3.3 高频方波注入法辨识电感 $L_d, L_q$

• 原理：对于一个RL电路，当施加一个高频电压信号时，感抗 $\omega L$ 远大于电阻 $R$，电路主要呈现电感特性。电压方程 $v = R i + L \frac{di}{dt}$ 中，$\frac{di}{dt}$ 项占主导地位。
• 步骤：
  1. 锁住转子：同辨识电阻。
  2. 注入高频电压：在d轴（或q轴）上注入一个正负交替的电压方波。例如，在一个PWM周期前半段，施加 $+V_{test}$，后半段施加 $-V_{test}$。
  3. 测量电流斜率：在施加正电压的阶段，电流会线性上升，测量其斜率 $(\frac{di}{dt})_+$；在施加负电压的阶段，电流会线性下降，测量其斜率 $(\frac{di}{dt})_-$。
  4. 计算电感：根据 $v = L \frac{di}{dt}$，可以得到 $L = \frac{V_{test}}{\text{斜率}}$。更精确地，$L_d = \frac{2 \cdot V_{test}}{(\frac{di_d}{dt})_+ - (\frac{di_d}{dt})_-}$。辨识$L_q$的方法完全相同，只需将电压注入q轴。
• 注意事项：注入的方波频率要足够高，以忽略电阻项的影响；但频率也不能太高，要保证MCU的ADC有足够的时间进行多次采样来计算斜率。

2.3.4 扫频法辨识系统传递函数（重点章节）

• 原理：扫频法是系统辨识中最为经典和强大的方法之一。其核心思想是，向系统注入一系列不同频率的正弦信号，并测量系统在每个频率下的响应（输出信号的幅值和相位），从而绘制出系统的伯德图（Bode Plot）。伯德图就像是系统的“指纹”，完整地描述了系统从低频到高频的动态特性。
• 步骤（以辨识速度环传递函数为例）：
  1. 搭建测试环境：保持电流环正常运行，断开速度环的PID控制器。
  2. 注入正弦信号：直接向电流环的q轴电流指令 $I_{q\_ref}$ 注入一个正弦信号：$I_{q\_ref}(t) = A \cdot \sin(2\pi f \cdot t)$。这个$I_{q\_ref}$在此处作为系统的输入。
  3. 测量系统响应：同时，测量电机的速度响应 $\omega_{fb}(t)$，作为系统的输出。
  4. 分析响应：对于每个测试频率 $f$，都需要分析输出信号 $\omega_{fb}(t)$ 相对于输入信号的增益（幅值比）和相位差。这通常需要使用相关性分析或FFT等算法在嵌入式端完成，或将原始数据传到PC端进行分析。
  5. 绘制伯德图：以频率为横轴，增益（通常用dB表示）和相位为纵轴，将所有测试点连接起来，就得到了系统的伯德图。
  6. 拟合传递函数：观察伯德图的形状，例如，从其转折频率、低频增益和斜率等特征，可以拟合出系统的传递函数模型，例如一阶或二阶模型。从拟合出的模型系数中，就可以反向计算出 $J$ 和 $B$ 等物理参数。这一步通常在MATLAB中完成，效率和精度最高。

2.3.5 拖动法/阶跃响应法辨识转动惯量 J

• 原理：基于牛顿第二定律的旋转形式 $T = J \alpha$，其中 $T$ 是转矩，$\alpha$ 是角加速度。如果我们能施加一个已知的恒定转矩，并测量出产生的恒定角加速度，就能计算出转动惯量 $J$。
• 步骤：
  1. 设置开环转矩：在FOC中，保持电流环工作，给q轴一个恒定的阶跃电流指令 $I_{q\_ref}$（忽略摩擦，认为转矩 $T = \frac{3}{2}N_p \psi_f I_{q\_ref}$）。
  2. 测量速度曲线：记录电机从静止开始，在恒定转矩作用下的速度变化曲线。理论上，速度 $\omega_m(t)$ 应该是一条斜线。
  3. 计算角加速度：计算速度曲线的斜率，即为角加速度 $\alpha = \frac{d\omega_m}{dt}$。
  4. 计算惯量：$J = T / \alpha$。

2.4 系统辨识的嵌入式代码实现思路

在嵌入式MCU中，可以设计一个状态机来自动化整个离线辨识流程。

C语言伪代码结构：

typedef enum {
    STATE_IDLE,
    STATE_IDENTIFY_R,
    STATE_IDENTIFY_L,
    STATE_IDENTIFY_J,
    STATE_DONE,
    STATE_ERROR
} IdentificationState_t;

IdentificationState_t g_id_state = STATE_IDLE;
float identified_R, identified_L, identified_J;

void Identification_Process(void) {
    switch (g_id_state) {
        case STATE_IDLE:
            // 等待启动指令
            break;
        case STATE_IDENTIFY_R:
            // 执行直流注入法辨识电阻的逻辑
            // ...
            if (is_R_identification_complete()) {
                identified_R = get_calculated_R();
                g_id_state = STATE_IDENTIFY_L;
            }
            break;
        case STATE_IDENTIFY_L:
            // 执行高频注入法辨识电感的逻辑
            // ...
            if (is_L_identification_complete()) {
                identified_L = get_calculated_L();
                g_id_state = STATE_IDENTIFY_J;
            }
            break;
        case STATE_IDENTIFY_J:
            // 执行阶跃响应法辨识惯量的逻辑
            // ...
            if (is_J_identification_complete()) {
                identified_J = get_calculated_J();
                g_id_state = STATE_DONE;
            }
            break;
        case STATE_DONE:
            // 所有参数辨识完成，可以保存参数并停止
            break;
        case STATE_ERROR:
            // 处理错误状态
            break;
    }
}

2.5 MATLAB在系统辨识与建模中的核心应用

2.5.1 引言：为何使用MATLAB？

虽然可以在嵌入式端实现简单的辨识算法，但要进行精确、鲁棒的系统辨识，尤其是在处理扫频法等复杂数据时，MATLAB是当之无愧的“瑞士军刀”。它是连接**物理世界（从MCU采集的原始数据）和理论世界（精确的数学模型）**的最强大桥梁。借助其强大的 System Identification Toolbox™ 和 Control System Toolbox™，我们可以极大地简化辨识流程，并获得更高的模型精度。

2.5.2 辨识工作流详解：从数据到模型

1. 数据准备与导入 首先，在嵌入式设备上运行扫频或阶跃响应程序，将输入信号（如$I_{q\_ref}$）和输出信号（如$\omega_{fb}$）以及对应的时间戳，通过串口打印出来，并保存为CSV文件（例如 scan_data.csv）。

% MATLAB代码: 导入数据
data = readtable('scan_data.csv');
% 假设CSV文件有三列: 'timestamp', 'input', 'output'
t = data.timestamp;
u = data.input;
y = data.output;
sample_time = t(2) - t(1); % 计算采样时间

2. 数据预处理 原始数据通常包含噪声和直流偏置，需要进行预处理。

% MATLAB代码: 数据预处理
u = detrend(u); % 去除输入信号的直流偏置
y = detrend(y); % 去除输出信号的直流偏置
% 还可以使用 filter 函数进行滤波
% 将数据封装成标准辨识对象 iddata
id_data = iddata(y, u, sample_time);

3. 模型辨识与参数估计 (核心)

• 图形化工具：在MATLAB命令行输入 systemIdentification，会打开图形化APP。您可以导入id_data，尝试用不同的模型（如传递函数、状态空间模型）去拟合数据，界面会直观地显示拟合效果。
• 命令行方式（更高效、可脚本化）：
  matlab

  % 假设我们根据物理知识，认为速度环模型是一个二阶系统
  % G(s) = K / (J*s^2 + B*s + 1)
  % 这里的模型结构是两个极点，没有零点
  num_poles = 2;
  num_zeros = 0;
  sys_est = tfest(id_data, num_poles, num_zeros); % 使用tfest进行传递函数估计
  

  对于扫频法获得的数据（频率f, 增益amp, 相位phase），应使用frd对象：
  matlab

  % MATLAB代码: 处理扫频数据
  w = 2 * pi * f; % 转换为角频率
  response = amp .* exp(1i * deg2rad(phase)); % 转换为复数响应
  frd_data = frd(response, w); % 创建频率响应数据对象
  sys_est_from_freq = tfest(frd_data, num_poles, num_zeros); % 从频率响应数据进行拟合
  

4. 模型验证 辨识出的模型是否可信？需要与原始数据进行对比。

% MATLAB代码: 模型验证
compare(id_data, sys_est);
legend('Measured Output', 'Estimated Model Output');

compare函数会生成一张图，将真实测量值与模型预测值放在一起。同时，窗口标题会显示“拟合优度”（Fit to estimation data），例如“92.5%”。这个百分比越高，代表模型对真实系统的描述越准确。

2.5.3 从辨识模型到参数提取

我们得到的是一个数学模型，如 $G(s) = \frac{C}{s^2+As+B}$。如何反向计算出物理参数 $J, B$？ 这需要将标准物理模型与辨识出的模型进行对比。 速度环的物理模型通常为：$G(s) = \frac{\omega_m(s)}{T_{em}(s)} = \frac{1}{Js+B}$ (一阶模型)。 通过转矩常数 $K_t = \frac{3}{2}N_p \psi_f$，我们有 $T_{em}(s) = K_t I_q(s)$。 所以，我们辨识的传递函数 $\frac{\omega_m(s)}{I_q(s)}$ 应该是 $G(s) = \frac{K_t}{Js+B} = \frac{K_t/B}{(J/B)s+1}$。 通过对比辨识出的模型系数，就可以解出$J$和$B$。

2.5.4 脚本化与自动化

最终要求： 以下是一个完整的MATLAB脚本范例，贯穿了从数据加载到参数提取，再到控制器设计的全过程。

%% 现代永磁同步电机FOC系统辨识与控制器设计脚本
% Author: DeepResearch
% Date: 2025-08-07

clc; clear; close all;

%% 1. 数据加载与准备
disp('1. Loading data...');
data = readtable('speed_step_response.csv'); % 加载阶跃响应数据
t = data.timestamp;
Iq_ref_input = data.input;  % 输入：q轴电流指令
omega_fb_output = data.output; % 输出：机械角速度
sample_time = t(2) - t(1);
Kt = 1.5 * 4 * 0.05; % 假设电机转矩常数 Kt = 1.5 * Np * Psi_f 已知

%% 2. 数据预处理与封装
disp('2. Pre-processing data...');
% 创建iddata对象
id_data = iddata(omega_fb_output, Iq_ref_input, sample_time, ...
                 'InputName', 'Iq_ref', 'OutputName', 'Omega_fb');
% 去除直流偏置
id_data = detrend(id_data);

%% 3. 系统辨识
disp('3. Identifying system model...');
% 我们期望的模型是 G(s) = Omega(s)/Iq(s) = Kt / (J*s + B)
% 这是一个一阶系统
num_poles = 1;
num_zeros = 0;
% 使用tfest进行传递函数估计
sys_tf = tfest(id_data, num_poles, num_zeros);

disp('Identified Transfer Function:');
disp(sys_tf);

%% 4. 模型验证
disp('4. Validating model...');
figure;
compare(id_data, sys_tf);
title('Model Validation: Measured vs. Estimated');
grid on;

%% 5. 物理参数提取
disp('5. Extracting physical parameters...');
% 辨识出的模型形式为: K / (T*s + 1)
% 我们的物理模型为: (Kt/B) / ((J/B)*s + 1)
% 通过对比系数:
% K = Kt/B  =>  B = Kt / sys_tf.num{1}
% T = J/B   =>  J = T * B = sys_tf.den{1}(1) * B
[num, den] = tfdata(sys_tf, 'v');
K = num(end) / den(end);
T = den(1) / den(end);

B_est = Kt / K; % 估算的摩擦系数 B
J_est = T * B_est; % 估算的转动惯量 J

fprintf('Estimated Friction (B): %.4f N*m*s/rad\n', B_est);
fprintf('Estimated Inertia (J):  %.6f kg*m^2\n', J_est);

%% 6. (可选) 基于模型的控制器自动整定
disp('6. Auto-tuning PID controller...');
% 创建我们估算出的被控对象模型P
P = tf(Kt, [J_est, B_est]);
% 设定期望的开环带宽 (例如 50 Hz)
target_bandwidth_hz = 50;
wc = target_bandwidth_hz * 2 * pi;
% 使用pidtune自动设计一个PI控制器
[C_pi, info] = pidtune(P, 'PI', wc);

fprintf('\nAuto-tuned Speed PI Controller:\n');
fprintf('  Kp = %.4f\n', C_pi.Kp);
fprintf('  Ki = %.4f\n', C_pi.Ki);

% 显示闭环响应
figure;
step(feedback(C_pi*P, 1));
title('Closed-loop Step Response with Auto-tuned PI Controller');
grid on;

这个脚本完美地展示了系统辨识的强大威力：我们从一堆看似杂乱的输入输出数据开始，最终不仅得到了精确的物理参数 $J$ 和 $B$，还利用这些参数自动设计出了一个性能优异的PI控制器。这，就是从“玄学”调参迈向现代控制工程的关键一步。

**第三章：PID控制器的深度设计与实践 **

欢迎来到本指南的心脏地带。在这一章，我们将彻底剖析FOC系统中最核心的控制器——PID。我们将摒弃“凭感觉”调参的旧方法，建立一套基于模型的、系统化的设计流程。学完本章，您将有能力为您的电机设计出响应快速、超调微小且抗扰动能力强的控制器。

3.1 PID理论回顾与数字化

PID（Proportional-Integral-Derivative）控制器是工业控制领域当之无愧的基石。其标准形式为：

其中，$e(t) = r(t) - y(t)$ 是参考值与测量值之间的误差。

在嵌入式系统中，我们使用离散化的PID。常见的有位置式和增量式两种。

• 位置式PID：直接计算当前时刻需要输出的控制总量。

  $$u[k] = K_p e[k] + K_i \sum_{j=0}^{k} e[j] T_s + K_d \frac{e[k] - e[k-1]}{T_s}$$

  其中 $T_s$ 是控制周期。其缺点是积分项需要累加全部历史误差，容易产生巨大的累积值（积分饱和），且在手动/自动切换时会产生剧烈跳变。

• 增量式PID：计算当前时刻控制量的增量 $\Delta u[k]$。

  $$\Delta u[k] = u[k] - u[k-1] = K_p(e[k]-e[k-1]) + K_i e[k] T_s + K_d \frac{e[k]-2e[k-1]+e[k-2]}{T_s}$$

  最终输出为 $u[k] = u[k-1] + \Delta u[k]$。增量式PID的输出与过去的误差状态无关，只与最近几次的误差有关，这使得它在控制模式切换时能实现平滑过渡，并且积分饱和问题的影响相对较小。因此，在电机控制中，增量式PID更为常用。

3.2 基于模型的PID参数整定（重点）

这是本章的精髓。我们将利用第二章辨识出的电机参数($R_s, L_s, J, B$)，像解数学题一样精确地计算出PID增益。

3.2.1 电流环PI控制器设计

• 目标：电流环是FOC的心跳，必须极快且稳定。我们的设计目标是根据期望的电流环带宽来设计PI参数。带宽（Bandwidth, $\omega_{bw}$）衡量了系统跟踪快速变化信号的能力，带宽越高，响应越快。通常，电流环的带宽应设置在1-2kHz，远高于速度环。

• 模型：回顾dq轴电压方程，我们只看q轴（d轴同理），并暂时忽略速度耦合项（该项将由前馈处理）：

  $$v_q = R_s i_q + L_s \frac{di_q}{dt}$$

  这是一个典型的一阶RL电路。其传递函数（被控对象模型）为：

  $$G_p(s) = \frac{I_q(s)}{V_q(s)} = \frac{1}{L_s s + R_s}$$

• 控制器设计（极点配置法）： 我们的PI控制器传递函数为 $G_c(s) = K_{p\_c} + \frac{K_{i\_c}}{s}$。 整个电流环的闭环传递函数为：

  $$G_{cl}(s) = \frac{G_c(s)G_p(s)}{1 + G_c(s)G_p(s)} = \frac{(K_{p\_c} + \frac{K_{i\_c}}{s}) \frac{1}{L_s s + R_s}}{1 + (K_{p\_c} + \frac{K_{i\_c}}{s}) \frac{1}{L_s s + R_s}}$$

  化简后得到：

  $$G_{cl}(s) = \frac{K_{p\_c}s + K_{i\_c}}{L_s s^2 + (R_s + K_{p\_c})s + K_{i\_c}}$$

  这里的关键技巧是极点-零点对消。我们设置PI控制器的零点 $s = -K_{i\_c}/K_{p\_c}$ 去抵消被控对象的极点 $s = -R_s/L_s$。 令 $\frac{K_{i\_c}}{K_{p\_c}} = \frac{R_s}{L_s}$，即 $K_{i\_c} = K_{p\_c} \frac{R_s}{L_s}$。 代入上式，闭环传递函数被奇迹般地简化为一个标准的一阶系统：

  $$G_{cl}(s) = \frac{K_{p\_c}/L_s}{s + K_{p\_c}/L_s}$$

  我们期望的闭环系统是一个带宽为 $\omega_{bw\_c}$ 的一阶低通滤波器，其标准形式为 $\frac{\omega_{bw\_c}}{s + \omega_{bw\_c}}$。 对比两式，可得 $\omega_{bw\_c} = K_{p\_c}/L_s$。

• 最终计算公式：

  $$K_{p\_c} = \omega_{bw\_c} \cdot L_s$$ $$K_{i\_c} = \omega_{bw\_c} \cdot R_s$$

  其中，$\omega_{bw\_c}$ 是我们期望的电流环带宽（单位：rad/s）。例如，若期望带宽为1000Hz，则 $\omega_{bw\_c} = 2\pi \cdot 1000$。 看，我们利用辨识出的 $L_s$ 和 $R_s$，只通过设定一个有明确物理意义的“带宽”，就直接计算出了最优的PI参数！

3.2.2 速度环PID控制器设计

• 目标：速度环的设计目标是根据期望的速度环带宽和阻尼比来设计参数，确保系统在快速响应的同时没有过大的超调。速度环带宽通常远低于电流环，例如50-100Hz。

• 模型：速度环的被控对象是整个电流环+电机机械部分。由于电流环设计得非常快，我们可以近似认为它是一个理想的增益为1的环节（即 $I_{q\_fb} \approx I_{q\_ref}$）。 因此，速度环的被控对象简化为电机的机械模型，其输入为转矩 $T_{em}$，输出为速度 $\omega_m$：

  $$T_{em} = J \frac{d\omega_m}{dt} + B \omega_m$$

  其传递函数为 $G_m(s) = \frac{\omega_m(s)}{T_{em}(s)} = \frac{1}{Js + B}$。 由于 $T_{em} = K_t \cdot I_q$（其中 $K_t = \frac{3}{2}N_p \psi_f$ 是转矩常数），我们辨识的实际对象是：

  $$G_p(s) = \frac{\omega_m(s)}{I_q(s)} = \frac{K_t}{Js + B}$$

• 控制器设计（标准二阶系统法）： 速度环通常使用PI控制器（PD控制器也可以，取决于具体需求）。我们以PI控制器 $G_c(s) = K_{p\_s} + \frac{K_{i\_s}}{s}$ 为例。 闭环传递函数的分母（特征方程）为 $1 + G_c(s)G_p(s) = 0$。

  $$1 + (K_{p\_s} + \frac{K_{i\_s}}{s}) \frac{K_t}{Js + B} = 0 \implies s(Js+B) + K_t(K_{p\_s}s + K_{i\_s}) = 0$$

  整理得：

  $$Js^2 + (B + K_t K_{p\_s})s + K_t K_{i\_s} = 0$$

  这是一个标准的二阶系统。其标准形式为 $s^2 + 2\zeta\omega_n s + \omega_n^2 = 0$。其中 $\omega_n$ 是自然振荡频率，$\zeta$ 是阻尼比（$\zeta=1$为临界阻尼，响应最快无超调）。我们将$\omega_n$设置为期望的速度环带宽$\omega_{bw\_s}$。 对比系数可得：

  $$\frac{B + K_t K_{p\_s}}{J} = 2\zeta\omega_{bw\_s} \quad \text{and} \quad \frac{K_t K_{i\_s}}{J} = \omega_{bw\_s}^2$$

• 最终计算公式：

  $$K_{p\_s} = \frac{2\zeta\omega_{bw\_s} J - B}{K_t}$$ $$K_{i\_s} = \frac{\omega_{bw\_s}^2 J}{K_t}$$

  通过设定期望的带宽$\omega_{bw\_s}$和阻尼比$\zeta$（通常取0.707~1），我们就可以利用辨识出的$J$和$B$以及已知的$K_t$来精确计算速度环PI参数。

3.3 性能飞跃的关键——前馈控制的引入（重点）

PID本质上是“事后诸葛亮”，它看到误差后才开始行动。而前馈（Feedforward）控制则是“先见之明”，它根据参考指令和系统模型，主动地预测出系统所需要的控制量，并将其直接叠加到PID的输出上。PID只需要负责消除模型不准或外部扰动带来的微小误差即可。

这种“PID（反馈）+ 前馈”的结构，也称为2-DOF（双自由度）控制器，因为它分别对指令跟踪和扰动抑制进行了独立设计。

3.3.2 FOC中的核心前馈项

• 解耦前馈（电流环）： 还记得我们设计电流环时忽略的速度耦合项吗？现在我们把它加回来。完整的dq电压方程是：

  $$v_d = R_s i_d + L_s \frac{di_d}{dt} \underline{- \omega_e L_q i_q} \\ v_q = R_s i_q + L_s \frac{di_q}{dt} \underline{+ \omega_e L_d i_d + \omega_e \psi_f}$$

  下划线部分就是耦合项/反电动势项。它们是已知的扰动。我们可以在PI控制器的输出上，直接把这些项补偿掉。

  • d轴前馈电压：$V_{d\_ff} = -\omega_e L_q I_{q\_fb}$
  • q轴前馈电压：$V_{q\_ff} = +\omega_e L_d I_{d\_fb} + \omega_e \psi_f$ （对于表贴式电机 $L_d=L_q=L_s$）

• 惯量与摩擦前馈（速度环）： 速度环的输出是期望的转矩指令 $I_{q\_ref}$。根据机械模型 $T_{em} = J \alpha + B \omega$，我们可以根据期望的加速度和速度来预测所需要的转矩。

  • 摩擦前馈：$T_{ff\_friction} = B \cdot \omega_{ref}$
  • 惯量前馈：$T_{ff\_inertia} = J \cdot \alpha_{ref} = J \cdot \frac{d\omega_{ref}}{dt}$ 总的前馈转矩为 $T_{ff} = T_{ff\_friction} + T_{ff\_inertia}$，转换成电流指令为 $I_{q\_ff} = T_{ff} / K_t$。这个 $I_{q\_ff}$ 会直接加到速度环PID的输出上。

3.4 嵌入式实现的“必修课”

理论很美好，但在资源有限的MCU上实现时，必须处理好几个关键的工程问题。

3.4.1 积分抗饱和 (Anti-Windup)

• 问题：当电机堵转或指令超出物理极限时，控制器输出达到饱和（如PWM占空比达到100%），但误差依然存在。此时，标准PID的积分项会无休止地累积，形成一个巨大的“积分炸弹”。一旦误差反向，这个巨大的积分项无法迅速消退，导致系统产生剧烈的超调和长时间的振荡。
• 解决方案1：Clamping（钳位法） 最简单的方法。当控制器输出达到饱和时，如果误差和输出同向，就停止积分。
  c

  // C代码: Clamping Anti-Windup
  pi_out = controller->p_term + controller->i_term;
  if (pi_out > controller->out_max) {
      pi_out = controller->out_max;
      // 如果误差方向还会继续增大积分，则冻结积分
      if (error > 0.0f) {
          // 不更新积分项
      } else {
          controller->i_term += error * controller->ki * controller->ts;
      }
  } else if (pi_out < controller->out_min) {
      pi_out = controller->out_min;
      // 如果误差方向还会继续减小积分，则冻结积分
      if (error < 0.0f) {
          // 不更新积分项
      } else {
          controller->i_term += error * controller->ki * controller->ts;
      }
  } else {
      // 未饱和，正常积分
      controller->i_term += error * controller->ki * controller->ts;
  }
  

• 解决方案2：Back-Calculation（反演计算法） 更优雅、效果更好的方法。它计算出饱和输出与未饱和输出之间的“溢出量”，然后将这个溢出量以一定的比例反馈给积分器，强迫积分器退出饱和状态。
  c

  // C代码: Back-Calculation Anti-Windup
  // 1. 正常计算PI输出
  un_sat_out = controller->p_term + controller->i_term;
  
  // 2. 对输出进行饱和限制
  sat_out = un_sat_out;
  if (sat_out > controller->out_max) sat_out = controller->out_max;
  if (sat_out < controller->out_min) sat_out = controller->out_min;
  
  // 3. 计算饱和误差，并反馈给积分器
  float saturation_error = sat_out - un_sat_out;
  controller->i_term += controller->ki * error * controller->ts + controller->ka * saturation_error;
  // controller->ka 是反饱和增益, 通常取ki或ki的倍数
  

3.4.2 微分项的噪声抑制

• 问题：微分器对高频噪声非常敏感。对反馈信号直接求微分，会极大地放大噪声，导致输出抖动。
• 解决方案：不完全微分 或 低通滤波器 与其使用纯微分 $K_d \frac{de}{dt}$，不如使用一个带低通滤波效果的微分器。其传递函数为 $G_d(s) = \frac{K_d s}{T_f s + 1}$，其中 $T_f$ 是滤波时间常数。 离散化后的C代码实现非常简洁：
  c

  // C代码: 带低通滤波的微分项
  // Td: 微分时间常数, N: 滤波系数(通常取2-20)
  // Tf = Td / N
  // 此处使用基于测量值的微分(Derivative on Measurement)，以避免设定值阶跃时的微分冲击
  controller->d_term = (controller->kd * (controller->prev_measurement - measurement) 
                     + controller->d_filter_alpha * controller->prev_d_term);
  // 其中 d_filter_alpha = Tf / (Tf + Ts)
  

3.4.3 前馈通道的平滑处理

• 问题：当速度参考值ω_ref发生阶跃变化时，惯量前馈项 $J \frac{d\omega_{ref}}{dt}$ 会产生一个理论上无穷大的脉冲，导致控制量突变和系统振荡。
• 解决方案：斜坡函数或低通滤波
  1. 斜坡函数：不直接使用阶跃的ω_ref，而是通过一个斜坡发生器，将阶跃信号转化为平滑的斜坡信号。
  2. 低通滤波：对计算出的前馈项进行低通滤波，滤除其高频突变成分。

3.4.4 Setpoint Weighting (设定点加权)

• 问题：当设定值阶跃时，比例项 $K_p \cdot (ref - fb)$ 会产生巨大的“比例冲击”，导致超调。
• 解决方案：只对反馈值应用完整的比例作用，而对设定值应用一个小于1的权重b。 $$P_{term} = K_p \cdot (b \cdot ref - fb)$$ 当b=0时，比例项只作用于反馈，完全消除了比例冲击，但可能会降低响应速度。通常b取0.5~0.8。

3.5 完整代码示例

下面是一个高度工程化的PI控制器C函数，它整合了我们讨论过的所有设计细节。

typedef struct {
    // Gains
    float kp;
    float ki;
    float ka; // Anti-windup gain for back-calculation

    // State
    float i_term;
    
    // Limits
    float out_max;
    float out_min;
    
    // Sample time
    float ts;
} PI_Controller_t;

/**
 * @brief 更新一个PI控制器，包含反演计算抗饱和和前馈
 * @param controller PI控制器实例
 * @param error 当前误差 (ref - feedback)
 * @param feedforward 前馈值
 * @return 最终的控制器输出
 */
float PI_Controller_Update(PI_Controller_t* controller, float error, float feedforward)
{
    // 1. 计算比例项
    float p_term = controller->kp * error;

    // 2. 计算未饱和的控制器输出 (仅反馈部分)
    float un_sat_out_fb = p_term + controller->i_term;

    // 3. 将前馈项加入，得到总的未饱和输出
    float un_sat_out_total = un_sat_out_fb + feedforward;

    // 4. 对总输出进行饱和限制
    float sat_out_total = un_sat_out_total;
    if (sat_out_total > controller->out_max) {
        sat_out_total = controller->out_max;
    }
    if (sat_out_total < controller->out_min) {
        sat_out_total = controller->out_min;
    }

    // 5. 使用Back-Calculation更新积分项
    // 饱和误差只应考虑反馈控制器的饱和情况
    float saturation_error = sat_out_total - un_sat_out_total;
    controller->i_term += controller->ki * error * controller->ts + controller->ka * saturation_error;
    
    // (可选) 对积分项自身也进行钳位，提供双重保险
    if (controller->i_term > controller->out_max) controller->i_term = controller->out_max;
    if (controller->i_term < controller->out_min) controller->i_term = controller->out_min;

    return sat_out_total;
}


// --- 在主FOC控制循环中的调用 ---
// FOC_Controller_Update() 伪代码
void FOC_Main_Loop(void) {
    // ... 获取 speed_ref, speed_fb, id_fb, iq_fb ...
    
    // 1. --- 速度环计算 ---
    float speed_error = speed_ref - speed_fb;
    // (此处应加入速度参考斜坡和前馈计算)
    float speed_ff = calculate_speed_feedforward(speed_ref); // 根据J,B计算
    float iq_ref_total = PI_Controller_Update(&speed_pi, speed_error, speed_ff);

    // 2. --- 电流环计算 ---
    float id_ref = 0.0f; // Id=0控制
    float id_error = id_ref - id_fb;
    float iq_error = iq_ref_total - iq_fb;

    // 计算电流环解耦前馈项
    float vd_ff = - motor.omega_e * motor.Lq * iq_fb;
    float vq_ff =   motor.omega_e * motor.Ld * id_fb + motor.omega_e * motor.psi_f;

    // 更新两个电流PI控制器
    float vd_ref = PI_Controller_Update(&id_pi, id_error, vd_ff);
    float vq_ref = PI_Controller_Update(&iq_pi, iq_error, vq_ff);

    // ... 后续进行逆Park变换和SVPWM ...
}

至此，我们已经完整地构建了一个专业的、基于模型的、鲁棒的PID控制系统。它不再是“黑盒”，而是每一个参数、每一行代码都有据可依的精密工程。这是通往高性能电机控制的必经之路，也是最坚实的一步。

第四章：最优控制入门——LQR控制器设计

当我们已经将基于模型的PID+前馈技术运用到极致后，可能会遇到一些新的挑战：例如，如何在系统的响应速度和控制能量（即电流消耗）之间找到一个“最优”的平衡？如何协调多个相互耦合的变量？线性二次调节器（Linear-Quadratic Regulator, LQR）为这类最优化问题提供了一个优雅而强大的数学框架。

4.1 LQR的适用场景：何时以及为何要从PID升级到LQR？

PID是一个单输入单输出（SISO）控制器，我们为速度环、d轴电流环、q轴电流环分别设计了独立的控制器。但我们知道，d/q轴之间存在速度耦合项，它们并非完全独立。虽然我们用前馈进行了补偿，但这是一种解耦的思路。

LQR则是一个多输入多输出（MIMO）控制器，它可以将整个系统视为一个整体进行优化。LQR的优势在于：

1. 原生处理多变量耦合：LQR直接处理多变量的状态空间模型，天然地考虑了变量间的耦合效应，无需手动设计解耦补偿。
2. 性能与能耗的量化权衡：LQR通过代价函数（Cost Function）让我们能够明确地、量化地指定对系统状态误差和控制能量消耗的重视程度，从而在性能和能耗之间找到最佳平衡点。
3. 保证闭环稳定性：只要系统的状态空间模型是可控的，并且代价函数中的权重矩阵是正定的，LQR理论上保证能找到一个使闭环系统稳定的唯一最优反馈控制器。
4. 设计流程系统化：相比PID需要分别整定多个参数，LQR的设计过程更为系统化，主要是对权重矩阵Q和R的调整。

因此，当应用场景对能效、多变量协调控制有极高要求时（如高精度伺服系统、电池供电的机器人），LQR是一个值得考虑的升级选项。

4.2 LQR理论精要

LQR的核心是解决以下两个问题：

1. 系统描述：如何用状态空间模型来描述我们的PMSM？
2. 优化目标：如何定义一个“最优”的控制目标？

1. 构建PMSM的线性状态空间模型

状态空间模型用一组一阶微分方程来描述系统，其标准形式为：

其中，$\mathbf{x}$ 是系统的状态向量，$\mathbf{u}$ 是控制输入向量，$\mathbf{A}$ 是系统矩阵，$\mathbf{B}$ 是输入矩阵。

对于PMSM，我们可以选择状态向量为 $\mathbf{x} = [i_d, i_q]^T$，控制输入为 $\mathbf{u} = [v_d, v_q]^T$。 从dq轴电压方程出发（为简化，暂时忽略常数反电动势项 $\omega_e\psi_f$，它可作为前馈项处理）：

整理成 $\dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u}$ 的形式，可得：

这样，我们就得到了描述电机电流动态的A矩阵和B矩阵。

2. 定义代价函数 J

LQR的目标是找到一个控制律 $\mathbf{u}(t)$，使得从当前到未来无穷时间的代价函数 $J$ 最小。

这个公式是LQR的灵魂，让我们直观地解释它：

• $\mathbf{x}^T \mathbf{Q} \mathbf{x}$：状态惩罚项。它衡量了系统状态（如$i_d, i_q$）偏离零点（即目标值）的程度。$\mathbf{Q}$ 是一个半正定的状态权重矩阵。$\mathbf{Q}$ 矩阵中的元素越大，意味着我们越不希望对应的状态变量出现偏差，希望它能更快地回到零点。
• $\mathbf{u}^T \mathbf{R} \mathbf{u}$：控制能量惩罚项。它衡量了控制输入（如$v_d, v_q$）的大小，代表了控制所付出的“代价”或消耗的能量。$\mathbf{R}$ 是一个正定的控制权重矩阵。$\mathbf{R}$ 矩阵中的元素越大，意味着我们越不希望使用过大的控制输入，系统会变得更“温和”，更节省能量。

LQR的求解依赖于代数黎卡提方程（Algebraic Riccati Equation, ARE），通过求解该方程可以得到唯一的、能使闭环系统稳定且性能指标最优的反馈增益。

Q和R的物理意义与调参技巧

$\mathbf{Q}$ 和 $\mathbf{R}$ 通常选择为对角矩阵，这使得调参过程更直观：

• $q_{id}, q_{iq}$：分别代表对d轴和q轴电流误差的“惩罚”力度。如果$q_{iq}$远大于$q_{id}$，控制器会优先保证$i_q$的跟踪精度。
• $r_{vd}, r_{vq}$：分别代表对d轴和q轴电压消耗的“惩罚”力度。
• 核心权衡：Q/R的比值决定了系统的响应速度和控制能量消耗之间的平衡。
  • 增大Q或减小R：意味着我们更关心状态误差，不惜消耗更多能量也要让系统快速响应。系统会变得更激进，响应更快。
  • 减小Q或增大R：意味着我们更关心节能，容忍状态误差存在更长时间。系统会变得更保守，响应更平缓。

4.3 LQR设计工作流

LQR的强大之处在于，其求解过程有标准化的数学方法。给定$\mathbf{A, B, Q, R}$，控制器设计分为离线设计和在线实现两步。

1. 离线设计：求解反馈增益矩阵K

LQR理论证明，使代价函数$J$最小的最优控制律是一个线性的状态反馈：

其中，反馈增益矩阵 $\mathbf{K}$ 的计算公式为：

而矩阵 $\mathbf{P}$ 是通过求解**代数黎卡提方程（Algebraic Riccati Equation, ARE）**得到的：

手动求解ARE非常复杂，但在MATLAB或Python中，这只是一行代码的事。

MATLAB/Python脚本示例：

%% LQR Controller Design for PMSM Current Loop
clc; clear; close all;

% 1. 电机参数 (来自第二章的辨识结果)
Rs = 0.5;      % Stator Resistance
Ld = 0.001;    % d-axis Inductance
Lq = 0.0015;   % q-axis Inductance
omega_e = 100; % Electrical speed (rad/s)

% 2. 状态空间模型
A = [-Rs/Ld,  omega_e*Lq/Ld;
     -omega_e*Ld/Lq, -Rs/Lq];
B = [1/Ld, 0;
     0,    1/Lq];

% 3. LQR权重矩阵 (设计的核心)
% 我们更关心Iq的快速跟踪，并且希望节省控制能量
Q = [1,   0;      % 惩罚id误差
     0, 100];    % 重点惩罚iq误差
R = [1, 0;      % 惩罚vd使用
     0, 1];    % 惩罚vq使用

% 4. 求解LQR增益矩阵 K
% 在MATLAB中，使用 `lqr` 函数
[K, S, e] = lqr(A, B, Q, R);

% 在Python (control库)中，使用 `control.lqr`
# import control as ct
# K, S, e = ct.lqr(A, B, Q, R)

disp('Calculated LQR Gain Matrix K:');
disp(K);
% K 是一个 2x2 的矩阵，如 [k11, k12; k21, k22]

这个离线计算出的矩阵 K 是一个包含了所有电机参数和我们性能偏好的“浓缩精华”。

2. 在线实现：轻量级控制律

在线实现异常简单！我们只需要将离线算好的矩阵K烧录到MCU中，然后在每个控制周期执行一次矩阵乘法。

C代码示例：

// LQR反馈增益矩阵K (从MATLAB计算得到)
const float K[2][2] = {
    {k11, k12}, // K_11, K_12
    {k21, k22}  // K_21, K_22
};

// 当前状态x (误差)
float x[2]; // x[0] = id_ref - id_fb; x[1] = iq_ref - iq_fb;

// 控制输入u
float u[2]; // u[0] = vd; u[1] = vq;

// --- 在FOC控制循环中 ---
void LQR_Current_Controller_Update(void) {
    // 1. 获取状态误差向量 x
    x[0] = id_ref - id_fb;
    x[1] = iq_ref - iq_fb;

    // 2. 计算控制律 u = -Kx
    u[0] = -(K[0][0] * x[0] + K[0][1] * x[1]);
    u[1] = -(K[1][0] * x[0] + K[1][1] * x[1]);
    
    // 3. (重要!) 添加前馈补偿
    // LQR只负责反馈调节，前馈项仍然需要
    float vd_ff = - motor.omega_e * motor.Lq * iq_fb;
    float vq_ff =   motor.omega_e * motor.Ld * id_fb + motor.omega_e * motor.psi_f;
    
    vd_ref = u[0] + vd_ff;
    vq_ref = u[1] + vq_ff;

    // ... 后续进行饱和限制、逆Park变换和SVPWM ...
}

LQR的在线计算量非常小，仅仅是几次乘法和加法，对于现代MCU来说毫无压力。

4.4 LQR的实践挑战：状态观测器

【提醒】 标准LQR理论 u = -Kx 假设所有的状态变量 x 都是可以精确测量的。但在实际系统中，这往往是一个奢望。

在我们的例子中，状态是$i_d$和$i_q$，它们可以通过电流传感器和坐标变换得到，所以问题不大。但如果我们想用LQR来设计速度环，状态向量可能会包含角加速度和负载转矩等无法直接测量的量。

这时，就需要引入状态观测器（State Observer）。观测器的作用就像一个“虚拟传感器”，它利用已知的系统模型和可测量的输入输出（如$v_q, \omega_m$），来估计出那些不可测量的状态。

常见的观测器有：

• 龙伯格（Luenberger）观测器：一种结构相对简单的观测器。
• 卡尔曼滤波器（Kalman Filter）：一种更强大的最优状态估计算法，它不仅能估计状态，还能有效地滤除测量噪声和过程噪声。

将LQR控制器和状态观测器（如卡尔曼滤波器）结合起来，就构成了**LQG（Linear-Quadratic-Gaussian）**控制，这是现代控制理论中一个非常经典和完整的框架。引入观测器会增加算法的复杂度和计算量，但这通常是让高级控制理论在现实世界中落地的必要一步。

第五章：超越物理传感——观测器、滤波器与无感FOC技术

至此，我们所有的讨论都建立在一个前提之上：我们拥有一颗精确的位置传感器（如编码器）来获取转子的角度和速度。然而，在许多应用中，出于成本、体积、可靠性的考虑，我们希望去掉这个物理传感器，实现所谓的“无传感器控制”（Sensorless FOC）。

这可能吗？答案是肯定的。其背后的核心技术，就是本章的主角——状态观测器。我们将深入讲解观测器的思想，并探讨几种在无感FOC中主流的实现方法。

5.1 为何需要观测器？——“虚拟传感器”的诞生

观测器（Observer）是一种利用系统模型和可测量的输入/输出信号，来估计系统内部不可测量的状态变量的算法。它本质上是一个运行在MCU中的、与实际物理系统并行的“软件模型”。

上图展示了观测器的基本思想：

1. 并行仿真：观测器内部有一个与真实电机完全相同的数学模型。它接收与真实电机完全相同的控制输入（如$V_{\alpha}, V_{\beta}$）。
2. 预测输出：基于内部状态 $\hat{\mathbf{x}}$，观测器预测出系统应有的输出 $\hat{\mathbf{y}}$（如$\hat{I}_{\alpha}, \hat{I}_{\beta}$）。
3. 比较与修正：将观测器的预测输出 $\hat{\mathbf{y}}$ 与传感器的真实测量输出 $\mathbf{y}$（这里是电流传感器的值）进行比较，得到预测误差 $\mathbf{e} = \mathbf{y} - \hat{\mathbf{y}}$。
4. 负反馈校正：这个误差就像PID控制器中的误差一样，通过一个增益矩阵L进行放大，形成一个修正项，反馈到观测器模型的微分方程中，强迫观测器的状态 $\hat{\mathbf{x}}$ 向真实状态收敛。

通过这个闭环修正过程，观测器的内部状态（如估计的反电动势、估计的角度和速度）就能非常精确地跟踪真实电机的状态。因此，观测器就成了一个强大的**“虚拟传感器”**。

5.2 无感FOC的核心：基于反电动势（BEMF）的观测

无感控制的关键在于，如何从可测量的电压和电流中，提取出与转子位置和速度强相关的那个“神秘信号”？答案就是反电动势（Back Electromotive Force, BEMF）。

回顾第一章的dq轴电压方程，其中包含项 $\omega_e \psi_f$。在$\alpha\beta$静止坐标系下，反电动势BEMF是一个向量 $\vec{E}$：

这个方程揭示了天大的秘密：

1. BEMF的幅值 $|\vec{E}| = \sqrt{E_\alpha^2 + E_\beta^2} = \omega_e \psi_f$，它与电角速度 $\omega_e$ 成正比。
2. BEMF的相位 包含了 $\sin(\theta_e)$ 和 $\cos(\theta_e)$，它直接暴露了转子的电角度 $\theta_e$！

如果我们能通过观测器精确地估计出 $E_\alpha$ 和 $E_\beta$，我们就能通过反正切函数计算出角度：

并通过幅值计算出速度：

至此，无感FOC的技术路线图已经清晰：设计一个观测器来估计BEMF，然后从BEMF中解码出角度和速度。

5.3 主流无感算法详解（重点）

5.3.1 滑模观测器（Sliding Mode Observer, SMO）

SMO是目前工业界应用最广泛、最鲁棒的无感算法之一。

• 核心思想：滑模控制本身是一种非线性控制方法，其特点是“简单粗暴”但极其有效。它不追求让误差平滑地趋近于零，而是通过一个高速切换的控制律（类似Bang-Bang控制），强行将系统状态“按”在一个预设的“滑模面”上。一旦系统状态到达滑模面，就会沿着它稳定地滑向目标点。

• 在FOC中的应用：

  1. 建立电流模型：在$\alpha\beta$坐标系下，电机的电流方程为： $$\frac{d}{dt}\begin{bmatrix} i_\alpha \\ i_\beta \end{bmatrix} = -\frac{R_s}{L_s}\begin{bmatrix} i_\alpha \\ i_\beta \end{bmatrix} + \frac{1}{L_s}\begin{bmatrix} v_\alpha \\ v_\beta \end{bmatrix} - \frac{1}{L_s}\begin{bmatrix} E_\alpha \\ E_\beta \end{bmatrix}$$
  2. 构造滑模面：定义电流误差 $\tilde{i}_{\alpha\beta} = \hat{i}_{\alpha\beta} - i_{\alpha\beta}$，其中 $\hat{i}$ 是观测值，$i$ 是真实测量值。滑模面就定义为这个电流误差 $S = \tilde{i}_{\alpha\beta} = 0$。
  3. 设计滑模控制律：观测器的核心方程如下，注意其中的sign函数： $$\frac{d}{dt}\begin{bmatrix} \hat{i}_\alpha \\ \hat{i}_\beta \end{bmatrix} = -\frac{R_s}{L_s}\begin{bmatrix} \hat{i}_\alpha \\ \hat{i}_\beta \end{bmatrix} + \frac{1}{L_s}\begin{bmatrix} v_\alpha \\ v_\beta \end{bmatrix} - \frac{1}{L_s} \cdot k \cdot \text{sign}(\tilde{i}_{\alpha\beta})$$ 这里的 $k \cdot \text{sign}(\tilde{i}_{\alpha\beta})$ 就是滑模控制项。sign函数是一个符号函数（正数为1，负数为-1），它产生一个剧烈切换的控制作用。理论上，这个切换项的等效输出会逼近真实的反电动势BEMF。
  4. 提取BEMF：sign函数的输出是一个高频的方波状信号，包含了大量噪声（即“抖振”）。为了提取出平滑的BEMF估计值，因此，我们不能直接用它，而是将其通过一个低通滤波器（Low-Pass Filter, LPF），滤波后的输出就是我们对BEMF的估计值 $\hat{E}_{\alpha\beta}$。

• SMO的优缺点：

  • 优点：对参数变化不敏感，鲁棒性极强，算法实现相对简单。
  • 缺点：存在固有的“抖振（Chattering）”问题，即sign函数的高频切换会导致估计出的BEMF含有纹波，需要仔细设计LPF来平衡滤波效果和相位延迟。

• C伪代码实现思路:

  c

  // SMO 伪代码
  // error_i_alpha = i_alpha_estimated - i_alpha_measured;
  // sign_out_alpha = (error_i_alpha > 0) ? 1.0f : -1.0f;
  // smo_output_alpha = smo_gain * sign_out_alpha;
  
  // 更新估算的电流 (省略了其他项)
  // di_alpha_dt = ... - (1/Ls) * smo_output_alpha;
  // i_alpha_estimated += di_alpha_dt * Ts;
  
  // 将滑模输出通过低通滤波器，得到BEMF估计值
  // E_alpha_estimated = LPF_Update(&bemf_lpf_alpha, smo_output_alpha);
  // E_beta_estimated = LPF_Update(&bemf_lpf_beta, smo_output_beta);
  
  // 从BEMF中解码角度和速度
  // angle = atan2(-E_alpha_estimated, E_beta_estimated);
  // speed = sqrt(E_alpha_estimated^2 + E_beta_estimated^2) / PSI_F;
  

5.3.2 龙伯格观测器（Luenberger Observer）

龙伯格观测器是一种线性的状态观测器，相比SMO，它更“温和”，没有抖振问题。

• 核心思想：它将观测器误差的修正项设计为与误差本身成线性比例关系，而不是像SMO那样使用非线性的sign函数。

  $$\frac{d\hat{\mathbf{x}}}{dt} = \mathbf{A}\hat{\mathbf{x}} + \mathbf{B}\mathbf{u} + \mathbf{L}(\mathbf{y} - \hat{\mathbf{y}})$$

  这里的 $\mathbf{L}$ 就是观测器增益矩阵。设计的核心就是如何选择合适的 $\mathbf{L}$，使得误差动态 $(\dot{\mathbf{e}} = (\mathbf{A-LC})\mathbf{e})$ 的极点位于左半平面且收敛速度足够快。这个设计过程可以通过极点配置来完成，与我们设计PID控制器的思想非常相似。

• 优点：输出平滑，没有抖振，理论分析更成熟。

• 缺点：对电机参数 ($R_s, L_s$) 的准确性依赖较高，参数失配时性能会下降。

5.3.3 模型参考自适应系统（MRAS）

MRAS是另一种非常流行的无感方案。

• 核心思想：它包含两个模型。

  1. 参考模型：不包含待估计参数（如速度）的电机模型。通常是电机的电压模型。
  2. 可调模型：包含待估计参数的电机模型。通常是电机的电流模型。 这两个模型都接收相同的输入。MRAS的目标是，通过一个自适应律，不断调整可调模型中的参数（即估计的速度 $\hat{\omega}_e$），使得两个模型的输出误差趋近于零。

  常用的Popov超稳定性理论可以推导出这个自适应律，通常形式为一个PI控制器。也就是说，MRAS的速度估计算法，最终可以简化为一个结构简单的PI控制器，其输入是两个模型输出的误差。

• 优点：结构清晰，物理意义明确，实现形式可以简化为一个PI控制器，易于理解和调试。

• 缺点：同样对电机参数敏感。

5.4 无感控制的“阿喀琉斯之踵”

所有基于BEMF的无感方案，都有一个共同的、致命的弱点： 在零速和极低速区域，BEMF非常微弱，甚至为零！ 因为 $|\vec{E}| = \omega_e \psi_f$，当 $\omega_e \to 0$ 时，BEMF会淹没在噪声和电压测量的误差中，无法被可靠地观测。这就是为什么大多数无感FOC系统在启动和极低速运行时性能不佳，甚至无法稳定运行。

5.5 低速与静止时的解决方案：高频注入法（HFI）

为了解决低速问题，工程师们开发了不依赖BEMF的高频注入法。

• 核心思想：利用PMSM的凸极特性。即使是表贴式电机，由于磁路饱和等原因，其电感在空间上也并非完全均匀，存在微小的差异，这种差异称为“凸极效应”。

• 其机理如下：

  1. 注入高频信号：在估算的d轴方向注入一个高频的正弦电压信号。该信号频率远高于电机转速，不产生平均转矩。
  2. 测量电流响应：由于电机存在凸极性（$L_d \neq L_q$），定子电感会随转子位置变化。注入的高频电压所产生的高频电流，其幅值会受到当前位置电感的影响。
  3. 解调位置误差：通过对响应电流进行解调，可以提取出一个与估计位置和真实位置之间的误差相关的信号。当估计位置准确时，该误差信号为零。
  4. 锁相环跟踪：将此位置误差信号输入一个锁相环（PLL），即可持续地跟踪并估计出转子的精确位置。

  HFI在d轴方向（或一个估计的d轴方向）注入一个高频的正弦电压或方波信号。这个信号本身不产生平均转矩。

  • 如果注入方向与真实的d轴（磁阻最小的方向）一致，那么响应电流中的高频分量幅值最大。
  • 如果注入方向与真实的q轴（磁阻最大的方向）一致，那么响应电流中的高频分量幅值最小。

  通过解调响应电流中的高频分量，就可以得到一个与转子位置误差相关的信号，然后通过一个锁相环（PLL）或观测器来跟踪这个误差信号，从而估计出转子的真实位置。

• HFI的优缺点：

  • 优点：能在零速和极低速下精确估计转子位置，弥补了BEMF法的盲区。
  • 缺点：会引入额外的电流谐波，产生轻微的噪声和振动；算法实现更复杂；需要电机具有一定的凸极效应。