雙足機器人多電機協同控制系統設計

(湖北大學 計算機與信息工程學院，武漢 430062)

0 引言

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

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

本文采用“ARM+FPGA”的雙芯片異構控制方案。結合ARM控制能力強、成本低、操作簡單的優點和FPGA多通道控制的優點，以ARM芯片為主控芯片，控制所有命令的發出與執行；FPGA用來做輔控芯片，在接到命令之后，發揮自己并行處理的能力去控制電機的運行狀態，在ARM和PPGA的分工協作下完成對多個電機的協同控制。

1 多電機協同控制系統總體設計

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

圖1 多電機協同控制系統的具體功能模塊框圖

1.1 上位機

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

1.2 ARM控制器

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

1.3 FPGA控制器

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

ARM與FPGA之間是雙向通信，ARM控制器傳送上位機命令給FPGA，同時FPGA也要把運行狀態信息傳給ARM，主控ARM芯片總攬大局，實時控制整個運行過程，實時控制模塊FPGA芯片則是命令的傳送者，傳給步進電機驅動器去執行。

1.4 步進電機驅動器與光柵傳感

步進電機驅動器的作用是由于芯片產生的波形PWM信號都比較小，不足以驅動電機，因此還需要在控制電路后面加上電機控制系統，步進電機驅動器如圖2所示。

圖2 步進電機驅動器結構框圖

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

2 控制系統的ARM+FPGA設計實現

2.1 控制系統架構設計

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

圖3 STM32 與 FPGA 的通信連接圖

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

2.2 STM32軟件設計

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

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

2.2.1 STM32主程序

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

圖4 STM32主要程序流程圖

2.2.2 STM32子程序

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

圖5 STM32子程序流程圖

2.2.3 STM32與FPGA的通信

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

2.3 控制系統的FPGA設計

2.3.1 系統硬件設計

FPGA控制系統主要的任務是對50 MHz的頻率進行分頻，獲得各通道的時鐘頻率；傳達各步進電機的啟動、停止、運行等命令；對各步進電機的運行狀態鎖定，并且將狀態發送給上位機；接收STM32發來的指令信息并且轉化處理之后傳給電機控制系統。根據FPGA在系統的功能，FPGA的功能電路模塊主要包括：電源模塊、下載模塊、與電機通信模塊和與STM32的通訊模塊。FPGA的主要功能模塊電路如圖6所示。

圖6 FPGA功能模塊電路圖

2.3.2 基于IP核的FPGA設計

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

圖7 FPGA完整的頂層設計原理圖

3 實驗結果及分析

要檢驗控制系統最終達到的控制效果，需構建一個測試平臺進行系統調試。首先進行散件電機的檢驗，再組建成真正的雙足機器人，從觀察機器人的運動狀態，用示波器檢測，以及上位機顯示的機器人的運行數據三個方面來驗證系統的真實可行性。通過觀察實物電機和機器人的運動、分析示波器的波形以及上位機顯示的運動數據三個方面來說明本套控制系統的可行性。

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

上位機與系統板的通信采用改進串口協議，協議有幀頭、數據長度、指令和參數構成，協議以0x55開頭，表示有數據到達，接著是數據長度，用來校驗數據，然后是指令，對串口的數據進行操作，包括要控制的電機號，程序下載的電機號以及動作等。在上位機頁面設置了6路電機的操作模式來驗證系統控制電機的同時性。多電機協同控制系統的重點是完成對多臺電機的同步控制，所以應當對多臺電機控制的同步性進行檢測。

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

表1 機器人前進的各電機數據表

從表中數據來看，控制非常精確，可以達到0.14度的精確度，而且在150 ms甚至更短的時間內，六路電機可以同時運行，并且各個電機可以有不同的動作，來共同協調完成一系列完整的動作。通過系統的運行狀態，可以得出控制系統達到了良好的控制效果。

4 結論

根據觀察機器人的運動狀態和分析上位機顯示的數據，可以很明確的看出該控制系統是能夠同步、協同而精確控制雙足機器人中多路電機的。本設計證明了以ARM+FPGA為核心的控制系統的可行性，硬件和軟件的完美結合共同構成雙足機器人多電機協同控制系統，該系統具有良好的應用價值。