基于FPGA的四足機器人控制系統(tǒng)

殷勇華，卞新高，汪 赟，朱燈林

（河海大學 機電工程學院，常州 213022）

0 引言

四足機器人是一個復雜的運動控制系統(tǒng)，其具有多關(guān)節(jié)、多驅(qū)動器、多傳感器、多自由度的特點[1]。本實驗室自行研制的JQRI00型四足步行機器人樣機，由機架、腿、關(guān)節(jié)以及足四部分組成，如圖1所示。電控柜、液壓系統(tǒng)、電機以及作業(yè)裝置等主要由機架負責承載；每條腿由三個自由度構(gòu)成，分為上臂（含足部）、下臂、胯部三個部分，上臂與下臂、胯部與上臂、機架與胯部之間分別通過三個轉(zhuǎn)動副連接，以便各腿能夠?qū)崿F(xiàn)在一定范圍內(nèi)的自由運動；通過液壓缸驅(qū)動各關(guān)節(jié)的運動，每個液壓缸都可與另外兩個運動臂構(gòu)成穩(wěn)定的三角形結(jié)構(gòu)以適應重載運行；每個液壓缸的運動由電磁比例換向閥和液壓缸內(nèi)同軸安裝的位移傳感器構(gòu)成閉環(huán)速度以及位移控制；腿與地面的接觸狀態(tài)識別則由足底安裝的接觸力傳感器完成[2]。

1 控制系統(tǒng)功能需求

四足步行機器人的運動控制、運行狀態(tài)以及故障的監(jiān)測和診斷等都需要運動控制系統(tǒng)來完成。具體包括：

圖1 JQRI00型四足步行機器人

1）運動數(shù)據(jù)的采集與處理功能

對四足步行機器人運動過程中各傳感器輸出的模擬量信號進行采樣以及必要的處理，及時反饋給關(guān)節(jié)伺服控制器進行實時運動控制，同時將數(shù)據(jù)通過主控制器上傳給監(jiān)控計算機進行機器人位姿動態(tài)顯示。

2）運動控制功能

自動行走時控制四足步行機器人按照操作人員發(fā)出的運動指令和預定的步態(tài)規(guī)劃路徑自動運行，在調(diào)試階段完成由監(jiān)控計算機人機界面發(fā)送的各自由度獨立動作或部分連續(xù)動作命令規(guī)定的運動要求。

3）系統(tǒng)參數(shù)存儲功能

有些諸如PI控制的比例系數(shù)與積分系數(shù)、液壓缸運動位移上下限、比例閥閥芯正反向運動死區(qū)等參數(shù)常量需要存儲在系統(tǒng)的FLASH中，以便機器人正常工作時，控制程序可以直接讀取FLASH中相關(guān)參數(shù)并加載到相應的控制程序中。

4）監(jiān)控管理功能

將機器人運動過程中檢測到的實時數(shù)據(jù)如通訊狀態(tài)、故障信號、運行狀態(tài)等信息實時發(fā)送到監(jiān)控計算機并進行存儲。監(jiān)控計算機需要根據(jù)系統(tǒng)要求設計相應的人機交互界面，并以數(shù)字和動畫相結(jié)合的方式直觀地反映出四足機器人的運行狀態(tài)、故障信號等。通過該界面，能夠方便地瀏覽當前以及歷史數(shù)據(jù)。

2 控制系統(tǒng)的構(gòu)成

目前的機器人控制系統(tǒng)結(jié)構(gòu)主要有集中式、分布式、分層遞階式三種形式[3,4]。分布式系統(tǒng)結(jié)構(gòu)由多個相對獨立的子系統(tǒng)構(gòu)成，每個子系統(tǒng)采用獨立的微控制器來控制，彼此間通過總線實現(xiàn)數(shù)據(jù)的交換。分布式控制系統(tǒng)具有模塊化，可靠性高、實時性強、擴展性好的優(yōu)點[5,6]。

JQRI00型四足機器人是一個具有一定的負載能力和能適應復雜環(huán)境行走的機器人。其控制系統(tǒng)根據(jù)功能需求設計為三層結(jié)構(gòu)，如圖2所示。整個控制系統(tǒng)采用多層分布式結(jié)構(gòu)，由監(jiān)控層、控制層、設備層三部分組成[7]。控制層中各控制器之間通過專門設計的FPGA通信模塊交換數(shù)據(jù)，其中主控制器將規(guī)劃好的行走路線、步態(tài)位姿等信息細分解到各子控制器，控制層通過基于UDP協(xié)議的無線通信與監(jiān)控計算機進行數(shù)據(jù)交換。設備層是控制系統(tǒng)的底層，負責執(zhí)行具體的動作。

圖2 控制系統(tǒng)結(jié)構(gòu)

根據(jù)上述硬件功能配置，軟件功能也相應的分為三個模塊：

1）監(jiān)控模塊：對JQRI00型四足機器人行走過程中的運行狀態(tài)進行實時監(jiān)控。

2）主控制器模塊：對JQRI00型四足機器人進行路徑、步態(tài)和位姿規(guī)劃，并把規(guī)劃好的足底坐標轉(zhuǎn)換成各個關(guān)節(jié)的控制量，同時將各子控制器采集的運動狀態(tài)參數(shù)上傳給監(jiān)控機。

3）子控制器模塊：對機器人各個關(guān)節(jié)的比例閥進行控制，驅(qū)動關(guān)節(jié)運動，同時采集運動過程中的狀態(tài)參數(shù)并實時傳遞給主控制器。包括四個子控制器，每個子控制器獨立控制一條腿。

三個功能模塊之間相對獨立，完成各自的任務并相互之間進行數(shù)據(jù)通信。監(jiān)控模塊、主控制器和子控制器使用不同的CPU。目前常用的控制器有單片機、DSP、ARM、PC等。由于行走機器人控制系統(tǒng)要求精度高、實時性強、靈活性高。本系統(tǒng)中主控制器采用32位ARM9內(nèi)核的工控主板；子控制器采用德州儀器公司針對實時控制應用專門推出的C2000系列32位微控制器——TMS320F28027；而監(jiān)控計算機主要實現(xiàn)數(shù)據(jù)監(jiān)控，要求較好的人機交互性，這里選用自帶無線網(wǎng)卡的普通筆記本電腦實現(xiàn)。

2.1 主控制器功能結(jié)構(gòu)

主控制器是JQRI00型四足機器人控制系統(tǒng)中的核心部分。實際運行時，主控制器采用ARM9內(nèi)核的嵌入式模塊，在WINCE5.0操作系統(tǒng)下以VC++語言進行運動規(guī)劃程序設計，完成機器人整體姿態(tài)分析、運動規(guī)劃與診斷。主控制器連接有并行ISA總線、以太網(wǎng)接口、RS232接口和帶光電隔離的輸入輸出模塊。各部分主要完成以下功能：

1）并行ISA總線與通信模塊的并行ISA 總線接口相連，實現(xiàn)主控制器對通信模塊內(nèi)各組雙端口RAM 的直接讀寫[8]。

2）以太網(wǎng)接口連接無線路由器，與監(jiān)控機定時通信，發(fā)送所有計算、測量的數(shù)值給監(jiān)控機，并接收監(jiān)控機發(fā)送的控制命令。

3）RS232接口連接傾角傳感器，接收傾角傳感器發(fā)送的空間3個角度的值。

4）帶光電隔離的輸入輸出模塊連接油泵，負責控制油泵的啟動以及停止并為將來的后續(xù)擴展作預留。

5）FPGA通信模塊利用FPGA的可編程邏輯設計功能，在FPGA內(nèi)構(gòu)建有四組雙端口RAM模塊以存儲實時數(shù)據(jù)和控制指令，通信模塊具有一個并行ISA總線接口以及四個高速串行SPI總線接口，SPI總線接口與子控制器相連接。

6）主控制器上還可以連接有USB接口，USB接口以及以太網(wǎng)接口模塊負責完成主控制器的在線配置以及調(diào)試。

主控制器的結(jié)構(gòu)如圖3所示。

圖3 主控制器的結(jié)構(gòu)

2.2 子控制器功能結(jié)構(gòu)

子控制器的主要功能是接收主控制器命令數(shù)據(jù)，伺服驅(qū)動各關(guān)節(jié)動作。獲取機器人運行過程中液壓缸位移量、運動方向、通信故障等信息并反饋至主控制器進行處理分析[2]，如圖4所示。子控制器包含高速串行SPI總線、ADC模塊、模擬量采集模塊、PWM模塊、伺服驅(qū)動模塊以及差分驅(qū)動模塊。各部分主要完成以下功能：

1）高速串行SPI總線經(jīng)差分驅(qū)動模塊與專用通信模塊中的SPI接口相連，以便實現(xiàn)子控制器對雙端口RAM模塊的實時讀寫[9,10]。

2）模擬量采集模塊分別連接位移傳感器和力傳感器，模擬量采集模塊將位移傳感器及力傳感器反饋的電流信號轉(zhuǎn)換為合適的電壓信號后輸入ADC模塊，以實現(xiàn)對位移及力信號的采集與轉(zhuǎn)換。實際中，位移傳感器與各液壓缸同軸安裝；多個力傳感器安裝在腿的各關(guān)節(jié)處以及足底。

3）ADC模塊連接模擬量采集模塊，模擬量采集模塊采集傳感器傳輸?shù)母鞣N模擬量，將其傳輸給ADC 模塊，ADC模塊進行模數(shù)轉(zhuǎn)換，將采集的信息轉(zhuǎn)換為數(shù)字信號后傳輸給子控制器，子控制器根據(jù)ADC模塊傳過來的信息，再結(jié)合從通信模塊傳過來的控制指令，計算得到控制器輸出量，控制器輸出量經(jīng)過PWM模塊，得到模擬控制量，再經(jīng)過伺服驅(qū)動模塊，驅(qū)動液壓缸進行動作。

4）伺服驅(qū)動模塊包括PWM轉(zhuǎn)DA電路與恒流斬波電路，通過控制PWM信號占空比的大小來實現(xiàn)控制通過比例閥線圈電流的大小，進而控制閥芯的運動方向與開度，最終實現(xiàn)對液壓缸運動方向與速度的控制。

5）由于主控制器與位于四條腿上的子控制器有一定距離，采用差分驅(qū)動模塊可以有效提高信號的傳輸距離并抑制噪聲。

圖4 子控制器的結(jié)構(gòu)

2.3 FPGA通信

現(xiàn)場可編程門陣列（Field－Programmable Gate Array），簡稱FPGA，它是在PAL、GAL、CPLD等可編程器件的基礎上進一步發(fā)展起來的，是為了在克服定制電路缺點的同時解決原有可編程器件門電路數(shù)量有限的不足而研發(fā)出來的一種半定制電路[11]。FPGA集成有內(nèi)嵌乘法器、專用運算電路以及可靈活配置的片內(nèi)RAM等資源，再加上其并行性、可靈活配置的位寬以及豐富的電平接口，都使其成為本課題中進行通信模塊設計的最佳選擇[12]。

如圖5所示，通信模塊包括并行ISA總線接口以及四個串行SPI總線接口，每個串行SPI總線接口各連接有雙端口RAM模塊，串行SPI總線接口與雙端口RAM模塊的一端相連，雙端口RAM模塊的另一端與并行ISA總線接口相連。

雙端口RAM存儲相應的狀態(tài)信息和控制指令，且狀態(tài)信息和控制指令數(shù)據(jù)在各組雙端口RAM中分區(qū)配置；其一端與并行ISA總線接口相連，另一端分別與對應的高速串行SPI總線接口連接。并行ISA總線接口一端與主控制器的并行ISA總線相連，另一端與各組雙端口RAM模塊相連，以便實現(xiàn)主控制器對雙端口RAM模塊的實時讀寫。串行SPI總線接口一端與雙端口RAM模塊連接另一端與子控制器SPI總線連接，以便實現(xiàn)子控制器對雙端口RAM模塊的實時讀寫[13,14]。

圖5 通信模塊的結(jié)構(gòu)

圖6是通信模塊的內(nèi)部結(jié)構(gòu)示意圖。其包括：讀寄存器、寫寄存器、發(fā)送寄存器、接收寄存器、脈沖計數(shù)器、地址寄存器、讀地址計數(shù)器、寫地址計數(shù)器和讀寫控制模塊等。

脈沖計數(shù)器分別連接發(fā)送寄存器、接收寄存器和讀寫控制模塊，讀寫控制模塊分別連接讀地址計數(shù)器、地址寄存器、寫地址計數(shù)器和雙端口RAM模塊，地址寄存器連接雙端口RAM模塊，接收寄存器連接寫寄存器，寫寄存器連接雙端口RAM模塊，雙端口RAM模塊連接讀寄存器，讀寄存器連接發(fā)送寄存器。

通信模塊中各SPI模塊均工作于從機模式，子控制器中的SPI模塊工作于主機模式。當SPI模塊檢測到主機選擇線（SS）下降沿的跳變信號時，啟動SPI通信，首先將讀、寫地址計數(shù)器，地址寄存器以及SCK 脈沖計數(shù)器初始化；然后將當前讀地址計數(shù)器的值賦給雙端口RAM模塊的地址寄存器，同時讀寫控制模塊產(chǎn)生讀信號以及使能信號，讀取雙端口RAM模塊中儲存的由精簡ISA總線寫入的命令數(shù)據(jù)的第一個字節(jié)賦給讀寄存器并將其載入SPI發(fā)送寄存器；當接收到第一個SCK脈沖時：在脈沖的上升沿將發(fā)送寄存器的最高位移出并送上主機輸入/從機輸出線（MISO），在脈沖的下降沿接收主機輸出/從機輸入線（MOSI）上的信號并移入接收寄存器的最低位；接收到第二個脈沖時，重復第一個脈沖的過程，直到完成八個脈沖的動作后將脈沖計數(shù)器清零，同時將接收寄存器的值賦給寫寄存器，并將寫地址計數(shù)器的值賦給雙端口RAM模塊的地址寄存器，讀寫控制模塊產(chǎn)生寫信號以及使能信號，完成第一個接收數(shù)據(jù)的寫入雙端口RAM模塊，最后，讀、寫地址計數(shù)器加1，至此，一個字節(jié)數(shù)據(jù)的完整通信就完成了，重復上述過程直到從機選擇線（SS）信號被拉高，表明此次SPI通信結(jié)束，等待檢測到下一個從機選擇線（SS）下降沿信號開始下一次通信。通信模塊控制邏輯流程如圖7所示。

圖6 通信模塊的內(nèi)部結(jié)構(gòu)

圖7 SPI模塊控制邏輯

3 結(jié)束語

在四足機器人原理樣機上進行的大量模擬試驗表明，基于FPGA的四足機器人分布式控制系統(tǒng)，具有實時數(shù)據(jù)通信能力、能夠進行有效路徑規(guī)劃和實時精確控制關(guān)節(jié)運動。主控制器負責機器人整體姿態(tài)分析、運動規(guī)劃與診斷；子控制器負責每條腿運動參數(shù)采集和各自由度運動的速度、位置控制；通信模塊在FPGA內(nèi)構(gòu)建多組雙端口的RAM模塊存儲實時數(shù)據(jù)和控制指令，主控制器通過并行ISA總線直接讀寫所有各組雙端口RAM模塊，子控制器通過串行SPI總線直接讀寫對應一組雙端口RAM模塊，狀態(tài)信息和控制指令數(shù)據(jù)在各組雙端口RAM模塊中分區(qū)配置，可靠地實現(xiàn)主控制器與各子控制器間毫秒級的高速數(shù)據(jù)交換。

[1]Matuszek C,Herbst E,Zettlemoyer L,et al.Learning to parse natural language commands to a robot control system[C]//Experimental Robotics.Springer International Publishing,2013:403-415.

[2]卞新高,朱燈林,王紅路,等.基于CAN總線的四足機器人控制系統(tǒng)[J].2011.

[3]陳學東,孫翔,賈文川.多足步行機器人運動規(guī)劃與控制[M].武漢:華中科技大學出版社,2006,6.

[4]陳德明.四足仿生機器人運動控制系統(tǒng)的設計與實現(xiàn)[D].西北工業(yè)大學,2007,4.

[5]王沫楠,孫立寧.多運動方式移動機器人控制系統(tǒng)設計[J].電機與控制學報,2005,9.

[6]樊昌信,曹麗娜.通信原理[M].北京:國防工業(yè)出版社2006,9.

[7]汪赟.基于FPGA的四足機器人控制系統(tǒng)設計[D].河海大學,2013,6.

[8]蘇旭武,楊光友,程良明,等.一種基于ISA總線的FPGA在線配置方法[J].機械與電子,2005(5):47-49.

[9]王玨文,金偉信,蔡一兵,等.基于FPGA的SPI總線接口的實現(xiàn)[J].現(xiàn)代電子技術(shù),2010,33(14):102-104.

[10]王松.基于FPGA的串行外圍接口SPI設計與實現(xiàn)[J].微計算機信息,2010,32:117-119.

[11]鄧強華.基于SOPC的數(shù)字光端機的設計[D].中南大學,2009.

[12]吳厚航.深入淺出玩轉(zhuǎn)FPGA[M].北京:北京航空航天大學出版社,2010

[13]Nie Hua,Liu Kaihua,Sun Chunguang,Liu Zhigang .Research on serial port communication between DSP and FPGA[J].Electronic Measurement Technology,2006,29(6):112-114.

[14]Tanaka H,Ohnishi K,Nishi H,et al.Implementation of Bilateral Control System Based on Acceleration Control Using FPGA for Multi-DOF Haptic Endoscopic Surgery Robot[J].Industrial Electronics,IEEE Transactions on,2009,56(3):618-627.