Xác định tọa độ trọng tâm các khâu trong hệ tọa độ cố đinh...22... Như vậy, cánh tay robot này có chuyển động trong không gian 3 chiều.chuyển động, cả khâu cố định này và khớp 1 đều chịu
MÔ HÌNH HÓA TOÁN HỌC CÁNH TAY ROBOT 5 BẬC TỰ DO
Phân tích mô hình cánh tay 5 bậc tự do
Mô hình cánh tay robot 5 bậc tự do được mô tả như hình 1.
Hình 1.1 Mô hình cánh tay robot 5 bậc tự do
Mô hình cánh tay gồm 5 khâu và 5 khớp tương ứng Khớp 1 và khớp 4 là khớp tịnh tiến. Khớp 2 là khớp quay trụ Khớp 3 và khớp 5 là khớp quay bản lề Như vậy, cánh tay robot này có chuyển động trong không gian 3 chiều.
Khớp 1 là khớp tịnh tiến và được truyền động nhờ khâu cố định có chiều dài L 0 Khi chuyển động, cả khâu cố định này và khớp 1 đều chịu tải truyền động nên quá trình tính toán có thể coi khâu cố định này và khớp hợp với nhau là khâu 1.
Mô hình hóa toán học cánh tay robot 5 bậc tự do
Mô hình cánh tay (Hình 1) được xây dựng theo quy tắc DH Theo đó, các bước được thực hiện như sau:
1.2.1 Đặt các hệ tọa độ cố định và hệ tọa độ địa phương Đặt hệ tọa độ cố định ( OXY Z ) 0 với gốc O 0 tại điểm đầu khâu 1 với các các trục như hình vẽ.
Do khớp 1 là khớp tịnh tiến nên trục ( OZ ) 1 cần đặt trùng với hướng dịch chuyển của khớp q 1 Chú ý rằng, khớp q 1 là dịch chuyển dài nên có đơn vị là mét (m) Dễ dàng đặt hệ ( OXY Z ) 1 sao cho gốc O 1 trùng với gốc O 0 , trục ( OX ) 1 trùng với trục ( OX ) 0
Khớp 2 là khớp quay thẳng đứng với biến khớp q rad 2 ( ) nên trục ( OZ ) 2 đi qua trục quay của khớp và trùng với hướng chiều dài khâu 2 như hình vẽ Điểm đặt của hệ ( OXY Z ) 2 là tại điểm đầu của khâu 2.
Khớp 3 là khớp quay bản lề với biến khớp q rad 3 ( ), trục quay của khớp 3 vuông góc với trục quay của khớp 2 Do đó, trục ( OZ ) 3 đi qua tâm khớp 3 và trục ( OZ ) 3 vuông góc với trục
( OZ ) 2 Trục ( OX ) 3 hướng dọc theo chiều dài khâu 3 Điểm đặt của hệ ( OXY Z ) 3 là điểm đầu khâu 3.
Khớp 4 là khớp tịnh tiến nên trục ( OZ ) 4 cần đặt trùng với hướng dịch chuyển của khớp q 1 Chú ý rằng, khớp q 1 là dịch chuyển dài nên có đơn vị là mét (m) Dễ dàng đặt hệ
( OXY Z ) 4 sao cho gốc O 4 ở đầu khâu 4, trục ( OX ) 4 trùng với chiều dài khâu 3 như hình vẽ. Khớp 5 tương tự như khớp 3 với biến khớp q rad 5 ( ) Trục ( OX ) 5 hướng dọc theo chiều dài khâu 5 Điểm đặt của hệ ( OXY Z ) 5 là điểm đầu khâu 5.
Tại điểm thao tác E, hệ tọa độ có thể đặt tùy ý nhưng vẫn phải tuân thủ quy tắc DH.
Quy tắc DH được tuân thủ cho khớp i để dịch chuyển hệ tọa độ ( OXY Z ) i 1 sang hệ tọa độ ( OXY Z ) i bất kỳ theo nguyên tắc: Quay trục ( OZ ) i 1 với góc q i 1 , tịnh tiến trục ( OZ ) i 1 với chiều dài d i 1 , tịnh tiến trục ( OX ) i 1 với chiều dài a i 1 và quay trục ( OX ) i 1 với góc i 1
Chuyển đổi từ hệ (OXYZ) 0 sang hệ (OXYZ) 1
Trục ( OZ ) 0 không thể quay nên q 0 0
. Trục ( OZ ) 0 không thể tịnh tiến nên d 0 0
Trục ( OX ) 0 không thể tịnh tiến nên a 0 0
Trục ( OX ) 0 có thể quay cùng chiều kim đồng hồ góc 90 0 hay 2(rad ) nên 0 2
(chú ý rằng, nhìn từ đầu trục ( OX ) 0 , nếu quay cùng chiều kim đồng hồ thì góc quay nhận giá trị âm, ngược chiều kim đồng hồ thì nhận giá trị dương).
Chuyển đổi từ hệ (OXYZ) 1 sang hệ (OXYZ) 2
Trục ( OZ ) 1 không thể quay nên q 1 0
Trục ( OZ ) 1 có thể tịnh tiến nên d 1 0 để đưa gốc O 1 sang gốc O 2 (lúc này d m 1 ( ) là biến khớp nên có thể đặt q 1 d m 1 ( ) cho thống nhất và thuận lợi trong tính toán sau này, chỉ cần chú ý đơn vị đo của biến khớp).
Trục ( OX ) 1 không thể tịnh tiến nên a 1 0
Trục ( OX ) 1 có thể quay ngược chiều kim đồng hồ góc 90 0 hay 2(rad ) để hoàn thành chuyển đổi hệ tọa độ ( OXY Z ) 1 sang hệ ( OXY Z ) 2 đúng như hình vẽ đã xác định ở bước 1 nên
Chuyển đổi từ hệ (OXYZ) 2 sang hệ (OXYZ) 3
Trục ( OZ ) 2 có thể quay góc bất kỳ xung quanh khớp 2 nên q 2 0 và q rad 2 ( ) là biến khớp.
Tuy nhiên, với trục ( OX ) 2 như hình vẽ, góc q 2 cần phải quay quá ( )
Do đó, góc khớp thực tế là q 2 2
Trục ( OZ ) 2 có thể tịnh tiến dọc khâu 2 nên d 2 0 để đưa gốc O 2 sang gốc O 3 và giá trị d m 2 ( ) là 1 giá trị cố định, bằng đúng chiều dài khâu (d 2 L m 2 ( ) ).
Trục ( OX ) 2 không thể tịnh tiến nên a 2 0
Trục ( OX ) 2 cần phải quay ngược chiều kim đồng hồ (nhìn từ đầu trục) góc 90 0 hay
2rad để đưa trục ( OZ ) 2 thành trục ( OZ ) 3 và hoàn thành chuyển đổi hệ tọa độ ( OXY Z ) 2 sang hệ ( OXY Z ) 3 đúng như hình vẽ đã xác định ở bước 1 nên 2 2
Chuyển đổi từ hệ (OXYZ) 3 sang hệ (OXYZ) 4
Trục ( OZ ) 3 có thể quay góc bất kỳ xung quanh khớp 3 nên q 3 0
, góc quay này tùy ý đưa trục ( OX ) 3 trùng với chiều dài khâu 3 Lúc này, q rad 3 ( ) là biến khớp
Trục ( OZ ) 3 không thể tịnh tiến nên d 3 0
Trục ( OX ) 3 có thể tịnh tiến đưa gốc O 3 đến gốc O 4 nên a 3 L m 3 ( )
. Trục ( OX ) 3 cần phải quay ngược chiều kim đồng hồ (nhìn từ đầu trục) góc 90 0 hay
2rad để đưa trục ( OZ ) 3 thành trục ( OZ ) 4 và hoàn thành chuyển đổi hệ tọa độ ( OXY Z ) 3 sang hệ ( OXY Z ) 4 đúng như hình vẽ đã xác định ở bước 1 nên 3 2.
Chuyển đổi từ hệ (OXYZ) 4 sang hệ (OXYZ) 5
Trục ( OZ ) 4 không thể quay nên q 4 0
Trục ( OZ ) 4 có thể tịnh tiến nên d 4 0 để đưa gốc O 4 sang gốc O 5 (lúc này d m 4 ( ) là biến khớp nên có thể đặt q 4 d m 4 ( ) cho thống nhất và thuận lợi trong tính toán sau này, chỉ cần chú ý đơn vị đo của biến khớp).
Trục ( OX ) 4 không thể tịnh tiến nên a 4 0
Trục ( OX ) 4 có thể quay cùng chiều kim đồng hồ góc 90 0 hay 2(rad ) nên 4 2
(chú ý rằng, nhìn từ đầu trục ( OX ) 4 , nếu quay cùng chiều kim đồng hồ thì góc quay nhận giá trị âm, ngược chiều kim đồng hồ thì nhận giá trị dương).
Chuyển đổi từ hệ (OXYZ) 5 sang hệ tọa độ thao tác (điểm E)
Trục ( OZ ) 5 có thể quay góc bất kỳ xung quanh khớp 5 nên q 5 0, góc quay này tùy ý đưa trục ( OX ) 5 trùng với chiều dài khâu 5 Lúc này, q rad 5 ( ) là biến khớp
Trục ( OZ ) 5 không thể tịnh tiến nên d 5 0
Trục ( OX ) 5 có thể tịnh tiến đưa gốc O 5 đến điểm thao tác E nên a 5 L m 5 ( ) Trục ( OX ) 5 không cần quay nên 4 0
Các thông số DH được mô tả trong bảng 1.1
Việc tính các ma trận H i và ma trận D i được thực hiện trên các phần mềm Maple và Matlab Trong đó, quy ước:
1 1 2 2 1 2 12 1 1 2 2 1 2 12 cos c q ,cos c ,cos( q q ) c ,sin q s ,sin q s ,sin( q q ) s , tương tự cho các khớp 3, khớp 4 và khớp 5.
Ma trận H 01 chuyển đổi hệ tọa độ (OXYZ) 0 sang hệ (OXYZ) 1 :
Ma trận H 12 chuyển đổi hệ tọa độ ( OXY Z ) 1 sang hệ ( OXY Z ) 2 :
Ma trận H 23 chuyển đổi hệ tọa độ ( OXY Z ) 2 sang hệ ( OXY Z ) 3 :
Ma trận H 34 chuyển đổi hệ tọa độ ( OXY Z ) 3 sang hệ ( OXY Z ) 4 :
Ma trận H 45 chuyển đổi hệ tọa độ ( OXY Z ) 4 sang hệ ( OXY Z ) 5 :
Ma trận H 5E chuyển đổi hệ tọa độ ( OXY Z ) 4 sang hệ tọa độ điểm thao tác ( OXY Z ) E :
Ma trận chuyển đổi thuần nhất từ hệ ( OXY Z ) i so với hệ tọa độ cố định ( OXY Z ) 0 được xác định như sau Trong đó, quy ước:
1 1 2 2 1 2 12 1 1 2 2 1 2 12 cos ,cos ,cos( ) ,sin ,sin ,sin( ) q c q c q q c q s q s q q s , tương tự cho các khớp 3, khớp 4 và khớp 5.
Ma trận D 01 mô tả hướng khâu 1 và tọa độ gốc O 1 so với hệ ( OXY Z ) 0
Ma trận D 02 mô tả hướng khâu 2 và tọa độ gốc O 2 so với hệ ( OXY Z ) 0
Ma trận D 03 mô tả hướng khâu 3 và tọa độ gốc O 3 so với hệ ( OXY Z ) 0
Ma trận D 04 mô tả hướng khâu 4 và tọa độ gốc O 4 so với hệ ( OXY Z ) 0
Ma trận D 05 mô tả hướng khâu 5 và tọa độ gốc O 5 so với hệ ( OXY Z ) 0
Ma trận D 0E mô tả hướng khâu thao tác (bàn kẹp) và tọa độ điểm E so với hệ ( OXY Z ) 0
Như vậy, tọa độ điểm thao tác cuối được xác định:
1.2.6 Kiểm tra tọa độ điểm thao tác
Hình dáng robot xác định bằng hình học như hình 2.
Hình 1.2 Mô hình Robot ở vị trí 1
Tọa độ điểm E trong hệ tọa độ ( OXY Z ) 0 xác định bằng hình học:
Tọa độ điểm E xác định bằng tính toán trong Maple:
Như vậy, tọa độ điểm E được xác định là trùng khớp.
Hình dáng robot xác định bằng hình học như hình 3
Hình 1.3 Mô hình Robot ở vị trí 2
Tọa độ điểm E trong hệ tọa độ ( OXY Z ) 0 xác định bằng hình học:
Tọa độ điểm E xác định bằng tính toán trong Maple:
Như vậy, tọa độ điểm E được xác định là trùng khớp.
BÀI TOÁN ĐỘNG HỌC CÁNH TAY ROBOT 5 BẬC TỰ DO
Phương trình động học
2.1.2 Phương trình động học sin( 2)(( 5cos( 5) 3)cos( 3) - sin( 3)( 5sin( 5) - 4))
(( 5cos( 5) 3)cos( 3) - sin( 3)( 5sin( 5) - 4))cos( 2) 1
2.1.2 Các thông số cấu hình Robot
Bài toán động học thuận
Phương trình các biến khớp:
2.2.2 Đầu ra Đầu ra là quỹ đạo điểm thao tác x y z m E , E , ( ) E
Bài toán động học ngược
Robot di chuyển từ điểm A tới điểm B trong không gian thao tác Quỹ đạo được lập trình trong không gian khớp theo dạng Điểm tới Điểm (Point to Point) theo quy luật là hàm đa thức bậc ba.
3.2 Lập trình quỹ đạo cho cánh tay Robot 5 bậc tự do
Trong phần này, ta xem xét làm thế nào các chuyển động của robot có thể lập được trong không gian khớp với các đặc tính cho trước Từ đó, ta tìm được quỹ đạo của robot khi đi từ điểm ban đầu đến điểm cuối mà thỏa mãn yêu cầu về vận tốc và gia tốc.
Tọa độ điểm (0,0.88,0.08)A với các góc khớp tương ứng
Tọa độ điểm ( 0.4,0.66,1)B với các góc khớp tương ứng 0.4 3 2 0
Thời gian di chuyển là t T 10( )s
Phương trình quỹ đạo được cho ở dạng đa thức bậc 3:
Với điều kiện ban đầu, ta xác định được các hệ số như sau:
Phương trình quỹ đạo các biến khớp:
LẬP TRÌNH QUỸ ĐẠO CHO CÁNH TAY ROBOT 5 BẬC TỰ DO
Mô tả bài toán
Robot di chuyển từ điểm A tới điểm B trong không gian thao tác Quỹ đạo được lập trình trong không gian khớp theo dạng Điểm tới Điểm (Point to Point) theo quy luật là hàm đa thức bậc ba.
Lập trình quỹ đạo cho cánh tay Robot 5 bậc tự do
Trong phần này, ta xem xét làm thế nào các chuyển động của robot có thể lập được trong không gian khớp với các đặc tính cho trước Từ đó, ta tìm được quỹ đạo của robot khi đi từ điểm ban đầu đến điểm cuối mà thỏa mãn yêu cầu về vận tốc và gia tốc.
Tọa độ điểm (0,0.88,0.08)A với các góc khớp tương ứng
Tọa độ điểm ( 0.4,0.66,1)B với các góc khớp tương ứng 0.4 3 2 0
Thời gian di chuyển là t T 10( )s
Phương trình quỹ đạo được cho ở dạng đa thức bậc 3:
Với điều kiện ban đầu, ta xác định được các hệ số như sau:
Phương trình quỹ đạo các biến khớp:
Các thông số hình học được cho trong file “Parameters”, chương trình giải nằm trong file
1 – Phương trình quỹ đạo biến khớp 2 - Phương trình vận tốc biến khớp
3 - Phương trình gia tốc biến khớp
Hình 3.2 Giá trị các biến khớp
Hình 3.3 Giá trị vận tốc biến khớp
Hình 3.4: Giá trị gia tốc biến khớp
BÀI TOÁN ĐỘNG LỰC HỌC CÁNH TAY ROBOT 5 BẬC TỰ DO
Xây dựng phương trình động lực học
4.1.1 Dạng phương trình động lực học
Phương trình dộng lực học trong không gian khớp
Trong đó: là ma trận quán tính.M là ma trận Coriolis.C là ma trận thế năng trọng trườngG
4.1.2 Xác định tọa độ trọng tâm các khâu theo hệ tọa độ địa phương
Tọa độ trọng tâm khâu 1:
Tọa độ trọng tâm khâu 2:
Tọa độ trọng tâm khâu 3:
Tọa độ trọng tâm khâu 4:
Tọa độ trọng tâm khâu 5:
4.1.3 Xác định tọa độ trọng tâm các khâu trong hệ tọa độ cố đinh
Tọa độ trọng tâm khâu 1 trong hệ tọa độ cố định:
Tọa độ trọng tâm khâu 2 trong hệ tọa độ cố đinh:
Tọa độ trọng tâm khâu 3 trong hệ tọa độ cố định:
Tọa độ trọng tâm khâu 4 trong hệ tọa độ cố định:
Tọa độ trọng tâm khâu 5 trong hệ tọa độ cố định:
4.1.4 Xác định ma trận Jacobian tịnh tiến
Ma trận Jacobian tịnh tiến khâu 1:
Ma trận Jacobian tịnh tiến khâu 2:
Ma trận Jacobian tịnh tiến khâu 3:
Ma trận Jacobian tịnh tiến khâu 4:
Ma trận Jacobian tịnh tiến khâu 5:
Do ma trận Jacobian tịnh tiến khâu 5 quá phức tạp nên ta có thể xem lại trong file Maple
“DongLucHocNguoc.mw” trong thư mục “Dong luc hoc nguoc”
4.1.5 Xác định ma trận Jacobian quay
Ma trận Jacobian quay khâu 1:
Ma trận Jacobian quay khâu 2:
Ma trận Jacobian quay khâu 3:
Ma trận Jacobian quay khâu 4:
Ma trận Jacobian quay khâu 5:
4.1.6 Xác định ma trận khối lượng
Phần tử hàng 1 cột 1 ma trận M:
Phần tử hàng 1 cột 2 ma trận M:
Phần tử hàng 1 cột 3 ma trận M:
Phần tư hàng 1 cột 4 ma trận M:
Phần tử hàng 1 cột 5 ma trận M:
Phần tử hàng 2 cột 1 ma trận M:
Phần tử hàng 2 cột 2 ma trận M quá phức tạp, ta có thể xem lại trong file Maple
“DongLucHocNguoc.mw” trong thư muc “Dong luc hoc nguoc”
Phần tử hàng 2 cột 3 ma trận M:
Phần tử hàng 2 cột 4 ma trận M:
Phần tử hàng 2 cột 5 ma trận M:
Phần tử hàng 3 cột 1 ma trận M:
Phần tử hàng 3 cột 2 ma trận M:
Phần tử hàng 3 cột 3 ma trận M:
Phần tử hàng 3 cột 4 ma trận M:
Phần tử hàng 3 cột 5 ma trận M:
Phần tử hàng 4 cột 1 ma trận M:
Phần tử hàng 4 cột 2 ma trận M:
Phần tử hàng 4 cột 3 ma trận M:
Phần tử hàng 4 cột 4 ma trận M:
Phần tử hàng 4 cột 5 ma trận M:
Phần tử hàng 5 cột 1 ma trận M:
Phần tử hàng 5 cột 2 ma trận M:
Phần tử hàng 5 cột 3 ma trận M:
Phần tử hàng 5 cột 4 ma trận M:
Phần tử hàng 5 cột 5 ma trận M:
4.1.7 Xác định ma trận Coriolis
Do ma trận Coriolis của hệ quá phức tạp nên ta có thể xem lại trong file Maple
“BaiTap.mw” trong thư mục “Dong luc hoc nguoc”
4.1.8 Xác định thế năng trọng trường
Phần tử hàng 1 vector thế năng trọng trường:
Phần tử hàng 2 vector thế năng trọng trường:
Do phần tử hàng 3 vector thế năng trọng trường quá phức tạp nên ta có thể xem lại rtong file Maple “DongLucHocNguoc.mw” trong thư mục “Dong luc hoc nguoc”.
Phần tử hàng 4 vector thế năng trọng trường:
Bài toán động lực học ngược
Ta sẽ giải bài toán động lực học ngược ứng với quỹ đạo đã được lập trình trong bài toán
“Lập trình quỹ đạo trong không gian khớp” ở chương 3.
Phương trình quỹ đạo các biến khớp:
Phương trình vận tốc biến khớp:
Phương trình gia tốc biến khớp:
Hình 4.2 Mô hình tính toán Động lực học ngược bằng Simulink
6 Phương trình quỹ đạo các khớp
7 PHương trình vận tốc các khớp
8 Phương trình gia tốc các khớp
9 Phương trình động lực học ngược
Hình 4.2: Đồ thị biểu diễn Moment lực của các khớp qua bài toán động lực học ngược
Việc tính toán, mô phỏng được các bài toán về động học và động lực học Robot giúp chúng ta hình dung cách trực quan, rõ ràng về các khả năng, các giới hạn và yêu cầu của hệ thống Robot Từ đó ta có thể lựa chọn linh kiện, cấu hình Robot để đáp ứng với yêu cầu thực tế, giảm thiểu sai sót, hao hụt cũng như tối ưu để hệ thống vận hành ổn định, đạt hiệu quả cao. Qua môn học này, chúng ta nắm được ý nghĩa của động học và động lực học trong thiết kế, điều khiển và khai thác Robot Hiểu được lý thuyết về động học và động lực học của các hệ thống Robot điển hình, thường gặp Có kỹ năng xây dựng và giải được các bài toán với các cấu hình Robot khác nhau Sử dụng được các công cụ hỗ trợ tính toán và mô phỏng (Maple,Matlab, Simulink…) để thiết lập và giải các bài toán động học và động lực học.
