基于dSPACE的工業(yè)機器人運動控制系統(tǒng)仿真分析及實驗

路博凡, 韓家哺, 張艷偉

(上海工程技術大學 機械與汽車工程學院，上海 201620)

0 引 言

近些年來，越來越多的汽車工廠在一定精度工業(yè)生產[1]中開始用串聯(lián)機器人打磨輪轂來代替人工打磨，做法有著低成本高效率等優(yōu)點，所以對于串聯(lián)機械臂控制的研發(fā)也是一種大趨勢。之前在國內對串聯(lián)機械臂以傳統(tǒng)方法控制較多，是以求出六自由度的機器人的正解[2]、IPC+PMAC的控制方法在RTLinux上開發(fā)工業(yè)機器人的控制系統(tǒng)[3]。求解的逆利用仿生機器人[4],機器人進行運動學標定[5]、或是進行焊接路徑的視覺識別與軌跡規(guī)劃[6]用運行程序、停止程序運行以及反饋界面從而實現(xiàn)了實驗平臺的搭建[7]。但這種控制方法設計周期長成本高，現(xiàn)在使用Lapusan與Simulink的便利實現(xiàn)了，機器人的仿真模型和末端軌跡反映實驗并借助 dSPACE 搭建了機器人的控制系統(tǒng)[8]。因為MATLAB/Simulink是同一家公司生產的所以可以在操作使用上無縫連接，通過 Simulink 建模仿真的方法模擬控制器，可以在現(xiàn)實開發(fā)中節(jié)約成本以及減少因為硬件配套而要重新開發(fā)驅動的時間從而縮短了串聯(lián)機器人控制系統(tǒng)研發(fā)周期[9-10]。

在正?？焖俅罱刂颇Ｐ偷幕A上對電機參數及機械臂的Simscape物理模型進行改進，從而得到更好的控制效果。

1 ER20-1700機器人的運動建模

ER20-1700機器人是國內市場上主流機械臂的一種，主要用于切割、打磨、上下料等場合，具有6個自由度，并且末端位置和姿態(tài)存在解耦關系。ER20-1700實物模型如圖1所示。

圖1 ER20-1700機器人結構

2 機器人運動學正逆解

2.1 確定機器人改進D-H參數

用改進D-H方法進行運動學建模，坐標系的X軸方向和連桿兩端連接關節(jié)的公垂線共線，坐標系的Z軸方向和連桿所連接的左端關節(jié)的軸線共線，根據右手定則，來確定Y軸方向，然后連桿、連桿左端關節(jié)以及坐標系都是同一個編號i-1。ai-1為兩個關節(jié)軸線之間的公垂線長度即連桿長度。di,θi描述了關節(jié)自身的運動狀態(tài)，其中di被稱為關節(jié)偏距，θi被稱為關節(jié)轉角。

圖2為機器人的運動模型圖，圖中{O-XYZ}是機器人的基座標系，{ot-xtytzt}是機器人末端坐標，L0,L1,L2,L3,L4,Hb是對應機器人的連桿之間的距離，分別是50、270、70、299、78.5和321.5 mm,機器人的DH參數見表1。

圖2 機器人運動模型圖

表1 機器人DH參數表

2.2 機器人運動學逆解

根據末端坐標系和基座標系之間的關系矩陣：

采用代數法來分析機器人的逆運動學。根據前面建立的機器人的MDH坐標圖，可知機器人的4、5、6軸交于機器人末端的一個點，所以這3軸的變化不會影響該點的位置變化，因此可以先求解前3軸，再求解后3軸，求解步驟如下：

(1) 對θ1求解。

其中矩陣各元素的具體表達式為

這里ci,si，表示cos(ti)和sin(ti)的縮略形式，等式右邊中第2行第4列元素為零，聯(lián)立m24=pyc1-pxs1-L4ayc1+L4axs1可以解出θ1：

(2) 對θ3求解。

1T2-10T1-1T6Te-1=

在等式右邊的結果中，選取和θ2相關的部分，分別是第1行第4個結果和第2行第4個結果：

c2m34+(m14-L0)s2=L1+L2c3-L3s3(m14-L0)c2-

s2m34=L3c3+L2s3

對上式進行平方相加整理可得：

A1c3+B1s3=C1

其中：

A1=37 800B1=-161 460

C1=2 500-100m14+m142+m342-167 201

β1=arctan(A1/B1)

θ3=±arcsin(C1/2.6*1010)-β1

(3) 求解θ2。

將求得的θ3結果代入下式

c2m34+(m14-L0)s2=L1+L2c3-L3s3

可以求得θ2其中：

(4) 求解θ4，θ5，θ6。

取MDH中變換矩陣的旋轉部分R1～R6，這樣可以構建等式：

R3-1R2-1R1-1R=R4R5R6

分別取等式兩邊矩陣中第3列，第1行和第3行的元素相除，可得：

當c5=0的時候，機器人第4軸和第6軸重合，機器人失去一個自由度，關節(jié)進入奇異位置，這是在規(guī)劃機器人關節(jié)空間運動時需要避免的情況。

當cos(θ4)≠0的時候，可以利用R3-1R2-1R1-1R=R4R5R6等式中第3列第2行除以第3列第1行，解得θ5：

θ5=arctan(c4(axc23c1+ayc23s1-azs23)/

axs23c1+ays23s1+azc23)

當cos(θ4)=0的時候，可以利用R3-1R2-1R1-1R=R4R5R6中第3列第2行除以第3列第3行，解得θ5：

當c5=0時，可以充分利用R3-1R2-1R1-1R=R4R5R6中第二行第二列除以第二行第一列，解得θ6：

3 ER20-1700機器人控制系統(tǒng)設計

3.1 基于SimMechanic工具箱的機器人建模

首先將打開已經完成的三維建模裝配體，注意關節(jié)之間的約束關系并在導入Matlab之前要盡量簡化模型，使之保證導入的完整性和正確性。

(1) 在工具欄內點擊Simscape Multibody Link內的Export來生成MATLAB可以讀取的step和xml格式文件。

(2) 導出xml格式文件之后，在MATLAB命令窗口中輸入指令:smimport；來生成Simscape物理模型。

(3) 將Torque設置為Automatically Computed，Motion設置為Provide By Input，將傳感器的信號勾選作為輸出。對于六自由機械臂而言，需要控制的是6個旋轉坐標系，為了實現(xiàn)控制，對其內部屬性進行修改。

關節(jié)1配置完后如圖3所示。

圖3 關節(jié)1模型圖

(4) 將每個Rotation配置成這樣之后，就可以將外部的角度信號輸入給機械臂Simscape模型中的每個關節(jié),輸出信號選擇為角度值。配置完畢之后選中所有單擊右鍵Creat Subsystem from Select將整個Simscape封裝。改進后的機械臂Simscape物理模型如圖4所示。

圖4 改進后的機械臂Simscape物理模型

3.2 搭建電機驅動Simscape物理模型

因為在Simulink中步進電機模型輸出的是位置信號并且不需要編碼器來檢測所以可以很好地滿足角度輸出的要求，并且給到的PWM信號通過步進電機?？梢暂敵雠ぞ豏。因此需要搭建步進電機模型。

(1) 在MATLAB命令窗口輸入elec_motor_stepper指令直接創(chuàng)建步進電機模型。將默認輸入給電機的階躍信號改為Input1，再除上步距角1.8°得到步進電機的步數，再給到電機的控制器，將步數轉化為對應的PWM信號。

(2) 為了讓電機信號能夠輸出給各個關節(jié)，并且適應不同初始角度狀態(tài)的Simscape，需要在電機的輸出a后接PS-Simulink元件將電機的物理角度信號轉換成Simulink的模擬值，修改后的電機模型如圖5所示。

圖5 改進后步進電機模型圖

3.3 機械臂Simscape正逆解仿真

將上面兩個模塊進行封裝整理后，并加入之前做好的機械臂的正解逆解算法。經整理后最終的仿真模型如圖6所示。

圖6 機械臂仿真模型圖

在模型下，在下方輸入角度可以得到通過正解算法得到機器人末端的位置。在上方輸入末端位置坐標可以使仿真模型中的機械臂轉動相應角度。為驗證其仿真的準確性，把之前的正逆解算法通過編程寫入MATLAB的機器人工具箱中，出仿真結果。先驗證正解的準確性同時輸入各關節(jié)角度(-30,-60,100,-30,60,-30)與之前的仿真結果對比完全一致，如圖7所示。

圖7 仿真結果比對圖

然后，再開始驗證逆解準確性在仿真模型上端輸入末端位置(x=29.219,y=-177.95,z=578.13,R=159.925,P=29.249,Y=80.075)然后可以通過模型中scope模塊觀察到各個關節(jié)旋轉的角度，如圖8所示。

圖8 各關節(jié)旋轉角度仿真結果圖

比對圖7可得仿真模型在輸入末端指令后的 2 s 內達到指定姿態(tài)。由圖10可知關節(jié)1在1 s內達到-30度位置，關節(jié)2在2 s內達到-60度位置，關節(jié)3在2 s內達到100度位置，關節(jié)4在1 s內達到-30度位置，關節(jié)5在1 s內達到60度位置，關節(jié)6在1 s內達到-30度位置，于逆解運算結果保持一致。由此可得逆解算法正確。

3.4 機械臂的軌跡規(guī)劃

在軌跡規(guī)劃中，首先在MATLAB中運用編程建立機器人模型。定義軌跡規(guī)劃中的初始關節(jié)角度(First_Theta)和終止關節(jié)角度(Final_Theta)并定義其步數為777。隨后使用jtraj函數帶入初始角度、終止角度和步數后，可以得到關于位置、速度和加速度的一個6*777的矩陣，隨后把得到的數據用編程制成圖像?？傻脵C械臂運動時各關節(jié)的位置曲線、速度曲線和加速度曲線如圖9所示。

圖9 各關節(jié)的位置曲線、速度曲線和加速度曲線圖

然后通過之前計算的正運動學公式輸入初始關節(jié)角度和終止關節(jié)角度可以得到起始和終止的位姿矩陣。接下來就利用ctraj在笛卡爾空間規(guī)劃軌跡，在ctraj函數中輸入步數起始和終止的位姿矩陣，就可以得到表示2點之間運動信息的齊次旋轉矩陣，是一個6*777的矩陣，使用transl函數在齊次旋轉矩陣中提取移動變量，相當于笛卡爾坐標系的點的位置。最后用編程的手段畫出A1到A2直線和B2圓的軌跡,如圖10所示。

圖10 A1到A2直線和B2圓軌跡圖

4 實驗平臺的搭建與結果

4.1 實驗設備介紹

實驗平臺由dSPACEDS1007組件系統(tǒng)，包含12個DA接口、12個編碼器接口、2路RS422接口和24個IO接口;一臺電機伺服驅動器、一塊轉接板、一臺ER20-1700機械臂、緊急制動裝置、一臺pc控制臺以及力矩傳感器組成如圖11所示。

圖11 串聯(lián)機器人實驗平臺圖

4.2 實驗過程與結果

由于Dspace和Simulink是由同一家公司生產的，所以可以直接把仿真環(huán)境下系統(tǒng)轉化為程序導入其中。當在Simulink軟件中編譯好程序后，需要在Control Desk軟件中進行調試和變量的讀取。打開Control Desk軟件后，先加載Simulink軟件編譯的sdf文件，然后可以看見一個調試界面，通過配置完以后的實驗界面先控制機械臂以0.05 m/s的速度走完0.05 m長的直線再控制機械臂以0.025 m/s的速度走完半徑為0.025 m的圓為觀測其數值顯示變量的歷史變化軌跡，讓靈活的觸發(fā)式測量和多個控件同步比較。

如圖12所示，給出了機械臂跟蹤直線和圓時機械臂的關節(jié)誤差。通過Dspace把實驗數據導入MATLAB中制作成實驗對比圖。其中虛線是系統(tǒng)規(guī)劃在x、y坐標系下走的路徑，實線是機械臂在x、y坐標系下的實際移動距離。最大跟蹤誤差小于 3 mm。故證明了系統(tǒng)及方法的有效性。

圖12 走直線與圓實驗結果對比圖

5 結 論

從零搭建ER20-1700機器人的控制系統(tǒng),先在 MATLAB/SIMULINK 中搭建仿真模型并進行仿真實驗。 仿真結果表明,逆解算法搭建成功。再在進行實際實驗，通過dSPACES進行走直線和畫圓實驗。實驗結果表明最大跟蹤誤差小于3 mm，控制方法可行。