一種新型混聯(lián)碼垛機(jī)器人控制系統(tǒng)的研究

陳潤六，趙峰惠鄭小民，王紅州，鄒曉暉

（1.江西省機(jī)械科學(xué)研究所，江西南昌330002；2.江西現(xiàn)代職業(yè)技術(shù)學(xué)院，江西南昌330002）

新產(chǎn)品開發(fā)

一種新型混聯(lián)碼垛機(jī)器人控制系統(tǒng)的研究

陳潤六1，趙峰惠2鄭小民1，王紅州1，鄒曉暉1

（1.江西省機(jī)械科學(xué)研究所，江西南昌330002；2.江西現(xiàn)代職業(yè)技術(shù)學(xué)院，江西南昌330002）

介紹了一種新型混聯(lián)碼垛機(jī)器人的控制系統(tǒng)，闡述了其控制流程、軟硬件選型和設(shè)計(jì)、人機(jī)交互界面等。系統(tǒng)并聯(lián)部分采用三臺電動(dòng)缸作為執(zhí)行機(jī)構(gòu)，主軸和動(dòng)平臺轉(zhuǎn)軸采用兩臺電機(jī)加減速器組合?？刂品椒椤肮I(yè)控制機(jī)+運(yùn)動(dòng)控制器”模式，運(yùn)動(dòng)控制器采用STM32F4系列單片機(jī)作為主芯片。樣機(jī)的運(yùn)行表明：本系統(tǒng)安全可靠，成本較低，具有良好的市場應(yīng)用前景。

混聯(lián)；碼垛機(jī)器人；STM32；電動(dòng)缸

在碼垛機(jī)器人的控制系統(tǒng)研究領(lǐng)域，控制方式主要有以“工業(yè)控制機(jī)（IPC）+運(yùn)動(dòng)控制器”為核心的運(yùn)動(dòng)控制系統(tǒng)和嵌入式運(yùn)動(dòng)控制系統(tǒng)。其中，前者的運(yùn)動(dòng)控制器可以采用傳統(tǒng)PLC[1，2]、ARM系列單片機(jī)[3]、DSP類控制器（如PMAC或TRIO運(yùn)動(dòng)控制器）[4，5]、PCC[6]或者前三者搭配使用[7]；后者常采用ARM系列處理器作為主控芯片，嵌入Linux或WinCE操作系

統(tǒng)[8，9]。

本文采用的是ST公司的STM32F4系列，其基于ARM Cortex-M4，最大的特點(diǎn)是具有ARM+DSP的功能，能夠支持浮點(diǎn)運(yùn)算，且成本較低。

1 混聯(lián)碼垛機(jī)器人的機(jī)械結(jié)構(gòu)

采用2-UPS/U并聯(lián)構(gòu)型實(shí)現(xiàn)碼垛機(jī)器人的旋轉(zhuǎn)與抬升運(yùn)動(dòng)。串聯(lián)機(jī)構(gòu)以結(jié)構(gòu)簡單、動(dòng)作靈活、工作空間大等優(yōu)勢成為現(xiàn)有碼垛機(jī)器人所采用的主流構(gòu)型，但其剛性差、誤差累積、慣量大等問題也使得其性能受到制約，因此串聯(lián)結(jié)構(gòu)的機(jī)器人為達(dá)到一定的剛性不得不使用笨重的機(jī)械手臂，而笨重的機(jī)械手臂又致使機(jī)構(gòu)慣量大，動(dòng)力學(xué)性能差。另外，對于驅(qū)動(dòng)安裝在關(guān)節(jié)處的機(jī)械手，機(jī)構(gòu)對驅(qū)動(dòng)扭矩的要求非常高，這就不得不采用減速比大且昂貴的減速裝置，造成機(jī)器人的荷重自重比小且造價(jià)高。本項(xiàng)目將碼垛機(jī)器人負(fù)荷最大的底盤回轉(zhuǎn)部分及大臂抬升部分改為并聯(lián)結(jié)構(gòu)，大臂通過具有旋轉(zhuǎn)和抬升運(yùn)動(dòng)的虎克鉸與機(jī)架聯(lián)接（取消驅(qū)動(dòng)裝置），兩根具有伸縮功能的伺服驅(qū)動(dòng)直線推桿兩頭通過虎克鉸與球鉸分別與機(jī)架和大臂上端聯(lián)接，大臂和兩根直線推桿構(gòu)成空間三棱錐結(jié)構(gòu)，直線推桿的伸縮可實(shí)現(xiàn)大臂的旋轉(zhuǎn)和抬升兩個(gè)自由度的運(yùn)動(dòng)。兩根直線推桿在兩端聯(lián)接的情況下成為兩條無約束支鏈，推桿只受拉壓兩力，因此這兩根直線推桿的力學(xué)性能可以發(fā)揮到最優(yōu)狀態(tài)，其機(jī)械結(jié)構(gòu)見圖1。三棱錐結(jié)構(gòu)的優(yōu)點(diǎn)是剛性特別好，尤其兩根直線推桿只受拉壓力，因此該并聯(lián)部分可以做成輕桿，可大大提高機(jī)器人的剛性并降低機(jī)器人的自重，制作出來的實(shí)體樣機(jī)如圖2所示。

圖1 結(jié)構(gòu)仿真

圖2 實(shí)體樣機(jī)

2 控制流程及軟硬件設(shè)計(jì)

2.1 控制流程

圖3為本系統(tǒng)的控制流程。主控制回路邏輯如下：監(jiān)控計(jì)算機(jī)通過以太網(wǎng)通訊，把指令姿態(tài)和控制命令傳輸給微型控制器，微型控制器通過位姿轉(zhuǎn)換和解算，計(jì)算出三只電動(dòng)缸和兩臺減速器的運(yùn)動(dòng)參數(shù)（電動(dòng)缸伸縮量和減速器轉(zhuǎn)角），并通過CAN通訊把相應(yīng)的位移量傳遞給伺服驅(qū)動(dòng)器，進(jìn)而控制電動(dòng)缸的伸縮量和減速器轉(zhuǎn)角，以實(shí)現(xiàn)上平臺空間四自由度運(yùn)動(dòng)；同時(shí)控制計(jì)算機(jī)通過CAN通訊，得到電機(jī)的實(shí)際位置通過轉(zhuǎn)換和解算后，將平臺姿態(tài)發(fā)送給監(jiān)控計(jì)算機(jī)。反饋回路邏輯為：編碼器實(shí)時(shí)反饋電動(dòng)缸位移信號和減速器轉(zhuǎn)角信號，實(shí)現(xiàn)五臺電機(jī)位置閉環(huán)控制，使其達(dá)到所要求的位移量，那么平臺也就達(dá)到所期望的運(yùn)動(dòng)姿態(tài)。監(jiān)測回路：為了保證平臺安全可靠的運(yùn)行，平臺實(shí)時(shí)控制計(jì)算機(jī)，通過控制卡實(shí)時(shí)讀取平臺極限位置信號，故障安全報(bào)警信號，并實(shí)時(shí)處理，同時(shí)實(shí)時(shí)計(jì)算機(jī)平臺位姿和運(yùn)行狀態(tài)，返回給監(jiān)控計(jì)算機(jī)人機(jī)界面，供用戶監(jiān)控。

圖3 控制流程

2.2 運(yùn)動(dòng)控制器

運(yùn)動(dòng)控制器的硬件電路由STM32F407微控制器和外圍電路組成，主要包括：STM32F407基本電路、電源電路、時(shí)鐘電路、復(fù)位電路、串口通信電路、以太網(wǎng)通信電路、JTAG調(diào)試接口電路、CAN總線通信電路、AD/DA電路、濾波放大電路、數(shù)字量輸入輸出電路等。圖4所示為主控制器總體設(shè)計(jì)框圖，圖5為系統(tǒng)硬件方案，圖6為運(yùn)動(dòng)控制器實(shí)體外觀。

圖4 主控制器總體設(shè)計(jì)框圖

圖5 系統(tǒng)硬件方案

圖6 運(yùn)動(dòng)控制器

2.2.1 基于ARM Cortex-M4的STM32F4系列優(yōu)勢

浮點(diǎn)運(yùn)算一直是定點(diǎn)CPU的難題，采用單片機(jī)進(jìn)行浮點(diǎn)運(yùn)算較為困難。現(xiàn)在有FPU或者硬件浮點(diǎn)運(yùn)算能力的主要有高端DSP（比如TI F28335/C6000/ DM6XX/OMAP等），通用CPU（X87數(shù)學(xué)協(xié)處理器）和高級的ARM+DSP處理器等。STM32F4屬于Cortex M4構(gòu)架，這和M0、M3的最大不同就是支持浮點(diǎn)指令集，在處理數(shù)學(xué)運(yùn)算時(shí)，能發(fā)揮其巨大的優(yōu)勢。

ST公司的STM32F4系列是高性能的基于ARM Cortex-M4的32位微控制器，工作頻率高達(dá)168 MHz.Cortex M4具有浮點(diǎn)單元(FPU），支持所有的ARM單精度數(shù)據(jù)處理指令和數(shù)據(jù)類型，具有DSP處理指令和自適應(yīng)的實(shí)時(shí)加速器(ART）。STM32 F4改進(jìn)了一些外設(shè)，如全雙工I2S、小于1uA的RTC以及2.44 MHz/s采樣率的ADC，STM32 F4的通信接口高達(dá)15個(gè)。STM32 F4還包含512 KB～1 MB的片內(nèi)內(nèi)存存儲(chǔ)器，多達(dá)192 KB的SRAM.系統(tǒng)采用STM32F4作為主控芯片。

2.2.2 電源模塊

主要是為整個(gè)硬件平臺系統(tǒng)正常工作提供所有必須的電源，包括為主控芯片的工作提供穩(wěn)定的3.3 V工作電壓，為放大電路的運(yùn)算放大器提供±15V. STM32F4主控芯片具有獨(dú)立的模擬電源引腳，模擬部分和數(shù)字部分分開供電，這樣有效提高了模擬系統(tǒng)的抗噪性。

2.2.3 運(yùn)動(dòng)算法

運(yùn)動(dòng)平臺的解算方法分為位置反解和位置正解。

已知輸出件的位置和姿態(tài)，求解輸入件的位置稱為機(jī)構(gòu)的位置反解。要實(shí)現(xiàn)對平臺空間姿態(tài)的控制，就必需推導(dǎo)出平臺的位置反解方程。

已知機(jī)構(gòu)輸入件的位置，求解機(jī)構(gòu)輸出件的位置和姿態(tài)稱為機(jī)構(gòu)的位置正解。本文所采用的是牛頓法，其基本原理就是將非線性方程組變成線性方程組，求出近似解，然后在此近似解基礎(chǔ)上進(jìn)一步迭代，逐步逼近非線性方程組真解。

本課題組在前期工作中已將正反解算法得出，并轉(zhuǎn)化成代碼寫入芯片[10]。

2.3 主監(jiān)控軟件

監(jiān)控軟件主要由主操作窗口、監(jiān)控窗口、外部輸入列表窗口、單步運(yùn)行窗口、點(diǎn)動(dòng)運(yùn)行窗口、高級用戶窗口等組成，其示意圖如圖7所示，現(xiàn)分述如下：

圖7 監(jiān)控軟件示意圖

主操作窗口主要包括平臺的開機(jī)/關(guān)機(jī)、回零位、暫停/恢復(fù)等操作輸入按鈕；

監(jiān)控窗口主要用于監(jiān)控系統(tǒng)當(dāng)前狀態(tài)，包括實(shí)時(shí)顯示平臺實(shí)際姿態(tài)，電機(jī)伺服狀態(tài)等；

外部列表輸入窗口，將外部輸入可執(zhí)行外部路徑文件（TXT文件），并自動(dòng)按照規(guī)劃路徑運(yùn)行。選取資源文件，系統(tǒng)讀取當(dāng)前文件，并按照文件內(nèi)容運(yùn)行到指定位置；

單步運(yùn)行窗口包括單步點(diǎn)動(dòng)輸入及單步連續(xù)運(yùn)動(dòng)兩部分。單步點(diǎn)動(dòng)輸入用于平臺微動(dòng)控制，步距及速度均可設(shè)置；單步連續(xù)運(yùn)動(dòng)，需設(shè)定目標(biāo)位置與運(yùn)行速度，平臺自動(dòng)運(yùn)行到指令位置。

2.4 手持終端

手持終端與主計(jì)算機(jī)通過無線WIFI傳輸，主要由姿態(tài)、系統(tǒng)狀態(tài)、點(diǎn)動(dòng)窗口、當(dāng)前狀態(tài)顯示窗口等組成，其示意圖如圖8所示，現(xiàn)分述如下：

圖8 手持終端操作軟件示意圖

（1）主控窗口：主要包括平臺回零位及退出等操作輸入按鈕；

（2）顯示窗口：主要用于監(jiān)控系統(tǒng)當(dāng)前狀態(tài)，包括實(shí)時(shí)顯示平臺當(dāng)前姿態(tài)，系統(tǒng)狀態(tài)，單步運(yùn)行步距等；

①網(wǎng)絡(luò)狀態(tài)：

“NetRun”-網(wǎng)絡(luò)正常；

“NetErr”-網(wǎng)絡(luò)故障。

②當(dāng)前步距：當(dāng)前點(diǎn)動(dòng)運(yùn)行步距。

③當(dāng)前狀態(tài)：

“NULL”-無操作；

“SinMove”-單步運(yùn)行；

“SinMotion”-正弦運(yùn)動(dòng)；

“BackToMid”-正在回中位；

“SinStep”-單步點(diǎn)動(dòng)運(yùn)行；

“ListExcu”-列表運(yùn)行；

“TorqueErr”-電機(jī)扭矩超限警告。

（3）輸入窗口：包括單步點(diǎn)動(dòng)輸入。單步點(diǎn)動(dòng)輸入用于平臺微動(dòng)控制，步距可由用戶設(shè)置。單步點(diǎn)動(dòng)輸入分為兩種工作模式：

①單步點(diǎn)動(dòng)：每點(diǎn)擊一次按鈕，平臺微動(dòng)一步；

②單步連續(xù)點(diǎn)動(dòng)：連續(xù)點(diǎn)擊兩次，長按按鈕，系統(tǒng)連續(xù)運(yùn)動(dòng)，松開按鈕，系統(tǒng)停止運(yùn)動(dòng)。

3 結(jié)束語

本文研發(fā)的新型混聯(lián)碼垛機(jī)器人控制系統(tǒng)以基于ARM構(gòu)架的STM32系列微控制器為核心，采用以太網(wǎng)芯片、CanOpen總線芯片以及接口等組成。控制算法包含了位置反解和正解，實(shí)現(xiàn)了該機(jī)的示教功能。設(shè)計(jì)的主監(jiān)控軟件及手持終端的用戶界面友好，充分考慮了樣機(jī)調(diào)試、運(yùn)行、示教等所需的功能實(shí)現(xiàn)。實(shí)驗(yàn)表明：該系統(tǒng)工作穩(wěn)定可靠，成本低廉，有著非常大的市場潛力。

[1]張豐華，韓寶玲，羅慶生.基于PLC的新型工業(yè)碼垛機(jī)器人控制系統(tǒng)設(shè)計(jì)[J].計(jì)算機(jī)測量與控制，2009，17（11）：2191-2193.

[2]侯輝.碼垛機(jī)器人控制系統(tǒng)研究[D].鄭州：鄭州大學(xué)，2013：23-26.

[3]王占軍.工業(yè)碼垛機(jī)器人機(jī)械結(jié)構(gòu)與控制系統(tǒng)的研究[D].淄博：山東理工大學(xué)，2015：44-48.

[4]李成偉，朱秀麗，贠超.碼垛機(jī)器人機(jī)構(gòu)設(shè)計(jì)與控制系統(tǒng)研究[J].機(jī)電工程，2008，25（12）：81-84.

[5]余曉明.四自由度碼垛機(jī)器人設(shè)計(jì)及其控制系統(tǒng)性能研究[D].成都：西南交通大學(xué)，2008：12-14

[6]劉興磊.基于現(xiàn)場總線的碼垛機(jī)器人控制系統(tǒng)研究[D].上海：東華大學(xué)，2010：17-19.

[7]李成偉，贠超.碼垛機(jī)器人控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[J].機(jī)電產(chǎn)品開發(fā)與創(chuàng)新，2008，21（5）：9-11.

[8]楊洋.碼垛機(jī)器人嵌入式控制系統(tǒng)研究[D].南京：南京林業(yè)大學(xué)，2013：14-16.

[9]曹波，曹其新，陳培華.基于模塊化設(shè)計(jì)的碼垛機(jī)器人控制系統(tǒng)開發(fā)[J].機(jī)床與液壓，2012，40（23）：90-92.

[10]鄒曉暉，王紅州，陳潤六.一種新型混聯(lián)碼垛機(jī)器人的設(shè)計(jì)與分析[J].制造業(yè)自動(dòng)化，2016，38（11）：91-94.

Study on Control System of A New Type of Hybrid Palletizing Robot

CHEN Run-liu1，ZHAO Feng-hui2，ZHENG Xiao-min1，WANG Hong-zhou1，ZOU Xiao-hui1
(1.Jiangxi Institute of Mechanical Science，Nanchang Jiangxi 330002，China；2.Jiangxi Modern Polytechnic College，Nanchang Jiangxi 330002，China）

This paper introduces the control system of a new type of hybrid palletizing robot，mainly expounds the control flow，the selection and design of hardware and software，the man-machine interface，etc..In the parallel part of the system，three electric cylinders are used as the executing mechanism，and the main shaft and the rotating shaft of the moving platform are composed of two motor and speed reducers.With the control method being“industrial personal computer+motion controller"mode，the motion controller uses STM32F4 Series MCU as the main chip.The operation shows that the system is safe，reliable，low cost，and has good market prospects.

hybrid；palletizing robot；STM32；electric cylinder

TP242.2

A< class="emphasis_bold">文章編號：1

1672-545X（2017）05-0069-03

2017-02-05

江西省科技支撐重大課題：一種新型混聯(lián)碼垛機(jī)器人的研發(fā)：（項(xiàng)目編號：20152ACE50009）；江西省重大科技成果轉(zhuǎn)移轉(zhuǎn)化計(jì)劃項(xiàng)目：基于并聯(lián)機(jī)構(gòu)的碼垛機(jī)器人研發(fā)成果轉(zhuǎn)移轉(zhuǎn)化(項(xiàng)目編號：2016ACI90009)

陳潤六（1987-），男，江西九江人，碩士，工程師，研究領(lǐng)域：機(jī)械裝備的設(shè)計(jì)及制造研究，機(jī)器人控制系統(tǒng)研究。