基于五次B樣條插值的汽車輪轂打磨機器人協(xié)作控制

胡文娟

(商洛職業(yè)技術(shù)學(xué)院 機電工程學(xué)院，商洛 726000)

0 引 言

機器人作為新型的生產(chǎn)工具，隨著其技術(shù)的發(fā)展被廣泛應(yīng)用于各個領(lǐng)域，在生產(chǎn)的各個環(huán)節(jié)中扮演著重要的角色。雙臂協(xié)作機器人掃除了傳統(tǒng)機器人部署成本高等障礙，為中小型企業(yè)實現(xiàn)智能制造、提高生產(chǎn)效率和安全性帶來希望。然而，在雙臂協(xié)作機器人的應(yīng)用中，其運動控制是當下機器人研究的重點和難點。

近年來，隨著機器人研究的深入，針對機器人運動軌跡的控制方法相繼提出。文獻[1]提出了一種基于合成運動的圓弧軌跡規(guī)劃方法，該方法利用圓弧軌跡和機器人在直角坐標空間中的運動軌跡與運動狀態(tài)方程，基于合成運動的圓弧軌跡規(guī)劃方法對Delta機器人進行軌跡規(guī)劃。文獻[2]以工業(yè)生產(chǎn)中常用的六自由度串聯(lián)機器人PUMA560為研究對象，在關(guān)節(jié)空間采用改進B樣條曲線進行軌跡擬合規(guī)劃，實現(xiàn)了對機器人各關(guān)節(jié)時間最優(yōu)的運動軌跡規(guī)劃，提高了機器人的運行效率。文獻[3]基于B樣條和改進遺傳算法，獲取了機器人時間最優(yōu)軌跡規(guī)劃方案，有效縮短了軌跡運行時間。文獻[4]采用五次多項式插值對AUBO-i5協(xié)作機器人進行軌跡規(guī)劃，有效減少了協(xié)作機器人結(jié)構(gòu)剛性差而造成的機械臂穩(wěn)定性問題，實現(xiàn)了AUBO-i5協(xié)作機器人的軌跡規(guī)劃。以上研究在機器人軌跡規(guī)劃已取得一定的成果，但主要停留在單一機器人軌跡規(guī)劃上，少部分研究對雙臂協(xié)作機器人的軌跡規(guī)劃進行研究，或是對某一特定協(xié)作機器人軌跡進行規(guī)劃，缺乏通用協(xié)作機器人軌跡規(guī)劃問題的研究。

本文參考上述的機器人軌跡規(guī)劃研究成果，基于五次B樣條插值方法，以汽車輪轂打磨的雙機器人協(xié)作問題為研究對象，對雙機器人的協(xié)作軌跡進行規(guī)劃，從而為提供軌跡控制精度。而本研究的貢獻在于重點比較了五次多項式插值、五次B樣條插值和七次B樣條插值在雙機器人局部協(xié)作中應(yīng)用的優(yōu)劣，為雙機器人甚至更多機器人局部協(xié)作提供了一種新的參考。

1 機器人運動學(xué)分析

1.1 運動學(xué)模型構(gòu)建

雙機器人協(xié)作打磨選用國產(chǎn)ER20-C10工業(yè)機器人為研究對象，并采用D-H法建立機器人運動學(xué)模型，其中ER20-C10工業(yè)機器人連桿與關(guān)節(jié)幾何參數(shù)如表1所示[5]。表1中，αi=1,L、αi=1,R為機器人左右連桿扭角，ai為zi到zi+1沿xi的距離，αi為zi到zi+1沿xi旋轉(zhuǎn)的角度；di為xi-1到xi沿zi的距離；θi為xi-1到xi沿zi旋轉(zhuǎn)的角度；(x0,y0,z0)為世界坐標系；(xL0,yL0,zL0)、(xR0,yR0,zR0)為左右兩個機器人的基礎(chǔ)坐標系，兩個機器人的距離為2.4 m。

表1 ER20-C10工業(yè)機器人連桿與關(guān)節(jié)幾何參數(shù)

坐標原點Oi在ai公垂線與關(guān)節(jié)Ji軸線交點上，Zi軸與關(guān)節(jié)Ji軸重合，方向任意；Xi軸與ai重合，方向為指向Ji+1軸，Yi根據(jù)右手定則確定，則構(gòu)建雙臂協(xié)作機器人的坐標系,如圖1所示。

圖1 雙臂協(xié)作機器人坐標系

1.2 運動學(xué)求解

1.2.1 正運動學(xué)求解

(i=1,2,3,4,5,6)

(1)

(2)

式中：c=cos,s=sin 。

(3)

將機器人各個關(guān)節(jié)變量代入式(3)，可得到夾持機器人運動學(xué)正解。

1.2.2 逆運動學(xué)求解

逆運動學(xué)求解即根據(jù)機器人杠桿參數(shù)和期望位姿，求解機器人各關(guān)節(jié)的運動角[7]。考慮到ER20-C10工業(yè)機器人三軸相交于一點，本研究采用逆變法[8]進行機器人逆運動學(xué)求解。

以夾持機器人為例，假設(shè)工具坐標系為{e}，則{e}到世界坐標系的轉(zhuǎn)換矩陣：

(4)

機器人末端執(zhí)行器位姿矩陣：

(5)

則：

(6)

由式(6)可求解逆運動學(xué)解：

(7)

式中：si=sinθi，ci=cosθi，sij=sinθi+sinθj，cij=cosθi+cosθj，i,j=1,2,3…。

由求解結(jié)果可知，機器人存在多個解，可根據(jù)不同需求選擇合適解進行位姿規(guī)劃。

1.3 運動學(xué)仿真

為驗證以上運動學(xué)模型的可行性，基于MATLAB軟件中Robotics toolbox工具包搭建雙機械臂模型。雙機械臂三維模型參數(shù)采用艾芙特工業(yè)機器人的參數(shù)，具體三維模型如圖2所示。

圖2 雙機械臂三維模型

1.3.1 正運動學(xué)仿真分析

假設(shè)雙機械臂的夾持機械臂和打磨機械臂分別從初始位置A(0,0,0,0,0,0)和C(0,0,0,0,0,0)運動到B(-π/3,-π/6,2π/3,-π/4,π/6,π/2)和D(5π/6,3π/5,π/2,π/3,π/4,-π/6)，運用jrtaj模塊對運動軌跡進行跟蹤，得到如圖3所示的軌跡仿真結(jié)果。由圖3可知，雙機械臂關(guān)節(jié)角曲線平滑，說明其運動平穩(wěn)，及正運動學(xué)算法準確。

圖3 雙機械臂軌跡仿真結(jié)果

1.3.2 逆運動學(xué)驗證

在MATLAB軟件中根據(jù)逆運動學(xué)求解方法編寫求逆解算法，并代入夾持機器人運動到B(-π/3,-π/6,2π/3,-π/4,π/6,π/2)和打磨機械臂運動到D(5π/6,3π/5,π/2,π/3,π/4,-π/6)的末端位姿矩陣PB和PD，可得到機器人的逆解分別如表2和表3所示。由表可知，每個位姿對應(yīng)8個逆解，其中表2中Q3與B(-π/3,-π/6,2π/3,-π/4,π/6,π/2)相同，表3中Q7與D(5π/6,3π/5,π/2,π/3,π/4,-π/6)相同，說明逆運動求解正確。

表2 PB的逆解 單位：(°)

表3 PD的逆解 單位：(°)

2 機器人軌跡規(guī)劃

軌跡規(guī)劃分為關(guān)節(jié)空間規(guī)劃和笛卡爾空間軌跡規(guī)劃兩種。其中，關(guān)節(jié)空間規(guī)劃適用于點對點軌跡規(guī)劃，笛卡爾空間軌跡規(guī)劃適合連續(xù)路徑規(guī)劃。根據(jù)雙臂協(xié)作機器人的路徑特點，為連續(xù)路徑規(guī)劃。本研究采用笛卡爾空間軌跡規(guī)劃方法規(guī)劃雙臂協(xié)作機器人的軌跡[9]。

雙臂協(xié)作機器人在笛卡爾空間中的規(guī)劃基本思路如圖4所示。首先獲取汽車輪轂特征點的位姿信息，包括笛卡爾空間坐標系中的直角坐標和變換姿態(tài)角，然后利用逆解求解方法獲取機器人各個關(guān)節(jié)的關(guān)節(jié)變量，并利用相關(guān)算法對關(guān)節(jié)空間軌跡進行規(guī)劃，最后根據(jù)正運動學(xué)求解模型獲取執(zhí)行器運動軌跡，實現(xiàn)對給定路徑點的規(guī)劃。

圖4 雙臂協(xié)作機器人在笛卡爾空間中的規(guī)劃

關(guān)節(jié)空間軌跡規(guī)劃中，本研究采用五次B樣條曲線算法。五次B樣條曲線即利用五次多項式插值法擬合出機器人各個關(guān)節(jié)的運動軌跡曲線，然后通過B樣條曲線對每個路徑點條件進行約束[10]。五次B樣條曲線函數(shù)的構(gòu)造方法如下：

假設(shè)第i段五次B樣條曲線函數(shù)：

θi(t)=F0(t)Ai-1+F1(t)Ai+F2(t)Ai+1+

F3(t)Ai+2+F4(t)Ai+3+F5(t)Ai+4

(8)

式中：0≤t≤1表示時間；F(t)為t的參數(shù)多項式，即B樣條基函數(shù)；Ai-1,Ai,Ai+1,Ai+2為第i段五次B樣條曲線的控制點。根據(jù)曲線連續(xù)性可知，相鄰點段B樣條曲線的連接處函數(shù)值相同，連接處的1階導(dǎo)數(shù)和2階導(dǎo)數(shù)連續(xù)，即：

θi(1)=θi+1(0)

(9)

(10)

(11)

同時，根據(jù)B樣條基函數(shù)規(guī)范性[11]：

F0(t)+F1(t)+F2(t)+F3(t)+F4(t)+F5(t)=1

(12)

設(shè)五次多項式Fi(t)=Ait5+Bit4+Cit3+Dit2+Eit+Fi，則聯(lián)合式(7)～式(11)，可得Fi(t)的對應(yīng)系數(shù)，進而求解得到：

(13)

因此，第i段五次B樣條曲線函數(shù)可表示：

(14)

根據(jù)上式和機器人關(guān)節(jié)路徑點，即可得到五次B樣條曲線，進而獲取機器人各個關(guān)節(jié)的運動軌跡。

3 實驗研究

3.1 實驗環(huán)境搭建

3.1.1 機器人打磨系統(tǒng)構(gòu)建

本次實驗構(gòu)建的雙臂協(xié)作機器人打磨系統(tǒng)整體硬件架構(gòu)如圖5所示，主要由上位機、兩臺機器人、控制器、傳感器、伺服驅(qū)動器構(gòu)成。其中，上位機通過MATLAB/Simulink進行編程，在控制器中執(zhí)行；兩臺機器人為國產(chǎn)ER20-C10工業(yè)機器人；控制器為dSPACE控制器；傳感器是六維力傳感器；伺服驅(qū)動器選用固高科技公司的EtherCAT總線型伺服驅(qū)動器[12-13]。

圖5 機器人打磨系統(tǒng)整體硬件框架

基于上述硬件，采用MATLAB2018a軟件進行實驗。其中，機器人的運動控制程序通過Simulink進行編譯，在dSPACE Controldesk中執(zhí)行。上位機與控制器通過Ethernet接口進行通信，控制器與伺服驅(qū)動器通過模擬量發(fā)送信號，通過增量式編碼器讀取編碼器信息，通過串口通訊讀取力矩傳感器信息[14]。

3.1.2 軌跡規(guī)劃系統(tǒng)搭建

軌跡規(guī)劃系統(tǒng)包括機器人打磨系統(tǒng)建模和電機驅(qū)動物理模型建模兩個部分。其中，機器人打磨系統(tǒng)通過MATLAB軟件中的Simscape進行建模。首先利用三維軟件SolidWorks繪制機器人打磨系統(tǒng)，利用Simscape multibody link插件導(dǎo)出MATLAB可讀取的xml文件。然后利用MATLAB軟件中的Smimport指令生成Simscape物理模型。

電機驅(qū)動物理模型構(gòu)建的目的是讓電機信號可輸出給機器人的各個關(guān)節(jié)，并適應(yīng)不同角度和狀態(tài)的Simscape。因此，首先需將電機輸出轉(zhuǎn)換為Simulink的模擬值，然后通過Simulink-PS將模擬信號轉(zhuǎn)換為機器人每個關(guān)節(jié)的角度信號。構(gòu)建的電機模型如圖6所示。最后，按照同樣方法建立所有電機模型，并封裝到機器人的物理模型，即實現(xiàn)了電機驅(qū)動物理模型的構(gòu)建。

圖6 電機模型

為檢驗電機驅(qū)動物理模型是否正確，將機器人12個關(guān)節(jié)角度設(shè)置為0，得到機器人的初始位姿如圖7所示。由圖7可知，機器人坐標系初始狀態(tài)一致，說明構(gòu)建的電機驅(qū)動物理模型正確。

圖7 模型初始位姿

3.2 數(shù)據(jù)來源及預(yù)處理

本次實驗數(shù)據(jù)來自FARO公司生產(chǎn)的三維激光掃描儀采集的高精度輪轂點云數(shù)據(jù)。三維激光掃描儀的規(guī)格參數(shù)如表4所示，部分掃描得到的輪轂點云數(shù)據(jù)坐標如表5所示[15]。

考慮到三維激光掃描儀采集的點云數(shù)據(jù)存在噪聲，且數(shù)據(jù)量巨大。為加快數(shù)據(jù)的處理速度和效率，本研究利用InnovMetric企業(yè)推出的點云處理分析軟件polyWorks對采集到的輪轂點云數(shù)據(jù)進行預(yù)處理。具體操作如下：

表4 三維激光掃描儀規(guī)格參數(shù)

表5 部分輪轂點云數(shù)據(jù)坐標信息 單位：mm

(1)數(shù)據(jù)簡化

針對采集的輪轂點云數(shù)據(jù)數(shù)量巨大的問題，對數(shù)據(jù)進行了簡化處理。首先，查看采集的點云數(shù)據(jù)中是否存在過度密集的點云數(shù)據(jù)或噪聲，若存在則利用MeshLab軟件精簡數(shù)據(jù)數(shù)量。然后根據(jù)輪轂為對稱旋轉(zhuǎn)體的特點，通過建立經(jīng)輪轂中心軸的平面方程，僅保留兩個平面內(nèi)的點，對點云數(shù)據(jù)進行進一步簡化。

(2)數(shù)據(jù)排序

考慮到簡化后的輪轂點云數(shù)據(jù)分散零亂，而輪轂為對稱旋轉(zhuǎn)體，需對簡化后的點云數(shù)據(jù)集進行排序。首先對點云數(shù)據(jù)的點進行依次編號，并將Z、Y、X、J點云的變化存入對應(yīng)z[],y[],x[],j[]，同時將所有數(shù)據(jù)存放于臨時數(shù)組o[]中。然后，由大到小對Z坐標進行排序，改變編號。同時按照變動后的數(shù)據(jù)j[]，使X、Y的坐標隨之排序：

o[i]←y[j[i]]

y[i]←o[i]

o[i]←x[j[i]]

x[i]←o[i]

最后，當Z完成排序后，同理對Y坐標進行排序。如此便完成了Z、Y、X點云數(shù)據(jù)的優(yōu)先排序，避免了軌跡規(guī)劃時路徑點分散的問題。

(3)數(shù)據(jù)坐標變換

由于采集的點云坐標系與機器人的坐標系不一致，因此還需對點云數(shù)據(jù)坐標進行轉(zhuǎn)換。將點云坐標系中的位姿信息通過坐標系之間的轉(zhuǎn)換矩陣，轉(zhuǎn)換為世界坐標系下的點云信息。然后按照Z、Y、X的優(yōu)先排序從小到大和逆運動學(xué)算法求解機器人的6個關(guān)節(jié)運動角度，并以m×6的矩陣形式進行存儲。

通過上述點云數(shù)據(jù)簡化、優(yōu)先級排序和坐標變換，可獲取用于機器人運動軌跡規(guī)劃的世界坐標系下的輪轂點云數(shù)據(jù)。為便于后續(xù)驗證，選取1 000個點云數(shù)據(jù)進行實驗。

3.3 結(jié)果與分析

3.3.1 機器人軌跡規(guī)劃結(jié)果

在MATLAB軟件中對五次B樣條函數(shù)規(guī)劃的機器人軌跡進行仿真驗證，部分關(guān)節(jié)的仿真結(jié)果如圖8所示。由仿真結(jié)果可知，五次B樣條曲線關(guān)節(jié)角度、角加速度和角速度曲線平滑，且未發(fā)生突變，說明五次B樣條曲線法規(guī)劃出的機器人軌跡可保證機器人在執(zhí)行命令時平穩(wěn)運行。

圖8 五次B樣條軌跡對機器人關(guān)節(jié)6的軌跡仿真結(jié)果

為進一步驗證五次B樣條曲線法的有效性，實驗對比了五次多項式插值算法、七次B樣條插值算法與五次B樣條插值算法對機器人關(guān)節(jié)6的速度、角速度和加速度的影響，結(jié)果如圖9所示。由圖9可知，相較于五次多項式插值算法和七次B樣條插值算法，五次B樣條曲線對機器人關(guān)節(jié)6的速度和加速度規(guī)劃的曲線均得到了有效的改善，最大加速度降低了0.78%和0.69%。由此說明，采用五次B樣條曲線規(guī)劃出來的機器人關(guān)節(jié)軌跡可降低機器人機械臂的慣性沖擊。此外，相較于五次多項式插值算法和七次B樣條插值算法，五次B樣條曲線的計算過程更簡單，進而提高了算法的運行速度和對機器人的控制效率。綜合來看，五次B樣條曲線在機器人軌跡規(guī)劃中具有一定的優(yōu)勢。

圖9 不同算法規(guī)劃速度與加速度軌跡對比

3.3.2 機器人打磨過程仿真結(jié)果

根據(jù)逆解模型求解機器人各個關(guān)節(jié)運動角度，并利用五次B樣條插值法對各個關(guān)節(jié)的軌跡進行規(guī)劃，并基于Simscape物理模型對打磨過程進行仿真，結(jié)果如圖10所示。由動態(tài)仿真結(jié)果可知，整個機器人打磨過程中，機器人關(guān)節(jié)之間未發(fā)生碰撞和干涉等情況，且未超出限制范圍，未發(fā)生突變。

圖10 打磨過程仿真結(jié)果示例

3.3.3 機器人控制仿真結(jié)果

為檢驗機器人的控制效果，利用MATLAB軟件上的編譯器進行編譯，并調(diào)用Simulink工作區(qū)進行存儲，將生成的C代碼下載到控制軟件中，然后通過Diagnostic Viewer查看程序是否寫入。最后，進入控制界面控制機器人，讓機器人從初始狀態(tài)移動到軌跡規(guī)劃的起點，實現(xiàn)點到點的控制。當機器人所有關(guān)節(jié)到達預(yù)設(shè)起點后，控制機器人各個關(guān)節(jié)在世界坐標系下進行聯(lián)動，并讓機器人按照仿真軌跡運動。機器人打磨輪轂過程中的瞬時畫面如圖11所示。由圖11可知，所設(shè)計的控制系統(tǒng)可有效實現(xiàn)雙臂協(xié)作機器人對輪轂的打磨作業(yè)，且打磨過程中，雙臂協(xié)作機器人的姿態(tài)平穩(wěn)，路徑平滑可靠，具有可行性和較強的實用性。

圖11 機器人打磨輪轂瞬時畫面

4 結(jié) 語

綜上所述，本研究通過利用D-H構(gòu)建的雙臂協(xié)作機器人運動學(xué)模型，并利用五次B樣條曲線對機器人軌跡進行規(guī)劃，實現(xiàn)了雙臂協(xié)作機器人的智能控制。整個控制中，雙臂協(xié)作機器人之間未出現(xiàn)碰撞和相互干涉的現(xiàn)象，具有較強的可行性和一定的實用性。但由于條件限制，本研究仍存在一些不足有待改進，在機器人路徑規(guī)劃中，由于需要使用夾持機器人固定輪轂到預(yù)設(shè)工位，當夾持機器人位姿發(fā)生變化時，打磨軌跡需要進行重新規(guī)劃，不能保證打磨軌跡的實時性。下一步研究將嘗試加入機器視覺，以提高機器人軌跡規(guī)劃的實時性。