借助Verilog，在FPGA中實(shí)現(xiàn)了帶編碼器的兩臺(tái)電機(jī)的電機(jī)控制系統(tǒng)的RTL級(jí)設(shè)計(jì)。

介紹

借助硬件描述語(yǔ)言 (HDL) Verilog 和 AMD Vivado 設(shè)計(jì)套件，在 AMD Spartan-7 FPGA 中實(shí)現(xiàn)帶編碼器的兩個(gè)電機(jī)的控制器系統(tǒng)的 RTL 設(shè)計(jì)。

在這個(gè)項(xiàng)目中，使用了搭載 Spartan-7 FPGA開(kāi)發(fā)板，將使用它作為電機(jī)控制器系統(tǒng)。

在項(xiàng)目開(kāi)始前，肯定會(huì)有人問(wèn)：“既然用MCU編程要簡(jiǎn)單快捷得多，為什么還要用 FPGA 來(lái)做這么復(fù)雜的工作呢？” 這個(gè)問(wèn)題問(wèn)得好。所以，我們將簡(jiǎn)要探討一下人們選擇 FPGA 的各種原因。

在 FPGA 與MCU的對(duì)比競(jìng)爭(zhēng)之后，將介紹電機(jī)驅(qū)動(dòng)器、H 橋、PWM、編碼器、PID 反饋控制系統(tǒng)、編碼和測(cè)試、收集日期的分析等多方面的基本概念。然后，將詳細(xì)介紹如何構(gòu)建自己的 FPGAbot。

FPGA 與MCU

為特定產(chǎn)品或可交付系統(tǒng)選擇“最佳”處理技術(shù)，在許多方面對(duì)其成功至關(guān)重要。影響因素包括支持成本、供應(yīng)鏈、盈利能力、壽命等等。從工程角度來(lái)看，決策中最具影響力的典型因素是時(shí)間、成本、面積和功耗。

FPGA 優(yōu)勢(shì)

FPGA 以其低延遲、固有的并行處理能力和靈活性而聞名。此外，F(xiàn)PGA還能為許多獨(dú)立的計(jì)算模塊、外部傳感器和執(zhí)行器等提供高確定性和精確的定時(shí)/延時(shí)。因此，會(huì)發(fā)現(xiàn) FPGA 被用于許多關(guān)鍵任務(wù)應(yīng)用，例如：

1）軍事和航空航天系統(tǒng)

雷達(dá)系統(tǒng) - 例如實(shí)時(shí)（RT）信號(hào)處理和數(shù)據(jù)采集。 無(wú)人駕駛飛行器（UAV）——例如飛行控制、傳感器處理和安全通信。

航空電子設(shè)備、衛(wèi)星、導(dǎo)彈制導(dǎo)系統(tǒng)等

2）工業(yè)控制系統(tǒng)（ICS）

對(duì)電網(wǎng)、煉油廠等關(guān)鍵基礎(chǔ)設(shè)施進(jìn)行實(shí)時(shí)控制和監(jiān)控。

工業(yè)自動(dòng)化 - 機(jī)器控制，實(shí)現(xiàn)故障檢測(cè)、冗余和容錯(cuò)控制。

功能安全——例如汽車(chē)和工業(yè)控制的容錯(cuò)系統(tǒng)。

3）醫(yī)療器械

診斷成像系統(tǒng) - 快速準(zhǔn)確的數(shù)據(jù)處理。

病人監(jiān)護(hù)——FPGA 為病人監(jiān)護(hù)設(shè)備提供高性能、RT 監(jiān)護(hù)和信號(hào)分析。

救生設(shè)備...

ETC。

4） 電信等

許多關(guān)鍵任務(wù)應(yīng)用程序需要實(shí)時(shí)處理、確定性行為、低延遲、可編程性、并行處理能力、安全性和功率效率。

MCU優(yōu)勢(shì)

對(duì)于成本敏感的產(chǎn)品或普通愛(ài)好者來(lái)說(shuō)，MCU 是節(jié)省成本和加快系統(tǒng)開(kāi)發(fā)周轉(zhuǎn)的更好選擇。

微控制器使用標(biāo)準(zhǔn)編程語(yǔ)言進(jìn)行軟件開(kāi)發(fā)，例如 Python/microPython 和 C/C++。FPGA 設(shè)計(jì)的學(xué)習(xí)曲線(xiàn)（架構(gòu)、硬件描述語(yǔ)言、時(shí)序問(wèn)題等）則要陡峭得多，需要投入大量時(shí)間和經(jīng)驗(yàn)才能高效完成。

MCU 也更適合外設(shè)接口和執(zhí)行重復(fù)性任務(wù)。通用架構(gòu)、串行執(zhí)行流程以及易于外設(shè)集成的特性，使得 MCU 在非關(guān)鍵任務(wù)系統(tǒng)中更具吸引力。

了解了這些之后，現(xiàn)在讓我們深入了解移動(dòng)機(jī)器人電機(jī)控制系統(tǒng)的細(xì)節(jié)！

高級(jí)電機(jī)控制器系統(tǒng)設(shè)計(jì)

本節(jié)介紹了移動(dòng)機(jī)器人電機(jī)控制器設(shè)計(jì)所需的主要外部元件和FPGA功能模塊。此外，還提供了系統(tǒng)的高層原理圖。

電機(jī)驅(qū)動(dòng)器

FPGA 需要分別驅(qū)動(dòng)兩個(gè) 12V 直流有刷電機(jī)。我以前用過(guò) 12V 直流電機(jī)，發(fā)現(xiàn)這個(gè)電壓水平能夠滿(mǎn)足我的 DIY 機(jī)器人所需的扭矩。我通常會(huì)選擇低轉(zhuǎn)速電機(jī)（例如 190 RPM），因?yàn)樗鼈儽雀咿D(zhuǎn)速電機(jī)提供更大的扭矩。

L298N H 橋電機(jī)驅(qū)動(dòng)器是驅(qū)動(dòng) 12V 電機(jī)的絕佳選擇，因?yàn)樗鼉r(jià)格低廉，業(yè)余愛(ài)好者已經(jīng)寫(xiě)了很多關(guān)于其使用的文章，并且易于連接。

L298N 本質(zhì)上是一個(gè)放大器，其數(shù)字接口由較小的輸入控制電壓（例如 3.3V 或 5V）控制，并在較大的直流電壓范圍內(nèi)產(chǎn)生比例輸出。L298N 模塊可在 5 至 35V 直流電壓 (Vs) 范圍內(nèi)工作。由于我們使用的是 12V 直流電機(jī)，因此源電壓為 12V，如圖 1 所示。

注意：如果要啟用 L298N 板載的 5V 電源調(diào)節(jié)器，則最大輸入直流電源不應(yīng)超過(guò) 12V。否則，5V 直流調(diào)節(jié)器會(huì)過(guò)熱并關(guān)閉。

編碼器

我總是購(gòu)買(mǎi)配備霍爾效應(yīng)傳感器的直流電機(jī)。這些傳感器可以測(cè)量每個(gè)電機(jī)的速度和旋轉(zhuǎn)方向。在本設(shè)計(jì)中，編碼器反饋用于PID算法，以保持FPGA機(jī)器人直線(xiàn)穩(wěn)定地移動(dòng)。里程也可以根據(jù)這些信息計(jì)算出來(lái)。將在后面的技術(shù)回顧部分和FPGA電機(jī)控制器設(shè)計(jì)細(xì)節(jié)部分詳細(xì)討論編碼器的工作原理和使用方法。

因此，該系統(tǒng)至少需要以下功能元素：

頂層架構(gòu)

可以選擇 (1) 在使能端使用脈沖寬度調(diào)制 (PWM)，并通過(guò) IN1、2、3 和 4 上的高低邏輯電平控制方向（圖 2a）；或 (2) 在所有四個(gè)輸入端口上使用 PWM，并將使能端設(shè)置為邏輯高電平（圖 2b）。雖然已經(jīng)創(chuàng)建了這兩個(gè)版本，但在本項(xiàng)目描述中，將選擇選項(xiàng) (1)（圖 2b）。選擇選項(xiàng) (1) 的原因?qū)⒃?L298N 部分解釋。

輸入設(shè)定點(diǎn)（PWM 的占空比設(shè)置）和旋轉(zhuǎn)方向源信號(hào)選擇器。

編碼器計(jì)數(shù)器

PID 反饋控制系統(tǒng)/算法將電機(jī)速度同步到設(shè)定值。

數(shù)字控制器，在本例中是 Spartan-7 FPGA。

圖1：L298N和12V編碼器電機(jī)原理圖。

圖 2a：帶有PWM源啟用輸入的FPGA電機(jī)控制器框圖。

圖2b：帶有PWM源INx輸入的FPGA電機(jī)控制器框圖。

對(duì)于上面列出的前兩點(diǎn)，即從設(shè)定值生成PWM信號(hào)所需的邏輯，了解L298N的工作原理非常有幫助。因此，下一節(jié)（可選閱讀）將介紹這部分內(nèi)容以及其他背景知識(shí)。

技術(shù)回顧

本節(jié)將對(duì)組件進(jìn)行技術(shù)回顧，并介紹與這些組件接口所需的數(shù)字和模擬概念。本節(jié)涵蓋的信息并非執(zhí)行本項(xiàng)目的必要條件，但有助于理解創(chuàng)建各種模塊和邏輯的原因。

L298N電機(jī)驅(qū)動(dòng)器

如圖 3 所示，L298N 可歸類(lèi)為一種直流放大器，它在其輸入端轉(zhuǎn)換脈沖寬度調(diào)制 (PWM) 信號(hào)，在本例中為 0 和 3.3VDC，并將其按比例轉(zhuǎn)換為穩(wěn)定的 0V 直流電壓至 Vs，如前所述。

圖3：L298N電機(jī)驅(qū)動(dòng)器

FPGA 的較低輸出電壓 0 - 3.3V 能夠驅(qū)動(dòng) TTL 邏輯輸入 (IN1 - IN4) 并使能 A 和 B 的原因在于 [1] 中列出的規(guī)格以及圖 4b 所示的規(guī)格。其中說(shuō)明如下：

控制信號(hào)輸入電壓范圍：

邏輯低電平：-0.3V≤Vin≤1.5V

邏輯高電平：2.3V ≤ Vin ≤ Vss

其中，Vss 由 L298N 模塊板載的 5V 直流穩(wěn)壓器供電。由于 FPGA 不會(huì)從 L298N 接收數(shù)字?jǐn)?shù)據(jù)，因此無(wú)需邏輯電平轉(zhuǎn)換器。

L298N 模塊是基于 L298 集成電路 (IC) 構(gòu)建的。L298 IC 是一款高電壓、高電流雙全橋驅(qū)動(dòng)器。L298 IC 的額定電壓 Vs = 5V 至 46V（圖 2a 和 2b），但 L298N 模塊在 35V 以上的電壓下存在散熱和元件限制。

圖 4a：L298IC-雙全橋驅(qū)動(dòng)器。[1]

圖 4b：L298部分電氣特性列表[1]。

L298 雙 H 橋

讓我們快速看一下 H 橋電路。如果還沒(méi)有完全理解下面的信息，不用擔(dān)心，但為了完整性，我想補(bǔ)充一下。另外，如果愿意，可以跳過(guò)本節(jié)。

圖 5 是 [1] 中 L298 框圖的復(fù)制版。圖中展示了數(shù)字電路（與門(mén)和非門(mén)）和模擬電路（BJT 晶體管和電阻）的混合。邏輯門(mén)和 BJT 晶體管構(gòu)成了電流控制電路。為了更好地理解電流控制電路的整體功能，下面給出了該圖的簡(jiǎn)化版本及其說(shuō)明。

圖5:L298IC框圖[1]

圖 6 用開(kāi)關(guān)概念代替了電流控制電路。這展示了 H 橋電路的工作原理，我們也將用它來(lái)解釋在 IN 引腳和 EN 引腳上使用 PWM 的區(qū)別。

圖6：簡(jiǎn)化的單H橋電路表示

要使電機(jī)正轉(zhuǎn)，需要閉合 S1 和 S4，完成從 Vs 經(jīng)電機(jī)到地的電路（即電流路徑）。S3 和 S2 根據(jù)需要保持?jǐn)嚅_(kāi)。要使電機(jī)反轉(zhuǎn)，則需要相反的情況，即閉合 S3 和 S2，斷開(kāi) S1 和 S4。圖 7a 和 b 說(shuō)明了這一概念。

圖 7a 和 b：電機(jī)正向和反向旋轉(zhuǎn)。

將此電路分析與圖6聯(lián)系起來(lái)，讓我們看看用與邏輯門(mén)和BJT實(shí)現(xiàn)的開(kāi)關(guān)。下面的邏輯方程等同于與門(mén)的輸出，我們?cè)谶@里將其標(biāo)記為_(kāi)d（代表數(shù)字）。

SW1_d = IN1 AND ENA

SW2_d = NOT IN1 AND ENA

SW3_d = IN2 AND ENA

SW4_d = NOT IN2 AND ENA

當(dāng)上述任意 SWx_d 等于邏輯高電平時(shí)，相應(yīng)的 BJT 導(dǎo)通。這是通過(guò)基極-發(fā)射極正向偏置（Vbe 超過(guò)基極輸入和發(fā)射極輸出引腳之間 +0.7V 的壓降閾值）來(lái)實(shí)現(xiàn)的。當(dāng)集電極電壓分別大于基極和發(fā)射極電壓（Vc >> Vb > Ve）時(shí)，導(dǎo)通的晶體管就像一個(gè)閉合的開(kāi)關(guān)。參見(jiàn)圖 8。

圖8：NPN晶體管[5]。

希望開(kāi)關(guān)類(lèi)比能夠簡(jiǎn)化數(shù)字晶體管的控制邏輯。如果感興趣并且具備相關(guān)背景知識(shí)，建議找一本好書(shū)或參考資料，進(jìn)一步了解晶體管的工作原理以及不同類(lèi)型的晶體管。

接下來(lái)，如果需要復(fù)習(xí)一下 PWM，我們來(lái)快速回顧一下。如果不需要，可以跳過(guò)此部分。

PWM

為了使連接到 L298N 電機(jī)驅(qū)動(dòng)器的電機(jī)實(shí)現(xiàn)變速，我們使用了 PWM（脈沖寬度調(diào)制）。PWM 是一種獲取可變模擬電壓的數(shù)字方法。因此，方波數(shù)字信號(hào)的占空比（邏輯高電平時(shí)間與總脈沖周期的關(guān)系）可以通過(guò)增加或減少來(lái)改變模擬電路中的平均電壓（圖 9）。例如，如果 NPN 晶體管的基極-發(fā)射極結(jié)在給定的時(shí)間內(nèi)導(dǎo)通時(shí)間更長(zhǎng)，則電流會(huì)更大，從而導(dǎo)致集電極輸出連接和發(fā)射極（在本例中為地）之間的電壓降 (Vce) 更大。圖 10 演示了這一概念 [7]。

圖9：各種占空比產(chǎn)生的 PWM[6]。

圖10：使用 PWM 和 NPN BJT 的直流電機(jī)電路[6]。

L298N PWM輸入響應(yīng)特性

如本文開(kāi)頭所述，使用 PWM 實(shí)現(xiàn)變速有兩種不同的方法。方法 1 是用 PWM 驅(qū)動(dòng)輸入引腳（IN1、2、3 和 4），并將使能引腳設(shè)置為高電平，如圖 11a 所示。方法 2 是將輸入引腳對(duì)設(shè)置為高電平和低電平，以獲得正確的旋轉(zhuǎn)方向，并用 PWM 驅(qū)動(dòng)使能引腳，以實(shí)現(xiàn)變速，如圖 11b 所示。

圖11a：使用PWM控制電機(jī)速度的方法1[2]。

圖 11b：使用 PWM 控制電機(jī)速度的方法 2 [2]。

使用方法 1 可獲得如圖 7a 和 7b 所示的電流。唯一的區(qū)別在于每個(gè)脈沖周期內(nèi)從 Vs 到 Gnd 的電流流動(dòng)時(shí)間。

使用方法 2 會(huì)得到略有不同的響應(yīng)。如果驅(qū)動(dòng)電機(jī)正向旋轉(zhuǎn)，IN1 上的 PWM 信號(hào)將處于活動(dòng)狀態(tài)，而 IN2 上的 PWM 信號(hào)將保持在低電平。由于 ENA 已通過(guò)跳線(xiàn)設(shè)置為邏輯高電平，因此開(kāi)關(guān)切換完全由 IN1 上的有效 PWM 信號(hào)控制。將 IN2 和 ENA 都設(shè)置為受控的穩(wěn)定值（0 和 1）后，SW3 始終設(shè)置為“關(guān)閉”或“打開(kāi)”，SW4 始終設(shè)置為“打開(kāi)”或“閉合”。但是，用于 SW4（~In1 和 EnA）的非門(mén)將在 PWM 占空比的低電平期間打開(kāi) SW2。這發(fā)生在 SW1 處于“關(guān)閉”或“打開(kāi)”狀態(tài)時(shí)。因此，電機(jī)的正極和負(fù)極連接到地。這會(huì)導(dǎo)致在 SW2 處于“打開(kāi)”狀態(tài)時(shí)釋放反向的感應(yīng)電動(dòng)勢(shì) (EMF)。這種配置在較低轉(zhuǎn)速下有用，但在較高轉(zhuǎn)速下，由于反向 EMF 電壓的反復(fù)損耗，會(huì)導(dǎo)致更高的電流消耗。

圖12a和b：左圖，In1+PWM 周期的一部分。右圖，PWM周期的接地狀態(tài)。

12V直流編碼器齒輪電機(jī)

我們?yōu)橐苿?dòng)機(jī)器人選擇的直流電機(jī)是專(zhuān)門(mén)配備磁編碼器的。這對(duì)于此類(lèi)直流電機(jī)來(lái)說(shuō)很常見(jiàn)。對(duì)電機(jī)控制精度要求更高的直流電機(jī)則使用光學(xué)編碼器。

旋轉(zhuǎn)磁編碼器有兩個(gè)霍爾效應(yīng)傳感器。它們相對(duì)于電機(jī)軸線(xiàn)呈 90 度角放置。圖 13 展示了這種布置如何產(chǎn)生相位差 90 度的編碼器信號(hào)。

圖13：霍爾效應(yīng)傳感器用于測(cè)量轉(zhuǎn)速和方向[8]。

此圖簡(jiǎn)化了，因?yàn)橹皇褂昧藘蓚€(gè)磁極。大多數(shù)帶有磁編碼器的電機(jī)都有數(shù)十到數(shù)百個(gè)這樣的磁極，從而提高了電機(jī)旋轉(zhuǎn)位置的分辨率。此外，有些系統(tǒng)會(huì)配備 4 個(gè)霍爾傳感器，以進(jìn)一步提高精度。

上面顯示的霍爾效應(yīng)傳感器產(chǎn)生的兩個(gè)信號(hào)被指定為通道 A 和 B。這些通道具有以下特點(diǎn)：

它們的相位差為 90 度（見(jiàn)圖 14）。

如果 A 領(lǐng)先于 B，那么電機(jī)就只能朝一個(gè)可能的方向轉(zhuǎn)動(dòng)。

如果 B 領(lǐng)先于 A，那么電動(dòng)機(jī)的轉(zhuǎn)動(dòng)方向與前一個(gè)子彈的轉(zhuǎn)動(dòng)方向相反。

方波的頻率與齒輪的旋轉(zhuǎn)速度成正比。

圖14：編碼器通道A和B 波形。

通過(guò)計(jì)算電機(jī)單次旋轉(zhuǎn)產(chǎn)生的脈沖數(shù)，可以推導(dǎo)出由所用物理極數(shù)獲得的分辨率。該信息可用于估算機(jī)器人的里程，方法是使用累積計(jì)數(shù)（我喜歡稱(chēng)之為“tics”）來(lái)計(jì)數(shù)/旋轉(zhuǎn)，并使用車(chē)輪的直徑（或半徑）來(lái)計(jì)算其周長(zhǎng)。

FPGA電機(jī)控制器設(shè)計(jì)細(xì)節(jié)

本節(jié)將詳細(xì)介紹移動(dòng)機(jī)器人電機(jī)驅(qū)動(dòng)系統(tǒng)的關(guān)鍵 Verilog 代碼。為了方便起見(jiàn)，該代碼將與圖 2a 所示的系統(tǒng)框圖關(guān)聯(lián)起來(lái)，并在下面的圖 15 中復(fù)制。此外，我推導(dǎo)的 PID 算法可用作超前-跟隨電機(jī)速度比較系統(tǒng)，并將進(jìn)行詳細(xì)解釋。

Verilog 源層次結(jié)構(gòu)

從圖 15 所示的初始頂層設(shè)計(jì)中，派生出 Verilog 模塊層次結(jié)構(gòu)，然后在 AMD Vivado 中創(chuàng)建（圖 16）。

圖15：電機(jī)控制系統(tǒng)的初始和最終框圖

圖16：AMDVivado中的Verilog模塊層次結(jié)構(gòu)

外部引腳聲明

接下來(lái)是用于 FPGA 與外部接口的輸入和輸出信號(hào)聲明的 Verilog 代碼（圖 17）。在本例中，它用于 L298N、兩個(gè) 12V 直流編碼器電機(jī)（圖 1）以及 RC 接收器單元（未顯示）。

圖17：Verilog-頂層模塊I/O定義至外部引引腳

編碼器計(jì)數(shù)器模塊

編碼器計(jì)數(shù)器模塊所用的代碼源自fpga4fun.com [9]（http://fpga4fun.com/）。在那里，可以找到詳細(xì)的解釋以及描述所用邏輯的波形圖。本質(zhì)上，只需使用系統(tǒng)時(shí)鐘進(jìn)行過(guò)采樣、幾個(gè)邏輯門(mén)和 D 觸發(fā)器（為跨時(shí)鐘域添加了幾個(gè)額外的觸發(fā)器）即可確定自旋的計(jì)數(shù)和方向。圖 18 展示了代碼的實(shí)現(xiàn)。

圖18：編碼器計(jì)數(shù)器邏輯[9]。

我還添加了僅創(chuàng)建正計(jì)數(shù)的邏輯。這對(duì)于我們稍后介紹的PID是必需的。

RC信號(hào)選擇模塊

此模塊的名稱(chēng)有點(diǎn)用詞不當(dāng)。最初，該模塊僅用于接收來(lái)自 RC 模塊的方向信號(hào)，并將每個(gè)信號(hào)轉(zhuǎn)換為兩位值（圖 19）。一位指示左輪應(yīng)該前進(jìn)還是后退，另一位指示右輪。每位都位于每個(gè)設(shè)定值（左輪和右輪）的最高有效位 (MSB)。目前，來(lái)自此模塊并進(jìn)入 PID 模塊的左右電機(jī)設(shè)定值是相同的值（位 [6:0]）。可以在頂部模塊中更改用戶(hù)設(shè)定值，方法是將 curr_setpt1 和 2 初始化為新值，然后運(yùn)行 Vivado 進(jìn)行綜合、實(shí)現(xiàn)并生成比特流。此過(guò)程最多需要兩分鐘左右。

圖19：rc_signal_selectv 的 RC 啟用代碼部分

圖 20：添加到rc_signal_select.y的非 RC 模式的附加多路復(fù)用邏輯。

PWM模塊

如上所述，用戶(hù)設(shè)定值（頂層名為 curr_setpt1 & 2）存儲(chǔ)在 8 位寄存器的低 7 位中。因此，設(shè)定值取值范圍為 0 至 127。該值表示饋入 L298N 的 PWM 的占空比。PWM 如何將設(shè)定值轉(zhuǎn)換為占空比時(shí)間寬度？這是通過(guò) PWM 模塊中的 7 位計(jì)數(shù)器實(shí)現(xiàn)的。PWM 的輸出最初為邏輯高電平（計(jì)數(shù)器 = 0）。當(dāng)以 256 kHz 時(shí)鐘頻率計(jì)數(shù)的計(jì)數(shù)器計(jì)數(shù)到設(shè)定值時(shí)，PWM 輸出邏輯低電平，直到計(jì)數(shù)器達(dá)到 127。結(jié)果是一個(gè) 2kHz 方波 PWM，占空比為設(shè)定值/127。

使用 Vivado 時(shí)鐘向?qū)?100 MHz 系統(tǒng)時(shí)鐘生成了一個(gè) 8.192 MHz 時(shí)鐘。這樣，256 kHz 模塊中計(jì)數(shù)器的一位只需要與邏輯“1”進(jìn)行比較。最終的 PWM 頻率為 2 kHz，接近 1.5 kHz 的示例 [2]，并且運(yùn)行良好。

PID

PID 控制器模塊的設(shè)計(jì)并不像我想象的那么簡(jiǎn)單。問(wèn)題在于設(shè)定值（0 - 127）與每設(shè)定采樣周期 T（例如 100 毫秒）讀回的增量計(jì)數(shù)之間的轉(zhuǎn)換。圖 21 展示了討論中的 PID 反饋回路框圖。

圖 21：時(shí)間采樣PID 控制反饋回路

對(duì)我來(lái)說(shuō)，這里的問(wèn)題是我沒(méi)有看到（也沒(méi)有產(chǎn)生）從 delta tics（計(jì)數(shù)）/T 回到設(shè)定值的低錯(cuò)誤轉(zhuǎn)換。

因此，決定使用設(shè)定點(diǎn)輸入 r[k] 作為基準(zhǔn)值，并添加一個(gè)基于兩個(gè)電機(jī)先前計(jì)數(shù)和當(dāng)前計(jì)數(shù)差異的修改值（誤差值），從而包括每個(gè)電機(jī)每個(gè)周期 T（速度）總計(jì)數(shù)差異的離散時(shí)間分析/比較。

根據(jù)使用這些小型直流編碼器電機(jī)的經(jīng)驗(yàn)，注意到，在相同的PWM占空比設(shè)置下，一個(gè)電機(jī)的扭矩往往大于另一個(gè)電機(jī)。因此，在PID控制算法中，可以快速確定響應(yīng)速度更快的電機(jī)，并將其用作主電機(jī)，以匹配速度和行駛距離。這樣，速度修改只會(huì)針對(duì)速度較慢的電機(jī)的設(shè)定值輸入r[k]進(jìn)行。否則，電機(jī)的轉(zhuǎn)速會(huì)非常快地上升。

讓我們看一下所提出的領(lǐng)先-跟隨方程，然后看一下這個(gè)離散領(lǐng)先-跟隨電機(jī)反饋控制回路的偽代碼。

e[k] = Feedback_1[k] - Feedback_2[k]; (1)

其中，e[k] 是在采樣周期 k 處計(jì)算出的誤差，F(xiàn)eedback_n[k] 是在采樣周期 k 處電機(jī) n（即 n = 1 或 2）的編碼器計(jì)數(shù)器值。

Δx1[k] = Feedback_1[k] - PrevFeedback_1[k-1] (2)

Δx2[k] = Feedback_2[k] - PrevFeedback_2[k-1] (3)

Δx1x2[k] = Δx1[k] - Δx2[k] (4)

其中，PrevFeedback_n[k-1] 是電機(jī) n 的編碼器計(jì)數(shù) Feedback[k-1] 在采樣周期 k-1 保存，Δx1x2[k] 是電機(jī) 1 和 2 的轉(zhuǎn)速差。

與圖21所示的PID控制系統(tǒng)一樣，有一些權(quán)重值（常數(shù)Ki、Ke和Kv）用于將公式(1)和公式(4)相乘，然后再相加。因此，得到的公式如下（圖22所示的部分代碼）：

我們將在下面討論局部放電后誤差方程的總體方程為：

u[k] = setpoint[k] + motorNSpdMod (5)

其中，N 再次表示電機(jī)數(shù)量 1 或 2。

可能注意到，公式 (5) 缺少積分元素。我將解釋如何在 u[k]（公式 5）中添加積分，這是平滑 FPGA 代碼部分中誤差振蕩（反復(fù)從正到負(fù)）的關(guān)鍵，但最初并未考慮到這一點(diǎn)。

在 Vivado 中使用 Verilog 編寫(xiě)設(shè)計(jì)代碼后，我創(chuàng)建了一個(gè)直流編碼器電機(jī)的仿真模型，并在測(cè)試臺(tái)上進(jìn)行了兩次實(shí)例化。該電機(jī)模型從被測(cè)單元 (UUT) 采樣 PWM 波輸入，并將計(jì)算出的轉(zhuǎn)速轉(zhuǎn)換為相應(yīng)頻率的編碼器 A/B 通道波形。編碼器波形通道被輸入到 UUT 中。此外，我添加了一個(gè)“阻力”值，使電機(jī)的轉(zhuǎn)速相對(duì)于其他電機(jī)有所降低。這使得速度較慢的電機(jī)的編碼器數(shù)量較少，從而與實(shí)際電機(jī)和系統(tǒng)響應(yīng)具有一定的相似性。當(dāng)然，這是一個(gè)非常簡(jiǎn)單的模型，因?yàn)閷?duì)一個(gè)電機(jī)增加的阻力是恒定的。如果在電機(jī)模型中至少創(chuàng)建一個(gè)簡(jiǎn)單的物理慣性響應(yīng)，將會(huì)非常有利。該模型還假設(shè)移動(dòng)機(jī)器人使用恒定的平坦水平表面。

不幸的是，我看到機(jī)器人落地后的響應(yīng)與仿真結(jié)果不同，而我之前預(yù)想的沒(méi)有物理模型會(huì)是這樣的。

FPGA 中使用的公式經(jīng)過(guò)測(cè)試，并根據(jù)需要進(jìn)行了修改（例如，縮短 T 值、調(diào)整 Ke 和 Kv 值等），最初使用 Arduino Mega MCU 和 FPGAbot（不含 FPGA）在 C/C++ 中進(jìn)行了驗(yàn)證。這縮短了驗(yàn)證和修改的周期。以下代碼片段來(lái)自 Mega 測(cè)試（函數(shù) PD_Alg(args)），代表了領(lǐng)先跟隨算法（圖 22）。

圖 22：C/C++代碼中的領(lǐng)先-跟隨PD算法。

代碼的下半部分，motorNSpdMod 會(huì)根據(jù)最大設(shè)定值 (255 - C_fwdSpd - 20) 進(jìn)行檢查，以防止值過(guò)高。255 是最大占空比值（0 到 255），C_fwdSpd 實(shí)際上是兩個(gè)電機(jī)的設(shè)定值（相同值），而 20 只是一個(gè)任意值，在 Arduino IDE 控制臺(tái)上查看 Serial.print() 滾動(dòng)顯示的數(shù)據(jù)時(shí)，這個(gè)值似乎是合理的，如圖 23 所示。

圖 23：FPGAbot在地毯上運(yùn)行的Serial.print()調(diào)試數(shù)據(jù)

從打印出來(lái)的數(shù)據(jù)來(lái)看，顯然可以通過(guò)稍微調(diào)整 Ke 和 Kv，甚至在混合中添加一個(gè)積分元素來(lái)實(shí)現(xiàn)更好的響應(yīng)。

FPGA 變體的領(lǐng)先-跟隨 PID 方程實(shí)現(xiàn)

圖 23a 和 b 分別給出了在領(lǐng)先-跟隨方程 (5) 中添加積分之前和之后的仿真結(jié)果。

圖 24a：領(lǐng)先-跟隨PID Verilog 實(shí)現(xiàn)仿真。

圖 24b：領(lǐng)先-跟隨PID Verilog 實(shí)現(xiàn)仿真。

下圖（圖 25a 和 b）分別表示 e[k]（誤差）、dx1dx2（電機(jī)速度變化量）和 u[k]（電機(jī)調(diào)制值）。圖 25b 還添加了 Ki*integral[k]（調(diào)制值之和乘以分?jǐn)?shù)）的數(shù)據(jù)，這些數(shù)據(jù)列于公式 (6) 中。

u[k] = setpoint[k] + motorNSpdMod + Ki ΣmotorNSpdMod (6)

圖 25a：領(lǐng)先-跟隨控制算法的Verilog 實(shí)現(xiàn)中沒(méi)有積分部分。

圖 25b：添加了積分的領(lǐng)先跟隨控制算法的Verilog實(shí)現(xiàn)。

從這兩幅圖中不難看出，本例中積分非常迅速地抑制了誤差和速度增量的振蕩。這使得 u[k] 的調(diào)整非常平緩，與原始設(shè)定值吻合。

領(lǐng)先-跟隨FB控制算法Verilog代碼

圖 22 所示的 C/C++ 代碼在 MCU 上按順序運(yùn)行。這就是它們的工作原理。為了在 Verilog 中復(fù)制順序計(jì)算，使用了一個(gè)狀態(tài)機(jī)，每 10 ns（100 MHz 系統(tǒng)時(shí)鐘）從一個(gè)計(jì)算狀態(tài)切換到另一個(gè)計(jì)算狀態(tài)。該算法的計(jì)算分布在 7 個(gè)狀態(tài)中，計(jì)算時(shí)間為 70 ns。并不擔(dān)心將計(jì)算時(shí)間縮短幾十納秒，并犧牲代碼的可讀性。與采樣周期 T 相比，節(jié)省的時(shí)間在持續(xù)時(shí)間上要小三個(gè)數(shù)量級(jí)。為了使其更具可讀性，將計(jì)算拆分成多個(gè)簡(jiǎn)潔的階段會(huì)創(chuàng)建數(shù)據(jù)依賴(lài)關(guān)系，從而需要額外的時(shí)鐘周期。

為了避免使用浮點(diǎn)數(shù)表示 Ki、Ke 和 Kv，從而節(jié)省大量 FPGA 邏輯資源，通過(guò)將誤差、速度增量和積分右移（除以 2^n）來(lái)近似這些小數(shù)值。然而，在 Verilog 中使用有符號(hào)和無(wú)符號(hào)變量（寄存器）時(shí)需要謹(jǐn)慎。我發(fā)現(xiàn)，如果使用邏輯移位“>>”，負(fù)數(shù)右移最終會(huì)得到一個(gè)很大的正數(shù)。為了避免這種情況，需要使用算術(shù)右移表示“>>>”。

圖 26a、b 和 c 中的代碼演示了目前為止在 Verilog 中實(shí)現(xiàn) PID 領(lǐng)先-跟隨反饋 (FB) 控制算法所涵蓋的概念，并在 Vivado 中進(jìn)行了仿真（圖 24a 和 b）。

圖 26a：Verilog 領(lǐng)先-跟隨算法 e[k] 和 dx1dx2 計(jì)算。

圖 26b：使用ke*e[K] 和Kv*dx1dx2計(jì)算motorNSpdMod

圖 26c:Verilog 領(lǐng)先-跟隨算法 u[k] 計(jì)算

其他

一些其他的細(xì)節(jié)，可以看一組圖片吧~

項(xiàng)目地址

完整的項(xiàng)目可以在Github上找到。

來(lái)源：本文轉(zhuǎn)載自O(shè)PENFPGA公眾號(hào)