在當今的自動化、新能源汽車和智能機器人領域，無刷直流電機（BLDC）和永磁同步電機（PMSM）因其高效率、高功率密度而成為絕對的主流。
其中，人形機器人的電機控制系統是“肌肉系統”與“運動神經中樞”，直接決定了機器人運動的響應速度、定位精度、力量柔順性、動態穩定性與能效。它遠不止是“讓電機轉起來”那么簡單，而是一套融合了精密電力電子、先進控制算法、實時通信和高可靠性設計的復雜工程。
前 言
人形機器人的電機控制系統大致可以分為三層：執行層、協調層和決策層。其中，執行層是整個控制系統最底層、最核心的部分，其核心任務是在極短的時間尺度內（通常微秒級）精確控制電機的電流（即轉矩）。
圖1 – 執行層框圖
其中，整個執行層最核心的就是FOC，一個典型的FOC控制環路包含三個嵌套的閉環：電流環，速度環和位置環。
圖2 – FOC算法框圖
今天，我們將重點聊聊執行層，以英飛凌XMC系列MCU為例，拋開繁瑣的寄存器配置，帶大家深入了解高性能的FOC控制，全方位拆解其FOC算法的核心實現細節，看看英飛凌是如何將復雜的數學理論與MCU硬件特性完美結合的。
圖3 - PMSM FOC 電機控制框圖
一、硬件鏈路
上面的框圖把整個硬件系統分成了三個部分：
硬件：PWM單元、ADC、位置接口、數學協處理器。
外設：逆變器、電機、分流電阻、轉子傳感器。
軟件：FOC算法、SVM調制、電流計算、PLL估算器。
1. 前向控制路徑（參考輸入→電機驅動）
輸入：Ref_Speed（參考轉速）和 ω_REF（參考角速度）作為FOC算法的輸入。
FOC模塊：核心算法單元，根據參考值和反饋信號，輸出：
θ (轉子電氣角度，用于Park變換)；
SVM Modulator（空間矢量調制器）：將θ和|V_ref|轉換為三相PWM占空比信號。
CCU8 PWM Unit（PWM生成單元）：生成PWM波形，驅動3-Phase 2-Level Voltage Source Inverter（三相兩電平電壓源逆變器）。
逆變器→電機：逆變器將直流母線電壓Vdc轉換為三相交流電壓，驅動電機（Motor）的u、v、w三相繞組。
2. 電流檢測與反饋路徑（電機→FOC）
電流采樣：逆變器通過1x R電流采樣（單分流）或3x R電流采樣（三分流）檢測電機相電流Iu、Iv、Iw。
ADC（模數轉換器）：將模擬電流信號轉換為數字信號。
Current Calculation（電流計算）：對數字電流進行Clarke變換，得到兩相靜止坐標系電流Iα、Iβ。
3. 轉子位置/速度反饋路徑
① 無感反饋 (Sensorless Feedbacks)
PLL Estimator（鎖相環估算器）：核心無感觀測器，利用Iα、Iβ和θ（來自FOC的初始角度），通過數學計算（MATH模塊，如XMC1300的CORDIC或XMC4400的FPU）估算：
φ（磁鏈角度，用于Park變換）；
ω（轉子轉速）。
輸出：φ和ω反饋至FOC模塊，形成閉環控制。
② 傳感器反饋（Angle and Speed feedbacks from Hall Sensors/Encoder/Resolver）
Rotor Sensor（轉子傳感器）：如霍爾傳感器、編碼器，直接檢測轉子機械位置。
POSIF（Position Interface）：硬件位置接口，處理傳感器信號（如霍爾編碼）。
CCU4（定時器）：配合POSIF計算轉子角度和轉速。
Angle & Speed Calculation（角度/速度計算）：通過MATH模塊將傳感器信號轉換為θ（角度）和ω（轉速），反饋至FOC模塊。
二、FOC算法核心：坐標變換與磁場定向
FOC的本質，是通過一系列的數學變換，將三相靜止坐標系下難以直接控制的交流量，轉化為旋轉坐標系下類似于直流電機控制的直流量。
圖4 - 算法流程框圖
1. Clarke與Park變換的物理意義
FOC的第一步是獲取反饋電流。通過讀取三相電流（Iu, Iv, Iw），執行Clarke變換，將其投影到兩相靜止坐標系（α-β軸）：
Iα=IuIβ=31(Iu+2Iv)
此時，電流仍然是隨時間變化的交流量。為了實現類似直流電機的控制，必須引入Park變換，利用估算出的轉子角度（?），將α-β軸投影到隨轉子同步旋轉的d-q坐標系上：
Id=Iαcos(θ)+Iβsin(θ)Iq=?Iαsin(θ)+Iβcos(θ)
在d-q坐標系下，代表勵磁電流，控制磁通的大小（通常我們希望它為0以最大化效率），代表轉矩電流，直接決定了電機的輸出扭矩。至此，交流電機被“直流化”了。
其中，XMC1300/XMC1400通過CORDIC協處理器，XMC4400/XMC4800通過FPU實現，輸出左移14位（Q31格式）以適配硬件輸入。
三、PI參數整定與
dq解耦算法
控制系統的靈魂在于PI參數的整定。PI參數決定了FOC系統的穩定性（是否振蕩）、動態響應（是否快速）、穩態精度（是否準確）三大核心性能。
在FOC中，電流環的PI參數直接影響轉矩控制，速度環的PI參數直接影響轉速控制，兩者共同決定了電機的控制品質（如加速性能、抗負載能力、定位精度）。
1. PI控制器的基本作用：比例（P）與積分（I）的分工
PI控制器是FOC的核心調節器，其數學表達式為：
其中，e(t)e(t)是誤差信號（如參考電流與實際電流的差值），Kp是比例增益，Ki是積分增益。
比例項 (P）：快速響應誤差，決定系統的動態響應速度（如電流/轉速變化的快慢）。
積分項 (I）：消除穩態誤差（如負載變化后轉速是否回到設定值），決定系統的穩態精度。
2. OC雙閉環結構中PI參數的關鍵作用
FOC采用速度外環+電流內環的級聯控制，PI參數在兩個環中承擔不同但同樣重要的角色：
2.1 電流內環：轉矩控制的“神經中樞”
電流環直接控制電機的轉矩（Iq）和磁通（Id），是FOC的“內層執行器”。
Kp（比例增益）：決定電流響應的速度。Kp越大，電流跟蹤參考值的速度越快，但過大會導致超調（電流超過設定值）甚至振蕩（系統不穩定）。
Ki（積分增益）：消除穩態誤差（如負載突變后，電流是否精確等于設定值）。Ki過小會導致穩態誤差（如電機帶載后轉速下降），過大則可能引起積分飽和（積分項累積過大，導致響應延遲）。
2.2 速度外環：轉速控制的“大腦”
速度環根據參考轉速（Ref_Speed）與實際轉速（由PLL估算器或傳感器提供）的誤差，輸出轉矩電流參考值（I_q_{ref}），是FOC的“上層決策器”。
Kp（比例增益）：決定轉速跟蹤的動態性能（如轉速變化的快慢）。Kp越大，轉速響應越快，但過大會導致超調（轉速超過設定值）甚至振蕩（如電機啟動時轉速波動）。
Ki（積分增益）：消除穩態轉速誤差（如參考轉速為1000 RPM，實際是否精確達到）。Ki過小會導致轉速穩態誤差（如負載增加后轉速下降），過大則可能引起積分飽和（如長時間過載后，積分項累積過大，導致轉速無法恢復）。
3. PI參數與電機模型的強關聯：零極點對消
FOC的PI參數并非隨意設置，而是基于電機模型（電感L、電阻R）整定，核心方法是零極點對消（Pole-Zero Cancellation）：
通過配置Kp和Ki，使控制器的零點抵消電機的極點（Ls+R），將系統簡化為一階低通濾波器，極大提升穩定性。
ωc是截止頻率（經驗值設為最大電氣轉速的3倍）
Ts是PWM周期
4. 高速dq軸解耦
在高速運轉時，d軸和q軸電流之間存在強烈的交叉耦合（反電動勢擾動）。如果不加補償，PI控制器很難穩定。XMC內置了前饋解耦算法：
Vq=Vqpi?ωLqIqVd=Vdpi+ωLdId
通過在PI輸出基礎上疊加與轉速和電感相關的補償量，算法徹底解除了兩軸之間的干擾，使得高速下的動態性能依然如同獨立控制一般精準。
四、反饋核心：
PLL估算器算法
在FOC閉環運行期間，轉子的位置和速度反饋完全依賴于英飛凌的專利IP——PLL Estimator（鎖相環估算器）。
與傳統的滑模觀測器（SMO）相比，英飛凌使用的PLL估算器有以下極具吸引力的特點：
僅需一個電機參數：定子電感L。
兩步CORDIC/FPU流水線：通過分步計算（電流相位差→電壓角度誤差），將復雜三角函數拆解為可并行處理的CORDIC/FPU任務，既保證精度（角度誤差<0.1°），又避免CPU過載。
動態角度標定：16+USER_RES_INC位精度，轉子角度采用16+USER_RES_INC位（默認+3位，共19位）表示，提升分辨率（360°/2^19 ≈ 0.00068°），滿足高速電機（如10,000 RPM）的精準控制需求。
具體的算法流程如下：
第一步：電流-電壓相位差計算（PLL_Imag）
輸入：電壓空間矢量角度（Vref_AngleQ31）、靜止坐標系電流（I_Alpha_1Q31, I_Beta_1Q31）。
硬件加速：啟動CORDIC協處理器（旋轉模式），計算電流矢量與電壓矢量的相位差（Delta_IV）。
輸出：Delta_IV（反映電流與電壓的相位偏差）。
第二步：讀取電流幅值（PLL_Imag_GetResult）
作用：讀取CORDIC計算結果，獲取電流幅值（Current_I_Mag），為下一步提供參數。
第三步：電壓-角度誤差計算（PLL_Vref）
輸入：Delta_IV（上一步結果）、上一周期電壓幅值（Vref32）、PLL內部PI控制器輸出（PLL_UK）、定子電感（Phase_L）。
硬件加速：啟動CORDIC協處理器（旋轉模式），結合電機電感L，計算電壓矢量與轉子位置的誤差（VrefxSinDelta）。
輸出：VrefxSinDelta（反映轉子角度與電壓矢量的偏差）。
第四步：讀取電壓-角度誤差（PLL_Vref_GetResult）
作用：讀取CORDIC結果，獲取角度誤差信號。
第五步：輸出角度與轉速（PLL_GetPosSpd）
輸入：PLL_Estimator結構體指針。
作用：內部PI控制器根據角度誤差調節，最終輸出：
RotorAngleQ31：轉子電氣角度（16+USER_RES_INC位，默認19位精度）。
RotorSpeed_In：轉子轉速（單位：度/秒）。
五、空間矢量調制 (SVM) 的百花齊放
FOC的PI控制器輸出的是d軸和q軸電壓，需要通過逆Park變換和空間矢量調制（SVM）轉化為實際驅動逆變橋的PWM信號。英飛凌的XMC系列在SVM算法上的實現堪稱教科書級別，支持多達5種調制模式。
1. 基礎7段與5段SVM
標準7段SVM通過合理安排零矢量（V0, V7）和有效矢量（V1-V6）的作用時間，使得輸出電壓更接近正弦波，諧波極小。5段SVM則去除了其中一個零矢量，降低了開關損耗，但紋波略大。
圖5 - 7段 VS 5段SVM
2. 為單分流而生：PZV（偽零矢量）與4段SVM
單分流采樣因其極低的BOM成本備受消費類和工業領域青睞。但單分流有個致命弱點：在某些扇區交界處，有效矢量作用時間太短（），導致ADC根本沒有足夠的時間去采樣相電流。
為了解決這個“采樣盲區”，文檔引入了PZV（Pseudo Zero Vector，偽零矢量）算法。當檢測到有效時間不足時，算法強制注入一個非真實的零矢量時間，人為拉長有效矢量的持續時間，確保ADC能在下橋臂導通時采到電流。
然而，注入PZV會犧牲輸出電壓的幅值，導致電機無法達到最高轉速。因此，算法又引入了4段SVM。當電機轉速較高（>75%最大轉速）且自然有效時間足夠時，自動切換回4段SVM（不注入），從而突破電壓限制，讓電機轉得更快。這種根據工況動態切換調制策略的算法，是高級電機控制的精髓。
圖6 - PZV SVM
圖7 - 4段SVM
3. 過調制
當電機需要極高速運行，超過常規正弦調制的極限（86% ）時，算法啟動過調制模式。此時T1 + T2 > T_s，輸出電壓不再保持純正弦，而是逼近方波（六邊形磁鏈軌跡）。算法通過等比例縮減T1和T2，將其拉回六邊形邊界，最大化利用直流母線電壓，代價是引入了更多的諧波噪聲。
六、啟動算法：MET三步
啟動算法
無感FOC最大的痛點在于“零速啟動”。因為在靜止和低速時，反電動勢極小，PLL估算器無法準確捕捉轉子位置。如果強行注入電流，很容易導致電機反轉或失步。
XMC MCU采用了三步啟動法。這是一種從開環到閉環的平滑過渡機制。
V/f 開環爬坡：按斜坡提升電壓/頻率，電機低速啟動。
MET 最大效率跟蹤：當速度達到閾值（如500 RPM）時，狀態機切入MET狀態。調整定子磁通方向，確保與轉子磁通垂直（90°電角度）。
FOC閉環：當MET算法判定電機已穩定，狀態機無縫切換至FOC_CLOSED_LOOP，真正開始依靠PLL估算器反饋的角度進行磁場定向控制。
通過這種三步走策略，確保了無論是空載還是帶載，電機都能平滑啟動，并且在低速區不消耗過多無用電流，完美兼顧了啟動成功率與低速效率。
圖8 – 三步啟動
七、定點數算法的極致壓榨
在XMC1300這類不含FPU的芯片上，為了保證運算速度和精度，FOC算法全部采用定點數運算。這也是很多工程師覺得FOC代碼難懂的原因。其核心思想是：用整數表示物理量，并盡可能利用變量的動態范圍。
Q15格式：用于電流。最大可測電流被映射到0x7FFF（32767），這意味著即使是微小的電流變化，也能在整數域體現出明顯的差值。
Q31格式：用于角度。16位角度（0~65535代表0~360度）在送入CORDIC前，會被左移14位變成Q31格式，以滿足CORDIC硬件對輸入精度的苛刻要求。
動態Speed Scaling：由于電機轉速范圍極大，軟件通過公式 N = log_2(max_speed_integer) 動態計算速度的移位因子，使得無論電機最高轉速是1000 RPM還是10000 RPM，速度變量都能以最優分辨率存放在32位寄存器中，既不溢出，也不損失精度。
八、結語
從MET平滑啟動到僅需單參數的PLL估算器，從解決單分流盲區的PZV算法到精準的零極點對消PI整定，英飛凌XMC的這套PMSM FOC算法架構，為電機控制工程師提供了一個極具參考價值的高性能、低成本落地方案。
