五自由度工業機器人控制系統設計

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

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

0 引言

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

1 控制系統硬件總體設計

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

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

圖1 工業機器人控制系統總體硬件框圖

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

2 控制系統軟件設計

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

圖2 控制系統軟件結構圖

3 DSP控制模塊軟硬件設計

DSP控制模塊的硬件電路設計包括DSP最小系統和通信電路的設計。DSP應用系統的最小系統一般包括：電源電路、時鐘電路、復位電路、DSP芯片和JTAG接口等其他電路。通信電路主要包括DSP與FPGA的雙端RAM通信、DSP與驅動的CAN通信和DSP與上位機的網絡通信電路。

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

圖3 DSP主程序流程框圖

DSP上電系統復位后，進行系統初始化、GPIO初始化、重新映射外部中斷3和外部中斷4、初始化外設主要是W5300網口管理模塊和eCAN模塊初始化。然后進入循環檢查W5300網口模塊是否連接，若socket已經成功連接，則允許外部中斷3和外部中斷4，然后循環等待外部中斷；若socket沒有連接成功，則禁止外部中斷3和外部中斷4，然后循環等待socket連接。本文中的外部中斷3選用GPIO62引腳作為觸發源，外部中斷4選用GPIO63引腳作為觸發源；在設計硬件電路時將DSP的GPIO62引腳和GPIO63引腳通過PC104接口與FPGA的引腳相連，由FPGA的引腳作為觸發源；同時需要在軟件中初始化配置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; //上升沿觸發中斷

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; //下降沿觸發中斷

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

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

// QUALPRD3控制GPIO63～56引腳

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

4 FPGA采集模塊軟硬件設計

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

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

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

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

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

圖5 FPGA采集程序結構框圖

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

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

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

表1 AB狀態變化關系

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

5 數據通信程序設計

5.1 DSP與驅動器CAN通信

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

圖6 CAN發送信息流程圖

5.2 DSP與FPGA雙端RAM通信

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

5.3 DSP與上位機通信

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

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

6 人機界面

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

圖8 上位機人機界面

7 結語

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