變插值周期五次多項(xiàng)式機(jī)器人關(guān)節(jié)空間規(guī)劃

涂 孔，張 華

(浙江理工大學(xué)機(jī)械與自動(dòng)控制學(xué)院，杭州 310018)

?

變插值周期五次多項(xiàng)式機(jī)器人關(guān)節(jié)空間規(guī)劃

涂 孔，張 華

(浙江理工大學(xué)機(jī)械與自動(dòng)控制學(xué)院，杭州 310018)

為了控制機(jī)器人平穩(wěn)、精確地到達(dá)示教點(diǎn)，對(duì)機(jī)器人關(guān)節(jié)空間五次多項(xiàng)式插值函數(shù)進(jìn)行了研究，提出了變插值周期的插值方法。將該插值算法應(yīng)用于四自由度工業(yè)機(jī)器人，建立了基于D-H (Denavit-Hartenberg)坐標(biāo)系的正逆運(yùn)動(dòng)學(xué)方程，并在運(yùn)動(dòng)控制系統(tǒng)中實(shí)現(xiàn)算法。通過(guò)示教得到始末點(diǎn)位姿，調(diào)用算法程序控制機(jī)器人到達(dá)示教終點(diǎn)。采集各軸編碼器反饋脈沖值并計(jì)算得到各軸頻率和加速度曲線，使用加速度傳感器和百分表分別測(cè)試末端執(zhí)行器運(yùn)動(dòng)速度和機(jī)器人的重復(fù)定位精度。試驗(yàn)結(jié)果表明：各軸頻率和加速度曲線符合五次多項(xiàng)式函數(shù)速度、加速度曲線變化趨勢(shì)，末端執(zhí)行器速度變化連續(xù)，機(jī)器人運(yùn)行平穩(wěn)，重復(fù)定位精度為0.01 mm。

五次多項(xiàng)式；變插值周期；D-H坐標(biāo)系；運(yùn)動(dòng)學(xué)方程

0 引 言

人工作業(yè)常會(huì)出現(xiàn)產(chǎn)品質(zhì)量不穩(wěn)定、效率低等問(wèn)題。隨著勞動(dòng)力成本的增加，機(jī)器人代替人工作業(yè)已成為將來(lái)的發(fā)展趨勢(shì)。軌跡規(guī)劃[1]是機(jī)器人運(yùn)動(dòng)控制的重要內(nèi)容，合理的規(guī)劃方法可以使機(jī)器人快速、準(zhǔn)確、平穩(wěn)地到達(dá)目標(biāo)點(diǎn)。軌跡規(guī)劃可在關(guān)節(jié)空間或笛卡爾空間中進(jìn)行。在關(guān)節(jié)空間中規(guī)劃時(shí)，無(wú)需進(jìn)行大量反解計(jì)算，插值速度快，精度能夠滿足一般作業(yè)要求[2-3]。故研究關(guān)節(jié)空間軌跡規(guī)劃具有重要意義。

機(jī)器人在啟動(dòng)和停止時(shí)不能產(chǎn)生沖擊，在運(yùn)動(dòng)過(guò)程中需保持平穩(wěn)。王霆等[4]采用拋物線擬合的線性插值函數(shù)對(duì)關(guān)節(jié)空間進(jìn)行規(guī)劃，這種方法雖然速度曲線連續(xù)，但是因加速度存在突變，容易造成機(jī)器振動(dòng)。基于多項(xiàng)式插值的曲線比基于線性插值的曲線更加平滑，沒(méi)有突變。[5]常用的多項(xiàng)式插值曲線有三次、五次曲線。三次多項(xiàng)式插值加速度曲線不平滑[6]，五次多項(xiàng)式插值函數(shù)可實(shí)現(xiàn)平滑的加速度曲線，能夠保證機(jī)器人運(yùn)動(dòng)的平穩(wěn)性和精度[7-9]。雖然多項(xiàng)式插值的次數(shù)越大插值函數(shù)曲線越平滑，但是約束條件和計(jì)算量也越大，對(duì)處理器性能要求也越高。[10]并且在用多項(xiàng)式擬合曲線時(shí)，多項(xiàng)式次數(shù)越高，擬合多項(xiàng)式在插值區(qū)間端點(diǎn)處越容易產(chǎn)生震蕩，即龍格現(xiàn)象[11]。本文綜合考慮機(jī)器人運(yùn)動(dòng)的平穩(wěn)性及約束條件和對(duì)處理器的要求，將五次多項(xiàng)式插值函數(shù)作為研究對(duì)象。為實(shí)現(xiàn)關(guān)節(jié)空間五次多項(xiàng)式軌跡規(guī)劃，通過(guò)改變插值周期的方法對(duì)多項(xiàng)式函數(shù)進(jìn)行插值。該方法易于理解，便于實(shí)現(xiàn)，可以控制機(jī)器人平穩(wěn)、精確地運(yùn)動(dòng)。本文將該算法應(yīng)用在四自由度工業(yè)機(jī)器人上，以驗(yàn)證該算法實(shí)現(xiàn)效果。

1 五次多項(xiàng)式插值算法及實(shí)現(xiàn)

1.1 五次多項(xiàng)式系數(shù)求解

五次多項(xiàng)式插值通式為:

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(1)

其中：t為時(shí)間，s；θ(t)為t時(shí)刻對(duì)應(yīng)的關(guān)節(jié)角，(°)；a0，a1，…，a5為系數(shù)。為了求取多項(xiàng)式的系數(shù)，必須滿足如下約束條件：

(2)

(3)

1.2 插值算法實(shí)現(xiàn)

伺服電機(jī)旋轉(zhuǎn)的角度由控制器發(fā)出的脈沖個(gè)數(shù)決定，電機(jī)轉(zhuǎn)速由發(fā)出的脈沖頻率決定。故需將示教位姿經(jīng)運(yùn)動(dòng)學(xué)反解得到的關(guān)節(jié)角換算成對(duì)應(yīng)的脈沖值，關(guān)節(jié)角隨時(shí)間的變化轉(zhuǎn)化為脈沖值隨時(shí)間的變化，通過(guò)控制脈沖發(fā)出的個(gè)數(shù)和頻率來(lái)控制機(jī)器人運(yùn)動(dòng)到達(dá)示教點(diǎn)。所以式(1)可轉(zhuǎn)化為：

任何一部偉大的作品都是它所處時(shí)代環(huán)境的產(chǎn)物，李漁在他的文學(xué)道路上選擇了通俗文學(xué)，選擇了戲曲小說(shuō)的創(chuàng)作，與明末清初小說(shuō)創(chuàng)作的興盛、市民文化走向成熟有密切聯(lián)系。李漁從事通俗文學(xué)創(chuàng)作和理論著述主要是在清初，明清二代是中國(guó)古代通俗文學(xué)創(chuàng)作和理論空前繁榮和迅速發(fā)展的極端，明清易代只是更換了封建統(tǒng)治者和生產(chǎn)關(guān)系，并沒(méi)有從根本上改變通俗文學(xué)長(zhǎng)足進(jìn)步的蓬勃趨勢(shì)。［2］

P(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(4)

其中：P(t)為t時(shí)刻對(duì)應(yīng)的脈沖值。將方程離散化，在各插值點(diǎn)處的脈沖值為：

P(i)=a0+a1(iΔt)+a2(iΔt)2+a3(iΔt)3+a4(iΔt)4+a5(iΔt)5

(5)

其中：i為插值次數(shù)。Δt為插值周期，s。各插值點(diǎn)處的理論頻率為：

f(i)=(P(i)-P(i-1))/Δt

(6)

脈沖輸出形式采用PWM波輸出，PWM是占空比可調(diào)的矩形波，非常適合電機(jī)的控制?？刂葡到y(tǒng)的主控芯片為STM32F407。該芯片有豐富的定時(shí)器資源，大部分定時(shí)器都含有PWM輸出通道，只需配置相關(guān)寄存器就可以輸出PWM波，通過(guò)調(diào)整PWM波產(chǎn)生的周期可以調(diào)整脈沖輸出頻率。PWM波輸出的實(shí)際脈沖頻率值f由式(7)計(jì)算：

(7)

其中：fCLK為時(shí)鐘源頻率，Hz；ARR為控制脈沖周期的寄存器值。

由式(7)可知，當(dāng)fCLK一定時(shí)，將式 (6)計(jì)算出的理論頻率f(i)代入式(7)可得ARR的值，將ARR的值加載到定時(shí)器上即可輸出對(duì)應(yīng)的脈沖頻率。但由于ARR寄存器是整型類型，故實(shí)際輸出的脈沖頻率會(huì)大于或等于理論脈沖頻率，且理論頻率越大，產(chǎn)生的誤差越大。若每隔固定插補(bǔ)周期去改變輸出脈沖頻率，這就使單個(gè)插值周期內(nèi)產(chǎn)生的脈沖數(shù)和目標(biāo)脈沖數(shù)不等，從而造成脈沖個(gè)數(shù)輸出的誤差。

本文采用變插值周期的插值方法來(lái)保證單個(gè)插值周期輸出的脈沖個(gè)數(shù)，即當(dāng)?shù)竭_(dá)各插值點(diǎn)脈沖值時(shí)計(jì)算下一個(gè)插值點(diǎn)所需頻率值，通過(guò)控制插值點(diǎn)處脈沖值來(lái)調(diào)整插值周期。為了便于計(jì)算，式(5)以Δt代入計(jì)算，使插值點(diǎn)脈沖值始終落在曲線上，不會(huì)出現(xiàn)累計(jì)誤差，實(shí)際插值時(shí)間是一個(gè)變化的接近Δt的數(shù)。

采用變插值周期的方法，不僅適用于五次多項(xiàng)式插值函數(shù)，也適用于任意已知解析式的簡(jiǎn)單插值函數(shù)。該方法控制簡(jiǎn)單，易于實(shí)現(xiàn)。

2 插值算法應(yīng)用

2.1 四自由度機(jī)器人運(yùn)動(dòng)學(xué)方程

機(jī)器人運(yùn)動(dòng)學(xué)方程的求解是軌跡規(guī)劃和控制的基礎(chǔ)。本文研究的四自由度工業(yè)機(jī)器人有4個(gè)關(guān)節(jié)，分別為上下軸移動(dòng)關(guān)節(jié)、擺臂軸轉(zhuǎn)動(dòng)關(guān)節(jié)、伸縮軸移動(dòng)關(guān)節(jié)和校正軸轉(zhuǎn)動(dòng)關(guān)節(jié)。該機(jī)器人是一個(gè)圓柱坐標(biāo)型機(jī)器人，4個(gè)關(guān)節(jié)均由伺服電機(jī)驅(qū)動(dòng)。

運(yùn)用D-H參數(shù)法構(gòu)建機(jī)器人的坐標(biāo)系如圖1所示。圖中d1為上下軸關(guān)節(jié)變化量，mm；θ1為擺臂軸關(guān)節(jié)變化量,(°)；d2為伸縮軸關(guān)節(jié)變化量,mm；θ2為校正軸關(guān)節(jié)變化量,(°)；L1=400 mm；L2=800 mm；L3=150 mm。

圖1 四自由度工業(yè)機(jī)器人D-H坐標(biāo)系

在圖1坐標(biāo)系下，機(jī)器人末端執(zhí)行器中心在基坐標(biāo)系0中的位姿矩陣為：

(8)

四自由度機(jī)器人的D-H參數(shù)見表1。

表1 四自由度工業(yè)機(jī)器人D-H參數(shù)

由D-H參數(shù)可得相鄰兩連桿的相對(duì)位姿矩陣分別為：

(9)

(10)

(11)

(12)

(13)

由式(8)-(13)可得機(jī)器人末端執(zhí)行器正運(yùn)動(dòng)學(xué)方程為：

(14)

由正運(yùn)動(dòng)學(xué)方程(14)可解得逆運(yùn)動(dòng)學(xué)方程為：

(15)

本文用向量F(d1,θ1,d2,θ2)表示由逆運(yùn)動(dòng)學(xué)方程計(jì)算得到的解。

2.2 插值流程圖

實(shí)現(xiàn)四自由度機(jī)器人位姿控制的五次多項(xiàng)式插值流程如圖2所示。

圖2 五次多項(xiàng)式插值流程

3 試 驗(yàn)

3.1 試驗(yàn)平臺(tái)簡(jiǎn)介

為驗(yàn)證插值算法實(shí)現(xiàn)的效果，搭建了如圖3所示的測(cè)試平臺(tái)。由示教器示教得到起點(diǎn)、終點(diǎn)位姿。分兩部分測(cè)試。第一部分由百分表測(cè)試重復(fù)定位精度，同時(shí)，在控制系統(tǒng)中編寫程序采集各軸控制電機(jī)編碼器反饋的脈沖值，并計(jì)算得到脈沖頻率和加速度值。第二部分由加速度傳感器采集末端執(zhí)行器的速度，經(jīng)數(shù)據(jù)采集儀處理后在電腦上實(shí)時(shí)顯示。

1.四自由度工業(yè)機(jī)器人；2.加速度傳感器；3.百分表；4.示教器；5.數(shù)據(jù)采集儀；6.電腦圖3 試驗(yàn)平臺(tái)

3.2 試驗(yàn)數(shù)據(jù)和分析

由示教器示教得到起點(diǎn)位置矢量p1(948.4962，-193.6945，420)，法線矢量n1(0.9397， -0.3420 ,0)，終點(diǎn)位置矢量p2(1194.6982，237.1066，420)，法線矢量n2(0.9397， 0.3420 ,0)。設(shè)定起點(diǎn)到終點(diǎn)運(yùn)行時(shí)間為1 s，起點(diǎn)和終點(diǎn)速度、加速度為零。

由示教起點(diǎn)和示教終點(diǎn)的位姿經(jīng)逆運(yùn)動(dòng)學(xué)計(jì)算后可得起點(diǎn)解向量F1(20，-10, 20，-10)，終點(diǎn)解向量F2(20，10, 270，10)。由解向量可知上下軸關(guān)節(jié)參數(shù)沒(méi)有變化。

每隔10 ms采集伸縮、擺臂、校正軸編碼器反饋的脈沖數(shù)，并計(jì)算各軸脈沖頻率和加速度值，其曲線如圖4所示。由圖4可以看出，所實(shí)現(xiàn)的機(jī)器人各軸頻率和加速度曲線符合五次多項(xiàng)式理論函數(shù)速度和加速度曲線的變化趨勢(shì)。頻率變化曲線平滑，加速度變化曲線沒(méi)有突變但存在較小的波動(dòng)。伸縮軸頻率和加速度曲線較其他軸變化幅度大，這和示教起點(diǎn)到終點(diǎn)伸縮軸關(guān)節(jié)參數(shù)變化較大相符。圖4中各軸運(yùn)行時(shí)間為1 s ,起點(diǎn)和終點(diǎn)處速度為零，可達(dá)到同時(shí)啟停。

圖4 伸縮、擺臂、校正軸的頻率、加速度變化曲線

加速度傳感器測(cè)得末端執(zhí)行器x、y、z方向速度變化如圖5所示。由圖5可以看出，末端執(zhí)行器速度曲線總體平滑，z方向速度在0 mm/s附近波動(dòng)。產(chǎn)生波動(dòng)的原因由機(jī)器人桿件彈性變形及測(cè)量誤差等因素造成。試驗(yàn)過(guò)程中，機(jī)器人能夠平穩(wěn)地運(yùn)行。

圖5 末端執(zhí)行器x、y、z方向速度曲線

機(jī)器人在示教起點(diǎn)和示教終點(diǎn)之間往復(fù)運(yùn)動(dòng)20次，百分表測(cè)得重復(fù)定位精度為±0.01 mm，且試驗(yàn)數(shù)據(jù)無(wú)累計(jì)誤差，可滿足一般工業(yè)要求。

4 結(jié) 論

本文給出了關(guān)節(jié)空間五次多項(xiàng)式插值詳細(xì)的實(shí)現(xiàn)過(guò)程，并提出了一種變周期插值方法，該方法易于實(shí)現(xiàn)，控制簡(jiǎn)單，無(wú)累計(jì)誤差，適用于任意已知解析式的簡(jiǎn)單插值函數(shù)。本文推導(dǎo)了一種四自由度工業(yè)機(jī)器人的運(yùn)動(dòng)學(xué)方程，為具有相似結(jié)構(gòu)的機(jī)器人提供了參考。將該算法應(yīng)用于四自由度工業(yè)機(jī)器人上，試驗(yàn)表明，機(jī)器人運(yùn)動(dòng)平穩(wěn)，重復(fù)定位精度為±0.01mm。同時(shí)也驗(yàn)證了五次多項(xiàng)式插值算法可平穩(wěn)地控制機(jī)器人運(yùn)動(dòng)。

[1] 劉極峰,易際明.機(jī)器人技術(shù)基礎(chǔ)[M].北京:高等教育出版社,2003:81-83.

[2] 李偉光,許陽(yáng)釗.搬運(yùn)機(jī)器人的軌跡規(guī)劃[J].組合機(jī)床與自動(dòng)化加工技術(shù),2010(5):83-85.

[3] 李萬(wàn)莉,陳熙巍,茹蘭.基于SimMechanics的4自由度機(jī)器人的軌跡規(guī)劃和仿真系統(tǒng)設(shè)計(jì)[J].中國(guó)工程機(jī)械學(xué)報(bào),2008,6(2):144-148.

[4] 王霆,管小清.拋物線軌跡規(guī)劃在工業(yè)機(jī)器人中的應(yīng)用[J].機(jī)電產(chǎn)品開發(fā)與創(chuàng)新,2012,25(3):18-24.

[5] 屠海斌,張侃健,方仕雄,等.四自由度機(jī)器人軌跡規(guī)劃算法研究與實(shí)現(xiàn)[J].工業(yè)控制計(jì)算機(jī),2015,28(5):120-125.

[6] 孫亮,馬江,阮曉鋼.六自由度機(jī)械臂軌跡規(guī)劃與仿真研究[J].控制工程,2010,17(3):388-392.

[7] 黎波,嚴(yán)駿,郭剛,等.挖掘機(jī)工作裝置復(fù)雜軌跡跟蹤實(shí)驗(yàn)[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),2012,43(12):39-43.

[8] 劉涼,陳超英.考慮關(guān)節(jié)摩擦的并聯(lián)機(jī)器人平滑軌跡規(guī)劃[J].機(jī)械工程學(xué)報(bào),2014,50(19):9-15.

[9]GOSSELINC,FOUCAULTS.Dynamicpoint-to-pointtrajectoryplanningofatwo-DOFcable-suspendedparallelrobot[J].IEEETransactionsonRobotics,2014,30(3):728-736.

[10] 劉鵬,張永貴,劉晨榮,等.Motomanup50機(jī)器人的軌跡規(guī)劃及仿真研究[J].機(jī)械制造與自動(dòng)化,2016(3):162-164.

[11] 壽媛,陳豫眉.利用MQ擬插值解決高次插值所出現(xiàn)的龍格現(xiàn)象[J].洛陽(yáng)師范學(xué)院學(xué)報(bào),2016,35(5):6-9.

[12] 蔡自興.機(jī)器人學(xué)[M].北京:清華大學(xué)出版社,2000:46-47.

(責(zé)任編輯： 康 鋒)

Robot Joint Space Planning Based on Quintic Polynomial of Variable Interpolation Cycle

TUKong,ZHANGHua

(Faculty of Mechanical Engineering & Automation, Zhejiang Sci-Tech University, Hangzhou 310018, China)

In order to control robot to reach the teaching point smoothly and accurately, the quintic polynomial interpolating function of robot joint space was studied, and this paper proposed an interpolation method of variable interpolation cycle. This interpolation algorithm was applied to a 4-DOF (4 degrees of freedom) industrial robot, and the direct and the inverse kinematics equations were established based on D-H (Denavit-Hartenberg) coordinate system. The interpolation algorithm was implemented in the motion control system. The poses of starting point and termination point were obtained by teaching, then the algorithm program was called to control the robot to reach the terminal point of teaching. The encoder feedback pulse values of each axis were collected to draw the joint frequency and acceleration curve. The acceleration sensor and dial indicator were used to test the velocity of terminal manipulator and repeated positioning accuracy of the robot. The experiment results showed that the joint frequency and acceleration curve conform to the change trend of quintic polynomial velocity and acceleration curve; the velocity change of terminal manipulator is continuous, and the robot moves smoothly; the repeated positioning accuracy is 0.01 mm.

quintic polynomial; variable interpolation cycle; D-H coordinate system; kinematics equation

10.3969/j.issn.1673-3851.2017.05.012

2016-09-20 網(wǎng)絡(luò)出版日期：2017-01-03

國(guó)家自然科學(xué)基金項(xiàng)目(51307151)；浙江理工大學(xué)科研啟動(dòng)基金項(xiàng)目(13022155-Y)；機(jī)械工程浙江省高校重中之重(一級(jí))學(xué)科優(yōu)秀青年人才培養(yǎng)基金項(xiàng)目(ZSTUME01B07)；浙江省科技廳公益技術(shù)研究工業(yè)項(xiàng)目(2017C31036)

涂 孔(1990-)，男，安徽郎溪人，碩士研究生，主要從事機(jī)器人控制方面的研究。

張 華，E-mail: zhanghua@zstu.edu.cn

TP242.2

A

1673- 3851 (2017) 03- 0376- 05