劉 力

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

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

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

1 機(jī)械臂動(dòng)力學(xué)模型建立及正向運(yùn)動(dòng)求解

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

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

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

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

Hi=

(1)

(2)

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

(3)

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

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

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

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

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

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

(4)

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

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

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

(5)

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

(6)

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

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

D=nΔL

(7)

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

(8)

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

(9)

其中第k個(gè)精插補(bǔ)點(diǎn)Pk的坐標(biāo)值為：

(10)

如果在線段P1P2上插入m個(gè)精插補(bǔ)點(diǎn)，那么與線段P1P2相對(duì)應(yīng)的機(jī)械臂移動(dòng)軌跡總長(zhǎng)度L′為：

(11)

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

2 實(shí)驗(yàn)

2.1 實(shí)驗(yàn)平臺(tái)

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

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

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

2.2 機(jī)械臂運(yùn)動(dòng)軌跡修正效果驗(yàn)證

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

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

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

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

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

圖5 直線插補(bǔ)曲線修正效果圖

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

2.3 機(jī)械臂運(yùn)動(dòng)軌跡偏差及控制復(fù)雜度對(duì)比

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

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

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

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

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

3 結(jié)束語(yǔ)

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