Báo cáo lập trình mô phỏng robot

1
LỜI NÓI ĐẦU
Thế kỷ 21 là thế kỷ của khoa học kỹ thuật hiện đại và tiên tiến, nó đi sâu vào toàn
bộ đời sống của con ngƣời, giúp con ngƣời trong mọi lĩnh vực, nó thay đổi cơ bắp con
ngƣời từ những công việc đơn giản đến phức tạp, trong đó có ngành Cơ Điện Tử.
Sự đi lên của ngành cơ khí gắn liền với sự ứng dụng công nghệ mới. Để đáp ứng
nhu cầu ngày càng cao của xã hội và tiếp cận nền khoa học kỹ thuật trong tƣơng lai,
thì con ngƣời đã tạo ra robot. Đó là sự thay đổi lực lƣợng sản xuất trong mọi ngành
nghề bằng việc thay đổi sức lao động của ngƣời bằng máy móc, nhằm đảm bảo tăng
năng suất lao động, sản lƣợng cũng nhƣ chất lƣợng sản phẩm. Do đó việc sử dụng các
tay máy hay còn gọi là robot công nghiệp vào trong sản xuất đang đƣợc ƣa chuộng vì
chúng đáp ứng đƣợc yêu cầu trên. Nhƣ chúng ta đã biết, robot công nghiệp có rất
nhiều ƣu điểm đặc biệt là chất lƣợng và độ chính xác, ngoài ra còn kể đến hiệu quả và
kinh tế cao, có thể làm việc trong môi trƣờng độc hại mà con ngƣời không làm việc
đƣợc, các công việc yêu cầu cẩn thận không đƣợc nhầm lẫn, thao tác nhẹ nhàng tinh tế
đòi hỏi trình độ của thợ bậc cao, và đặc biệt là robot không bị căng thẳng nhƣ con
ngƣời nên có thể làm việc liên tục không ngừng nghỉ.
Việc tìm hiểu nghiên cứu mô phỏng robot trong khuôn khổ môn học Lập trình mô
phỏng các hệ cơ điện tử sẽ là cơ sở để chúng em tính toán, thiết kế robot trong công
nghiệp phục vụ sản xuất. Cụ thể ở đây chúng em chọn đề tài mô phỏng hoạt động của
Robot viết chữ.Em xin cảm ơn thầy Đỗ Đăng Khoa đã hƣớng dẫn chúng em để chúng
em có thể hoàn thành project này một cách tốt nhất.

Hà Nội, ngày 10 tháng 6 năm 2017
Nhóm sinh viên thực hiện
1. Nguyễn Đức Tiến
2. Phạm Ngoc Tùng
3. Vũ Nhật Hiếu

1

2

BẢNG PHÂN CÔNG NHIỆM VỤ VÀ ĐÁNH GIÁ CÁC THÀNH VIÊN
 Phân công nhiệm vụ cho báo cáo trong quá trình học:
STT

Họ và tên

Phân công nhiệm vụ

1

Vũ Nhật Hiếu

Tìm hiểu cách tạo file .*STL và lớp CElement

2

Nguyễn Đức Tiến

Tìm hiểu lớp CRobot và làm slide báo cáo

3

Phạm Ngọc Tùng

Tìm hiểu cách mở file .*STL vào OpenGL và lớp
CModel

 Phân công nhiệm vụ cho project cuối kỳ:
STT

Họ tên

Phân công nhiệm vụ

1

Vũ Nhật Hiếu

Giải bài toán động học

Đánh giá của giáo viên

ngƣợc, động học thuận,
thiết kế quỹ đạo
2

Nguyễn Đức Tiến

Xử lí file Solid xuất bản vẽ,
làm báo cáo,slide,thuyết
trình

3

Phạm Ngọc Tùng

Code chính, thiết kế đồ họa

và môi trƣờng mô phỏng
OpenGL

2


3

Mục lục
LỜI NÓI ĐẦU .................................................................................................................... 1
CHƢƠNG 1: TỔNG QUAN ............................................................................................. 5
1.1 Giới thiệu chung ...................................................................................................... 5
1.2 Giới thiệu về dự án .................................................................................................. 6
1.3 Yêu cầu bài toán ...................................................................................................... 6
1.4 Ứng dụng của robot ................................................................................................. 6
CHƢƠNG 2: MÔ HÌNH HOÁ ROBOT 3DOF .............................................................. 8
2.1

Phân tích mô hình robot ...................................................................................... 8

2.2

Bản vẽ chi tiết và các tính chất cơ học khác của các khâu Robot ................... 8

2.3

Mô hình ROBOT................................................................................................ 13

CHƢƠNG 3: TÍNH TOÁN VÀ THIẾT KẾ ROBOT .................................................. 14
3.1 Mô hình Robot đơn giản ....................................................................................... 14

3.2. Bản D-H và các ma trận biến đổi toạ độ thuần nhất......................................... 14
3.3. Bài toán động học thuận ...................................................................................... 16
3.3.1 Xây dựng quy luật chuyển động thao tác của robot theo mục đích ứng
dụng 17
3.4 Bài toán động học ngƣợc ....................................................................................... 18
3.5

Thiết kế quỹ đạo ................................................................................................. 19

CHƢƠNG 4: XÂY DỰNG CHƢƠNG TRÌNH MÔ PHỎNG.................................... 22
4.1 Giới thiệu về môi trƣờng mô phỏng OPENGL ................................................... 22
4.2 Xây dựng bài toán mô phỏng ............................................................................... 22
4.3 Làm việc với file .*STL ......................................................................................... 31
3


4
4.4 Nhúng file *.STL vào môi trƣờng OPENGL ...................................................... 37
4.4 Các lớp có trong chƣơng trình mô phỏng ........................................................... 42
4.5. Thiết kế Dialog ...................................................................................................... 44
KẾT LUẬN ...................................................................................................................... 49
TÀI LIỆU THAM KHẢO............................................................................................... 50

4


5

CHƢƠNG 1: TỔNG QUAN
1.1 Giới thiệu chung

Ngày nay, robot công nghiệp có vai trò rất quan trọng và đƣợc ứng dụng rộng rãi
trong quá trình sản xuất của nền công nghiệp hiện đại. Để chế tạo ra một mô hình
robot và đem ra phục vụ sản xuất phải trải qua rất nhiều giai đoạn nhƣ: Tìm hiểu về
các loại robot và các đặc tính hoạt động của nó, tính toán một cách chính xác các khâu
của các cơ cấu, các phƣơng trình hoạt động và thiết kế các mô hình 3D sao cho tối ƣu
và hiệu quả nhất. Một mô hình robot hoạt động trơn tru và đáp ứng đƣợc các yêu cầu
kỹ thuật đề ra đòi hỏi việc chế tạo là hết sức tỉ mỉ và chi tiết từng bƣớc một. Trong một
mô hình robot bao gồm rất nhiều phần nhƣ Cơ khí để chế tạo ra các chi tiết, điện tử để
điều khiển hoạt động và tin học để lập trình cho các cơ cấu hoạt động theo các quỹ đạo
và yêu cầu kỹ thuật đề ra. Một phần cũng rất quan trọng mà mô hình nào cũng phải
thực hiện đó là mô phỏng quá trình hoạt động của nó.
Sau khi hoàn thiện mô hình robot thì mô phỏng chính là kiểm tra trên máy tính
xem mô hình có hoạt động tốt hay không, có thực hiện đƣợc các yêu cầu đề ra hay
không, sau đó đƣa ra vào các thông số đã tính toán, giả lập quá trình hoạt động của đối
tƣợng một cách trực quan nhất. Trên cơ sở đó có thể kiểm nghiệm đƣợc quá trình hoạt
động của cơ cấu có đúng theo kết quả tính toán và thiết kế hay không. Mặt khác cũng
có thể thay đổi một cách linh hoạt các số liệu, các thông số ban đầu để tìm ra lời giải
cho bài toán tối ƣu cơ cấu. Mô phỏng còn đóng vai trò rất lớn trong việc giảng dạy,
báo cáo, truyền đạt thông tin về sản phẩm cho mọi ngƣời. Giúp cho nhà thiết kế có cái
nhìn tổng quan về sản phẩm để từ đó có thể cải tiến sản phẩm, hoặc đƣa ra quá trình
sản xuất đơn giản, tối ƣu cho sản phẩm. Vì vậy, có thể thấy mô phỏng là khâu rất quan
trọng, không thể thiếu trong quá trình sản xuất robot công nghiệp
Trong lĩnh vực cơ khí có rất nhiều phần mềm nổi tiếng nhƣ: AutoCAD, Inventor,
SolidWorks, Catia ... Dùng để tính toán, thiết kế và mô phỏng , kiểm tra các điều kiện
làm việc của các chi tiết. Tuy nhiên, các phần mềm này còn bị hạn chế khi cần mô
phỏng kết cấu hoạt động theo một quy trình tính toán thiết kế cụ thể ví dụ nhƣ hoạt
5


6

động của robot theo 1 quỹ đạo cụ thể. Khi đó, để có thể chủ động , linh hoạt trong quá
trình tính toán và thiết kế thì đơn giản và tiện lợi là lập trình mô phỏng, qua đó ta có
thể xây dựng và đáp ứng đúng với nội dung và mục đích thiết kế.
1.2 Giới thiệu về dự án
Để lập trình mô phỏng đƣợc hoạt động của robot thì bọn em đã sử dụng Visual
C++ vào thƣ viện đồ họa OpenGL để xấy dựng đƣợc bài toán mô phỏng phù hợp với
yêu cầu. Cụ thể ở đây chúng em đã lựa chọn robot viết chữ có 3 bậc tự do, có các ƣu
điểm nhƣ sau:
- Tiết kiệm diện tích khâu đế.
- Dễ dàng thực hiện với đối tƣợng nằm trong mặt phẳng nằm ngang.
- Kết câu đơn giản nhƣng vẫn đảm bảo tính linh hoạt.
1.3 Yêu cầu bài toán
Yêu cầu vị trí: Robot thao tác trong không gian đúng với quỹ đạo thao tác đặt ra.
Yêu cầu về hướng:Hƣớng của đầu hàn tạo với mặt của chi tiết những góc theo yêu cầu
Với cấu trúc robot có 4 bậc tự do trở nên thao tác linh hoạt, tuy nhiên việc tính
toán sẽ trở lên khó khăn, phức tạp hơn. Do đó nếu chọn các cấu trúc robot này sẽ làm
phức tạp quá trình tính toán, gây tốn kém.
Để đủ đảm bảo yêu cầu bài toán đặt ra ta chọn cơ cấu robot có 3 bậc tự dođể tiến
hành quá trình tính toán và thiết kế.
Phần mềm sử dụng là Visual Studio và môi trƣờng đồ hoạ OpenGL. Yêu cầu đặt ra
phải mô phỏng chính xác chuyển động của Robot thông qua việc giải quyết các bài
toán động học thuận và động học ngƣợc, ứng dụng các kiến thức đã đƣợc học trong
môn học Robotics và Lập trình mô phỏng Robot và các hệ trong cơ điện tử.
1.4 Ứng dụng của robot
Robot có thể phát triển để đƣa vào các ngành công nghiệp sản xuất nhằm góp
phần nâng cao năng suất dây truyền công nghệ, giảm giá thành, nâng cao chất lƣợng
và khả năng cạnh tranh của sản phẩm, đồng thời cải thiện điều kiện lao động. Robot
6

7
cũng có thể thay thế cho con ngƣời trong các công việc độc hại, nguy hiểm hay môi
trƣờng khắc nghiệt. Đặc biệt robot có thể làm việc với độ chính xác cao.

7


8
CHƢƠNG 2: MÔ HÌNH HOÁ ROBOT 3DOF
2.1 Phân tích mô hình robot
Số bậc tự do cần thiết:
Để đảm bảo robot có khả năng đƣa đầu chạm tới mọi vị trí trong không gian

-

làm việc thì robot cần từ 3 bậc tự do trở lên.
-

Các khâu có trong mô hình robot:

Khâu

Chuyển động

Chức năng

0

Cố định

Dùng để gá đặt ,định vị robot và liên kết với khâu 1

1

Quay

Liên kết với khâu 0 và khâu 2 , tạo ra chuyển động
quay cho khâu 2

2

Quay

Liên kết với khâu 1 và khâu 2 ,tạo ra chuyển động
tịnh tiến cho khâu 3

3

Tịnh Tiến

Liên kết với khâu 2, và giúp xác định điểm đầu bút

2.2 Bản vẽ chi tiết và các tính chất cơ học khác của các khâu Robot
2.2.2 Bản vẽ chi tiết các khâu
-

Khâu 0:

8

9

-

Khâu 1:

-

Khâu 2 :

9


10
-

Khâu 3:

2.2.2 Tính chất cơ học của các khâu robot
 Chọn vật liệu, khối lƣợng và momen của từng khâu:
-

Chọn vật liệu các khâu là thép cacbon thƣờng (plain carbon steel)

 Tính chất cơ học các khâu:

10

11
-

Khâu 0:

Hình 1: Tính chất cơ học của khâu 0

-

Khâu 1:

Hình 2: Tính chất cơ học của khâu 1
11


12
-

Khâu 2:

Hình 3: Tính chất cơ học của khâu 2
-

Khâu 3:

Hình 4: Tính chất cơ học của khâu 3
12


13

2.3 Mô hình ROBOT

13


14
CHƢƠNG 3: TÍNH TOÁN VÀ THIẾT KẾ ROBOT
3.1 Mô hình Robot đơn giản

z2
x2

a2
c3

a1

d3
Ø2

z0

z3

z1

d1

x3

x1

y0
x0
Ø1

x'1
Hình 5: Sơ đồ động học
3.2. Bản D-H và các ma trận biến đổi toạ độ thuần nhất
Bảng thông số DH:

Joint

i

di

ai

1

1

d1

a1

2

2

0

a2

3

0

d3

0

i

0

Bảng 6: Bảng tham số D - H

14


15
Các đặc trƣng kỹ thuật:

a1 (l1 )

a2 (l2 )

d1

( m)

( m)

(m)

0,018

0,020

0,022

q3

q1

q2

d3

1

2

Các ma trận biến đổi tọa độ thuần nhất:

cos 1 01 sin 1 a1 cos 1 
 sin  0  cos  a sin  
1

1
1
1
0
A1  
 0
1
0
d1 


0
0
1 
 0
1
0
2
A3  
0

0

0
1
0
0

cos  2
 sin 

2
1
A2  
 0

 0

0  sin  2
0 cos  2
1
0
0
0

a2 cos  2 
a2 sin  2 
0 

1 

0 0
0 0 
1 d3 

0 1

cos 1 cos  2
 sin  cos 
1
2

0
A2  
 sin  2

0


 sin 1
cos 1
0
0

 cos 1 sin  2
 sin 1 sin  2
cos 2
0

a1 cos 1  a2 cos 1 cos  2 ) 
a1 sin 1  a2 sin 1 cos  2 

d1  a2 sin  2

1


Chƣơng trình xuất ma trận DH trong Matple

15

16

Kết quả

 cos 1cos 2
sin  cos 
1
2
0
A3  
 sin  2

0


 sin 1
cos 1
0
0

cos1 sin  2
 sin 1 sin  2
cos  2
0

a1 cos 1  a2 cos 1 cos  2  d 3cos 1 sin  2 ) 
a1 sin 1  a2 sin 1 cos  2  d3 sin 1 sin  2 

d1  d3 cos  2  a2 sin  2


1


3.3. Bài toán động học thuận
-

Vận tốc điểm tác động cuối E:

Tọa độ điểm tác động cuối E:

 xE  a1 cos1  a2 cos1 cos  2  d3 cos 1 sin  2

 yE  a1 sin 1  a2 sin 1 cos 2  d3 sin 1 sin  2
 z  d  d cos  a sin 
1
3
2
2
2
 E
Vận tốc điểm tác động cuối:
16


17
 d3 (1 sin 1 sin  2   2 cos 1 cos  2 )  cos 1 sin  2 d3  a11 sin 1  a2 (1 sin 1 cos  2   2 cos 1 sin  2 ) 


vE  rE   d3 (1 cos 1 sin  2   2 sin 1 cos  2 )  sin 1 sin  2 d3  a11 cos 1  a2 (1 cos 1 cos  2   2 sin 1 sin  2 ) 



d3 cos  2  a2 2 cos  2  d3 2 sin  2


=
 d3 sin 1 sin  2  a2 sin 1 cos  2  a1 sin 1
 d cos  sin   a cos   a cos  cos 
1
2
1
1
2
1
2
 3
0


-

(d3 cos 1 cos  2  a2 cos 1 sin  2 )  cos 1 sin  2  1 
 
(d3 sin 1 cos  2  a2 sin 1 sin  2 )  sin 1 sin  2   2 
 
a2 cos  2  d3 sin  2
cos  2
  d3 

Vận tốc góc khâu thao tác:

Ma trận quay:

 cos 1cos 2
0
R3  sin 1 cos  2
 sin  2

 sin 1
cos 1
0

cos1 sin  2 
 sin 1 sin  2 

cos  2

Ta có các ma trận:

0

0

 cos 1cos 2
R    sin 1
cos1 sin  2
T
3

sin 1 cos  2
cos 1

 sin 1 sin  2

sin  2 
0 
cos  2 

  sin 11 cos  2  cos 1 2 sin  2

R3   cos 11 cos  2  sin 1 2 sin  2

cos  2 2


-

1 cos 1
1 sin 1
0

sin 11 sin  2  cos 1 cos  2 2 

 cos 11 sin  2  sin 1 cos  2 2 

 sin  2 2


 0
1
 2 cos1 



0
 2 sin 1 
Vận tốc góc khâu thao tác:   R3 R   1
 2 cos1  2 sin 1

0


T
3

 x    2 sin 1 


    y     2 sin 1 

 z   1


3.3.1 Xây dựng quy luật chuyển động thao tác của robot theo mục đích ứng dụng

17


18
-

Không gian làm việc của robot
Giả sử cho giới hạn chuyển động của các khâu tại các khớp:

1  1800  1800

0
0
 2  90  90
d  0,1  0.1
 3

Hình 7: Đƣờng quỹ đạo trong không gian làm việc của robot

3.4 Bài toán động học ngƣợc

Ta có :

p  xE

yE

zE

  

p   x E

yE

zE

  

T

p   x E

yE

zE

  

T

T

18


19

 c11 ( ,  , ) c12 ( ,  , ) c13 ( ,  , )
c ( ,  , ) c ( ,  , ) c ( ,  , )
22
23
 21
 c31 ( ,  , ) c32 ( ,  , ) c33 ( ,  , )

0
0
0



xE   c11 ( q) c12 ( q) c13 ( q)
yE  c21 ( q) c22 ( q) c23 ( q)

z E   c31 ( q) c32 ( q) c33 ( q)
 
1  0
0
0

x( q ) 
y ( q) 
z ( q) 

1 

Hệ phƣơng trình đại số phi tuyến, độc lập tuyến tính:

f1  x  q   x E  0

f 2  y  q   y E  0
f  z q  z  0
  E
3

f 4  c11  q   c11  , ,    0
f  c q  c , ,   0

22  

22 
5
f 6  c33  q   c33  , ,    0


 xE  a1 cos1  a2 cos1 cos  2  d3 cos 1 sin  2

 yE  a1 sin 1  a2 sin 1 cos 2  d3 sin 1 sin  2
 z  d  d cos  a sin 
1
3
2
2
2
 E

3.5

Thiết kế quỹ đạo

Sau khi đã có không gian làm việc của robot, ta xây dựng cho robot một quỹ đạo
chuyển động trong không gian thao tác.
Để thỏa mãn điều kiện về vận tốc đầu và cuối ta thiết lập quan hệ x = x(t),
y = y(t), z = z(t) là đa thức bậc 3:

 x(t )  ax 0  ax1t  ax 2t 2  ax 3t 3

2
3
 y (t )  a y 0  a y1t  a y 2t  a y 3t


2
3
 z (t )  az 0  az1t  az 2t  az 3t
Thực hiện di chuyển từ vị trí điểm A( x1 , y1 , z1 ) đến vị trí B( x2 , y2 , z2 ) .
Với x(t )  ax 0  ax1t  ax 2t 2  ax 3t 3
19


20

t  t1

x  x1

x  x1  0

t  t2

x  x2

x  x2  0

 x1  ax 0  ax1t1  ax 2t12  ax 3t13

2
 x1  ax1  2ax 2t1  3ax 3t1  0

2
3

 x2  ax 0  ax1t2  a x 2t2  a x 3t2
 x  a  2a t  3a t 2  0
x1
x2 2
x3 2
 2


x2t13  3t12t2 x2  3t1t2 2 x1  t23 x1
ax 0 
(t1  t2 )3


6t1t2 ( x1  x2 )
ax1  
(t1  t2 )3

 
a  3( x1  x2 )(t1  t2 )
 x2
(t1  t2 )3

a   2( x1  x2 )
 x 3
(t1  t2 )3

Tƣơng tự với y và z ta tính đƣợc :


y2t13  3t12t2 y2  3t1t2 2 y1  t23 y1

a

 y0
(t1  t2 )3


6t1t2 ( y1  y2 )
 a y1  
(t1  t2 )3


a  3( y1  y2 )(t1  t2 )
 y2
(t1  t2 )3

a   2( y1  y2 )
 y 3
(t1  t2 )3


z2t13  3t12t2 z2  3t1t2 2 z1  t2 3 z1
a

 z0
(t1  t2 )3


6t1t2 ( z1  z2 )
 a z1  
(t1  t2 )3



a  3( z1  z2 )(t1  t2 )
 z2
(t1  t2 )3

a   2( z1  z2 )
 z 3
(t1  t2 )3

 Tạo độ suy rộng biến khớp q(1 , 2 , d3 )
Nhắc lại tọa độ của điểm thao tác khâu cuối, ma trận:
 cos 1cos 2
sin  cos 
1
2
0
A3  
 sin  2

0


 sin 1
cos 1
0
0

cos1 sin  2
 sin 1 sin  2

cos  2
0

a1 cos 1  a2 cos 1 cos  2  d 3cos 1 sin  2 ) 
a1 sin 1  a2 sin 1 cos  2  d3 sin 1 sin  2 

d1  d3 cos  2  a2 sin  2

1


Ta cần giải hệ phƣơng trình sau để tìm ra biến khớp q(1 , 2 , d3 ) :

20


21

a1 cos 1  a2 cos(1   2 )  xE

a1 sin 1  a2 sin(1   2 )  yE
d  d  z
3
E
 1
Trong đó a1 = 0.018m, a2 = 0.02m là chiều dài khâu 1 và khâu 2.
Giải hệ phƣơng trình bằng phƣơng pháp lặp Newton-Raphson sử dụng phần mềm
Matlab.
Ma trận Jacobi của hệ phƣơng trình là đạo hàm riêng các phƣơng trình theo biến
q(1 , 2 , d3 )

21


22

CHƢƠNG 4: XÂY DỰNG CHƢƠNG TRÌNH MÔ PHỎNG
4.1 Giới thiệu về môi trƣờng mô phỏng OPENGL

Hình x: Logo chính thức của OPENGL

OpenGL (tiếng Anh Open Graphics Library) là một tiêu chuẩn kỹ thuật đồ
họa có mục đích định ra một giao diện lập trình ứng dụng (tiếng Anh: API) đồ họa 3
chiều. OpenGL cũng có thể đƣợc dùng trong các ứng dụng đồ họa 2 chiều. Giao diện
lập trình này chứa khoảng 250 hàm để vẽ các cảnh phức tạp từ những hàm đơn giản.
Nó đƣợc dùng rộng rãi trong các trò chơi điện tử. Ngoài ra nó còn dùng trong các ứng
dụng CAD, thực tế ảo, mô phỏng khoa học, mô phỏng thông tin, phát triển trò chơi.
OpenGL còn có một đối thủ cạnh tranh là DirectX của Microsoft.
Không gian trong OpenGL đƣợc miêu tả qua hình học xạ ảnh. Một điểm trong
không gian này có tất cả bốn tọa độ. Cách thể hiện các điểm trong không gian bằng 4
tọa độ cho phép xử lý các điểm vô tận một cách tổng quát. Vì vậy mã nguồn các ứng
dụng đã đƣợc đơn giản hóa đi nhiều.
4.2 Xây dựng bài toán mô phỏng
4.2.1 Khởi tạo Project trong OPENGL
Bƣớc 1: Khởi động Visual Studio C++(Visual Studio2015)

22


23

Bƣớc 2:File/NewProject/MFCApplication và đặt tên cho project RobotSimulator

23


24
Bƣớc 3: Chọn tùy chỉnh cho Project là Single Document và MFC Standard.Rồi chọn
Finish

24


25

Mã nguồn của chƣơng trình MFC cơ bản:

 Thêm lớp COpenGLInit vào project
 Copy các tệp OpenGLInit.h và OpenGLInit.cpp vào thƣ mục OpenGL nằm
trong thƣ mục của project

25