基于直插補模型的機(jī)器人機(jī)械臂運動控制軌跡智能修正研究

劉 力

(延安職業(yè)技術(shù)學(xué)院機(jī)電工程系，陜西 延安 716000)

機(jī)器人是“工業(yè)4.0”時代的產(chǎn)物[1]，也是現(xiàn)代制造技術(shù)、信息技術(shù)、計算機(jī)科學(xué)技術(shù)、電氣自動化控制技術(shù)、無線傳感技術(shù)等多學(xué)科融合發(fā)展的成果[2]。隨著機(jī)器人機(jī)械臂工作效率、控制精度的不斷改善與提高，其在高精度工業(yè)制造、軍工國防、航空航天、醫(yī)療衛(wèi)生、農(nóng)業(yè)生產(chǎn)等領(lǐng)域有著越來越廣泛的應(yīng)用[3]。機(jī)械臂移動軌跡規(guī)劃與補償控制會直接影響其最終的控制精度[4]，因此一直以來軌跡控制都是該領(lǐng)域的研究熱點之一。機(jī)械臂軌跡控制首先要精確計算出機(jī)械臂末端執(zhí)行器的空間坐標(biāo)值、關(guān)節(jié)旋轉(zhuǎn)角度偏差值，然后再利用插值補償算法以理想的運動軌跡為基準(zhǔn)[5]，實現(xiàn)對機(jī)械臂運動軌跡的實時修正。匯總國內(nèi)外現(xiàn)有的研究成果可以發(fā)現(xiàn)，在機(jī)器人機(jī)械臂軌跡規(guī)劃與智能修正方面主要采用PID(比例、積分、微分)控制、模糊控制、人工智能仿生控制及多項式插值控制等多種算法。

PID控制是工業(yè)控制領(lǐng)域的經(jīng)典算法之一，具有算法簡潔、穩(wěn)定性強等優(yōu)點，但PID控制在控制精度、適用性等方面有較強的局限性，不適用于復(fù)雜度較高的機(jī)械臂系統(tǒng)[6]；模糊控制是處理非線性問題的有效方案之一，但模糊控制的局限性在于模糊規(guī)則的制定與模糊隸屬度函數(shù)的選擇過于主觀，往往會影響最終判定結(jié)果的客觀性[7]；人工智能控制是近年來工業(yè)控制領(lǐng)域的研究熱點和主流方向，人工智能控制多采用模擬人腦、人體神經(jīng)網(wǎng)絡(luò)、動物種群活動等特征，實現(xiàn)一種動態(tài)化的尋優(yōu)和求解[8]，人工智能算法在故障檢測領(lǐng)域的應(yīng)用范圍和應(yīng)用效果遠(yuǎn)優(yōu)于工業(yè)控制領(lǐng)域，但在全局控制過程中容易陷入局部最優(yōu)解[9]；基于多項式的插值控制具有良好的控制精度，但整個控制過程的算法過于復(fù)雜，往往不具有實際應(yīng)用價值或可操作性[10]。直線插補模型是一種精度極高的點群逼近方式[11]，本文將直線插補經(jīng)典算法應(yīng)用到機(jī)械臂軌跡控制領(lǐng)域，并對算法的插補方式進(jìn)行優(yōu)化，即采用多段逼近的方式，在不增加算法復(fù)雜度的情況下改善機(jī)械臂軌跡的智能修正控制精度。

1 機(jī)械臂動力學(xué)模型建立及正向運動求解

1.1 機(jī)械臂空間坐標(biāo)分析

機(jī)器人機(jī)械臂運動軌跡的智能修正與實時糾偏是在機(jī)械臂動力學(xué)模型基礎(chǔ)上完成的，軌跡的修正是在局部、小范圍內(nèi)進(jìn)行的，如果運動中的軌跡偏差過大應(yīng)重新調(diào)整系統(tǒng)程序。建立機(jī)械臂連桿坐標(biāo)系時涉及到的系統(tǒng)參數(shù)包括連桿長度Li、旋轉(zhuǎn)角度βi、連桿的偏距ηi、關(guān)節(jié)的角度θi，各參數(shù)的空間位置及變換關(guān)系如圖1所示。

圖1 機(jī)械臂連桿、關(guān)節(jié)變化示意圖

確定了機(jī)械臂第i個關(guān)節(jié)在空間的坐標(biāo)維度后，根據(jù)機(jī)械臂連桿與關(guān)節(jié)的空間位置關(guān)系，建立每一個關(guān)節(jié)和連桿的空間坐標(biāo)系，并進(jìn)行廣義上的坐標(biāo)變換。機(jī)械臂連桿和關(guān)節(jié)的運動方程可以基于坐標(biāo)變換矩陣推導(dǎo)得到。例如從機(jī)械臂連桿(i-1)到連桿i的空間坐標(biāo)矩陣變換過程如下：繞zi-1軸旋轉(zhuǎn)βi角度，此時xi-1軸與xi軸旋轉(zhuǎn)到同一個平面；zi-1軸在空間內(nèi)平移ηi-1個單位的距離，使xi-1軸與xi軸重合；平移連桿(i-1)使其坐標(biāo)系與連桿i的坐標(biāo)系原點重合，此時旋轉(zhuǎn)xi-1軸，使空間坐標(biāo)軸zi-1與zi重合。通過機(jī)器人機(jī)械臂連桿和關(guān)節(jié)的坐標(biāo)變換，可以得到空間范圍內(nèi)連桿與關(guān)節(jié)的齊次坐標(biāo)變化矩陣Hi：

Hi=

(1)

(2)

機(jī)械臂正運動求解過程主要利用D-H矩陣求出各關(guān)節(jié)和連桿的齊次坐標(biāo)變化矩陣，再利用式(2)確定機(jī)械臂末端執(zhí)行器的空間位置和姿態(tài)，此時式(2)可以改寫為：

(3)

式中：ζ,o,L,τ分別為關(guān)節(jié)旋轉(zhuǎn)角度、坐標(biāo)中心移動距離、機(jī)械連桿位移和關(guān)節(jié)偏置角度。

矩陣中的各項參數(shù)可利用式(1)和式(2)中的參數(shù)化簡得出，在正向求解中只要當(dāng)機(jī)器人機(jī)械臂各關(guān)節(jié)參數(shù)、關(guān)節(jié)旋轉(zhuǎn)角度、連桿位移確定之后，末端執(zhí)行器的空間位姿矩陣參數(shù)也隨之確定。

1.2 基于直插補模型的機(jī)械臂軌跡修正

機(jī)械臂軌跡規(guī)劃過程中，末端執(zhí)行器的行進(jìn)路徑受到連桿位移、移動速度、加速度等多種參數(shù)的影響及狀態(tài)約束。在笛卡爾空間坐標(biāo)系內(nèi)，機(jī)械臂按設(shè)定軌跡行進(jìn),不僅要考慮到躲避障礙物造成的軌跡偏移，還要關(guān)注關(guān)節(jié)奇異、連桿不連續(xù)、系統(tǒng)誤差、控制信號延遲等因素的影響所導(dǎo)致的運動軌跡偏差。直線插補模型采用足夠短的直線段擬合和糾正末端執(zhí)行器的軌跡偏差，為滿足糾偏模型弓高誤差最小化要求，本文采用內(nèi)接弦線法調(diào)整線段的長度，具體擬合過程如圖2所示。

圖2 內(nèi)接弦線法擬合過程

圖中點P1和P2是機(jī)械臂末端執(zhí)行器運動軌跡上的任意兩點，當(dāng)弓高誤差趨近于零時，P1和P2兩點趨于重合，即線段P1P2的長度趨近于零。如果末端執(zhí)行器運動軌跡出現(xiàn)偏差，則采用直線插補法擬合曲線，糾正原有的弧形軌跡路線。設(shè)行進(jìn)軌跡曲線的曲率半徑為r，則線段P1P2與弧線之間的最大弓高誤差d為：

(4)

式中：L為末端執(zhí)行器連桿長度。

最大弓高誤差必須小于軌跡曲線的曲率半徑離散精度κ：

r2-2rκ+κ2≤r2-(L/2)2

(5)

當(dāng)離散精度κ很小時，忽略κ2，可以推導(dǎo)出：

(6)

隨著擬合精度的不斷提高，插入的線段PiPj的長度越來越小。將一組PiPj線段擬合成為新的軌跡曲線。在直線插補過程中通常以固定的時間周期逼近新的行進(jìn)軌跡，但機(jī)械臂速度或加速度過大會導(dǎo)致弓高誤差過大，從而使修正后的軌跡精度降低。直線插補程序以固定的插補周期為基礎(chǔ)，在每個采樣周期內(nèi)都要根據(jù)機(jī)械臂的行進(jìn)速度、加速度、位移總量確定出下一個插補周期的位移增量、角度增量信息。

在精插補過程中仍舊以圖2所示的二維平面內(nèi)的線段P1P2為例，其中P1為起點，P2為終點，給定一個從P1到P2方向的速度v，為重新擬合機(jī)械臂行進(jìn)軌跡曲線，在線段P1P2上取一點Pk作為精插補點，取Pk+1為Pk的下一個精插補點，Δx和Δy分別為與x軸和y軸平行的進(jìn)給向量分量，則Pk與Pk+1之間的距離ΔL為Δx與Δy之和。在整個插補周期T之內(nèi)，直線段被分成n段，設(shè)線段P1P2的長度為D，則：

D=nΔL

(7)

采用連續(xù)小直線段插補不僅能夠最大限度地降低最大弓高誤差，也可以通過多次加速進(jìn)給提高軌跡的擬合精度。采用基于多線段直插補的方式能夠滿足機(jī)械臂末端執(zhí)行器精確移動要求，即使出現(xiàn)了移動軌跡偏差，通過對多個直線插補線段的曲線擬合，也可以連續(xù)修正軌跡的偏差。仍以6R機(jī)械臂為例，當(dāng)末端執(zhí)行器傾斜移動時需要同時對第1～4關(guān)節(jié)進(jìn)行連續(xù)的空間補償，以抵銷關(guān)節(jié)之間移動所造成的關(guān)節(jié)軌跡偏差。計算合適的直線插補位移量，保證每個關(guān)節(jié)移動過程中弓高誤差均被控制在較低的值，即擬合后的軌跡曲率與精度滿足機(jī)械臂運行的精度要求。線段P1P2的軌跡擬合方程為：

(8)

式中：xP1,yP1和xP2，yP2為P1和P2點的坐標(biāo)值；f(x,y)為軌跡坐標(biāo)擬合方程。根據(jù)機(jī)械臂控制精度的要求，在線段P1P2插入(n-1)個分割點，將線段分成n段，則x軸和y軸方向上的進(jìn)給向量分量Δx和Δy分別為：

(9)

其中第k個精插補點Pk的坐標(biāo)值為：

(10)

如果在線段P1P2上插入m個精插補點，那么與線段P1P2相對應(yīng)的機(jī)械臂移動軌跡總長度L′為：

(11)

在實際的機(jī)械臂直線插補軌跡控制中，以脈沖信號控制機(jī)械臂系統(tǒng)的伺服馬達(dá)，為了與精插補點的擬合過程相一致，輸入的脈沖信號數(shù)量必須與精插補點的數(shù)量m相對應(yīng)，且恒為整數(shù)。清除機(jī)械臂控制系統(tǒng)中已經(jīng)設(shè)定的運動軌跡，按照新擬合的軌跡調(diào)整連桿、關(guān)節(jié)的旋轉(zhuǎn)角度、位移等，系統(tǒng)原設(shè)定的運動速度、關(guān)節(jié)加速度等參數(shù)也要做相應(yīng)的調(diào)整，最后按新的軌跡運行。修正后的機(jī)械臂運功軌跡更趨近于理論值，從而使機(jī)械臂末端執(zhí)行器的控制精度得到本質(zhì)上的改善。

2 實驗

2.1 實驗平臺

本文實驗以6R模塊化機(jī)器人機(jī)械臂為研究對象，使用無線傳感器采集機(jī)械臂各關(guān)節(jié)的旋轉(zhuǎn)角度和連桿位移，通過上位機(jī)實時觀測與修正機(jī)械臂移動過程中的軌跡。上位機(jī)的配置為：Intel core i7處理器，工作主頻3.6 GHz，運行內(nèi)存和數(shù)據(jù)存儲內(nèi)存分別為8 GB和512 GB。6R機(jī)械臂的結(jié)構(gòu)設(shè)計如圖3所示。

圖3 實驗用6R機(jī)械臂系統(tǒng)結(jié)構(gòu)圖

機(jī)械臂各關(guān)節(jié)與連桿的接觸點都安裝了運動穩(wěn)定裝置和無線傳感器，一方面能夠減輕關(guān)節(jié)結(jié)構(gòu)之間的相互影響與摩擦干擾，保證機(jī)械臂按照事先設(shè)定好的軌跡路線行進(jìn)；另一方面如果機(jī)械臂在某一段軌跡由于復(fù)雜因素發(fā)生了偏移，傳感器會在第一時間預(yù)警并將發(fā)生偏移的數(shù)據(jù)傳遞到上位機(jī)，以便及時糾偏。

2.2 機(jī)械臂運動軌跡修正效果驗證

在上位機(jī)中安裝MATLAB仿真軟件，取如圖4所示的P0-P1段機(jī)械臂末端執(zhí)行器的行進(jìn)曲線，對機(jī)械臂的運動過程進(jìn)行仿真，其中P2～P5為行進(jìn)軌跡起止點之間的中間點。

圖4 機(jī)械臂末端行進(jìn)軌跡圖

機(jī)器人機(jī)械臂行進(jìn)軌跡上P0～P5點的空間坐標(biāo)見表1。

表1 機(jī)械臂軌跡采樣點的空間坐標(biāo) 單位:mm

機(jī)械臂關(guān)節(jié)終端傳感器的數(shù)據(jù)反饋顯示，機(jī)械臂末端執(zhí)行裝置從P2點開始出現(xiàn)軌跡偏離，即曲線P2P3、P3P4和P4P5均出現(xiàn)了不同程度的偏離。以P4P5段為例分析糾偏效果，如圖5所示。

圖5 直線插補曲線修正效果圖

P4P5直線插補修正結(jié)果顯示，直線插補擬合的14條線段與曲線的弓高誤差最大值僅為0.01 mm，表明經(jīng)過軌跡修整和線段擬合之后的機(jī)器人機(jī)械臂行進(jìn)軌跡，更接近于理想狀態(tài)下的行進(jìn)軌跡。

2.3 機(jī)械臂運動軌跡偏差及控制復(fù)雜度對比

機(jī)械臂末端執(zhí)行器的軌跡偏差由機(jī)械臂各個關(guān)節(jié)旋轉(zhuǎn)角度及連桿位移的偏差導(dǎo)致，在軌跡智能修正過程中關(guān)節(jié)和連桿的角度、位移偏差會逐漸縮小，偏差值越小表明修正的效果越好。在圖4所示的P0-P5段軌跡修復(fù)過程中，觀測各關(guān)節(jié)和連桿的角度偏差，同時引入經(jīng)典PID控制算法和模糊控制算法參與對比。關(guān)節(jié)旋轉(zhuǎn)角度偏差值對比見表2。

表2 機(jī)械臂關(guān)節(jié)旋轉(zhuǎn)角度偏差 單位:(°)

由表2可知，經(jīng)過直線插補模型軌跡修正后的關(guān)節(jié)角度偏差相比傳統(tǒng)PID控制和模糊控制修正方法的偏差值更低，具有更好的軌跡修正效果。修正算法的復(fù)雜度也是衡量算法性能的重要指標(biāo)，在算法復(fù)雜度測試中引入人工智能體系中的深度學(xué)習(xí)算法和多項式插值算法進(jìn)行軌跡修正，軌跡修正完成后各種算法的迭代循環(huán)次數(shù)如圖6所示。

圖6 修正算法的復(fù)雜度分析

由圖可以看出，深度學(xué)習(xí)修正算法完成軌跡修正的迭代次數(shù)為37次，多項式插值迭代算法迭代次數(shù)達(dá)到了43次，都遠(yuǎn)高于直線插補修正方法，證明直線插補模型算法更加簡潔。

3 結(jié)束語

機(jī)器人機(jī)械臂在運動中會出現(xiàn)偏離預(yù)定理想軌跡的現(xiàn)象，本文采用計算復(fù)雜度更低的直線插補模型糾正機(jī)械臂運動中的軌跡偏移。直線插補模型采用長度很小的直線段擬合和糾正機(jī)械臂關(guān)節(jié)旋轉(zhuǎn)角度及連桿的位移偏差，并將最大弓高誤差控制在允許的最小范圍內(nèi)，采用該模型擬合后的曲線更接近于理想軌跡。實驗結(jié)果表明，直線插補模型的軌跡修正效果良好，計算的復(fù)雜度更低。