豐 非，扈宏杰

（北京航空航天大學(xué) 自動化科學(xué)與電氣工程學(xué)院，北京 100191）

隨著我國工業(yè)的高速發(fā)展，平面關(guān)節(jié)型SCARA機器人在工業(yè)生產(chǎn)的各個領(lǐng)域得到了廣泛的應(yīng)用[1]。建立精確的機器人動力學(xué)模型，是提高機器人的運動精度的一個重要因素。而機器人的動力學(xué)參數(shù)辨識是機器人動力學(xué)建模的關(guān)鍵。

在此，基于D-H參數(shù)法建立了SCARA機器人的運動學(xué)模型，建立了機器人的動力學(xué)模型；針對模型中存在的不確定的參數(shù)，選取合適的激勵軌跡，提出了一種混沌的粒子群算法，對機器人的慣性參數(shù)進(jìn)行辨識，與傳統(tǒng)的遞推最小二乘法和基本粒子群算法進(jìn)行對比，通過Matlab仿真實驗，驗證了改進(jìn)的混沌粒子群算法能夠有效避免粒子群算法的早熟收斂問題，跳出局部最優(yōu)，極大地提高了計算精度和全局尋優(yōu)能力。

1 運動學(xué)建模

基于D-H參數(shù)方法[2]，對機器人各個連桿坐標(biāo)系進(jìn)行描述，如圖1所示。SCARA機器人的第1關(guān)節(jié)、第2關(guān)節(jié)以及第4關(guān)節(jié)為轉(zhuǎn)動關(guān)節(jié)，第3關(guān)節(jié)為移動關(guān)節(jié)。其中機器人的D-H參數(shù)見表1[3]。

圖1 SCARA機器人連桿坐標(biāo)系Fig.1 Link frames of SCARA

表1 SCARA機器人的D-H參數(shù)Tab.1 D-H link parameters of SCARA

由連桿變換公式、矩陣公式和變換傳遞式，可以得到第4關(guān)節(jié)到基坐標(biāo)系的齊次變換矩陣[4]為

式中：t1=cos（θ1+θ2+θ4）；t2=-sin（θ1+θ2+θ4）；t3=a2cos·（θ1+θ2）+a1cosθ1；t4=sin（θ1+θ2+θ4）；t5=cos（θ1+θ2+θ4）；t5=cos（θ1+θ2+θ4）；t6=a2sin（θ1+θ2）+a1sinθ1；t7=d1+d2+d3

通過調(diào)用Matlab中的 Robotic Toolbox[5]，可以直觀地將建立的運動學(xué)模型表示出來。通過調(diào)節(jié)4個關(guān)節(jié)的各個變量，可以看到機器人形態(tài)的變化，根據(jù)輸入的各個關(guān)節(jié)變量和第4關(guān)節(jié)末端的位置，驗證機器人運動學(xué)建模的正確性。

2 動力學(xué)建模

選用Lagrange法對SCARA機器人進(jìn)行動力學(xué)建模[6]。Lagrange方程從能量的角度上對系統(tǒng)進(jìn)行分析，通過Lagrange方程可以將忽略摩擦的機器人的動力學(xué)模型寫為

式中：M（θ）為慣性力項；C（θ，θ˙）為科氏力和離心力項；G（θ）為重力項。

由于機械臂的后2個關(guān)節(jié)重量遠(yuǎn)遠(yuǎn)小于前2個關(guān)節(jié)，因此在建立動力學(xué)模型的時候，可以簡化模型計算，將機器人的末端2個關(guān)節(jié)固連在第2個關(guān)節(jié)上，視為負(fù)載。這樣，機器人模型就簡化為一個平面雙臂機器人模型，同時由于機械臂只在水平面內(nèi)運動，重力項的作用可以忽略為0，大大簡化了計算復(fù)雜度[7]。簡化后的動力學(xué)方程可以寫成二階形式：

式中：m1，m2分別為SCARA機器人第1和第2關(guān)節(jié)的質(zhì)量；I1，I2分別為相對于桿件質(zhì)心的轉(zhuǎn)動慣量；a1，a2為機械臂的連桿長度；x1，x2為質(zhì)心位置；θ1，θ2為關(guān)節(jié)角。

分離式（4）中的運動學(xué)參數(shù)和慣性參數(shù)，將式（3）改寫成為Ax=B的線性形式：

式中：ξ的參數(shù)為機器人的慣性參數(shù)和運動學(xué)參數(shù)，對于一個給定的機器人，其慣性參數(shù)為一給定的常量；為觀測矩陣，其中的參數(shù)全部由組成，不依賴于機器人的運動學(xué)參數(shù)和慣性參數(shù)。通過計算，可以得到觀測矩陣的各個參數(shù)為

ξ陣中的各個參數(shù)向量為

需辨識的ξ陣中一共有6個參數(shù)。在激勵軌跡確定的情況下，測量機器人各個關(guān)節(jié)的力矩輸出，就可以通過參數(shù)辨識的方法建立確定的機器人動力學(xué)模型。

3 參數(shù)辨識及激勵軌跡設(shè)計

設(shè)計激勵軌跡，使得機器人充分運動，在滿足系統(tǒng)實際約束的同時，最大化地激發(fā)所有需要辨識的參數(shù)，減少辨識的誤差，是參數(shù)辨識的重要問題之一。

目前常用的激勵軌跡一般為傅里葉級數(shù)[8]。機器人第 i 個關(guān)節(jié)的角位移 θi，角速度以及加速度的軌跡為

式中：ωf為傅里葉級數(shù)的基頻。傅里葉函數(shù)的周期為 2π/ωf。 每一個傅里葉級數(shù)中包含有（2N+1）個參數(shù)，即為 an，bn，θ0。 這里為了減小計算量，選取 N=3，故優(yōu)化激勵軌跡中需要優(yōu)化的參數(shù)共有7個。

在優(yōu)化激勵軌跡之前，應(yīng)首先使機器人滿足實際工作的約束條件。即根據(jù)機器人的實際情況，將系統(tǒng)的運動范圍、角速度、角加速度限制在一定的范圍內(nèi)，故約束條件為

在滿足上述約束后，采用退化矩陣的條件數(shù)來優(yōu)化軌跡各個參數(shù)。條件數(shù)越接近于1，方程組的狀態(tài)越好。為此，優(yōu)化激勵軌跡參數(shù)的問題就變成了確定系數(shù) an，bn，θ0，使得觀測矩陣 W的條件數(shù)最小。

為減少計算量，使用遺傳優(yōu)化的方法對參數(shù)進(jìn)行優(yōu)化，采用Matlab中的遺傳算法工具箱進(jìn)行求解，選擇交叉概率pc=0.5，變異概率pm=0.2。經(jīng)過遺傳優(yōu)化，得到第1關(guān)節(jié)和第2關(guān)節(jié)的傅里葉級數(shù)參數(shù)，見表2。

表2 優(yōu)化的傅里葉級數(shù)參數(shù)Tab.2 Optimized fourier parameters

根據(jù)優(yōu)化的參數(shù)，獲得如圖2所示的激勵軌跡。由圖可見，第1關(guān)節(jié)和第2關(guān)節(jié)的角位移、角速度和角加速度均符合機器人的物理約束。

圖2 兩個關(guān)節(jié)激勵軌跡的角位移、角速度、角加速度Fig.2 Angle，velocity and acceleration of two links on the incentive trajectory

4 混沌PSO方法辨識參數(shù)

粒子群算法源于對鳥群捕食行為的研究，是一種基于群體協(xié)作的隨機搜索算法[9]。PSO算法中，每個粒子都有一個由被優(yōu)化的函數(shù)所所決定的適應(yīng)度函數(shù)，還有一個決定著粒子飛行方向和距離的速度，粒子追隨當(dāng)前的最優(yōu)粒子在解空間中搜索解[10]。

粒子速度及位置更新公式為

式中：i=1，2，…，m，其中 m 為種群規(guī)模；d=1，2，…，G其中G為搜索空間維數(shù)；w為慣性權(quán)重；c1，c2為學(xué)習(xí)因子；r1，r2為服從 U（0，1）分布的隨機數(shù)；pid為局部最優(yōu)值；pgd為全局最優(yōu)值。由式（6）知，需要辨識的參數(shù)共有6個，因此令粒子的搜索空間維數(shù)G=6。

在此利用PSO算法的收斂速度快和混沌運動的遍歷性的優(yōu)點，針對傳統(tǒng)的PSO方法中存在的收斂過慢和“早熟”的問題，提出了一種基于混沌優(yōu)化思想的混沌粒子群（CPSO）算法，改善了PSO算法固有的容易陷入局部最優(yōu)的缺點，提高了算法的收斂速度和精度。

利用混沌粒子群進(jìn)行優(yōu)化的基本思想，主要有以下兩方面：①采用混沌序列對粒子的位置和速度進(jìn)行初始化，通過混沌的方法提高種群的多樣性和種群搜索的遍歷性；②以目前搜索到的最優(yōu)位置為基礎(chǔ)產(chǎn)生一個混沌序列，用該序列中的最優(yōu)位置粒子替代當(dāng)前粒子群中的一個粒子的位置，以此避免陷入局部最優(yōu)的情況出現(xiàn)[11]。

在粒子信息初始化的階段，隨機產(chǎn)生1個六維每個分量數(shù)值在0～1之間的向量，選用Logisitc方程對其進(jìn)行混沌初始化，得到6個混沌向量，然后將這六個向量載波到對應(yīng)變量的取值區(qū)間中去，得到初始解。

Logisitc方程為

式中：μ為控制參量，取 μ＝2；設(shè) 0≤sn≤1。在每一次迭代結(jié)束前，對得到的最優(yōu)位置進(jìn)行混沌優(yōu)化。將pgd（i=1，2，…，6）映射到 Logisitc 方程的定義域［0，1］；si=（pgd-xmin）（xmax-xmin）；用 Logisitc 方程進(jìn)行迭代，產(chǎn)生關(guān)于每一個粒子的pgd的混沌序列；將產(chǎn)生的混沌序列通過逆映射返回到原解空間，從而得到一組新的pgd；對其進(jìn)行適應(yīng)度計算，從這一組pgd中選取性能最好的作為全局最優(yōu)值，并將該組數(shù)據(jù)pgd取代當(dāng)前群體中的任意一個粒子的位置，繼續(xù)參與迭代，直至滿足誤差要求或完成迭代為止。

5 仿真結(jié)果與分析

對機器人參數(shù)進(jìn)行辨識，將適應(yīng)度函數(shù)定義為

分別使用基本PSO算法、混沌PSO算法對系統(tǒng)進(jìn)行辨識，選?。悍N群個數(shù)為200個；學(xué)習(xí)因子c1=1.3，c2=1.8；慣性權(quán)重w從0.9遞減至0.2。設(shè)置最大迭代次數(shù)為200次。得到的適應(yīng)度函數(shù)變化曲線如圖3所示。

圖3 CPSO和PSO的適應(yīng)度函數(shù)變化曲線Fig.3 Fitness function changes curve of CPSO and PSO

由圖可見，應(yīng)用混沌序列優(yōu)化粒子群算法中的粒子初始化過程，能夠使系統(tǒng)的初始解更加優(yōu)秀。同時，在更新速度和位置信息的時候，應(yīng)用混沌PSO的優(yōu)化方法可以使得適應(yīng)度函數(shù)的收斂速度大大加快，能夠提高對最優(yōu)值的全局搜索能力。

采用傳統(tǒng)的遞推最小二乘、基本PSO算法和混沌PSO算法，最終得到的適應(yīng)度函數(shù)值和各個辨識參數(shù)見表3。

表3 參數(shù)辨識結(jié)果Tab.3 Result of parameters ddentification

由表可知，遞推最小二乘法辨識的系統(tǒng)模型誤差較大，基本PSO和混沌PSO方法對系模型辨識效果較好。采用混沌PSO方法對機器人參數(shù)進(jìn)行辨識，得到的適應(yīng)度函數(shù)最小，得到的模型相比遞推最小二乘和基本PSO更符合實際模型。

把采用混沌PSO方法辨識出的各個參數(shù)代入式（5），得到基于辨識模型的2個關(guān)節(jié)的力矩輸出，將其與實際模型的力矩輸出進(jìn)行對比，如圖4所示。由圖可見，2個關(guān)節(jié)的力矩輸出曲線與其實際模型力矩輸出曲線均基本吻合，說明采用混沌粒子群算法對機器人模型進(jìn)行辨識具有較高的準(zhǔn)確度。

圖4 采用CPSO優(yōu)化的辨識模型和實際模型力矩輸出Fig.4 Torque of actual model and identification model using CPSO

分別將遞推最小二乘法、基本粒子群法、混沌粒子群法辨識得到的模型與實際SCARA機器人模型進(jìn)行比較，得到的輸出力矩誤差如圖5所示。由圖可見，采用混沌粒子群算法優(yōu)化的機器人模型與實際模型最為接近，誤差最小。由此進(jìn)一步驗證了混沌PSO算法相比傳統(tǒng)的最小二乘和粒子群方法的優(yōu)越性。

6 結(jié)語

對SCARA機器人進(jìn)行運動學(xué)建模和動力學(xué)建模，提出了簡化的SCARA機器人模型，設(shè)計優(yōu)化的激勵軌跡，對該模型進(jìn)行參數(shù)辨識。在傳統(tǒng)的PSO算法的基礎(chǔ)上，提出了混沌PSO優(yōu)化算法，將混沌的遍歷性和粒子群收斂快的特點相結(jié)合。將該算法與遞推最小二乘、傳統(tǒng)PSO算法進(jìn)行對比，通過仿真實驗，驗證其對于機器人模型參數(shù)辨識的正確性。仿真結(jié)果表明，該改進(jìn)方法能夠有效地改善粒子群算法的早熟收斂問題，跳出局部最優(yōu)，極大地提高了計算精度和全局尋優(yōu)能力。

圖5 兩個關(guān)節(jié)力矩誤差Fig.5 Torque error of two links

[1]劉振宇，趙彬，朱海波.六自由度機械臂分揀系統(tǒng)仿真平臺研究[J].機械設(shè)計與制造，2013，34（2）：210-213.

[2]Craig J J.Introduction to Robotics：Mechanics and Control[M].3rd ed.Prentice Hall，2005：62-89.

[3] 崔敏其.SCARA機器人的拉格朗日動力學(xué)建模[J].機械設(shè)計與制造，2013，34（12）：76-78.

[4]贠超.機器人學(xué)導(dǎo)論[M].3版.北京：機械工業(yè)出版社，2006：59.

[5]Corke P.A robotics toolbox for Matlab[J].IEEE Robotics and Automation Magazine，1996，3（1）：24-32.

[6]Ishibashi N，Maeda Y.Learning of inverse-dynamics for SCARA robot[C]//SICE Annual Conference（SICE），2011 Proceedings of IEEE，2011：1300-1303.

[7]閆昊.SCARA機器人動力學(xué)分析及魯棒性控制研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2013.

[8]劉金琨，沈曉蓉，趙龍.系統(tǒng)辨識理論及Matlab仿真[M].北京：電子工業(yè)出版社，2013：203.

[9]Lu Jian’an，Chen Yonghua.Particle swarm optimization （PSO）based topology optimization of part design with fuzzy parameter tuning[J].Computer-Aided Design and Applications，2013，（9）：62-68.

[10]Fan H Y.A modification to particle swarm optimization algorithm[J].Engineering Computations，2002，19（7-8）：970-989.

[11]唐賢倫.混沌粒子群優(yōu)化算法理論及應(yīng)用研究[D].重慶：重慶大學(xué)，2007.