劉 鵬，宋 濤，贠 超，高志慧

（北京航空航天大學(xué) 機(jī)械工程及自動(dòng)化學(xué)院，北京 100191）

0 引 言

隨著工業(yè)自動(dòng)化技術(shù)的提高，焊接機(jī)器人技術(shù)日趨成熟，現(xiàn)已大量應(yīng)用到工業(yè)生產(chǎn)中。為實(shí)現(xiàn)對(duì)石化容器、梁構(gòu)等大型焊件的自動(dòng)焊接，筆者所在的課題組與北京中電華強(qiáng)焊接工程技術(shù)有限公司聯(lián)合研制了7 自由度大型焊接機(jī)器人“中電一號(hào)”。

為了保證焊接質(zhì)量，弧焊機(jī)器人作業(yè)過程中，不僅對(duì)末端執(zhí)行器（焊絲）的位姿、速度及加速度有很高的要求，而且要求運(yùn)動(dòng)軌跡為連續(xù)平滑的曲線。因此研究者需要建立機(jī)器人運(yùn)動(dòng)模型，并對(duì)其運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃，從而滿足焊接工作的要求。

本研究運(yùn)用D-H 坐標(biāo)系理論，分析該機(jī)器人的運(yùn)動(dòng)學(xué)問題[1]；并在運(yùn)動(dòng)學(xué)分析的基礎(chǔ)上，在關(guān)節(jié)空間采用三次多項(xiàng)式函數(shù)插值法，研究機(jī)器人軌跡規(guī)劃的問題；最后在Matlab7.8 平臺(tái)上，運(yùn)用機(jī)器人工具箱[2]進(jìn)行仿真，以驗(yàn)證軌跡規(guī)劃的合理性。

1 運(yùn)動(dòng)學(xué)分析

1.1 機(jī)器人結(jié)構(gòu)分析及D-H 坐標(biāo)系建立

“中電一號(hào)”是個(gè)7 自由度關(guān)節(jié)式機(jī)器人，其結(jié)構(gòu)圖如圖1 所示。該機(jī)器人由腰部、大臂、小臂和腕部等部分組成，所有關(guān)節(jié)均為轉(zhuǎn)動(dòng)關(guān)節(jié)。

圖1 7 自由度機(jī)器人結(jié)構(gòu)圖

從理論上來說，該7 自由度機(jī)器人為冗余度機(jī)器人。但是實(shí)際上，機(jī)器人的第7 關(guān)節(jié)僅是為了滿足焊接工藝需要，使焊槍在焊接過程中保持轉(zhuǎn)動(dòng)，不對(duì)機(jī)器人末端的運(yùn)動(dòng)軌跡產(chǎn)生影響。因此，研究者在進(jìn)行運(yùn)動(dòng)學(xué)分析時(shí)，只需建立前6 個(gè)關(guān)節(jié)的運(yùn)動(dòng)學(xué)模型。

本研究按照D-H 方法建立其連桿坐標(biāo)系，D-H 坐標(biāo)系的建立如圖2 所示。

圖2 D-H 坐標(biāo)系的建立

1.2 機(jī)器人正逆運(yùn)動(dòng)學(xué)分析

1.2.1 運(yùn)動(dòng)學(xué)正解

D-H 參數(shù)表如表1 所示。

根據(jù)連桿坐標(biāo)系和D-H 參數(shù)表，機(jī)器人運(yùn)動(dòng)學(xué)方程可描述為：

式中：i-1iT—第i 個(gè)連桿坐標(biāo)系相對(duì)于第i-1 個(gè)連桿坐標(biāo)系的齊次變換矩陣。

且有：

式中：cθi=cosθi，sθi=sinθi，cαi=cosαi，sαi=sinαi(i=0,1…6)。

表1 D-H 參數(shù)表

將表1 中的連桿參數(shù)代入到式（1，2）中，得到機(jī)器人末端相對(duì)于基坐標(biāo)系的位姿矩陣60Τ 。同時(shí)，末端在基座標(biāo)系下的位姿亦可表示為：

式中：

1.2.2 運(yùn)動(dòng)學(xué)逆解

機(jī)器人逆運(yùn)動(dòng)學(xué)求解一般有兩種方法：封閉解法和數(shù)值解法[3]。封閉解法計(jì)算速度快，效率高，便于實(shí)時(shí)控制；數(shù)值解法是一種迭代法，不能求出所有的解。對(duì)于本研究設(shè)計(jì)的焊接機(jī)器人，由圖2 可以看出，其第2、3、4 關(guān)節(jié)軸相互平行，在結(jié)構(gòu)上滿足Pieper 準(zhǔn)則[4]，可以采用封閉解法。因此，本研究采用封閉解法來求解該逆運(yùn)動(dòng)學(xué)問題，則：

（1）求解 θ1。

在式（3）兩端同乘以01T-1，式（4）左邊第2 行第4 列元素為常數(shù)，將等式對(duì)應(yīng)元素等同起來，可得：

由三角代換可得：

其中，正、負(fù)號(hào)對(duì)應(yīng)的兩個(gè)解對(duì)應(yīng)著θ1的兩個(gè)可能。

（2）求解 θ5。

在確定θ1的一個(gè)解之后，再觀察方程式（4）兩端第2 行前3 個(gè)元素，聯(lián)立等式，可求得：

（3）求解 θ6。

令等式（4）兩端第2 行第1 列及第2 行第2 列元素對(duì)應(yīng)相等，則有：

如果 sθ5≠ 0，則可解得：

（4）求解 θ2。

令式（4）左右兩邊第1 行第3 列、第3 行第3 列元素對(duì)應(yīng)相等，則有：

若 sθ5≠ 0，可解得：

結(jié)合式（3），可得下列關(guān)系式：

若 sθ5≠ 0，則令：

通過上式可解得：

（5）求解 θ3。

結(jié)合式（8，9），有：

進(jìn)而可解得：

（6）求解 θ4。

至此，6 個(gè)關(guān)節(jié)變量的逆解均已求得。θ1、θ2 以及θ5有兩個(gè)解，而θ3、θ4 和θ6 均只有一個(gè)解。因此滿足Pieper 準(zhǔn)則的機(jī)器人最多有8 組解，但受關(guān)節(jié)轉(zhuǎn)動(dòng)范圍的限制，某些解是偽解，不可實(shí)現(xiàn)[5]。

1.3 雅克比求解

焊接速度的均勻與否，直接關(guān)系著焊接質(zhì)量的好壞[6]。因此，本研究必須通過雅克比對(duì)機(jī)器人進(jìn)行速度的規(guī)劃。對(duì)于n 個(gè)關(guān)節(jié)的機(jī)器人，雅可比矩陣是6 ×n 矩陣，前3 行代表對(duì)夾手線速度v 的傳遞比，后3行代表對(duì)夾手角速度ω的傳遞比，即：

式中：n=6；雅克比矩陣—6 ×6 方陣；JLi，JAi—關(guān)節(jié)i 的單位關(guān)節(jié)速度引起夾手線速度和角速度。

本研究采用微分變換法求解雅克比矩陣。

因?yàn)橹须娨惶?hào)各個(gè)關(guān)節(jié)為轉(zhuǎn)動(dòng)關(guān)節(jié)，對(duì)于任意的關(guān)節(jié)i，連桿i 相對(duì)于連桿i-1 繞坐標(biāo)系{}i 的zi 軸所作微分運(yùn)動(dòng)dθi，其微分運(yùn)動(dòng)矢量為：

由微分變換公式[7]，可解得夾手相應(yīng)的微分運(yùn)動(dòng)矢量為：

因此，可得雅克比矩陣J(q)的第i 列如下：

式（12～14）中：i=1,2…6；n,o,a,p—niT 的4 個(gè)列矢量。

受篇幅所限，對(duì)于雅克比矩陣各元素的具體數(shù)值，本研究不再給以表述。下文將在機(jī)器人運(yùn)動(dòng)學(xué)分析的基礎(chǔ)上，對(duì)其軌跡規(guī)劃進(jìn)行研究。

2 軌跡規(guī)劃

關(guān)節(jié)空間的軌跡規(guī)劃具有計(jì)算簡(jiǎn)單、不發(fā)生機(jī)構(gòu)奇異性等特點(diǎn)[8]。關(guān)節(jié)空間的軌跡規(guī)劃有拋物線過渡線性插值、三次多項(xiàng)式插值、五次多項(xiàng)式插值及B 樣條插值法[9]。綜合考慮系統(tǒng)的穩(wěn)定性及控制的實(shí)時(shí)性，本研究在三次多項(xiàng)式插值法的基礎(chǔ)上，在關(guān)節(jié)空間構(gòu)造了過路徑點(diǎn)的三次多項(xiàng)運(yùn)動(dòng)路徑。該算法簡(jiǎn)單、計(jì)算量小，且具有較高的控制精度，結(jié)合研究課題需要，本研究采用該算法進(jìn)行關(guān)節(jié)空間軌跡規(guī)劃的研究。

2.1 笛卡爾空間到關(guān)節(jié)空間轉(zhuǎn)化

假設(shè)笛卡爾空間有一連續(xù)軌跡被離散為n 個(gè)空間位姿節(jié)點(diǎn)序列{T}，其上任意兩個(gè)相鄰的節(jié)點(diǎn)的位姿可表示為Ti、Ti+1，速度可表示為[viωi]T和[vi+1ωi+1]T。

根據(jù)1.3.2 部分內(nèi)容，可求得兩節(jié)點(diǎn)所對(duì)應(yīng)的兩組關(guān)節(jié)角度為 Θi、Θi+1；由1.4 節(jié)部分的內(nèi)容，可推導(dǎo)出：

由式（15），從而求得其對(duì)應(yīng)的關(guān)節(jié)速度為Qi和Qi+1。因此，對(duì)笛卡爾空間軌跡的規(guī)劃可間接轉(zhuǎn)化為對(duì)關(guān)節(jié)空間的軌跡規(guī)劃。

2.2 過路徑點(diǎn)的三次多項(xiàng)式插值

根據(jù)2.1 節(jié)中假設(shè)，在機(jī)械手運(yùn)動(dòng)過程中，需經(jīng)過n 個(gè)空間位姿點(diǎn)。這n 個(gè)位姿點(diǎn)對(duì)應(yīng)著關(guān)節(jié)空間的n組關(guān)節(jié)角度，也就意味著關(guān)節(jié)空間中各關(guān)節(jié)必過這n組關(guān)節(jié)角度。

對(duì)任意兩組相鄰的關(guān)節(jié)角度，本研究取其中某個(gè)關(guān)節(jié)進(jìn)行分析，稱該關(guān)節(jié)前一個(gè)關(guān)節(jié)角度θi為起始點(diǎn)θ0，則后一個(gè)關(guān)節(jié)角度θi+1為終止點(diǎn)θf。該段運(yùn)動(dòng)軌跡的描述，可用起始點(diǎn)關(guān)節(jié)角度與終止點(diǎn)關(guān)節(jié)角度的一個(gè)平滑插值函數(shù)θ(t)來表示，θ(t)在t0=0 時(shí)刻的值是θ0，在終端時(shí)刻tf的值是終止關(guān)節(jié)角度θf。

為了實(shí)現(xiàn)單個(gè)關(guān)節(jié)的平穩(wěn)運(yùn)動(dòng)，軌跡函數(shù)θ(t)至少需要滿足4 個(gè)約束條件。其中，兩個(gè)約束條件是起始點(diǎn)和終止點(diǎn)對(duì)應(yīng)的關(guān)節(jié)角度：

為了滿足關(guān)節(jié)速度的連續(xù)性要求，在起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)速度須滿足：

設(shè)三次多項(xiàng)式的表達(dá)式為：

則結(jié)合式（16，17）可以求得三次多項(xiàng)式的系數(shù)如下式所示：

式中：θ0，θf，θ˙0，θ˙f—起始點(diǎn)和終止點(diǎn)的角度和角速度。

關(guān)于它們的求解方式，2.1 節(jié)中已闡述，不再贅述。

3 仿真驗(yàn)證

本研究以Matlab7.8 為實(shí)驗(yàn)平臺(tái)，運(yùn)用機(jī)器人工具箱，對(duì)機(jī)器人運(yùn)動(dòng)學(xué)、軌跡規(guī)劃進(jìn)行仿真分析[10]。

3.1 運(yùn)動(dòng)學(xué)建模

筆者運(yùn)用機(jī)器人工具箱，結(jié)合D-H 參數(shù)，編寫相關(guān)代碼，以建立機(jī)器人模型：

3.2 運(yùn)動(dòng)學(xué)仿真

本研究假設(shè)機(jī)器人初始關(guān)節(jié)角度為q0=[0 0 0 0 0 0]，經(jīng)歷2 s 運(yùn)動(dòng)到N（2.386，-0.066，2.431）時(shí)的關(guān)節(jié)角度為qn=[0 1.625-1.644 0.019 1.57 0]。

由運(yùn)動(dòng)學(xué)方程，可求得機(jī)器人末端初始位姿矩陣為：

機(jī)器人末端終止位姿矩陣為：

仿真中機(jī)器人將平穩(wěn)地從初始位姿運(yùn)動(dòng)到終止位姿。機(jī)器人末端運(yùn)動(dòng)軌跡如圖3 所示。

圖3 機(jī)器人末端運(yùn)動(dòng)軌跡

通過仿真，可以觀察到機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)平穩(wěn)，驗(yàn)證了連桿參數(shù)設(shè)計(jì)的合理性和正解算法的正確性。

3.3 關(guān)節(jié)空間軌跡規(guī)劃仿真

根據(jù)機(jī)器人末端軌跡，本研究通過運(yùn)動(dòng)學(xué)反解得出各關(guān)節(jié)位移，由逆雅克比求出關(guān)節(jié)速度。再利用三次多項(xiàng)式進(jìn)行插值，分別計(jì)算出6 個(gè)關(guān)節(jié)各自對(duì)應(yīng)的a0，a1，a2，a3，實(shí)現(xiàn)各個(gè)關(guān)節(jié)的軌跡規(guī)劃。

本研究以Robotics Toolbox 為工具，驗(yàn)證關(guān)節(jié)空間三次多項(xiàng)式插值規(guī)劃的合理性。假設(shè)機(jī)器人為剛體，末端沒有負(fù)載，仿真時(shí)間為2 s，機(jī)器人各關(guān)節(jié)從q0=[0 0 0 0 0 0]，運(yùn)動(dòng)到 qn=[p i/6 pi/5 pi/4 pi/3 pi/2 pi/1]，各變量隨時(shí)間變化的曲線圖如圖4 所示。

從圖4 中可以看出，各個(gè)關(guān)節(jié)的速度及加速度曲線連續(xù)且平滑，對(duì)各關(guān)節(jié)既無剛性沖擊又無柔性沖擊，故基本能夠滿足控制要求。同時(shí)也證明了機(jī)器人各連桿參數(shù)設(shè)計(jì)的合理性，以及基于關(guān)節(jié)空間的軌跡規(guī)劃方法的可行性。該技術(shù)目前已應(yīng)用于機(jī)器人點(diǎn)焊、角焊作業(yè)中，實(shí)際使用效果較好。

4 結(jié)束語

本研究運(yùn)用D-H 坐標(biāo)系理論，研究了“中電一號(hào)”焊接機(jī)器人的運(yùn)動(dòng)學(xué)問題，并在運(yùn)動(dòng)學(xué)分析的基礎(chǔ)上，采用過路徑點(diǎn)的三次多項(xiàng)式函數(shù)插值法，實(shí)現(xiàn)了機(jī)器人在關(guān)節(jié)空間的軌跡規(guī)劃。

在Matlab7.8 平臺(tái)上，本研究建立了機(jī)器人運(yùn)動(dòng)學(xué)模型，對(duì)機(jī)器人運(yùn)動(dòng)學(xué)問題和關(guān)節(jié)空間軌跡規(guī)劃進(jìn)行仿真驗(yàn)證。仿真結(jié)果表明，機(jī)器人連桿參數(shù)設(shè)計(jì)合理、運(yùn)動(dòng)學(xué)模型建立正確，同時(shí)也驗(yàn)證了基于關(guān)節(jié)空間的三次多項(xiàng)式函數(shù)插值算法的軌跡規(guī)劃方法合理可行，為機(jī)器人動(dòng)力學(xué)及焊接機(jī)器人今后在馬鞍面多層多道焊接的研究打下了基礎(chǔ)。

圖4 各變量隨時(shí)間變化的曲線圖

（References）：

[1]劉松國(guó).六自由度串聯(lián)機(jī)器人運(yùn)動(dòng)優(yōu)化與軌跡跟蹤控制研究[D].杭州：浙江大學(xué)流體傳動(dòng)及控制國(guó)家重點(diǎn)實(shí)驗(yàn)室，2009.

[2]謝 斌，蔡自興.基于Matlab Robotics Toolbox 的機(jī)器人學(xué)仿真實(shí)驗(yàn)教學(xué)[J].計(jì)算機(jī)教育，2010（19）：140-143.

[3]熊有倫.機(jī)器人學(xué)[M].北京：機(jī)械工業(yè)出版社，1993.

[4]呂世增，張大衛(wèi)，劉海年.基于吳方法的6R 機(jī)器人逆運(yùn)動(dòng)學(xué)旋量方程求解[J].機(jī)械工程學(xué)報(bào)，2010，46（17）：35-41.

[5]王 偉，謝明紅，周國(guó)義.6-DOF 工業(yè)機(jī)器人逆解優(yōu)化及其工作空間的研究[J].機(jī)械與電子，2011（1）：57-60.

[6]毛志偉，李舒揚(yáng)，葛文韜，等.移動(dòng)焊接機(jī)器人大折角角焊縫跟蹤及工藝[J].焊接學(xué)報(bào)，2011，32（2）：33-36.

[7]蔡自興.機(jī)器入學(xué)[M].北京：清華大學(xué)出版社，2000.

[8]KUBOTA N，ARAKAWA T.Trajectory generation for redun?dantmanipulator using virus evolutionary genetic algorithm.[C]//Robotics and Automation，1997-Proceedings，IEEE In?ternational Conference on Albuquerque，USA：IEEE，1997：205-210.

[9]馮曉波.機(jī)器人準(zhǔn)確制孔技術(shù)研究[D].杭州：浙江大學(xué)機(jī)械工程學(xué)系，2011.

[10]程永倫，朱世強(qiáng)，羅利佳，等.基于Matlab 的QJ-6R 焊接機(jī)器人運(yùn)動(dòng)學(xué)分析及仿真[J].機(jī)電工程，2007，24（11）：107-110.