智能噴膠機器人的姿態(tài)控制與軌跡規(guī)劃

吳拱星

(黎明職業(yè)大學(xué) 智能制造工程學(xué)院， 福建 泉州 362000)

目前，我國制鞋業(yè)自動化水平較低，在噴膠流程上，只有少數(shù)企業(yè)的個別工序使用了自動化裝備，大部分工序仍依靠工人手工完成，生產(chǎn)效率較低。為促進制鞋業(yè)健康發(fā)展，急需研發(fā)自動化智能噴膠生產(chǎn)線。為此，本文針對智能噴膠機器人開發(fā)的關(guān)鍵技術(shù)，重點研究機器人的姿態(tài)控制與軌跡規(guī)劃。

在機器人焊接、噴涂、打磨拋光等工業(yè)領(lǐng)域，機械臂末端工具的姿態(tài)影響其作業(yè)結(jié)果。工業(yè)機器人的作業(yè)對象包含很多不規(guī)則的曲面，不規(guī)則曲面的路徑規(guī)劃對機器人的作業(yè)過程有很大的影響。在鞋底噴膠工序中，鞋底為各種型號的不規(guī)則曲面，機器人噴膠路徑需根據(jù)不同型號的鞋底進行自適應(yīng)規(guī)劃。因此，噴膠路徑規(guī)劃是整個工序的重點。目前空間機械臂的路徑規(guī)劃問題多集中于機械臂的避障規(guī)劃，而關(guān)于末端工具姿態(tài)控制的路徑規(guī)劃研究較少。KIM提出一種基于鞋底和鞋幫三維掃描數(shù)據(jù)來自動提取噴膠軌跡的方法，該方法操作復(fù)雜，很難有效地控制精度[1]。KWON等提出一種基于鞋底輪廓線生成噴膠軌跡的方法，它能根據(jù)給定的鞋底輪廓線自動生成噴膠軌跡，但提取的軌跡是平面曲線，通用性差，不能滿足個性化制鞋的要求[2]。武傳宇、WANG等根據(jù)線結(jié)構(gòu)光三維測量原理，獲取鞋幫底面的曲面信息，并算出了噴膠軌跡，但該方法并未提及噴膠姿態(tài)的控制問題[3-4]。

點云配準(zhǔn)是指通過掃描采樣獲得兩組有重復(fù)區(qū)域或形狀相近的點云數(shù)據(jù)，采用合適的坐標(biāo)變換使兩組點云數(shù)據(jù)盡量在空間上重合，從而合并在一個統(tǒng)一的坐標(biāo)系下。用于點云配準(zhǔn)的主要方法有最近點迭代法(Iterative Closest Point，ICP )、正態(tài)分布法(Normal Distributions Transform，NDT)、奇異矩陣法(Singular Value Decomposition，SVD)和極大似然法，其中ICP是最常用的點云配準(zhǔn)方法[5-6]。實際上，每種方法都有其局限性，比如ICP受限于其初始位置，極大似然法則受限于噪聲點等，于是針對不同應(yīng)用場景產(chǎn)生了多種改進算法。本文以工業(yè)六軸機械臂為研究對象，結(jié)合鞋底噴膠的應(yīng)用，對鞋底噴膠路徑進行規(guī)劃，控制機器人末端工具的姿態(tài)，實現(xiàn)機器人智能噴膠；通過機器視覺與點云配準(zhǔn)技術(shù)實現(xiàn)鞋底、鞋面噴膠位置的自動計算。傳統(tǒng)的配準(zhǔn)對象都是高度重復(fù)的一對點云，配準(zhǔn)相對容易，而本文匹配的點云對是通過線激光傳感器掃描得到的鞋底和鞋面兩組點云數(shù)據(jù)，點云對之間無完全重疊的部分，只是形狀相似，并且點云對的初始位置是可翻轉(zhuǎn)的，難以直接通過ICP進行精確配準(zhǔn)。目前這種配準(zhǔn)對象的配準(zhǔn)方法相對罕見。為了使點云對盡可能重疊匹配，本文先采用SVD快速得到點云對的粗匹配結(jié)果，再利用ICP精確匹配點云對，使得點云對統(tǒng)一到同一坐標(biāo)系下；同時，針對實際生產(chǎn)中存在的特殊情況，擬利用Bezier曲線擬合方法對鞋底點云的輪廓線進行局部變形，以期順利實現(xiàn)智能噴膠，達(dá)到理想的噴膠效果。

1 智能噴膠機器人的控制流程

機器人智能噴膠系統(tǒng)如圖1所示。它主要包括機器人、線激光傳感器、膠槍及工控機等。智能噴膠機器人能夠根據(jù)不同的鞋型進行自適應(yīng)控制，完成鞋面和鞋底的噴膠任務(wù)，具體包括鞋底噴膠、鞋底鞋面匹配噴膠。其控制流程如圖2所示。系統(tǒng)通過線激光傳感器掃描鞋底，得到鞋底曲面，對數(shù)據(jù)處理后進行鞋底噴膠路徑規(guī)劃；在完成鞋底路徑規(guī)劃的條件下，實現(xiàn)鞋底鞋面的點云數(shù)據(jù)匹配，從而在工控機上生成控制程序，導(dǎo)入機器人，控制機械臂完成噴膠任務(wù)。

圖1 機器人智能噴膠系統(tǒng)

圖2 智能噴膠機器人的控制流程

2 鞋底噴膠的軌跡規(guī)劃

由線激光傳感器得到的鞋底曲面信息是由一組三維點云數(shù)據(jù)構(gòu)成的。在鞋底噴膠軌跡規(guī)劃中，將鞋底三維點云數(shù)據(jù)投射到XOY平面上，提取鞋底最外圈輪廓，并對輪廓曲線進行偏置處理，生成噴膠軌跡曲線。XOY平面的鞋底輪廓曲線為：

r(u)=(x(u),y(u))

(1)

偏置處理后曲線為：

(2)

式中：d為偏置(d>0，表示沿著曲線r(u)的外法線方向移動d個單位；d<0 ，表示沿著曲線r(u)的內(nèi)法線方向移動d個單位)；N(u) 為單位法向量，即

(3)

設(shè)置偏置的距離，對輪廓曲線上的每個數(shù)據(jù)點及兩個相鄰點進行分段處理，可近似求得曲線上每個點對應(yīng)的法線。根據(jù)式(2)和式(3)可得對應(yīng)偏置的鞋底噴膠軌跡。

3 機器人關(guān)節(jié)的運動學(xué)逆解

鞋底是不規(guī)則的曲面，鞋底的噴膠作業(yè)若采用普通的路徑規(guī)劃方法，則可能產(chǎn)生噴膠不完整、不均勻的現(xiàn)象。在鞋底噴膠中，使末端工具的姿態(tài)跟隨噴膠軌跡曲線斜率的改變而改變，能夠保證鞋底的完整均勻噴膠。這就需要根據(jù)機器人噴膠時的姿態(tài)對機器人關(guān)節(jié)進行控制，通過運動學(xué)逆解得到機器人關(guān)節(jié)轉(zhuǎn)動的角度。

作為本文研究對象的工業(yè)機器人為安川Motoman-MH5F機械臂。其連桿坐標(biāo)系如圖3所示。機器人以六自由度連桿相串聯(lián)。具體來說，關(guān)節(jié)1繞基礎(chǔ)坐標(biāo)系{0}的Z軸轉(zhuǎn)動，關(guān)節(jié)2、3、5繞X軸轉(zhuǎn)動，關(guān)節(jié)4、6繞Y軸轉(zhuǎn)動。

圖3 安川Motoman-MH5F機器臂的連桿坐標(biāo)系

根據(jù)鞋底噴膠軌跡曲線及膠槍的姿態(tài)，結(jié)合機器人運動學(xué)方程逆解確定變換矩陣。該變換矩陣為：

(4)

相應(yīng)地，可得到機器人關(guān)節(jié)1到關(guān)節(jié)6轉(zhuǎn)過的角度θ1至θ6。

采用二階多項式擬合方法，對鞋底噴膠軌跡曲線進行平滑處理，先后以4圈噴膠軌跡為對象，將每圈軌跡曲線近似十等分，得到的噴膠軌跡曲線如圖4所示。

圖4 平滑處理后的噴膠軌跡曲線

4 鞋底鞋面的匹配算法

運用ICP匹配點云的關(guān)鍵是點云對的位置初始化。當(dāng)點云對的初始位置相差較大時，匹配效果往往較差[7]。在鞋面噴膠需要匹配的原始點云對中，鞋底是17 027×3(像素)的雙精度數(shù)據(jù)類型的矩陣，鞋面是18 173×3(像素)的雙精度數(shù)據(jù)類型的矩陣。原始點云對如圖5所示。

圖5 原始點云對

運用ICP實現(xiàn)精確配準(zhǔn)前需要對點云對的位置進行初始化。利用SVD可以消除點云對的旋轉(zhuǎn)錯位和平移錯位，初步求出點云對的旋轉(zhuǎn)矩陣和平移向量，完成位置初始化，得到粗匹配結(jié)果。

設(shè)A∈Rm×n(矩陣R包含m行、n列)，ATA的特征值的非負(fù)平方根稱作A的奇異值；A的奇異值的全體記作σ(A)。當(dāng)A為復(fù)矩陣Cm×n(矩陣C包含m行、n列)時，只需將ATA改為AHA，上述定義即仍然成立。根據(jù)奇異值分解定理，設(shè)A∈Rm×n，則必存在正交矩陣U=[u1,…,um]∈Rm×m和V=[v1,…,vn]∈Rn×n，使得：

(5)

式中，∑r=diag(σ1，σr)(這里，σ1≥σ2≥…≥σr>0)，為矩陣A的全部非零奇異值。

在現(xiàn)代科學(xué)計算中，SVD的應(yīng)用廣泛，如用于確定投影算子。假設(shè)矩陣A∈Rm×m的秩為r，并將式(5)改寫為：

A=U∑VT

(6)

粗匹配的旋轉(zhuǎn)矩陣為：

(7)

粗匹配的平移向量為：

T=[-84,823 9 421.686 6 109.538 3]

(8)

點云對粗匹配的結(jié)果如圖6所示。

圖6 點云對粗匹配結(jié)果

5 ICP精確匹配

點云對的SVD配準(zhǔn)只是實現(xiàn)點云的大致匹配，并未達(dá)到精確配準(zhǔn)的要求，但它完成了點云的位置初始化工作。因此，對點云對進行SVD配準(zhǔn)后，可進一步利用ICP對點云進行精確配準(zhǔn)。ICP是一種基于自由形態(tài)曲面的配準(zhǔn)方法。其基本思想是：首先根據(jù)原始模型上的點集，在目標(biāo)模型上找到對應(yīng)的點集，即找到初始匹配點，以對應(yīng)點間距離的平方和最小為原則，建立目標(biāo)方程；然后，根據(jù)最小二乘原理求取轉(zhuǎn)換參數(shù)，即可完成對應(yīng)點的精確匹配。其數(shù)學(xué)表示為：已知兩個對應(yīng)點集合X′={x1,x2，…，xn}和P′={p1,p2，…，pn}，求解旋轉(zhuǎn)矩陣R′和平移向量T′，使得目標(biāo)函數(shù)的值最小。

(9)

利用ICP進行15次迭代，得到的精確匹配旋轉(zhuǎn)矩陣為：

(10)

精確匹配的平移向量為：

T′=[-2.789 9 14.451 4 -60.955 5]T

(11)

采用ICP精確匹配的結(jié)果如圖7所示。

圖7 采用ICP精確匹配的結(jié)果

通過模擬及實際測試可知，本文采用的機器人姿態(tài)控制與軌跡規(guī)劃方法能夠滿足制鞋噴膠自動化的要求。

6 結(jié)語

以工業(yè)六軸機械臂為研究對象，對其鞋底、鞋面噴膠路徑進行規(guī)劃，控制機器人末端工具的姿態(tài)，實現(xiàn)了機器人智能噴膠。通過線激光傳感器掃描得到鞋底的曲面信息，提取鞋底輪廓曲線，對其進行偏置處理，生成鞋底的噴膠軌跡，并根據(jù)鞋底的輪廓曲線對噴膠軌跡進行修正，達(dá)到了平滑曲線的效果。此外，針對鞋底鞋面擬合噴膠問題，首先采用SVD進行粗匹配，使得點云對方向一致且相互靠近，然后通過ICP實現(xiàn)了精確匹配。通過模擬及實際測試可知，本文采用的機器人姿態(tài)控制與軌跡規(guī)劃方法能夠滿足制鞋噴膠自動化的要求。