基于單片機(jī)的六自由度機(jī)械手臂控制系統(tǒng)設(shè)計(jì)

陳心怡 張春雨 朱麗華

（1.池州職業(yè)技術(shù)學(xué)院，安徽 池州 247000;2.安徽科技學(xué)院，安徽 滁州 239000）

一、引言

機(jī)械臂也稱工業(yè)機(jī)器人，是以運(yùn)動(dòng)作為控制對(duì)象的智能控制裝備，主要由手臂、舵機(jī)、抓手等組成[1]。機(jī)器人手臂的張開、夾緊等系列動(dòng)作由電動(dòng)機(jī)驅(qū)動(dòng)，并準(zhǔn)確地反饋到可編程邏輯控制器。在工業(yè)生產(chǎn)中控制機(jī)械臂完成我們所需要的夾取和分揀動(dòng)作，本項(xiàng)目便是圍繞六自由度機(jī)械臂控制系統(tǒng)展開的。通常一個(gè)52系列的單片機(jī)包含有FLASH ROM、RAM、3個(gè)16位的定時(shí)器/計(jì)數(shù)器和1個(gè)UART等，它具有性能可靠、性價(jià)比高等特點(diǎn)。在工業(yè)生產(chǎn)中，往往需要機(jī)械手臂完成一些相對(duì)復(fù)雜的控制運(yùn)動(dòng)，機(jī)械手臂系統(tǒng)中一個(gè)單片機(jī)遠(yuǎn)遠(yuǎn)不夠，仍需要TK-A66自由度機(jī)械手臂、1501數(shù)字舵機(jī)等控制機(jī)械臂的運(yùn)動(dòng)。

本文就是用單片機(jī)作為核心控制器，LD1501-MG數(shù)字舵機(jī)作為TK-A66自由度機(jī)械手臂的運(yùn)動(dòng)控制單元，結(jié)合制成的6自由度機(jī)械臂的控制系統(tǒng)。系統(tǒng)采用單片機(jī)芯片STC89C52作為主控制器，A/D轉(zhuǎn)換采用以雙積分方式運(yùn)行的ICL7135，利用定時(shí)計(jì)數(shù)器的計(jì)數(shù)功能，測(cè)量外部電壓，省去很多處部電路[2]。單片機(jī)通過產(chǎn)生PWM信號(hào)控制機(jī)械臂的舵機(jī)，從而控制6自由度的機(jī)械手臂。

二、算法設(shè)計(jì)

（一）建立運(yùn)動(dòng)學(xué)模型

如圖1所示，具有三維開環(huán)鏈結(jié)構(gòu)的機(jī)械手分別由基座、增幅臂和旋轉(zhuǎn)手臂組成，可通過肩部旋轉(zhuǎn)和俯仰、肘部和腕部俯仰、腕部偏轉(zhuǎn)和旋轉(zhuǎn)6個(gè)旋轉(zhuǎn)關(guān)節(jié)實(shí)現(xiàn)6個(gè)自由度的運(yùn)動(dòng)，進(jìn)而保證機(jī)械手在工作空間中的所有位置狀態(tài)[3]。

圖1機(jī)械臂模型

為準(zhǔn)確反映關(guān)節(jié)坐標(biāo)與機(jī)械端位置之間的關(guān)系，本文根據(jù)D-H原則和笛卡爾坐標(biāo)系建立機(jī)械臂關(guān)節(jié)的坐標(biāo)系及機(jī)械臂的正向運(yùn)動(dòng)學(xué)方程，但逆運(yùn)動(dòng)學(xué)模式在實(shí)際應(yīng)用過程中更為普遍[4]。機(jī)械臂控制系統(tǒng)的運(yùn)行可以通過調(diào)整關(guān)機(jī)位置來實(shí)現(xiàn)，這就需要通過逆運(yùn)動(dòng)學(xué)模型，計(jì)算出完成理想軌跡時(shí)每個(gè)關(guān)節(jié)所需達(dá)到的角度值。在確定了機(jī)械臂的構(gòu)型后應(yīng)以D-H法則確定運(yùn)動(dòng)學(xué)參數(shù)，如表1所示。

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

根據(jù)圖1中的構(gòu)型建立參考坐標(biāo)系和關(guān)節(jié)坐標(biāo)系，再用齊次方程描述機(jī)械臂各連桿的空間幾何關(guān)系，用4×4齊次矩陣來表示相鄰連桿之間的幾何關(guān)系，再算出機(jī)械臂相對(duì)于參考坐標(biāo)系的后端地址。鑒于該坐標(biāo)系中存在偏移量α1和α2，故χ1、χ2和χ3的空間關(guān)系并不重合，如圖2所示。

圖2 1,2,3坐標(biāo)空間關(guān)系

（二）運(yùn)動(dòng)控制算法

運(yùn)動(dòng)軌跡和速度是機(jī)械臂兩大控制變量，軌跡控制算法是機(jī)械臂控制系統(tǒng)的最關(guān)鍵部分。相對(duì)于二維軌跡控制算法，六自由度軌跡控制算不是二維插補(bǔ)算法的簡(jiǎn)單組合，綜合曲線特征，采用分段曲線逼近擬合軌跡曲線。為了提高擬合度，須精確計(jì)算出各段曲線交點(diǎn)值，以保證運(yùn)動(dòng)狀態(tài)參數(shù)的連續(xù)性[5]。考慮到既要使機(jī)械臂能沿著既定的軌跡運(yùn)動(dòng)，又要確保運(yùn)動(dòng)過程中的實(shí)時(shí)可控性，本文采用Trial Mode運(yùn)動(dòng)模式的控制算法，即通過階梯形分割其速度曲線，在每個(gè)采樣周期內(nèi)分段進(jìn)行提取設(shè)計(jì)，確保對(duì)插補(bǔ)算法指定軌跡進(jìn)行約束。

（三）調(diào)試運(yùn)動(dòng)規(guī)劃算法

合理設(shè)計(jì)機(jī)械臂算法調(diào)試軟件，本文軟件采用C語(yǔ)言編寫算法，借助WinCE系統(tǒng)平臺(tái)構(gòu)建其運(yùn)動(dòng)規(guī)劃算法的實(shí)驗(yàn)平臺(tái)，采用MFC設(shè)計(jì)合理的程序框架，來驗(yàn)證算法功能以確保算法函數(shù)的可行性與控制的精準(zhǔn)度，確保對(duì)不同運(yùn)動(dòng)模式的仿真驗(yàn)證。

三、硬件設(shè)計(jì)

將硬件設(shè)計(jì)過程分為單片機(jī)模塊和舵機(jī)控制模塊兩個(gè)部分，系統(tǒng)原理圖如圖3所示。在機(jī)械臂硬件設(shè)計(jì)過程時(shí)，其構(gòu)成部件的選擇應(yīng)匹配于機(jī)械臂的基本技術(shù)參數(shù)，之后根據(jù)對(duì)關(guān)機(jī)轉(zhuǎn)矩等計(jì)算推演，合理選擇滿足功率要求的電機(jī)。綜合考慮，本硬件設(shè)計(jì)中核心的器件選用STC89C52型號(hào)的微處理器，內(nèi)置8KB可在線編程閃存，具有A/D轉(zhuǎn)換功能，并能產(chǎn)生PWM信號(hào)。另一方面，產(chǎn)生的PWM信號(hào)控制機(jī)械臂舵機(jī)的制動(dòng)[6]。

圖3系統(tǒng)原理圖

（一）單片機(jī)模塊

單片機(jī)模塊所使用的STC89C52單片機(jī)，包括單片機(jī)芯片、復(fù)位電路、時(shí)鐘電路和電源電路。該芯片采用CMOS工藝，具有密度高、存儲(chǔ)信息不易丟失的特點(diǎn)，與C51指令集兼容。Internal RAM允許重復(fù)在線編程，還可通過SPI串行端口或同一非易失性存儲(chǔ)器進(jìn)行重新編輯。通過將8位CPU與可線上下載的Flash進(jìn)行集成，可有效地解決復(fù)雜的控制問題。內(nèi)部系統(tǒng)包括8K字節(jié)可在線編程ISP閃存，4.0-5.5V電源，完全靜態(tài)工作0Hz～24MHz，256字節(jié)內(nèi)部RAM，32根可編程I/O線，8個(gè)中斷源和雙數(shù)據(jù)指針，2個(gè)16位定時(shí)器/計(jì)數(shù)器。

（二）舵機(jī)控制模塊

本系統(tǒng)采用數(shù)字舵機(jī)LD1501-MG分2組進(jìn)行實(shí)驗(yàn)，第一組中舵機(jī)與單片機(jī)共用同一電源，第二組中使用不同電源供電。數(shù)據(jù)表明，第一組單片機(jī)運(yùn)行不穩(wěn)定，第二組中兩者運(yùn)行較好。所以本系統(tǒng)采用舵機(jī)與單片機(jī)公用不同電源方案，且舵機(jī)所需的電源功率要大于單片機(jī)的電源功率，如圖4所示[7]。

2）其他土地利用類型都有不同程度的減少，其中水域和耕地減少的最為明顯，1985－2000年和2000－2016年水域的減少速度分別為0.08%和0.19%，耕地的減少速度分別為0.12%和0.14%；

圖4舵機(jī)驅(qū)動(dòng)電路圖

四、軟件設(shè)計(jì)

（一）總體設(shè)計(jì)

本設(shè)計(jì)利用上位機(jī)操作系統(tǒng)及監(jiān)控程序?qū)崿F(xiàn)信息通訊、軌跡規(guī)劃及運(yùn)動(dòng)算法控制等功能[8]。根據(jù)操作步驟，上位機(jī)提取INI文件和數(shù)據(jù)庫(kù)中的參數(shù)，并在可視化界面中輕易完成機(jī)械臂軌跡規(guī)劃和關(guān)節(jié)角位置的設(shè)定。信息通信過程首先要初始化通信端口，通信總線與各關(guān)節(jié)進(jìn)行通信連接，發(fā)出連接信號(hào)并讀取數(shù)據(jù)，進(jìn)而發(fā)出指令并驅(qū)動(dòng)電動(dòng)機(jī)運(yùn)動(dòng)。為兼顧系統(tǒng)軟件功能可以同步完成，本控制系統(tǒng)采由C51語(yǔ)言編程來實(shí)現(xiàn)，程序結(jié)構(gòu)采用模塊式，由主程序、初末置換函數(shù)、速度控制函數(shù)、機(jī)械臂控制函數(shù)、定時(shí)器T0子程序、定時(shí)器T1子程序6部分組成。結(jié)構(gòu)清晰，便于分析調(diào)試。

（二）主程序設(shè)計(jì)

主程序是將整個(gè)程序分成若干模塊，在程序中利用函數(shù)調(diào)用的方式，連接各個(gè)子程序，傳遞之間的參數(shù)。系統(tǒng)的主程序按照以下流程執(zhí)行。

程序開始，對(duì)全局變量進(jìn)行聲明，對(duì)相關(guān)數(shù)據(jù)及參數(shù)初始化，3個(gè)定時(shí)器數(shù)據(jù)初始化，將輸出引腳設(shè)置為強(qiáng)推挽輸出，為舵機(jī)提供足夠的電流。中斷設(shè)置，定時(shí)器初始化并開啟中斷，設(shè)置定時(shí)器TCON和TMOD參數(shù)。

中斷請(qǐng)求，接到信號(hào)請(qǐng)求則調(diào)用pwm[]數(shù)組更新函數(shù)舵機(jī)調(diào)速子程序vpwm()。流程圖如圖5所示。

（三）舵機(jī)控制程序設(shè)計(jì)

圖5程序流程圖

為保證機(jī)械臂平穩(wěn)運(yùn)行，需要對(duì)舵機(jī)進(jìn)行精確控制，以實(shí)現(xiàn)速度調(diào)節(jié)，實(shí)際上在程序中對(duì)舵機(jī)控制信號(hào)PWM占空比進(jìn)行調(diào)節(jié)即可。PWM是脈沖寬度調(diào)制信號(hào)，具有易控制、迅速和動(dòng)態(tài)響應(yīng)好等特點(diǎn)，被廣泛應(yīng)用在人工智能機(jī)器人及航模設(shè)計(jì)的舵機(jī)及直流電機(jī)的控制中，其PWM設(shè)置格式應(yīng)注意以下幾點(diǎn)：

（1）上升沿保證有0.5ms，介于0.5ms至2.5ms之間；

(2)要求用程序?qū)崿F(xiàn)連續(xù)的PWM信號(hào)，對(duì)于硬件體現(xiàn)出來的機(jī)械臂動(dòng)作更加連續(xù)、流暢。

圖6多路舵機(jī)PWM速度控制程序流程圖

五、系統(tǒng)仿真與實(shí)現(xiàn)

為使控制系統(tǒng)的開發(fā)更加的高效便捷，在進(jìn)行硬件電路的焊接前，先進(jìn)行仿真。本控制系統(tǒng)設(shè)計(jì)，用Protues仿真軟件搭建仿真系統(tǒng)的電路圖，由于舵機(jī)在Protues元器件庫(kù)中沒有加入，本仿真可用Protues自帶的虛擬示波器Oscilloscope代替。通過觀察Oscilloscope顯示的PWM控制信號(hào)的占空比，判斷控制系統(tǒng)的控制性能?？筛鶕?jù)仿真結(jié)果，通過調(diào)整單片機(jī)程序，改變占空比，從而得到電機(jī)平均速度，達(dá)到調(diào)速的目的。

硬件調(diào)試階段首先測(cè)試舵機(jī)，將舵機(jī)測(cè)試程序燒錄到單片機(jī)系統(tǒng)中，測(cè)試并記錄舵機(jī)偏轉(zhuǎn)的角度并與預(yù)期角度進(jìn)行比較，誤差角度控制在±1度以內(nèi)，對(duì)不能達(dá)到要求的舵機(jī)不予選用。選擇好舵機(jī)以后根據(jù)機(jī)械臂結(jié)構(gòu)圖組裝好機(jī)械臂，通過軟件編程程序控制機(jī)械臂實(shí)現(xiàn)6自由度運(yùn)動(dòng)。最后，組裝硬件并反復(fù)調(diào)試。在調(diào)試的過程中，根據(jù)實(shí)際情況，機(jī)械臂的重量，平穩(wěn)程度等，可通過編程來控制發(fā)送PWM信號(hào)的時(shí)間，以適應(yīng)不同距離的測(cè)量需要，以達(dá)到準(zhǔn)確抓取目標(biāo)的任務(wù)，確保實(shí)現(xiàn)6自由度機(jī)械臂的運(yùn)動(dòng)控制功能。

六、結(jié)語(yǔ)

機(jī)械臂作為應(yīng)用廣泛的運(yùn)動(dòng)執(zhí)行機(jī)構(gòu)，其運(yùn)動(dòng)穩(wěn)定性在現(xiàn)代工業(yè)生產(chǎn)中意義重大。本方案在反復(fù)比較國(guó)內(nèi)外同類產(chǎn)品的基礎(chǔ)上，深入分析其適用行業(yè)特點(diǎn)和功能需求，確定總體設(shè)計(jì)方案使用STC89C52作為主控制器、LD1501-MG數(shù)字舵機(jī)作為控制單元，主要由機(jī)械臂電路與單片機(jī)控制電路兩大部分構(gòu)成。

機(jī)械臂控制系統(tǒng)中建立運(yùn)動(dòng)學(xué)模型，運(yùn)用TM算法，對(duì)機(jī)械臂手臂的運(yùn)動(dòng)速度和軌跡進(jìn)行控制設(shè)計(jì)，實(shí)現(xiàn)機(jī)械臂手臂運(yùn)動(dòng)軌跡的控制功能。在本系統(tǒng)控制程序中，利用單片機(jī)產(chǎn)生的PWM信號(hào)對(duì)舵機(jī)進(jìn)行控制，給出了微分插補(bǔ)法生成多路舵機(jī)PWM速度控制信號(hào)的程序?qū)崿F(xiàn)方法，通過微分插補(bǔ)法進(jìn)行舵機(jī)速度的控制，實(shí)現(xiàn)機(jī)械臂狀態(tài)的控制和調(diào)整，使機(jī)械臂的運(yùn)行更加平穩(wěn)。此系統(tǒng)的仿真實(shí)現(xiàn)受限于無(wú)障礙的空間環(huán)境，其控制和軌跡算法應(yīng)結(jié)合不同的復(fù)雜環(huán)境進(jìn)行跟進(jìn)調(diào)整，使其功能和應(yīng)用價(jià)值更為凸顯。