雙足機(jī)器人多電機(jī)協(xié)同控制系統(tǒng)設(shè)計(jì)

(湖北大學(xué) 計(jì)算機(jī)與信息工程學(xué)院，武漢 430062)

0 引言

雙足機(jī)器人是機(jī)器人研究領(lǐng)域最活躍的研究熱點(diǎn)之一，運(yùn)動控制系統(tǒng)作為雙足機(jī)器人研究的核心內(nèi)容，它直接決定了機(jī)器人性能的優(yōu)劣[1-2]。它不僅控制機(jī)器人的啟動、運(yùn)行和停止，而且對控制速度和精度提出了嚴(yán)格的要求，同時要求對機(jī)器人中多個電機(jī)的控制要能達(dá)到協(xié)同的效果，從而使機(jī)器人完成難度較大、更加接近人的動作。因此在雙足機(jī)器人的控制中，對多路步進(jìn)電機(jī)進(jìn)行協(xié)同控制是核心[3-4]。

要實(shí)現(xiàn)對步進(jìn)電機(jī)的控制，一般采用單片機(jī)控制，但單片機(jī)的單時鐘系統(tǒng)滿足不了多個電機(jī)協(xié)同控制的需求，通常會運(yùn)用多個單片機(jī)芯片控制，但是多個CPU同時控制就會面對資源浪費(fèi)、相互干擾而穩(wěn)定性差、系統(tǒng)設(shè)計(jì)困難等問題；采用專用芯片控制法，控制精確、協(xié)同性強(qiáng)，但是可移植性差；采用DSP控制方法設(shè)計(jì)復(fù)雜，技術(shù)要求較高，較少采用；FPGA的控制能力很強(qiáng)大，但是單純采用FPGA芯片控制時，需要高性能的FPGA芯片，這種芯片價格昂貴，因此其應(yīng)用范圍受到一定限制[5-7]。對于雙足機(jī)器人控制系統(tǒng)，其多路電機(jī)的控制精度與速度要求并不算很高，為了節(jié)省成本，需要設(shè)計(jì)出既能同時協(xié)同控制多路電機(jī)，而且控制精確、性價比高的控制方案[8-10]。

本文采用“ARM+FPGA”的雙芯片異構(gòu)控制方案。結(jié)合ARM控制能力強(qiáng)、成本低、操作簡單的優(yōu)點(diǎn)和FPGA多通道控制的優(yōu)點(diǎn)，以ARM芯片為主控芯片，控制所有命令的發(fā)出與執(zhí)行；FPGA用來做輔控芯片，在接到命令之后，發(fā)揮自己并行處理的能力去控制電機(jī)的運(yùn)行狀態(tài)，在ARM和PPGA的分工協(xié)作下完成對多個電機(jī)的協(xié)同控制。

1 多電機(jī)協(xié)同控制系統(tǒng)總體設(shè)計(jì)

雙足機(jī)器人ARM+FPGA多電機(jī)協(xié)同控制系統(tǒng)包含有人機(jī)交互控制系統(tǒng)，實(shí)時控制系統(tǒng)，電機(jī)控制系統(tǒng)三大部分組成。人機(jī)交互系統(tǒng)在PC端完成，主要是接受人的命令，使用者按照自己的需要去給系統(tǒng)發(fā)送命令從而控制電機(jī)；實(shí)時控制系統(tǒng)就是負(fù)責(zé)指令的執(zhí)行，這也是控制系統(tǒng)的關(guān)鍵；電機(jī)控制系統(tǒng)則是最后命令的實(shí)行者，可以完成對多臺電機(jī)的同時控制。ARM+FPGA多電機(jī)協(xié)同控制系統(tǒng)的具體模塊如圖1所示。

圖1 多電機(jī)協(xié)同控制系統(tǒng)的具體功能模塊框圖

1.1 上位機(jī)

在上位機(jī)上裝有軟件用于人機(jī)交互，人機(jī)交互界面采用 C#開發(fā)語言設(shè)計(jì)。上位機(jī)采用一般的PC機(jī)主要負(fù)責(zé)具體命令的設(shè)置，我們通過PC機(jī)給電機(jī)設(shè)置一系列的動作，讓電機(jī)工作。同時PC機(jī)也需要把電機(jī)運(yùn)行的數(shù)據(jù)，比如每個電機(jī)運(yùn)行的位置，以及運(yùn)行的時間顯示出來。PC機(jī)還需要保存數(shù)據(jù)并與ARM芯片雙向通信，把命令傳給ARM去執(zhí)行，并把ARM上的信息顯示出來。PC機(jī)主要起人與系統(tǒng)的交互作用，它包括參數(shù)的設(shè)置，狀態(tài)的顯示還有通信。參數(shù)設(shè)置主要是對多臺電機(jī)通道的選擇以及電機(jī)的動作、頻率和步數(shù)的設(shè)置；狀態(tài)的顯示包括命令發(fā)出的狀況以及FPGA芯片傳回命令的情況，還有機(jī)器運(yùn)轉(zhuǎn)的情況等等；通信模塊的主要功能就是上位機(jī)與ARM系統(tǒng)的通信，主要負(fù)責(zé)PC機(jī)接受到的命令的輸送，由于需要實(shí)時控制所以采用網(wǎng)口通信的方式，通過網(wǎng)線將PC和ARM控制器鏈接起來。

1.2 ARM控制器

ARM 控制器是整個控制系統(tǒng)的核心，主要用于電機(jī)驅(qū)動算法的實(shí)現(xiàn)，與上位機(jī)和FPGA 進(jìn)行通信，以及控制輸出PWM脈沖信號。ARM控制器采用STM32F103ZET6芯片，它包含JTAG下載模塊、通信模塊、I/O接口模塊、與FPGA通信模塊等。JTAG下載模塊的功能是將在PC端軟件上編譯好的程序下載到ARM控制器里面，從而STM32F103芯片才能對系統(tǒng)進(jìn)行控制；通信模塊則是ARM控制器與上位機(jī)PC的聯(lián)系通道；I/O 接口模塊則是ARM系統(tǒng)對外界的控制，比如指示燈、紅外線以及一些參數(shù)的設(shè)計(jì)等等，通過I/O口來實(shí)現(xiàn)對外接設(shè)備的控制；與FPGA的通信模塊則與FPGA相連，將主控命令傳送給STM32F103芯片。ARM控制器與PC端上位機(jī)以及FPGA進(jìn)行雙向通信，傳遞數(shù)據(jù)和控制命令。

1.3 FPGA控制器

在該電機(jī)控制系統(tǒng)中,FPGA控制器起上傳下達(dá)的作用，它首先接收ARM的指令，通過輸出脈沖指令，將指令傳送到電機(jī)控制系統(tǒng)，由電機(jī)控制系統(tǒng)去具體執(zhí)行控制電機(jī)運(yùn)行，還接收電機(jī)傳回來的數(shù)據(jù)，并根據(jù)返回信號控制脈沖輸出。FPGA功能模塊采用EP2C35F672C8N芯片，它包含有JTAG下載模塊，I/O 接口模塊，與ARM系統(tǒng)通信模塊，還有與電機(jī)控制系統(tǒng)連接的脈沖輸出模塊。JTAG下載模塊把PC端軟件上編譯好的程序下載到芯片，I/O接口模塊與電機(jī)的控制系統(tǒng)相連，控制電機(jī)的運(yùn)行，EP2C35F672C8N芯片需要對主頻進(jìn)行分頻，通過I/O 接口輸出多路頻率來控制多路電機(jī)；它的通信模塊與ARM系統(tǒng)相連接，負(fù)責(zé)接收ARM系統(tǒng)的命令并返回相應(yīng)的狀態(tài)。

ARM與FPGA之間是雙向通信，ARM控制器傳送上位機(jī)命令給FPGA，同時FPGA也要把運(yùn)行狀態(tài)信息傳給ARM，主控ARM芯片總攬大局，實(shí)時控制整個運(yùn)行過程，實(shí)時控制模塊FPGA芯片則是命令的傳送者，傳給步進(jìn)電機(jī)驅(qū)動器去執(zhí)行。

1.4 步進(jìn)電機(jī)驅(qū)動器與光柵傳感

步進(jìn)電機(jī)驅(qū)動器的作用是由于芯片產(chǎn)生的波形PWM信號都比較小，不足以驅(qū)動電機(jī)，因此還需要在控制電路后面加上電機(jī)控制系統(tǒng)，步進(jìn)電機(jī)驅(qū)動器如圖2所示。

圖2 步進(jìn)電機(jī)驅(qū)動器結(jié)構(gòu)框圖

步進(jìn)電機(jī)驅(qū)動器包括信號產(chǎn)生源、步進(jìn)控制器及功率放大器等。此系統(tǒng)接受FPGA控制器產(chǎn)生的脈沖信號，接著步進(jìn)控制器把脈沖信號轉(zhuǎn)換成環(huán)形脈沖，這樣才能控制步進(jìn)電機(jī)正轉(zhuǎn)和反轉(zhuǎn)。由于脈沖信號太弱而不能驅(qū)動電機(jī)，這時候就需要功率放大器把環(huán)型脈沖信號放大。光柵傳感器把電機(jī)相關(guān)信息(速度、位置或扭矩)傳給FPGA再傳給ARM來與控制信號作對比，實(shí)現(xiàn)比較精確的閉環(huán)控制。

2 控制系統(tǒng)的ARM+FPGA設(shè)計(jì)實(shí)現(xiàn)

2.1 控制系統(tǒng)架構(gòu)設(shè)計(jì)

根據(jù)任務(wù)要求，ARM控制芯片選用ST系列的STM32F103ZET6芯片，STM32F103使用的是高性能的ARM CortexTM-M3 32位的RISC內(nèi)核，工作頻率高達(dá)72 MHz，內(nèi)置高速存儲器，有144個端口，有2個12位的ADC和3個通用16位定時器以及1個PWM定時器，還有標(biāo)準(zhǔn)和先進(jìn)的通信接口，以及USART接口和CAN接口。FPGA控制芯片使用Altera公司經(jīng)濟(jì)型的Cyclone系列的EP2C35F672C8N芯片，具有豐富的邏輯單元和I/O接口，價格低，可滿足本控制系統(tǒng)的設(shè)計(jì)需求。在本電機(jī)控制系統(tǒng)中，兩部分的主控芯片ARM和FPGA還需要相互的交換數(shù)據(jù)，來達(dá)到協(xié)同控制的目的。因此在ARM和FPGA之間的通信方式也是關(guān)鍵，此系統(tǒng)中我們選擇FSMC(Flexible StaticMemory Controller，可變靜態(tài)存儲控制器)的通信方式，把FPGA當(dāng)作ARM的一個SRAM來使用，針對兩個控制芯片的特點(diǎn)，我們設(shè)計(jì)了如圖3的通信電路，采用 SRAM 的方式連接。PB3～7是5個控制信號端口，8位地址線負(fù)責(zé)發(fā)送命令到FPGA來具體控制6個電機(jī)的運(yùn)行，8個數(shù)據(jù)通信線路是ARM與FPGA的通信通道，wr=1時寫數(shù)據(jù)，STM32給FPGA發(fā)命令，rd=1時讀取6路電機(jī)各個電機(jī)運(yùn)行的狀態(tài)是否正常。

圖3 STM32 與 FPGA 的通信連接圖

ARM+FPGA控制系統(tǒng)中，F(xiàn)PGA把各臺電機(jī)的運(yùn)行狀態(tài)信息傳送給STM32，并最終在上位機(jī)人機(jī)交互操作界面上實(shí)時顯示。在控制過程中，數(shù)據(jù)刷新速度特別快，F(xiàn)PGA要把數(shù)據(jù)準(zhǔn)確地傳送給STM32，則需要對當(dāng)前狀態(tài)值等數(shù)據(jù)給以鎖存，用鎖存信號lock對數(shù)據(jù)進(jìn)行鎖存，數(shù)據(jù)鎖存以后，就可以傳送數(shù)據(jù)了。如果rd_stm32是高電平，就把鎖存的數(shù)據(jù)保存至寄存器，等候STM32進(jìn)行讀取。FPGA需要從STM32獲取在上位機(jī)人機(jī)交互操作界面設(shè)定的各電機(jī)的運(yùn)行參數(shù)與控制命令，在FPGA需要從STM32讀取的數(shù)據(jù)時，通過FPGA端口的控制線與地址線，把數(shù)據(jù)保存至寄存器。

2.2 STM32軟件設(shè)計(jì)

STM32軟件所要完成的主要任務(wù)是將上位機(jī)傳來的數(shù)據(jù)指令進(jìn)行相應(yīng)的處理與轉(zhuǎn)化，然后發(fā)送給FPGA芯片，同時從FPGA芯片讀取電機(jī)的實(shí)時運(yùn)行狀態(tài)，并且做相應(yīng)的數(shù)據(jù)處理再返回到上位機(jī)顯示出來，還需要接收光柵傳感器發(fā)來的數(shù)據(jù)并與控制命令對比，通過模糊PID控制修正參數(shù)，再進(jìn)行電機(jī)的閉環(huán)控制。

STM32控制程序的設(shè)計(jì)是通過模糊PID算法控制電機(jī)的運(yùn)行，模糊PID算法主要是在STM32里面實(shí)現(xiàn)的，STM32的開發(fā)平臺是Keil MDK，Keil是美國Keil Soft公司開發(fā)的微處理器軟件開發(fā)平臺，是目前ARM內(nèi)核單片機(jī)開發(fā)的主流工具，Keil提供了包括C編譯器、宏匯編、鏈接器、庫管理和仿真調(diào)試器[6]，在Win10系統(tǒng)上運(yùn)行。本文開發(fā)用的是2013年發(fā)布的最新版本Keil uVision5。

2.2.1 STM32主程序

STM32的電機(jī)控制程序功能是由主程序和中斷程序配合協(xié)調(diào)共同完成，主程序包括系統(tǒng)初始化程序，主循環(huán)程序。軟件首先必須對整個系統(tǒng)和設(shè)備初始化，包括時鐘的初始化、GPIO口的初始化配置、定時器的配置、PWM波的配置、ADC的配置初始化以及中斷變量的設(shè)置。在整個系統(tǒng)初始完成之后還需要進(jìn)入不斷的循環(huán)之中，這就包括對上位機(jī)傳來的命令的接收執(zhí)行，對設(shè)備傳感器傳來的轉(zhuǎn)速，加速度進(jìn)行計(jì)算比較并在程序中做相應(yīng)的算法處理，以及設(shè)備故障檢測、電壓電流檢測、工作狀態(tài)檢測等來不斷判斷機(jī)器是否正常運(yùn)行以及給機(jī)器提供正常運(yùn)行的指令。主要的程序流程圖如圖4所示。

圖4 STM32主要程序流程圖

2.2.2 STM32子程序

主程序是框架，而子程序則是系統(tǒng)的核心和主要部分，由他們來保證系統(tǒng)功能的正常實(shí)現(xiàn)，在子程序中，PWM波的產(chǎn)生是主線，通過對光耦傳感器傳來的速度，以及采集的電流進(jìn)行算法分析，對PWM信號進(jìn)行計(jì)算修正，再輸出新的信號，從而完成對電流、轉(zhuǎn)速和位置的三環(huán)控制，ADC程序中斷為輔以完成電機(jī)速度位置的某些轉(zhuǎn)換。主要程序包括位置檢測程序、電流檢測程序、模糊PID算法程序、UART通信程序、電流current換相中斷函數(shù)等，其流程圖如圖5所示。

圖5 STM32子程序流程圖

2.2.3 STM32與FPGA的通信

STM32與FPGA的通信有兩個方面：一方面是STM32把上位機(jī)的命令通過轉(zhuǎn)化發(fā)送給FPGA；另一方面FPGA也需要把自己運(yùn)行產(chǎn)生信號的信息返回給STM32，考慮到靈活性、及時性以及雙向性，我們采用FSMC(Flexible Static Memory Controller)可變靜態(tài)存儲控制器的通信方式，它是STM32系列中內(nèi)部集成的256KB以上的Flash，支持多種靜態(tài)存儲器類型，而且可以同步突發(fā)訪問。

2.3 控制系統(tǒng)的FPGA設(shè)計(jì)

2.3.1 系統(tǒng)硬件設(shè)計(jì)

FPGA控制系統(tǒng)主要的任務(wù)是對50 MHz的頻率進(jìn)行分頻，獲得各通道的時鐘頻率；傳達(dá)各步進(jìn)電機(jī)的啟動、停止、運(yùn)行等命令；對各步進(jìn)電機(jī)的運(yùn)行狀態(tài)鎖定，并且將狀態(tài)發(fā)送給上位機(jī)；接收STM32發(fā)來的指令信息并且轉(zhuǎn)化處理之后傳給電機(jī)控制系統(tǒng)。根據(jù)FPGA在系統(tǒng)的功能，F(xiàn)PGA的功能電路模塊主要包括：電源模塊、下載模塊、與電機(jī)通信模塊和與STM32的通訊模塊。FPGA的主要功能模塊電路如圖6所示。

圖6 FPGA功能模塊電路圖

2.3.2 基于IP核的FPGA設(shè)計(jì)

設(shè)計(jì)平臺采用Quartus II，它是Altera公司的綜合性CPLD/FPGA開發(fā)軟件，該軟件支持原理圖、VHDL、Verilong等多種設(shè)計(jì)輸入，而且自帶有仿真器，可以完成從設(shè)計(jì)、仿真、適配到最后形成最終電路的完整過程而不需要其他的平臺。此外，在QuartusII上面提供了免費(fèi)的LPM調(diào)試模塊，比如計(jì)數(shù)器、加法器、乘法器等，而且還有特制的有償IP核供使用。本文基于系統(tǒng)提供的鎖相環(huán)IP核模塊進(jìn)行設(shè)計(jì)，通過對PLL模塊例化，并且把整個IP核模塊和PLL模塊添加到頂層電路圖中去，整個頂層設(shè)計(jì)如圖7所示。

圖7 FPGA完整的頂層設(shè)計(jì)原理圖

3 實(shí)驗(yàn)結(jié)果及分析

要檢驗(yàn)控制系統(tǒng)最終達(dá)到的控制效果，需構(gòu)建一個測試平臺進(jìn)行系統(tǒng)調(diào)試。首先進(jìn)行散件電機(jī)的檢驗(yàn)，再組建成真正的雙足機(jī)器人，從觀察機(jī)器人的運(yùn)動狀態(tài)，用示波器檢測，以及上位機(jī)顯示的機(jī)器人的運(yùn)行數(shù)據(jù)三個方面來驗(yàn)證系統(tǒng)的真實(shí)可行性。通過觀察實(shí)物電機(jī)和機(jī)器人的運(yùn)動、分析示波器的波形以及上位機(jī)顯示的運(yùn)動數(shù)據(jù)三個方面來說明本套控制系統(tǒng)的可行性。

要對系統(tǒng)調(diào)試必須要對系統(tǒng)進(jìn)行控制，這就必須要有人機(jī)交互界面，人機(jī)交互界面部分所要完成的任務(wù)主要有：與用戶進(jìn)行數(shù)據(jù)交換，人們根據(jù)需要把動作通過人機(jī)交互界面下達(dá)給控制系統(tǒng)，然后交給步進(jìn)電機(jī)去執(zhí)行。人機(jī)交互界面是由C#寫成的，在windows 系統(tǒng)上都可使用，借用的平臺是 Visualstudio 2013，利用.NET Framework2.0 庫中I/O口和串口控件設(shè)計(jì)而成的，通過設(shè)置串口通信所需的參數(shù)，打開串口便可以進(jìn)行數(shù)據(jù)傳輸，達(dá)到控制的目的。

上位機(jī)與系統(tǒng)板的通信采用改進(jìn)串口協(xié)議，協(xié)議有幀頭、數(shù)據(jù)長度、指令和參數(shù)構(gòu)成，協(xié)議以0x55開頭，表示有數(shù)據(jù)到達(dá)，接著是數(shù)據(jù)長度，用來校驗(yàn)數(shù)據(jù)，然后是指令，對串口的數(shù)據(jù)進(jìn)行操作，包括要控制的電機(jī)號，程序下載的電機(jī)號以及動作等。在上位機(jī)頁面設(shè)置了6路電機(jī)的操作模式來驗(yàn)證系統(tǒng)控制電機(jī)的同時性。多電機(jī)協(xié)同控制系統(tǒng)的重點(diǎn)是完成對多臺電機(jī)的同步控制，所以應(yīng)當(dāng)對多臺電機(jī)控制的同步性進(jìn)行檢測。

我們把 6 路電機(jī)用作雙足機(jī)器人的腿上的 6 個關(guān)節(jié)，來驗(yàn)證雙足機(jī)器人中多路電機(jī)協(xié)同控制的性能，每條腿用三個電機(jī)相當(dāng)于人腿上的三個關(guān)節(jié)，即從上到下依次是髖關(guān)節(jié)、膝關(guān)節(jié)和足關(guān)節(jié)。人的腿上的一系列動作需要各個關(guān)節(jié)協(xié)同起來才能完成，機(jī)器人腿上的動作則需要各個電機(jī)協(xié)同工作才能完成。觀察分析 6 路電機(jī)在前進(jìn)動作完成期間的數(shù)據(jù)，數(shù)據(jù)如表1所示。

表1 機(jī)器人前進(jìn)的各電機(jī)數(shù)據(jù)表

從表中數(shù)據(jù)來看，控制非常精確，可以達(dá)到0.14度的精確度，而且在150 ms甚至更短的時間內(nèi)，六路電機(jī)可以同時運(yùn)行，并且各個電機(jī)可以有不同的動作，來共同協(xié)調(diào)完成一系列完整的動作。通過系統(tǒng)的運(yùn)行狀態(tài)，可以得出控制系統(tǒng)達(dá)到了良好的控制效果。

4 結(jié)論

根據(jù)觀察機(jī)器人的運(yùn)動狀態(tài)和分析上位機(jī)顯示的數(shù)據(jù)，可以很明確的看出該控制系統(tǒng)是能夠同步、協(xié)同而精確控制雙足機(jī)器人中多路電機(jī)的。本設(shè)計(jì)證明了以ARM+FPGA為核心的控制系統(tǒng)的可行性，硬件和軟件的完美結(jié)合共同構(gòu)成雙足機(jī)器人多電機(jī)協(xié)同控制系統(tǒng)，該系統(tǒng)具有良好的應(yīng)用價值。