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

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

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

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

−
 
= =
 
 
 
(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

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

• 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
−

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

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à

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,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)

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]





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

>
>
>
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):

> 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