基于Matlab/Simulink的工業機器人力控制仿真研究

黃小翀，倪受東

（南京工業大學 機械與動力工程學院，南京 211800）

隨著“中國制造2025”的提出，工業機器人已經廣泛地應用于工業生產領域之中，機器人力控制是工業機器人研究的一個重點方向。當機器人手臂在執行與作業環境有接觸的操作 （如焊接或者打磨）時，在特定的作業環境，機器人要求任意產生的作用力具有較高的柔順性和機器人在自由空間作業時對位置伺服以及機械結構要求高強度的剛度之間存在矛盾，然而可以運用合適的控制方法來控制機器人與作業環境之間的接觸力，能夠有效地解決這一矛盾。

在機器人的力控制中，根據其運動特性可以歸為四大類：阻抗控制策略、力/位混合控制策略、自適應控制策略和智能控制策略[1]。本文在工業機器人的力控制研究中采用阻抗控制的方法。阻抗控制算法是一種通過間接地方式去解決機械臂力/位置控制。通過控制機械手臂末端位置偏移量的方式間接的控制末端執行器與環境之間的接觸力。在力覺控制算法中，Hogan提出的阻抗控制占有重要的地位，可以很好地將運動軌跡和力控結合在一個動態系統中。這樣可以大大簡化位置和力控制的復雜控制策略，并且能夠消除環境變化和擾動帶來的影響。阻抗控制的特點是利用間接的方式對機器人與環境的作用力進行控制，根據位置（或速度）與機器人末端的作用力之間的關系，通過調整系統反饋回來的位置誤差、剛度或速度誤差，以此達到控制力的目的[2-4]。同時，接觸過程的彈性變形顯得特別重要。所以，狹義地稱為柔順性控制。這一類型的力控制主要有位置和速度2種基本形式。通過將力反饋信號轉換成位置調整量的方式，通常叫做剛度控制的力控制;通過將力反饋信號轉換成速度調整量的方式，通常叫做阻尼控制的力控制;通過將力反饋信號同時轉換為速度和位置兩者的修正量的方式，叫做阻抗控制[5]。

目前實際應用中，大多數用的都是速度環控制反饋和位置環控制反饋相結合的力控制，而阻抗控制在兩者基礎上加入了力反饋。通過將阻抗控制運用到機器人柔順控制中，可以改善傳統的末端位置不準確。同時，在一些對末端位置和末端力矩要求嚴格的實際應用中，提供了更好的控制方法。所以，采用阻抗控制的方法，可以有效地控制機器人的力矩，從而提高機器人末端的位置精度。

1 工業機器人結構及仿真模型

1.1 工業機器人結構

六自由度工業機器人由6個旋轉關節組成。其結構模型如圖1所示。

圖1 工業機器人結構Fig.1 Industrial robot structure

其中，大臂、前臂、小臂都為旋轉關節，且運動在同一個平面內，關節運動之間具有連續性、相關性等。為了方便研究，本文將六自由度的工業機器人簡化成二連桿機構，了解其運動特性，并對其進行動力學分析及仿真研究。

1.2 機器人動力學分析

二連桿機構示意如圖2所示。

圖2 機械臂示意Fig.2 Mechanical arm diagram

圖中，機械臂1的質量為m1，長度為l1，夾角為q1；機械臂 2 的質量為m2，長度為l2，夾角為q2。 （角度順時針為負逆時針為正）

如圖2所示，由數學幾何方法可以得機械臂2末端B位置的坐標為

由式（1）可以求出雅可比矩陣為

n自由度的機器人動力學方程為[6]

式中：D（q）是n×n為正定對稱矩陣；是n×1的離心力和哥氏力矢量；G（q）是n×1的重力矢量；q為關節位置；τ為關節驅動力矩。

由拉格朗日方程式得：

結合式（5）～式（7）可得出二自由度機械臂的慣性矩陣、離心力哥氏力矢量、重力矢量如下：

1.3 阻抗控制律分析

當處于受力空間是機械手臂受到環境的作用H（q，q˙）=C（q，q˙）q˙+G（q）力，令其動力學方程可以表示為

阻抗控制關系：

式（12）中，Fe是與環境接觸作用產生的真實力。M，B，K分別為機械手目標慣性矩陣、阻尼矩陣、剛度矩陣，E=Xe-X表示手臂目標位置和當前位置的位移。

為實現上述的目標控制，采用式（13）的阻抗控制律[7]：

由控制律公式（13）得到此算法的原理框圖如圖3所示。

圖3 阻抗控制算法的原理框圖Fig.3 Principle block diagram of impedance control algorithm

2 阻抗控制系統參數選取及仿真

2.1 搭建控制系統

根據圖3阻抗控制算法原理框圖搭建控制系統仿真平臺，輸入信號是正余弦信號和反饋回來的實際位置信號做比較，得到差值E。輸入信號經過微分處理和反饋回來的速度信號做差得到速度差值E˙。分別通過增益環節乘以M-1K，M-1B，再加上Xd的二階導數和-J˙θ˙，再通過S函數，在S函數中乘以D（θ）J-1計算出扭矩，再加上離心力、哥氏力、重力引起的關節力矩h（θ，θ˙），從而求得加載在機器人關節上的扭矩 τ＝［τ1，τ2］。作為 ROBOT 的輸入信號，調用S函數進行動力學逆解，求出相應的角度、角速度反饋給輸入信號。在Simulink仿真平臺搭建控制系統如圖4所示，仿真中采用定步長，Runge（ode4）算法[8-10]。

圖4 阻抗控制算法仿真平臺Fig.4 Simulation platform of impedance control algorithm

仿真平臺ROBOT控制部分主要程序如下：

2.2 參數的選取以及仿真結果

將正弦信號同時接入到連桿2末端位置的XY方向上，為了簡便接下來的仿真過程，只需考慮X方向上輸出的跟蹤信號，分析輸出信號的響應曲線，就可以調整出相應的慣性系數，阻尼系數，剛度系數。時間從0～3 s，仿真步長設置為固定0.001 s[11]。

通過以上方法得出一組最佳系數如下：

M=I（I為單位矩陣）；

設l1=1 m，質量m1=1 kg；l2=1 m，質量m2=1 kg。兩個關節的角位置變量分別是（π/3，π/6），兩個關節的速度（0，0）均為零，它的末端位置P（0.866，1.5）。以下的仿真實驗中選取慣性系數值M=1，阻尼系數值B=50，剛度系數值K=615。假設沒有與外界環境作用，機械手作用力0，現在分別在機械手末端輸入信號X=cost；Y=sint，XY的末端軌跡是以（0，0）為圓心，半徑為1的圓。仿真步長設置為0.001 s，時間10 s。機械臂末端X、Y以及XY的軌跡圖和XY的誤差分別為圖5～圖8所示。

圖5 X方向上的跟蹤情況Fig.5 X direction tracking

圖6 Y方向上的跟蹤情況Fig.6 Y direction tracking

圖7 X/Y的誤差分析Fig.7 Error analysis of X/Y

圖8 末端的運動軌跡Fig.8 Terminal trajectory

由圖5、圖6可知，在X和Y方向上，分別在0.263 s，0.356 s已經可以跟蹤上給定的信號軌跡，隨后X、Y方向的誤差幾乎接近0，說明阻抗控制算法在消除機器人手臂末端位置的誤差有顯著的作用，能夠提高位置控制精度。

3 結語

通過在Matlab/Simulink環境下進行阻抗控制算法的機器人力控制仿真，借助Simulink強大的S-Function進行控制程序編寫，實現阻抗控制算法的仿真實驗。從以上仿真結果可知，在X和Y方向上，都在短時間內跟蹤上期望的信號軌跡。相比傳統的力控制可以保證更高的精度，從而得出阻抗控制算法可以有效的消除控制誤差，在機器人力控制上的應用可以提高機器人末端的位置精度。以后可以延伸到三自由度甚至更多自由的機器人，同時也可以通過Matlab/Simulink進行自適應控制，模糊控制等方法的仿真實驗。