Phân Tích Và Lựa Chọn Cấu Trúc
Phân Tích Mục Đích Ứng Dụng Robot
Trong thời đại công nghệ phát triển mạnh mẽ, động cơ điện xoay chiều 3 pha và động cơ servo đã trở thành những thiết bị quan trọng và không thể thiếu trong đời sống Chúng biến đổi điện năng thành cơ năng và được ứng dụng rộng rãi trong nhiều lĩnh vực, từ sinh hoạt hàng ngày như quạt, máy bơm nước đến máy sấy tóc.
+ Công nghiệp: Động cơ servo dùng trong robot công nghiệp, động cơ xoay chiều 3 pha dùng trong vận tải bang chuyền,…
+ Các lĩnh vực công nghệ cao: Máy bay, tàu thủy, robot đi trên sao hỏa, …
Vỏ động cơ là phần đơn giản nhất trong mọi loại động cơ, có khả năng sản xuất theo dây chuyền tự động một cách hiệu quả.
Việc tích hợp tự động hóa và robot vào dây chuyền sản xuất giúp nâng cao năng suất và đáp ứng nhu cầu sản xuất lớn mà không cần sử dụng nhiều nhân công.
Robot thực hiện gắp sản phẩm trên dây chuyền tự động
Phân Tích Yêu Cầu Kỹ Thuật Thao Tác
Sản phẩm của chúng tôi là vỏ động cơ, được sản xuất bằng phương pháp đúc áp lực Đáp ứng yêu cầu từ đề bài, nhóm đã xây dựng quy trình thao tác chi tiết để đảm bảo chất lượng sản phẩm.
Mô hình mô tả công nghệ sản xuất được nhóm đưa ra
Quy trình của robot được diễn ra như sau :
- Khuôn đúc được robot hoặc công nhân để vào đầu bang chuyền 1
- Qua cảm biến, khuôn được phát hiện và dừng khi tới đúng vị trí dưới máy đúc áp lực
- Máy đúc đổ đầy kim loại đúc ( nhôm) vào khuôn, khuôn tiếp tục di chuyển tới cuối băng chuyền 1
- Khuôn tới máy tháo khuôn, phôi được tháo ra tới khâu gia công tiếp theo
1.2.1 Đối Tượng Thao Tác, Dạng Thao Tác Đối tượng thao tác của robot ở đây là khuôn cùng sản phẩm đúc bên trong
Kích thước đối tượng như sau:
Khuôn có kích thước hình hộp chữ nhật 150mm x 100mm x 100mm, bao gồm một lỗ trụ đường kính 50mm xuyên qua, với khoảng rỗng tương ứng với kích thước của phôi sản phẩm Phôi được thiết kế dưới dạng hình trụ tròn rỗng, với đường kính trong là 65mm.
Chiều dài L = 100mm, có 16 cánh kích thước như sau
Đối tượng thao tác trong bài viết này là khuôn hình chữ nhật được chế tạo từ thép hợp kim không gỉ có tính từ, được đặt ở vị trí thẳng đứng và được di chuyển đến vị trí thao tác thông qua hệ thống băng tải.
Dạng thao tác trong quy trình này là hút-thả, trong đó khuôn được giữ ở vị trí nhờ lực hút từ nam châm điện tới mặt khuôn Hệ số ma sát gần như bằng không, tạo điều kiện thuận lợi cho quá trình thao tác.
* Mô tả các thành phần hệ thống:
Băng chuyền 1 đưa khuôn tới máy đúc và đưa khuôn đã đúc tới vị trí thao tác của robot
Kích thước Dài x Rộng x Cao: 1650mm x 200mm x 300mm
Băng chuyền 2 đưa khuôn đã có phôi từ vị trí thả của robot tới bàn quay và máy tháo khuôn
Kích thước Dài x Rộng x Cao: 1650mm x 200mm x 300mm
Mỗi khuôn cách nhau 600mm
Máy đúc có nhiệm vụ bơm kim loại( nhôm ) lỏng vào khuôn với thời gian cho mỗi lần bơm ( cũng là thời gian băng chuyền dừng ) là 4s
1.2.2 Phân Tích Yêu Cầu Về Vị Trí Và Hướng Của Khâu Thao Tác
Khuôn chứa sản phẩm hoàn thiện được băng tải vận chuyển đến vị trí thao tác của robot, nơi robot sẽ gắp khuôn từ băng tải 1 và chuyển sang băng tải 2 để đưa đến máy tháo khuôn Để quá trình gắp khuôn diễn ra suôn sẻ, hệ tọa độ của khâu thao tác phải trùng khớp với hệ tọa độ của khuôn trên băng tải.
Để đảm bảo quá trình thao tác hiệu quả với khuôn hình chữ nhật, mặt cơ cấu hút nam châm cần phải song song với bề mặt khuôn Nếu mặt nam châm không song song, sẽ xảy ra hiện tượng vênh, lệch, dẫn đến khuôn không được giữ chắc, gây nguy cơ rơi và hỏng hóc trong quá trình di chuyển.
Mặt cơ cấu hút nam châm phải song song với mặt trên khuôn
1.2.3 Yêu Cầu Về Vận Tốc Và Gia Tốc Khi Thao Tác
Các khuôn chứa sản phẩm hoàn thiện được vận chuyển từ băng tải đến vị trí để robot thao tác, với khoảng cách giữa các khuôn là 600mm Thời gian băng tải di chuyển các khuôn từ vị trí sau lên vị trí trước là 4 giây.
Robot thực hiện quá trình gắp khuôn từ băng tải 1 sang băng tải 2 trong thời gian tối đa 4 giây, do đó, tốc độ thao tác của robot cần phải đáp ứng yêu cầu này và đảm bảo chuyển động ổn định trong suốt quá trình làm việc.
Gia tốc là đại lượng phản ánh sự thay đổi của vận tốc theo thời gian Trong quá trình robot gắp khuôn từ băng chuyền 1 sang băng chuyền 2, thời gian chạy ngược lại được khống chế trong 1 giây để giảm thời gian chạy không tải Do đó, vận tốc thay đổi đột ngột, yêu cầu gia tốc phải đáp ứng điều kiện này.
1.2.4 Yêu Cầu Về Không Gian Thao Tác
Trong bài toán nhóm, chúng em thiết kế một robot để gắp khuôn từ băng chuyền thứ nhất sang băng chuyền thứ hai Không gian thao tác của robot nằm trong nhà xưởng, giữa hai băng chuyền.
Kích thước của không gian thao tác robot: chiều cao: 600mm, đường kính không gian thao tác 650mm
Xác Định Các Đặc Trưng Kỹ Thuật
1.3.1 Số Bậc Tự Do Cần Thiết
Trong không gian bất kỳ, một cơ cấu có p khâu động và nj khớp động được xác định bởi định lý Grubler, với số bậc tự do tính theo công thức f = 6(p-nj) + số bậc tự do thừa Đối với cơ cấu phẳng, số bậc tự do cũng được tính tương tự Để gắp khuôn đúc từ băng chuyền 1 sang băng chuyền 2, robot cần ít nhất 3 bậc tự do để xác định chính xác vị trí của khuôn trong không gian Vì khuôn có hình chữ nhật, hướng của khâu thao tác phải song song với bề mặt khuôn, do đó, cần sử dụng cơ cấu cơ khí phù hợp để đảm bảo điều này.
1.3.2 Yêu Cầu Về Tải Trọng Đối tượng thao tác ở đây là khuôn và phôi có tổng khối lượng 10kg sử dụng lực hút giữa nam châm của khâu thao tác và bề mặt của khuôn nên yêu cầu kết cấu cơ khí của robot phải vững chắc, chịu được tải trọng lớn và hoạt động ổn định trong suốt quá trình gắp thả khuôn từ băng chuyền 1 sang băng chuyền thứ 2.
1.3.3 Vùng Làm Việc Có Thể Với Tới Của Robot
Trên hình là khoảng cách từ tâm khâu 0 tới 2 băng chuyền
Robot thực hiện thao tác gắp khuôn từ vị trí A sang vị trí D
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
Như đã phân tích ở trên với robot gắp khuôn từ băng chuyền 1 sang băng truyền
2 thì robot phải có 3 bậc tự do với một cơ cấu xác định hướng bằng cơ khí.
Các phương án thiết kế để có thể đáp ứng được yêu cầu bài toán trên là:
- Ưu điểm: Robot phù hợp với yêu cầu kẹp chi tiết, đảm bảo đúng vị trí,dễ thay thế, độ cứng vững tốt.
- Nhược điểm: Kết cấu cồng kềnh do nhiều khớp tịnh tiến
- Ưu điểm: Robot phù hợp với yêu cầu kẹp chi tiết, đảm bảo đúng vị trí,dễ thay thế
- Nhược điểm: Khá cồng kềnh phức tạp, độ lịnh hoạt không cao
- Ưu điểm: Robot phù hợp với yêu cầu kẹp chi tiết, đảm bảo đưa chi tiết đến đúng vị trí.
- Nhược điểm: Còn hơi cồng kềnh, thay thế phức tạp, không đảm bảo độ cứng vững
- Ưu điểm: Robot linh hoạt phù hợp với yêu cầu mài, dễ thay thế Không gian làm việc rộng.
- Nhược điểm: Chưa đảm bảo độ cứng vững.
Mô hình kết cấu thứ 4 được xem là tối ưu nhất cho việc gắp khuôn từ băng chuyền thứ nhất sang băng chuyền thứ 2, và đây cũng là dạng kết cấu phổ biến nhất trong Robot Scara Mặc dù các mô hình khác có thể thực hiện các thao tác yêu cầu, nhưng chúng thường có kết cấu phức tạp, khó triển khai trong thực tế, hoặc thiếu tính linh hoạt và kích thước cồng kềnh, chiếm nhiều không gian.
Thiết Kế 3D Mô Hình Robot
Thiết Kế 3D
Nhóm đã thực hiện vẽ 3D cho robot như dưới đây:
+ Mô hình của cả hệ thống:
+ Mô hình của máy đúc áp lực:
Robot đơn giản hóa ( dùng cho mô phỏng matlab) Robot thực tế
+ Mô hình của băng tải và sản phẩm đúc+ Khuôn :
Lập bản vẽ 2D
Từ các bản vẽ thiết kế 3D ta có thể lập được các bản vẽ 2D của robot và hệ thống như dưới đây:
Hút nam châm điện Khuôn
Xác Định Các Thông Số Đặc Trưng Hình Học – Khối Lượng
Robot thực hiện thao tác gắp phôi từ vị trí A đến vị trí B, với robot được đặt tại vị trí O Do đó, kích thước các khâu của robot được xác định theo điều kiện a1 + a2 ≥ Xmax = 500mm.
Từ đó ta xác định đươc a1= 350mm;a200mm với a1 ,a2 lần lượt là chiều dài khâu1,khâu2
Chiều cao của băng tải là 300 mm, trong khi khuôn đặt trên băng tải cao 100 mm, vì vậy chúng ta chọn kích thước d0 cho khâu 0 là 54 mm Phần chân lắp thêm cho robot có kích thước dx là 246 mm, nhằm đảm bảo chiều cao làm việc phù hợp mà không làm cho robot trở nên quá cồng kềnh Khoảng cách dịch chuyển của khâu thao tác cuối được xác định là l3 = 350 mm.
Khảo Sát Bài Toán Động Học Robot
Khảo Sát Bài Toán Động Học Thuận
Với bài toán động học thuận thì các biến khớp đã biết, yêu cầu tìm vị trí của khâu thao tác.
Trước hết ta gắn các hệ trục tọa độ vào robot, từ đó ta xác định được bảng các tham số động học Denavit-Hartenberg như sau.
Hình 3.1: Hệ trục tọa độ trên robot
Bảng tham số động học D-H của robot là:
Theo quy tắc D-H ta có ma trận biến đổi tọa độ thuần nhất như sau:
Lần lượt thay các hàng của bảng thông số động học vào ma trận trên ta được các ma trận truyền tương ứng với các khâu:
Ma trận biến đổi tọa độ thuần nhất của khâu thao tác EE là:
Ma trận được trình bày dưới dạng trong đó: là ma trận cosin chỉ hướng của khâu thao tác so với hệ tọa độ gốc, và là ma trận chỉ vị trí của khâu thao tác so với hệ tọa độ gốc.
Vị trí khâu thao tác:
Ma trận tọa độ điểm tác động cuối E:
Vận tốc góc, gia tốc góc của khâu thao tác trong hệ tọa độ cố định
Ta suy ra vận tốc góc của khâu thao tác theo hệ tọa độ cố định từ ma trận trên:
Khảo Sát Bài Toán Động Học Ngược Robot
Ở bài toán động học ngược thì vị trí của khâu thao tác coi như đã biết ta phải đi tìm giá trị cho các biến khớp.
Cho tọa độ khâu cuối E-E cố định (xE,yE,zE)
Giải hệ phương trình đại số tuyến tính:
Khảo Sát Bài Toán Tĩnh Học Robot
Cơ Sở Lý Thuyết Bài Toán Tĩnh Học Robot
Các khâu trong robot được cấu tạo từ các thanh đồng chất với tiết diện ngang không đáng kể, có khối lượng lần lượt là m1, m2, m3 Lực tác dụng lên khâu thao tác tại điểm E được biểu diễn bằng vectơ F = [Fx, Fy, Fz]T Việc chọn vị trí của robot sẽ được thực hiện dựa trên bài toán động học.
Khi ta cho lực tác dụng vào điểm E, ta tính lực, momen tác dụng vào các khớp để đảm bảo Robot cân bằng tĩnh.
Ta có hệ phương trình cân bằng lực:
Dạng ma trận trong hệ tọa độ cơ sở:
là lực do khâu i-1 tác dụng lên khâu i ở khớp thứ i trong hệ tọa độ cơ sở.
0 Mi,i-1 = T là mô men do khâu i-1 tác dụng lên khâu i ở khớp thứ i trong hệ tọa độ cơ sở.
là trọng lực của khâu i trong hệ tọa độ cơ sở.
0 ri = 0 Ri iri là vector có gốc là O0 nối với Oi trong hệ tọa độ cơ sở.
0 Ri = 0 R1 1R2… i-1 Ri là ma trận quay biến đổi từ hệ tọa độ 0 đến hệ tọa độ thứ i
= là vector có gốc Oi-1 nối với Oi trong hệ tọa độ khâu i.
0 rci = 0 Ai irci là vector có gốc O0 nối với Ci trong hệ tọa độ cơ sở
i rci : là vector có gốc Oi nối với Ci trong hệ tọa độ khâu i
Thiết Lập Bài Toán Tĩnh Học Cho Robot
4.2.1 Xác định các thông số
Từ các ma trận trên ta tìm được:
Các ma trận sóng các khâu đối với khâu cơ sở và khối tâm khâu cơ sở :
4.2.2 Xác định các lực và momen
Ta thế các ma trận trên vào hệ phương trình tọa độ cơ sở để tìm lực và momen trên từng khớp:
Ta thay các thông số và hệ phương trình trên ta được
; Tìm lực và momen của khâu 2 ta dựa vào hệ phương trình:
Tiến hành thay lực và momen tính được ở khâu 3 vào hệ phương trình trên ta được:
Tìm lực và momen của khâu 1 ta dựa vào hệ phương trình:
Tiến hành thay lực và momen tính được ở khâu 2 vào hệ phương trình trên ta được:
Tiến hành thay số liệu về lực và momen cũng như các thông số thực tế gồm có:
Và kết quả ta thu được là :
CHƯƠNG 5 Khảo Sát Bài Toán Động Lực Học Robot
Phân tích động lực học robot là quá trình khảo sát và tính toán các đại lượng đặc trưng cho chuyển động của robot dưới tác động của lực Qua đó, chúng ta thiết lập được mối liên hệ giữa các đại lượng này và xây dựng phương trình vi phân mô tả chuyển động của robot.
Các thông số đầu vào :
Joint Khối lượng Vị trí khối tâm
5.2 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 :
Lagrange loại II có dạng tổng quát như sau:
Ui : Lực/Momen điều khiển ứng với tạo độ suy rộng qi n: Số bậc tự do
T: Động năng của cả hệ robot
: Thế năng của cả hệ robot qi: Tọa độ suy rộng thứ i
Qi: Lực suy rộng của các lực không thế ứng với tọa độ suy rộng qi
Dạng ma trận phương trình Lagrange 2:
Tính ma trận khối lượng:
Trong đó: M(q) là ma trận khối lượng
C (q, q) ˙ ˙ q là ma trận đặc trưng cho lực quán tính và lực Coriolit
Ma trận tensor quán tính :
Ma trận Jacobi tịnh tiến:
Từ đó: Ta thay các thông số vào phương trình vi phân chuyển động của Robot:
CHƯƠNG 6 Thiết Kế Qũy Đạo Chuyển Động
6.1 Xác Định Không Gian Làm Việc Của Robot
Ta đã biết điểm tác động cuối của robot, khi ta giới hạn các biến khớp ta sẽ tìm được vùng không gian làm việc của robot
Không gian làm việc trong không gian 3D Không gian làm việc trên mặt phẳng
Sử dụng phần mềm matlab ta có thể vẽ được vùng làm việc của robot như trên
6.2 Cơ Sở Về Thiết Kế Qũy Đạo
Thiết kế quỹ đạo chuyển động của robot là yếu tố quan trọng trong việc điều khiển robot di chuyển giữa các vị trí trong không gian làm việc Đường đi và quỹ đạo được xác định là các thông số thiết yếu cho hệ thống điều khiển vị trí của robot Vì vậy, độ chính xác của quỹ đạo thiết kế có ảnh hưởng lớn đến chất lượng di chuyển của robot.
Yêu cầu thiết kế quỹ đạo chuyển động của robot là :
Khâu chấp hành phải đảm bảo đi qua lần lượt các điểm trong không gian làm việc hoặc di chuyển theo một quỹ đạo xác định
Quỹ đạo của robot phải là đường liên tục về vị trí trong một khoảng nhất định
Không có bước nhảy về vận tốc và gia tốc
Quỹ đạo là các đường cong dạng :
Đa thức bậc 2 : x(t)=a+bt+ct 2
Đa thức bậc 3 : x(t)=a+bt+ct 2 +dt 3
Đa thức bậc cao : x(t)=a+bt+ +kt n
Ta sử dụng quỹ đạo dạng đa thức bậc 3 để thiết kế.
6.3 Thiết Kế Qũy Đạo Chuyển Động
6.3.1 Thiết kế quỹ đạo trong không gian khớp
Cho hai điểm A và B trong không gian làm việc với tọa độ điểm tác động cuối (xE, yE, zE) và hướng của khâu thao tác, cần thiết kế quỹ đạo chuyển động từ A đến B một cách linh hoạt và hiệu quả.
Theo bài toán động học ngược, chúng ta đã xác định được các biến khớp tại điểm A và B Để thiết kế quỹ đạo, chúng ta chọn đa thức hàm bậc 3 theo thời gian, với i=1,2 tương ứng với các biến khớp.
Ta được hệ phương trình như sau :
Giả sử cho t=5s robot đi từ điểm A( 0,2 + ; 0,25+ ) tới điểm B ( 0,5; )
Tương ứng với tọa độ khớp là: A( ); và điểm B( ).
6.3.2 Thiết kế quỹ đạo trong không gian thao tác
Xét bài toán quỹ đạo của điểm tác động cuối theo đường thẳng từ A đến B trong tE(s) Phương trình đường thẳng từ A(x0, y0) đến điểm B(xE, yE) là :
=> Để thỏa mãn điều khiện về vận tốc đầu và cuối ta thiết lập quan hệ x=x(t) là đa thức bậc 3 :
Với các điều kiện như sau :
Ví dụ robot đi qua điểm A(0.8 ;0.1) và điểm B(-0.2 ;0.5) và khoảng thời gian te=2s
6.3.3 Thiết kế quỹ đạo cho phần mô phỏng trên Matlab
Thực hiện bài toán quỹ đạo chu kì thời gian đặc trưng trong robot công nghiệp như sau:
- Di chuyển 50mm theo phương thẳng đứng đi xuống
- Di chuyển 50mm theo phương thẳng đứng đi lên
- Di chuyển sang ngang 300mm
- Tiếp tục di chuyển 50mm theo phương thẳng đứng đi xuống
- Di chuyển 50mm theo phương thẳng đứng đi lên trên
- Cuối cùng di chuyển 300mm về vị trí ban đầu.
Theo yêu cầu như trên, em đặt toạ độ cho các điểm A,B,C và D như sau:
Với chu kì thời gian đã lựa chọn là T=4s với thời gian cho từng chuyển động như sau:
Chuyển động i Thời gian chuyển động Khoảng dịch chuyển
CHƯƠNG 7 Thiết Kế Hệ Dẫn Động Robot
Tay robot có 3 bậc tự do, thiết kế cơ khí dạng 2 khớp quay, 1 khớp tịnh tiến.
- Thân robot: Là khâu cố định, đặt thẳng đứng giữ robot cố định khi làm việc, gắn với khâu động 1 qua khớp quay 1 với trục z 01thẳng đứng
Khâu dẫn động nằm ngang của robot được đặt vuông góc với trục thẳng đứng, cho phép nó thực hiện các chuyển động linh hoạt trong suốt quá trình làm việc Khâu này có khả năng quay quanh trục z thông qua khớp quay 1, giúp tăng cường khả năng hoạt động và hiệu suất của robot.
- Khâu 2: Khâu động có khả năng quay trong mặt phẳng vuông trục thẳng đứng qua khớp quay 2 nối với khâu 1
- Khâu 3: trục vít me - đai ốc bi (trục vít tịnh tiến, đai ốc quay).
Các trục quay, các khớp đều là thẳng đứng.
- Khâu quay 1, 2: hệ bánh răng (hộp giảm tốc)
- Khâu 3: khâu tịnh tiến (trục vít me - đai ốc bi), truyền động đai răng
- Khâu 3 nối với tay máy là nam châm điện
Tính toán thiết kế bộ truyền các khâu
Các yêu cầu của robot thiết kế:
- Cấu trúc 3 bậc tự do gồm 2 quay 1 tịnh tiến
-Tầm với 650mm,hành trình trục z khâu 3 300mm, tải trọng 10kg ( gồm khuôn+ sản phẩm đúc)
-Cấu trúc dựa theo cấu trúc của robot scara DENSO HM G series
Tay gắp sử dụng ở đây là tay gắp nam châm điện MHM-X7400A của hãng SMC Ưu điểm : nhỏ gọn, dễ kết nối, tải được tải trọng lớn
Tải trọng max :200N~20kg, khối lượng 0,59kg
Cơ cấu sử dụng: trục vít me- đai ốc bi Truyền động vít me - đai ốc được dùng để biến chuyển động quay thành chuyển động tịnh tiến.
Vít, bi: Thép 40CrMn ([ σ c ]x5 (Mpa) Đai ốc: Thép 18CrMnTi ( Thép không gỉ )
Sơ đồ truyền động Đai ốc lựa chọn d o x P h L L 1 L 2 L 3 L 4 L 5 L 6 L 7 L 8 L 9
Tính ch n c c u truy n ng khâu 3ọ ơ ấ ề độ
S d ng c c u truy n ng cho khâu 3 là ai r ng Do truy n l c b ng n kh p, truy nử ụ ơ ấ ề độ đ ă ề ự ằ ă ớ ề ng ai r ng có nh ng u i m: Không có tr t, t s truy n l n (u 12, ôi khi u < độ đ ă ữ ư đ ể ượ ỉ ố ề ớ ≤ đ
20), hi u su t cao, không c n l c c ng ban u l n, l c tác d ng lên tr c và lên nh ệ ấ ầ ự ă đầ ớ ự ụ ụ ổ ỏ
Do t s truy n l a ch n là 1:2 nên bánh ai ch ng là bánh l n, bánh ai b ng làỷ ố ề ự ọ đ ủ độ ớ đ ị độ bánh nh ỏ
S r ng c a bánh ai b ng: ố ă ủ đ ị độ z 2 = u z 1
1 = 1 2 là t s truy n c a b truy nỉ ố ề ủ ộ ề + n 1 ,n 2 là t c quay c a bánh ch ng và b ngố độ ủ ủ độ ị độ
S r ng c a bánh ai ch ng: ố ă ủ đ ủ độ z 1 = 2 z 2 = 2.14= 28
S tính toán kho ng cách tr cơ đồ ả ụ
Ch n ọ z d c ng kính vòng chia c a các bánh ai: Đườ ủ đ d 1 = m z 1 = 2 28 F (mm); d 2 = m z 2 2.14( (mm) ng kính ngoài c a bánh ai: Đườ ủ đ
Tính ch n bi ch n khâu 3ọ ổ đỡ ặ
Do vít me được ch n là lo i rotating nut nh trên nên bi ch n ọ ạ ư ổ đỡ ặ đượ ắc l p ch t v i aiặ ớ đ c c a vitme, t c là ng kính l d c a l n là d ố ủ ứ đườ ỗ ủ ổ ă D 1 = 40 (mm)
Chọn theo hãng SKF là vòng bi số hiệu 7208 BECBJ.
Vị trí đặt ổ bi theo tham khảo của hãng SKF
Khâu 3 của robot, theo nhiệm vụ thiết kế, có thể đạt vận tốc tối đa 2780 mm/s khi hoạt động không tải Trong tình huống này, lực dọc trục, momen xoắn và công suất động cơ đều được thiết kế cho chế độ không tải Do đó, khối lượng của khâu 3 chỉ bao gồm khối lượng của trục vít me và tay kẹp.
F amax khôngtải = ( m vít + m tay kẹp ) ( q ¨ 3 – g) = ( 2+0,59 ).(44.13 -9,81) = 88,89 (N)
Momen xoắn truyền vào trục vít me / momen quay của đai ốc:
Tốc độ quay đai ốc n 3 = 60 Z p v 3 max = 60.2,78 1.0,025 = 6672 (v/p) với Z là số mối ren
Chọn tỷ số truyền của bộ truyền động là u = 1:2, tốc độ thực tế của động cơ: n đc3
Tính toán công suất của động cơ: W đc 3 = 9550 T 3 n η 3 p = 9550.0,898 0,36.6672 = 0,28 (kW)
Từ các thông số trên, ta chọn động cơ truyền động cho trục vít me là động cơ AC Servo Motor HG-JR53 với thông số
Công suất tối đa 0,5kW
Momen xoắn cực đại 4,8 Nm
Tốc độ quay tối đa 6000 v/p
Khâu 2 sử dụng hộp giảm tốc và động cơ servo
Hộp giảm tốc RV-25N với tỉ số truyền 41, nặng 3,8kg
T ng t khâu 3, ng c d n ng khâu 2 c ng ươ ự độ ơ ẫ độ ũ được thi t k khi robot ch y ế ế ạ không t i v i t c t i a.ả ớ ố độ ố đ
Momen xoắn cực đại đặt tại khớp của khâu 2: T 2 max = 110 (Nm)
Momen xoắn cần thiết của động cơ khâu 2 là:
41 =2.68 (Nm) Tốc độ tối đa cần thiết của động cơ khâu 2 là: n đc2 A n 2 A × 111=¿ 4551(vòng/phút)
T các thông s trên, ta ch n ng c truy n ng cho tr c vít me là ng c ACừ ố ọ độ ơ ề độ ụ độ ơ Servo Motor HG-JR153 v i thông s ớ ố
Khâu 1 sử dụng hộp giảm tốc và động cơ servo
Hộp giảm tốc RV-25N với tỉ số truyền 41, nặng 3,8kg
T ng t khâu 3, ng c d n ng khâu 1 c ng ươ ự độ ơ ẫ độ ũ được thi t k khi robot ch yế ế ạ không t i v i t c t i a.ả ớ ố độ ố đ
Momen xoắn cực đại đặt tại khớp của khâu 1: T 1 max = 112 (Nm)
Coi hi u su t truy n ng c a b gi m t c cyclon là ệ ấ ề độ ủ ộ ả ố = 0,9
Tính toán công su t c a ng c : ấ ủ độ ơ W dc 1 = T 9550 1 max n 2 = 112.75
Momen xoắn cần thiết của động cơ khâu 1 là:
Tốc độ tối đa cần thiết của động cơ khâu 2 là: n đc2 = 41n 2 A × 75=¿ 3075(vòng/phút)
T các thông s trên, ta ch n ng c truy n ng cho tr c vít me là ng c ACừ ố ọ độ ơ ề độ ụ độ ơ Servo Motor HG-JR103 v i thông s ớ ố
CHƯƠNG 8 Thiết Kế Hệ Thống Điều Khiển
8.1 Chọn luật điều khiển, thiết kế mô hình điều khiển
Sau khi nhận dữ liệu đầu vào liên quan đến quỹ đạo của khâu tác động cuối hoặc khớp, hệ thống điều khiển cần điều chỉnh chuyển động của robot để đảm bảo di chuyển theo quỹ đạo đã được thiết lập.
Hiện nay có nhiều phương pháp điều khiển robot như:
+ Điều khiển tự do và điều khiển có tương tác với đối tượng.
+ Điều khiển phân tán và điều khiển tập trung
+ Điều khiển thích nghi phi tuyến.
Mô hình hệ tay máy xây dựng được xác định là một hệ đa biến và phi tuyến Trong báo cáo này, nhóm nghiên cứu áp dụng phương pháp điều khiển tập trung kết hợp với mô hình động lực học nhằm thiết kế hệ thống điều khiển cho robot.
Bộ điều khiển PD hiện đang được sử dụng phổ biến trong việc điều khiển robot và nhiều hệ thống khác Nhiệm vụ chính của bộ PD là giảm sai lệch e(t) của hệ thống về 0, đồng thời đảm bảo quá trình quá độ đáp ứng các tiêu chuẩn chất lượng cơ bản.
- Nếu sai lệch e(t) càng lớn thì thông qua khâu khuếch đại (P), tín hiệu u(t) càng lớn.
- Nếu thay đổi sai lệch của e(t) càng lớn thì thông qua thành phần vi phân (D), phản ứng thích hợp của u(t) sẽ càng nhanh.
Bộ điều khiển PID được mô tả bằng mô hình vào ra:
Hình 8.1: Mô hình hệ thống sử dụng bộ điều khiển PID
Ta có phương trình động lực học của robot đã xây dựng được từ nội dung trước.
Kỹ thuật tuyến tính hóa thông qua bù truyền thẳng, hay còn gọi là điều khiển trước mô men, giúp tính toán các lực và mô men cần thiết để tạo ra quỹ đạo mong muốn Nhờ đó, bài toán phi tuyến phức tạp được chuyển đổi thành bài toán tuyến tính dễ giải quyết.
Phương trình lực điều khiển được chọn như sau :
Hình 8.2: Mô hình đối tượng điều khiển
Kết hợp hai mô hình dẫn đến một luật điều khiển tuyến tính: Tín hiệu điều khiển U’ được xác định
Hình 8.3: Mô hình hệ thống.
Hệ số Kp và Kd là các ma trận đường chéo kích thước (nxn) có ảnh hưởng lớn đến độ chính xác của quỹ đạo khi dừng tại vị trí bề mặt công tác Để đảm bảo độ chính xác cao, hệ số Kd và Kp thường được chọn với giảm chấn tới hạn là 1.
Để tránh hiện tượng công hưởng, cần chọn ωn < ωr /2, trong đó J là mô men quán tính và ki là mô men quán tính của khâu i J thay đổi theo hình dạng tay máy, vì vậy cần chọn giá trị lớn nhất để tính toán Việc xác định các giá trị này có thể thực hiện thông qua mô phỏng với các thông số cụ thể về kích thước và vật liệu thực tế Trong thực tế, các giá trị Kp và Kv thường được xác định dựa trên kinh nghiệm, đồng thời phải đảm bảo nằm trong giới hạn vật lý của robot.
8.2 Mô phỏng bằng phần mềm matlab
Hình 8.4 : Mô hình điều khiển PD và Lực
Code trong phần matlab funtion của torque tính lực dẫn động tại các khớp function U = dong_luc_hoc( input )
%u ph?y u_phay=input(1:n); q=input(1+n:2*n); q1=q(1); q2=q(2); qdot=input(1+2*n:3*n); q1_d=qdot(1); q2_d=qdot(2);
%%=============================== Thong so dau vao=========================% Kich thuoc, khoi luong a1=0.350;%m a2=0.30;%m m13;%kg m2;%kg m3=2.59;%kg g=9.81;%m/s^2
+m3*(a1*a1+a2*a2+2*a1*a2*cos(q2)); m12 I2zz+I3zz+m2*(0.5*a1*a2*cos(q2)+a2*a2/4)+m3*(a2*a2+a1* a2*cos(q2)); m13 = 0; m21 I2zz+I3zz+m2*(0.5*a1*a2*cos(q2)+a2*a2/4)+m3*(a2*a2+a1* a2*cos(q2)); m22 = I2zz+I3zz+m2*a2*a2/4+m3*a2*a2; m23 = 0; m31 = 0; m32 = 0; m33 = m3;
%%============================Ma tran corilois============================= c11 = -(0.5*m2+m3)*a1*a2*q2_d*sin(q2); c12 = -(0.5*m2+m3)*(q1_d+q2_d)*a1*a2*sin(q2); c13 = 0; c21 = (0.5*m2+m3)*a1*a2*q1_d*sin(q2); c22 = 0; c33 = 0;
%%============================== Luc suy rong ko the=======================G=[0; 0; m3*g];
Kết quả mô phỏng trên matlab : Đồ thị vị trí và sai lệch vị trí của các biến khớp
Thiết kế Robot gắp khuôn đúc đến vị trí tháo khuôn là một đề tài thực tiễn đầy thách thức, nhưng nhờ sự hướng dẫn của thầy Phan Bùi Khôi, nhóm em đã hoàn thành các yêu cầu của học phần Trong quá trình học, em đã tìm hiểu về cách xây dựng mô hình robot scara, cũng như tính toán thiết kế robot và hệ thống điều khiển Các công việc cụ thể đã được hoàn thành bao gồm việc thiết kế và lập trình cho robot.
- Đặt ra vấn đề và đưa ra phương pháp giải quyết cho bài toán đề bài đặt ra
- Xây dựng kết cấu cho robot
- Tính toán động học thuận, đông học nghịch
- Tính toán động lực học
- Xét tính ổn định của hệ thống
- Thiết kế hệ thống điều khiển trong Matlab Simulink
- Lập trình điều khiển và mô phỏng trên matlab
Do thời gian và kiến thức hạn chế, nhóm em chỉ có thể giải quyết một số vấn đề cơ bản trong thiết kế hệ thống điều khiển robot Còn nhiều thách thức cần khắc phục để sản phẩm robot hoàn thiện hơn Vì vậy, nhóm em rất mong nhận được ý kiến đóng góp từ thầy để cải thiện đề tài này.
Nhóm em xin chân thành cảm ơn!
Thiết Kế Qũy Đạo Chuyển Động
Xác Định Không Gian Làm Việc Của Robot
Ta đã biết điểm tác động cuối của robot, khi ta giới hạn các biến khớp ta sẽ tìm được vùng không gian làm việc của robot
Không gian làm việc trong không gian 3D Không gian làm việc trên mặt phẳng
Sử dụng phần mềm matlab ta có thể vẽ được vùng làm việc của robot như trên
Cơ Sở Về Thiết Kế Qũy Đạo
Thiết kế quỹ đạo chuyển động của robot là yếu tố quan trọng trong việc điều khiển robot di chuyển từ vị trí này đến vị trí khác trong không gian làm việc Đường đi và quỹ đạo được xác định là các tham số chính cho hệ thống điều khiển vị trí của robot Vì vậy, độ chính xác của quỹ đạo thiết kế sẽ ảnh hưởng trực tiếp đến chất lượng di chuyển của robot.
Yêu cầu thiết kế quỹ đạo chuyển động của robot là :
Khâu chấp hành phải đảm bảo đi qua lần lượt các điểm trong không gian làm việc hoặc di chuyển theo một quỹ đạo xác định
Quỹ đạo của robot phải là đường liên tục về vị trí trong một khoảng nhất định
Không có bước nhảy về vận tốc và gia tốc
Quỹ đạo là các đường cong dạng :
Đa thức bậc 2 : x(t)=a+bt+ct 2
Đa thức bậc 3 : x(t)=a+bt+ct 2 +dt 3
Đa thức bậc cao : x(t)=a+bt+ +kt n
Ta sử dụng quỹ đạo dạng đa thức bậc 3 để thiết kế.
Thiết Kế Qũy Đạo Chuyển Động
6.3.1 Thiết kế quỹ đạo trong không gian khớp
Chọn hai điểm A và B bất kỳ trong không gian làm việc với tọa độ điểm tác động cuối là (xE, yE, zE) và xác định hướng của khâu thao tác Tiến hành thiết kế quỹ đạo chuyển động từ điểm A đến điểm B một cách linh hoạt và hiệu quả.
Theo bài toán động học ngược, chúng ta đã xác định các biến khớp tại A và B Để thiết kế quỹ đạo, chúng ta chọn đa thức hàm bậc 3 theo thời gian, với i = 1, 2 tương ứng với các biến khớp.
Ta được hệ phương trình như sau :
Giả sử cho t=5s robot đi từ điểm A( 0,2 + ; 0,25+ ) tới điểm B ( 0,5; )
Tương ứng với tọa độ khớp là: A( ); và điểm B( ).
6.3.2 Thiết kế quỹ đạo trong không gian thao tác
Xét bài toán quỹ đạo của điểm tác động cuối theo đường thẳng từ A đến B trong tE(s) Phương trình đường thẳng từ A(x0, y0) đến điểm B(xE, yE) là :
=> Để thỏa mãn điều khiện về vận tốc đầu và cuối ta thiết lập quan hệ x=x(t) là đa thức bậc 3 :
Với các điều kiện như sau :
Ví dụ robot đi qua điểm A(0.8 ;0.1) và điểm B(-0.2 ;0.5) và khoảng thời gian te=2s
6.3.3 Thiết kế quỹ đạo cho phần mô phỏng trên Matlab
Thực hiện bài toán quỹ đạo chu kì thời gian đặc trưng trong robot công nghiệp như sau:
- Di chuyển 50mm theo phương thẳng đứng đi xuống
- Di chuyển 50mm theo phương thẳng đứng đi lên
- Di chuyển sang ngang 300mm
- Tiếp tục di chuyển 50mm theo phương thẳng đứng đi xuống
- Di chuyển 50mm theo phương thẳng đứng đi lên trên
- Cuối cùng di chuyển 300mm về vị trí ban đầu.
Theo yêu cầu như trên, em đặt toạ độ cho các điểm A,B,C và D như sau:
Thiết Kế Hệ Dẫn Động Robot
Tay gắp
Tay gắp sử dụng ở đây là tay gắp nam châm điện MHM-X7400A của hãng SMC Ưu điểm : nhỏ gọn, dễ kết nối, tải được tải trọng lớn
Tải trọng max :200N~20kg, khối lượng 0,59kg
Khâu 3
Cơ cấu sử dụng: trục vít me- đai ốc bi Truyền động vít me - đai ốc được dùng để biến chuyển động quay thành chuyển động tịnh tiến.
Vít, bi: Thép 40CrMn ([ σ c ]x5 (Mpa) Đai ốc: Thép 18CrMnTi ( Thép không gỉ )
Sơ đồ truyền động Đai ốc lựa chọn d o x P h L L 1 L 2 L 3 L 4 L 5 L 6 L 7 L 8 L 9
Tính ch n c c u truy n ng khâu 3ọ ơ ấ ề độ
S d ng c c u truy n ng cho khâu 3 là ai r ng Do truy n l c b ng n kh p, truy nử ụ ơ ấ ề độ đ ă ề ự ằ ă ớ ề ng ai r ng có nh ng u i m: Không có tr t, t s truy n l n (u 12, ôi khi u < độ đ ă ữ ư đ ể ượ ỉ ố ề ớ ≤ đ
20), hi u su t cao, không c n l c c ng ban u l n, l c tác d ng lên tr c và lên nh ệ ấ ầ ự ă đầ ớ ự ụ ụ ổ ỏ
Do t s truy n l a ch n là 1:2 nên bánh ai ch ng là bánh l n, bánh ai b ng làỷ ố ề ự ọ đ ủ độ ớ đ ị độ bánh nh ỏ
S r ng c a bánh ai b ng: ố ă ủ đ ị độ z 2 = u z 1
1 = 1 2 là t s truy n c a b truy nỉ ố ề ủ ộ ề + n 1 ,n 2 là t c quay c a bánh ch ng và b ngố độ ủ ủ độ ị độ
S r ng c a bánh ai ch ng: ố ă ủ đ ủ độ z 1 = 2 z 2 = 2.14= 28
S tính toán kho ng cách tr cơ đồ ả ụ
Ch n ọ z d c ng kính vòng chia c a các bánh ai: Đườ ủ đ d 1 = m z 1 = 2 28 F (mm); d 2 = m z 2 2.14( (mm) ng kính ngoài c a bánh ai: Đườ ủ đ
Tính ch n bi ch n khâu 3ọ ổ đỡ ặ
Do vít me được ch n là lo i rotating nut nh trên nên bi ch n ọ ạ ư ổ đỡ ặ đượ ắc l p ch t v i aiặ ớ đ c c a vitme, t c là ng kính l d c a l n là d ố ủ ứ đườ ỗ ủ ổ ă D 1 = 40 (mm)
Chọn theo hãng SKF là vòng bi số hiệu 7208 BECBJ.
Vị trí đặt ổ bi theo tham khảo của hãng SKF
Khâu 3 của robot có khả năng đạt vận tốc tối đa 2780 mm/s khi hoạt động không tải Do đó, lực dọc trục, momen xoắn và công suất động cơ đều được thiết kế cho chế độ không tải Khối lượng của khâu 3 chỉ bao gồm khối lượng của trục vít me và tay kẹp.
F amax khôngtải = ( m vít + m tay kẹp ) ( q ¨ 3 – g) = ( 2+0,59 ).(44.13 -9,81) = 88,89 (N)
Momen xoắn truyền vào trục vít me / momen quay của đai ốc:
Tốc độ quay đai ốc n 3 = 60 Z p v 3 max = 60.2,78 1.0,025 = 6672 (v/p) với Z là số mối ren
Chọn tỷ số truyền của bộ truyền động là u = 1:2, tốc độ thực tế của động cơ: n đc3
Tính toán công suất của động cơ: W đc 3 = 9550 T 3 n η 3 p = 9550.0,898 0,36.6672 = 0,28 (kW)
Từ các thông số trên, ta chọn động cơ truyền động cho trục vít me là động cơ AC Servo Motor HG-JR53 với thông số
Công suất tối đa 0,5kW
Momen xoắn cực đại 4,8 Nm
Tốc độ quay tối đa 6000 v/p
Khâu 2
Khâu 2 sử dụng hộp giảm tốc và động cơ servo
Hộp giảm tốc RV-25N với tỉ số truyền 41, nặng 3,8kg
T ng t khâu 3, ng c d n ng khâu 2 c ng ươ ự độ ơ ẫ độ ũ được thi t k khi robot ch y ế ế ạ không t i v i t c t i a.ả ớ ố độ ố đ
Momen xoắn cực đại đặt tại khớp của khâu 2: T 2 max = 110 (Nm)
Momen xoắn cần thiết của động cơ khâu 2 là:
41 =2.68 (Nm) Tốc độ tối đa cần thiết của động cơ khâu 2 là: n đc2 A n 2 A × 111=¿ 4551(vòng/phút)
T các thông s trên, ta ch n ng c truy n ng cho tr c vít me là ng c ACừ ố ọ độ ơ ề độ ụ độ ơ Servo Motor HG-JR153 v i thông s ớ ố
Khâu 1
Khâu 1 sử dụng hộp giảm tốc và động cơ servo
Hộp giảm tốc RV-25N với tỉ số truyền 41, nặng 3,8kg
T ng t khâu 3, ng c d n ng khâu 1 c ng ươ ự độ ơ ẫ độ ũ được thi t k khi robot ch yế ế ạ không t i v i t c t i a.ả ớ ố độ ố đ
Momen xoắn cực đại đặt tại khớp của khâu 1: T 1 max = 112 (Nm)
Coi hi u su t truy n ng c a b gi m t c cyclon là ệ ấ ề độ ủ ộ ả ố = 0,9
Tính toán công su t c a ng c : ấ ủ độ ơ W dc 1 = T 9550 1 max n 2 = 112.75
Momen xoắn cần thiết của động cơ khâu 1 là:
Tốc độ tối đa cần thiết của động cơ khâu 2 là: n đc2 = 41n 2 A × 75=¿ 3075(vòng/phút)
T các thông s trên, ta ch n ng c truy n ng cho tr c vít me là ng c ACừ ố ọ độ ơ ề độ ụ độ ơ Servo Motor HG-JR103 v i thông s ớ ố
Thiết Kế Hệ Thống Điều Khiển
Chọn luật điều khiển, thiết kế mô hình điều khiển
Sau khi nhận được dữ liệu đầu vào liên quan đến quỹ đạo của khâu tác động cuối hoặc khớp, hệ thống điều khiển cần điều chỉnh chuyển động của robot để theo đúng quỹ đạo đã được xác định.
Hiện nay có nhiều phương pháp điều khiển robot như:
+ Điều khiển tự do và điều khiển có tương tác với đối tượng.
+ Điều khiển phân tán và điều khiển tập trung
+ Điều khiển thích nghi phi tuyến.
Mô hình hệ tay máy xây dựng được xác định là một hệ đa biến và phi tuyến Trong báo cáo này, nhóm nghiên cứu áp dụng phương pháp điều khiển tập trung kết hợp với mô hình động lực học để thiết kế hệ thống điều khiển cho robot.
Bộ điều khiển PD hiện nay được sử dụng phổ biến trong việc điều khiển robot và nhiều hệ thống khác nhau Nhiệm vụ chính của bộ PD là giảm sai lệch e(t) của hệ thống về 0, đảm bảo quá trình quá độ đạt các yêu cầu chất lượng cơ bản.
- Nếu sai lệch e(t) càng lớn thì thông qua khâu khuếch đại (P), tín hiệu u(t) càng lớn.
- Nếu thay đổi sai lệch của e(t) càng lớn thì thông qua thành phần vi phân (D), phản ứng thích hợp của u(t) sẽ càng nhanh.
Bộ điều khiển PID được mô tả bằng mô hình vào ra:
Hình 8.1: Mô hình hệ thống sử dụng bộ điều khiển PID
Ta có phương trình động lực học của robot đã xây dựng được từ nội dung trước.
Kỹ thuật tuyến tính hóa thông qua bù truyền thẳng, hay còn gọi là điều khiển trước mô men, là phương pháp cần thiết để xác định các lực và mô men cần thiết cho việc tạo ra quỹ đạo mong muốn Bằng cách tính toán trước các yếu tố này và kết nối chúng vào hệ thống kỹ thuật cần điều khiển, bài toán phi tuyến phức tạp sẽ được chuyển đổi thành bài toán tuyến tính dễ giải quyết hơn.
Phương trình lực điều khiển được chọn như sau :
Hình 8.2: Mô hình đối tượng điều khiển
Kết hợp hai mô hình dẫn đến một luật điều khiển tuyến tính: Tín hiệu điều khiển U’ được xác định
Hình 8.3: Mô hình hệ thống.
Các hệ số Kp và Kd là các ma trận đường chéo kích thước (nxn) có ảnh hưởng đến độ chính xác của quỹ đạo khi dừng tại vị trí bề mặt công tác Để đạt được độ chính xác tối ưu, hệ số Kd và Kp thường được chọn với giảm chấn tới hạn là 1.
Để tránh hiện tượng công hưởng, cần đảm bảo tần số ωn nhỏ hơn ωr/2, trong đó J là mô men quán tính và ki là mô men quán tính của khâu i Mô men quán tính J sẽ thay đổi tùy theo hình dạng tay máy, vì vậy cần chọn giá trị lớn nhất để tính toán Việc xác định các giá trị này có thể thực hiện thông qua mô phỏng với các thông số cụ thể về kích thước và vật liệu thực tế Trong thực tế, các giá trị Kp và Kv thường được lựa chọn dựa trên kinh nghiệm, đồng thời phải đảm bảo nằm trong giới hạn vật lý của robot.
Mô phỏng bằng phần mềm matlab
Hình 8.4 : Mô hình điều khiển PD và Lực
Code trong phần matlab funtion của torque tính lực dẫn động tại các khớp function U = dong_luc_hoc( input )
%u ph?y u_phay=input(1:n); q=input(1+n:2*n); q1=q(1); q2=q(2); qdot=input(1+2*n:3*n); q1_d=qdot(1); q2_d=qdot(2);
%%=============================== Thong so dau vao=========================% Kich thuoc, khoi luong a1=0.350;%m a2=0.30;%m m13;%kg m2;%kg m3=2.59;%kg g=9.81;%m/s^2
+m3*(a1*a1+a2*a2+2*a1*a2*cos(q2)); m12 I2zz+I3zz+m2*(0.5*a1*a2*cos(q2)+a2*a2/4)+m3*(a2*a2+a1* a2*cos(q2)); m13 = 0; m21 I2zz+I3zz+m2*(0.5*a1*a2*cos(q2)+a2*a2/4)+m3*(a2*a2+a1* a2*cos(q2)); m22 = I2zz+I3zz+m2*a2*a2/4+m3*a2*a2; m23 = 0; m31 = 0; m32 = 0; m33 = m3;
%%============================Ma tran corilois============================= c11 = -(0.5*m2+m3)*a1*a2*q2_d*sin(q2); c12 = -(0.5*m2+m3)*(q1_d+q2_d)*a1*a2*sin(q2); c13 = 0; c21 = (0.5*m2+m3)*a1*a2*q1_d*sin(q2); c22 = 0; c33 = 0;
%%============================== Luc suy rong ko the=======================G=[0; 0; m3*g];
Kết quả mô phỏng trên matlab : Đồ thị vị trí và sai lệch vị trí của các biến khớp
