基于FOC 的越野汽車系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

戴測(cè)，袁楊洋，許霄竹，沈世斌

（南京師范大學(xué) 電氣與自動(dòng)化工程學(xué)院，江蘇南京，210046）

0 引言

無(wú)人駕駛汽車因其安全性、穩(wěn)定性和便捷性被視為未來(lái)汽車發(fā)展的重要方向。但是，室外環(huán)境復(fù)雜多變、單一傳感器信息質(zhì)量的抗干擾能力弱、多信息融合手段簡(jiǎn)陋等問(wèn)題嚴(yán)重影響著車輛在城市中的定位，目前，高精度導(dǎo)航定位已經(jīng)成為無(wú)人駕駛汽車發(fā)展道路上的一大挑戰(zhàn)與阻礙。本課題以大學(xué)生智能汽車競(jìng)賽為背景，基于智能小車這一硬件載體，研究多傳感器融合架構(gòu)與算法實(shí)現(xiàn)方案，設(shè)計(jì)具有自主定位導(dǎo)航的無(wú)人車系統(tǒng)，并設(shè)計(jì)以FOC 為主體的無(wú)刷電機(jī)控制方案來(lái)為車模提供強(qiáng)大且穩(wěn)定的動(dòng)力。

1 磁場(chǎng)定向控制理論（FOC）

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

2 基于卡爾曼濾波融合的多傳感器導(dǎo)航系統(tǒng)

由于GPS 本身10Hz 頻率的限制，面對(duì)越野車的高速行駛，顯然100ms 更新一次的GPS 信息無(wú)法滿足高速循跡時(shí)循跡所需求的穩(wěn)定性。因此，我們需要利用慣性元器件陀螺儀的高更新速率的特性，對(duì)短時(shí)間范圍內(nèi)的坐標(biāo)進(jìn)行預(yù)測(cè)更新，來(lái)獲得這100ms 以內(nèi)的較為準(zhǔn)確的車模自身的姿態(tài)。

針對(duì)越野車的本身運(yùn)動(dòng)情況，我們可以近似將其視為一個(gè)CTRV 的運(yùn)動(dòng)模型（恒定轉(zhuǎn)彎率和速度幅度模型）。我們可以將GPS 所獲得的坐標(biāo)結(jié)合四元數(shù)的航向角速度作為一個(gè)觀測(cè)量用來(lái)更新誤差，利用短時(shí)間內(nèi)所獲得的預(yù)測(cè)量用于更新當(dāng)前位置。至此，我們引入卡爾曼濾波來(lái)估計(jì)車輛位置。

常規(guī)的卡爾曼濾波，例如智能車直立組中會(huì)用它來(lái)對(duì)加速度計(jì)和角速度計(jì)進(jìn)行濾波，用來(lái)獲得當(dāng)前的準(zhǔn)確角度。但是常規(guī)的卡爾曼濾波僅僅適用于線性的模型，因?yàn)槌Ｒ?guī)的卡爾曼濾波的狀態(tài)轉(zhuǎn)移矩陣不論如何相乘，都只能得到線性關(guān)系。

圖1 Foc 控制框圖

因此，面對(duì)CTRV 的運(yùn)動(dòng)問(wèn)題，我們可以引入擴(kuò)展卡爾曼濾波（Extend Kalman Filter），簡(jiǎn)稱EKF。EKF 和通常我們所用的卡爾曼濾波一樣，也需要狀態(tài)轉(zhuǎn)移矩陣，不同的是，由于卡爾曼濾波僅能用來(lái)處理線性的問(wèn)題，EKF 會(huì)對(duì)非線性的觀測(cè)方程以及預(yù)測(cè)方程進(jìn)行求導(dǎo)，即在均值處進(jìn)行一階的泰勒展開(kāi)。由上可得，只要對(duì)狀態(tài)轉(zhuǎn)移矩陣和觀測(cè)矩陣求偏導(dǎo)數(shù)，即為求兩個(gè)矩陣的雅各比矩陣，就可以獲得當(dāng)前狀態(tài)近似的狀態(tài)轉(zhuǎn)移矩陣或是觀測(cè)矩陣。

在獲得EKF 的理論模型之后，我們隊(duì)通過(guò)實(shí)地采集數(shù)據(jù)，用Python 建模對(duì)EKF 的有效性進(jìn)行了驗(yàn)證。密集的藍(lán)點(diǎn)為EKF 處理后的坐標(biāo)，間斷的紅點(diǎn)為GPS 采集到的點(diǎn)?？梢灾庇^地看到，在使用相關(guān)算法后，大幅提升了位置信息數(shù)據(jù)更新的速度。

圖2 擴(kuò)展卡爾曼濾波示意圖

3 整體系統(tǒng)方案設(shè)計(jì)

3.1 整體設(shè)計(jì)

系統(tǒng)框圖如圖3所示，由傳感器系統(tǒng)、動(dòng)力系統(tǒng)、轉(zhuǎn)向系統(tǒng)、信號(hào)采集板、主控制板、電機(jī)驅(qū)動(dòng)板以及供電幾個(gè)模塊構(gòu)成。傳感器系統(tǒng)負(fù)責(zé)環(huán)境數(shù)據(jù)的獲取，為車模的環(huán)境感知提供數(shù)據(jù)。

圖3 整體系統(tǒng)結(jié)構(gòu)框圖

圖4 整體代碼結(jié)構(gòu)

本次設(shè)計(jì)中使用衛(wèi)星定位模塊、慣性測(cè)量單元與測(cè)距模塊，構(gòu)建了多傳感器融合的定位系統(tǒng)。

轉(zhuǎn)向系統(tǒng)由舵機(jī)與連桿構(gòu)成，通過(guò)主控制板給定舵機(jī)信號(hào)控制舵機(jī)打角方向帶動(dòng)轉(zhuǎn)向連桿從而控制車模運(yùn)行方向。

信號(hào)采集板為自制印刷電路板，負(fù)責(zé)將各傳感器的信號(hào)線與供電線匯集在一起，由一個(gè)接口統(tǒng)一連接到主控制板上，避免將各傳感器線路直接接在主控制板上導(dǎo)致接線混亂。

主控制板負(fù)責(zé)室外越野車模的總體控制，是該系統(tǒng)的大腦。主控制板上的接口電路將傳感器采集的信號(hào)送往單片機(jī)，單片機(jī)對(duì)這些數(shù)據(jù)使用特定算法進(jìn)行處理，從而實(shí)現(xiàn)車輛的環(huán)境感知，進(jìn)而對(duì)車模的電機(jī)、舵機(jī)發(fā)出控制信號(hào)，完成室外環(huán)境下整個(gè)系統(tǒng)的無(wú)人運(yùn)行。

電機(jī)驅(qū)動(dòng)板負(fù)責(zé)驅(qū)動(dòng)直流無(wú)刷電機(jī)，是整個(gè)系統(tǒng)的動(dòng)力控制單元，通過(guò)設(shè)計(jì)無(wú)刷電機(jī)驅(qū)動(dòng)板上單片機(jī)的算法能夠高效、合理地對(duì)無(wú)刷電機(jī)的轉(zhuǎn)矩、轉(zhuǎn)速進(jìn)行閉環(huán)控制，并且在板上留有與主板通訊的接口，可以實(shí)現(xiàn)獲取主控制板設(shè)定的目標(biāo)速度后對(duì)無(wú)刷電機(jī)的轉(zhuǎn)速進(jìn)行控制，達(dá)到該目標(biāo)速度，也可將當(dāng)前電機(jī)的實(shí)際轉(zhuǎn)速實(shí)時(shí)反饋給主控制板。

3.2 整體代碼結(jié)構(gòu)

首先，打開(kāi)電源開(kāi)關(guān)車模上電，單片機(jī) TC387 上電啟動(dòng)，各模塊初始化，依次為：

（1）按鍵初始化；

（2）屏幕初始化；

（3）舵機(jī)初始化；

（4）陀螺儀初始化；

（5）無(wú)刷電機(jī)初始化；

（6）激光測(cè)距初始化；

（7）GNSS 模塊初始化；

（8）串口中斷初始化；

（9）人機(jī)交互界面初始化；

（10）定時(shí)器中斷初始化。

通過(guò)按鍵選擇是否進(jìn)行經(jīng)緯度數(shù)據(jù)采集，如果進(jìn)行采集則可通過(guò)按鍵將數(shù)據(jù)存入單片機(jī)并在采集完成后進(jìn)行預(yù)處理。如果不需要采集，則直接讀取 Flash 數(shù) 據(jù)并開(kāi)始導(dǎo)航，計(jì)算實(shí)時(shí)經(jīng)緯度與下一個(gè)目標(biāo)點(diǎn) N 的角度 gps_ang[N]和距離，將 IMU 數(shù) 據(jù)以 gps_ang[N]為基準(zhǔn)進(jìn)行卡爾曼濾波，以 IMU 的數(shù)據(jù)和 gps_ang[N]作為輸入進(jìn)行轉(zhuǎn)向控制，采用的是位置式 PID 的方向控制；同時(shí)運(yùn)行基于增量式 PID的速度控制。在這一過(guò)程中循環(huán)判斷，是否到達(dá)下一個(gè)目標(biāo)點(diǎn)，是則 N+1 切換目標(biāo)點(diǎn)。

3.3 無(wú)刷電機(jī)驅(qū)動(dòng)控制方式

無(wú)刷電機(jī)的三個(gè)半橋我們有三種驅(qū)動(dòng)方式。方式一：上橋給PWM 信號(hào)下橋通過(guò)IO 控制開(kāi)啟與關(guān)閉。方式二：上橋通過(guò)IO 控制開(kāi)啟與關(guān)閉下橋給PWM 信號(hào)。方式三：上下橋都給PWM 信號(hào)，但是需要注意的是上下橋的PWM信號(hào)必須是互補(bǔ)的，因?yàn)樯舷聵虿荒芡瑫r(shí)導(dǎo)通，否則出現(xiàn)電源與地短路的情況，由于MOS 開(kāi)關(guān)具有延時(shí)，所以互補(bǔ)PWM 信號(hào)還必須設(shè)置死區(qū)，作用是當(dāng)其中一個(gè)橋關(guān)閉之后要延時(shí)一段時(shí)間再打開(kāi)另外一個(gè)橋，避免出現(xiàn)電源對(duì)地短路的情況。在我們的控制中則使用上述方式進(jìn)行驅(qū)動(dòng)。

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

3.4 無(wú)刷電機(jī)驅(qū)動(dòng)板軟件設(shè)計(jì)

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

圖5 FOC 控制結(jié)構(gòu)框圖

無(wú)刷驅(qū)動(dòng)控制程序由主函數(shù)與中斷服務(wù)函數(shù)兩部分組成。主函數(shù)中進(jìn)行各類外設(shè)與軟件資源的初始化，將電機(jī)轉(zhuǎn)至 a 軸并對(duì)編碼器進(jìn)行零點(diǎn)校正，確保電機(jī)在 a 軸時(shí)編碼器輸出角度為 0o。在主循環(huán)中通過(guò)串口持續(xù)發(fā)送電機(jī)的轉(zhuǎn)速、轉(zhuǎn)子當(dāng)前角度及電流值至上位機(jī)，以便觀察電機(jī)運(yùn)動(dòng)情況。圖6為主函數(shù)流程圖。

圖6 主函數(shù)流程圖

本次設(shè)計(jì)中使用了三個(gè)中斷服務(wù)函數(shù)，分別為一個(gè)定時(shí)器比較中斷、一個(gè)定時(shí)中斷以及一個(gè)外部中斷。定時(shí)器比較中斷是本次設(shè)計(jì)中的核心中斷。其原理是設(shè)置比較值，當(dāng)定時(shí)器計(jì)數(shù)到該比較值時(shí)觸發(fā)此中斷。通過(guò)設(shè)置合理的比較值，在每個(gè) PWM 周期結(jié)束時(shí)觸發(fā)該中斷，此中斷內(nèi)進(jìn)行FOC 電流環(huán)控制流程從而更新三路互補(bǔ) PWM 的占空比以驅(qū)動(dòng)三相逆變電路，從而使電機(jī)三相繞組產(chǎn)生所需的磁場(chǎng)帶動(dòng)電機(jī)旋轉(zhuǎn)。圖7 為定時(shí)器比較中斷流程圖。

圖7 定時(shí)器比較中斷流程圖

根據(jù) FOC 控制過(guò)程的理論基礎(chǔ)，對(duì) FOC 算法進(jìn)行編程實(shí)現(xiàn)。首先進(jìn)行各個(gè)功能模塊的函數(shù)編寫，功能模塊主要分為轉(zhuǎn)子位置檢測(cè)模塊、坐標(biāo)變換模塊、SVPWM 生成模塊、ADC 模塊以及 PID 模塊。位置獲取模塊通過(guò)獲得正交編碼器的數(shù)據(jù)并處理完成角度的計(jì)算。英飛凌提供了正交解碼的解算庫(kù)，通過(guò)使用 TC212 的 GPT12 模塊中的 T5 定時(shí)器對(duì)ABZ 正交編碼器的A、B 相脈沖進(jìn)行計(jì)數(shù)，引腳配置完成后通過(guò)代碼讀取相應(yīng)寄存器內(nèi)的值即可獲取當(dāng)前計(jì)數(shù)的脈沖個(gè)數(shù)。獲取的脈沖數(shù)為累計(jì)脈沖個(gè)數(shù)，該值對(duì) 1024 取余后乘 360 即為當(dāng)前轉(zhuǎn)子的角度值。

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