基于DSP平臺的排爆機械臂控制系統設計

陳朝大，杜啟亮，秦傳波，田聯房，肖志遠

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

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

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

1 系統設計方案及其原理功能

1.1 機械臂設計方案

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

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

圖1 機械臂設計方案

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

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

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

圖2 排爆機械臂實物圖

1.2 控制策略分析

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

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

閉環反饋控制系統如圖3所示。

圖3 閉環反饋控制系統

直流伺服電機的基本特性有機械特性、調節特性及動態特性3 種。閉環伺服系統的動態特性分析依據直流伺服電機的動態特性方程，如式(1)所示:

對式(1)進行拉普拉斯變換，可求得直流伺服電機閉環系統的傳遞函數如式(2)所示:

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

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

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

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

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

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

2 制作調試及實現

2.1 軟件流程設計

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

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

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

圖4 系統軟件設計流程圖

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

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

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

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

該系統設定foscclk=30 MHz，div=10，則fsysclkout=150 MHz。

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

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 的時間基準和系統時鐘的關系如公式(4)所示。

式(4)中:ftbclk表示EPWM 時間基準，fsysclkout表示系統時鐘，fhspclkdiv表示高速時間基準預分頻，fclkdiv表示時間基準預分頻。

該系統設定fclkdiv=128，fhspclkdiv=1，fsysclkout=150 MHz，則ftbclk=1.171 8 MHz。

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

則時間基準周期寄存器tTBPRD=11 718，所以頻率f=1/T=50 Hz，滿足系統設計要求。

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

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

該系統設定th1=0.5 ms，th2=1.5 ms，th3=2.5 ms，則 tCMPA1= 11 425，tCMPA2= 10 840，tCMPA3=10 253。可得伺服電機輸出臂位置和CMPA 值之間的關系如表3所示。

表3 伺服電機輸出臂位置和CMPA 值關系表

此次設計精度達到0.15°，而精度控制要求為1°，達到設計要求。

2.3 測試及分析

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

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

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

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

3 結束語

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

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

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

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

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

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

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

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

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