基于FOC 的越野汽車系統設計與實現

戴測，袁楊洋，許霄竹，沈世斌

（南京師范大學 電氣與自動化工程學院，江蘇南京，210046）

0 引言

無人駕駛汽車因其安全性、穩定性和便捷性被視為未來汽車發展的重要方向。但是，室外環境復雜多變、單一傳感器信息質量的抗干擾能力弱、多信息融合手段簡陋等問題嚴重影響著車輛在城市中的定位，目前，高精度導航定位已經成為無人駕駛汽車發展道路上的一大挑戰與阻礙。本課題以大學生智能汽車競賽為背景，基于智能小車這一硬件載體，研究多傳感器融合架構與算法實現方案，設計具有自主定位導航的無人車系統，并設計以FOC 為主體的無刷電機控制方案來為車模提供強大且穩定的動力。

1 磁場定向控制理論（FOC）

磁場定向控制(FOC) 理論以矢量變換為基礎，將定子電流經過數學變換分解為轉矩電流分量和磁通電流分量，分別控制轉矩和磁通，把復雜的交流電機控制方式轉換為直流電機控制。這種控制方式具有輸出轉矩平穩、調速范圍寬、動態性能好、電壓利用率高等優點，廣泛應用于 PMSM 控制。FOC 控制的思路是選擇電機某一旋轉磁場軸作為特定的同步旋轉坐標軸。磁場定向軸的選擇有 3 種：轉子磁場定向、氣隙磁場定向、定子磁場定向。氣隙磁場定向和定子磁場定向在磁鏈關系中均存在耦合，使得矢量控制結構更加復雜；轉子磁場定向是仿照直流電動機的控制方式，利用坐標變換將交流電動機的定子電流分解成磁場分量電流(相當于勵磁電流)和轉矩分量電流(相當于負載電流) 并分別加以控制，即磁通電流分量和轉矩電流分量二者完全解耦，從而獲得類似于直流調速系統的動態性能。

2 基于卡爾曼濾波融合的多傳感器導航系統

由于GPS 本身10Hz 頻率的限制，面對越野車的高速行駛，顯然100ms 更新一次的GPS 信息無法滿足高速循跡時循跡所需求的穩定性。因此，我們需要利用慣性元器件陀螺儀的高更新速率的特性，對短時間范圍內的坐標進行預測更新，來獲得這100ms 以內的較為準確的車模自身的姿態。

針對越野車的本身運動情況，我們可以近似將其視為一個CTRV 的運動模型（恒定轉彎率和速度幅度模型）。我們可以將GPS 所獲得的坐標結合四元數的航向角速度作為一個觀測量用來更新誤差，利用短時間內所獲得的預測量用于更新當前位置。至此，我們引入卡爾曼濾波來估計車輛位置。

常規的卡爾曼濾波，例如智能車直立組中會用它來對加速度計和角速度計進行濾波，用來獲得當前的準確角度。但是常規的卡爾曼濾波僅僅適用于線性的模型，因為常規的卡爾曼濾波的狀態轉移矩陣不論如何相乘，都只能得到線性關系。

圖1 Foc 控制框圖

因此，面對CTRV 的運動問題，我們可以引入擴展卡爾曼濾波（Extend Kalman Filter），簡稱EKF。EKF 和通常我們所用的卡爾曼濾波一樣，也需要狀態轉移矩陣，不同的是，由于卡爾曼濾波僅能用來處理線性的問題，EKF 會對非線性的觀測方程以及預測方程進行求導，即在均值處進行一階的泰勒展開。由上可得，只要對狀態轉移矩陣和觀測矩陣求偏導數，即為求兩個矩陣的雅各比矩陣，就可以獲得當前狀態近似的狀態轉移矩陣或是觀測矩陣。

在獲得EKF 的理論模型之后，我們隊通過實地采集數據，用Python 建模對EKF 的有效性進行了驗證。密集的藍點為EKF 處理后的坐標，間斷的紅點為GPS 采集到的點。可以直觀地看到，在使用相關算法后，大幅提升了位置信息數據更新的速度。

圖2 擴展卡爾曼濾波示意圖

3 整體系統方案設計

3.1 整體設計

系統框圖如圖3所示，由傳感器系統、動力系統、轉向系統、信號采集板、主控制板、電機驅動板以及供電幾個模塊構成。傳感器系統負責環境數據的獲取，為車模的環境感知提供數據。

圖3 整體系統結構框圖

圖4 整體代碼結構

本次設計中使用衛星定位模塊、慣性測量單元與測距模塊，構建了多傳感器融合的定位系統。

轉向系統由舵機與連桿構成，通過主控制板給定舵機信號控制舵機打角方向帶動轉向連桿從而控制車模運行方向。

信號采集板為自制印刷電路板，負責將各傳感器的信號線與供電線匯集在一起，由一個接口統一連接到主控制板上，避免將各傳感器線路直接接在主控制板上導致接線混亂。

主控制板負責室外越野車模的總體控制，是該系統的大腦。主控制板上的接口電路將傳感器采集的信號送往單片機，單片機對這些數據使用特定算法進行處理，從而實現車輛的環境感知，進而對車模的電機、舵機發出控制信號，完成室外環境下整個系統的無人運行。

電機驅動板負責驅動直流無刷電機，是整個系統的動力控制單元，通過設計無刷電機驅動板上單片機的算法能夠高效、合理地對無刷電機的轉矩、轉速進行閉環控制，并且在板上留有與主板通訊的接口，可以實現獲取主控制板設定的目標速度后對無刷電機的轉速進行控制，達到該目標速度，也可將當前電機的實際轉速實時反饋給主控制板。

3.2 整體代碼結構

首先，打開電源開關車模上電，單片機 TC387 上電啟動，各模塊初始化，依次為：

（1）按鍵初始化；

（2）屏幕初始化；

（3）舵機初始化；

（4）陀螺儀初始化；

（5）無刷電機初始化；

（6）激光測距初始化；

（7）GNSS 模塊初始化；

（8）串口中斷初始化；

（9）人機交互界面初始化；

（10）定時器中斷初始化。

通過按鍵選擇是否進行經緯度數據采集，如果進行采集則可通過按鍵將數據存入單片機并在采集完成后進行預處理。如果不需要采集，則直接讀取 Flash 數 據并開始導航，計算實時經緯度與下一個目標點 N 的角度 gps_ang[N]和距離，將 IMU 數 據以 gps_ang[N]為基準進行卡爾曼濾波，以 IMU 的數據和 gps_ang[N]作為輸入進行轉向控制，采用的是位置式 PID 的方向控制；同時運行基于增量式 PID的速度控制。在這一過程中循環判斷，是否到達下一個目標點，是則 N+1 切換目標點。

3.3 無刷電機驅動控制方式

無刷電機的三個半橋我們有三種驅動方式。方式一：上橋給PWM 信號下橋通過IO 控制開啟與關閉。方式二：上橋通過IO 控制開啟與關閉下橋給PWM 信號。方式三：上下橋都給PWM 信號，但是需要注意的是上下橋的PWM信號必須是互補的，因為上下橋不能同時導通，否則出現電源與地短路的情況，由于MOS 開關具有延時，所以互補PWM 信號還必須設置死區，作用是當其中一個橋關閉之后要延時一段時間再打開另外一個橋，避免出現電源對地短路的情況。在我們的控制中則使用上述方式進行驅動。

由于使用CCU6 模塊管理無刷驅動信息，需要CPU 頻繁地處理無刷驅動的換相控制。所以我們使用英飛凌基于AURIX 系列MCU 的GTM 模塊的無刷驅動方案，可以使無刷代碼和控制代碼合二為一，同時無刷代碼幾乎不消耗CPU 資源。

3.4 無刷電機驅動板軟件設計

傳統的無刷直流電機大多應用六步方波法來控制，對于典型的三相BLDC 電機，有 6 個不同的工作區間，每個區間中對特定的兩相繞組通電，產生 6 種方向相差 60°的磁場，按規律循環切換六種磁場的產生從而帶動無刷電機的旋轉，但方波驅動出力大，轉矩脈動和噪聲也比較大，影響著無刷直流電機在高精度、低噪聲環境下的應用。隨著電機控制技術的發展，正弦波驅動控制理念被廣泛應用到無刷電機的控制中，有效改善六步方波驅動中存在的缺陷。因此本次設計使用正弦波驅動中的磁場定向控制(Field oriented Control：FOC)技術對直流無刷電機進行轉動控制。FOC結構框圖如圖5 所示。

圖5 FOC 控制結構框圖

無刷驅動控制程序由主函數與中斷服務函數兩部分組成。主函數中進行各類外設與軟件資源的初始化，將電機轉至 a 軸并對編碼器進行零點校正，確保電機在 a 軸時編碼器輸出角度為 0o。在主循環中通過串口持續發送電機的轉速、轉子當前角度及電流值至上位機，以便觀察電機運動情況。圖6為主函數流程圖。

圖6 主函數流程圖

本次設計中使用了三個中斷服務函數，分別為一個定時器比較中斷、一個定時中斷以及一個外部中斷。定時器比較中斷是本次設計中的核心中斷。其原理是設置比較值，當定時器計數到該比較值時觸發此中斷。通過設置合理的比較值，在每個 PWM 周期結束時觸發該中斷，此中斷內進行FOC 電流環控制流程從而更新三路互補 PWM 的占空比以驅動三相逆變電路，從而使電機三相繞組產生所需的磁場帶動電機旋轉。圖7 為定時器比較中斷流程圖。

圖7 定時器比較中斷流程圖

根據 FOC 控制過程的理論基礎，對 FOC 算法進行編程實現。首先進行各個功能模塊的函數編寫，功能模塊主要分為轉子位置檢測模塊、坐標變換模塊、SVPWM 生成模塊、ADC 模塊以及 PID 模塊。位置獲取模塊通過獲得正交編碼器的數據并處理完成角度的計算。英飛凌提供了正交解碼的解算庫，通過使用 TC212 的 GPT12 模塊中的 T5 定時器對ABZ 正交編碼器的A、B 相脈沖進行計數，引腳配置完成后通過代碼讀取相應寄存器內的值即可獲取當前計數的脈沖個數。獲取的脈沖數為累計脈沖個數，該值對 1024 取余后乘 360 即為當前轉子的角度值。

此外，還需要初始化 Z 相采集引腳的外部中斷，使用上升沿觸發的方式。坐標變換模塊的代碼實現過程中大量使用了三角函數的運算，若調用自帶的三角函數運算函數會涉及大量的浮點數運算，占用 CPU 較長的運算時間，還可能會導致程序的卡頓。因此本次設計中優化了三角函數的計算方式，使用查表的方式對三角函數進行快速運算，優化程序運行速度。SVPWM 模塊的程序編寫中有大量的條件判斷，此處使用了 switch 語句進行各條件的跳轉，避免了使用多個 if 語句的嵌套導致程序冗雜與較差的可讀性與可維護性。PID 模塊通過編程實現了速度-電流串級 PID 算法，電流環作為內環，速度環作為外環，均使用 PI 調節。速度 PI 為串級 PID 的外環，將編碼器測量的電機轉速作為測量值，將給定的目標轉速作為給定值，經過增量式 PI 運算后得到目標電流值 Itar_d、Itar_q，該環可以對電機的轉速進行控制。電流 PI 為串級 PID 的內環，將 Park 變換得出的 Id 與 Iq 作為測量值，將增量速度 PI 的輸出 Itar_d、Itar_q 作為給定值，經過增量式 PI 運算后得到控制電壓 Uα與 Uβ，該環可以對電機的扭矩進行控制。最終實現無刷電機速度與扭矩雙閉環控制。