五自由度工業(yè)機器人控制系統(tǒng)設(shè)計

朱慶爽 ，王平 ，董登峰 ，周維虎

(1. 南京航空航天大學(xué) 自動化學(xué)院，江蘇 南京 210016； 2. 中國科學(xué)院 光電研究院，北京 100094)

0 引言

傳統(tǒng)的工業(yè)機器人控制系統(tǒng)通常采用專用微處理器、專用計算機和專用機器人語言的封閉式控制系統(tǒng)結(jié)構(gòu)，這種結(jié)構(gòu)限制了機器人系統(tǒng)的開放性和通用性。隨著中國制造業(yè)的升級轉(zhuǎn)型，工業(yè)機器人的使用越來越多，然而工業(yè)機器人的市場份額主要被外國企業(yè)占據(jù)，而且價格比較昂貴，并不能滿足國內(nèi)的一些中小企的需求。因此國內(nèi)的中小企業(yè)更需要價格便宜，開放性較高，操作簡便的自動化設(shè)備[1]。本文主要目的是采用“PC+DSP+FPGA”架構(gòu)設(shè)計實現(xiàn)一個五自由度工業(yè)機器人控制系統(tǒng)。并且使開發(fā)的五自由度工業(yè)機器人控制系統(tǒng)可以完成注塑機的下料作業(yè)任務(wù)，同時具有較高的開放性、較低的成本、良好的性能和可操作性。

1 控制系統(tǒng)硬件總體設(shè)計

硬件部分是工業(yè)機器人控制系統(tǒng)的基礎(chǔ)，其中硬件的性能對工業(yè)機器人控制系統(tǒng)控制的精度和實時性有決定性影響。本文設(shè)計的控制系統(tǒng)主要包括人機接口設(shè)備和運動控制器2個部分。人機接口設(shè)備提供可視化監(jiān)控界面，并且可以通過人機接口對機器人完成示教操作；運動控制器則負責(zé)運動學(xué)解算、插補運算、軌跡規(guī)劃以及控制機器人各個關(guān)節(jié)的位置和加減速等[2]。

為了滿足工業(yè)機器人的作業(yè)任務(wù)，同時使控制系統(tǒng)的硬件具有較好擴展性、較高的性能以及低成本的特點。由于DSP具有強大的計算能力，非常適合工業(yè)機器人的運動學(xué)解算和軌跡規(guī)劃等大量復(fù)雜的實時計算，以及FPGA強大的并行計算能力可以實現(xiàn)對五路增量式光電編碼器的快速采集，所以采用DSP+FPGA結(jié)構(gòu)作為控制系統(tǒng)的運動控制器。同時由于通用的PC平臺擁有良好軟件開發(fā)環(huán)境，并且具有豐富的通信接口，例如以太網(wǎng)、串口、USB接口等，因此控制系統(tǒng)的人機接口設(shè)備選擇了PC，也就是控制系統(tǒng)的上位機。綜上所述，下料工業(yè)機器人控制系統(tǒng)采用了基于“PC+DSP+FPGA”的架構(gòu)[3-5]。圖1是五自由度工業(yè)機器人控制系統(tǒng)總體硬件框圖。系統(tǒng)主要由上位機(PC)、DSP控制模塊、FPGA采集模塊、驅(qū)動器、伺服電機以及減速器等構(gòu)成。

圖1 工業(yè)機器人控制系統(tǒng)總體硬件框圖

用戶可以在上位機示教和檢測機器人的運動狀態(tài)，在示教過程中上位機的作用相當(dāng)于示教器。上位機通過以太網(wǎng)將機器人的作業(yè)順序、位置和速度信息發(fā)送給DSP控制模塊，DSP控制模塊將這些示教信息保存起來。示教結(jié)束后用戶在上位機上選擇軌跡的生成方式命令發(fā)送給DSP控制模塊后由其生成相應(yīng)的運動軌跡；在執(zhí)行工作任務(wù)時，機器人可以按照DSP生成的軌跡進行運動。

2 控制系統(tǒng)軟件設(shè)計

控制系統(tǒng)的軟件由DSP控制程序、FPGA采集程序和數(shù)據(jù)通信程序以及上位機的人機界面程序4個部分組成?？刂葡到y(tǒng)軟件結(jié)構(gòu)如圖2所示。DSP控制程序主要完成單關(guān)節(jié)運動控制、運動學(xué)解算、軌跡規(guī)劃、軌跡生成和參數(shù)設(shè)置等功能，以上功能的實現(xiàn)是用C語言在CCS3.3集成開發(fā)環(huán)境上編寫調(diào)試的。FPGA采集程序是完成對五路伺服電機的增量式光電編碼器信號的采集功能，其功能是用Verilog硬件描述語言在ISE 13.4集成開發(fā)環(huán)境上編寫實現(xiàn)。數(shù)據(jù)通信程序主要完成DSP與上位機的通信、DSP與FPGA的數(shù)據(jù)通信和DSP與交流伺服電機驅(qū)動器的通信。人機界面程序主要是完成對機器人示教再現(xiàn)以及顯示機器人運行狀態(tài)的功能。上位機為一臺安裝Windows 7操作系統(tǒng)的計算機，在Visual Studio 2010集成開發(fā)環(huán)境下進行開發(fā)和調(diào)試，用C#語言實現(xiàn)了上位機的人機界面程序。

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

3 DSP控制模塊軟硬件設(shè)計

DSP控制模塊的硬件電路設(shè)計包括DSP最小系統(tǒng)和通信電路的設(shè)計。DSP應(yīng)用系統(tǒng)的最小系統(tǒng)一般包括：電源電路、時鐘電路、復(fù)位電路、DSP芯片和JTAG接口等其他電路。通信電路主要包括DSP與FPGA的雙端RAM通信、DSP與驅(qū)動的CAN通信和DSP與上位機的網(wǎng)絡(luò)通信電路。

DSP程序是整個控制系統(tǒng)軟件的最重要部分，接收來自上位機人機界面發(fā)送的指令后，通過DSP計算出每個時刻各個關(guān)節(jié)角的給定值發(fā)送給交流伺服電機驅(qū)動器驅(qū)動機器人完成上位機的指令。DSP的程序主要有單關(guān)節(jié)運動控制、運動學(xué)解算、關(guān)節(jié)空間軌跡規(guī)劃、笛卡兒空間軌跡規(guī)劃以及軌跡生成等子程序。為了使DSP程序邏輯更加清晰同時方便代碼的維護，因此在編寫以上子程序時分別在不同的C文件中實現(xiàn)上述功能，同時為每個C文件編寫與之對應(yīng)的頭文件，這只需在主程序中包含以上頭文件就可以了，使程序結(jié)構(gòu)簡單清晰，方便修改和維護。DSP主程序流程框圖如圖3所示。

圖3 DSP主程序流程框圖

DSP上電系統(tǒng)復(fù)位后，進行系統(tǒng)初始化、GPIO初始化、重新映射外部中斷3和外部中斷4、初始化外設(shè)主要是W5300網(wǎng)口管理模塊和eCAN模塊初始化。然后進入循環(huán)檢查W5300網(wǎng)口模塊是否連接，若socket已經(jīng)成功連接，則允許外部中斷3和外部中斷4，然后循環(huán)等待外部中斷；若socket沒有連接成功，則禁止外部中斷3和外部中斷4，然后循環(huán)等待socket連接。本文中的外部中斷3選用GPIO62引腳作為觸發(fā)源，外部中斷4選用GPIO63引腳作為觸發(fā)源；在設(shè)計硬件電路時將DSP的GPIO62引腳和GPIO63引腳通過PC104接口與FPGA的引腳相連，由FPGA的引腳作為觸發(fā)源；同時需要在軟件中初始化配置GPIO62引腳和GPIO63引腳[6]。代碼如下：

GpioCtrlRegs.GPBMUX2.bit.GPIO62 = 0; // GPIO62作為通用輸入輸出

GpioCtrlRegs.GPBDIR.bit.GPIO62 = 0; //GPIO62作為輸入

GpioCtrlRegs.GPBQSEL2.bit.GPIO62 = 1; // 輸入量化限定3個采樣周期

GpioIntRegs.GPIOXINT3SEL.bit.GPIOSEL = 62; // GPIO62為外部中斷XINT3的中斷源

XIntruptRegs.XINT3CR.bit.POLARITY = 1; //上升沿觸發(fā)中斷

XIntruptRegs.XINT3CR.bit.ENABLE = 1; // XINT3 中斷允許

GpioCtrlRegs.GPBMUX2.bit.GPIO63 = 0; // GPIO63作為通用輸入輸出

GpioCtrlRegs.GPBDIR.bit.GPIO63 = 0; //GPIO63作為輸入

GpioCtrlRegs.GPBQSEL2.bit.GPIO63 = 1; //輸入量化限定3個采樣周期

GpioIntRegs.GPIOXINT4SEL.bit.GPIOSEL = 63; // GPIO63為外部中斷XINT3的中斷源

XIntruptRegs.XINT4CR.bit.POLARITY = 0; //下降沿觸發(fā)中斷

XIntruptRegs.XINT4CR.bit.ENABLE = 1; //XINT4 中斷允許

GpioCtrlRegs.GPBCTRL.bit.QUALPRD3 = 0x03; // 中斷周期為：2* QUALPRD3*Tsys

// QUALPRD3控制GPIO63～56引腳

按照以上硬件和程序的設(shè)計，外部中斷3和外部中斷4的觸發(fā)源是以FPGA產(chǎn)生的時鐘作為時基，在程序中可以設(shè)置中斷時間間隔來定時觸發(fā)外部中斷3和外部中斷4。這里設(shè)計編碼器的采樣周期為外部中斷時間。當(dāng)該中斷時鐘的上升沿到來時，觸發(fā)外部中斷3進入中斷服務(wù)子程序中，在外部中斷3服務(wù)子程序中接收來自上位機的指令，讀取FPGA采集的數(shù)據(jù)并且根據(jù)相應(yīng)的指令調(diào)用不同的子程序來計算處理，然后將處理的結(jié)果封裝成驅(qū)動器的CAN協(xié)議指令格式，存儲在指定的位置后返回外部中斷3服務(wù)子程序。當(dāng)該中斷時鐘的下降沿到來時，觸發(fā)外部中斷4進入中斷服務(wù)子程序中，在外部中斷4服務(wù)子程序中將外部中斷3服務(wù)子程序中生成的驅(qū)動器的指令發(fā)送給驅(qū)動器，然后將FPGA采集的數(shù)據(jù)發(fā)送給上位機后返回外部中斷4服務(wù)子程序。

4 FPGA采集模塊軟硬件設(shè)計

FPGA采集模塊的硬件電路設(shè)計包括FPGA最小系統(tǒng)和編碼器采集電路設(shè)計。其中FPGA最小系統(tǒng)主要包括：外部時鐘、復(fù)位電路、電源電路、下載電路、FPGA芯片和JTAG接口電路等。編碼器采集電路主要對五路伺服電機的增量式光電編碼器信號進行采集。

FPGA采集程序是用Verilog硬件描述語言在ISE13.4軟件開發(fā)平臺編程實現(xiàn)的。FPGA數(shù)據(jù)采集程序主要包括同步時鐘產(chǎn)生模塊和五路增量式光電編碼器采集模塊。由于DSP的外部中斷3和外部中斷4中斷的基準(zhǔn)時鐘信號是由FPGA產(chǎn)生的，也就是這里的FPGA產(chǎn)生的同步時鐘信號。因此它不僅是DSP與FPGA雙端RAM通信基準(zhǔn)時鐘，也是DSP控制器與上位機通信的基準(zhǔn)時鐘。由于本文所選擇的交流伺服電機驅(qū)動器的電流環(huán)控制頻率為10 kHz，因此同步時鐘的頻率也選擇為10 kHz；而位置環(huán)的控制頻率和增量式光電編碼器的采集頻率均采用1 kHz。在硬件電路設(shè)計時為FPGA工作時鐘選為50 MHz，因此可以非常簡單地通過Verilog語言編程對50 MHz分頻得到10 kHz的同步信號，由于其程序比較簡單，這里不再詳細地介紹如何產(chǎn)生同步時鐘信號。

本文所選的SEM交流伺服電機集成了增量式光電編碼器輸出的A、B、Z 3對差分脈沖信號，將3對差分信號經(jīng)過單端信號轉(zhuǎn)換芯片轉(zhuǎn)換為3路單端信號A、B、Z送入FPGA芯片。其編碼器的輸出波形如圖4 所示。

圖4 增量式光電編碼器輸出波形

在FPGA采集程序中，通過判斷A和B的相位關(guān)系確定伺服電機的轉(zhuǎn)動方向；然后對A和B的脈沖計數(shù)來確定伺服電機轉(zhuǎn)過的角度，并且在檢查到Z相脈沖時對計數(shù)器清零。所以FPGA采集程序主要包括2個模塊：濾波模塊和解碼計數(shù)模塊。FPGA采集程序結(jié)構(gòu)框圖如圖5所示。

圖5 FPGA采集程序結(jié)構(gòu)框圖

設(shè)計濾波模塊主要是消除外界環(huán)境產(chǎn)生的噪聲信號對編碼器信號的影響。該程序中的濾波模塊主要是濾除高頻噪聲對編碼器信號的干擾，就是將高于編碼器正常工作時產(chǎn)生的脈沖頻率的脈沖信號除去。

對編碼器信號進行濾波后，信號進入解碼計數(shù)模塊。編碼器輸出的是正交脈沖信號，因此這里對編碼器的A、B信號進行正交解碼得到計數(shù)信號。計數(shù)信號是在A相和B相上升沿和下降沿各自產(chǎn)生一個脈沖信號，因此計數(shù)信號的頻率為A相、B相頻率的4倍。然后對其計數(shù)從而提高了編碼器的測量精度。

在解碼計數(shù)模塊中將編碼器的輸出脈沖高電平用1表示，低電平用0表示。如圖4所示，一個信號周期中AB有4組狀態(tài)：10、11、01、00。由于A、B信號是正交脈沖信號，當(dāng)電機正向轉(zhuǎn)動時A相的相位超前B相90°，則AB兩相的狀態(tài)變化依次為：00、10、11、01、00；，當(dāng)電機反向轉(zhuǎn)動時B相的相位超前A相90°，則AB兩相的狀態(tài)變化依次為：00、01、11、10、00；因此可以根據(jù)AB兩相的當(dāng)前狀態(tài)和上一狀態(tài)的變化關(guān)系判斷電機的運動：正向運動、反向運動、靜止不動、錯誤4種結(jié)果。詳細結(jié)果見表1。

表1 AB狀態(tài)變化關(guān)系

在解碼計數(shù)模塊中按照表1對A和B兩相的相位關(guān)系進行解碼得到電機轉(zhuǎn)動的方向信號。編碼器采集程序具體實現(xiàn)過程如下[7]：首先將輸入信號A和B組合為1個位寬為2位的wire型信號AB；然后在解碼計數(shù)模塊中定義2個位寬為2位的reg型變量cur_state和pre_state，分別存儲上一狀態(tài)和當(dāng)前狀態(tài)，并且將它們初始化為cur_state=2’b00、pre_state=2’b00。如果當(dāng)前狀態(tài)和上一狀態(tài)一樣時，則電機處于靜止不動狀態(tài)；當(dāng)A或者B信號發(fā)生變化時，將當(dāng)前狀態(tài)存儲在cur_state寄存器中，并且根據(jù)表1判斷電機的轉(zhuǎn)動方向，然后計數(shù)器模塊進行相應(yīng)的加1或者減1運算，并且將上一狀態(tài)pre_state更新為當(dāng)前狀態(tài)cur_state；同時如果檢測到Z相脈沖信號，則對計數(shù)器進行回零操作，并將過零計數(shù)器加1表示電機旋轉(zhuǎn)了1圈。等待A、B、Z信號的變化觸發(fā)下次計數(shù)，這樣反復(fù)進行就可以完成對編碼器信號的采集。

5 數(shù)據(jù)通信程序設(shè)計

5.1 DSP與驅(qū)動器CAN通信

DSP向驅(qū)動器發(fā)送信息的流程圖如圖6所示。首先對eCAN模塊初始化，并且清除發(fā)送請求寄存器CANTRS，然后根據(jù)通訊協(xié)議初始化郵箱的標(biāo)識寄存器MSGID；將協(xié)議中的數(shù)據(jù)分別寫入消息數(shù)據(jù)寄存器CANMDL和CANMDH中后置位相應(yīng)的TRS(1為啟動發(fā)送，0為無操作)。等待發(fā)送應(yīng)答寄存器相應(yīng)的TA置為；最后復(fù)位發(fā)送應(yīng)答寄存器相應(yīng)的TA等待下次發(fā)送數(shù)據(jù)。

圖6 CAN發(fā)送信息流程圖

5.2 DSP與FPGA雙端RAM通信

程序的設(shè)計思路是FPGA采集完所有的數(shù)據(jù)后存放在片內(nèi)SRAM指定的地址中，其存儲空間為4 K字，地址范圍是0x004000～0x004FFF，并且片選信號為XZCS6，因此將此處的存儲空間映射到了區(qū)域0。工作原理為：同步時鐘是DSP與FPGA雙端RAM通信基準(zhǔn)時鐘，其時鐘的頻率為10 kHz；由于增量式光電編碼器的采集頻率為1 kHz(也就是編碼器的計數(shù)信號的存儲頻率)，因此需要將同步時鐘分頻得到1 kHz的編碼器的采集時鐘。而位置環(huán)的控制頻率也是1 kHz，即外部中斷的頻率。當(dāng)以同步時鐘為基準(zhǔn)時鐘設(shè)定的中斷定時到來時，將觸發(fā)外部中斷3和外部中斷4；進入外部中斷子程序，在這里DSP首先使能片選信號XZCS6，然后給出上述指定的地址，當(dāng)讀信號到來時，讀取FPGA對5路編碼器的采集數(shù)據(jù)。由于控制頻率和編碼器采集頻率都是1 kHz，為了保證當(dāng)讀編碼器的采集數(shù)據(jù)時，F(xiàn)PGA能夠完成對5路編碼器數(shù)據(jù)的采集并且存放在指定的地址中。所以這里在第k時刻讀取第k-1時刻采集的數(shù)據(jù)。但是這樣就造成電機的控制和上位機的數(shù)據(jù)顯示造成一個時鐘周期的延時，由于時間較短引起的延遲可以忽略。

5.3 DSP與上位機通信

DSP通過網(wǎng)絡(luò)通信與上位機進行數(shù)據(jù)的傳輸。網(wǎng)絡(luò)協(xié)議采用TCP傳輸控制協(xié)議，TCP是有連接的、可靠的傳輸協(xié)議。其中DSP作為服務(wù)端，上位機作為客戶端。圖7為DSP與上位機通信流程圖，其中圖7(a)上位機客戶端程序通信程序流程圖，圖7(b)DSP服務(wù)端程序通信程序流程圖。在網(wǎng)絡(luò)通信中，DSP服務(wù)端需要首先開啟網(wǎng)絡(luò)服務(wù)；首先將DSP端的網(wǎng)絡(luò)芯片W5300進行初始化，為其硬件地址、網(wǎng)關(guān)地址、子網(wǎng)掩碼、網(wǎng)關(guān)地址、IP地址設(shè)置好相應(yīng)的地址，并且配置TX和RX存儲器的大小，以及將SOCKET0設(shè)置為TCP模式然后打開此端口。然后調(diào)用SOCKET_Listen(0)函數(shù)檢測SOCKET0是否有上位機連接，循環(huán)等待上位機客戶端連接。上位機客戶端根據(jù)DSP服務(wù)端的IP地址和端口號設(shè)置異步通信回調(diào)函數(shù)AsynCallback()和實例化客戶端TcpClient；通過判斷回調(diào)函數(shù)返回的結(jié)果判斷socket是否連接成功。由上位機客戶端發(fā)起連接請求，如果DSP和上位機連接成功，則將連接標(biāo)志位con_flag置為1。然后在DSP服務(wù)端調(diào)用數(shù)據(jù)接收函數(shù)Rx_Process()和數(shù)據(jù)發(fā)送函數(shù)Tx_Process()進行數(shù)據(jù)傳輸，在上位機客戶端調(diào)用數(shù)據(jù)接收函數(shù)BinaryReader.Read()與數(shù)據(jù)發(fā)送函數(shù)BinaryWriter.Write()進行數(shù)據(jù)傳輸。若連接斷開，則在服務(wù)端將SOCKET0關(guān)閉并將con_flag置為0；在客戶端將實例化的客戶端TcpClient關(guān)閉并將con_flag置為0。

圖7 與上位機通信程序流程圖

6 人機界面

上位機界面主要實現(xiàn)的功能是顯示下料工業(yè)機器人每個關(guān)節(jié)的實時狀態(tài)、對機器人進行示教、軌跡生成以及控制機器人的運行和停止等。如圖8上位機人機界面所示。首先在通信區(qū)域單擊“建立連接”按鈕，向DSP服務(wù)端發(fā)送連接請求，如果連接成功，則在相應(yīng)的textBox顯示服務(wù)器的IP地址：192.168.0.20和端口號：50000。否則顯示未連接。當(dāng)連接成功時則DSP服務(wù)端將每個關(guān)節(jié)的實際位置發(fā)送給上位機，在顯示關(guān)機實時位置區(qū)域中實時更新每個關(guān)節(jié)的實際位置。在示教區(qū)域單擊相應(yīng)關(guān)節(jié)的“正轉(zhuǎn)”、“反轉(zhuǎn)”，上位機則發(fā)送相應(yīng)的指令給DSP，然后發(fā)送給相應(yīng)關(guān)節(jié)的驅(qū)動器驅(qū)動關(guān)節(jié)運動；如果有單擊“記錄”按鈕，上位機則發(fā)送記錄指令，DSP記錄此時各個關(guān)節(jié)的關(guān)節(jié)變量值并保存在指定位置；在示教結(jié)束前選擇軌跡生成方式，單擊示教“結(jié)束”按鈕，上位機則將示教結(jié)束指令和軌跡生成方式發(fā)送給DSP，然后DSP按照記錄的示教點和軌跡生成方式生成機器人的運動軌跡，并且將運動軌跡標(biāo)志位置為1，即Traj=1。當(dāng)有單擊執(zhí)行區(qū)域的“運行”按鈕，則上位機發(fā)送運行指令給DSP，DSP然后去判斷運動軌跡標(biāo)志位，若Traj=1則DSP將之前生成的軌跡數(shù)據(jù)發(fā)送給驅(qū)動器驅(qū)動機器人運動，若Traj=0則機器人靜止不動；當(dāng)有單擊執(zhí)行區(qū)域的“停止”按鈕，則上位機發(fā)送停止指令給DSP，DSP停止給驅(qū)動器發(fā)送軌跡數(shù)據(jù)。

圖8 上位機人機界面

7 結(jié)語

設(shè)計了基于“DSP+FPGA+PC”架構(gòu)的控制系統(tǒng)。然后用本文開發(fā)的控制系統(tǒng)和自主設(shè)計的工業(yè)機器人本體機械結(jié)構(gòu)組成的五自由度工業(yè)機器人，并進行示教實驗，實驗結(jié)果表明初步實現(xiàn)了注塑機下料任務(wù)。同時為了使控制系統(tǒng)具有一定的開放性，在軟件設(shè)置中可以對不同的機器人機械本體通過參數(shù)修改實現(xiàn)控制，并且在電路設(shè)計時預(yù)留了一些接口，例如：串口、6路編碼器采集，6路電流采集等，方便以后擴展。