BTL TÍNH TOÁN THIẾT KẾ ROBOT GẮP CHI TIẾT

1. MỤC LỤC
LỜI MỞ ĐẦU 4
CHƯƠNG I: PHÂN TÍCH LỰA CHỌN CẤU TRÚC ROBOT 5
I/ Phân tích mục đích ứng dụng của Robot: 5
II/ Phân tích yêu cầu kĩ thuật thao tác: 6
2.1. Đối tượng thao tác, dạng thao tác: 6
2.2. Yêu cầu về vị trí: 6
2.3. Yêu cầu về hướng của khâu thao tác: 6
2.4 Yêu cầu về vận tốc, gia tốc khi thao tác: 6
2.5 Yêu cầu về không gian thao tác: 6
III/ Xác định các đặc trưng kĩ thuật: 7
3.1. Số bậc tự do cần thiết: 7
3.2. Vùng làm việc có thể với tới của Robot: 7
3.3. Yêu cầu về tải trọng: 7
IV/ Các phương án thiết kế cấu trúc Robot, cấu trúc các khâu khớp, phân tích,
chọn phương án thực hiện: 7
4.1. Các phương án thiết kế cấu trúc Robot: 7
4.2. Phân tích và lựa chọn cấu trúc Robot: 8
5.1. Thiết kế 3D: 9
5.2. Các thông số đặc trưng hình học – khối lượng: 9
CHƯƠNG II: THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG 10
I/ Khảo sát động học thuận, khảo sát động học ngược: 10
1.1. Đặt hệ trục tọa độ theo Denavit – Hartenberg 10
1.2. Thiết lập bảng DH 11
1.3. Ma trận Denavit – Hartenberg 12
1.4. Bài toán động học thuận robot 13
1.5. Khảo sát bài toán động học ngược 19
II/ Xây dựng quy luật chuyển động thao tác của robot. 20
CHƯƠNG III: BÀI TOÁN TĨNH HỌC 27
CHƯƠNG IV: BÀI TOÁN ĐỘNG LỰC HỌC 33
I/ Phương trình Lagrange dạng ma trận: 33
~ 1 ~

2. II/ Thiết lập phương trình vi phân chuyển động của robot: 37
CHƯƠNG V: THIẾT KẾ HỆ DẪN ĐỘNG CHO ROBOT 43
I/ Tính toán hệ dẫn động: 44
1.1. Tính chọn động cơ cho khớp 1: 45
II/ Tính toán thiết kế bộ truyền bánh răng 50
2.1. Xác định khoảng cách trục 50
2.2. Xác định thông số ăn khớp 51
III/ Kiểm nghiệm bộ truyền bánh răng 52
3.1. Kiểm nghiệm độ bền tiếp xúc 52
3.2. Kiểm nghiệm độ bền uốn 53
IV/ Tính toán thiết kế khớp nối: 56
4.1. Tính chọn khớp nối 56
4.2. Chọn khớp nối 56
V/ Tính toán thiết kế trục: 57
5.1. Xác định lực và sơ đồ đặt lực 57
5.2. Xác định đường kính trục 57
5.3. Kiểm nghiệm độ bền trục 58
VI/ Chọn ổ lăn: 62
VII/ Thiết kế 3D và kiểm nghiệm bền các khâu: 64
CHƯƠNG VI: THIẾT KẾ ĐIỀU KHIỂN ROBOT 68
I/ Điều khiển phản hồi và điều khiển vòng kín: 68
II/ Thiết kế bộ điều khiển PD 69
2.1. Bộ điều khiển PD không bù trọng lực: 69
2.2. Bài toán điều khiển 69
KẾT LUẬN 76
TÀI LIỆU THAM KHẢO 77
~ 2 ~

3. LỜI MỞ ĐẦU
Chúng ta đang sống trong thời đại nền công nghiệp 4.0 - nền công nghiệp sản
xuất hàng loạt với sự trợ giúp của robot, máy tự động dưới sự điều khiển của máy tính.
Nền khoa học kỹ thuật ngày căng phát triển mạnh mẽ dẫn tới thay đổi lớn trong sản
xuất. Sự thay đổi lực lượng sản xuất trong mọi ngành nghề, sự thay thế hoạt động chân
tay của con người bằng máy móc, robot.
Robot có được vị trí như vậy là nhờ chúng có những ưu điểm đặc biệt về chất lượng,
độ chinh xác và tính kinh tế. Robot có thể làm việc không bị ảnh hưởng bởi những yếu
tố chủ quan, khách quan như có thể làm việc không biết mệt mỏi, làm việc trong môi
trường ô nhiễm, độc hại, làm việc nơi có nhiệt độ/áp suất cao, làm việc nơi nguy hiểm,
… không những vậy nhờ những thiết kế cơ khí chính xác và những thuật toán điều
khiển mà robot có khả năng làm những công việc yêu cầu độ cẩn thận, tinh tế, làm
việc chính xác không có sự nhầm lẫn như con người.
Tay máy Robot đã có mặt trong sản xuất từ nhiều năm trước, ngày nay tay máy Robot
đã dùng ở nhiều lĩnh vực sản xuất, xuất phát từ những ưu điểm mà tay máy Robot đó
và đúc kết lại trong quá trình sản xuất làm việc, tay máy 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, có thể làm việc trong môi trường
độc hại, v.v…Do đó việc đầu tư nghiên cứu, chế tạo ra những tay máy 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à tương lai.
Ngày nay việc dùng robot ứng dụng trong ngành công nghiệp cơ khí nặng là một trong
những nhu cầu rất cần thiết , việc di chuyển một cách khéo léo và chính xác là điều
khó khăn đối với con người, chính vì thế mà việc nghiên cứu chế tạo ra một thiết bị
như cánh tay robot để làm được việc đó có ý nghĩa rất lớn.
Việc tìm hiểu nghiên cứu Robot trong khuôn khổ môn học Tính toán thiết kế robot sẽ
là cơ sở để chúng em tính toán, thiết kế cũng như điều khiển các loại Robot trong công
nghiệp phục vụ sản xuất. Cụ thể, ở đây chúng em chọn đề tài tính toán, thiết kế mô
hình Robot ứng dụng gắp chi tiết đúc từ máy công nghệ đưa ra vị trí tháo khuôn.
~ 3 ~

4. CHƯƠNG I: PHÂN TÍCH LỰA CHỌN CẤU TRÚC
ROBOT
I/ Phân tích mục đích ứng dụng của Robot:
Trong ngành công nghiệp sản xuất cơ khí nặng, Công nghệ Đúc là công nghệ
chế tạo sản phẩm bằng phương pháp rót vật liệu ở dạng chảy lỏng vào khuôn để tạo ra
sản phẩm có hình dạng theo khuôn mẫu. Đa phần công nghệ đúc thực hiện với các vật
liệu kim loại. Việc gắp chi tiết đúc từ máy công nghệ đưa ra vị trí tháo khuôn là những
công việc lặp đi lặp lại rất đơn giản và nhàm chán, nếu sử dụng công nhân trực tiếp
làm việc này thì năng suất đạt được không cao, đồng thời gây căng thẳng mệt mỏi, ảnh
hưởng đến chất lượng và tiến độ công việc. Ngoài ra việc áp dụng khoa học công nghệ
sử dụng robot thay thế con người trong công việc này đang là xu thế, robot làm việc
không biết mệt mỏi, không bị ảnh hưởng bởi tâm lí và ít bị tác động bởi môi trường,
năng suất cao, độ chính xác tin cậy cao, tính ra giá trị lợi nhuận thu về thì sử dụng
robot rẻ hơn lao động con người rất nhiều. Do vậy, nhóm chúng em quyết định sẽ thiết
kế một mô hình robot để ứng dụng trong việc gắp chi tiết đúc từ máy công nghệ đưa ra
vị trí tháo khuôn.
II/ Phân tích yêu cầu kĩ thuật thao tác:
2.1. Đối tượng thao tác, dạng thao tác:
Đối tượng thao tác ở đây là chi tiết đúc được sản xuất từ máy công nghệ.
Dạng thao tác: Khâu tác động cuối cùng để gắp chi tiết đúc là hệ thống kẹp chi tiết và
nhấc lên vuông góc với bề mặt chứa chi tiết đúc và di chuyển theo chiều tịnh tiến.
~ 4 ~

5. 2.2. Yêu cầu về vị trí:
Các chi tiết đúc khi được sản xuất từ máy công nghệ nằm trên một băng chuyền, khâu
tác động cuối của robot phải đưa đến đúng vị trí để tháo khuôn.
2.3. Yêu cầu về hướng của khâu thao tác:
Hướng của khâu thao tác không thể tuỳ ý mà phải phù hợp với hướng định trước của
chi tiết đúc được sản xuất ra từ máy công nghệ trên băng chuyền. Hướng của khâu tác
động cuối nằm trên mặt phẳng song song với mặt đất XoYo, định hướng bằng các góc
quay quanh trục Zo.
2.4 Yêu cầu về vận tốc, gia tốc khi thao tác:
Yêu cầu về vận tốc: Mỗi chu kỳ làm việc khoảng 4-5 giây.
Yêu cầu về gia tốc: Gia tốc của robot không được quá cao và thay đổi đột ngột có thể
làm chi tiết đúc ra khỏi đầu kẹp của khâu tác động cuối.
2.5Yêu cầu về không gian thao tác:
Băng chuyền đã có, mỗi băng chuyền rộng 500mm, cao 1000mm, nằm vuông góc với
nhau. Không gian thao tác yêu cầu của robot cần phải bao quát được cả 2 bằng chuyển
để có thể lấy từ bằng chuyền 1 đưa sang băng chuyền 2 một cách dễ dàng. ( Ở đây
băng truyền 1 chính là vị trí ban đầu của chi tiết đúc khi vừa được sản xuất từ máy
Công nghệ, băng truyền 2 là vị trí mình đưa chi tiết đến để tháo khuôn)
III/ Xác định các đặc trưng kĩ thuật:
3.1. Số bậc tự do cần thiết:
Nhiệm vụ của robot là gắp các chi tiết đúc được đưa ra từ máy công nghệ rồi nhận biết
và đưa nó về vị trí để tháo khuôn sao cho đúng vị trí yêu cầu. Robot dự định thiết kế sẽ
là robot 3 bậc tự do RRT,RTT,TTT,TRT...
3.2. Vùng làm việc có thể với tới của Robot:
Là vị trí xa nhất mà robot có thể với tới, đó sẽ là vị trí chi tiết đúc ở vị trí xa nhất khi
được đưa ra từ máy công nghệ trên băng chuyền.
3.3. Yêu cầu về tải trọng:
Trong thực tế, chi tiết đúc ta có thể ước tính nặng trung bình khoảng 1kg/1chi tiết, mỗi
lần Robot thao tác được với 4 chi tiết, như vậy tải trọng ngoài tác dụng lên Robot sẽ là
4kg. Ta lấy tải trọng tính toán ngoài là 4 kg.
IV/ Các phương án thiết kế cấu trúc Robot, cấu trúc các khâu
khớp, phân tích, chọn phương án thực hiện:
Dựa vào yêu cầu kỹ thuật thao tác nhóm chúng em lên ý tưởng Robot dự định thiết kế
sẽ là robot 3 bậc tự do RRT,RTT,TTT,TRT...
~ 5 ~

6. Dự định thiết kế tay gắp chi tiết đúc sẽ là hệ thống kẹp và nhấc lên vuông góc với bề
mặt chứa các chi tiết đúc và di chuyển theo chiều tịnh tiến.
4.1. Các phương án thiết kế cấu trúc Robot:
Phương án 1 Phương án 2
~ 6 ~

7. Phương án 3 Phương án 4
4.2. Phân tích và lựa chọn cấu trúc Robot:
Như vậy để robot có thể hoạt động đúng yêu cầu thì cần có ít nhất 3 bậc tự do: 2 bậc
để xác định tọa độ điểm trong mặt phẳng, 1 bậc để xác định chiều cao trong không
gian.
Sau khi phân tích ,trong các cấu trúc đề xuất, ta thấy cấu trúc 2,3,4 kém linh hoạt do có
2 khâu tịnh tiến. Tuy nhiên cấu trúc 1 là đơn giản, gọn hơn cả. Chính vì thế nhóm lựa
chọn cấu trúc 1: Robot 3 bậc tự do RRT, gồm 2 khâu quay để xác định vị trí chi tiết
đúc trên mặt phẳng, 1 khâu tịnh tiến để xác định độ cao chi tiết đúc.
Khớp 1 là khớp quay, dùng bộ truyền bánh răng trụ răng thẳng.
Khớp 2 là khớp quay, dùng bộ truyền bánh răng trụ răng thẳng.
Khớp 3 là khớp tịnh tiến, dùng xi lanh khí nén.
Hành Trình: Khâu 1: 0-90 độ
Khâu 2: 0-90 đọ
Khâu 3: 300mm
~ 7 ~

8. V/ Thiết kế mô hình Robot:
5.1. Thiết kế 3D:
Dựa trên sự phân tích ở phần trước, nhóm đã thiết kế mô hình 3D trên phần mềm
Solidworks
5.2. Các thông số đặc trưng hình học – khối lượng:
mm
~ 8 ~

9. CHƯƠNG II: THIẾT KẾ QUỸ ĐẠO CHUYỂN
ĐỘNG
I/ Khảo sát động học thuận, khảo sát động học ngược:
1.1. Đặt hệ trục tọa độ theo Denavit – Hartenberg
Đối với các robot công nghiệp, Denavit – Hartenberg (1995) đã đưa ra cách chọn các
hệ trục tọa độ có gốc tại khớp thứ i như sau:
- Trục zi-1 được chọn dọc theo trục của khớp động thứ i.
- Trục xi-1 được chọn dọc theo đường vuông góc chung của 2 trục zi-2 và zi-1,
hướng đi từ trục zi-2 sang zi-1. Nếu trục zi-1 cắt trục zi-2 thì hướng của trục xi-1
được chọn tùy ý, miễn là vuông góc với zi-1. Khi 2 trục zi-1 và zi-2 song song với
nhau, trục xi-1 có thể chọn hướng theo pháp tuyến chung nào cũng được.
- Gốc tọa độ Oi-1 được chọn tại giao điểm của trục xi-1 và trục zi-1
- Trục yi-1 được chọn sao cho hệ (Oxyz)i-1 là hệ quy chiếu thuận.
Chú ý:
- Đối với hệ tọa độ (Oxyz)0 theo quy ước trên ta mới chỉ chọn được trục z0, còn
trục x0 chưa có trong quy ước trên. Ta có thể chọn trục x0 được chọn một cách
tùy ý, miễn là x0 vuông góc với z0.
- Đối với hệ tọa độ (Oxyz)n, do không có khớp n+1, nên theo quy ước ta không
xác định được trục zn. Trục zn không được xác định duy nhất, trong khi trục xn
lại được chọn theo pháp tuyến của trục zn-1. Trong trường hợp này, nếu khớp là
khớp quay ta có thể chọn trục zn song song với trục zn-1. Ngoài ra ta có thể chọn
tùy ý sao cho hợp lý.
- Khi khớp thứ i là khớp tịnh tiến, về nguyên tắc ta có thể chọn trục zi-1 một cách tùy
ý. Người ta thường chọn trục zi-1 dọc theo trục của khớp tịnh tiến này.
Từ quy tắc trên ta xây dựng được các hệ tọa độ như hình vẽ:
~ 9 ~

10. Hình 2.1 Hệ trục tọa độ của robot
1.2. Thiết lập bảng DH
Vị trí của hệ tọa độ khớp (Oxyz)i đối với hệ tọa độ khớp (Oxyz)i-1 được xác định
bởi bốn tham số Denavit- Hartenberg , , ,
i i i i
d a
  như sau:
- i
 : góc quay quanh trục để trục trùng với trục ()
- i
d : dịch chuyển tịnh tiến dọc trục để gốc tọa độ chuyển đến là giao điểm của
trục và trục
- i
a : dịch chuyển dọc trục để điểm chuyển đến điểm
- i
 : góc quay quanh trục sao cho trục () trùng với trục
Hình 2.2. Các tham số Denavit – Hartenberg
Như vậy từ các hệ tọa độ đã xây dựng ở trên, ta có các tham số DH như sau:
Bảng 2.1. Các tham số Denavit – Hartenberg
~ 10 ~

11. Khâu i
 i
d i
a i

1 1
 1
d 1
a 0
2 2
 2
d 2
a 0
3 0 3
d 0 0
Trong đó 1 2 3
, ,d
  là các biến khớp, còn 1 2 1 2
a ,a ,d ,d
là hằng số. Ta đặt
1 1 2 2 3 3
, ,
  
q q d q
 
1.3. Ma trận Denavit – Hartenberg
Ta có thể chuyển hệ tọa độ khớp (Oxyz)i-1 sang hệ tọa độ (Oxyz)i bằng bốn phép
biến đổi cơ bản như sau:
- Quay quanh trục zi-1 một góc i
 .
- Dịch chuyển tịnh tiến dọc trục zi-1 một đoạn di.
- Dịch chuyển tịnh tiến dọc trục xi một đoạn ai.
- Quay quanh trục xi một góc i
 .
Như vậy ma trận của phép biến đổi hệ tọa độ khớp (Oxyz)i-1 sang hệ tọa độ (Oxyz)i ,
kí hiệu là
1
i
i
A

, là tích của 4 ma trận biến đổi cơ bản và có dạng như sau:
1
cos sin 0 0 1 0 0 0 1 0 0 1 0 0 0
sin cos 0 0 0 1 0 0 0 1 0 0 0 cos sin 0
0 0 1 0 0 0 1 0 0 1 0 0 sin cos 0
0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1
cos -cos sin sin sin cos
sin cos co
i i i
i i i i
i
i
i i i
i i i i i i i
i i
a
A
d
a
 
   
 
     
 


� �
� �
� �
� �
� �
� �
� �
� �

� �
� �
� �
� �
� �
� �
� �
� �
� �
� �
� �
� �
� �
� �
� �
� �

=
 
s -sin cos sin
2.1
0 sin cos
0 0 0 1
i i i i i
i i i
a
d
   
 
� �
� �
� �
� �
� �
� �
Ma trận được xác định bởi công thức (2.1) được gọi là ma trận Denavit – Hartenberg.
Nó cho ta biết thông tin về vị trí của khâu thứ i của robot đối với hệ quy chiếu (Oxyz)i-
1.
Áp dụng liên tiếp các phép biến đổi (2.1) đối với robot n khâu ta có:
0
0 0 1 1
1 2
...
0 1
n n E
n n T
R r
A A A A
 � �
  � �
� � (2.2)
~ 11 ~

12. trong đó:  
T
E E E E
r x ,y ,z

là véc tơ mô tả vị trí điểm tác động cuối trong hệ tọa độ
(Oxyz)0
0
n
R là ma trận cosin chỉ hướng của khâu thao tác đối với hệ tọa độ (Oxyz)0
1.4. Bài toán động học thuận robot
Nhiệm vụ của bài toán động học thuận là xác định vị trí của khâu thao tác, hay nói
cách khác là vị trí điểm tác động cuối và hướng của khâu thao tác đối với hệ tọa độ cố
định với điều kiện các biến khớp đã biết. Ở đây ta sẽ xác định từ ma trận DH của khâu
thao tác
0
3
A
Tính các ma trận DH
Đặt cosq1 = C1, sinq1 = S1, cosq2 = C2, sinq2 = S2, cos(q1 + q2) = C12, cos(q2 + q3) = C23,
và thay các giá trị trong bảng thông số DH vào công thức (2.1) và ta được các ma trận
biến đổi DH giữa các hệ trục tọa độ như sau:
1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1
0
1
1 1
cos( ( )) sin( ( )) 0 cos( ( )) 0
sin( ( )) cos( ( )) 0 sin( ( )) 0
0 0 1 0 0 1
0 0 0 1 0 0 0 1
q t q t a q t C S a C
q t q t a q t S C a S
A
d d
 
� � � �
� � � �
� � � �
 
� � � �
� � � �
� � � �
(2.3)
2 2 2 2 2 2 2 2
2 2 2 2 2 2 2 2
1
2
2 2
cos( ( )) sin( ( )) 0 cos( ( )) 0
sin( ( )) cos( ( )) 0 sin( ( )) 0
0 0 1 0 0 1
0 0 0 1 0 0 0 1
q t q t a q t C S a C
q t q t a q t S C a S
A
d d
 
� � � �
� � � �
� � � �
 
� � � �
� � � �
� � � �
2
3
3 3
1 0 0 0 1 0 0 0
0 1 0 0 0 1 0 0
0 0 1 ( ) 0 0 1
0 0 0 1 0 0 0 1
A
q t d
� � � �
� � � �
� � � �
 
� � � �
� � � �
� � � �
1 2 1 2 2 1 2 1 1
1 2 1 2 2 1 2 1 1
0 0 1
2 1 2
1 2
12 12 1 1 2 12
12 12 1 1 2 1
cos( ( ) ( )) sin( ( ) ( )) 0 cos( ( ) ( )) cos( ( ))
sin( ( ) ( )) cos( ( ) ( )) 0 sin( ( ) ( )) sin( ( ))
.
0 0 1
0 0 0 1
0
0
q t q t q t q t a q t q t a q t
q t q t q t q t a q t q t a q t
A A A
d d
C S a C a C
S C a S a S
    
� �
� �
   
� �
 
� �

� �
� �
 


   
0 0
2 2 2
1 2
( )
0 0 1 0 1
0 0 0
.4
1
2
T
R q r q
d d
� �
� � � �
� �
 � �
� �
 � �
� �
� �
~ 12 ~

13.    
12 12 1 1 2 12
0 0
12 12 1 1 2 12
0 0 1 2 3 3
3 1 2 3
1 2 3
0
0
. .
0 0 1 0 1
0 0 0 1
T
C S a C a C
S C a S a S R q r q
A A A A
d d q
 
� �
� �
 � �
� �
   � �
� �
  � �
� �
� �
(2.5)
Xác định vị trí điểm tác động cuối:
Từ (2.2) và (2.5) suy ra:
 
2 1 2 1 1 2 1 2 1 1 3 2 1
cos( ) cos( ) sin( ) sin( )
T
E
r a q q a q a q q a q q d d
      
Mà  
T
E E E E
r x ,y ,z

nên:
2 1 2 1 1
2 1 2 1 1
3 2 1
cos( ) cos( )
sin( ) sin( )
(2.6)
E
E
E
x a q q a q
y a q q a q
z q d d
  
  
  
Xác định hướng khâu thao tác.
Theo (2.2) và (2.5) suy ra:
1 2 1 2
0
3 1 2 1 2
cos( ) sin( ) 0
sin( ) cos( ) 0
0 0 1
q q q q
R q q q q
  
� �
� �
  
� �
� �
� �
Hướng của khâu thao tác so với hệ tọa độ (Oxyz)0 được xác định qua ma trận
0
3
R .
Ngoài ra còn có thể xác định qua các gócα,β,η trong phép quay Cardan.
Chú thích: Phép quay Cardan là phép biến đổi hệ tọa độ cố định (Oxyz)i sang hệ tọa độ
động (Oxyz)i+3 bằng cách quay liên tiếp quanh các trục của hệ tọa độ động, cụ thể là:
- Quay (Oxyz)i góc α quanh trục xi. Hệ (Oxyz)i (Oxyz)i+1
- Quay (Oxyz)i+1 góc β quanh trục yi+1. Hệ (Oxyz)i+1(Oxyz)i+2
- Quay (Oxyz)i+2 góc η quanh trục zi+2. Hệ (Oxyz)i+2 (Oxyz)i+3
~ 13 ~

14. Hình 2.3. Các góc quay Cardan
Ma trận quay Cardan biểu diển hướng của hê tọa độ động (Oxyz)i+3 so với hệ tọa độ
cố định (Oxyz)I là tích của ba ma trận quay thành phần cơ bản, được tính
theo công thức sau:
 
cos cosη -cos sin sin
sin sin cos +cos sin -sin sin sin +cos cos -sin cos 2.7
-cos sin cos +sin sin cos sin sin +sin cos cos cos
CD
R
   
           
           
� �
� �
� �
� �
� �

Như vậy bằng cách so sánh 2 ma trận quay CD
R
và 3
0
R , ta có thể tìm được các góc
Cardan , ,
   biểu diễn hướng của khâu thao tác đối với hệ tọa độ cố định (Oxyz)0.
0
3 CD
R R
 hay:
1 2 1 2
1 2 1 2
cos(q +q ) -sin(q +q ) 0 cosβcosη -cosβsinη sinβ
sin(q +q ) cos(q +q ) 0 = sinαsinβcosη+cosαsinη -sinαsinβsinη+cosαcosη -sinαcosβ
0 0 1 -cosαsinβcosη+sinαsinη cosαsinβsinη+sinαcosη cosαcosβ
� � � �
� � � �
� � � �
� � � �
� � � �
So sánh các phần tử tương ứng của 2 ma trận, kết hợp hệ trục tọa độ trong hình 2.2 ta có:
 
1 2
= 0, = , = 2.8
q q
    
Tính vận tốc, gia tốc điểm tác động cuối và vận tốc góc, gia tốc góc các khâu.
Sau khi đã biết các biến khớp 1 2 3
q , q , q và các vận tốc khớp, gia tốc khớp, ta sẽ đi tính
vận tốc góc, gia tốc góc các khâu cũng như vận tốc, gia tốc điểm tác động cuối.
- Vận tốc, gia tốc điểm thao tác cuối E
v :
 
T
E E E E E
v =r = x y z
& & & &
(2.9)
 
T
E E E E E
a =v = x y z
& &
& &
& &
&
~ 14 ~


O




1
i
z 
2 3
,
i i
z z
 
i
y
1
,
i i
x x 
2
i
x  3
i
x 
3
i
y 
1 2
,
i i
y y
 
i
z

15. - Vận tốc góc, gia tốc góc các khâu:
Toán tử sóng của véc tơ vận tốc góc khâu i trong hệ tọa độ động (Oxyz)i được tính
theo công thức sau:
11 12 13
i 0 T 0
i i i 21 22 23
31 32 33
ω ω ω
ω = R R = ω ω ω
ω ω ω
� �
� �
� �
� �
� �
% % %
&
% % % %
% % %
(2.10)
Khi đó vận tốc góc của khâu i tính trong hệ tọa độ động (Oxyz)i là:
 
T
i
i 32 13 21
ω = ω ω ω
% % %
(2.11)
a, Tính vận tốc góc, gia tốc góc các khâu
- Xét khâu 1, từ (2.2) và (2.3) ta có:
1 1
0
1 1 1
C -S 0
R = S C 0
0 0 1
� �
� �
� �
� �
� �
Suy ra:
1 1 1 1 1 1 1
1 0 T 0
1 1 1 1 1 1 1 1 1 1
C S 0 -q S -q C 0 0 -q 0
ω = R R = -S C 0 q C -q S 0 = q 0 0
0 0 1 0 0 0 0 0 0
� �
� � � �
� �
� � � �
� �
� � � �
� �
� � � �
� �
� � � �
& & &
&
% & & &
Như vậy vận tốc góc và gia tốc góc của khâu 1 tính trong hệ tọa độ động gắn với
khâu là:
 
 
T
1
1 1
1 1
1 1 1
ω = 0 0 q
ε = ω = 0 0 q
&
& &
&
- Xét khâu 2, từ (2.2) và (2.4) ta có:
12 12
0
2 12 12
C -S 0
R = S C 0
0 0 1
� �
� �
� �
� �
� �
Suy ra:
 
 
12 12 1 2 12 1 2 12 1 2
2 0 T 0
2 2 2 12 12 1 2 12 1 2 12 1 2
C S 0 -(q +q )S -(q +q )C 0 0 - q +q 0
ω = R R = -S C 0 (q +q )C -(q +q )S 0 = q +q 0 0
0 0 1 0 0 0 0 0 0
� �
� �
� �
� �
� �
� �
� �
� �
� �
� �
� �
� �
� �
� � � �
& & & & & &
&
% & & & & & &
Như vậy vận tốc góc, gia tốc góc của khâu 2 tính trong hệ tọa độ động gắn chặt với khâu là :
~ 15 ~

16.  
 
T
2
2 1 2
T
2 2
2 2 1 2
ω 0 0 q +q
ε = ω 0 0 q +q
=
=
& &
& &
& &
&
- Xét khâu 3, từ (2.2) và (2.5) ta có:
12 12
0
3 12 12
C -S 0
R = S C 0
0 0 1
� �
� �
� �
� �
� �
Suy ra:
 
12 12 1 2 12 1 2 12 1 2
3 0 T 0
3 3 3 12 12 1 2 12 1 2 12 1 2
C S 0 -(q +q )S -(q +q )C 0 0 -(q +q ) 0
ω = R R = -S C 0 (q +q )C -(q +q )S 0 = q +q 0 0
0 0 1 0 0 0 0 0 0
� �
� � � �
� �
� � � �
� �
� � � �
� �
� � � �
� �
� � � �
& & & & & &
&
% & & & & & &
Như vậy vận tốc góc, gia tốc góc của khâu 3 tính trong hệ tọa độ động gắn chặt với
khâu là :
 
 
T
1 2
T
3
3 3 1 2
3
3
3
0 0 q +q
ε = 0 0 q +q
ω =
ω =
& &
&
& &
&
&
b, Tính vận tốc, gia tốc điểm tác động cuối
Từ (2.6) ta có véc tơ vị trí điểm tác động cuối là:
E 2 12 1 1
E E 2 12 1 1
E 3 2 1
x a C +a C
r = y = a S +a S
z q +d +d
� �
� �
� �
� �
� �
� �
� �
� �
� � � �
Lần lượt đạo hàm E
r theo thời gian, ta thu được vận tốc và gia tốc điểm tác cuối là:
2 1 2 12 1 1 1
E E 2 1 2 12 1 1 1
3
- a (q +q )S -a q S
v =r = a (q +q )C + a q C
q
� �
� �
� �
� �
� �
� �
� �
& & &
& & & &
&
 
 
2
2
2 1 2 12 1 1 1 1 1 1 2 12 1 2
2 2
E E 2 12 1 2 2 1 2 12 1 1 1 1 1
3
- a (q +q )S - a q S -a C q -a C q +q
a =v = -a S q +q +a (q +q )C - a q S +a C q
q
� �
� �
� �
� �
� �
� �
� �
&
& &
& &
& & & &
& & &
& &
& & &
&
&
&
&
Mô phỏng bằng matlab động học thuận
Chọn quy luật chuyển động của các khâu của robot
~ 16 ~

17. Cho các hằng số: d1=600mm, a1=500mm, a2=400mm, d2=10mm
Các biến khớp:
1 2 3
3 3
; ; 5 , 0,1,2,...
2 100 4 200
q t q t q t t
   
       
Cho t=[0,100];
Khi đó vị trí của điểm tác động cuối:
5
400sin( ) 500sin( )
4 200 100
5
400cos( ) 500cos( )
4 200 100
5 610
xE t t
yE t t
zE t
 

 

   
  
 
Hình 2.4. Đồ thị q1(t) Hình 2.5. Đồ thị q2(t)
Hình 2.6. Đồ thị q3(t) Hình 2.7. Đồ thị xE
~ 17 ~

18. Hình 2.10. Miền làm việc của điểm cuối trong không gian (x,y,z)
1.5. Khảo sát bài toán động học ngược
Nhiệm vụ của bài toán động học ngược là tìm các biến khớp với điều kiện tọa
độ và hướng khâu thao tác đã biết. Cụ thể trong bài toán này, bài toán động học ngược
có nhiệm vụ tìm 1 2 3
q , q , q với điều kiện ,
E E E
x ,y z đã biết.
tacó:
2 12 1 1
2 12 1 1
3 2 1
=
=
E
E
E
x a C a C
y a S a S
z q d d

�
�

�
�   
�
-Bình phương 2 vế của phương trình
,
E E
x y
ta có:
2 2 2 2 2
2 12 1 1 2 12 1 1
2 2 2 2 2
2 12 1 1 2 12 1 1
2
2
E
E
x a C a C a C a C
y a S a S a S a S
�   
�
  
�
~ 18 ~
Hình 2.8. Đồ thị yE Hình 2.9. Đồ thị zE

19. 2 2 2 2
1 2 2 1 12 1 12 1
2 2 2 2
1 2 2 1 2
2 ( )
2
E E
E E
x y a a a a C C S S
x y a a a a C
    
�
   
2 2 2 2
1 2
2
2 1
cos(q )=
2
E E
x y a a
a a
  
2
2 2
( ) 1
sin q C
 
�
2 2 2
atan 2(sin(q ),cos(q ))
q 
Ta thay sin(q2),cos(q2) vào 2 phương trình
,
E E
x y
ta có:
2 2 1 1 2 1 2
2 2 1 2 2 1 1
= ( )
= +( )S
E
E
x a C a C a S S
y a S C a C a
 
�
�

�
Giải hệ phương trình đại số tuyến tính:
1 2 2 2 2 2 2 2 2
1 2 1 1 2
2 2 1 2 2
2 E E
a C a S a
a a C a a x y
a S a C a
 
      

2 2
1 2 2 2 1
1 2 2
(C )
E
E E E
E
x S a
a x S y x a
y a C a

    

1 2 2
2 2 2 2 1
2 2
(C )
E
E E E
E
a C a x
a y S x y a
S a y

    
Từ đó ta có:
2 2 2 1
1 2 2
(C )
os(q )= E E E
E E
a x S y x a
c
x y
 
�

2 2 2 1
1 2 2
(C )
sin( )= E E E
E E
a y S x y a
q
x y
 

1 1 1
atan 2(S ,C )
q 
Từ: 3 2 1 3 2 1
( )
E E
z q d d q z d d
      
Như vậy đã giải quyết xong bài toán động học ngược.
II/ Xây dựng quy luật chuyển động thao tác của robot.
2.1. Khái niệm, nhiệm vụ của thiết kế quỹ đạo
Thiết kế quỹ đạo chuyển động là xây dựng quy luật chuyển động cho các khâu của
robot đảm bảo thỏa mãn yêu cầu về vị trí, hướng, hoặc thỏa mãn cả vị trí và hướng của
~ 19 ~

20. khâu thao tác trong không gian theo thời gian. Mục tiêu là tạo ra bộ tham số đầu vào
cho hệ thống điều khiển chuyển động để đảm bảo Robot thực hiện theo quỹ đạo đã
được lập trình.
Có hai kỹ thuật thiết kế quỹ đạo chính là:
-Thiết kế quỹ đạo trong không gian khớp.
-Thiết kế quỹ đạo trong không gian thao tác.
Các thông số của Robot:
Chiều dài các khâu: a1=0,5 m, a2=0,4m,d1=0,6m,d2=0,01m
Giới hạn các góc khớp:
q1 [min = 2


, max = 2

] (rad)
q2 [min =
3
4


, max =
3
4

] (rad)
q3 [min =0, max = 500] (mm)
2.2. Thiết kế quỹ đạo trong không gian khớp
Thiết kế quỹ đạo trong không gian khớp cho Robot
Trong trường hợp chuyển động từ điểm tới điểm, người ta có thể quan tâm hoặc không
quan tâm đến việc khâu thao tác đi qua một số điểm trung gian. Khi đó kỹ thuật thiết
kế quỹ đạo trong không gian khớp hay được sử dụng. Thiết kế quỹ đạo trong không
gian khớp đòi hỏi phải xây dựng được các hàm phụ thuộc thời gian của các biến khớp
và các đạo hàm bậc nhất, bậc hai của nó. Qua đó mô tả được chuyển động cần thiết
của Robot.
a. Đề bài
Đối với Robot, ta chọn nội dung thiết kế quỹ đạo như sau: Chọn hai điểm A, B bất
kì trong không gian làm việc, biết tọa độ (xE, yE, zE), hướng của khâu thao tác  và thời
gian làm việc tc. Yêu cầu xây dựng hàm phụ thuộc thời gian của các biến khớp và đạo
hàm bậc nhất của chúng, sao cho thỏa mãn điều kiện: điểm tác động cuối đi từ điểm A
đến điểm B trong thời gian tc, vận tốc tại điểm A và điểm B đều bằng 0.
b.Lời giải
Giả sử: khi điểm tác động cuối ở điểm A ( 0
, , , ,
A A A A
x y z t t
  ), ta có:
 
0 0,
i
i
q t q
  
0 0
i
i
q t q

& &
tại điểm B ( , , , ,
B B B B c
x y z t t
  ), ta có:
  ,
i
i c c
q t q
   i
i c c
q t q

& &
(2.12)
Chọn hàm phụ thuộc thời gian của biến khớp qi là đa thức bậc ba có dạng:
~ 20 ~

21. 2 3
0 1 2 3
= i i i i
i
q (t) a + a t + a t + a t (2.13)
Trong đó 0 1 2 3
, , ,
i i i i
a a a a làcác hằng số cần xác định.
Theo giả thiết tại thời điểm t0 và tc, Robot đứng yên nên:
0 ; 0
i i
0 c
q q
 
& &
(2.14)
Đạo hàm phương trình (2.13) ta có:
2
1 2 3
i i i
i
q (t)= a + 2 a t +3 a t
&
(2.15)
Sử dụng (2.12), (2.13), (2.14),(2.15) ta nhận được hệ bốn phương trình sau:
2 3
1
2 3
0
2
1 2
1
3
0
0
i i
i 0 0 0
i i i i
i c
i
i 0
i i i
i c c c
i
c c c c
i
c
q (t )= a = q
q (t )=
q (t )= a =
q (t )= a +2 a t +3 a
a + a
t =
t + a t + a t = q
q =
�
�
�
�
�
�
�
&
&
&
Giải các phương trình trên ta được các hệ số :
i i i i
0 1 2 3
a , a , a , a theo các công thứcsau:
0 1 2 3
2 3
; = 0; ; (2.16)
i i i i
i i i i i
0 c 0 c
0
c c
q - q q - q
a = q a a = - 3 a = 2
t t
Từ bốn hệ số trên ta thu được hàm phụ thuộc thời gian của các biến khớp dạng đa
thứcbậc ba (2.13). Những giá trị đó là tín hiệu đặt cho bộ điều khiển vị trí để truyển
động khớp di chuyển đến vị trí tương ứng.
Ví dụ
Bàitoán: Thiết kế quỹ đạo trong không gian khớp cho Robot sao cho điểm tác động cuối
đi từ điểm A
 
50 2; 450 2;610

đến điểm B  
101.2; 341.9;660
 
trong thời gian tc =
10s.
Giải:
Trước hết ta giải bài toán động học ngược Robot, để tìm giá trị cụ thể các biến khớp tại
vị trí các điểm A và B.
Sử dụng các công thức ở tính toán động học ngược, ta tính toán được cụ thể:
Tại điểm
 
50 2; 450 2;610
A 
,suy ra
1 2 3
0 0 0
π π
= - ; = - ; = 0
4 2
q q q
Tại điểm B 
101.2; 341.9;660
 
suyra
3π 3π
= - ; = - ; = 50
10 4
1 2 3
c c c
q q q
a. Xây dựng đa thức phụ thuộc thời gian của các biến khớp.
Sử dụng công thức (2.16) với = 0;
0
t = 10
c
t ta nhận được các hệ số sau:
Khớp 1:
1 1 1 1
0 1 2 3
π -3π π
= - ; =0; = ; =
4 2000 10000
a a a a
~ 21 ~

22. Khớp 2:
2 2 2 2
0 1 2 3
-π -3π π
= ; =0; = ; =
2 400 2000
a a a a
Khớp 3:
3 3 3 3
0 1 2 3
= 0 ; =0; =1.5 ; = -0.1
a a a a
Suy ra:
2 3
1
-π 3π π
= - + ;
4 2000 10000
q (t) t t  
1
-3π 3π
= + ;
1000 10000
2
q t t t
&
 
1
-3π 3π
=
1000 5000
q t t

&
&
2
π 3π π
= - - + ;
2 400 2000
2 3
q (t) t t  
2
-3π 3π
= ;
200 2000
2
q t t t

&
 
2
-3π 3π
=
200 1000
q t t

&
&
3
1
= 1.5 - ;
10
2 3
q (t) t t  
3
3
= 3 - ;
10
2
q t t t
&  
3
3
= 3 -
5
q t t
&
&
Với các đa thức
     
1 2 3
, ,
q t q t q t
đã xây dựng ở trên, ta vẽ được các đồ thị sau:
Hình 2.11. Đồ thị vị trí, vận tốc và gia tốc khớp 1 theo thời gian
~ 22 ~

23. Hình 2.12. Đồ thị vị trí, vận tốc và gia tốc khớp 2 theo thời gian
Hình 2.13. Đồ thị vị trí, vận tốc và gia tốc khớp 3 theo thời gian
2.3. Thiết kế quỹ đạo trong không gian thao tác
Thiết kế quỹ đạo trong không gian thao tác
Trong trường hợp chuyển động liên tục trên đường dịch chuyển, kỹ thuật thiết kế
quỹ đạo trong không gian thao tác hay được sử dụng.
~ 23 ~

24. Đối với Robot này, ta chọn bài toán thiết kế quỹ đạo trong không gian thao tác sao
cho quỹ đạo của điểm tác động cuối là đường thẳng từ điểm A đến điểm B (bất kì
trong vùng làm việc)
Với điểm A có tọa độ A(x0,y0,z0) và điểm B(xc,yc,zc), ta có phương trình đường
thẳng trong không gian thao tác :
 
2.17
c 0 0 c c 0
c 0 c 0
0 0 0
c 0 0 c c 0
c 0 c 0 c 0
c 0 c 0
y - y y x - y x
y = x+
x - x x - x
x - x y - y z - z
= =
z - z z x - z x
x - x y - y z - z
z = x+
x - x x - x
�
�
�
� �
�
�
�
Ta chọn x = x(t)là đa thức bậc 3 có dạng:
  2 3
0 1 2 3
x t a a t a t a t
   
Điều kiện:        
0, 0
0 A c B 0 c
x t = x , x t = x , v t = v t =
Tương tự như thiết kế quỹ đạo trong không gian khớp, ta tìm được các hệ số
0 1 2 3
a , a , a , a theo công thức :
 
   
     
 
0 0
0 0 1 2 3
2 3
3 2
; 0; ; (2.18)
c c
c c
x t x t x t x t
a x t a a a
t t
  
   
Ví dụ
Bài toán:Thiết kế quỹ đạo chuyển động trong không gian thao tác cho Robot, sao cho
điểm tác động cuối di chuyển từ điểm A
 
50 2; 450 2;0

đến điểm B
 
101.2; 341.9;0
 
với quỹ đạo là đường thẳng trong thời gian 10s
Giải:
Ta chọn x = x(t) là đa thức bậc 3 có dạng:
  2 3
0 1 2 3
x t a a t a t a t
   
Điều kiện:        
0, 0
0 A c B 0 c
x t = x , x t = x , v t = v t =
Tương tự như thiết kế quỹ đạo trong không gian khớp, ta tìm được các hệ số
0 1 2 3
a , a , a , a theo công thức :
 
   
     
 
0 0
0 0 1 2 3
2 3
3 2
; 0; ; (2.19)
c c
c c
x t x t x t x t
a x t a a a
t t
  
   
Thay tọa độ của điểm A và điểm B, tc=10 vào công thức (2.19) ta tìm được:
   
0 1 2 3
3 -101.2-50 2 -2 -101.2-50 2
a =50 2; a =0; a = =-5,16; a = =0.34 (2.20)
100 1000
  2 3
x t = 50 2 - 5,16t + 0.34t
341,9 450 2 ( 450 2)( 101,2) 341,9.50 2
101,2 50 2 101,2 50 2
1.71 515.26
y = x+
x
    
   
  
~ 24 ~

25.   2 3
= 636.18 + 8.8t - 0,58t
y t 
Với x(t), y(t), như trên ta vẽ được các đồ thị sau:
Hình 2.14. Đồ thị xE, yE
-Sau khi xác định được quỹ đạo chuyển động ta áp dụng bài toán động học ngược tìm
ra các quỹ đạo chuyển động của biến khớp phụ thuộc theo hàm thời gian t:
Lần lượt là:
2 2 2
atan 2(sin(q ),cos(q ))
q 
1 1 1
atan 2(S ,C )
q 
3 2 1 3 2 1
( )
E E
z q d d q z d d
      
~ 25 ~

26. CHƯƠNG III: BÀI TOÁN TĨNH HỌC
Coi các khâu là thanh đồng chất, tiết diện ngang không đang kể, khối lượng các
khâu lần lượt là m0, m1 = 16(kg), m2 = 8(kg), m3 = 10(kg). Cho các lực và momen tác
dụng vào khâu thao tác E là:
T
0 0
4,3 z
F 0 0 F
� �
 
� � ,
 
T
0
4,3
M 0 0 0

Vị trí tương đối của gốc hệ tọa độ
i i
i 1
r và vị trí trọng tâm của các khâu i
i i
c
r
được xác
định:
3
3 3 3
0 0
2
T
c
d
r
� �
 � �
� �
 
3 3
2 3
0 0 d
T
r 
2 2 2
2 0 0
2
T
c
a
r

� �
 � �
� �  
2 2
1 2
-a 0 0
T
r 
1 1 1
1 0 0
2
T
c
d
r

� �
 � �
� �  
1 1
0 1
0 0 -d
T
r 
~ 26 ~

27. Vectơ gia tốc trọng trường và trọng lực các khâu biểu diễn trong hệ tọa độ cơ sở:
 
 
T
0
i
T
0
i i i i
g 0 0 g
P m g 0 0 m g
 
  
Áp dụng hệ phương trình đệ quy để tính toán lực trong các khớp:
0 0 0
i,i 1 i 1,i
0 0 0 0 0 0
i,i 1 i 1,i 1 i,i 1
. .
i
i
i i
i c i
F F P
M M r F r P
 
   
�  
�
�
  
�
� % %
Ta tính các vectơ i
0 i 0 i
i 1 c
r , r

sau đó thiết lập các ma trận đối xứng lệch i
0 i 0 i
i 1 c
r , r

% %
từ các
vectơ này.
Hệ phương trình cân bằng trong hệ tọa độ cơ sở với hệ lực khâu i:
0 0 0
i,i 1 i 1,i
0 0 0 0 0 0
i,i 1 i 1,i 1 i,i 1
. .
i
i
i i
i c i
F F P
M M r F r P
 
   
�  
�
�
  
�
� % %
Trong đó:
0
, 1
i i
F 
: Lực tác dụng từ khâu i-1 sang khâu i
0
, 1
i i
M 
: Mô men lực tác dụng từ khâu i-1 sang khâu i
0
1,
i i
F
: Lực tác dụng từ khâu i sang khâu i+1
0
1,
i i
M 
: Mômen lực tác dụng từ khâu i sang khâu i+1
0
1
i
i
r
%
: Ma trận sóng của vectơ có gốc tại Oi mút tại Oi-1
0
i
i
c
r
%
: Ma trận sóng của vectơ có gốc tại Oi mút tại Ci
0
i
P : Trọng lực tác dụng lên khâu i
Ta có
~ 27 ~

28. 0
1
2 2
0
2 2 2
2 3 2 3
0
3 2 3 2 3
1 0 0
0 1 0
0 0 1
cos sin 1
sin cos 1
0 0 1
cos( ) sin( ) 0
sin( ) cos( ) 0
0 0 1
R
R
R
 
 
   
   
� �
� �
 � �
� �
� �

� �
� �
 � �
� �
� �
  
� �
� �
  
� �
� �
� �
Phương trình cân bằng hệ lực khâu 3 trong hệ tọa độ cố định ta có:
3
0 0 0
3,2 4,3 3
0 0 0 3 0 0 3 0
3,2 4,3 2 3,2 3
. .
c
F F P
M M r F r P
�  
�
�
  
�
� % %
3
3 3 3
0 0
2
T
c
d
r
� �
 � �
� �  
3 3
2 3
0 0
T
r d
  
0
3 0 0
T
g g
 
 
0
3 3
0 0
T
P m g
 
3 3
0 3 0 3 3
3
3
0
0
2
c c
r R r
d
� �
� �
� �
 
� � �
� �
� �
� �
0 3 0 3 3
2 3 2
3
0
0
r R r
d
� �
� �
 
� � �
� �
� �
3
0 3
2 3
0 0
0 0
0 0 0
d
r d

� �
� �
 � �
� �
� �
%
3
3
0 3 3
0 0
2
0 0
2
0 0 0
c
d
d
r

� �
� �
� �
� �

� �
� �
� �
� �
� �
%
~ 28 ~

29.  
 
0
4,3
0
4,3
0 0
0 0 0
T
z
T
F F
M
 

0
3,2
3
0
0
z
F
F m g
� �
� �
 � �
� �
 
� �
0
3,2
0
0
0
M
��
��

��
��
��
Phương trình cân bằng hệ lực khâu 2 trong hệ tọa độ cố định ta có:
2
0 0 0
2,1 3,2 2
0 0 0 2 0 0 2 0
2,1 3,2 1 2,1 2
. .
c
F F P
M M r F r P
�  
�
�
  
�
� % %
2
2 2 2
0 0
2
T
c
a
r

� �
 � �
� �  
2 2
1 2 0 0
T
r a
 
 
0
2 0 0
T
g g
   
0
2 2
0 0
T
P m g
 
2 2
2 2
0 2 0 2 2 2 2
2
cos
2
sin
.
2
0
c c
a
a
r R r



� �
� �
� �

� �
 
� �
� �
� �
� �
� �
2 2
0 2 0 2 2
1 2 1 2 2
cos
. = sin
0
a
r R r a



� �
� �
 
� �
� �
� �
~ 29 ~

30. 2 2
0 2
1 2 2
2 2 2 2
0 0 sin
0 0 cos
sin cos 0
a
r a
a a


 

� �
� �
 � �
� �

� �
%
2
2 2
0 2 2 2
2 2 2 2
sin
0 0
2
cos
0 0
2
sin cos
0
2 2
c
a
a
r
a a


 

� �
� �
� �
� �

� �
� �

� �
� �
� �
%
0
2,1
2 3
0
0
( )
z
F
F g m m
� �
� �
 � �
� �
  
� �
2
2 2 3
0 2
2,1 2 2 3
sin
2
cos
2
0
z
z
m
a F g m
m
M a F g m


� �
� �
� �
  
� �
� �
� �
� �
� �
� �
� �
� �
� �
    
� �
� �
� �
� �
� �
� �
� �
� �
� �
� �
Phương trình cân bằng hệ lực khâu 1 trong hệ tọa độ cố định ta có:
1
0 0 0
1,0 2,1 1
0 0 0 1 0 0 1 0
1,2 2,1 0 1,0 1
. .
c
F F P
M M r F r P
�  
�
�
  
�
� % %
1
1 1 1
0 0
2
T
c
d
r

� �
 � �
� �  
1 1
0 1
0 0
T
r d
 
 
0
1 0 0
T
g g
   
0
1 1
0 0
T
P m g
 
~ 30 ~

31. 1 1
0 1 0 1 1
1
1
0
0
2
c c
r R r
d
� �
� �
� �
 
� � �
� �

� �
� �
0 1 0 1 1
0 1 0
1
0
0
r R r
d
� �
� �
 
� � �
� �

� �
1
0 1
0 1
0 0
0 0
0 0 0
d
r d
� �
� �
 
� �
� �
� �
%
1
1
0 1 1
0 0
2
0 0
2
0 0 0
c
d
d
r
� �
� �
� �

� �

� �
� �
� �
� �
� �
%
 
0
1,0
1 2 3
0
0
z
F
F g m m m
� �
� �
 � �
� �
   
� �
2
2 2 3
0 2
2,1 2 2 3
sin
2
cos
2
0
z
z
m
a F g m
m
M a F g m


� �
� �
� �
  
� �
� �
� �
� �
� �
� �
� �
� �
� �
    
� �
� �
� �
� �
� �
� �
� �
� �
� �
� �
~ 31 ~

32. CHƯƠNG IV: BÀI TOÁN ĐỘNG LỰC HỌC
I/ Phương trình Lagrange dạng ma trận:
Để tính toán động lực học robot, ta sẽ đi thiết lập phương trình vi phân chuyển
động của robot. Phương trình vi phân chuyển động của robot được xây dựng theo
phương trình Lagrange loại II dạng ma trận có dạng tổng quát như sau:
*
i
d T T
Q 1,2,3,...,n
dt q q q
i
� � 
� � �
    
� �
� � �
� �
&
(4.1)
Với: T – động năng của robot.
 - thế năng của robot.
Qi
*
– lực suy rộng không thế.
1.1. Động năng của robot:
Động năng của robot được tính như sau:
1 2 n i
1
T = T +T +...+T T
n
i
 �
Với i
T là động năng khâu i, tính theo công thức:
T i T i
i Ci i Ci i i i
1 1
T = v m v +ω I ω (4.2)
2 2
Trong đó:
mi – khối lượng khâu i
Ci
v – vận tốc dài của khối tâm khâu i trong hệ tọa độ cố định
i
i
ω - vận tốc góc khâu i tính trong hệ tọa độ động
i
I - ma trận ten xơ quán tính của khâu i đối với khối tâm của nó trong hệ tọa độ động.
Đặt Ci
r là véc tơ xác định vị trí khối tâm của khâu i trong hệ tọa độ cố định, Ci
r
tính theo công thức:
0 0 i
Ci Oi i Ci
r = r + R r (4.3)
trong đó -
0
Oi
r là vị trí điểm gốc hệ tọa độ động Oi đối với hệ tọa độ cố định
-
0
i
R là ma trận cosin chỉ hướng của hệ tọa độ động (Oxyz)i đối với hệ
tọa độ cố định.
~ 32 ~

33. -
i
Ci
r là véc tơ xác định vị trí khối tâm của khâu i trong hệ tọa độ (Oxyz)i
Khi đó vận tốc dài của khối tâm khâu i và vận tốc góc khâu i có thể tính như sau:
1
Ci Ci Ci
2
1 2 n
Ci
Ci Ci Ci
Ci
1 2 n
Ci
Ci Ci Ci
1 2 n n
q
x x x
...
q
q q q
x
.
y y y r
v r y ... q
.
q q q q
z
.
z z z
...
q q q q
Ci
Ci Ci
� �
� �
� � �
� �
� �
� � � � �
� �
� �
� �
� �
� � � �
� �
   
� �
� �
� � � � � �
� �
� �
� �
� � � �
� �
� � �
� �
� �
� � �
� �
� �
&
&
&
& &
&
&
&
1
x x x
1 2 n 2
i
T y y y
i i
i x y z
1 2 n
z z z
1 2 n n
q
ω ω ω
...
q q q q
.
ω ω ω w
ω ω ω ω ... q
.
q q q q
.
ω ω ω
...
q q q q
� �
� � �
� �
� �
� �
� � �
� �
� �
� �
� � �
� � �
� �
  
� �
� �
� � � � � �
� �
� �
� �
� �
� � �
� �
� �
� � �
� �
� �
&
& & & &
&
& & & &
& & & &
Đặt
i
rω
J , J
q q
Ci
Ti Ri
� �
 
� �
i
& (4.4)
Khi đó:
i
Ci Ti i Ri
v =J q,ω =J q
& &
Với Ti
J
- ma trận Jacobian tịnh tiến của khâu i
Ri
J
- ma trận Jacobian quay của khâu i
Thay vào (4.2) ta được:
T T T
i Ti i Ti Ri i Ri
1
T = q (J m J +J I J )q
2
& &
Vậy động năng của robot là:
 
n n
T T T
i Ti i Ti Ri i Ri
i=1 i=1
T
1
T= T = q (J m J +J I J ) q
2
1
= q M(q)q
2
� �
& &
& &
(4.5)
Trong đó:
~ 33 ~

34. 11 12 1n
n
21 22 2n
T T
Ti i Ti Ri i Ri
i=1
n1 n2 nn nxn
m (q) m (q) .. m
m (q) m (q) .. m
M(q)= (J m J +J I J )=
.. .. .. ..
m (q) m (q) .. m
� �
� �
� �
� �
� �
� �
�
(4.6)
Ta gọi M(q) là ma trận khối lượng suy rộng
1.2.Thế năng của robot:
n
T
i 0 Ci
i=1
Π=- m g r (4.7)
�
Trong đó:
i
m
- khối lượng khâu i
 
T
0
g = 0,0,-g
g – gia tốc trọng trường, g=9,81 m/s2
.
Đặt
 
T
1 2 i
g(q)= g (q) g (q) ... g q
� �
� �với
 
i
i
g q
q

�

� (4.8)
1.3. Lực suy rộng của các lực không thế:
n
Tj Tj
j Ti i Ri i
i=1
Q = (J .F+J .M )
�
n
T1 T1 T1 T1 T1
1 Ti i R1 i T3 3 R3 3 T3 3
i=1
Q = (J F +J M )=J F +J M =J F
�
n
T2 T2 T2 T2 T2
2 Ti i R1 i T3 3 R3 3 T3 3
i=1
Q = (J F +J M )=J F +J M =J F
�
n
T3 T3 T3 T3 T3
3 Ti i R1 i T3 3 R3 3 T3 3
i=1
Q = (J F+J M )=J F +J M =J F
�
1
2
3
Q
Q= Q
Q
� �
� �
� �
� �
� � (4.9)
~ 34 ~

35. 3
0 0
0 0
z
F
F P
� � � �
� � � �
 
� � � �
� � � �
 
� � � �
(P - trọng lực của khuôn)
1.4. Thiết lập phương trình vi phân chuyển động của robot:
Xét biểu thức (4.5), đặt
  ij nxn
M q = m
� �
� � ta có:
n n
T
jk j k
k=1 j=1
1 1
T= q M(q)q= m (q)q q
2 2
�
�
& & &&
Suy ra:
 
 
 
i
ij
ij j k l ij j
1 1 1
ij j
1
j
j
k
i
1
1
m (q).q
d
T
q
m
T
m q q q q m q q
m q
d
q
dt dt
q q
n
k
n n n n
j j k j
n
j
i

 

 

 
�
�
�
�
� �
�
 
� �
� �
� �
� �
�
�
� �
&
&
& & &
&
&
&
&
&
(4.10)
Mặt khác
 
j
j k
1
k
i i
1
m q
T
q
q
2 q
1
q
n n
j k
 
�
�

� �
�
� &&
(4.11)
Thay (4.8), (4.9), (4.10) vào (4.1) ta có:
 
 
 
ij jk
ij j k j i i
1 1 1 k i
m m q
m q q q q g qτ
q q
1
2
n n n
j j k
  
 

�
 
� �
� �
� �
� �
�
&
& &&
(4.12)
Ta đưa vào ký hiệu:
 
   
   
ij jk
ijk ij,k jk,i
k i
1 1
h
m q m q
q
q m q m q
2 q 2
� �
� �
   
Do  
M q là ma trận đối xứng nên ji,k jk,i ki,j
m = m = m
.Từ đó suy ra:
     
ijk ij,k ik,j kj,i ik,j ji,k kj,i
1 1
h q m + m - m m + m - m
2 2
 
(4.13)
Thay (4.12) vào (4.11) ta được phương trình vi phân chuyển động của robot:
     
ij j ijk k j i i
1 1 1
m q q h q q q g qτ
n n n
j j k
  
  
� �
�
&
& &&
(4.14)
Đặt
   
ij ijk k
1
c q q h q q
n
k
 �
, & &
, ta được:
~ 35 ~

36.    
ijk k j
1
j
1
i j
1
h q q q c q,q q
n n n
j k j
  

�
� �
&& & &
(4.15)
   
ij ik,j ji,k kj,i k
1
1
c q,q m + m - m q
2
n
k

� �
& &
(4.16)
Thay (4.12) vào (4.11) ta được:
     
n n
ij j i i
j=
j
1 =1
j
j
i
+ c q
m q q +g q +Q=τ
,q q
� � & &
&
&
Hệ phương trình vi phân chuyển động của robot có thể viết dưới dạng ma trận
như sau:
   
M(q)q+C(q,q)q+g(q)+Q=τ t 4.17
&
& &&
Trong đó
  ij nxn
M q = m
� �
� � là ma trận khối lượng suy rộng, tính theo công thức (4.6).
  ij nxn
C q,q = c
� �
� �
&
là ma trận ly tâm – Coriolis, tính theo công thức (4.16)
g(q) là vector lực do trọng lực gây ra, tính theo công thức (4.8).
Q là lực suy rộng của các lực không thế (4.9)
 
t
τ
là vector lực dẫn động từ các động cơ.
II/ Thiết lập phương trình vi phân chuyển động của robot:
Ta có hình vẽ thể hiện các tham số động lực học của Robot như hình 4.1 dưới đây:
~ 36 ~

37. Hình 4.1. Các tham số động lực học
Coi các khâu là đồng chất, tiết diện đều, khi đó trọng tâm các khâu sẽ nằm ở giữa
khâu
Từ công thức (4.3) ta xác định được vị trí khối tâm khâu 1 trong hệ tọa độ cố định:
C1
1 1 1
1 1 1 1
0 0 1
O1 1 C1 1 1 1 1 1 1
1
1
1
a a C
- 2
a C C -S 0 2
1
r = r + R r = a S + S C 0 0 = a S
2
d 0 0 1 0
d
� �
� � � �
� �
� � � � � �
� �
� � � � � �
� �
� � � � � �
� �
� � � � � �
� � � �
� � � �
� � � �
� �
Vận tốc khối tâm khâu 1 trong hệ tọa độ cố định:
C1 C1
1 1 1
1 1 1
1
- a S q
2
1
a C q
2
0
v r
= =
� �
� �
� �
� �
� �
� �
� �
� �
� �
&
&
&
Tương tự ta tính được vị trí khối tâm khâu 2 trong hệ tọa độ cố định:
~ 37 ~
1
q

38. 2 2 12 1 1
2 12 1 1 12 12
0 0 2
C2 O2 2 C2 2 12 1 1 12 12 2 12 1 1
1 2
1 2
1
a a C +a C
- 2
a C + a C C -S 0 2
1
r = r + R r = a S +a S + S C 0 0 = a S +a S
2
0 0 1 0
d +d
d +d
� �
� � � �
� �
� � � � � �
� �
� � � � � �
� �
� � � � � �
� �
� � � � � �
� �
� � � � � �
� � � �
� �
Vận tốc khối tâm khâu 2 trong hệ tọa độ cố định:
 
 
2 12 1 2 1 1 1
C2 C2 2 12 1 2 1 1 1
1
- a S q +q -a S q
2
1
v =r = a C q +q +a C q
2
0
� �
� �
� �
� �
� �
� �
� �
� �
� �
& & &
& & & &
Vị trí khối tâm khâu 3 trong hệ tọa độ cố định:
1 1 12 2 12 12 1 1 12 2
C3 1 1 12 2 12 12 1 1 12 2
1 2 3 3 3max
1 2
C a +C a C -S 0 0 C a +C a
r = S a +S a + S -C 0 0 = S a +S a
0 0 1 3.
d +d +
2 2
d d q q q
� � � �
� � � �
� � � �
� � � �
� � � �
� � � �
� � � �
� � � �
� � � �
  � �
� � � � � �
� � � �
Vận tốc khối tâm thứ 3 trong hệ tọa độ cố định:
2 12 1 2 1 1 1
C3 C3 2 12 1 2 1 1 1
3
-a S (q +q ) - a S q
v =r = a C (q +q ) + a C q
3
q
2
� �
� �
� �
� �
� �
� �
� �
& & &
& & & &
&
Từ đó ta tính được các ma trận Jacobi tịnh tiến của khâu i (JTi) và Jacobi quay của
khâu i (JRi) theo công thức (4.4) như sau:
1 1
T1 1 1
1
0 0
2
1
J 0 0
2
0 0 0
a S
a C
� �

� �
� �
� �

� �
� �
� �
� �
� �
~ 38 ~
Tải bản FULL (file word 77 trang): bit.ly/2Ywib4t
Dự phòng: fb.com/KhoTaiLieuAZ

39. 2 12 1 1 2 12
T2 2 12 1 1 2 12
1 1
0
2 2
1 1
J 0
2 2
0 0 0
a S a S a S
a C a C a C
� �
  
� �
� �
� �
 
� �
� �
� �
� �
� �
1 1 12 2 2 12
T3 1 1 12 2 2 12
-S a -S a -a S 0
J = C a +C a a C 0
3
0 0
2
� �
� �
� �
� �
� �
� �
� �
R1
0 0 0
J 0 0 0
1 0 0
� �
� �

� �
� �
� �
R2
0 0 0
J = 0 0 0
1 1 0
� �
� �
� �
� �
� �
R3
0 0 0
J = 0 0 0
1 1 0
� �
� �
� �
� �
� �
Ma trận ten xơ quán tính khối của các khâu so với hệ quy chiếu động gắn chặt với khâu là:
1
I =
0 0
0 0
0 0
1x
1y
1z
I
I
I
� �
� �
� �
� �
� �
, 2
I =
0 0
0 0
0 0
2x
2y
2z
I
I
I
� �
� �
� �
� �
� �
, 3
I =
0 0
0 0
0 0
3x
3y
3z
I
I
I
� �
� �
� �
� �
� �
,
• Ma trận khối lượng suy rộng
11 12 13
3
21 22 23
i=1
31 32 33
T T
Ti i Ti Ri i Ri
m m m
M(q)= = m m m
m m m
(J m J +J I J )
� �
� �
� �
� �
� �
�
Với 1 2 3
, ,
m m m đã biết và các ma trận T1 T2 T3 T4 R1 R2 R3 1 2 3
J ,J ,J ,J ,J ,J ,J ,,I ,I ,I đã tính
toán ở trên ta tính được:
~ 39 ~
Tải bản FULL (file word 77 trang): bit.ly/2Ywib4t
Dự phòng: fb.com/KhoTaiLieuAZ

40.  
2 2 2 2 2
11 1 1 2 1 2 1 2 2 3 2 1 2 1 2 3 1 2
1 1
2
4 4
z z z
m m a m a a a C a m C a a a a I I I
� �
         
� �
� �
 
2 2
2 2 1 2 2 3 2 1 2 2 2 3
1 1
2 4
12 z z
m = m a a C a m C a a a I I
� �
    
� �
� �
0
13
m =
21 12
m = m
2 2
2 2 2 2 3 3
1
4
22 z z
m = a m I a m I
  
0
23
m =
31 13
m = m
;
32 23
m = m
3
33
m = m
• Vector g(q)
Theo công thức (4.7) ta có thế năng của robot là:
4
T
i 0 Ci
i=1
Π= - m g r
�
với  
T
0
g 0,0,-g

và các ma trận  
i=1,2,3
Ci
r
đã tính ở trên ta tính được thế năng của
robot:
1 1 2 1 2 3 3 2 1 3max
1
Π= ( )
2
gm d gm d d gm q d d d
� �
     
� �
� �
Theo công thức 4.8, ta có :
     
 
 
 
T
1 2 3
1
2
3 3
g(q)=[g q , g q , g q ]
g q =0
g q =0
g q =-gm
• Ma trận Coriolis C(q,q)
&
~ 40 ~
5310148