借助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)
-
FPGA
+關(guān)注
關(guān)注
1645文章
22026瀏覽量
617652 -
機(jī)器人
+關(guān)注
關(guān)注
213文章
29681瀏覽量
212562 -
Verilog
+關(guān)注
關(guān)注
29文章
1367瀏覽量
112173 -
電機(jī)控制器
+關(guān)注
關(guān)注
21文章
559瀏覽量
32687
原文標(biāo)題:RTL 級(jí)機(jī)器人電機(jī)控制器的 FPGA 設(shè)計(jì)
文章出處:【微信號(hào):FPGA研究院,微信公眾號(hào):FPGA研究院】歡迎添加關(guān)注!文章轉(zhuǎn)載請(qǐng)注明出處。
發(fā)布評(píng)論請(qǐng)先 登錄
控制器、伺服電機(jī)、減速機(jī)成機(jī)器人產(chǎn)業(yè)主要瓶頸
【OK210申請(qǐng)】機(jī)器人控制器
【TL6748 DSP申請(qǐng)】工業(yè)機(jī)器人控制系統(tǒng)
選擇機(jī)器人電機(jī)的幾點(diǎn)思考
如何利用ARM研究嵌入式服務(wù)機(jī)器人控制器?
如何設(shè)計(jì)智能滅火機(jī)器人控制器?
工業(yè)機(jī)器人控制器有什么功能?
【送貨上門(mén)】SC3150三協(xié)機(jī)器人控制器
小白學(xué)機(jī)器人伺服電機(jī)模塊相關(guān)資料分享
相撲機(jī)器人控制器資料分享
通過(guò)多軸工業(yè)機(jī)器人來(lái)聊聊機(jī)器人控制系統(tǒng)以及控制器的實(shí)現(xiàn)
淺談兒童陪護(hù)機(jī)器人
機(jī)器人控制器有哪些類(lèi)型_機(jī)器人控制器發(fā)展
常見(jiàn)的機(jī)器人控制器
機(jī)器人關(guān)節(jié)伺服電機(jī)PID串級(jí)控制

評(píng)論