摘要：為了實現(xiàn)SCARA機器人的高速高精度控制，將帶偏心負(fù)載的SCARA機器人簡化成三連桿機構(gòu),并建立機構(gòu)的慣性力模型，進(jìn)行辨識計算出機器人各慣性項系數(shù)的值，建立基于慣性力模型的加速度自適應(yīng)控制方法，自適應(yīng)計算關(guān)節(jié)一理論最大運動角加速度。通過實驗研究和分析，加速度自適應(yīng)控制方法能夠充分發(fā)揮機器人電機的機械性能，防止電機因電流過大而報警，該方法能較好地應(yīng)用于SCARA機器人的工程控制中。

1引言

工業(yè)機器人（后文統(tǒng)稱機器人）已廣泛應(yīng)用于現(xiàn)代工業(yè)，隨著機器人和自動化技術(shù)的發(fā)展，其高速運動時的準(zhǔn)確性和穩(wěn)定性日益成為檢驗機器人性能的重要指標(biāo)，是機器人的運動控制系統(tǒng)必須考慮的因素。在高速運動的情況下，機器人的非線性控制特性十分明顯，并且實際存在的各種復(fù)雜不確性因素也嚴(yán)重影響著機器人的控制精度。SCARA（SelectiveComplianceAssemblyRobotArm）機器人廣泛應(yīng)用于3C裝配、抓取等對機器人的運動速度和精度要求更高的場合，因此對SCARA機器人進(jìn)行優(yōu)化控制方法研究，以實現(xiàn)高速高精度的控制目標(biāo)具有重要的理論意義和實用價值[1-3]。

為了實現(xiàn)機器人的高速高精度運動控制，國內(nèi)外的研究主要集中在以下幾個方面：（1）智能控制方法，主要包括時間最優(yōu)[4]、能量最優(yōu)[5]、加加速度最優(yōu)[6]等，以上智能尋優(yōu)算法，主要是對某些特定的軌跡，根據(jù)機器人關(guān)節(jié)幾何、速度和加速度等約束條件下，建立機器人運動時的關(guān)于時間、能量、加加速度等的目標(biāo)函數(shù)表達(dá)式，采用智能尋優(yōu)的方法計算最優(yōu)解。（2）基于動力學(xué)模型的控制方法，通過機器人動力學(xué)參數(shù)辨識，得到機器人的逆動力學(xué)模型，這樣，可以根據(jù)目標(biāo)軌跡計算出目標(biāo)軌跡各處的理論驅(qū)動力矩，再進(jìn)行相應(yīng)的動力學(xué)控制。

綜合來看，智能控制方法因為計算時間周期的原因，容易造成機器人控制器內(nèi)存資源的緊張，很難應(yīng)用在復(fù)雜的工業(yè)環(huán)境和實時插補的場合；基于動力學(xué)模型的運動控制，需要先進(jìn)行復(fù)雜的動力學(xué)參數(shù)辨識，對模型的精確度要求較高，且需要計算每個插補周期的理論驅(qū)動力矩值，計算量較大。

本文在實時開放式SCARA機器人控制平臺上，根據(jù)SCARA機器人的特點，建立簡化的關(guān)節(jié)慣性力模型；通過數(shù)據(jù)采集和處理，辨識出模型各系數(shù)的值；根據(jù)機器人基關(guān)節(jié)（關(guān)節(jié)一）電機的理論最大電流，自適應(yīng)計算插補路徑起點處的加速度和終點處的減速度。這樣，在開始插補前，可以確定最大的機器人關(guān)節(jié)運動加減速度，最大程度地利用關(guān)節(jié)一電機的機械性能，提高機器人運行的速度和精度。實驗結(jié)果表明該方法能較好地適用于SCARA機器人的高速高精度運動控制中。

2基于慣性力模型的SCARA機器人加速度自適應(yīng)控制方法

2.1SCARA機器人慣性力建模

串聯(lián)機器人的動力學(xué)方程可以表示為：

式（1）中，為各關(guān)節(jié)的驅(qū)動力矩，為慣性力項，為哥氏力和離心力項，為重力項。對于主要應(yīng)用于平面抓取、碼垛等領(lǐng)域的SCARA機器人，其高速運動時受力較大的主要是關(guān)節(jié)一。所以考慮關(guān)節(jié)一的動力學(xué)表達(dá)式時，可以忽略重力項的影響，而只考慮慣性力、哥氏力及離心力的作用。為了實現(xiàn)高速控制，一般將關(guān)節(jié)角加速度設(shè)置為角速度的10倍以上，這樣造成慣性力的作用遠(yuǎn)大于離心力和哥氏力。

圖1所示為典型的SCARA機器人高速運動時，關(guān)節(jié)一電機速度、加速度和電流（與力矩呈比例關(guān)系）分布曲線圖：

圖1典型的機器人運動時關(guān)節(jié)一電機速度、加速度和

電流分布曲線圖

Fig.1Typicaldistributioncurveofmotorspeed,accelerationandcurrentofjoint1whenrobotmoves

圖1中，橫坐標(biāo)為插補時間，單位是ms，黑色曲線為電機一速度曲線，單位為°/s，藍(lán)色曲線為電機一加速度曲線圖，單位為°/s2，紅色曲線為電機一電流曲線圖，單位為mA。從圖一可以看出，電流曲線的變化趨勢，       基本與加速度曲線的變化趨勢重合，因為式（1）中各項只有慣性力項與關(guān)節(jié)角加速度項相關(guān)，所以SCARA機器人進(jìn)行高速運動時，慣性力矩在關(guān)節(jié)驅(qū)動力矩各項中占據(jù)主導(dǎo)作用，建立機器人的慣性力模型并進(jìn)行基于模型的控制可以達(dá)到很好的控制效果。

對于如圖2所示的帶偏心負(fù)載的SCARA機器人，其主體結(jié)構(gòu)類似于一個簡化的平面三連桿模型，如圖3所示。

圖2帶偏心負(fù)載的SCARA機器人

Fig.2SCARArobotwitheccentricload

圖3簡化的平面三連桿SCARA機器人模型

Fig.3AsimplifiedplanarthreelinkSCARArobotmodel

圖3中，m1和m2分別為SCARA機器人大臂和小臂的質(zhì)量，l1和l2分別為機器人大小臂的長度，d1和d2為連桿質(zhì)心位置，d4為機器人末端所掛負(fù)載的偏心量，負(fù)載質(zhì)量為m4，為各連桿的轉(zhuǎn)角。使用拉格朗日方程[7]，進(jìn)行動力學(xué)方程推導(dǎo)：

式(2)中，k為系統(tǒng)的總動能，u為系統(tǒng)的總勢能，通過式(2)推導(dǎo)可以得到，在只考慮慣性力的情況下，第一關(guān)節(jié)的動力學(xué)表達(dá)式為：

式(3)中，分別為機器人動力學(xué)方程質(zhì)量矩陣的第一行三個元素，三項的數(shù)學(xué)表達(dá)式分別為：

其中：

式(5)中，分別為大小臂及負(fù)載的轉(zhuǎn)動慣量。在實際機器人的控制應(yīng)用時，在(4)式和(5)式中，各關(guān)節(jié)轉(zhuǎn)角和是可以精確已知的，在負(fù)載質(zhì)量和負(fù)載偏心一定的情況下，A1至A6等參數(shù)的值均為一定的，可以通過動力學(xué)參數(shù)辨識進(jìn)行辨識計算得到。

2.2慣性力模型的動力學(xué)參數(shù)辨識

傳統(tǒng)的機器人動力學(xué)參數(shù)辨識方法，需要首先離線優(yōu)化計算一條復(fù)雜的激勵軌跡[8]，一般使用多次多項式或傅里葉級數(shù)進(jìn)行關(guān)節(jié)軌跡激勵；然后使機器人運行激勵軌跡，通過大量的數(shù)據(jù)采集和處理，最后解超靜定方程組得到動力學(xué)參數(shù)的值。這里，結(jié)合SCARA機器人的特點，采用一種簡易的方法進(jìn)行以上參數(shù)的辨識。

根據(jù)前面的分析可以知道，在SCARA機器人進(jìn)行關(guān)節(jié)點位運動時，慣性力矩在關(guān)節(jié)力矩中占主導(dǎo)作用，特別是在關(guān)節(jié)角加速度最大處，根據(jù)式(3)，采用以下方法進(jìn)行數(shù)據(jù)采集：

(1)在一定的關(guān)節(jié)二轉(zhuǎn)角和關(guān)節(jié)四轉(zhuǎn)角下，以最快的速度只動關(guān)節(jié)一，采集關(guān)節(jié)一電機的電流和編碼器值信息；在不同的和下，重復(fù)以上操作，采集5-10組數(shù)據(jù)。此時，（3）式中只有起作用，對采集的數(shù)據(jù)進(jìn)行數(shù)據(jù)處理，可以建立關(guān)節(jié)一最大角加速度和最大驅(qū)動力矩（和電流呈線性比例關(guān)系）之間的數(shù)學(xué)表達(dá)式關(guān)系，即：

(2)在一定的關(guān)節(jié)一轉(zhuǎn)角和關(guān)節(jié)四轉(zhuǎn)角下，以最快的速度只動關(guān)節(jié)二，采集關(guān)節(jié)一電機的電流和關(guān)節(jié)二電機的編碼器值信息；在不同的和下，重復(fù)以上操作，采集5-10組數(shù)據(jù)。此時，(3)式中只有起作用，同理，可以建立關(guān)節(jié)二最大角加速度和關(guān)節(jié)一最大驅(qū)動力矩之間的數(shù)學(xué)表達(dá)式關(guān)系，即：

(3)在一定的關(guān)節(jié)一轉(zhuǎn)角和關(guān)節(jié)二轉(zhuǎn)角下，以最快的速度只動關(guān)節(jié)四，采集關(guān)節(jié)一電機的電流和關(guān)節(jié)四電機的編碼器值信息；在不同的和下，重復(fù)以上操作，采集5-10組數(shù)據(jù)。此時，(3)式中只有起作用，同理，可以建立關(guān)節(jié)四最大角加速度和關(guān)節(jié)一最大驅(qū)動力矩之間的數(shù)學(xué)表達(dá)式關(guān)系，即：

數(shù)據(jù)采集完成后，要進(jìn)行數(shù)據(jù)處理，因為采集的數(shù)據(jù)是電機的編碼器值和電流值，要帶入(6)-(8)式中進(jìn)行方程組求解，必須根據(jù)編碼器值，采用差分的方法計算關(guān)節(jié)角加速度，因為兩次差分得到的加速度噪聲太大，且采集的電流值也存在很大的高頻噪聲，所以還要采用相應(yīng)的濾波方法對采集的數(shù)據(jù)進(jìn)行處理。

首先，差分計算天然地具有噪聲放大的作用[9]，關(guān)節(jié)位置的直接前向差分會放大高頻噪聲的幅值，使得到角速度和角加速度產(chǎn)生波形失真，影響關(guān)節(jié)角加速度計算的精度。這里，根據(jù)電機編碼器值，采用中心差分法，每次計算五個點中中間點的關(guān)節(jié)角加速度值。

設(shè)定五個插補點的時間間隔為T(即機器人的理論插補周期)，設(shè)第三個點的關(guān)節(jié)轉(zhuǎn)角時間序列為T（根據(jù)關(guān)節(jié)編碼器值計算得到），則其他幾個點的時間序列分別為。分別對進(jìn)行泰勒四階展開，并相減可以得到：

式(9)和(10)中，和是常數(shù)，(9)式乘以8減去(10)式得:

總可以找到一個常數(shù)，使得式(12)成立。

這樣，通過采集的機器人運動時各時間序列的編碼器值，根據(jù)式(9)-式(15)，每次依次計算五個點中間點的關(guān)節(jié)角加速度值，即可以得到機器人運行各時間序列的關(guān)節(jié)角加速度值。

通過中心差分得到各時間序列的關(guān)節(jié)角加速度值，仍然會有很多高頻噪聲，同時，通過伺服電機底層采集的電流值，不經(jīng)過濾波也會有很大的計算誤差。所以，在MATLAB中構(gòu)建低通巴特沃斯濾波器[10]，它可以減小濾波過程中的相位偏移。分別對采集和處理的數(shù)據(jù)進(jìn)行依次濾波處理。濾波處理的順序為：關(guān)節(jié)轉(zhuǎn)角、關(guān)節(jié)角速度、關(guān)節(jié)角加速度、電流值。

經(jīng)過數(shù)據(jù)處理后，可以得到關(guān)節(jié)力矩和加速度值，取機器人每次運動時的最大關(guān)節(jié)角加速度值和最大力矩，分別帶入式(6)-式(8)，因為方程組的個數(shù)大于未知量的個數(shù)，所以采用最小二乘法，辨識計算得到慣性力模型的A1到A6各項參數(shù)的值。

2.3基于慣性力模型的SCARA機器人加速度自適應(yīng)控制

在SCARA機器人進(jìn)行快速關(guān)節(jié)點位運動時，因為關(guān)節(jié)加速過程主要在路徑起始和終止段，此時機器人可能會因各關(guān)節(jié)電機（特別是關(guān)節(jié)一）所需的驅(qū)動力矩過大而報警，特別是在機器人高速重載啟停時。這樣，可以根據(jù)關(guān)節(jié)一極限力矩的大小，通過慣性力模型和機器人關(guān)節(jié)點位運動起終點的位姿，自適應(yīng)計算關(guān)節(jié)一運動的加速度和減速度，以充分發(fā)揮電機的動力學(xué)性能，同時保證電機不因驅(qū)動力矩過大而報警。

設(shè)定機器人運行的起終點分別為和，因為SCARA機器人的關(guān)節(jié)三為垂直上下運動，對關(guān)節(jié)一力矩的影響可以忽略不計，所以沒有考慮進(jìn)來；設(shè)定根據(jù)關(guān)機一電機極限電流計算的關(guān)節(jié)一極限驅(qū)動力矩為。

在已經(jīng)通過辨識計算，得到慣性力模型的A1到A6各項參數(shù)的值后，在機器人運行路徑的起點P0處，將分別帶入式(4)，可以得到路徑起點P0處的質(zhì)量矩陣中與關(guān)節(jié)一驅(qū)動力矩相關(guān)的三項分別為：

因為SCARA機器人進(jìn)行關(guān)節(jié)點位運動時，關(guān)節(jié)一最大加速度和最大力矩出現(xiàn)的時間序列接近于路徑起點處，所以可以近似使用起點處的質(zhì)量矩陣項，作為最大關(guān)節(jié)力矩處的質(zhì)量矩陣項，將式(16)帶入式(3)，則：

在機器人進(jìn)行關(guān)節(jié)點位運動時，為了保證各關(guān)節(jié)同時啟停，各關(guān)節(jié)實際運動的加速度的比例關(guān)系是確定的，設(shè)定：

以關(guān)節(jié)一的極限力矩值，作為(19)式中的，則可以計算得到理論最大的關(guān)節(jié)一運動角加速度值為：

同理，可以得到路徑終點P1處的質(zhì)量矩陣中與關(guān)節(jié)一驅(qū)動力矩相關(guān)的三項分別為：

同時，可以得到路徑理論最大的關(guān)節(jié)一減速度為：

這樣，在確定了起終點位姿的情況下，可以根據(jù)關(guān)節(jié)一的極限力矩，在插補開始之前，自適應(yīng)規(guī)劃出關(guān)節(jié)一的理論最大關(guān)節(jié)角加速度和減速度，既可以保證機器人不因電機電流過大而報警，同時也滿足機器人高速運動的要求。

3實驗研究與分析

3.1實驗平臺

實驗平臺選用的是基于開放式控制器系統(tǒng)的SCARA機器人平臺，它通過高速DSP滿足實時插補的要求，其控制系統(tǒng)基于ARM+DSP架構(gòu)，上位機程序運行在ARM端，主要用于機器人示教點的選取及示教程序的編寫等功能的實現(xiàn)，機器人的運動控制程序運行在DSP端。ARM和DSP端通過中斷中的雙口RAM進(jìn)行數(shù)據(jù)交互，示教程序通過中斷從ARM端傳到DSP端，同時在機器人運行的過程中，電機的信息（如編碼器脈沖和電流值等）也會實時通過DSP中的中斷反饋到ARM端再保存。機器人控制系統(tǒng)框圖如圖4所示

圖4機器人控制系統(tǒng)框圖

Fig.4Theblockdiagramofrobotcontrolsystem

3.2實驗研究與分析

根據(jù)文獻(xiàn)[11]建立SCARA機器人坐標(biāo)系，確定機器人連桿參數(shù)如下：，機器人攜帶2kg、60mm偏心負(fù)載。

首先，根據(jù)前述的參數(shù)辨識方法，通過數(shù)據(jù)采集和數(shù)據(jù)處理，辨識出式(5)中慣性力模型的A1到A6各項參數(shù)的值。

在辨識時，選取一組機器人運動數(shù)據(jù)，對關(guān)節(jié)電機力矩采用截止頻率為10Hz的巴特沃斯低通濾波器進(jìn)行濾波，對于電機編碼器值，在每次差分前后，采用20Hz巴特沃斯低通濾波器進(jìn)行濾波，再進(jìn)行數(shù)學(xué)運算轉(zhuǎn)化為關(guān)節(jié)角加速度值，圖5和圖6分別是關(guān)節(jié)力矩和角加速度的濾波效果對比(綠色為濾波前曲線，紅色為濾波后曲線)：

圖5關(guān)節(jié)力矩的濾波效果圖

Fig.5Thefilteringeffectofjointtorque

圖6關(guān)節(jié)角加速度的濾波效果圖

Fig.6Thefilteringeffectofjointangularacceleration

從圖5和圖6可以看出經(jīng)過巴特沃斯低通濾波器進(jìn)行濾波后，差分后的高頻噪聲和讀到的力矩噪聲，能很好地濾除，且沒有相位偏移，這樣可以在很大程度上保證參數(shù)辨識的精度。

然后，在機器人的工作空間選取4個示教點（示教點的各關(guān)節(jié)轉(zhuǎn)角見表1），分兩組分別進(jìn)行關(guān)節(jié)點到點運動。機器人的運動路徑分別為P1到P2的點到點運動和P3到P4的點到點運動。為了加強實驗對比，機器人運動時，分別使用基于慣性力模型的SCARA機器人加速度自適應(yīng)控制方法（后文簡稱自適應(yīng)控制）和傳統(tǒng)的預(yù)先設(shè)定最大加速度的控制方法（后文簡稱傳統(tǒng)控制）進(jìn)行對比實驗，設(shè)定關(guān)節(jié)一的極限力矩為70Nm，預(yù)先設(shè)定的速度、加速度和減速度值（速度單位為，加速度單位為）如下圖7所示：

圖7設(shè)定的各關(guān)節(jié)初始速度和加速度值

Fig.7Initialvelocityandaccelerationofeachsetofjoints

表1實驗用示教點各關(guān)節(jié)轉(zhuǎn)角

Tab.1thejointrotationanglesofteachingpoints

在循環(huán)運行P1到P2的點到點運動時，兩種控制方法下的關(guān)節(jié)一關(guān)節(jié)力矩和關(guān)節(jié)角加速度的濾波后對比曲線圖分別如圖8（a）和8（b）所示：

圖8(a)兩種控制下關(guān)節(jié)一力矩對比圖

Fig.8(a)Thecontrastofjointtorqueoftwokindsofcontrol

圖8(b)兩種控制下關(guān)節(jié)一角加速度對比圖

Fig.8(b)Thecontrastofaccelerationofjointundertwocontrol

從圖8（a）可以看出，在循環(huán)運行P1到P2的點到點運動時，使用自適應(yīng)控制時，關(guān)節(jié)一的實際運行最大力矩控制在±70Nm左右，說明慣性力模型的精度可以達(dá)到要求，而使用傳統(tǒng)控制時則為50Nm，自適應(yīng)控制可以充分發(fā)揮電機的機械性能；同時，采用自適應(yīng)控制時，關(guān)節(jié)的加速度和減速度根據(jù)起點和終點處的慣性項（M11，M12，M14）大小不同，呈現(xiàn)不對稱分布，整體的運動節(jié)拍減小13.83%。

在循環(huán)運行P3到P4的點到點運動時，此時傳統(tǒng)控制下，全速運行時機器人關(guān)節(jié)一電機因電流過大而報警，所以傳統(tǒng)控制時按照85%倍率的運動學(xué)參數(shù)運行，兩種控制方法下的關(guān)節(jié)一關(guān)節(jié)力矩和關(guān)節(jié)角加速度的濾波后對比曲線圖分別如圖9（a）和9（b）所示，同時也分析了兩種控制情況下關(guān)節(jié)一電機軌跡跟蹤誤差曲線如圖10所示：

圖9(a)兩種控制下關(guān)節(jié)一力矩對比圖

Fig.9(a)Thecontrastofjointtorqueoftwokindsofcontrol

圖9(b)兩種控制下關(guān)節(jié)一角加速度對比圖

Fig.9(b)Thecontrastofaccelerationofjointundertwocontrol

圖10兩種控制下關(guān)節(jié)一電機軌跡跟蹤誤差對比圖

Fig.10Thecontrastoftrackingerrorofjointmotortrajectoryundertwocontrol

從圖9（a）可以看出，在循環(huán)運行P3到P4的點到點運動時，使用自適應(yīng)控制時，關(guān)節(jié)一的實際運行最大力矩控制在±70Nm左右，說明慣性力模型的精度可以達(dá)到要求，而使用傳統(tǒng)控制時，因為加減速度不能自適應(yīng)調(diào)整，起終點處的慣性力大小不同，造成起終點處的極限力矩分別為70Nm和-90Nm，終點處的實際驅(qū)動力矩已經(jīng)超過極限力矩，使機器人抖動加大；而采用自適應(yīng)控制時，關(guān)節(jié)的加速度和減速度根據(jù)起點和終點處的慣性力項大小不同（M11，M12，M14），呈現(xiàn)不對稱分布。從圖10可以看出，進(jìn)行自適應(yīng)控制時，關(guān)節(jié)一電機的軌跡跟蹤誤差有所減小，在路徑終點附近最大減小19.76%，說明自適應(yīng)控制時機器人運動的抖動較傳統(tǒng)控制有一定程度的減小。

4總結(jié)

本文對基于慣性力模型的SCARA機器人加速度自適應(yīng)控制進(jìn)行研究。通過將帶偏心負(fù)載的SCARA機器人簡化成一個三連桿模型，根據(jù)機器人的實際特點，建立機器人的慣性力模型；采用中心差分和濾波的方法進(jìn)行辨識數(shù)據(jù)的采集，辨識出各慣性項系數(shù)；建立基于慣性力模型的關(guān)節(jié)一角加速度自適應(yīng)控制方法。最后，在開放式控制器的機器人平臺上進(jìn)行了自適應(yīng)控制和傳統(tǒng)控制的對比實驗研究，實驗結(jié)果表明，進(jìn)行自適應(yīng)控制時，能根據(jù)機器人位姿在插補開始之前，自適應(yīng)調(diào)整關(guān)節(jié)一運行的最大角加速度，充分發(fā)揮機器人電機的機械性能，同時防止機器人因關(guān)節(jié)電機電流過大而報警，滿足SCARA機器人高速高精度控制的要求。