[發(fā)明專(zhuān)利]一種基于NC代碼的工業(yè)機(jī)器人軌跡規(guī)劃方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201210001503.1
• 申請(qǐng)日: 2012-01-05
• 公開(kāi)（公告）號(hào): CN102581849A
• 公開(kāi)（公告）日: 2012-07-18
• 發(fā)明（設(shè)計(jì)）人: 王引;仇曉黎;劉明燈;惠國(guó)保;趙鵬
• 申請(qǐng)（專(zhuān)利權(quán)）人: 東南大學(xué)
• 主分類(lèi)號(hào): B25J9/16
• 分類(lèi)號(hào): B25J9/16
• 代理公司: 南京天翼專(zhuān)利代理有限責(zé)任公司 32112
• 代理人: 黃明哲

1.一種基于NC代碼的工業(yè)機(jī)器人軌跡規(guī)劃方法，工業(yè)機(jī)器人的末端工具為固接在機(jī)器人第六自由度的剛體，其特征是以通用NC代碼為工業(yè)機(jī)器人的工作語(yǔ)言，將工業(yè)機(jī)器人的末端工具作為虛擬刀具，在編程坐標(biāo)系下，設(shè)置五個(gè)點(diǎn)O、P、Q、S、T來(lái)描述虛擬刀具的基本特征，點(diǎn)O設(shè)置在編程坐標(biāo)系的原點(diǎn)，點(diǎn)P設(shè)置在編程坐標(biāo)系的Z軸上，其他三個(gè)點(diǎn)Q、S、T根據(jù)末端工具的具體特征來(lái)設(shè)置，其中，設(shè)置Q點(diǎn)要使得刀具安裝后Q點(diǎn)與機(jī)器人手腕中心重合，且直線QS與直線ST互相垂直；將每條NC代碼的數(shù)據(jù)通過(guò)虛擬刀具的五點(diǎn)由編程坐標(biāo)系變換到機(jī)器人坐標(biāo)系，進(jìn)行軌跡規(guī)劃，并根據(jù)所述變換到機(jī)器人坐標(biāo)系后的五個(gè)點(diǎn)得到機(jī)器人手腕中心點(diǎn)的位置和手腕姿態(tài)，從而獲得機(jī)器人的逆解參數(shù)，進(jìn)行機(jī)器人逆運(yùn)動(dòng)學(xué)求解。

2.根據(jù)權(quán)利要求1所述的一種基于NC代碼的工業(yè)機(jī)器人軌跡規(guī)劃方法，其特征是當(dāng)虛擬刀具的五點(diǎn)變換到機(jī)器人坐標(biāo)系后，利用三次五次插值法進(jìn)行軌跡規(guī)劃，所述三次五次插值法為：先取路徑上的四個(gè)點(diǎn)O、P、Q、S進(jìn)行三次插值獲得中間兩點(diǎn)P、Q的速度、加速度信息，再取P、Q、S、T四個(gè)點(diǎn)進(jìn)行三次插值獲得前兩點(diǎn)P、Q的速度、加速度信息，將兩次獲取的P、Q點(diǎn)的速度、加速度取平均值，作為P、Q點(diǎn)最終的速度和加速度，加上P、Q兩點(diǎn)的位移信息，共6個(gè)條件，插值一條五次曲線，以此保證控制點(diǎn)的位移以及控制點(diǎn)導(dǎo)數(shù)連續(xù)，即在平滑性的基礎(chǔ)上獲得精確的數(shù)據(jù)點(diǎn)。

3.根據(jù)權(quán)利要求1或2所述的一種基于NC代碼的工業(yè)機(jī)器人軌跡規(guī)劃方法，其特征是工業(yè)機(jī)器人為五軸機(jī)床時(shí)，包括以下步驟：

S1)獲得NC代碼：先設(shè)置后處理器后生產(chǎn)代碼，設(shè)轉(zhuǎn)動(dòng)軸為B、C軸，后處理器中設(shè)置第四軸、第五軸偏置均為0，其基本格式為：

N**G**X**Y**Z**B**C**F**S**

其中，N為行號(hào)，G為插補(bǔ)方式，X、Y、Z為編程坐標(biāo)系中，工件表面上一點(diǎn)的坐標(biāo)，B、C為B、C軸轉(zhuǎn)角，F(xiàn)為進(jìn)給速度，S為主軸轉(zhuǎn)速；

S2)將虛擬刀具的5個(gè)點(diǎn)分別繞Y軸和Z軸旋轉(zhuǎn)相應(yīng)的角度θ，式(1)是繞Y軸旋轉(zhuǎn)的旋轉(zhuǎn)矩陣，式(2)是某點(diǎn)(x，y，z)繞Y軸旋轉(zhuǎn)變換得到(x′，y′，z′)：

Rot(y,θ)=cθ0sθ010-sθ0cθ---(1)]]>

x′y′z′=Rot(y,θ)xyz---(2)]]>

其中，sθ＝sinθ，cθ＝cosθ；

S3)(x′，y′，z′)加上刀尖點(diǎn)，即工件表面點(diǎn)的位置坐標(biāo)(X，Y，Z)，變換后得到在編程坐標(biāo)系{Piece}下虛擬刀具的5個(gè)點(diǎn)坐標(biāo)(x_P，y_P，x_P)，如式(3)：

xPyPzP=x′y′z′+XYZ---(3);]]>

S4)經(jīng)過(guò)機(jī)器人對(duì)工件進(jìn)行標(biāo)定，得到編程坐標(biāo)系{Piece}和機(jī)器人坐標(biāo)系{Base}之間的變換關(guān)系，求出{Base}坐標(biāo)系下虛擬刀具的5個(gè)點(diǎn)坐標(biāo)(x_B，y_B，z_B)，如式(4)所示：

xByBzB=RPBxPyPzP+PP0B---(4)]]>

其中，是{Piece}坐標(biāo)系相對(duì)于{Base}坐標(biāo)系的旋轉(zhuǎn)矩陣，為{Piece}坐標(biāo)系原點(diǎn)在{Base}坐標(biāo)系中的描述；

S5)在{Base}坐標(biāo)系下，利用三次五次插值法進(jìn)行軌跡規(guī)劃：

CAM后處理器處理出來(lái)的NC代碼具有連續(xù)的數(shù)據(jù)點(diǎn)，取作業(yè)過(guò)程中連續(xù)4個(gè)數(shù)據(jù)通過(guò)三次插值法進(jìn)行軌跡插值，以相鄰點(diǎn)的弦長(zhǎng)l為橫坐標(biāo)l=(x2-x1)2+(y2-y1)2+(z2-z1)2,]]>分別以X(l)、Y(l)、Z(l)、B(l)、C(l)為縱坐標(biāo)進(jìn)行計(jì)算，

對(duì)于X(l)，三次曲線分別設(shè)為：

x₁(l)＝a₁*l³+b₁*l²+c₁*l+d₁

(5)

x₂(l)＝a₂*l³+b₂*l²+c₂*l+d₂

將4個(gè)插值點(diǎn)數(shù)據(jù)代入：

X(l₁)＝a₁*l₁³+b₁*l₁²+c₁*l₁+d₁

X(l₂)＝a₁*l₂³+b₁*l₂²+c₁*l₂+d₁(6)

X(l₃)＝a₁*l₃³+b₁*l₃²+c₁*l₃+d₁

X(l₄)＝a₁*l₄³+b₁*l₄²+c₁*l₄+d₁

4個(gè)方程4個(gè)未知數(shù)，則參數(shù)a₁、b₁、c₁、d₁可求，軟件編程時(shí)寫(xiě)成矩陣形式，求出參數(shù)a₁、b₁、c₁、d₁，代入原方程，則可求出P、Q兩點(diǎn)的一階導(dǎo)數(shù)、二階導(dǎo)數(shù)：

X′_P1(l₂)＝3a₁*l₂²+2b₁*l₂+c₁

X′_Q1(l₃)＝3a₁*l₃²+2b₁*l₃+c₁(7)

X″_P1(l₂)＝6a₁*l₂+2b₁

X″_Q1(l₃)＝6a₁*l₃+2b₁

同理可得，從x₂(l)獲得的P、Q兩點(diǎn)的速度加速度為：

X′_P2(l₂)＝3a₂*l₂²+2b₂*l₂+c₂

X′X₂(l₃)＝3a₂*l₃²+2b₂*l₃+C₂(8)

X″_P2(l₂)＝6a₂*l₂+2b₂

X″_Q2(l₃)＝6a₂*l₃+2b₂

取兩次P、Q的速度加速度的平均值作為P、Q兩點(diǎn)最終的速度和加速度，得：

X′(l₂)＝(X′_P1(l₂)+X′_P2(l₂))/2

X′(l₃)＝(X′_Q1(l₃)+X′_Q2(l₃))/2(9)

X″(l₂)＝(X″_P1(l₂)+X″_P2(l₂))/2

X″(l₃)＝(X″_Q1(l₃)+X″_Q2(l₃))/2

至此，有了P、Q點(diǎn)共6個(gè)條件，設(shè)五次方程：

將P、Q點(diǎn)6個(gè)條件代入：

X′(l₂)＝f′_X(l₂)

X″(l₂)＝f″_X(l₂)(11)

X′(l₃)＝f′_X(l₃)

X″(l₃)＝f″_X(l₃)

6個(gè)方程6個(gè)未知數(shù)，同理可求出α、β、γ、μ、和ζ每個(gè)參數(shù)都是l₂l₃的表達(dá)式，軟件編程時(shí)寫(xiě)成矩陣形式求解；

至此，求出六個(gè)系數(shù)后可用等步長(zhǎng)法進(jìn)行插值進(jìn)算：

Y(l)、Z(l)、B(l)、C(l)為縱坐標(biāo)的計(jì)算方法與X(l)計(jì)算方法相同，全部計(jì)算結(jié)束后，NC代碼上的信息轉(zhuǎn)化到虛擬刀具上五個(gè)點(diǎn)在{Base}坐標(biāo)系下的坐標(biāo)；

S6)根據(jù){Base}坐標(biāo)系下虛擬刀具的5個(gè)點(diǎn)坐標(biāo)，求出機(jī)器人手腕中心Q點(diǎn)位姿：Q點(diǎn)坐標(biāo)就是手腕中心點(diǎn)的位置坐標(biāo)；Q點(diǎn)和S點(diǎn)確定了接近矢量S點(diǎn)和T點(diǎn)決定了方向矢量法線矢量從而三個(gè)矢量確定手腕中心點(diǎn)的姿態(tài)；

S7)根據(jù)S6)中的逆解參數(shù)，即S6)中的Q點(diǎn)坐標(biāo)和三個(gè)矢量進(jìn)行機(jī)器人逆運(yùn)動(dòng)學(xué)求解。