2024-04-29 Share: 手把手教你馬達FOC控制

• https://www.robotsfan.com/posts/d99d1c1a.html
• 

• 三向電機，分別為UVW三向，角度互差120度。若使用BLDC控制方法，如下圖每次換向增加60度，轉子只能到達六個位置，所以六步換向時會有振動。使用FOC控制方法可以讓轉子到達任意角度，所以運作起來會更加平滑。
• 
• 如果想到達40度的位置，只需要在0度方向通電一段時間，在60度方向通電一段時間，再在空矢量的狀態下通電一段時間（全橋000或111的位置為空矢量，空矢量的長度用來調整扭力。若想到達其他角度，只需改變0度和60度的通電長度比例。
• 要讓磁場旋轉起來，就需要輸入正弦電壓，但我們輸入的是直流電，我們馬上想到可以使用PWM波。透過不斷改變PWM脈寬就可以模擬正弦電壓，體現在電流上則為正弦電流。
• 下圖為一個完整的FOC流程圖：
• 
• 我們先來看正向通路：輸入$I_{d_{r}ef}$和$I_{q_{r}ef}$與下文反饋通路採樣得到的電流$I_d$和$I_q$進行PID控制輸出$U_d$和$U_q$（輸入的$I_{d_{r}ef}$通常為0，$I_{q_{r}ef}$前通常還需要接入一個速度PID構成速度環），再透過反Park變換轉換成$U_{\alpha }$和$U_{\beta }$，透過SVPWM模組控制定時器產生六路互補的PWM訊號，最後使用PWM訊號控制全橋MOS管的通斷，產生三向電壓使馬達轉動。

再來看回授通路：透過採樣電阻來擷取任意兩相電流，根據基爾霍夫電流定律可以算出第三相電流，將三向電流經由Clark變換轉換成 和 ，再透過Park變換轉換成 和 ，作為回授傳入PID控制器構成電流環。

上圖的Park變換和反Park變換需要目前的角度作為輸入；速度PID需要速度作為回饋。所以需要獲得馬達的速度與角度。角度和速度的獲取方法分為有感和無感。有感方式使用霍爾元件（Hall Sensor），安裝在馬達上就可以偵測馬達磁鐵的位置。無感使用觀測器（observer）獲得角度速度訊息，本文將使用擴展卡爾曼濾波觀測器（EKF），輸入為 、 、 、 。使用無感方式不需要霍爾感測器，可以減少連接線的數量，也可以減少成本。

為什麼要使用座標變換？馬達控制大多是在控制速度/轉矩，需要用PID閉環控制正弦交流電壓的振幅和角度，不是很容易實現，所以透過座標變化把正弦交流資訊分解成角度資訊（Q軸控制轉矩）和幅值資訊（D軸控制磁場）單獨控制。

FOC的變換中要滿足等振幅變換，即變換前後振幅不變。

座標變換都分為正向變換和反向變換，正向變換都是對電流進行操作的，反向變換都是對電壓進行操作的。

以下的變換均以聯立和矩陣兩種形式表示，以方便使用。

Clark變換 Clark變換實現了三向座標系 與直角座標系 間的轉換。

7C602099D7A9549EDA9C24D13AB98072

正向Clark變換 Clark反變換則將兩相信號轉換為三相信號。已知三相座標系 ，這三個基底向量不是正交的，所以可以將其正交化為一個直角座標系，命名為 座標系，變換公式為：

表示為矩陣形式：

我們一般在電路中只採集兩相電流，第三相電流可以使用基爾霍夫電流定律來得出（ ），故上式也可整理為以下形式：

矩陣形式：

由於變換前後 和 幅值要相同，所以要進行等幅值變換，變換係數為

，即變為

矩陣形式：

（這裡的係數在後文SVPWM里相電壓的振幅與電壓空間向量之間有一個

的係數相抵消）

MATLAB實作為：

image-20220330103933845

反向Clark變換 反Clark變換則將三相信號轉換為兩相信號。根據圖可以寫出：

矩陣形式：

MATLAB實作為：

image-20220330103915885

Park變換 Park變換實現了兩相座標系 與轉子座標系 間的轉換，此變換可以將正弦變數線性化。其中 指向轉子中心， 指向切線方向， 是轉子當前的角度，也就是說 座標系始終跟著轉子同步旋轉。

A20BC4DCAD2986A86187F746C2EDE88A

正向Park變換 則根據上圖可以寫出

很明顯上述變換可以用旋轉矩陣來表示，使用矩陣形式可以很方便地寫出：

（如果 軸為0，則功率全部輸出在 軸上。 ）

MATLAB實作為

image-20220330104003922

反Park變換 根據上面的推導可以求反Park變換

同理，使用旋轉矩陣可以求反變換的係數矩陣：

MATLAB實作為

image-20220330103857619

MATLAB仿真 為了更清楚地仿真，這裡不用矩陣形式表示，如需矩陣形式可以看我寫的另一篇文章。

請注意，正向變換都是對電流進行操作的，反向變換都是對電壓進行操作的。但在這節的模擬中，把正變換和反變換連在一起，這樣做沒有實際意義，只是為了驗證變化演算法。

輸入Vd為0，Vq為1，角度為由0到2pi的連續值。

image-20220330103716603

再來看子模組內部，輸入經過兩個逆變換，再經過兩個正變換。

image-20220330103816443

運行查看波形（新版本MATLAB常值輸入為一個圓圈）

image-20220330104412589

SVPWM 根據BLDC的六步換向，可以將一圈分為六個扇區，前文FOC引入章節已經講過，只需要控制每個狀態通電的時間就可以控制轉子到達任意角度。這就是SVPWM。

SVPWM的輸入為 和 ，輸出為三向計數器的比較值。所以應該先判斷用哪兩個相鄰向量，然後計算兩個相鄰向量的作用時長，然後將作用時長轉換成計數器的比較數值送入定時器。以下對這三個步驟進行講解。

扇區判斷 三相電壓可以表示為（ 為電壓幅值）：

將其轉換為 座標系，可以算出

從這個式子發現，可以從中算出角度資訊從而可以判斷在哪個扇區

由於除法和反三角函數對MCU來說計算量比較大。我們來找一個簡單演算法。

是關於cos的三角函數， 是關於sin的三角函數，可以得到：

扇區 扇區 扇區 扇區 扇區 扇區

可以看到，透過 的正負可以判斷是1/2/3扇區還是4/5/6扇區。

這個條件只將扇區分為兩個部分，我們還需要幾個條件來更細緻地分。將每個磁區的反三角函數範圍計算出來：

扇區

扇區

扇區

扇區

扇區

扇區

觀察這個結論， 和 似乎有關係，回顧反Clark變換， 和 的式子就是這種關係。所以可以把上面的結論往反Clark變換上湊。看一下反Clark變換的圖像，注意我們需要的關係裡的 是乘在 上的，所以我們把 和 反一下，對應的公式為：

產生上述公式的影像，其中黃色的線為 為 ，藍色的線 為

，紅色的線 為

，

image-20220330145027064

可以看到在每個磁區總有一向大於0，兩向小於0，所以 和 的正負可以當作判斷條件之一。我們順便還又一次得到了 這個判斷條件。整理一下上面的式子

扇區

扇區

扇區

扇區

扇區

扇區

所以透過計算

和

的正負可以判斷出是1/6扇區，3/4扇區，2扇區，5扇區的哪一組。

綜上，我們的判斷條件有： 、

和

。我們分別定義：

綜合這三個條件就可以判斷在哪個扇區。那麼有沒有一種演算法可以將這一堆判斷數值化並轉換成1~6的數字呢？可以用下面的公式：

式中A代表 的正負，B代表 的正負，C代表 的正負，大於0為1，小於0為0。最後轉換出來的的N即為1~6的數字：

扇區

至此，我們成功完成了扇區判斷。

計算相鄰向量作用時長 控制相鄰向量作用時長就可以控制轉子到達任意方向，下面進行分析。

六個向量的大小 六個MOS管可以產生8種狀態

image-20220330152650462

設上開下合為0（電流從O往對應的向流），上合下開為1（電流從對應的嚮往O流），表示其中的六個向量。

image-20220330152634407

放在一張圖中即為：

image-20220330152741018

還有兩個零向量（000和111），無電流，不產生磁場。

對於100的狀態，可以等效為下面的電路圖：

image-20220330154344462

可以計算出馬達中三個相電壓（每相對於馬達中間連接點的電壓）

同理可以計算其他所有方向向量的相電壓，可以看出，六個向量的大小均為

，即SVPWM相電壓幅值為

電壓利用率 電壓利用率等於合成向量的電壓除以母線電壓。下面在複平面計算合成向量的電壓 ：

根據歐拉公式可以推導出：

又因為三相電壓與相電壓幅值之間的關係：

帶入可以計算出 ：

合成向量的電壓是相電壓幅值的

倍，而SVPWM相電壓幅值 為

，所以

即合成向量的電壓等於母線電壓。所以SVPWM的電壓利用率是100%。

SVPWM輸出電壓是馬鞍波 由於中間連接點N的點位是浮動的，為三角波，而相電壓是每相對於電機中間連接點N的電壓，所以相電壓不是一個正弦波，而是一個正弦波與一個三角波疊加而成的，即為馬鞍波。網圖：

d94c427b1d664ffe9b585d58e60482f6

向量作用時長 合成向量的電壓是所在磁區兩個向量與空向量不同時長的組合，其中 為空向量作用時長：

由於SVPWM的輸入是 和 ，但是要控制 和 ，所以要找出他們的對應關係。

對於第一個扇區，將 在 座標系中表示：

其中 和 根據前面的計算均為

，可以解出：

同理，可以計算出所有六個扇區：

第二區

第三區

第四扇區

第五扇區

第六扇區

六個扇區中都有相同的項，其中包含前文判斷扇區所用的 、 、 。直接把前面已經計算好的變數拿過來使用，大大減少了計算量。式中的

為調製比，定義：

可以將六個磁區表示為：

扇區 扇區 扇區 扇區 扇區 扇區

注意：當非零向量作用時間 ，需要進行過飽和處理：

定時器比較值計算 我們前文算出來的 和 以秒為單位，在單晶片中使用定時器控制MOS管的通斷需要配置比較值，所以需要把 和 轉換為三個互補的定時器比較值 、 和 。

定時器應設定為中心對齊模式，若為向上計數，計數器會從0開始計數到最大值，再反向從最大值計數到0；向下計數反之。故只需要控制前半個週期的比較值就可以產生相對中心對稱的PWM波。

CB0D861C81399F86BFB03E925F9AF8AC

為了減少MOS管的開關損耗，提高其使用壽命，應盡量減少MOS管的開關次數。在一個磁區內切換狀態的時候，合理使用零向量可以保證每次切換只改變一個MOS管。

先來看第一扇區，以每次只改變一個MOS管為原則，則切換順序為：

可以看到，切換順序構成了一個環路。在一個週期內我們需要控制三段作用時數：

六個MOS需要產生六路PWM來控制他們的狀態。由於上下半橋是互補的，所以只需要產生三個PWM：

image-20220331151522966

由於是中心對齊模式，所以只需要控制半個週期的時長

和

。在半個週期內 出現了兩次，分別為000和111，在半個週期內將這兩段時間平均分配。即為

。

這樣就可以計算出三個定時器的比較值 、 和 ：

同理，可以計算出全部六個磁區的切換順序，這裡計算過程不再贅述。由影像和計算結果可以分析出，無論在哪個扇區中，三個定時器切換的圖像都是由上圖中三個方波構成，三個圖像排列組合也正好就是六個扇區的組合方式。故為了方便程式運行，將六個磁區的定時器比較值歸納如下：

設

此時六個扇區可以表示為：

扇區 扇區 扇區 扇區 扇區 扇區

也就是說程式設計時只需要計算 、 和 ，透過判斷就可以得到對應扇區的比較值。

至此，SVPWM輸出比較值 、 、 ，互補得到六個比較值，輸入到定時器，輸出三路PWM。

MATLAB仿真 假設需要產生10Khz的PWM波，則一個週期為0.0001秒。若單晶片主頻為180Mhz，預分頻係數為100-1，由下方計算公式：（其中 是計數值， 是預分頻值）

主頻

180,000,000/(18,000*100)=10,000Hz，則定時器的計數值應設定為18000-1。

在模型中設定 為24， 為18000。

image-20220331163919487

在foc子模組中，將park反變換的輸出輸入到SVPWM模組：

image-20220331163836635

• SVPWM模組程式碼為：

  unction [T1,T2,T3,sector] = fcn(Ualpha,Ubeta,Udc,Tpwm)
  
  % 初始化
  sector = single(0);
  T1 = single(0);
  T2 = single(0);
  T3 = single(0);
  
  % 第一步：扇区判断
  % 计算三个临时变量
  Ua = Ubeta;
  Ub = (sqrt(3)*Ualpha - Ubeta)/2;
  Uc = (-sqrt(3)*Ualpha - Ubeta)/2;
  % 计算判断扇区所用的ABC的值
  A=single(0);B=single(0);C=single(0);N=single(0);
  if(Ua>0)
      A = single(1);
  elseif(Ua<0)
      A = single(0);
  end
  if(Ub>0)
      B = single(1);
  elseif(Ub<0)
      B = single(0);
  end
  if(Uc>0)
      C = single(1);
  elseif(Uc<0)
      C = single(0);
  end
  % 计算判断扇区所用的N的值
  N = A + 2*B + 4*C;
  % 扇区判断
  switch (N)   
      case 3
          sector = single(1);
      case 1
          sector = single(2);
      case 5
          sector = single(3);
      case 4
          sector = single(4);
      case 6
          sector = single(5);
      case 2
          sector = single(6);
  end
  
  % 第二步：计算相邻矢量作用时长
  % 计算调制比
  Umr = sqrt(3)*Tpwm/Udc;
  % 计算三个临时变量
  X = Umr * Ua;
  Y = Umr * Ub;
  Z = Umr * Uc;
  % 分扇区计算Tx和Ty的值
  Tx=single(0);Ty=single(0);
  switch (sector)
      case 1
          Tx = Y; Ty = X;
      case 2
          Tx = -Y; Ty = -Z;
      case 3
          Tx = X; Ty = Z;
      case 4
          Tx = -X; Ty = -Y;
      case 5
          Tx = Z; Ty = Y;
      case 6
          Tx = -Z; Ty = -X;
  end
  
  % 第三步：定时器比较值计算
  % 干啥用的？
  if Tx+Ty > Tpwm
      Tx = Tx/(Tx+Ty);
      Ty = Ty/(Tx+Ty);
  else
      Tx = Tx;
      Ty = Ty;
  end
  % 计算三个临时变量
  Ta = (Tpwm-Tx-Ty)/4.0;
  Tb = Ta+Tx/2.0;
  Tc = Tb+Ty/2.0;
  % 分扇区计算定时器比较值T1、T2和T3的值
  switch (sector)
      case 1
          T1 = Ta; T2 = Tb; T3 = Tc;
      case 2
          T1 = Tb; T2 = Ta; T3 = Tc;
      case 3
          T1 = Tc; T2 = Ta; T3 = Tb;
      case 4
          T1 = Tc; T2 = Tb; T3 = Ta;
      case 5
          T1 = Tb; T2 = Tc; T3 = Ta;
      case 6
          T1 = Ta; T2 = Tc; T3 = Tb;
  end
  
  end

• 可以看到，定時器比較值為馬鞍波

image-20220331165323898

扇區為從1到6的循環

image-20220331165348883

• 經過前文的反Park變換、SVPWM、定時器與全橋，輸入的$U_d$和$U_q$最終轉換為三相電壓輸出到馬達。如果需要讓馬達以設定的電流值運行，就需要使用PI控制器閉迴路控制電流。但三相交流電流是不太容易實現閉迴路控制的，所以我們選擇對直流電流$I_d$和$I_q$進行閉環控制。
• 
• PI控制器輸入目標參考電流$I_{d_{ref}}$和$I_{q_{ref}}$，輸出$U_d$和$U_q$，現在還缺少回饋量$I_d$和$I_q$，這就需要用到前文說到的Park與Clark正變換。安裝取樣電阻，以單晶片的ADC擷取全橋上任意兩相的電流，透過基爾霍夫電流定律可計算第三相的電流，透過Clark變換計算出$I_{\alpha }$和$I_{\beta }$，再透過Park變換計算出反饋量$I_d$和$I_q$。
• 對於PI控制器參數的設定，以下為一個參考值（但還是需要自行調整）：
• $$\begin{array}{cccc}& P& I& \\ d& K_{pd}=\alpha L_d& K_{Id}=\alpha R\\ q& K_{pq}=\alpha L_q& K_{Iq}=\alpha R\end{array}$$
• 其中$\alpha$的值為：（其中$\tau$為馬達的時間常數）
• $$\begin{gathered} \alpha =\frac{2\pi }{\tau } \\ \tau =min\{\frac{L_d}{R},\frac{L_q}{R}\} \end{gathered}$$
• 電機分為表貼電機和內嵌式電機，表貼式電機的永久磁鐵貼在轉子表面，內嵌式電機的永久磁鐵安裝在轉子內。我們常用的電機都是表貼電機，對於錶貼電機，$L_d=L_q=L$，則可以得到PI控制器的參考參數：
• $$\begin{gathered} K_p=2\pi R \\ {K}_I=2\pi \frac{R^2}{L} \end{gathered}$$

• 霍爾感測器分為60度和120度
• 
• 畫一下轉一圈三個霍爾感測器的波形
• 
• 可以看到，這兩種排列方式除了組合方式不同，其他都一樣。
• 單晶片透過輸入擷取擷取三路高低電平的跳變，透過對三路訊號進行異或，則轉一圈可以進6次中斷，進入中斷後偵測IO電平就可以知道馬達對應角度。但現在一圈只能取得6個固定的角度值，無法取得到如80度這個角度。這時可以對速度進行積分得到角度，速度的取得可以透過查詢60-120之間定時器的計數值來獲得。但是轉到80度的時候沒辦法取得到120度時候的定時器計數值就無法取得當前段的速度，這時可以用前段的速度近似為當前段的速度。

• 前面的有感部分，是在馬達上安裝霍爾感測器，引出三條線到單晶片透過輸入捕獲算出當前角度，再與預設角度完成角度閉環。
• 而無感控制則不需要霍爾感測器，只需要三根UVW線輸出到馬達即可，去掉了霍爾感測器和三根線，節省了成本；有些使用場合無法使用霍爾感測器，則需要使用無感，例如空調壓縮機，內部充滿了潤滑油，無法安裝霍爾感測器；有些時候有感演算法或硬體出現問題也可以切換至無感，防止系統閘機，例如汽車運行中霍爾感測器突然壞了，為了使整個系統正常運行，就可以臨時切換到無感。上述這些都是無感控制的優點。無感控制現在已經越來越普及，空調、洗衣機、高階風扇、汽車都在用無感控制。

• 觀測器有很多種，擴展卡爾曼濾波觀測器、滑膜觀測器、龍伯格觀測器、自適應觀測器、磁鏈觀測器等。其中擴充卡爾曼濾波觀測器的低速效能比較好，所以本文將重點放在EKF。
• 狀態觀測器實質上就是用數學方法建立一個可以模擬真實被控對象的模型，用這個模型得知一些無法透過測量得到的狀態量。對於馬達系統來說，如果沒有感測器去測量馬達的轉速和轉子位置，那麼就可以透過建造狀態觀測器來估算馬達的轉子和轉子位置，這就是基於狀態觀測器的馬達的無感測器控制。
• 但是使用這種方法建立的觀測器有著許多問題：
  • 1, 抗干擾能力差：加入擾動或負載，此時的狀態觀測器很難保持正確的響應而導致輸出錯誤或系統崩潰；
  • 2, 存在誤差：誤差主要存在與系統誤差及測量誤差
    • 2.1, 系統誤差：建模的時候馬達的參數不可能完全精確，在建模的時候造成誤差。
    • 2.2, 測量誤差：觀測器需要電流作為輸入，而電流的採集就會有誤差。
• 所以為了解決以上問題，需要為狀態觀測器增加回饋，透過回授來修正狀態觀測器的輸出，讓觀測器盡可能的去貼近真實的馬達。
• 

• 線性系統的狀態空間方程式為：
• $$\{\begin{array}{l}\dot{x}=Ax+Bu\\ y=Hx\end{array}$$
• 永磁同步馬達的方程式：
• $$\{\begin{array}{l}{U}_{\alpha }=R{i}_{\alpha }+L_s\frac{d{i}_{\alpha }}{dt}-{\omega }_e{\psi }_{f}sin\theta \\ U_{\beta }=R{i}_{\beta }+L_s\frac{d{i}_{\beta }}{dt}+{\omega }_e{\psi }_{f}cos\theta \end{array}$$
• 上述方程式的意義為：將每一相抽象化為一個電阻，一個電感和一個反電動勢的串聯，則每一相的電壓為前面三項壓降總和（反電動勢為速度乘磁鏈）。方程式中的電阻R、電感L、和磁鏈flux為馬達的固有參數，電流可以透過取樣電阻得到，只剩下了角度和轉速，將馬達方程式轉換為狀態空間表達：
• $$\{\begin{array}{l}\frac{d{i}_{\alpha }}{dt}=-\frac{R}{L_s}{i}_{\alpha }+\frac{1}{L_s}{\omega }_e{\psi }_{f}sin\theta +\frac{1}{L_s}{U}_{\alpha }\\ \frac{d{i}_{\beta }}{dt}=-\frac{R}{L_s}{i}_{\beta }-\frac{1}{L_s}{\omega }_e{\psi }_{f}cos\theta +\frac{1}{L_s}{U}_{\beta }\\ \frac{d{\omega }_e}{dt}=0\\ \frac{d\theta }{dt}={\omega }_e\end{array}$$

• 接下來我們將上面的式子整理成矩陣形式，先確認狀態變數分別為$\dot{i_{\alpha }},\dot{i_{\beta }},\dot{{\omega }_e},\dot{\theta }$，則有：
• $$\begin{array}{r}\dot{x}=\left[\begin{array}{c}\dot{i_{\alpha }}\\ \dot{i_{\beta }}\\ \dot{{\omega }_e}\\ \dot{\theta }\end{array}\right]x=\left[\begin{array}{c}{i}_{\alpha }\\ i_{\beta }\\ {\omega }_e\\ \theta \end{array}\right]\end{array}$$
• 輸入$u$，對於馬達來說就是電壓，我們選擇$\alpha -\beta$座標系：
• $$\begin{array}{r}u=\left[\begin{array}{c}{U}_{\alpha }\\ U_{\beta }\end{array}\right]\end{array}$$
• 輸出矩陣$y$為通過採樣電阻測得的電流：
• $$\begin{array}{r}y=\left[\begin{array}{c}{i}_{\alpha }\\ i_{\beta }\end{array}\right]\end{array}$$
• 則$H$和$B$矩陣就可以表示為：
• $$\begin{array}{r}H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]B=\left[\begin{array}{cc}\frac{1}{L_s}& 0\\ 0& \frac{1}{L_s}\\ 0& 0\\ 0& 0\end{array}\right]\end{array}$$
• 此時矩陣形式的表示為：
• $$\begin{array}{rl}\left[\begin{array}{c}{i}_{\alpha }\\ {\dot{i}}_{\beta }\\ \dot{\omega }\\ \dot{\theta }\end{array}\right]& =\left[\begin{array}{c}-\frac{R}{L_S}{i}_{\alpha }+\frac{1}{L_s}{\omega }_e{\psi }_f\sin \theta \\ -\frac{R}{L_S}{i}_{\beta }-\frac{1}{L_s}{\omega }_e{\psi }_f\cos \theta \\ 0\\ {\omega }_e\end{array}\right]+\left[\begin{array}{cc}\frac{1}{L_s}& 0\\ 0& \frac{1}{L_s}\\ 0& 0\\ 0& 0\end{array}\right]\left[\begin{array}{c}{u}_{\alpha }\\ u_{\beta }\end{array}\right]\\ \left[\begin{array}{c}{i}_{\alpha }\\ i_{\beta }\end{array}\right]& =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]\left[\begin{array}{c}{i}_{\alpha }\\ i_{\beta }\\ {\omega }_e\\ \theta \end{array}\right]\end{array}$$
• 對比狀態空間表示，無法明確表示出線性的$Ax$，因為馬達系統不是線性的，所以用$f(x)$這非線性項替代這一項。$f(x)$定義為：
• $$\begin{array}{r}f(x)=\left[\begin{array}{c}-\frac{R}{L_s}{i}_{\alpha }+\frac{1}{L_s}{\omega }_e{\psi }_{f}sin\theta \\ -\frac{R}{L_s}{i}_{\beta }-\frac{1}{L_s}{\omega }_e{\psi }_{f}cos\theta \\ 0\\ {\omega }_e\end{array}\right]\end{array}$$
• 則此時的狀態空間方程式變為：
• $$\{\begin{array}{l}\dot{x}=f(x)+Bu\\ y=Hx\end{array}$$

• 關於卡爾曼濾波器的知識請看這篇文章：手撕卡爾曼濾波器
• 擴充卡爾曼濾波器（Extend Kalman Filter），簡稱EKF。帶有“擴展”二字，是因為卡爾曼濾波器只能處理線性系統，而電機是一個非線性，強耦合的系統，使用擴展卡爾曼濾波器可以處理這種非線性系統。

前文的狀態空間方程中 是非線性的，如果想對非線性系統進行卡爾曼濾波，需要對其線性化（Linearization），泰勒級數展開是將非線性系統線性化的方法。將 泰勒級數展開並取前兩項：

對 求偏導：

此時狀態空間方程式變為：

狀態方程式是建立在連續系統的基礎上，需要將其轉換成離散化系統後，才能在微控制器中實現卡爾曼濾波狀態觀測器。在離散化系統中，

，則狀態空間方程式表示為：

令 ，擴展卡爾曼濾波器的流程為以下五個步驟：

預測

計算預估值

計算誤差協方差

校正

計算卡爾曼增益

修正預估值

更新誤差協方差，用於下次計算

上面的五個方程中， 為採樣時間， 為模型誤差， 為測量誤差

• 在MATLAB中建立EKF觀測器，需要建立全域變數x和P用於保存每個迭代的值給下一個迭代使用：

  function  x_posteriori   = fcn (Ialpha,Ibeta,Ualpha,Ubeta,Ls,Rs,Flux)
  
  global x; 
  global P;
  
  R=[ 0.02 , 0 ; 0 , 0.02 ]; 
  Q=[ 0.01 , 0 , 0 , 0 ; 0 , 0.01 , 0 , 0 ; 0 , 0 , 0.01 , 0 ; 0 , 0 , 0 , 0.010 , Time= 0 , Time ]; 0.0001 ;
  
  
  u=[Ualpha;Ubeta]; 
  y=[Ialpha;Ibeta]; 
  H=[ 1 , 0 , 0 , 0 ; 0 , 1 , 0 , 0 ]; 
  B=[ 1 /Ls, 0 ; 0 , 1 /Ls ; 0 , 0 ; 0 , 0 ]; 
  F=[-Rs/Ls, 0 ,Flux* sin (x( 4 , 1 ))/Ls,x( 3 , 1 )*Flux* cos (x( 4 , 1 ))/Ls; 0 ,-Rs/Ls,-Flux* cos (x( 4 , 1 ))/Ls,x( 3 , 1 )*Flux* sin (x( 4 , 1 ))/Ls; 0 , 0 , 0 , 0 ; 0 , 0 , 1 , 0 ]; 
  f=[-Rs*Ialpha/Ls+x( 3 , 1 )*Flux* sin (x( 4 , 1 ))/Ls;-Rs*Ibeta /Ls-x( 3 , 1 )*Flux* cos (x( 4 , 1 ))/Ls; 0 ;x( 3 , 1 )]; 
  % 計算預估值
  x_prior=x+(f+B*u)* Time; 
  % 計算誤差協方差
  A=( eye ( 4 )+F*Time); 
  P_prior=A*P*A'+Q; 
  % 計算卡爾曼增益
  K=(P_prior*H')/(H*P_prior* H'+R); 
  % 修正預估值
  x_posteriori=x_prior+K*(yH*x_prior); 
  % 更新誤差協方差
  P_posteriori=( eye ( 4 )-K*H)*P_prior;
  
  if x_posteriori( 4 , 1 )>( 2 * pi ) 
      x_posteriori( 4 , 1 )=x_posteriori( 4 , 1 )-( 2 * pi ); 
  end
  
  x=x_posteriori; 
  P=P_posteriori;

• 調整R矩陣和Q矩陣至最優，查看觀測器效果：
• 
• 藍色為觀測角度，黃色為真實角度。可以看到經過很短時間的迭代，觀測器就已經可以很好地預測馬達角度，至此完成了EKF無感演算法。

• 前文電流閉環精準地控制了馬達電流從而控制轉矩，但要精確地控制馬達的速度就需要進行速度閉環。速度閉環是在電流閉環的外環，控制Id=0，透過前面有感或無感獲取到的速度與速度參考作比較，透過PID控制Iq的值。
• 對於速度閉環PID，一班只是用PD控制。$K_p$和$K_d$的理論計算值為：
• $$\begin{gathered} K_P=\frac{\beta J}{1.5P{\psi }_f} \\ {K}_I=\beta K_P \end{gathered}$$
• 其中$\beta$是速度環的頻寬，一般為50rad/s；$J$為電機轉動慣量，是電機的固有參數；$P$是電機的極對數；${\psi }_f$是馬達的磁鏈。
• 由於有觀測器的存在，觀測器的參數對速度環參數也會有影響，所以以上計算只是理論值，實際參數需要在理論值的基礎上透過觀察點擊運行效果進行調整。

• 152 person(s) visited this page until now.
• Back