冗余串聯(lián)機(jī)械臂軌跡規(guī)劃與控制*

□ 楊松山 □ 徐增丙 □ 王志剛

武漢科技大學(xué) 機(jī)械自動(dòng)化學(xué)院 武漢 430081

1 研究背景

隨著科學(xué)技術(shù)的發(fā)展,機(jī)械臂的應(yīng)用要求越來越高。利用虛擬樣機(jī)技術(shù)代替真實(shí)物理樣機(jī)試驗(yàn),在設(shè)計(jì)過程中可以提高物理樣機(jī)性能,避免樣機(jī)制造的高成本,縮短開發(fā)周期[1],對(duì)機(jī)械臂整體性能指標(biāo)還可以反復(fù)進(jìn)行試驗(yàn)與改進(jìn)[2]。筆者利用Adams動(dòng)力學(xué)仿真軟件進(jìn)行運(yùn)動(dòng)分析,觀察機(jī)械臂運(yùn)動(dòng)狀態(tài),在Matlab軟件環(huán)境下建立控制系統(tǒng),發(fā)揮兩款軟件各自集成的優(yōu)勢(shì),對(duì)機(jī)械臂進(jìn)行交互式仿真,從而使機(jī)械臂快速、精準(zhǔn)完成動(dòng)作,為實(shí)際樣機(jī)制作供理論依據(jù)。

2 機(jī)械臂模型

Adams軟件的三維實(shí)體建模環(huán)境較差,為了方便分析,降低結(jié)構(gòu)設(shè)計(jì)和控制系統(tǒng)設(shè)計(jì)的復(fù)雜程度,在SolidWorks軟件中建立機(jī)械臂實(shí)體模型。在結(jié)構(gòu)設(shè)計(jì)中進(jìn)行合理的簡(jiǎn)化,機(jī)械臂由底座、轉(zhuǎn)臺(tái)、四節(jié)臂架、執(zhí)行器組成。

機(jī)械臂模型如圖1所示。

▲圖1 機(jī)械臂模型

3 機(jī)械臂運(yùn)動(dòng)學(xué)分析

3.1 運(yùn)動(dòng)模型

建立機(jī)械臂的連桿坐標(biāo)系,然后利用Denavit-Hartenberg參數(shù)法分析機(jī)械臂運(yùn)動(dòng)學(xué)方程的求解過程[3]。建立連桿坐標(biāo)系的方法包括標(biāo)準(zhǔn)Denavit-Hartenberg法和改進(jìn)Denavit-Hartenberg法,標(biāo)準(zhǔn)Denavit-Hartenberg法與改進(jìn)Denavit-Hartenberg法在建立各連桿坐標(biāo)系時(shí),原點(diǎn)坐標(biāo)的選取有所不同。筆者采用標(biāo)準(zhǔn)Denavit-Hartenberg法建立機(jī)械臂連桿坐標(biāo)系[4-5],如圖2所示。兩相鄰連桿之間的關(guān)系用連桿長(zhǎng)度ai、連桿扭角αi、連桿偏距di、連桿夾角θi四個(gè)參數(shù)來描述[6],連桿參數(shù)見表1。

表1 連桿參數(shù)

▲圖2 機(jī)械臂連桿坐標(biāo)系

在Matlab軟件機(jī)器人工具箱環(huán)境下建立機(jī)械臂數(shù)學(xué)模型,通過Denavit-Hartenberg坐標(biāo)參數(shù)構(gòu)建每個(gè)關(guān)節(jié)位置[7],再通過 Link和Robot 函數(shù)對(duì)連桿關(guān)節(jié)進(jìn)行連接組合,完成機(jī)械臂建模,如圖3所示。

▲圖3 機(jī)械臂建模

3.2 運(yùn)動(dòng)學(xué)方程正解問題

(1)

(2)

nx=sinθ1sinθ6+cos(θ2+θ3+θ4+θ5)cosθ1

cosθ6

(3)

ny=cos(θ2+θ3+θ4+θ5)cosθ6sinθ1-cosθ1

sinθ6

(4)

nz=sin(θ2+θ3+θ4+θ5)cosθ6

(5)

ox=sinθ1cosθ6-cos(θ2+θ3+θ4+θ5)cosθ1

sinθ6

(6)

oy=-cosθ1cosθ6-cos(θ2+θ3+θ4+θ5)sinθ1

sinθ6

(7)

oz=-sin(θ2+θ3+θ4+θ5)sinθ6

(8)

ax=sin(θ2+θ3+θ4+θ5)cosθ1

(9)

ay=sin(θ2+θ3+θ4+θ5)sinθ1

(10)

az=-cos(θ2+θ3+θ4+θ5)

(11)

px=cosθ1[d6sin(θ2+θ3+θ4+θ5)+a3cos(θ2+

θ3)+a2cosθ2+a4cos(θ2+θ3+θ4)]

(12)

py=sinθ1[d6sin(θ2+θ3+θ4+θ5)+a3cos(θ2+

θ3)+a2cosθ2+a4cos(θ2+θ3+θ4)]

(13)

pz=d1-d6cos(θ2+θ3+θ4+θ5)+a3sin(θ2+

θ3)+a2sinθ2+a4sin(θ2+θ3+θ4)

(14)

3.3 運(yùn)動(dòng)學(xué)方程逆解問題

運(yùn)動(dòng)學(xué)方程的逆解指根據(jù)機(jī)械臂末端執(zhí)行器的位姿變換矩陣,通過運(yùn)動(dòng)學(xué)方程來求解關(guān)節(jié)位置,即已知機(jī)械臂末端的位置姿態(tài),計(jì)算出機(jī)械臂對(duì)應(yīng)的全部關(guān)節(jié)位置變量。機(jī)械臂末端執(zhí)行器在空間運(yùn)動(dòng)的軌跡由工作任務(wù)確定,通過軌跡規(guī)劃,將軌跡點(diǎn)映射到機(jī)械臂關(guān)節(jié)空間中,進(jìn)而選擇最合理的逆解。目前,機(jī)械臂逆運(yùn)動(dòng)學(xué)求解的主流方法有解析法、迭代法、代數(shù)法、幾何法[11-13]。

除式(2)外,機(jī)械臂末端期望位姿矩陣還包括:

(15)

(16)

(17)

在式(2)的等號(hào)兩側(cè)分別乘上對(duì)應(yīng)的逆矩陣,分離變量,根據(jù)左右矩陣對(duì)應(yīng)元素相等求解各關(guān)節(jié)變量的值。求解過程涉及反三角函數(shù),運(yùn)動(dòng)學(xué)逆解存在多解性。由于機(jī)械臂的結(jié)構(gòu)限制,不是所有求出的關(guān)節(jié)角都能滿足要求,因此有必要在所有解中選擇最佳解,如時(shí)間最短、能耗最小、能夠避開障礙物等。

4 軌跡規(guī)劃

機(jī)械臂軌跡規(guī)劃時(shí),根據(jù)作業(yè)任務(wù)要求,計(jì)算出預(yù)期的運(yùn)動(dòng)軌跡[14]。規(guī)劃既可以在關(guān)節(jié)空間中進(jìn)行,也可以在直角空間中進(jìn)行,要求所規(guī)劃的軌跡必須連續(xù)、平滑,并且使整個(gè)機(jī)械臂本體運(yùn)動(dòng)平穩(wěn)。當(dāng)機(jī)械臂的末端軌跡根據(jù)工作任務(wù)有嚴(yán)格要求時(shí),要選擇在合適的空間坐標(biāo)系中進(jìn)行規(guī)劃。筆者在笛卡爾空間坐標(biāo)系中進(jìn)行規(guī)劃,機(jī)械臂對(duì)封頭零件焊縫進(jìn)行探傷檢測(cè)時(shí),要求機(jī)械臂末端在空間中進(jìn)行直線運(yùn)動(dòng),同時(shí)末端執(zhí)行器根據(jù)工作的要求姿態(tài)不變,需要機(jī)械臂各個(gè)關(guān)節(jié)協(xié)調(diào)配合。

以通過機(jī)械臂末端從任務(wù)空間中點(diǎn)A(12.607,0,6.144)直線運(yùn)動(dòng)到點(diǎn)B(7,-4,1)為例,根據(jù)目標(biāo)點(diǎn)利用Transl函數(shù)進(jìn)行兩次平移變換,得到機(jī)械臂初始位置和終止位置的齊次變換矩陣。用函數(shù)Ctraj(T0,T1,N)和Plot3()規(guī)劃出機(jī)械臂由點(diǎn)A運(yùn)動(dòng)到點(diǎn)B時(shí)的運(yùn)動(dòng)軌跡,給定仿真時(shí)間為10 s,時(shí)間間隔為0.1 s,總時(shí)間步數(shù)為102步。機(jī)械臂末端點(diǎn)空間位移運(yùn)動(dòng)軌跡如圖4所示,機(jī)械臂末端點(diǎn)方向位移運(yùn)動(dòng)軌跡如圖5所示,機(jī)械臂各關(guān)節(jié)角位移運(yùn)動(dòng)軌跡如圖6所示。得到機(jī)械臂六個(gè)關(guān)節(jié)轉(zhuǎn)角與時(shí)間的關(guān)系,即為聯(lián)合仿真軌跡跟蹤的期望曲線。

▲圖4 機(jī)械臂末端點(diǎn)空間位移運(yùn)動(dòng)軌跡

▲圖5 機(jī)械臂末端點(diǎn)方向位移運(yùn)動(dòng)軌跡

▲圖6 機(jī)械臂各關(guān)節(jié)角位移運(yùn)動(dòng)軌跡

5 軌跡控制

為有效對(duì)機(jī)械臂虛擬樣機(jī)進(jìn)行運(yùn)動(dòng)控制,以使機(jī)械臂能夠準(zhǔn)確按照作業(yè)任務(wù)運(yùn)動(dòng),設(shè)計(jì)機(jī)械臂運(yùn)動(dòng)學(xué)控制流程,如圖7所示。為了對(duì)各旋轉(zhuǎn)關(guān)節(jié)位置進(jìn)行精確控制,采用了比例積分微分位置控制策略。

▲圖7 機(jī)械臂運(yùn)動(dòng)學(xué)控制流程

在SolidWorks軟件中建立簡(jiǎn)化后的機(jī)械臂虛擬樣機(jī)模型,以IGS格式保存文件。通過Adams/View窗口導(dǎo)入文件,在Adams軟件中對(duì)導(dǎo)入的虛擬樣機(jī)按照實(shí)際樣機(jī)的屬性進(jìn)行編輯定義[15]。在機(jī)械臂各關(guān)節(jié)處添加旋轉(zhuǎn)副約束和底座固定約束,保證模型各部件之間正確的相對(duì)運(yùn)動(dòng)關(guān)系,完成的機(jī)械臂虛擬樣機(jī)模型如圖8所示。為了有效實(shí)現(xiàn)Adams軟件與Matlab軟件的實(shí)時(shí)數(shù)據(jù)交換,建立輸入輸出狀態(tài)變量。針對(duì)此機(jī)械臂,需建立關(guān)節(jié)控制力矩、關(guān)節(jié)角位移、機(jī)械臂末端點(diǎn)各方向位移的狀態(tài)量。其中,各關(guān)節(jié)控制力矩與機(jī)械系統(tǒng)的輸入變量關(guān)聯(lián),關(guān)節(jié)角位移和機(jī)械臂末端點(diǎn)各方向位移與輸出變量關(guān)聯(lián)。Matlab/Simulink控制程序與Adams軟件虛擬樣機(jī)模型相連,各個(gè)關(guān)節(jié)的實(shí)際關(guān)節(jié)角位置被實(shí)時(shí)反饋至閉環(huán)控制系統(tǒng),作用在機(jī)械臂關(guān)節(jié)力矩上,實(shí)現(xiàn)關(guān)節(jié)角位置的實(shí)時(shí)跟蹤。機(jī)械臂子系統(tǒng)模塊中的期望關(guān)節(jié)角變量及相應(yīng)的實(shí)際關(guān)節(jié)角變量見表2,表2中,angle1～angle5對(duì)應(yīng)表1中的變量θ1～θ5。

▲圖8 機(jī)械臂虛擬樣機(jī)模型

表2 機(jī)械臂子系統(tǒng)變量

機(jī)械臂聯(lián)合仿真控制系統(tǒng)通過機(jī)械臂的每個(gè)關(guān)節(jié)角位置誤差獲得驅(qū)動(dòng)力矩來實(shí)現(xiàn)控制。在Matlab/Simulink軟件中建立聯(lián)合仿真控制系統(tǒng)模型,如圖9所示。

▲圖9 聯(lián)合仿真控制系統(tǒng)模型

6 聯(lián)合仿真系統(tǒng)試驗(yàn)

聯(lián)合仿真系統(tǒng)搭建完成后,通過仿真調(diào)節(jié)比例積分微分控制器參數(shù),利用位置偏差驅(qū)動(dòng)機(jī)械臂各關(guān)節(jié)的位置控制,從而實(shí)現(xiàn)末端軌跡的準(zhǔn)確跟蹤。機(jī)械臂末端點(diǎn)各方向誤差曲線如圖10所示。機(jī)械臂關(guān)節(jié)角跟蹤曲線仿真結(jié)果如圖11所示。圖11中,關(guān)節(jié)角較迅速地定位到指定位置,跟蹤到給定軌跡,并能持續(xù)穩(wěn)定地跟蹤。通過Adams軟件后處理得到機(jī)械臂各關(guān)節(jié)所需要的關(guān)節(jié)驅(qū)動(dòng)力矩,如圖12所示。機(jī)械臂關(guān)節(jié)驅(qū)動(dòng)力矩平滑,數(shù)值合理。機(jī)械臂末端在Adams環(huán)境中運(yùn)動(dòng)軌跡如圖13所示。

▲圖10 機(jī)械臂末端點(diǎn)各方向誤差曲線

7 結(jié)束語

筆者針對(duì)機(jī)械臂軌跡規(guī)劃和運(yùn)動(dòng)控制問題,運(yùn)用虛擬仿真技術(shù),基于Adams軟件與Matlab軟件進(jìn)行聯(lián)合仿真,通過在Matlab軟件機(jī)器人工具箱中對(duì)機(jī)械臂進(jìn)行路徑規(guī)劃,遵循比例積分微分控制規(guī)律,得到機(jī)械臂運(yùn)動(dòng)特性,使機(jī)械臂末端運(yùn)動(dòng)軌跡得到精確控制。通過Adams環(huán)境,可以動(dòng)態(tài)觀看機(jī)械臂運(yùn)動(dòng)過程,并且獲得機(jī)械臂關(guān)節(jié)驅(qū)動(dòng)力矩和動(dòng)力參數(shù)。筆者的研究結(jié)果可以為類似機(jī)械臂的設(shè)計(jì)與制作提供理論參考。

▲圖11 機(jī)械臂關(guān)節(jié)角跟蹤曲線

▲圖12 機(jī)械臂各關(guān)節(jié)驅(qū)動(dòng)力矩

▲圖13 機(jī)械臂末端運(yùn)動(dòng)軌跡