鈑金鉆孔機器人位置伺服控制系統設計與仿真*

孟 力，蘇工兵，周會勇，袁浪佳

（武漢紡織大學機械工程與自動化學院，湖北武漢 430073）

0 引言

國內現有鈑金加工柔性自動化生產線中僅僅覆蓋了上料、剪切、成型和下料工序，而鈑金加工中的壓鉚、鉆孔攻絲、焊接、打磨和拋光等工序不在其中。開展機器人鈑金加工鉆孔軌跡規劃和運動分析不僅能減少實現運動狀態控制系統參數反復的測試和改進的時間，而且能提高機器人程序運行的穩定性和精度[1-2]。本文作者以川崎BA006N型工業機器人加工大型鈑金通孔為研究對象，通過SolidWorks建立六自由度機器人與鈑金鉆孔定位加工三維虛擬模型，利用MATLAB中Robotics toolbox求解機器人末端不同工位的位姿和對應各關節的關節角，根據鈑金鉆孔的工藝和工序，規劃機器人末端執行器運動軌跡，并在ADAMS虛擬樣機模型中模擬機器人末端執行器運動軌跡，獲取機器人各關節的位置驅動函數。同時以單關節模糊PID位置控制直流電機模型為基礎，在MATLAB/simulink中將封裝單關節電機控制模型分別與封裝機器人六轉動關節模型組裝，構建機器人各關節位置跟蹤控制模型，并以ADAMS虛擬樣機仿真模型中獲取機器人各關節的位置驅動函數為輸入信號，仿真分析機器人鈑金鉆孔位置伺服控制系統各關節位置跟隨特性。

1 工程背景

1.1 六自由度機器人鈑金鉆孔結構尺寸

工業機器人選擇川崎BA006N型六自由度串聯機器人本體為研究對象，其結構參數及運動范圍如圖1所示。該機器人最大伸展距離1 445 mm，位置重復性±0.06 mm。選擇平面鈑金鉆孔為加工對象，其結構尺寸長為800 mm，寬500 mm，厚度16 mm，4個孔徑為φ14 mm，孔定位尺寸如圖2所示。鈑金被固定平臺臺架上并由夾具螺栓固定。

圖1 BA006N型機器人結構尺寸

圖2 鈑金件的工程簡圖

根據機器人本體實際結構尺寸運動范圍、臺架結構尺寸及鈑金鉆孔工位結構尺寸參數，并設定電鉆安裝在距機器人末端法蘭上，定位坐標系相對于機器人末端坐標系，X方向與其重合，Z方向相距47.5 mm，Y方向相距191.75 mm，鉆頭直徑為φ14 mm。利用SolidWorks建立機器人鈑金鉆孔三維模型如圖3所示。其中SolidWorks建模中原點坐標系選擇為機器人底座坐標系與基坐標系重合點。

圖3 六自由度機器人鈑金鉆孔三維模型

1.2 鈑金鉆孔工序規劃

圖4 鉆孔工序規劃

對待鈑金加工4個孔位進行編號，設定從初始位置機器人末端運動到距離1號孔上方3.00 mm時，鉆頭開始向下鉆孔，深度為20 mm，通孔后回到1號孔上方3.00 mm，然后依次完成2號孔、3號孔和4號孔鈑金4個孔的加工，從4號孔回到1號孔上方，機器人末端再回到初始位置，該工序設定可實現連續孔位加工。其結果如圖4所示。由此可得到4孔位相對基坐標系的位置坐標，1號孔（769.95，769.95，607），2號孔（769.95，830.05，607），3 號 孔 （830.05， 830.05， 607） 和 4 號 孔 （830.05，769.95，607）。

2 機器人不同工位狀態末端位姿矩陣

2.1 初始位置機器人末端位姿矩陣

以D-H法建立六自由度機器人連桿坐標系，設定機器人初始工作位如圖5所示，則各連桿參數如表1所示。

圖5 機器人連桿坐標系

表1 機器人連桿參數表

利用MATLAB（Robotics Toolbox）工具箱中SerialLink函數建立六自由機器人本體仿真模型[3]，將表1中的D-H參數輸入如下程序Link函數中；

qn=[0 pi/2 0 0 0 pi/2];%設置六自由度機器人初始位姿%

R.plot(qn)%繪制模型%

運行該程序可以得到初始姿態與SolidWorks初始姿態一致機器人三維模型，如圖6所示。并可計算得到機器人模型末端初始位置位姿矩陣如下：

圖6 MATLAB仿真機器人模型

2.2 鈑金不同孔位對應機器人末端位姿矩陣

所選定加工平面鈑金水平固定，機器人末端運動過程中姿態不變，機器人末端鉆孔過程中只有鉆頭與水平面垂直的軸向位移，由此整個運動過程工序和工位規劃如圖7所示。

圖7 鉆孔工序規劃

同樣MATLAB（Robotics Toolbox）工具箱中SerialLink函數，可得到如圖7所示工序規劃中7個工位的位姿矩陣：

分別將不同工位位姿矩陣 T0、T1、T2、T3、T4、T5、T6和T7代入MATLAB中q=ikine(R，T)函數，可求出機器人末端在上述不同位姿時機器人變量q，其中q為θ1、θ2、θ3、 θ4、θ5和θ66個關節角度值。設定相鄰位姿變換過程中轉動變化角分別為 Δθ1、 Δθ2、 Δθ3、 Δθ4、 Δθ5和Δθ6，該值通過相鄰工位之間后一個位姿矩陣關節角度值減去前一個位姿矩陣關節角度值獲得，并設定仿真運動時間14 s。計算結果與時間分配段如表2所示。

表2 機器人各關節角位移變化量

3 關節空間連續軌跡規劃

按照圖7所示機器人鈑金鉆孔工序將鉆頭的運動設定為鉆孔時豎直方向直線運動和相鄰孔位之間為曲線運動。分別采用三次多項式函數和STEP函數對直線運動和相鄰運動曲線實現點對點的連續軌跡規劃。鉆孔直線段使用POLY（x，x0，a0，a1，a2，a3）函數三次多項式驅動。其中a0、a1、a2、a3為三次多項式的系數，x、x0這里為時間變量。曲線運動軌跡使用STEP（time，x0，h0，x1，h1）函數，其中x0、x1為初始變量和終止變量（這里為時間），h0、h1為初始變量和終止變量（這里為關節變量）。直線段三次多項式表達式如下：

式中：Δθ關節轉動的角位移，t0為運動開始的初始時間，tf為運動結束的終點時間。

將表2中角位移變化量參數分別代入表達式（2）-（6）中，即可得到鉆孔直線段驅動POLY函數系數。而曲線段采用STEP函數根據輸入初始時間和終止時間的關節角度變量值自動實現三次多項式項插值。兩函數組合可作為機器人鈑金鉆孔ADAMS模型中機器人關節驅動函數，實現連續運動軌跡的仿真。

4 基于ADAMS運動軌跡仿真與分析

圖8 關節位置隨時間變化曲線

圖9 模糊PID位置跟蹤結構框圖

將SolidWorks中建立的機器人鈑金鉆孔三維模型導入ADAMS仿真軟件中，定義機器人三維模型材料信息，并按照機器人關節運動施加相應的固定副及旋轉副約束，同時對6個轉動關節和鉆頭旋轉副施加建立的函數旋轉驅動，建立虛擬樣機模型。

其驅動函數利用ADAMS中IF（expr0：expr1，expr2，expr3）函數（這里expr0表示時間，若expr0＜0，則函數結果為expr1;若expr0=0，則函數結果為expr2;若expr0＞0，則函數結果為expr3）將直線段的POLY函數驅動和曲線段的STEP函數組合成14S連續軌跡驅動函數。

利用表2中參數可分別獲得。為獲到機器人末端鉆頭運動軌跡曲線，將機器人鉆頭末端設置為MARKER標記點，通過仿真可標定出機器人末端鉆頭運動路線和動作，模擬機器人末端鉆頭鉆孔工序全過程，達到設定鈑金鉆孔的工藝和工序要求。

由此可通過ADAMS后處理提取機器人各個關節位置隨時間變化驅動函數，如圖8所示。該位置驅動函數為設計機器人關節驅動電機位置伺服控制器設計提供依據。

5 基于MATLAB/simulink關節驅動電機控制系統設計及仿真

以ADAMS后處理提取機器人各個關節位置驅動函數并沒有考慮關節驅動電機參數的影響，為實現機器人鈑金鉆孔運動軌跡跟蹤和位置伺服控制系統設計，選擇六自由度機器人關節驅動電機為直流電機，并構建六自由度串聯機械臂關節三閉環位置控制仿真實驗模型，其中位置伺服環采用模糊自適應PID調節器，轉速環與電流環采用PI調節器[4]。控制系統結構框圖如圖9所示。

5.1 模糊PID控制器的設計

為實現實時和高準確率的控制從而提高直流電動機的輸出性能，設計一種模糊PID控制器來調節PID控制器的增益參數[5]。模糊PID控制以電機轉角預期值與實際輸出值相比較得到轉角偏差E和轉角偏差變化率EC作為模糊輸入變量，設定E、EC論域定義為{-3，-2，-1，0，1，2，3}，E、EC模糊子集定義為{NB，NM，NS，ZO，PS，PM，PB}，服從高斯型隸屬度函數分布曲線。以PID控制器的KP、KI、KD作為輸出變量。設定KP、KI和KD的論域定義為{-3，-2，-1，0，1，2，3}， KP、 KI、 KD的模糊子集為{NB，NM，NS，ZO，PS，PM，PB}，服從三角形隸屬函數分布曲線。模糊推理器的核心是由“if...then...”語句構成的一系列的模糊控制規則，選取合適的控制規則將直接關系到三閉環位置控制系統性能。模糊控制器是按一定的語言規則進行工作的，而這些控制規則是建立在總結操作員控制經驗的基礎上， KP模糊控制規則表如表3所示。KI和KD模糊控制規則表獲取同KP方法[6-8]。

表3 K P模糊規則表

電動機控制器工作時，通過模糊控制指令模糊推理機制便檢測誤差e和ec的變化率，從而分別產生模糊變量E和EC，然后控制器實時對控制器原有的KP、KI和KD參數進行調節，從而使得控制器總是能夠對直流電動機產生最優控制信號[9-10]。利用MATLAB中模糊控制工具箱，通過編輯隸屬度函數，以表3所示模糊控制規則對模糊規矩進行設計，則模糊PID控制封裝模塊模型圖10所示。

5.2 單關節電機控制構建及仿真

為實現機器人關節運動要求，選擇Z4-100-1直流電機為機器人關節驅動電機來驗證控制器設計的合理性，其主要性能參數如表4所示。

圖10 模糊PID控制器模型

圖11 模糊PID直流電機位置跟蹤仿真模型

表4 電機參數

圖12 階躍信號仿真曲線

根據圖11模糊PID控制器結構框圖，利用圖12封裝的PID模糊控制器和電機參數，在MATLAB/simulink中建立單關節電機位置跟蹤模糊PID控制仿真模型。其中轉速環PI調節器Kn=3.5，電流環PI調節器Ki=3，轉速環濾波器時間常數Ton=0.02，電流環濾波時間常數Toi=0.002，變換器放大系數Ks=16，時間常數Ts=0.001 7，電磁時間常數Tl=La/R=0.009 86，機電時間常數Tm=GD2R/(375KeCm)=0.052 5。仿真模型見圖13。

為驗證電機控制器模型位置跟蹤動態性能，不考慮擾動的情況下，在2 s時給定一個幅值為1的階躍信號，得到動態跟隨曲線如圖12所示。從圖中可看出系統對階躍信號的相應特性，基本沒有超調，上升時間tr=0.466 s有延遲，說明控制系統的動態跟蹤性能達到位置伺服無超調的性能要求，避免機器人鈑金鉆孔時發生位置碰撞，從而為組裝機器人6個關節電機控制器提供依據。

5.3 六自由度機器人鈑金鉆孔位置伺服控制模型

為搭建六自由度機器人鈑金鉆孔位置伺服控制模型，利用Simcape插件將SolidWorks中機器人三維模型導入MATLAB，建立機械人轉動關節與連桿連接機械臂模型，轉動關節引出兩端口信號，一端口與電機模型相連作為信號輸入，另一端口輸出到示波器，顯示位置跟隨狀態。機械臂仿真模型如圖13所示。

圖13 機器人機械系統模型

在MATLAB/simulink中將封裝單關節電機控制模型分別與封裝機器人6關節輸入信號端連接，輸出端口連接示波器，得到六自由度機器人各關節位置控制模型，如圖14所示。將ADAMS中仿真得到如圖8所示的六關節轉動角與時間曲線作為輸入端信號，通過模糊PID控制器仿真得到各關節示波器顯示位置跟隨曲線如圖15所示。

圖14 機器臂六關節位置控制模型

由圖17可知，采用模糊PID位置控制器獲取的機器人六關節轉角曲線與各關節的位置驅動曲線具有較好的跟隨性，只是在機器人末端初始位置移動到1號孔上方和完成鉆孔工序后回到機器人末端初始位置時間上有延遲，而在機器人末端實現鈑金4個孔位鉆孔位置跟蹤精度較高，說明構建的機器人鈑金鉆孔仿真控制模型具有很好的動態跟蹤性能，也為機器人鈑金鉆孔伺服驅動控制系統程序設計提供參數依據。

6 結論

（1）以六自由度機器人鈑金鉆孔為研究對象，利用MATLAB中Robotics Toolbox工具、SolidWorks和ADAMS軟件建立虛擬定位鉆孔樣機仿真模型，根據設定鈑金鉆孔工序，通過STEP函數和三次多項式POLY函數對機器人鉆孔運動中的曲線和直線段規劃運動軌跡，并在ADAMS模擬機器人末端鉆頭鉆孔工序全過程，達到設定鈑金鉆孔的工藝和工序要求。獲取的機器人六關節驅動位置函數為機器人關節伺服驅動控制系統設計提供依據。

圖15 6關節模糊PID控制位置跟隨曲線

（2）以單關節模糊PID位置控制直流電機模型為基礎，在MATLAB/simulink中將封裝單關節電機控制模型分別與封裝機器人六轉動關節模型組裝，構建機器人各關節位置跟蹤控制模型，并以ADAMS虛擬樣機仿真模型中獲取了機器人各關節的位置驅動函數為跟隨信號，仿真分析模糊PID位置控制跟隨性，結果表明機器人鈑金鉆孔位置伺服控制系統各關節位置具有較好跟隨特性。為機器人鈑金柔性線自動化加工伺服驅動控制系統程序設計以及機器人實體運動和同步虛擬仿真提供技術支撐。