一種基于姿態(tài)控制的鞋底噴膠路徑的規(guī)劃方法
【專利摘要】本發(fā)明一種基于姿態(tài)控制的鞋底噴膠路徑的規(guī)劃方法，主要包括機(jī)器人、激光輪廓測(cè)量?jī)x、膠槍及工控機(jī)，通過(guò)激光輪廓測(cè)量?jī)x掃描鞋底，得到鞋底曲面的三維點(diǎn)云，數(shù)據(jù)處理后進(jìn)行噴膠路徑規(guī)劃，在工控機(jī)上生成控制程序?qū)霗C(jī)器人，根據(jù)不同的鞋型控制機(jī)器人末端膠槍的姿態(tài)，完成鞋底的噴膠工序，能夠使膠槍姿態(tài)跟隨鞋底輪廓擬合曲線的斜率進(jìn)行變化，可以根據(jù)不同的鞋型控制機(jī)器人的末端膠槍姿態(tài)，且由于本發(fā)明的噴膠路徑經(jīng)過(guò)平滑處理，能夠減少機(jī)器人電機(jī)換向的次數(shù)，提高機(jī)器人的運(yùn)行速度，從而提高噴膠效率，并延長(zhǎng)機(jī)器人的工作壽命。
【專利說(shuō)明】
一種基于姿態(tài)控制的鞋底噴膠路徑的規(guī)劃方法
技術(shù)領(lǐng)域
[0001 ]本發(fā)明設(shè)計(jì)一種基于姿態(tài)控制的鞋底噴膠路徑的規(guī)劃方法。
【背景技術(shù)】
[0002] 在機(jī)器人鞋底噴膠的工序中，由于鞋底為各種型號(hào)的不規(guī)則曲面，其噴膠路徑需 要根據(jù)不同的型號(hào)的鞋底進(jìn)行自適應(yīng)規(guī)劃。目前的鞋底噴膠路徑的規(guī)劃，主要是根據(jù)鞋底 輪廓獲取的，由于鞋底輪廓的不規(guī)則性，鞋底噴膠路徑存在不平滑的缺點(diǎn)，對(duì)噴膠的連續(xù)性 造成影響；另外，噴膠路徑規(guī)劃中缺少對(duì)機(jī)器人末端的膠槍進(jìn)行姿態(tài)控制，則機(jī)器人末端的 膠槍的不適當(dāng)姿態(tài)會(huì)造成噴膠不均勻及不完整。

【發(fā)明內(nèi)容】

[0003] 本發(fā)明的目的在于提供一種基于姿態(tài)控制的鞋底噴膠路徑的規(guī)劃方法，能根據(jù)各 種不同的鞋型，對(duì)鞋底噴膠路徑進(jìn)行規(guī)劃，控制機(jī)器人末端的膠槍姿態(tài)，使其跟隨鞋底輪廓 擬合曲線的斜率進(jìn)行變化，快速、完整地完成機(jī)器人鞋底噴膠。
[0004] 本發(fā)明一種基于姿態(tài)控制的鞋底噴膠路徑的規(guī)劃方法，具體包括如下步驟：
[0005] 步驟1、通過(guò)激光輪廓測(cè)量?jī)x掃描得到鞋底曲面信息，根據(jù)曲面信息提取鞋底輪廓 曲線，對(duì)鞋底輪廓曲線進(jìn)行偏置規(guī)劃，生成噴膠軌跡線：
[0006] 步驟2、對(duì)步驟1得到的噴膠軌跡曲線采用分段曲線擬合法進(jìn)行平滑處理，即在噴 膠軌跡曲線的每段區(qū)間上進(jìn)行局部最小二乘法擬合：
[0007] 步驟3、根據(jù)噴膠軌跡曲線的斜率控制機(jī)器人末端膠槍的姿態(tài)，結(jié)合機(jī)器人噴膠時(shí) 的運(yùn)動(dòng)模型，通過(guò)機(jī)器人逆運(yùn)動(dòng)學(xué)逆解得到機(jī)器人在噴膠過(guò)程中每個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)的序列角 度；
[0008] 步驟4、依據(jù)步驟3得到的機(jī)器人在噴膠過(guò)程中每個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)的序列角度，在工控 機(jī)上生成控制程序?qū)霗C(jī)器人，根據(jù)不同的鞋型控制機(jī)器人的末端膠槍姿態(tài)，完成鞋底的 噴膠工序。
[0009] 該步驟1具體為:通過(guò)激光輪廓測(cè)量?jī)x掃描得到由一組三維點(diǎn)云數(shù)據(jù)構(gòu)成的鞋底 曲面信息，將該鞋底三維點(diǎn)云數(shù)據(jù)投影到X0Y平面上，提取鞋底最外圈輪廓曲線，該X0Y平面 上的鞋底輪廓曲線以參數(shù)形式表示為Hu) = (X(u)，y(u))，其參數(shù)形式的偏置曲線為： r= ?+邀^其中，d是偏置的距離，N(u)為單位法向量，對(duì)輪廓曲線r (u)上的每個(gè) 數(shù)據(jù)點(diǎn)及兩個(gè)相鄰點(diǎn)進(jìn)行分段處理，近似求得輪廓曲線r(u)上每個(gè)數(shù)據(jù)點(diǎn)對(duì)應(yīng)的法線，設(shè) 置偏置的距離d，由式(1)得到偏置的噴膠軌跡曲線。
[0010] 該步驟3具體為：建立機(jī)器人各個(gè)連桿坐標(biāo)系，得到機(jī)器人連桿參數(shù)，建立坐標(biāo)系 {i}相對(duì)于坐標(biāo)系{i-1}的變換，從而得到機(jī)器人末端膠槍相對(duì)于基礎(chǔ)坐標(biāo)系的變換矩陣 丁6,0: _N 0 A P~~
[0011] ^ = a 〇 a J (2)
[0012] 式中0為規(guī)定安裝有膠槍的法蘭的自身方向的矢量，A為所述法蘭接近工件的矢 量，N為由方向矢量0和接近矢量A并結(jié)合右手法則得到的機(jī)器人末端所述法蘭的法線矢量， P為所述法蘭中心點(diǎn)的位置矢量，則機(jī)器人變換矩陣表示為： PX _3] r60 = n s 八⑶ '% P： 0 0 0 1
[0014]在鞋底噴膠的過(guò)程中，使膠槍在鞋底的投影始終垂直于噴膠軌跡曲線，結(jié)合擬合 后的噴膠軌跡曲線的方程，確定機(jī)器人變換矩陣T6,o,根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)方程逆解得到式 (4 )，相應(yīng)得到機(jī)器人關(guān)節(jié)1到關(guān)節(jié)6轉(zhuǎn)動(dòng)的角度0 :至06:
[0017] S6 = -nx(ciC23S4-SlC4)-ny(siC23S4+ClC4)-nzS23S4,
[0018] C6 = nx[ (ciC23C4+SlS4)c5-ClS23S5]+ny[ (siC23C4-ClS4)c5-SlS23S5]axClS23-nz(s23C4C5+ C23S5)
[0019] 上述Sj為sin9j的簡(jiǎn)寫，cj為cost的簡(jiǎn)寫，j = 1、2,…，6，S23為sin(02+03)的簡(jiǎn)寫， C23為COS (02+03)的簡(jiǎn)寫。
[0020] 本發(fā)明能夠使膠槍姿態(tài)跟隨鞋底輪廓擬合曲線的斜率進(jìn)行變化，可以根據(jù)不同的 鞋型控制機(jī)器人的末端膠槍姿態(tài)，且由于本發(fā)明的噴膠路徑經(jīng)過(guò)平滑處理，能夠減少機(jī)器 人電機(jī)換向的次數(shù)，提高機(jī)器人的運(yùn)行速度，從而提高噴膠效率，并延長(zhǎng)機(jī)器人的工作壽 命。
【附圖說(shuō)明】
[0021 ]圖1為本發(fā)明的流程框圖；
[0022]圖2為本發(fā)明中安川Motoman-MH5F機(jī)器人連桿坐標(biāo)系示意圖；
[0023]圖3為本發(fā)明中機(jī)器人連桿參數(shù)；
[0024]圖4為實(shí)施例中激光輪廓測(cè)量?jī)x掃描得到鞋底三維點(diǎn)云數(shù)據(jù)；
[0025] 圖5為實(shí)施例中提取的鞋底的外圈輪廓曲線；
[0026] 圖6為實(shí)施例中的鞋底噴膠軌跡曲線；
[0027] 圖7為實(shí)施例中的平滑處理后的鞋底噴膠軌跡曲線；
[0028] 圖8為實(shí)施例中機(jī)器人各關(guān)節(jié)轉(zhuǎn)動(dòng)的角度。
[0029]以下結(jié)合附圖和實(shí)施例對(duì)本發(fā)明作進(jìn)一步詳述。
【具體實(shí)施方式】
[0030]本發(fā)明一種基于姿態(tài)控制的鞋底噴膠路徑的規(guī)劃方法，主要包括機(jī)器人、激光輪 廓測(cè)量?jī)x、膠槍及工控機(jī)，通過(guò)激光輪廓測(cè)量?jī)x掃描鞋底，得到鞋底曲面的三維點(diǎn)云，數(shù)據(jù) 處理后進(jìn)行噴膠路徑規(guī)劃，在工控機(jī)上生成控制程序?qū)霗C(jī)器人，根據(jù)不同的鞋型控制機(jī) 器人末端膠槍的姿態(tài)，完成鞋底的噴膠工序，如圖1所示，具體包括如下步驟：
[0031]步驟1、通過(guò)激光輪廓測(cè)量?jī)x掃描得到鞋底曲面信息，根據(jù)曲面信息提取鞋底輪廓 曲線，對(duì)鞋底輪廓曲線進(jìn)行偏置規(guī)劃，生成噴膠軌跡線：
[0032]通過(guò)激光輪廓測(cè)量?jī)x掃描得到的鞋底曲面信息，是由一組三維點(diǎn)云構(gòu)成的數(shù)據(jù)， 將該鞋底三維點(diǎn)云數(shù)據(jù)投影到X0Y平面上，提取鞋底最外圈輪廓曲線，該X0Y平面上的鞋底 輪廓曲線以參數(shù)形式表示為r(u) = (x(u)，y(u))，其參數(shù)形式的偏置曲線為： r(L) = r(m) +祕(mì)⑷(其中，d是偏置的距離，N(u)為單位法向量，對(duì)輪廓曲線r(u)上的每個(gè) 數(shù)據(jù)點(diǎn)及兩個(gè)相鄰點(diǎn)進(jìn)行分段處理，近似求得輪廓曲線r(u)上每個(gè)數(shù)據(jù)點(diǎn)對(duì)應(yīng)的法線，設(shè) 置偏置的距離d，由式(1)得到偏置的噴膠軌跡曲線；
[0033]步驟2、對(duì)步驟1得到的噴膠軌跡曲線采用分段曲線擬合法進(jìn)行平滑處理，即在噴 膠軌跡曲線的每段區(qū)間上進(jìn)行局部最小二乘法擬合：
[0034] 根據(jù)鞋底外圈輪廓曲線偏置得到的噴膠軌跡曲線斜率變化劇烈，這會(huì)導(dǎo)致機(jī)器人 噴膠過(guò)程中需要?jiǎng)×易儞Q電機(jī)的轉(zhuǎn)向，會(huì)降低機(jī)器人的運(yùn)行速度，并對(duì)機(jī)器人的壽命產(chǎn)生 影響，本發(fā)明采用最小二乘法多項(xiàng)式擬合的方法對(duì)噴膠軌跡曲線進(jìn)行平滑處理，由于鞋底 噴膠軌跡曲線形狀不規(guī)則且數(shù)據(jù)點(diǎn)較多，擬合的多項(xiàng)式階數(shù)太低，擬合精度和效果不太理 想。要提高擬合精度和效果就需要提高曲線階數(shù)，但階數(shù)太高又帶來(lái)計(jì)算上的復(fù)雜性及其 他方面的不利。因此，采用分段曲線擬合，在每段區(qū)間上進(jìn)行局部最小二乘法擬合。
[0035] 步驟3、根據(jù)噴膠軌跡曲線的斜率控制機(jī)器人末端膠槍的姿態(tài)，結(jié)合機(jī)器人噴膠時(shí) 的運(yùn)動(dòng)模型(姿態(tài)），通過(guò)機(jī)器人逆運(yùn)動(dòng)學(xué)逆解得到機(jī)器人在噴膠過(guò)程中每個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)的序 列角度；
[0036] 在鞋底噴膠過(guò)程中，機(jī)器人末端膠槍的姿態(tài)只有跟隨噴膠軌跡曲線斜率的改變而 改變，才能保證鞋底的完整均勻噴膠。本發(fā)明中的膠槍安裝在6自由度的工業(yè)機(jī)器人上，以 安川Motoman-MH5F機(jī)械臂為例闡述膠槍姿態(tài)的控制方法，如圖2建立機(jī)器人各個(gè)連桿坐標(biāo) 系，機(jī)器人連桿參數(shù)如圖3所示。安川Motoman-MH5F機(jī)械臂有6個(gè)自由度，關(guān)節(jié)1繞基礎(chǔ)坐標(biāo) 系{〇}中的Z軸轉(zhuǎn)動(dòng)，關(guān)節(jié)2、3、5繞X軸轉(zhuǎn)動(dòng)，關(guān)節(jié)4、6繞Y軸轉(zhuǎn)動(dòng)。
[0037] 建立坐標(biāo)系{i}相對(duì)于坐標(biāo)系{i-1}的變換，從而得到機(jī)器人末端膠槍相對(duì)于基礎(chǔ) 坐標(biāo)系的變換矩陣Tm: ......... ~N O A P~
[0038] 〇 〇 〇 (2) _ --
[0039] 式中0為規(guī)定安裝有膠槍的法蘭的自身方向的矢量，A為所述法蘭接近工件的矢 量，N為由方向矢量0和接近矢量A并結(jié)合右手法則得到的機(jī)器人末端所述法蘭的法線矢量， P為所述法蘭中心點(diǎn)的位置矢量，則機(jī)器人變換矩陣表示為： Pr
[0040] 忍0= m八⑶ 'th 〇, P： 0 0 Q 1
[0041]在鞋底噴膠的過(guò)程中，使膠槍在鞋底的投影始終垂直于噴膠軌跡曲線，結(jié)合擬合 后的噴膠軌跡曲線的方程，確定機(jī)器人變換矩陣T6,o,根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)方程逆解得到式 (4)，相應(yīng)得到機(jī)器人關(guān)節(jié)1到關(guān)節(jié)6轉(zhuǎn)動(dòng)的角度0 1至06:
[0044] S6 = -nx(ciC23S4-SlC4)-ny(siC23S4+ClC4)-nzS23S4 ?
[0045] C6 = nx[ (ciC23C4+SlS4)c5-ClS23S5]+ny[ (siC23C4-ClS4)c5-SlS23S5]axClS23-nz(s23C4C5+ C23S5)
[0046] 上述sj為sinQj的簡(jiǎn)寫，cj為cosQj的簡(jiǎn)寫，j = 1、2,…，6，S23為sin(Q2+03)的簡(jiǎn)寫， C23為COS (02+03)的簡(jiǎn)寫；
[0047] 步驟4、依據(jù)步驟3得到的機(jī)器人在噴膠過(guò)程中每個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)的序列角度，在工控 機(jī)上生成控制程序?qū)霗C(jī)器人，根據(jù)不同的鞋型控制機(jī)器人的末端膠槍姿態(tài)，完成鞋底的 噴膠工序。
[0048] 實(shí)施例
[0049]激光輪廓測(cè)量?jī)x掃描得到鞋底三維點(diǎn)云數(shù)據(jù)，見(jiàn)圖4,并對(duì)三維點(diǎn)云數(shù)據(jù)進(jìn)行處 理，提取出鞋底的外圈輪廓曲線，如圖5所示，以鞋底的外圈輪廓曲線為基礎(chǔ)進(jìn)行偏置規(guī)劃， 根據(jù)膠槍的噴膠量及鞋底的輪廓寬度，設(shè)置偏置的距離為d = 5mm(向內(nèi)法線偏置），偏置次 數(shù)為4次，由式（1)得到的鞋底噴膠軌跡曲線，如圖6所示;采用二階多項(xiàng)式擬合的方法對(duì)鞋 底噴膠軌跡曲線進(jìn)行平滑處理，分別以4圈噴膠軌跡為對(duì)象，將每圈軌跡曲線近似十等分， 擬合后得到的噴膠軌跡曲線如圖7所示，噴膠軌跡曲線相對(duì)于擬合前的曲線，曲線斜率變化 平緩。為了判斷擬合程度的好壞，計(jì)算擬合數(shù)據(jù)和原始數(shù)據(jù)對(duì)應(yīng)點(diǎn)的均方根誤差：
(5)'式中f(Xl)為擬合數(shù)據(jù)， yi為原始數(shù)據(jù)，根據(jù)式(5)計(jì)算得到的均 方根誤差RMSE = 0.0049,說(shuō)明曲線的擬合程度較好。取鞋底最外圈噴膠軌跡，控制機(jī)器人的 末端膠槍沿著噴膠軌跡運(yùn)動(dòng)。在擬合軌跡曲線的基礎(chǔ)上，計(jì)算得到曲線的斜率，結(jié)合機(jī)器人 空間坐標(biāo)變換求得機(jī)器人位姿矩陣T6, Q，代入式(4)中，逆解得到機(jī)器人關(guān)節(jié)1到關(guān)節(jié)6轉(zhuǎn)過(guò) 的角度以至以，如圖8所示，從圖8可以看出機(jī)器人六個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)角度呈現(xiàn)連續(xù)性變化，避免 機(jī)器人電機(jī)頻繁換向，實(shí)現(xiàn)快速噴膠，保證機(jī)器人的工作壽命。
[0050]以上所述，僅是本發(fā)明較佳實(shí)施例而已，并非對(duì)本發(fā)明的技術(shù)范圍作任何限制，故 凡是依據(jù)本發(fā)明的技術(shù)實(shí)質(zhì)對(duì)以上實(shí)施例所作的任何細(xì)微修改、等同變化與修飾，均仍屬 于本發(fā)明技術(shù)方案的范圍內(nèi)。
【主權(quán)項(xiàng)】
1. 一種基于姿態(tài)控制的鞋底噴膠路徑的規(guī)劃方法，其特征在于包括如下步驟： 步驟1、通過(guò)激光輪廓測(cè)量?jī)x掃描得到鞋底曲面信息，根據(jù)曲面信息提取鞋底輪廓曲 線，對(duì)鞋底輪廓曲線進(jìn)行偏置規(guī)劃，生成噴膠軌跡線： 步驟2、對(duì)步驟1得到的噴膠軌跡曲線采用分段曲線擬合法進(jìn)行平滑處理，即在噴膠軌 跡曲線的每段區(qū)間上進(jìn)行局部最小二乘法擬合： 步驟3、根據(jù)噴膠軌跡曲線的斜率控制機(jī)器人末端膠槍的姿態(tài)，結(jié)合機(jī)器人噴膠時(shí)的運(yùn) 動(dòng)模型，通過(guò)機(jī)器人逆運(yùn)動(dòng)學(xué)逆解得到機(jī)器人在噴膠過(guò)程中每個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)的序列角度； 步驟4、依據(jù)步驟3得到的機(jī)器人在噴膠過(guò)程中每個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)的序列角度，在工控機(jī)上 生成控制程序?qū)霗C(jī)器人，根據(jù)不同的鞋型控制機(jī)器人的末端膠槍姿態(tài)，完成鞋底的噴膠 工序。2. 根據(jù)權(quán)利要求1所述的一種基于姿態(tài)控制的鞋底噴膠路徑的規(guī)劃方法，其特征在于 該步驟1具體為:通過(guò)激光輪廓測(cè)量?jī)x掃描得到由一組三維點(diǎn)云數(shù)據(jù)構(gòu)成的鞋底曲面信息， 將該鞋底三維點(diǎn)云數(shù)據(jù)投影到XOY平面上，提取鞋底最外圈輪廓曲線，該XOY平面上的鞋底 輪廓曲線以參數(shù)形式表示為r (U) = (x(u)，y (U))，其參數(shù)形式的偏置曲線為： ，(》)=，:(《.)'+避何（1.)，其中，d是偏置的距尚，N(u)為單位法向量，對(duì)輪廓曲線r(u)上的每個(gè) 數(shù)據(jù)點(diǎn)及兩個(gè)相鄰點(diǎn)進(jìn)行分段處理，近似求得輪廓曲線r(u)上每個(gè)數(shù)據(jù)點(diǎn)對(duì)應(yīng)的法線，設(shè) 置偏置的距離d，由式(1)得到偏置的噴膠軌跡曲線。3. 根據(jù)權(quán)利要求1所述的一種基于姿態(tài)控制的鞋底噴膠路徑的規(guī)劃方法，其特征在于 該步驟3具體為： 建立機(jī)器人各個(gè)連桿坐標(biāo)系，得到機(jī)器人連桿參數(shù)，建立坐標(biāo)系{i}相對(duì)于坐標(biāo)系{i-1}的變換，從而得到機(jī)器人末端膠槍相對(duì)于基礎(chǔ)坐標(biāo)系的變換矩陣T 6. 〇:式中〇為規(guī)定安裝有膠槍的法蘭的自身方向的矢量，A為所述法蘭接近工件的矢量，N為 由方向矢量〇和接近矢量A并結(jié)合右手法則得到的機(jī)器人末端所述法蘭的法線矢量，P為所 述法蘭中心點(diǎn)的位置矢量，則機(jī)器人變換矩陣表示為：在鞋底噴膠的過(guò)程中，使膠槍在鞋底的投影始終垂直于噴膠軌跡曲線，結(jié)合擬合后的 噴膠軌跡曲線的方程，確定機(jī)器人變換矩陣T6,〇,根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)方程逆解得到式(4)，相 應(yīng)得到機(jī)器人關(guān)節(jié)1到關(guān)節(jié)6轉(zhuǎn)動(dòng)的角度0 1至06:S6 = -nx(ciC23S4-SlC4)-ny(siC23S4+ClC4)-nzS23S4 ? C6 = nx[ (ciC23C4+SlS4)c5-ClS23S5]+ny[ (siC23C4-ClS4)c5-SlS23S5]axClS23-nz(s23C4C5+C23S5) 上述Sj為sinQj的簡(jiǎn)寫，Cj為cosQj的簡(jiǎn)寫，j = I、2,…，6，S23為sin(Q2+Q3)的簡(jiǎn)寫，C23為 cos(Q2+93)的簡(jiǎn)寫 D
【文檔編號(hào)】G06F17/16GK105894120SQ201610217580
【公開日】2016年8月24日
【申請(qǐng)日】2016年4月8日
【發(fā)明人】李俊, 謝銀輝
【申請(qǐng)人】泉州裝備制造研究所