機械臂控制系統仿真實驗設計

趙海濱， 于清文， 劉 沖， 陸志國， 顏世玉

(東北大學 機械工程與自動化學院， 沈陽 110819)

0 引 言

機器人是一門跨專業，高度綜合的新興學科[1]，無論在基礎理論方面還是在實踐應用方面發展速度都非常快。越來越多的高等學校面向高年級本科生和低年級研究生開設機器人學等機器人領域的相關課程，并對課程的改革進行了研究[2-4]。機器人課程的教學包括理論教學和實驗教學[5-7]。由于資金和設備等原因，各個高校往往以理論教學為主，對實驗教學不夠重視。在機器人課程的理論教學中存在大量的公式推導和微分方程等，對于學生比較抽象、復雜和難以理解。因此，采用Matlab/Simulink軟件進行機器人的仿真和控制實驗非常必要。

在機器人的教材中，機器人動力學部分的公式推導均以兩自由度機械臂為例，本文也以兩自由度機械臂為研究對象。根據機械臂的動力學方程，采用Matlab/Simulink軟件建立機械臂仿真模型，采用五階多項式進行軌跡規劃，并采用逆動力學進行機械臂的控制。通過機械臂的運動學方程獲取關節的位置坐標，并動態的顯示機械臂的運動過程。最后，對機械臂控制系統進行動態仿真，并對仿真結果進行了分析。學生可以修改參數，然后進行動態仿真。通過仿真實驗，向學生完整的展示機械臂的控制過程，能夠加深學生對機器人動力學、運動學和控制等理論的理解，增強學生的編程能力和學習興趣，有助于機器人課程的理論和實驗教學。

1 機械臂

本文以兩自由度機械臂為研究對象，通過拉格朗日方程建立動力學方程[1]為

(1)

(2)

在動力學方程中，機械臂的參數[8-9]如下：

c12=2c11

c21=0

g1=(m1+m2)gr1cos(q2)+m2gr2cos(q1+q2)

g2=m2gr2cos(q1+q2)

式中：m1和m2分別為兩個連桿的質量；r1和r2分別為兩個連桿的長度；J1和J2分別為兩個連桿的轉動慣量；g為重力加速度。

2 控制系統

本文以兩自由度機械臂為研究對象，采用Matlab/Simulink軟件建立綜合性仿真實驗平臺。根據動力學方程建立機械臂的模型，然后采用軌跡規劃和逆動力學控制進行機械臂的控制，最后通過運動學方程將機械臂在關節空間的坐標轉換為工作空間下的位置坐標，并將機械臂的運動通過動畫的形式進行顯示。機械臂控制仿真系統如圖1所示。

機械臂的控制方法有PD控制、帶重力補償的PD控制、逆動力學控制、魯棒控制和自適應控制等。本文采用常用的逆動力學控制方法。機械臂的軌跡規劃采用點到點運動方法，常用的有3階多項式軌跡規劃、5階多項式軌跡規劃、帶有拋物線的直線段方法和開關(bang-bang)方法等。本文采用5階多項式軌跡規劃方法。

圖1 機械臂控制系統原理圖

2.1 軌跡規劃

軌跡規劃采用點到點運動， 即規劃一條從初始位置q(t0)到最終位置q(tf)的軌跡。本文采用5階多項式軌跡規劃方法[10]。在t0時刻的角度、角速度和角加速度分別滿足：

(3)

在tf時刻的角度、角速度和角加速度分別滿足：

(4)

采用如下的5階多項式軌跡，

q(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(5)

根據起始位置和終點位置的角度、角速度和角加速度，可以得到下面的方程組：

(6)

通過求解該方程組可以得到參數ai(i=0,1,…,5)。然后，可以得到期望角度、角速度和角加速度分別為：

(7)

2.2 逆動力學控制

根據機械臂的動力學方程(1)，選擇控制輸入為

(8)

通過比較式(1)和式(8)可以得到

(9)

穩定的控制律y可以選擇為

(10)

期望軌跡為qd(t)，為了跟蹤該軌跡，選擇r為

(11)

將式(11)代入到式(10)，然后再代入到式(9)，可以得到二階微分方程

(12)

2.3 運動學方程

正運動學是根據機械臂各個關節的角度值，來確定工作空間下的位置坐標。對于具有兩個自由度的機械臂，q1和q2分別為兩個關節的角度，r1和r2分別為兩個關節的長度，則第1個關節末端的位置坐標x1,y1為

(13)

第2個關節末端的位置坐標x2,y2為

(14)

3 仿真實驗

Matlab功能強大、使用簡單方便，并且對問題的描述和求解符合人們的思維習慣和數學表達習慣，已經廣泛應用于動態系統仿真[11]。Simulink是Matlab軟件最重要的組件之一，能夠進行系統建模、仿真和綜合分析等[12]。Simulink和外界硬件的接口還可以建立半實物仿真和實時控制實驗[13]。在Simulink系統中用戶自定義函數庫中的Matlab Function模塊，可以采用Matlab語言非常方便靈活的建立復雜系統[14]。Matlab/Simulink軟件非常適合進行機器人的控制和仿真實驗[15-16]，因此本文采用Matlab/Simulink軟件建立仿真實驗系統。

兩自由度機械臂的參數選擇為：m1=0.5 kg，m2=1.5 kg，r1=1 m，r2=0.8 m，J1=5 kg·m，J2=5 kg·m，g=9.81 m/s2。在逆動力學控制中參數Kp=diag(9,9)，Kd=diag(6,6)。

采用采用Matlab/Simulink軟件建立機械臂控制系統仿真實驗平臺，如圖2所示。系統采用變步長的ode45算法，最大仿真步長為1 ms，仿真時間為4 s。在圖2中主要采用了Simulink中用戶自定義功能模塊庫中的Matlab Function模塊和積分模塊等。通過常量模塊Constant設置機械臂的初始角度和角速度。通過To Workspace模塊將運行結果保存在工作空間中。 機械臂兩個關節的期望角度分別采用5階多項式進行軌跡規劃。關節1和關節2的初始角度q1和q2分別為π/10和π/8，目標位置的角度分別為3π/4和π/2。關節1和關節2的初始角速度和目標位置的角速度均為0，初始角加速度和目標位置的角加速度也均為0。

通過軌跡規劃得到的期望角度、角速度和角加速度，以及機械臂的角度和角速度，采用式(8)進行逆動力學控制。在圖2中，逆動力學控制模塊內的代碼為

function tau = fcn(dq,q,qd,dqd,ddqd)

q1=q(1);q2=q(2);dq1=dq(1);dq2=dq(2);

m1=0.5;m2=1.5;r1=1;r2=0.8;g=9.81;J1=5;

J2=5;

d11=(m1+m2)*r1^2+m2*r2^2+2*m2*r1*r2*

cos(q2)+J1;

d12=m2*r2^2 +m2*r1*r2*cos(q2);

d21=d12;d22=m2*r2^2 +J2;

D=[d11,d12;d21,d22];

c11=m2*r1*r2*sin(q2);

圖2 機械臂控制系統仿真實驗平臺

C=[-c11*dq1,-2*c11*dq1; 0,c11*dq2];

g1=(m1+m2)*g*r1*cos(q2)+m2*g*r2*cos(q1+q2);

g2=m2*g*r2*cos(q1+q2);

G=[g1;g2];

w=3;e=q-qd;de=dq-dqd;

Kp=diag([w^2,w^2]);Kd=diag([2*w, 2*w]);

y=ddqd-Kd*de-Kp*e;

tau=D*y+C*dq+G;

機械臂控制系統運行后，機械臂的角度如圖3所示。關節1的角度q1從初始位置π/10運動到目標位置3π/4，關節2的角度q2從初始位置π/8運動到目標位置π/2。

圖3 機械臂的角度

機械臂的角速度如圖4所示。在圖4中，關節1的角速度初始值為0，然后速度逐漸增大，在第2 s達到最大值為0.957 rad/s，最后逐漸減小到0。關節2的角速度初始值為0，然后逐漸增大，并在第2 s達到最大值0.552 rad/s，最后逐漸減小到0。

圖4 機械臂的角速度

機械臂的驅動力矩如圖5所示。在圖5中，關節1的驅動力矩在0.605 s達到最大值33.83 Nm，在0.306 s達到最小值-15.36 Nm。關節2的驅動力矩在0.563 s達到最大值12.04 Nm，在0.275 s達到最小值-14.20 Nm。

通過機械臂的運動學方程，可以得到兩個關節的位置坐標。在關節空間中，通過逆動力學控制將機械臂從初始位置移動到目標位置。通過運動學方程，可以得到機械臂第一個關節初始位置的坐標為cos(π/10),sin(π/10)=0.951,0.309。機械臂第2個關節初始位置的橫坐標為cos(π/10)+0.8cos(π/10+π/8)=1.56

圖5 機械臂的驅動力矩

縱坐標為

sin(π/10)+0.8sin(π/10+π/8)=0.829

在工作空間中機械臂的運動軌跡如圖6所示。在圖6中，每間隔0.2 s繪制機械臂的位置，動態的顯示機械臂的運動過程。

圖6 機械臂的運動軌跡

4 結 語

通過Matlab/Simulink軟件建立了兩自由度機械臂控制系統仿真實驗平臺，能夠進行機械臂的點到點控制，并動態的顯示機械臂的運動過程。通過仿真實驗，學生可以對機械臂的整個控制系統有一個完整的了解，還可以自己編寫控制算法。通過機械臂控制系統的仿真實驗，有助于學生對機器人的基本理論的理解，能夠增強學生的實際編程能力，激發學生的學習興趣，有助于機器人課程的理論和實驗教學。