Tính toán thiết kế mô phỏng robot dùng maple

BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

ĐỒ ÁN CƠ ĐIỆN - TỬ
Đề tài: Tính toán thiết kế chế tạo và mô phỏng robot

NGƯỜI HƯỚNG DẪN KHOA HỌC
PGS.TS. Phan Bùi Khôi
Hà Nội – 2013
1
LỜI CAM ĐOAN
Tôi xin cam đoan đồ án “Tính toán thiết kế và mô phỏng robot stanford” là nghiên
cứu của tôi dưới sự hướng dẫn của PGS.TS. Phan Bùi Khôi. Các số liệu trong luận
văn có nguồn gốc rõ ràng và được trích dẫn. Nếu có gian lận nào tôi xin chịu mọi
trách nhiệm.
2
LỜI MỞ ĐẦU
Ở một tầm nhìn rộng, trên thế giới khái niệm robot đã không còn mới mẻ nữa, thậm chí
nó còn trở nên phổ biến ở những quốc gia phát triển. Robot được nghiên cứu, chế tạo,
phát triển để phục vụ những nhu cầu của con người, nên những chủng loại và kiểu dáng
tương đối phong phú và đa dạng. Công nghệ robot trên thế giới đã rất phát triển nhưng
con người không muốn dừng lại ở đó và vẫn muốn đẩy mạnh phát triển hơn nữa cho
robot, tiến đến giải phóng hoàn toàn sức lao động của con người.
Ở tầm nhìn hẹp hơn, Với nước ta, một nước đang phát triển, và đang trên con đường trở
này một nước công nghiệp tiên tiến. Robot lại là một trong những mảng không thể thiếu
trong công nghiệp hiện đại. Thế nên việc đào sâu nghiên cứu về robot đã được tiến hành
từ rất sớm. Tuy ra đời muộn hơn nhưng robot Việt Nam đã và đang dần bắt kịp với các
nước trên thế giới.
Trong phạm vi đồ án sau đây, em xin đề cập đến những vấn đề cơ bản khi nghiên cứu
một mô hình robot Stanford
Tuy còn thiếu xót, nhưng những phần cơ bản về điều khiển robot và robotics đều được

trình bày trong đồ án này. Mong Thầy ĐÀO ANH TÙNG và các bạn có thể đóng góp ý
kiến để em hoàn thiện hơn nữa bài tập này.
Sinh viên thực hiện
3
CHƯƠNG 1: GIỚI THIỆU CHUNG VỀ ROBOT VÀ MÔ HÌNH
THIẾT KẾ
1.1.Giới thiệu chung về robot:
Robot là máy, thiết bị tự động linh hoạt phục vụ con người:
- Có hình dạng giống người hoặc cánh tay người.
- Có khả năng thao tác tự động.
- Có khả năng bắt chước thao tác giống người.
Cuộc sống ngày càng văn minh hiện đại , mức sống của người dân ngày càng được
nâng cao, đòi hỏi phải nâng cao năng suất và chất lượng của sản phẩm .Vì vậy càng phải
ứng dụng rộng rãi các phương tiện tự động hóa vào sản xuất nên càng tăng nhanh nhu
cầu về ứng dụng Robot để tạo ra các hệ thống sản phẩm tự động và linh hoạt.
Robot là máy , thiết bị cố định hoặc di động , được tích hợp từ nhiều bộ phận trong đó
các bộ phận chính bao gồm:
- Cơ cấu chấp hành
- Hệ thống dẫn động
- Hệ thống điều khiển theo chương trình có khả năng lập trình linh hoạt.
- Hệ thống thông tin giám sát.
Trong những năm gần đây thì việc áp dụng các loại robot vào các dây chuyền sản
xuất ngày cầng được sử dụng rộng rãi ở doanh ngiệp . Ví dụ như các loại robot:
Robot hàn,Robot phun sơn ở các công ty sản xuất và lắp ráp ô tô. Đặc biệt là ở các
công ty sản xuất và lắp ráp các linh kiện điện tử . Đây là công việc đòi hỏi độ chính
xác cao , thường lặp lại nên dê gây mệt mỏi cho người làm vì vậy sử dụng Robot ở
các công ty này là rất phổ biến . Ngoài ra hiện nay ở các phân xưởng sản xuất sử
dụng rất nhiều loại xe Robocar hoặc ở các công việc trong môi trường độc hại ,
thường xuyên tiếp xúc với hóa chất độc hại thì cũng sử dụng Robot làm thay con
người.

4

Hình 1.1 : Một số hình ảnh về robot trong công nghiệp
1.2.Giới thiệu chung về robot Stanford và nguyên lý hoạt động của robot
Stanford:
1.2.1:Giới thiệu chung về robot Stanford:
Stanford Cart (1965-1979)
Từ năm 1979, phòng thí nghiệp Stanford tiếp tục đưa ra một thiết kế mới, xe tự định
vị Standford, được xây dựng với mục đích ban đầu là tạo ra một thiết bị điều khiển từ
xa lưu động (rover) trên mặt trăng. Từ năm 1967 cho đến 1970, nó được thiết kế lại để
5
có thể tự động đi theo một vạch kẻ màu trắng trên đường. Một máy tính từ xa điều
khiển hoạt động của xe này nhờ thông tin truyền về từ một camera vô tuyến gắn trên
xe. Năm 1977 Hans Moravec tiếp tục thiết kế lại, trang bị cho nó một camera vô tuyến
gắn trên đỉnh để chụp ảnh từ các góc độ khác nhau và chuyển về cho máy tính. Máy
tính sẽ tính toán khoảng cách xe và chướng ngại vật sau đó điều khiển xe đi vòng qua
những chướng ngại vật này. Năm 1979, Stanford Cart chỉ mất gần 5 giờ len lỏi qua một
căn phòng đầy bàn ghế - không hề có sự trợ giúp của con người. Đây quả là cột mốc
đánh dấu thành quả mới cho những cỗ xe tự định vị.
1.2.2 Nguyên lý hoạt động:
Thứ tự thao tác của robot, gồm 3 bước:
- Bước 1: Robot kẹp vật từ trên xuống, trên mặt phẳng bất kì trong miền làm
việc
- Bước 2: Tịnh tiến vật lên hoặc xuống theo phương Z nhờ khớp tịnh tiến.
- Bước 3: Di chuyển vật đến vị trí cần thiết trong mặt phẳng mới bằng việc quay
các góc xác định với độ dài các khâu là cố định.
CHƯƠNG 2: BÀI TOÁN ĐỘNG HỌC:
2.1.Hệ tọa độ trục và bảng DH.
2.1.1Thiết lập hệ tọa độ trục.
- Khâu đế: ta chọn hệ tọa độ X

0
Y
0
Z
0
có trục Z
0
chọn trùng với khớp 1,trục X
0
chọn
tùy ý sao cho phù hợp nhất như hình vẽ, trục Y
0
chọn theo quy tắc tam diện thuận.
- Khâu 1: ta chọn hệ tọa độ X
1
Y
1
Z
1
có trục Z
1
chọn trùng với khớp 2,trục X
1
// X
0
,
trục Y
1
chọn theo quy tắc tam diện thuận.
- Khâu 2 : ta chọn hệ tọa độ X

2
Y
2
Z
2
có trục Z
2
chọn trùng với khớp 3, trục X
2
// X
1
,
trục Y
2
chọn theo quy tắc tam diện thuận.
6
- Khâu 3 : ta chọn hệ tọa độ X
3
Y
3
Z
3
có trục Z
3
chọn trùng với khớp 4, trục X
3
// X
2
,
trục Y

3
chọn theo quy tắc tam diện thuận.
- Khâu 4 : ta chọn hệ tọa độ X
4
Y
4
Z
4
có trục Z
4
chọn trùng với khớp 5, trục X
4
// X
3
,
trục Y
4
chọn theo quy tắc tam diện thuận.
- Khâu 5 : ta chọn hệ tọa độ X
5
Y
5
Z
5
có trục Z
5
chọn trùng với khớp 6, trục X
4
trùng
X

5
, trục Y
5
chọn theo quy tắc tam diện thuận.
- Khâu 6 : ta chọn hệ tọa độ X
6
Y
6
Z
6
có trục Z
6
hướng thẳng đứng,trục X
6
// X
5
, trục
Y
6
chọn theo quy tắc tam diện thuận.
7
Hình 2.1 : Sơ đồ đặt tọa độ
8
2.1.2. Bảng Denavit – Hartenberg.
Từ việc chọn hệ tọa độ ta có bảng DH sau:
Trong đó các giá trị đã biết:
d
1
= 1; d
2

=0.5; q
3
=0.643; d
4
= 0.095; d
6
=0.2;
2.2.Hệ phương trình động học cơ bản của Stanford:
Ta có: Dạng tổng quát của ma trận Denavit – Hartenberg cho các khâu:
1
os sin os sin sin os
sin os os os sin sin
0 sin os
0 0 0 1
i i i i i i i
i i i i i i i
i
i i
i i i
c c a c
c c c a
H H
c d
θ θ α θ α θ
θ θ α θ α θ
α α
−
−
 
 

−
 
= =
 
 
 
(2.1)
Các ma trận H của robot STANFORD được xác định theo công thức (2.1)
• Ma trận mô tả vị trí và hướng của O
1
X
1
Y
1
Z
1
đối với O
0
X
0
Y
0
Z
0
: H
1
9
1 1
1 1
1

1
osq 0 sin q 0
sin q 0 cosq 0
0 1 0
0 0 0 1
c
H
d
−
 
 
 
=
 
−
 
 
(2.2)
• Ma trận mô tả vị trí và hướng của O
2
X
2
Y
2
Z
2
đối với O
1
X
1

Y
1
Z
1
: H
2
2 2
2 2
2
2
osq 0 sin q 0
sin q 0 cosq 0
0 1 0
0 0 0 1
c
H
d
 
 
−
 
=
 
 
 
(2.3)
• Ma trận mô tả vị trí và hướng của O
3
X
3

Y
3
Z
3
đối với O
2
X
2
Y
2
Z
2
: H
3
3
3
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
H
q
 
 
 
=
 
 
 
(2.4)

• Ma trận mô tả vị trí và hướng của O
4
X
4
Y
4
Z
4
đối với O
3
X
3
Y
3
Z
3
: H
4
4 4
4 4
4
4
osq 0 sin q 0
sin q 0 cos q 0
0 1 0
0 0 0 1
c
H
d
−

 
 
 
=
 
−
 
 
(2.5)
• Ma trận mô tả vị trí và hướng của O
5
X
5
Y
5
Z
5
đối với O
4
X
4
Y
4
Z
4
: H
5
5 5
5 5
5

osq 0 sin q 0
sin q 0 cosq 0
0 1 0 0
0 0 0 1
c
H
 
 
−
 
=
 
 
 
(2.6)
• Ma trận mô tả vị trí và hướng của O
6
X
6
Y
6
Z
6
đối với O
5
X
5
Y
5
Z

5
: H
6
10
6 6
6 6
6
6
osq sin q 0 0
sin q cos q 0 0
0 1 1
0 0 0 1
c
H
d
−
 
 
 
=
 
 
 

(2.7)
Từ các ma trận Denavit – Hartenberg ta tính được vị trí , hướng của khâu thao tác đối
với hệ tọa độ cố định O
0
X
0

Y
0
Z
0
là ma trận D
6
.

6 1 2 3 4 5 6
D H H H H H H=
(2.8)
Các giá trị H
1
, H
2
, H
3
, H
4
, H
5
, H
6
được xác định từ công thức (2.2),(2.3), ,(2.7).
Ma trận D
6
cho ta biết hướng và vị trí của khâu thao tác trong hệ tọa độ cố định hay nói
cách khác là vị trí của điểm tác động cuối và hướng của hệ tọa độ động gắn vào khâu
tại điểm tác động cuối trong hệ tọa độ cố định.
Mặt khác nếu ta gọi [ xe ye ze rotxe rotye rotze] là vector mô tả trực tiếp vị trí và

hướng của O
6
X
6
Y
6
Z
6
trong hệ tọa độ O
0
X
0
Y
0
Z
0
. Trong đó [xp yp zp] là tọa độ và
[rotxp rotyp rotzp] là các góc quay Roll-Pitch-Yaw của O
6
X
6
Y
6
Z
6
đối với O
0
X
0
Y

0
Z
0
.
Khi đó ta có:
E
6
: là ma trận cosin chỉ hướng mô tả hướng của O
6
X
6
Y
6
Z
6
đối với O
0
X
0
Y
0
Z
0
6
os os os sin sin sin os os sin os sin sin
sin os sin sin sin os os sin sin os os sin
sin sin os os os
c c c c c c
E c c c c c
c c c

θ θ θ
θ θ θ
θ θ θ
Φ Φ Ψ − Φ Ψ Φ Ψ + Φ Ψ
 
 
= Φ Φ Ψ + Φ Ψ Φ Ψ − Φ Ψ
 
 
Ψ Ψ
 
(2.9)
Trong đó ký hiệu , θ=rotxe Ψ=rotye, Φ=rotze.
Ma trận D
6
là ma trận mô tả vị trí và hướng của khâu thao tác trong hệ tọa độ cố định
thong qua các biến khớp q
i
Từ (2.8) và (2.9) suy ra hệ 6 phương trình độc lập:
11
1 6
2 6
3 6
4 6 6
5 6 6
6 6 6
[1,4]
[2,4]
[3,4]
[1,1] [1,1]

[2,2] [2,2]
[3,3] [3,3]
f D
f D
f D
f E D
f E D
f E D
=


=


=


= −


= −

= −


(2.10)
Các phương trình động học của robot Stanford như sau:
1
f = (0.2(cos(q1)cos(q2)cos(q4)-sin(q1)sin(q4)))sin(q5)+0.2cos(q1)sin(q2)cos(q5)
+0.738cos(q1)sin(q2)-0.5sin(q1)

2
f = (0.2(sin(q1)cos(q2)cos(q4)+cos(q1)sin(q4)))sin(q5)+0.2sin(q1)sin(q2)cos(q5)
+0.738sin(q1)sin(q2)+0.5cos(q1)
3
f = 1-0.2sin(q2)cos(q4)sin(q5)+0.2cos(q2)cos(q5)+0.738cos(q2)
4
f = cos(rotze)cos(rotxe)-((cos(q1)cos(q2)cos(q4)-sin(q1)sin(q4))*cos(q5)-cos(q1)sin(q2)sin(q5))cos(q6)
-(-cos(q1)cos(q2)sin(q4)-sin(q1)cos(q4))sin(q6)
5
f = sin(rotze)sin(rotye)sin(rotxe)+cos(rotze)cos(rotye)+((sin(q1)cos(q2)cos(q4)
+cos(q1)sin(q4))cos(q5)-sin(q1)sin(q2)sin(q5))sin(q6)-(-sin(q1)cos(q2)sin(q4)+cos(q1)cos(q4))cos(q6)
6
f = cos(rotxe)cos(rotye)+sin(q2)cos(q4)sin(q5)-cos(q2)cos(q5)
2.3. Bài toán vị trí.
2.3.1 Bài toán thuận.
• Biết trước giá trị của biến khớp (q
1
, q
2
, q
4
,q
5
, q
6
)
• Yêu cầu tìm các tọa độ của khâu cuối (xe, ye, ze, rotxe, rotye, rotze).
Vị trí của điểm tác động cuối lên đối tượng cần thao tác được xác định bởi tọa độ điểm
E(xe, ye, ze) , hướng của nó được xác định bởi các góc quay (rotxe, rotye, rotze).

12
Theo hệ phương trình (2.10):
Ba phương trình đầu xác định được vị trí của đối tượng:
D6[1, 4]
D6[2, 4]
D6[3, 4]
xe
ye
ze
=


=


=

(2.11)
Ba phương trình sau cho ta bài toán xác định hướng của điểm tác động cuối lên đối
tượng:
4
5
6
f = E6[1, 1]-D6[1, 1]
f = E6[2, 2]-D6[2, 2]
f = E6[3, 3]-D6[3, 3]






(2.12)
Ba phương trình f
4
, f
5
, f
6
đã được tính ở phần trên:
4
f = cos(rotze)cos(rotxe)-((cos(q1)cos(q2)cos(q4)-sin(q1)sin(q4))*cos(q5)-cos(q1)sin(q2)sin(q5))cos(q6)
-(-cos(q1)cos(q2)sin(q4)-sin(q1)cos(q4))sin(q6)
5
f = sin(rotze)sin(rotye)sin(rotxe)+cos(rotze)cos(rotye)+((sin(q1)cos(q2)cos(q4)
+cos(q1)sin(q4))cos(q5)-sin(q1)sin(q2)sin(q5))sin(q6)-(-sin(q1)cos(q2)sin(q4)+cos(q1)cos(q4))cos(q6)
6
f = cos(rotxe)cos(rotye)+sin(q2)cos(q4)sin(q5)-cos(q2)cos(q5)
Giải các phương trình trên ta sẽ tính được hướng của hệ tọa độ khâu thao tác đối với hệ
tọa độ cố định.
Ta được:
f(x)=0
Giải hệ 3pt: chu
13
4
5
6
=cos( ).cos( )=0
1
sin( ).sin( ).sin( )+cos( ).cos( )+ . 3 0
2

cos( ).cos( )=0
f
f
f
θ
θ
θ
Φ



= Φ Ψ Φ Ψ =


= Ψ


2
2
3
θ
Π

Φ =


−Π

Ψ =



Π

=


2.3.2 Bài toán ngược:
Bài toán ngược là bài toán có ý nghĩa rất quan trọng trong thực tế.
Khi biết quy luật chuyển động của khâu thao tác và ta phải tìm các giá trị của biến
khớp . Việc xác định các giá trị của biến khớp cho phép ta điều khiển robot theo đúng
quỹ đạo đã cho.
14
Hình 2.2 : Sơ đồ khối giải bài toán động học ngược
CHƯƠNG 3: SỬ DỤNG PHẦN MỀM TÍNH TOÁN MAPLE:
3.1 Các gói thư viện cần sử dụng trong maple:
>
15
>
>
>
>
>
3.2 Tính ma trận
 Thủ tục tính trận DH:
16
 Thủ tục tính ma trận cosin chỉ hướng:
Tính ma trận:
17
>
>

>
>
>
18
>
>
19
3.3.Bài toán thuận:

>
> restart;
> eqn1:=cos(rotze)*cos(rotxe):
eqn2:=sin(rotze)*sin(rotye)*sin(rotxe)+cos(rotze)*cos(rotye)+(1/2)*sqrt(3):
eqn3:=cos(rotxe)*cos(rotye):
> solve({eqn1=0,eqn2=0,eqn3=0},{rotze, rotye, rotxe});
20
3.4.Đồ thị:
>
>
>
>
21
>
Hình 3.1 : Đồ thị
3.5.Bài toán ngược:
> restart;
22
> with(LinearAlgebra):
> with(linalg):
> with(diffalg):

> with(plots):
> with(plottools):
> with(MTM):
> VectorToText:=proc(vt_)#vt_ type: vector
> local n_,st_,i_;
> n_:=Dimension(vt_);
> st_:="";
> for i_ from 1 to n_-1 do
> st_:=cat(st_,convert(vt_[i_],'string')," ");
> end do;
> st_:=cat(st_," ",convert(vt_[n_],'string'));
> return st_;
> end proc;
> NewRap:=proc(ff_,x0_,err_,X_) #x0: initial values, err: tolerance
> #p=[x,y,z,rotx,roty,rotz]; q=[q1 q6]
> local n,x1_,m,dX,jac,fff,x_,li_,iii,i,j,i_,fff_;
23
> x1_:=x0_;
> n:=Dimension(ff_);
> m:=Dimension(x0_);
> dX:=Vector(m);
> jac:=Matrix(n,m);
> fff_:=Vector(n);
> x_:=Vector(m);
> li_:=convert(x_,'list');
> for iii from 1 to 100 do
> for i from 1 to m do #give value to a list li_ in order to evaluate
jacobian matrix and fff_
> li_[i]:=X_[i]=x1_[i];
> end do;

> for i from 1 to n do
> fff_[i]:=evalf(eval(ff_[i],li_));
> end do;
> fff_;
> for i from 1 to n do
> for j from 1 to m do
> jac[i,j]:=evalf(eval(diff(ff_[i],X_[j]),li_));
> end do;
> end do;
> jac;
> dX:=-(MatrixInverse(jac)).fff_;
> x1_:=x1_+dX;
> if (VectorNorm(dX,2)<=err_) then
> return convert(x1_,'list');
24
> end if;
> end do;
> return NaN;
>
> end proc;
> rotxe:=3.14/2;rotye:=3.14;rotze:=0;
> #ffarm:=ImportVector("equation_DH6_Kuka.equ"):
ffarm := Vector(6);
25