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

胡文娟

(商洛職業技術學院 機電工程學院，商洛 726000)

0 引 言

機器人作為新型的生產工具，隨著其技術的發展被廣泛應用于各個領域，在生產的各個環節中扮演著重要的角色。雙臂協作機器人掃除了傳統機器人部署成本高等障礙，為中小型企業實現智能制造、提高生產效率和安全性帶來希望。然而，在雙臂協作機器人的應用中，其運動控制是當下機器人研究的重點和難點。

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

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

1 機器人運動學分析

1.1 運動學模型構建

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

表1 ER20-C10工業機器人連桿與關節幾何參數

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

圖1 雙臂協作機器人坐標系

1.2 運動學求解

1.2.1 正運動學求解

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

(1)

(2)

式中：c=cos,s=sin 。

(3)

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

1.2.2 逆運動學求解

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

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

(4)

機器人末端執行器位姿矩陣：

(5)

則：

(6)

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

(7)

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

由求解結果可知，機器人存在多個解，可根據不同需求選擇合適解進行位姿規劃。

1.3 運動學仿真

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

圖2 雙機械臂三維模型

1.3.1 正運動學仿真分析

假設雙機械臂的夾持機械臂和打磨機械臂分別從初始位置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所示的軌跡仿真結果。由圖3可知，雙機械臂關節角曲線平滑，說明其運動平穩，及正運動學算法準確。

圖3 雙機械臂軌跡仿真結果

1.3.2 逆運動學驗證

在MATLAB軟件中根據逆運動學求解方法編寫求逆解算法，并代入夾持機器人運動到B(-π/3,-π/6,2π/3,-π/4,π/6,π/2)和打磨機械臂運動到D(5π/6,3π/5,π/2,π/3,π/4,-π/6)的末端位姿矩陣PB和PD，可得到機器人的逆解分別如表2和表3所示。由表可知，每個位姿對應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 機器人軌跡規劃

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

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

圖4 雙臂協作機器人在笛卡爾空間中的規劃

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

假設第i段五次B樣條曲線函數：

θ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的參數多項式，即B樣條基函數；Ai-1,Ai,Ai+1,Ai+2為第i段五次B樣條曲線的控制點。根據曲線連續性可知，相鄰點段B樣條曲線的連接處函數值相同，連接處的1階導數和2階導數連續，即：

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

(9)

(10)

(11)

同時，根據B樣條基函數規范性[11]：

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

(12)

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

(13)

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

(14)

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

3 實驗研究

3.1 實驗環境搭建

3.1.1 機器人打磨系統構建

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

圖5 機器人打磨系統整體硬件框架

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

3.1.2 軌跡規劃系統搭建

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

電機驅動物理模型構建的目的是讓電機信號可輸出給機器人的各個關節，并適應不同角度和狀態的Simscape。因此，首先需將電機輸出轉換為Simulink的模擬值，然后通過Simulink-PS將模擬信號轉換為機器人每個關節的角度信號。構建的電機模型如圖6所示。最后，按照同樣方法建立所有電機模型，并封裝到機器人的物理模型，即實現了電機驅動物理模型的構建。

圖6 電機模型

為檢驗電機驅動物理模型是否正確，將機器人12個關節角度設置為0，得到機器人的初始位姿如圖7所示。由圖7可知，機器人坐標系初始狀態一致，說明構建的電機驅動物理模型正確。

圖7 模型初始位姿

3.2 數據來源及預處理

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

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

表4 三維激光掃描儀規格參數

表5 部分輪轂點云數據坐標信息 單位：mm

(1)數據簡化

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

(2)數據排序

考慮到簡化后的輪轂點云數據分散零亂，而輪轂為對稱旋轉體，需對簡化后的點云數據集進行排序。首先對點云數據的點進行依次編號，并將Z、Y、X、J點云的變化存入對應z[],y[],x[],j[]，同時將所有數據存放于臨時數組o[]中。然后，由大到小對Z坐標進行排序，改變編號。同時按照變動后的數據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點云數據的優先排序，避免了軌跡規劃時路徑點分散的問題。

(3)數據坐標變換

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

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

3.3 結果與分析

3.3.1 機器人軌跡規劃結果

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

圖8 五次B樣條軌跡對機器人關節6的軌跡仿真結果

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

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

3.3.2 機器人打磨過程仿真結果

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

圖10 打磨過程仿真結果示例

3.3.3 機器人控制仿真結果

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

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

4 結 語

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