劉士川，鄧甲昊，占銀玉，葉 勇

(1.北京理工大學(xué)機電動態(tài)控制重點實驗室，北京 100081；2.北京理工大學(xué)機電學(xué)院，北京 100081)

基于STM32的小型機械手的設(shè)計與控制方法

劉士川1，2，鄧甲昊1，2，占銀玉1，2，葉 勇1，2

(1.北京理工大學(xué)機電動態(tài)控制重點實驗室，北京 100081；2.北京理工大學(xué)機電學(xué)院，北京 100081)

針對機器手的每個關(guān)節(jié)需要進行位置和速度的控制，而伺服電機、步進電機等控制存在成本高，控制精度不夠等問題，提出了基于STM32的小型機械手的設(shè)計與控制方法。相對于電機控制來說，基于脈寬調(diào)制技術(shù)控制的舵機機械手臂有其控制簡單、靈活和動態(tài)響應(yīng)好、價格便宜等優(yōu)點。設(shè)計了機械手臂的整體結(jié)構(gòu)，分析了碼垛機械手的脈寬調(diào)制驅(qū)動方法，并在結(jié)構(gòu)設(shè)計的基礎(chǔ)上進行了運動學(xué)分析，得出了機械手的末端位姿。對機械手抓取目標物的控制系統(tǒng)進行了設(shè)計、編程及仿真實驗，采用實體建模軟件Solidworks建立小型碼垛機械手臂虛擬樣機模型。基于該模型，設(shè)計了抗環(huán)境噪聲與電路自身噪聲問題的電路，進行多次搬運實驗。實驗證明，該方法可以實現(xiàn)機械手對物體的準確抓取、運送等工作，有效地避免了電機定位不準、價格昂貴的情形，使得控制更為簡單靈活。

脈寬調(diào)制控制；機械手；運動學(xué)分析；結(jié)構(gòu)建模

0 引言

碼垛技術(shù)是現(xiàn)代工業(yè)生產(chǎn)以及物流運輸上一項必不可少的技術(shù)，該技術(shù)的成熟程度關(guān)系到工業(yè)加工的效率以及工業(yè)運輸?shù)臅r間問題，近幾年在工業(yè)生產(chǎn)中扮演著越來越重要的角色[1]。機械手(也稱末端執(zhí)行器)是碼垛機器人的重要組成部分之一，其工作性能、可靠性、結(jié)構(gòu)等參數(shù)關(guān)系到碼垛機器人的整體工作性能的好壞。機器手的驅(qū)動可以有很多選擇，比如直流電機、伺服電機、舵機、步進電機。由于機器手的每個關(guān)節(jié)需要進行位置和速度控制，直流電機要實現(xiàn)上述控制必須增加位置和速度傳感器，這就增加了安裝困難和機器人成本。伺服電機雖然可以滿足機械手的控制，但是伺服電機本身體積大、價格昂貴，很難應(yīng)用于機械手上。步進電機體積小、價格便宜，但是比較重、控制精度不夠。本文針對上述問題，采用舵機控制，提出了基于STM32的小型機械手的設(shè)計與控制方法。

1 機械手系統(tǒng)總體設(shè)計

1.1 小型碼垛機器手總體方案

本文主要是針對碼垛機器手設(shè)計一個機械手的結(jié)構(gòu)和控制系統(tǒng)，采用上位機和下位機結(jié)合的方式完成和實現(xiàn)搬運任務(wù)。一方面通過上位機遙控實現(xiàn)機械手上下抓取物體、姿態(tài)調(diào)整、轉(zhuǎn)動搬運物體等基本功能；另一方面機械手可以通過自身的紅外測距傳感器和姿態(tài)傳感器實現(xiàn)自身動作的調(diào)整和姿態(tài)的變換，并將姿態(tài)信息記入FLASH存儲中，每個完成的動作通過底座外接的LCD1602顯示屏展示出具體情況。上位機和下位機通過無線通訊的方式來傳遞機器人的控制命令和傳感信息，下位機則是通過ARM完成對各個模塊的控制，負責(zé)機器手的動作控制和姿態(tài)存儲，并將傳感信息傳遞給上位機，以達到精確控制機械手動作的完成。機械手的系統(tǒng)整體框圖如圖1所示。

圖1 機械手系統(tǒng)整體框圖Fig.1 Manipulator system block diagram

碼垛機械手的整體機械結(jié)構(gòu)如圖2所示。該小型碼垛機械手臂機械部分由4個部分組成[2-3]，包括前段的抓手部分、小臂、大臂、底座，總共三個自由度，完成這三個自由度一共需要4個SG-90舵機，分別安裝在小臂與抓取部分的關(guān)節(jié)處、小臂與大臂的關(guān)節(jié)處、大臂與底座的關(guān)節(jié)處、底座最下端的扭轉(zhuǎn)處。它在結(jié)構(gòu)上區(qū)別于一般的關(guān)節(jié)型機械手，它的大臂和小臂外側(cè)各有兩個平行四邊形輔助連桿結(jié)構(gòu)，這種設(shè)計結(jié)構(gòu)使得這個機械手有兩個特點：一是機械手有三個自由度，兩個平移和一個轉(zhuǎn)動，從而使得末端抓手能夠始終穩(wěn)定地保持抓取過程中的水平姿態(tài)；二是兩側(cè)的平行四邊形輔助結(jié)構(gòu)通過舵機銜接，使得手臂兩側(cè)等速連接，獲得平穩(wěn)的運動效果。

圖2 小型碼垛機械手的整體結(jié)構(gòu)Fig.2 Small palletizing robotic arm structure

1.2 小型碼垛機械手的驅(qū)動方法

本文采用的核心控制芯片為STM32芯片，是ARM Cortex-M內(nèi)核的32位微控制器，有休眠、停止、待機3種低功耗模式，采用ECOPACK封裝，最高工作頻率72 MHz，1.25 DMIPS/MHz。采用固定頻率、電壓模式結(jié)構(gòu)對信號進行控制，可應(yīng)用于舵機控制、Flash動畫、溫度調(diào)節(jié)等領(lǐng)域。

碼垛機械手的運動研究主要是對電機的控制，小型碼垛機械手實驗平臺包括四個舵機，其型號為SG-90。碼垛機械手的驅(qū)動方法采用脈沖寬度調(diào)制，脈寬調(diào)制技術(shù)可利用微處理器的數(shù)字輸出實現(xiàn)對模擬電路有效地選擇與控制，產(chǎn)生周期性的脈沖調(diào)制信號，然后通過調(diào)節(jié)相應(yīng)的占空比來對舵機的角度進行調(diào)整，從而實現(xiàn)對機械手的運動控制。具體的設(shè)計過程為設(shè)計定時器計數(shù)從0到999共1 000數(shù)為一個定時周期，設(shè)置預(yù)分頻為160，采用向上計數(shù)，配置PWM模式，設(shè)置跳變值，當計數(shù)器計數(shù)到這個跳變值以后進行跳變，低于這個值時為高電平輸出，設(shè)置4個使能通道，分別能夠輸出4個脈寬調(diào)制，采用比較模式寄存器，在程序末端使能重新裝載，用修改定時器中斷初值的方法形成了脈沖信號，調(diào)整時間段的寬度便可使伺服機靈活運動，從而能根據(jù)輸出的脈寬調(diào)制進行舵機角度的控制。

2 機械手末端控制運動學(xué)分析

2.1 機械手正向運動學(xué)分析

機器人運動學(xué)是指在不涉及產(chǎn)生運動的力或力矩的情況下，研究機器人末端執(zhí)行器相對于固定參考坐標系的運動幾何學(xué)關(guān)系[4-5]。機器人運動學(xué)研究的實質(zhì)就是考慮機器人位置、速度和加速度信息，有時甚至考慮機器人位置與時間的更高階導(dǎo)數(shù)。本小型碼垛機械手根據(jù)正向運動學(xué)即已知各個關(guān)節(jié)角的大小，求解機器人末端的位姿矩陣。機器人的正運動學(xué)可以通過各桿件的變化矩陣相乘得到，在各關(guān)節(jié)變量確定后，其末端的位置也隨之確定。對于小型碼垛機械手，可基于D-H參數(shù)建立運動學(xué)模型。D-H參數(shù)是為描述相鄰桿件間平移、偏移、旋轉(zhuǎn)和扭轉(zhuǎn)的關(guān)系，通過在每個關(guān)節(jié)處的桿件坐標系建立4×4齊次變換矩陣，表示它與前一桿件坐標系的位姿關(guān)系。采用修正的D-H參數(shù)模型建立坐標系，連桿的4個參數(shù)確定規(guī)則如下：

1)桿長ai：Zi-1沿Xi軸平移的距離；

2)關(guān)節(jié)扭轉(zhuǎn)角αi：Zi-1繞Xi-1軸旋轉(zhuǎn)的角度；

3)兩連桿距離di：Xi-1沿Zi-1軸平移距離；

4)旋轉(zhuǎn)角度θi：Xi-1繞Zi-1軸旋轉(zhuǎn)的角度。

其D-H參數(shù)如表1所示。

表1 D-H參數(shù)表

工具坐標系與第4坐標系關(guān)系為：α=0，a=y0，d=0，θ=0。

當碼垛機械手的手抓指尖位于要搬運貨物點，且手抓中心線位于搬運貨物的法向時，搬運的效率最高?；诖丝紤]，假設(shè)工作位置為(x0，y0，z0)，工件曲面在此處的法向為{nx，ny，nz}。結(jié)合以上分析可知坐標系的原點位置為p=(x0，y0，z0)，y軸的方向向量為n2={-nx，-ny，-nz}。構(gòu)建結(jié)構(gòu)圖如圖3所示。

圖3 小型碼垛機械手結(jié)構(gòu)簡圖Fig.3 Small palletizing robotic arm schematic

根據(jù)表1和修正D-H參數(shù)法的相關(guān)準則，得到各連桿之間的變換矩陣如下所示：

變換矩陣是3個關(guān)節(jié)變量的函數(shù),稱為機器人運動學(xué)方程，它將機器人的位姿從關(guān)節(jié)空間描述變換為直角坐標空間描述。有時為了簡化運算，把常量T通過矩陣逆運算移至等式左邊，從而直接計算T，這樣計算的結(jié)果意義是第4坐標系相對于極坐標系的位姿。變換矩陣T的意義是末端坐標系相對于極坐標系的位姿。

上式中各量所代表的量如下：nx=c1c2s3-c1s2，nx=c2c3s1-s1s2s3，nz=s2c3+c2s3，ox=-c1c2s3-c1c2c3，oy=-c2s1s3-s1s2s3，oz=-s2s3+c2c3，ax=s1，ay=-c1，az=0，px=c1c2L2L3-s1L1，py=c2c3s1L3+c2s1L2+s1c1，pz=s2c3L3+c2s3L3+s2L2；其中，si代表sinθ，ci代表cosθ。

2.2 機械手逆向運動學(xué)分析

姿態(tài)矩陣的第1，2，3列分別表示工具坐標系的xt，yt，zt軸與基坐標系的三個軸的夾角余弦，(px，py，pz)為工具坐標系原點在基坐標中的位姿[4]。在貨物點和該點法向量已知的條件下，可以求出位姿矩陣，繼續(xù)求解時，是一個循環(huán)體，循環(huán)變量為θ，每一步增加一定角度，求出機器人的位姿矩陣，然后按照3自由度機器人求解，求出的關(guān)節(jié)角度即為碼垛機械手的關(guān)節(jié)角度。

1)求關(guān)節(jié)角θ1

由上式可知：ax=s1，

反解出：θ1=sin-1ax。

2)求關(guān)節(jié)角θ2

由上式可知：px=c1c2L2L3-s1L1，ay=-c1，ax=s1，

3)求關(guān)節(jié)角θ3

由上式可知：py=c2c3s1L3+c2s1L2+s1c1，

4)解的整理

由計算得到了θ1、θ2、θ3三個解。在工程上，根據(jù)關(guān)節(jié)運動范圍和工作空間的約束，根據(jù)位置或能量最小變化原則，選取與位姿最為接近的運動節(jié)點，根據(jù)實際需要改進上述理論解去滿足末端位姿條件。

3 模型建立與實驗驗證

3.1 機械手虛擬樣機的搭建與軌跡仿真

搭建Solidworks軟件仿真平臺，采用Parasolid核心實體造型技術(shù)，建立機械手的虛擬樣機模型如圖4所示。將模型導(dǎo)入到ADAMS中，定義機械系統(tǒng)模型，創(chuàng)建剛體，得到運動軌跡仿真，如圖5所示。

圖4 機械手虛擬樣機模型Fig.4 Virtual prototyping

圖5 機械手運動軌跡Fig.5 Motion trail

3.2 小型機械手控制電路設(shè)計實現(xiàn)

電子產(chǎn)品更新周期快、個性化差異大的形勢，為進一步加快研制步伐，同時最大化地降低研制成本，采用電路模塊化的設(shè)計理念和方法[6-8]。本小型搬運機械手控制系統(tǒng)硬件組成為傳輸模塊、芯片控制模塊、電源轉(zhuǎn)換模塊、外界端口模塊、擴展控制模塊。

電源通過TP4056芯片對為控制手柄內(nèi)芯片進行充電，充電時7引腳輸出低電平，紅燈D2亮起；6引腳輸出高阻，綠燈D1滅；充滿時7引腳輸出高阻，紅燈D2滅，6引腳輸出低電平，綠燈D1亮起。充電電路如圖6所示。

圖6 機器手充電電路Fig.6 Machine hand charging circuit

外接電源采用AMS1117芯片進行電壓之間的轉(zhuǎn)換，它的穩(wěn)壓調(diào)整管是由一個PNP驅(qū)動的NPN管組成的，將供電電壓轉(zhuǎn)換為3.3 V為主控STM32芯片進行供電。nRF24L01芯片工作ISM頻段2.4～2.5 GHz，通過SPI接口設(shè)置頻道選擇和協(xié)議，實現(xiàn)遠程遙控。啟動舵機后，先進行電路復(fù)位初始化，將所有手臂豎直向上放置，通過MPU6050姿態(tài)控制芯片進行姿態(tài)調(diào)整，并將數(shù)據(jù)記入在FLASH中存儲，在對功能進行分解后，結(jié)合機器手臂的運動要求，設(shè)計出如圖7的控制架構(gòu)框圖。

圖7 機器手電路控制架構(gòu)框圖Fig.7 Machine control architecture block diagram of hand robotic arm

3.3 機械手脈寬仿真驗證

利用STM32分別輸出5%，50%，37.5%，25%占空比的四路PWM波程序圖，采用計數(shù)的方式輸出不同占空比的波形，通過電腦仿真產(chǎn)生如下展示的波形對舵機進行相應(yīng)角度的控制，如圖8所示。

本小型實驗平臺所采用的4路信號控制，用來實現(xiàn)對4路舵機的同時控制，并產(chǎn)生四路信號輸出，通過多路脈沖信號來實現(xiàn)抓手與機械臂之間的協(xié)調(diào)運動。首先機械臂處于靜止狀態(tài)，當抓取物體成功后，機械臂開始運動，此時的抓手處于靜止咬合狀態(tài)。當機械臂運動到規(guī)定位置以后，機械臂下放機械手，使得機械手能夠卸下物體，然后機械臂帶動機械手回歸初始位置，再重復(fù)上述過程。

圖8 脈寬調(diào)制仿真Fig.8 Pulse width modulation simulation

4 結(jié)論

本文提出了基于STM32的小型機械手的設(shè)計與控制方法，設(shè)計了一款采用脈寬調(diào)制技術(shù)的舵機機械手臂。該方法利用STM32控制簡單、精確度高的特點，使得開發(fā)工作量比起傳統(tǒng)的嵌入式系統(tǒng)大大減少。經(jīng)過試驗，實現(xiàn)3自由度機械手在人工控制和自動控制兩種模式下對重物的抓取，并能通過姿態(tài)傳感器MPU6050得到姿態(tài)的測量和修正，通過FLASH實現(xiàn)存儲。

本文重點研究了多舵機條件下以脈寬調(diào)制技術(shù)為基礎(chǔ)的聯(lián)合控制方法，并結(jié)合多個傳感器設(shè)計出控制系統(tǒng)，該系統(tǒng)有很好的通用性，為后續(xù)研究提供了借鑒與參考。

[1]胡洪國,高建華,楊汝清.碼垛技術(shù)綜述[J].組合機床與自動化加工技術(shù)，2000(6)：7-9.

[2] 付鐵,李金泉,陳懇，等.一種新型高速碼垛機械手的設(shè)計與實現(xiàn)[J].北京理工大學(xué)學(xué)報，2007(1):17-20.

[3]曾孔庚,王宏慶,丁原彥.高速機器人搬運碼垛系統(tǒng)構(gòu)成及技術(shù)特點[J].機器人技術(shù)及應(yīng)用,2001(4)：39 -42.

[4] 姚猛,韓寶玲,羅慶生，等.工業(yè)碼垛機器人機構(gòu)設(shè)計與運動學(xué)分析[J].組合機床與自動化加工技術(shù),2011,(5):31-33,37.

[5] 李建偉.碼垛機器人機構(gòu)設(shè)計與運動學(xué)研究[J].機械設(shè)計與制造，2009(6)：181-183.

[6] 肖永強.三自由度機器人及其控制系統(tǒng)研制[D].哈爾濱: 哈爾濱理工大學(xué)，2008.

[7] 馬綱,王之櫟,韓松元.一種新型搬運碼垛機械手的設(shè)計[J].機械設(shè)計與制造,2000(4)：26 -27.

[8] 劉永.弧焊機器人工作站智能化技術(shù)研究[D].南京：南京理工大學(xué), 2008.

Design and Implementation of Manipulator Based on PWM Technology

LIU Shichuan1,2, DENG Jiahao1,2, ZHAN Yinyu1,2, YE Yong1,2

(1. Science and Technology on Electromechanical Dynamic Control Laboratory, Beijing 100081, China; 2. School of Mechatronical Engineering, Beijing Institute of Technology, Beijing 100081, China)

In this paper,we presented the STM32 design of small robots and control method for machine hand of each joint, which need low cost and high precision location and speed of control. For the electro-mechanical control, this method was easy and flexible, it had the fast dynamic response and the expense was cheap. First of all , we introduced the architecture of the palletizing manipulator, analyzed the PWM technology. Secondly, we analyze the kinematics characteristic based on the structure to get the position of the manipulator end effector. We carried on the design, programming and the simulation experiments to the robot-control system, adopted 3D entities modeling software Solidworks which had established the virtual prototype model of the palletizing manipulator.

pulse width modulation；manipulator；kinematic analysis；structural modeling

2016-12-05

劉士川(1993— ), 男，山東德州人，碩士研究生，研究方向：電容引信探測、機器人、信號處理。E-mail:liushichuan@bit.edu.cn。

TP242.6

A

1008-1194(2017)02-0019-05