Đồ án: Cánh tay robot puma

Lời nói đầu
Trong sự nghiệp công nghiệp hóa, hiện đại hóa đất nước vấn đề tự động hóa
sản xuất có vai trò đặc biệt quan trọng .
Nhằm nâng cao năng suất dây chuyền công nghệ, nâng cao chất lượng và
khả năng cạnh tranh của sản phẩm, cải thiện điều kiện lao động , nâng cao năng
suất lao động…đặt ra là hệ thống sản xuất phải có tính linh hoạt cao. Robot công
nghiệp là một bộ phận quan trọng để tạo ra những hệ thống tự động đó.
Robot công nghiệp đã có mặt trong sản xuất từ nhiều năm trước, ngày nay
robot công nghiệp được dùng ở nhiều lĩnh vực sản xuất. đó là xuất phát từ những
ưu điểm mà robot đó đã được chọn và đúc kêt lại trong quá trình sản xuất,robot có
những tính năng mà con người không thể có được, khả năng làm việc ổn định,làm
việc trong môi trường độc hại… Do đó việc đầu tư nghiên cứu, chế tạo ra ngững
loại robot phục vụ cho công cuộc tự động hóa sản xuất là rất cần thiết cho hiện tại
và trong tương lai.
Môn học ĐỒ ÁN CƠ ĐIỆN TỬ giúp chúng em bước đầu làm quen,tìm hiểu,
học cách chế tạo và rất có ích cho chúng em sau này.
Trong quá trình học môn ĐỒ ÁN CƠ ĐIỆN TỬ chúng em còn nhiều thiếu
sót mong các thầy chỉ bảo thêm cho chúng em.
Chân thành cảm ơn
Nhóm sinh viên :
Nguyễn Đình Quảng
Nguyễn Đăng Thi
Nguyễn Văn Thảo
Ngô Văn Thắng

1


I-Xây dựng sơ đồ động học và các ma trận biểu diễn cấu trúc
1-Sơ đồ động học của PUMA.

2- Các ma trận chuyển biểu diễn cấu trúc của PUMA.

C 6  S 6
 S 6 C6
A6  
0
0

0
0
C 3
S3
A3  
0

0

0 0
0 0 
1 d6 

0 1

0 S 3 C 3a3 
0  C 3 S 3a3 
1
0
06 

0

0
1 

C 5
S5
A5  
0

0

0 S5
0  C5
1
0
0
0

C 2  S 2
S2 C2
A2  
0
0

0
0

0
0
0


1

0 C 2a2 
0 S 2a2 
1
d2 

0
1 

C 4 0  S 4 0 
S4 0
C 4 0 
A4  
 0 1
0
d4 


0
0
1
0
C1 0  S1
 S1 0
C1
A1  
0 1 0

0

0 0

0
0
0

1

Trong đó: để tiện theo dõi ta ký hiệu
Cx =cos(x): x là các biến khớp
Sx= sin(x)
II-Tính Toán Động Học Thuận:

2


Bảng thông số DH:
khâu
1
2
3
4
5
6

i




*
1
*
2
*
3

 4*
 5*
 6*

C 6  S 6
 S 6 C6
A6  
0
0

0
0
C 5
S5
A5  
0

0

i

ai

di

-90
0
90
-90
90
0

0
a2
a3
0
0
0

0
d2
0
d4
0
d6

0 0
0 0 
1 d6 

0 1

0 S5

0  C5
1
0
0
0

0
0
0

1

S 5d 6 
C 5C 6  C 5S 6 S 5
 S 5C 6  S 5S 6  C 5  C 5d 
6
4
T6  A5 .5 T6  
 S6
C6
0
0 


0
0
1 
 0
C 4 0  S 4 0 
S4 0

C 4 0 
A4  
 0 1
0
d4 


0
0
1
0
C 4C 5C 6  S 4 S 6  C 4C 5S 6  S 4C 6 C 4 S 5 C 4 S 5d 6 
 S 4C 5C 6  C 4 S 6  S 4C 5S 6  C 4C 6 S 4 S 5 S 4S 5d 
6 
3
T6  A4 .4 T6  

 S 5C 6
S 5S 6
C 5 C 5d 6  d 4 


0
0
0
1



3

C 3
S3
A3  
0

0
2

0 S 3 C 3a3 
0  C 3 S 3a3 
1
0
06 

0
0
1 

T6  A3 .3 T6

các hệ số trong ma trận:
2

T6 (1,1) C 2(C 4C 5C 6  S 4 S 6 _  S 3S 5S 6;

2

T6 (1,2) C 3 * ( C 4C 5S 6  S 4S 6)  S 3S 5S 6;

2

T6 (1,3) C 3C 4 S 5  S 3C 5;

2

T6 (2,1) 3(C 4C 5C 6  S 4 S 6)  C 3S 5C 6;

2

T6 (2,2)  S 3( C 4C 5S 6  S 4C 6)  C 3S 5S 6;

2

T6 (2,3)  S 3C 4 S 5  C 3C 5;

2

T6 (2,4)  S 3C 4 S 5d 6  C 3(C 5d 6  d 4)  S 3a3;

2

T6 (3,1)  S 4C 5C 6  C 4 S 6;

2

T6 (3,2)   S 4C 5S 6  C 4C 6;

2

T6 (3,3)  S 4 S 5;

2

T6 (3,4)  S 4 S 5d 6.

C 2  S 2
S2 C2
A2  
0
0

0
0
1

0 C 2a2 
0 S 2a2 
1
d2 

0
1 

T6  A2 .2 T6

các hệ số trong 1T6 là:

4

1

T6 (1,1) C 2(C 4C 5C 6  S 4 S 6)  S 23S 5C 6

1

T6 (1,2)  C 23(C 4C 5S 6  S 4C 6)  S 23S 5S 6

1

T6 (1,3) C 2(C 3C 4 S 5  S 3C 5)  S 2( S 3C 4 S 5  C 3C 5)

1

T6 (1,4) C 2[C 3C 4 S 5d 6  S 3(C 5d 6  d 4)  C 3a3]  S 2[ S 3C 4 S 5d 6  C 3(C 5d 6  d 4)  S 3a3]

 C 2a 2
1

T6 (2,1)  S 2[C 3(C 4C 5C 6  S 4 S 6)  S 3S 5C 6]  C 2[ S 3(C 4C 5C 6  S 4 S 6)  C 3S 5C 6]

1

T6 (2,2)  S 2[C 3(  C 4C 6 S 6  S 4C 6)  S 3S 5S 6]  C 3[ S 3( C 4C 5S 6  S 4C 6)  C 3S 5S 6]

1

T6 (2,3)  S 2(C 3C 4S 5  S 3C 5)  C 2( S 3C 4 S 5  C 3C 5)

1

T6 (2,4)  S 2[C 3C 4S 5d 6  S 3(C 5d 6  d 4)  C 3a3]  C 2[ S 3C 4d 6  C 3(C 5d 6  d 4)  S 3a3]

 S 2a 2
1

T6 (3,1)  S 4C 5C 6  C 4 S 6

1

T6 (3,2)  S 4C 5S 6  C 4C 6

1

T6 (3,3)  S 4 S 5

1

T6 (3,4)  S 4 S 5d 6  d 2
C1 0  S1
 S1 0
C1
A1  
0 1 0

0
0 0
0

0
0
0

1

T6  A1.1T6

các hệ số trong 0 T6 là:
N x  C1[C 23(C 4C 5C 6  S 4S 6)  S 23S 5S 6]  S1( S 4C 5C 6  C 4S 6]
Ox  C1[C 23(C 4C 5S 6  S 4C 6)  S 23S 5S 6]  S1(4C 5S 6  C 4C 6)
Ax  C1[C 2(C 3C 4S 5  S 3C 5)  S 2( S 3C 4S 5  C 3C 5)  S1S 4 S 5

5


Px  C1[C 2[C 3C 4 S 5d 6  S 3(C 5d 6  d 4)  C 3a3]  S 2[ S 3C 4 S 5d 6
C 3(C 5d 6  d 4)  S 3a3]  C 2 a 2]  S1( S 4 S 5d 6  d 2)
N y  S1[ C 23(C 4C 5C 6  S 4S 6)  S 23S 5S 6]  C1( S 4C 5S 6  C 4 S 6)
Oy  S1[C 23(C 4C 5S 6  S 4C 6)  S 23S 5S 6]  C1( S 4C 5S 6  C 4C 6)
Ay  S1[C 2(C 3C 4S 5  S 3C 5)  S 2( S 3C 4S 5  C 3C 5)  C1S 4S 5
Py  S1[C 2[C 3C 4S 5d 6  S 3(C 4d 6  d 4)  C 3a3]  S 2[ S 3C 4S 5d 6  3(C 5d 6  d 4)]  C 2a 2]
C1( S 4 S 5d 6  d 2)
N z  [ S 2(C 3(C 4C 5C 6  S 4S 6)  S 3S 5C 6)  C 2( S 3(C 4C 5C 6  S 4S 6)  C 3S 5S 6)]
Oz  [ S 2(C 3(C 4C 5S 6  S 4C 6)  S 3S 5S 6)  C 2( S 3(C 4C 5S 6  S 4C 6)  C3S 5S 6)]
Az  [ S 2(C 3C 4 S 5  S 3C 5)  C 2( S 3C 4 S 5  C 3C 5)]
Pz  [ S 2(C 33C 4 S 5d 6  S 3(C 5d 6  d 4)  C 3a3)  C 2[ S 3C 4S 5d 6  C 3(C 5d 6  d 4)  S 3a3]  S 2a 2]

III. PHƯƠNG TRÌNH ĐỘNG HỌC NGƯỢC ROBOT PUMA

Bài toán động học ngược là rất khó, đối với tay máy 6 khớp có ba khớp cuối
đồng quy tại một điểm, ta có thể tách bài toán động học ngược thành hai bài toán
đơn giản hơn là động học ngược vị trí và động học ngược hướng
+ Tìm vị trí giao điểm các trục cổ tay (tâm cổ tay)
+ Tìm hướng của cổ tay.
Ta biểu diễn thành hai hệ phương trình như sau:
Trong đó O và R là hướng và vị trí của dụng cụ, được biểu diễn đối với hệ tọa độ
cố định bên ngoài (world coordinate system). Ta phải giải bài toán trên đối với các
ẩn q1 ,  , q6 .
Cơ cấu 6 khớp quay có 3 khớp cuối giao nhau có phương pháp giải như sau:
Vị trí của tâm cổ tay,P C xác định qua vị trí công cụ (The given tool position)
và phương của Tool pointing ( Z 6 ).Do đó vị trí của tâm cổ tay phụ thuộc vào 3 biến
khớp đầu tiên.
The relative wrist oriention R36

6


Các biến khớp  4 ,5 , 6 xác định từ ma trận định hướng cổ tay (The arm
orientation) R03 và ma trận định hướng công cụ (The given tool orientation) R60 .
+ Ma trân trạng thái (The given tool pose) T60
+ Solve porions của động học ngược để tìm ra R30 (1 , 2 ,3 ) và R63 ( 4 ,5 , 6 ) .
+ Định vị trí của tâm cổ tay Oc có tọa độ cho trước như sau:
0
��
0
OC  O  d 6 .R. ��
0 trong đó O và R là hướng và vị trí của dụng cụ, được biểu
��
��

1
��

diễn đối với hệ tọa độ cố định bên ngoài (world coordinate system).
Pc as d 6 (cột cuối cùng của T60 ) – d 6 (tool offset length)* Z 6 (3 cột của ma trận T60 )

+ Thiết lập Pc = cột cuối cùng của R30 (1 , 2 ,3 ) để tìm ra các biến khớp
1 ,  2 , 3

Tính R63  R30 R60 sau khi đã thay giá trị của các biến khớp vào 1 , 2 ,3 vào
1

R30 (1 ,  2 , 3 )

+So sánh R63 và R63 ( 4 , 5 , 6 ) để rút ra  4 ,5 , 6
Tọa độ của điểm C trong hệ tọa độ R6 :
6

rC =

�0 �
� �
�0 �
�
d �
� 6�
�t �
�
�

Tọa độ của C trong hệ tọa độ R0 :
�
n
�x
�
ny
rC = TE .6rE = �
�
n
�z
�
�0

sx

ax

sy

ay

sz

az

0

0

p x ��0 �

�� �
p y ��0 �
�� �
 d6 �
p z ��
�� �
1
1 �
�� �

�
 a .d  x �
� x 6 p�
�
 a y .d 6  y p �
�
�(*)
 az .d6  z p �
�
�
�
1
�
�

Ma trận chuyển từ hệ tọa độ 0 � 3:

7

�
CC
�1 23
�S
0
R3= A1.A2.A3 = � 1
� S
� 23
�
�0

 S1 C1 . S23 C1 ( a3 .C23  a2 .C2 )  S1 .d 2 �
�
C1 S1 . S23 S1 ( a3 .C23  a2 .C2 ) C1 .d 2 �
�
 a3 . S23  a2 . S2  d1 �
C23
0
�
0
1
0
�
�

� Tọa độ của C trong R0 :
�
C .S .d  a .C C  a .C .C  S .d �
�1 23 4 3 1 23 2 1 2 1 2 �
�S . S .d  a .S C  a . S .C  C .d �

rC =0R3.4rC = �1 23 4 3 1 23 2 1 2 1 2 �(**)
�
�
d 4 .C23  a3 . S23  a2 . S2  d1
�
�
t
�
�

Đối chiếu (*) và (**) ta được hệ 3 phương trình 3 ẩn :

C1 ( S 23 .d 4  a3 .C23  a2 .C2 )  S1.d 2  x p  ax .d 6
S1 ( S 23 .d 4  a3 .C23  a2 .C2 )  C1.d 2  y p  a y .d 6
d 4 .C23  a3 .S23  a2 .S 2  z p  az .d 6  d1
Nhân (2) với cos 1 trừ đi (1) nhân với sin 1 :
d 2  ( y p  a y d 6 )cos1  ( x p  ax d 6 )sin1
�
d2
� 1  �Ar cos �
2
� ( y p  a y d 6 )  ( x p  ax d 6 ) 2
�

�
� A tan 2( x p  a x d 6 , y p  a y d 6 )
�
�

Nhân (2) với sin 1 cộng với (1) nhân vói cos 1 và kết hợp với (3) ta có hệ

S23 .d 4  a3 .C23  a2 .C2  ( y p  a y .d 6 ) sin   ( x p  ax .d 6 )cos1  Px
C23 .d 4  a3 .S23  a2 .S2  z p  az .d 6  d1  Py
Bình phương 2 vế cộng lại ta được:
d 42  a32  a22  2d 4 a2 sin 3  2a2 a3cos 3  Px2  Py2
� 2d 4 a2 sin  3  2a2 a3cos3  Px2  Py2  (a32  a22  d 42 )
�Px2  Py2  (a32  a22  d 42 ) �
� A tan 2(d 4 a2 , a2 a3 )
� 3  �Arc cos �
� 2 a d 2  a2
�
2
4
3
�
�

Khai triển hệ phương trình liên kết ta được:

8


(a3 .C3  a2  S3 .d4 )C2  (C3 .d4  a3 .sin 3 )sin 2  Px
(C3 .d 4  a3 .S3 )cos2  (a2  S3 .d4  a3 .cos3 ) sin 2  Py
  �
(a3 .C3  a2  S3 .d 4 ) 2  (C3 .d 4  a3 .S3 ) 2 �
�
�
c   �
(a3 .C3  a2  S3 .d 4 ) Px  (C3 .d 4  a3 .S3 ) Py �

�
�
s   �
(d 4 .C3  a3 S3 ) Px  (a2  d 4 S3  a3 .C3 ) Py �
�
�

 2  A tan 2(

s c
, )
 

�  2  A tan 2 �
(d 4C3  a3 S3 ) Px  (a2  d 4 S3  a3C3 ) Py , (a2  d 4 S3  a3C3 ) Px  (d 4C3  a3S 3 ) �
�
�
C4
�
�
S4
Ta có R63 = A4 A5 A6 = �
�0
�
�0

0 S4
0 C4
1 1
0

0

0 ��
C5
�
�
0 ��
S5
d 4 ��0
��
1 ��0

0 S5
0 C5
1 0
0 0

0 ��
C6
�
�
0 ��
S6
0 ��0
��
1 ��0

 S6
C6
0

0

0 0�
0 0�
�
1 d6 �
�
0 1�

=

Mặt khác:
A1 A2 A3 A4 A5 A6 = TE � R30 .R63  TE � R63  R30( 1) .TE
C1C23nx  S1C23 n y  S 23 nz
�
�
 S1nx  C1n y
�
�
C1S 23nx  S1S 23 n y  C23nz
�
0
�

C1C23 s x  S1C23 s y  S 23 s z
 S1sx  C1s y
C1S 23 sx  S1S 23 s y  C23 s z
0

C1C23a x  S1C23a y  S 23a z

 S1a x  C1a y
C1S 23a x  S1S 23a y  C23a z
0

f14 �
f 24 �
�
f 34 �
�
1 �

Chỉ cần quan tâm tới phần định hướng tức là ma trận [3,3].
So sánh các phần tử của 2 ma trận [3,3] :
R63  3,3 � cos 5  S 23 (C1ax  S1a y )  C23a z
� 5  �Arc cos �
S 23 (C1a x  S1a y )  C23a z �
�
�

Nếu sin 5 �0 �

9


R63  2,3 � sin  4 *sin  5   S1 * ax  C1 * a y
� sin  4 

 S1 * ax  C1 * a y
sin 5

R63  1,3 � cos 4 *sin 5  C23 (C1 * ax  S1 * a y )  S23 az
� cos 4 

C23 (C1 * ax  S1 * a y )  S 23az
sin 5

 4  A tan 2(

 S1 * a x  C1 * a y C23 (C1 * a x  S1 * a y )  S23a z
,
)
sin 5
sin 5

R63  3, 2 � sin 6 *sin 5  C1S23 sx  S1S23 s y  C23 sz
� sin  6 

C1S 23 sx  S1S23 s y  C23 s z
sin 5

R63  3,1 � cos6 *sin 5  C1 S23 nx  S1S 23n y  C23 nz
� cos 6  

 6  A tan 2(

C1S23nx  S1S23n y  C23nz
sin 5
S23 (C1sx  S1s y )  C23 sz
sin 5

,

S23 (C1nx  S1n y )  C23nz
sin 5

)

Vậy bài toán động học ngược cho ta kết quả như sau:
�
d2
1  �Ar cos �
� ( y p  a y d 6 )2  ( x p  ax d 6 ) 2
�

�
� A tan 2( x p  a x d 6 , y p  a y d 6 )
�
�

 2  A tan 2 �
(d 4C3  a3 S3 ) Px  ( a2  d 4 S3  a3C3 ) Py ,(a2  d 4 S3  a3C3 ) Px  (d 4C3  a3 S 3 ) �
�
�
�P 2  Py2  (a32  a22  d 42 ) �
�
3  �Arc cos � x
 A tan 2(d 4 a2 , a2 a3 )
� 2 a d 2  a2
�
2

4
3
�
�

 4  A tan 2(

 S1 * ax  C1 * a y C23 (C1 * ax  S1 * a y )  S 23a z
,
)
sin 5
sin 5

5  �Arc cos �
S 23 (C1a x  S1a y )  C23a z �
�
�
 6  A tan 2(

S23 (C1sx  S1s y )  C23 s z
sin 5

,

S23 (C1nx  S1n y )  C23nz
sin  5

)

10

V.THIẾT KẾ MÔ HÌNH ROBOT PUMA DÙNG SOLIDWORK.
1. Nhiệm vụ của robot.
Gắp một vật nặng khối lượng dưới 1Kg di chuyển trong miền làm việc
2. Phân tích và lựa chọn kết cấu.
2.1 Lựa chọn ổ bi, ổ lăn cho cơ cấu.
- Do tính chất làm việc chịu tải tương đối nhẹ (tải trọng có khối lượng dưới 1kg)
của Robot nên ta sẽ chọn ổ lăn cho các khâu như sau:
+ Khâu 1: Dùng Ổ bi đỡ một dãy có kí hiệu 305.
+ Khâu 2: Dùng Ổ bi đỡ một dãy có kí hiệu 1000902.
+ Khâu 3: Dùng Ổ bi đỡ một dãy có kí hiệu 1000901.
+ Khâu 4: Dùng Ổ bi đỡ một dãy có kí hiệu 1000901.
+ Khâu 5: Dùng Ổ bi đỡ một dãy có kí hiệu 1000099.
2.2 Lựa chọn động cơ cho Robot.
Do yêu cầu điều khiển chính xác vị trí điểm tác động cuối của robot, ta chọn
động cơ cho các khớp quay là loại động cơ Servo.

11


Đây là loại động cơ cho phép ta điều khiển chính xác cả tốc độ và vị trí của
rotor. Động cơ tích hợp một mạch encoder, phản hồi trạng thái vị trí của trục ra,
dựa vào đó ta giảm thiểu được sai số do tải trọng bên ngoài thay đổi…

Dựa vào yêu cầu làm việc của robot, ta chọn được động cơ cho các
khớp như sau:
Khớp

Số hiệu DC

Khớp

Số hiệu

1

T720-012

4

T406-012

2

T511-012

5

T404-012

3

T506-012

6

T402-011

Kích thước và thông số kỹ thuật các động cơ như sau:

12


13


14


3. Thiết kế mô hình dùng phần mềm SolidWorks
Bản vẽ chi tiết phần thiết kế (2D và 3D) của các khâu của robot được kèm
theo ở phần phụ lục. Say đây là một số hình ảnh mô hình 3D của robot.

15


VI. MÔ PHỎNG ĐỘNG HỌC ROBOT PUMA DÙNG VISUAL C++ VÀ OPENGL

Giao diện phần mềm mô phỏng được xây dựng như sau:

16


Phần mềm được xây dựng bằng visual C++. Phần đồ họa được lập trình
bằng các thư viện OpenGL. Giao diện chương trình gồm hai phần chính. Khung
bên phải chứa các nút lệnh điều khiển, và chọn các nhiệm vụ của Robot cần thực
hiện. Khung bên phải dùng để thể hiện mô hình của robot được dùng để mô phỏng.
Mô hình robot được vẽ bằng phần mềm SolidWork và xuất ra file cad 3D dưới
dạng STL sau đó dùng Visual C++ đọc các file này vào chương trình. Sau đó dùng

các hàm OpenGL để vẽ các mô hình này.
Phần động học thuận và ngược trong chương trình được tính trong các hàm
thành viên SolveInverseKinematic và SolveForwardKinematic của lớp
CSerialRobot.
Các hàm này như sau:
void CSerialRobot::SolveInverseKinematic(const CFrame& end ,float* jointVar)
{
CVector nn =end.GetXDir ().GetDirection ();

17


CVector ss=end.GetYDir ().GetDirection ();
CVector aa=end.GetZDir ().GetDirection ();
//CVector aa =CrossProduct (nn,ss);
CPoint3D pp =end.GetOrigin ();
float nx=nn.GetX ();
float ny=nn.GetY ();
float nz=nn.GetZ ();
float sx =ss.GetX ();
float sy =ss.GetY ();
float sz =ss.GetZ ();
float ax =aa.GetX ();
float ay =aa.GetY ();
float az =aa.GetZ ();
float px =pp.GetX ();
float py=pp.GetY ();
float pz=pp.GetZ ();
//joint vars are contained here
float tt1,tt2,tt3,tt4,tt5,tt6;

//link and joint configue
float a2=ROBOT_A2;
float a3=ROBOT_A3;
float d1 =ROBOT_D1;
float d2=ROBOT_D2;
float d4=ROBOT_D4;
float d6=ROBOT_D6;
// calculate theta 1

18


float m =px -ax*d6;
float n =py -ay*d6;
tt1= acosf(d2/sqrt(m*m+n*n)) -atan2(m,n);
float s1=sinf(tt1);
float c1=cosf(tt1);
float K1 = n*s1 + m * c1;
float K2 = pz -az*d6 -d1;
//calulate theta 3
float p=K1*K1+K2*K2 -(a3*a3+a2*a2 +d4*d4);
float q=2*a2*sqrt(d4*d4+a3*a3);
tt3= acosf(p/q) + atan2(d4,a3);
//check the criteria
float c3=cosf(tt3);
float s3=sinf(tt3);
//calculate theta 2
p=a3*c3 +a2 +s3 *d4;
q=d4*c3-a3*s3;
float delta =p*p +q*q;

float deltac =p*K1 +q*K2;
float deltas =q*K1 - p*K2;
tt2= atan2(deltas,deltac);
float s2=sinf(tt2);
float c2=cosf(tt2);
//calculate theta 5;

19


float s23 =sinf(tt2+tt3);
float c23 =cosf(tt2+tt3);
m=c1*ax+s1*ay;
tt5 = acosf(s23*m- c23*az);
float c5= cosf(tt5);
float s5= sinf(tt5);
//calculate theta4
m=c1 *ax +s1*ay;
n=-s1*ax +c1*ay;
if(tt5!=0)
{
tt4 =atan2(n/s5, (c23*m - s23*az)/s5);
}
else
{
tt4=0;
}
//calculate theta 6
tt6 =atan2((s23*(c1*sx +s1*sy)+c23*sz)/s5,
-(s23*(c1*nx +s1*ny)+c23*nz)/s5);

tt1=r2d(tt1);
tt2 =r2d(tt2);
tt3 =r2d(tt3);
tt4 =r2d(tt4);
tt5 =r2d(tt5);

20


tt6 =r2d(tt6);
jointVar[0] =tt1;
jointVar[1] =tt2;
jointVar[2] =tt3;
jointVar[3] =tt4;
jointVar[4] =tt5;
jointVar[5] =tt6;
MoveTo (tt1,tt2,tt3,tt4,tt5,tt6);
}
void CSerialRobot::SolveForwardKinematic
(const float tt1,const float tt2,const float tt3,const float tt4,
const float tt5,const float tt6,CFrame* frm)
{
float c1 =cosf(d2r(tt1));
float c2 =cosf(d2r(tt2));
float c3 =cosf(d2r(tt3));
float c4 =cosf(d2r(tt4));
float c5 =cosf(d2r(tt5));
float c6 =cosf(d2r(tt6));
////
float s1 =sinf(d2r(tt1));

float s2 =sinf(d2r(tt2));
float s3 =sinf(d2r(tt3));

21


float s4 =sinf(d2r(tt4));
float s5 =sinf(d2r(tt5));
float s6 =sinf(d2r(tt6));
float d6=ROBOT_D6;
float d4=ROBOT_D4;
float d2=ROBOT_D2;
float a3=ROBOT_A3;
float a2=ROBOT_A2;

float px =d6*(c5*(c1*c2*s3 + c1*c3*s2) - s5*(s1*s4 - c4*(c1*c2*c3 c1*s2*s3))) - d2*s1 + d4*(c1*c2*s3 + c1*c3*s2) + a2*c1*c2 - a3*c1*s2*s3 +
a3*c1*c2*c3;
float py =c1*d2 + d6*(c5*(c2*s1*s3 + c3*s1*s2) + s5*(c1*s4 +
c4*(c2*c3*s1 - s1*s2*s3))) + d4*(c2*s1*s3 + c3*s1*s2) + a2*c2*s1 - a3*s1*s2*s3
+ a3*c2*c3*s1;
float pz =d4*(c2*c3 - s2*s3) - a2*s2 + d6*(c5*(c2*c3 - s2*s3) c4*s5*(c2*s3 + c3*s2)) - a3*c2*s3 - a3*c3*s2;
frm->SetOrigin (CPoint3D(px,py,pz));
}
Một số kết quả mô phỏng:
- Điểm tác động cuối chạy theo đường xoắn ốc:

22


- Biên dạng chạy theo đường helixcal:

23


- Mô phỏng gắp một vật:

24


25