Hệ thống điều khiển trong không gian khớp- 123docz.net

Tín hiệu đặt đó là quỹ đạo bậc 3 của các biến khớp Hệ thống điều khiển phản hồi không bù G(q)

Luật điều khiển:

Hình 6.1 Sơ đồ cấu trúc điều khiển Robot với bộ điều khiển PD

LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com

Ta có phương trình động lực học : diag K ( p1, Kp2... Kpn)

Gọi H(q) =M(q) tránh nhầm với M la vector momen và V( , )q q = ( , )C q q q, M=U tiến đến ta coi Robot không chịu tác dụng của ngoại lực vì luật điều khiển bám quỹ đạo F=0 như vậy phương trình động lực học được rút gọn như sau :

M= H(q) q+ V( , )q q + G(q) (1)

Luật điều khiển : Mdk  K qp( d   q ) K qd( d  q )  Kp  Kd

Trong đó : Kp = diag K ( p1, Kp2... Kpn) ma trận đường chéo các hệ số khuyếch đại của từng khớp riêng biệt .

Với luật điều khiển này đã giả thiết thành phần mômen trọng lực G(p) đã được bù hoàn toàn .

Hệ thống điều khiển với cấu trúc bộ điều khiển như trên, ổn định tuyệt đối toàn cục. Thực vậy chọn hàm Liapunov có dạng như sau :

1( )

T T

VL   Kp q Hq (2)

Hàm VL biểu thị tổng năng lượng của hệ thống robot . Thành phần chứa Kp tỷ lệ với năng lượng đầu vào , thành phần sau là động năng của robot mà Kp và H là các ma trận có hệ số dương . Nên hàm VL>0 với q khác qd

Tính đạo hàm cấp 1 của VL ta nhận được :

1 ( )

T T T T T

VL   Kp    Kp   q Hq  q Hq  q Hq

Do tính chất đối xứng của các thành phần TKp  và q HqT ta tính được

T 1 T T

V   Kp   q Hq  q Hq

Page 49 Từ phương trình động lực học với giả thiêt không có thành phần momen trọng lực G(q) , ta nhận được phương trình sau :

 

1 ( , )

T T T

VL   Kp   q Hq  q M  V q q

Sử dụng thuộc tính của phương trình động lực học và áp dụng luật điều khiển (1) ta có :

1 1

( , ) ( ) d + ( )

2 2

T T T T T

VL  q Kd   q c q q q  q H q q   q K  q H  c q

Trong đó : V( , )q q = ( , )C q q q Do ma trận 1

2 H  c là ma trận đối xứng ngược nên - 1

( )

qT H  c q =0

Suy ra VL   q KT d  (3)

Từ 5.2 và 5.3 cho thấy rằng , mức độ dương của VL phụ thuộc vào Kp , mức độ âm của VL phụ thuộc vào Kd .Do đó tăng tốc độ hội tụ bằng tăng giá trị Kp . Nâng cao độ chính xác tinh của hệ thống đạt được bằng tăng hệ số Kp của khâu khuếch đại . Tuy nhiên , Kp và Kd quá lớn sẽ làm giảm độ ổn định và chất lượng quá trình quá độ như độ quá điều chỉnh , thời gian quá độ tăng .

Ta đã thiết lập được hệ phương trình vi phân động lực học đó là mô hình toán học của Robot hàn điểm mà ta đang thiết lập, mô phỏng điều khiển ta sử dụng phương trình vi phân động lực học là đối tương điều khiển .

Quỹ đạo đặt là quỹ đạo bậc 3 các biến khớp với điều khiển ĐIỂM-ĐIỂM . ta chọn 2 điểm A(x0,y0,z0) ,B(xc,yc, xc) bất kì trong không gian làm việc .Từ phương trình động học ngược ta tính ở phần trên ta xác định được góc khớp tại hai điểm

 Code chương trình mô phòng bằng Open GL

LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com

//DrawModel.h - includes functions to render objects and supporting functions

//Author: NGUYEN DINH DINH

#ifndef _DRAW_MODEL_H_

#define _DRAW_MODEL_H_

#include "GL/Glut.H"

#include "STLModel.h"

#include "Materials.h"

#include "Lights.h"

#include "Trajectory.h"

#include "DrawText.h"

#ifndef PI

#define PI 3.1415926535897932384626433832795

#endif /*PI*/

void CreateObjects();

void RenderObjects(double X, double Y, double Z, double angX, double angY, double angZ, double Scale = 1);

//implement code here

//parameters manage graphical objects

Page 51 //Parameters for model

double H0, L1, L2, q1, q2, q3, d1 ,a1, a2, a3;

//Trajector

double xt, yt, zt, t, stept;

int traj_size = 100;

CTrajectory traj;

void SolveInverseKinematic() {

xt = 50+20*sin(t);

yt = 50+15*cos(t);

zt = 50 + 25*sin(3*t);

if (2*6.28<t) {

xt=15*16*sin(t)*sin(t)*sin(t);yt=500+15*(13*cos(t) -5*cos(2*t)- 2*cos(3*t)-cos(4*t));zt=500;

}

if (6.28+12.56<t) {

xt=10*16*sin(t)*sin(t)*sin(t);yt=500+10*(13*cos(t) -5*cos(2*t)- 2*cos(3*t)-cos(4*t));zt=500;

}

if (6.28+18.84<t) {

LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com

xt=5*16*sin(t)*sin(t)*sin(t);yt=500+5*(13*cos(t)-5*cos(2*t)-2*cos(3*t)- cos(4*t));zt=500;

}

if (6.28+25.12<t) {

xt=2.5*16*sin(t)*sin(t)*sin(t);yt=500+2.5*(13*cos(t)-5*cos(2*t)- 2*cos(3*t)-cos(4*t));zt=500;

}

if (6.28+31.4<t) {

zt=600+60*cos(t)*sin(5*t) ;xt= 50*sin(5*t);yt= -600-60*sin(t)*sin(5*t);

}

q1=atan2(yt,xt);

q2=acos((xt*xt+yt*yt+(zt-d1)*(zt-d1)+a2*a2-

a3*a3)/(2*a2*sqrt(xt*xt+yt*yt+(zt-d1)*(zt-d1))))-atan2(d1- zt,sqrt(xt*xt+yt*yt));

q3=PI+acos((-xt*xt-yt*yt-(zt-d1)*(zt-d1)+a2*a2+a3*a3)/(2*a2*a3));

traj.add_point(xt, yt, zt);

}

void NextStep() {

Page 53 t += stept;

SolveInverseKinematic();

}

void DeleteObjects() {

if (glIsList(de)) ////SUA glDeleteLists(de, 1);

if (glIsList(khau1))

glDeleteLists(khau1, 1);

if (glIsList(khau2))

glDeleteLists(khau2, 1);

if (glIsList(khau3))

glDeleteLists(khau3, 1);

}

void CreateObjects() {

glEnable(GL_DEPTH_TEST);

DeleteObjects();

///SUA ReadModel("STL/De.STL", de);

ReadModel("STL/Khau1.STL", khau1);

LUAN VAN CHAT LUONG download : add luanvanchat@agmail.com

ReadModel("STL/Khau2.STL", khau2);

ReadModel("STL/Khau3.STL", khau3);

H0 = 100;

a1 = 155; a2 = 500; a3=400;

d1 = 130;

q1 = q2 = q3=0;

t = 0;

stept = 0.05;

traj_size = 1000;

traj.set_size(traj_size);

SolveInverseKinematic();

}

void DrawNotes(double X, double Y, double Z, double projection_scale = 1, double line_space = 1.15)

{

double space = 0, s = projection_scale*line_space;

void * font = GLUT_BITMAP_9_BY_15;

X += glutBitmapWidth(font, 'H');

space += projection_scale*glutBitmapHeight(font);

Page 55 space += 0.2*projection_scale*glutBitmapHeight(font);

DrawText("___________", X, Y-space, Z, 1, 0, 0, 1, font);

font = GLUT_BITMAP_9_BY_15;