基于DSP平臺的排爆機械臂控制系統(tǒng)設(shè)計

陳朝大，杜啟亮，秦傳波，田聯(lián)房，肖志遠(yuǎn)

(1.華南理工大學(xué)，廣東廣州510641;2.廣東技術(shù)師范學(xué)院天河學(xué)院，廣東廣州510540)

排爆機器人是針對公安、航空、軍事、礦井等危險、重要、有害環(huán)境下應(yīng)用的特種作業(yè)機器人，它不受任何外界電磁干擾，可以代替人在危險、重要、有害環(huán)境中進(jìn)行觀察、檢查、搬運、清理、操作及安放特殊裝置，用以保證人員的安全、健康。由于科技含量較高，排爆機器人往往“身價”不菲［1］。

“基于DSP +FPGA 平臺的排爆機器人設(shè)計”科技攻關(guān)項目，研究的切入點是針對航空、港口、客運站這類比較重要的場所，設(shè)計一款性價比高的排爆機器人。基于DSP 平臺的排爆機械臂控制系統(tǒng)，以DSP為處理核心，利用上位機向DSP 發(fā)送指令，驅(qū)動伺服電機轉(zhuǎn)動，仿生模仿人手的各類運動，從而實現(xiàn)把危險品放于隔離箱中。

1 系統(tǒng)設(shè)計方案及其原理功能

1.1 機械臂設(shè)計方案

利用上位機向DSP 發(fā)送指令，驅(qū)動機械臂，實現(xiàn)6 個關(guān)節(jié)的協(xié)調(diào)運動。DSP 通過地址數(shù)據(jù)總線和FPGA 連接，進(jìn)而控制驅(qū)動部分。驅(qū)動部分為8 個直流電機，以履帶形式走動，驅(qū)動部分前端含超聲波測距模塊及無線視頻采集模塊，能夠識別前方是否有障礙物。

機械臂設(shè)計方案如圖1所示。TMS320F28335 擁有浮點數(shù)處理單元，6 個直接存儲器訪問DMA 通道，12 個PWM 輸出通道，6 個高精度PWM 通道，3 個SCI 通信模塊，12 個可配置外部中斷，處理能力比F2812 更強，對于復(fù)雜的運動控制排爆機器臂而言，DSP 選擇F28335 更為合適。

圖1 機械臂設(shè)計方案

利用EPWM1A-EPWM6A 產(chǎn)生6 路PWM 脈沖信號，完成對機械臂6 個關(guān)節(jié)的控制。6 個關(guān)節(jié)點采用舵機(伺服電機)驅(qū)動，設(shè)計目標(biāo):機械臂總長度為400 mm，手爪(1 號舵機)能夠抓起500 g 的物品，工作電流1 ～3 A，精度控制在1°以內(nèi)。伺服電機類型特點比較如表1所示［2－3］。

表1 伺服電機類型特點比較

為了獲得一定的力矩抓起500 g 物體，又不至于機械臂因伺服電機質(zhì)量過大而影響總體性能表現(xiàn)，此次設(shè)計該采用Tower Pro MG945 伺服電機。PC 的TXD(發(fā)送)接DSP 的RXD(接收)，PC 的RXD 接DSP的TXD，完成上位機和下位機實時全雙工的串行通信。排爆機械臂實物圖如圖2所示。

圖2 排爆機械臂實物圖

1.2 控制策略分析

排爆機械臂采用的控制方法為:根據(jù)設(shè)定的相關(guān)參數(shù)選擇控制方式和執(zhí)行機構(gòu)，現(xiàn)場控制模塊將采集到的數(shù)據(jù)通過總線實時地發(fā)送給主控機，主控機的決策軟件根據(jù)上報數(shù)據(jù)和參數(shù)進(jìn)行運算，并向各控制模塊傳輸控制量，指示相關(guān)機構(gòu)動作。機械臂由6 路伺服電機驅(qū)動，控制器為TMS320F28335。該控制器帶有SCI 模塊，允許使用NRZ 協(xié)議進(jìn)行串行通訊，因此可以直接通過SCI 模塊與主控機通訊［4］。

直流伺服電機是一種將輸入信號轉(zhuǎn)換為轉(zhuǎn)軸上的角位移或角速度來執(zhí)行控制任務(wù)的直流電機，其轉(zhuǎn)速和轉(zhuǎn)向隨輸入信號的變化而變化，并具有一定的負(fù)載能力，在各類自動控制系統(tǒng)中廣泛用作執(zhí)行元件。直流伺服電機具有響應(yīng)迅速、精度和效率高、調(diào)速范圍寬、負(fù)載能力大、控制特性優(yōu)良等優(yōu)點。

閉環(huán)反饋控制系統(tǒng)如圖3所示。

圖3 閉環(huán)反饋控制系統(tǒng)

直流伺服電機的基本特性有機械特性、調(diào)節(jié)特性及動態(tài)特性3 種。閉環(huán)伺服系統(tǒng)的動態(tài)特性分析依據(jù)直流伺服電機的動態(tài)特性方程，如式(1)所示:

對式(1)進(jìn)行拉普拉斯變換，可求得直流伺服電機閉環(huán)系統(tǒng)的傳遞函數(shù)如式(2)所示:

式(2)中:ωn為電動機的無阻尼固有頻率;ξ 為電動機的阻尼比。

由式(2)可知，該系統(tǒng)為典型二階系統(tǒng)傳遞函數(shù)形式，得出如下兩個結(jié)論:

若ωn為一定值，當(dāng)阻尼比增加時，系統(tǒng)的振蕩程度會減弱;當(dāng)阻尼比大于等于1 時，系統(tǒng)響應(yīng)曲線為單調(diào)曲線，已經(jīng)沒有了振蕩;若ξ 為一定值，當(dāng)無阻尼固有頻率增加時，系統(tǒng)的響應(yīng)速度將加快，而響應(yīng)曲線的峰值將保持不變［5］。

伺服電機接收到一個脈沖，就會旋轉(zhuǎn)一個脈沖對應(yīng)的角度，從而實現(xiàn)位移。因為伺服電機本身具備發(fā)出脈沖的功能，所以伺服電機每旋轉(zhuǎn)一個角度，都會發(fā)出對應(yīng)數(shù)量的脈沖，這樣，和伺服電機接受的脈沖形成了呼應(yīng)，或者叫閉環(huán)。系統(tǒng)就會知道發(fā)了多少脈沖給伺服電機，同時又收了多少脈沖回來，就能夠很精確地控制電機的轉(zhuǎn)動，從而實現(xiàn)精確的定位。

輸入一個周期性的正向脈沖信號，這個周期性脈沖信號的高電平時間通常在1 ～2 ms 之間，而低電平時間應(yīng)在5 ～20 ms 之間。一個典型的20 ms 周期性脈沖的正脈沖寬度與RC 伺服電機的輸出臂位置的關(guān)系如表2所示。

表2 正脈沖寬度與伺服電機輸出臂位置的關(guān)系

2 制作調(diào)試及實現(xiàn)

2.1 軟件流程設(shè)計

軟件設(shè)計的開發(fā)環(huán)境為CCS3.3 系統(tǒng)，硬件平臺為TI 公司的TMS320F28335 型數(shù)字信號處理器。與以往的定點DSP 相比，該器件的精度高，成本低，功耗小，性能高，外設(shè)集成度高，數(shù)據(jù)以及程序存儲量大，A/D 轉(zhuǎn)換更精確快速等。

利用DSP 的EPWM 模塊，可以產(chǎn)生周期為20 ms的周期性脈沖，并且通過調(diào)節(jié)CMPA 的值改變輸入正脈沖寬度，從而實現(xiàn)伺服電機的0° ～180°的轉(zhuǎn)動，令機械臂6 個關(guān)節(jié)能夠按照設(shè)計要求運動。利用DSP的SCI 模塊，上位機向SCI 發(fā)送指令，SCI 模塊接收到指令后送DSP 處理，再送EPWM 模塊［6－7］。

串行通信接口SCI 是一個采用發(fā)送、接收雙線制的異步串行通信接口，即UART 口，它支持16 級的接收發(fā)送FIFO，從而降低了串口通信時CPU 的開銷。SCI 模塊支持CPU 和其他使用NRZ 的外圍設(shè)備之間的數(shù)字通信。為了保證數(shù)據(jù)的完整，SCI 模塊對接收到的數(shù)據(jù)進(jìn)行間斷檢測、奇偶校驗、超限檢測以及錯誤檢測。通過對16 位的波特率控制寄存器進(jìn)行編程，可以配置不同的SCI 通信速率。增強型SCI 的主要特點是自動波特率檢測和16 級發(fā)送/接收FIFO。發(fā)送和接收緩沖器增加了兩個16級的FIFO，并且都能產(chǎn)生CPU中斷。無論什么時候發(fā)送的FIFO 狀態(tài)位(TXFFST)與中斷觸發(fā)級別位(TXFFIL)相匹配，都能產(chǎn)生一個中斷觸發(fā)，從而實現(xiàn)可編程的中斷觸發(fā)。系統(tǒng)軟件設(shè)計流程圖如圖4所示。

圖4 系統(tǒng)軟件設(shè)計流程圖

圖4 中，系統(tǒng)初始化完成PLL 鎖相環(huán)、看門狗及外設(shè)時鐘設(shè)置，清除所有中斷并初始化PIE 向量表，把6 路EPWM 配置為外設(shè)引腳模式，把SCI 設(shè)置為FIFO 模式，打開串口中斷，用串口中斷接收到的數(shù)據(jù)來更新6 路EPWM 的CMPA［8］。

2.2 利用EPWM 模塊獲得精確頻率及精度計算

系統(tǒng)時鐘與外接晶振頻率之間的關(guān)系受鎖相環(huán)控制寄存器PLLCR ［3∶0］的控制，其關(guān)系式如公式(3)所示:

式(3)中:fsysclkout表示系統(tǒng)時鐘，foscclk表示外接晶振頻率，div 表示鎖相環(huán)控制寄存器PLLCR ［3∶0］。

該系統(tǒng)設(shè)定foscclk=30 MHz，div=10，則fsysclkout=150 MHz。

在CCS3.3 開發(fā)環(huán)境的EPwmSetup()子程序中抽取以下核心代碼進(jìn)行分析:

EPwm1Regs.CMPCTL.all = 0x50;// Immediate mode for CMPA and CMPB

EPwm1Regs.AQCTLA.all=0x60;

EPwm1Regs.TBCTL.all =0x1c10 + TBCTLVAL;//Enable Timer

因為JHJdefine TBCTLVAL 0x200e //up-down cut，遞增遞減模式

所以TBCTL =0x1c10 +0x200e =0x3c1e =0011，1100，0001，1110

EPwm1Regs.TBPRD=11718;

EPWM 的時間基準(zhǔn)和系統(tǒng)時鐘的關(guān)系如公式(4)所示。

式(4)中:ftbclk表示EPWM 時間基準(zhǔn)，fsysclkout表示系統(tǒng)時鐘，fhspclkdiv表示高速時間基準(zhǔn)預(yù)分頻，fclkdiv表示時間基準(zhǔn)預(yù)分頻。

該系統(tǒng)設(shè)定fclkdiv=128，fhspclkdiv=1，fsysclkout=150 MHz，則ftbclk=1.171 8 MHz。

要獲得T=20 ms 的周期性脈沖，因為采用遞增遞減模式，則t=T/2 =10 ms，把t 代入公式(5):

則時間基準(zhǔn)周期寄存器tTBPRD=11 718，所以頻率f=1/T=50 Hz，滿足系統(tǒng)設(shè)計要求。

計數(shù)比較A 寄存器值和時間基準(zhǔn)周期寄存器值的關(guān)系如公式(6)所示:

式(6)中:tCPMA表示計數(shù)比較A 寄存器值，tTBPRD表示時間基準(zhǔn)周期寄存器值，th表示輸入正脈沖寬度，T 表示周期。

該系統(tǒng)設(shè)定th1=0.5 ms，th2=1.5 ms，th3=2.5 ms，則 tCMPA1= 11 425，tCMPA2= 10 840，tCMPA3=10 253?？傻盟欧姍C輸出臂位置和CMPA 值之間的關(guān)系如表3所示。

表3 伺服電機輸出臂位置和CMPA 值關(guān)系表

此次設(shè)計精度達(dá)到0.15°，而精度控制要求為1°，達(dá)到設(shè)計要求。

2.3 測試及分析

由表3 得，把CMPA 的值(2CA1H、2A58H、280DH)通過串口測試軟件分別發(fā)送出去，用示波器捕獲DSP 的EPWM 腳波形如圖5(a)—(c)所示。

圖5 示波器捕獲DSP 的EPWM 腳波形

系統(tǒng)設(shè)定CMPA 值為2CA1H，并通過串口發(fā)送，用示波器觀測到正脈沖寬度為0.5 ms 的方波，如圖5(a)所示。設(shè)定CMPA 值為2A58H，并通過串口發(fā)送，用示波器觀測到正脈沖寬度為1.5 ms 的方波，如圖5(b)所示。設(shè)定CMPA 值為280DH，并通過串口發(fā)送，用示波器觀測到正脈沖寬度為2.5 ms 的方波，如圖5(c)所示。

由圖5 可知:此次設(shè)計實際測試數(shù)據(jù)完全符合表3，通過SCI 測試軟件發(fā)送數(shù)據(jù)，改變輸入正脈沖寬度，進(jìn)而控制伺服電機輸出位置，達(dá)到控制機械臂的目的。

3 結(jié)束語

排爆機械臂控制系統(tǒng)利用上位機向DSP 發(fā)送數(shù)據(jù)，采用FIFO 模式解決連續(xù)發(fā)送數(shù)據(jù)的技術(shù)難題，數(shù)據(jù)通過EPWM 直接驅(qū)動伺服電機，令機械臂能夠仿生人手的各類運動，達(dá)到代替人們處理危險物品的目的。此次設(shè)計實現(xiàn)完成機械臂總長度為450 mm，手爪能夠抓起500 g 的物品，精度控制在1°以內(nèi)。

【1】張濱，杜啟亮，田聯(lián)房.基于FPGA 的步進(jìn)電動機加減速控制器［J］.微特電機，2012(8):71－74.

【2】杭啟樂，司賓強，朱紀(jì)洪，等.基于DSP 和CPLD 的四相容錯電機控制系統(tǒng)硬件設(shè)計［J］.計算機測量與控制，2012，20(9):2413－2415.

【3】文海鷗，向中凡，朱柏樹，等.基于DSP_FPGA 和模糊自調(diào)整PID 控制的仿人機器人控制系統(tǒng)的設(shè)計與實現(xiàn)［J］.機床與液壓，2011，39(21):101－104.

【4】傅俊楠，秦傳波，杜啟亮，等.基于LM3S8962 的靜脈輸液滴速控制系統(tǒng)［J］.計算機測量與控制，2012，20(10):2693－2695，2699.

【5】劉永民，高磊.基于DSP_FPGA 與CAN 總線的跟蹤控制器設(shè)計［J］.光電技術(shù)應(yīng)用，2012，27(4):7－11.

【6】呂常智，姚志科.基于無線通信的移動機器人遠(yuǎn)程控制系統(tǒng)［J］.煤礦機械，2012，33(3):239－241.

【7】邵艷鳴，崔澤，韓增軍，等.一種七自由度機械臂的研制［J］.機械設(shè)計與制造，2011(10):115－117.

【8】方海峰，葛世榮.具有被動擺臂的四履帶機器人越障性能分析［J］.中國礦業(yè)大學(xué)學(xué)報，2010(5):682－686.