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

涂 孔，張 華

(浙江理工大學機械與自動控制學院，杭州 310018)

?

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

涂 孔，張 華

(浙江理工大學機械與自動控制學院，杭州 310018)

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

五次多項式；變插值周期；D-H坐標系；運動學方程

0 引 言

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

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

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

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

五次多項式插值通式為:

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

(1)

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

(2)

(3)

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

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

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

(4)

其中：P(t)為t時刻對應的脈沖值。將方程離散化，在各插值點處的脈沖值為：

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。各插值點處的理論頻率為：

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

(6)

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

(7)

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

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

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

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

2 插值算法應用

2.1 四自由度機器人運動學方程

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

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

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

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

(8)

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

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

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

(9)

(10)

(11)

(12)

(13)

由式(8)-(13)可得機器人末端執(zhí)行器正運動學方程為：

(14)

由正運動學方程(14)可解得逆運動學方程為：

(15)

本文用向量F(d1,θ1,d2,θ2)表示由逆運動學方程計算得到的解。

2.2 插值流程圖

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

圖2 五次多項式插值流程

3 試 驗

3.1 試驗平臺簡介

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

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

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

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

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

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

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

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

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

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

4 結(jié) 論

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

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

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

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

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

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

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

[7] 黎波,嚴駿,郭剛,等.挖掘機工作裝置復雜軌跡跟蹤實驗[J].農(nóng)業(yè)機械學報,2012,43(12):39-43.

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

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

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

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

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

(責任編輯： 康 鋒)

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)絡出版日期：2017-01-03

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

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

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

TP242.2

A

1673- 3851 (2017) 03- 0376- 05