1 Tổng quan Robot Mô tả robot Hình 1.1 : Cấu trúc tổng thể của cánh tay robot này có ba bậc tự do xoay, với khớp 1 tạo chuyển động xoay tròn ở phần đế, khớp 2 cho phép cánh tay dichuyển
ĐỘNG HỌC THUẬN
Mô tả về Robot
Cánh tay robot được mô tả trong Hình 1.1 có cấu trúc tổng thể với ba bậc tự do xoay Khớp 1 cho phép chuyển động xoay tròn ở phần đế, khớp 2 hỗ trợ di chuyển cánh tay lên xuống, trong khi khớp 3 có khả năng thay đổi góc hoặc vị trí của phần cuối cánh tay so với thân chính Cấu trúc này rất phù hợp cho việc điều khiển robot trong không gian hai chiều hoặc ba chiều, tùy thuộc vào cách bố trí và điều khiển các khớp.
Với thông số các khâu như sau:
Bảng 1 Thông số chiều dài các khâu robot ba bậc
Ký hiệu Mô tả Thông số Dơn vị
L0 Chiều dài tọa độ gốc 10 cm
Trình bày đặt trục theo modified DH
Hình 1 2 Mô hình robot 3 bậc
Mỗi khớp trong hệ thống robot cần được gán một hệ tọa độ riêng biệt (gồm trục x, y, z), bắt đầu từ khớp đầu tiên (khớp 1) và tiếp tục đến khớp cuối cùng Các trục tọa độ này được xác định dựa trên một quy tắc cụ thể.
Bước 1: Mô tả link: Hệ trục [1-3] tương ứng với link thứ i Một link có hai khớp đầu link
Trục được đặt tại khớp thứ {i} của link {i} và trùng với trục động cơ, đồng thời cũng nằm ở cuối link {i} tại khớp {i+1} Quá trình xác định các đường vuông góc chung sẽ bao gồm (a), và độ xoắn của link sẽ được thể hiện qua ( ).
Bước 2: Mô tả khớp: Một khớp được mô tả bởi ha thông số góc khớp ( ) và độ lệch khớp
(d) Góc khớp là góc giữa hai đường vuông góc chung, độ lệch khớp là khoảng cách giữa hai mặt phẳng chứa đường vuông góc chung
Bước 3 : Tiến hành đặt hệ trục phần đế robot
Hệ trục tọa độ được thiết lập tại link thứ 0, và có thể được định vị ở bất kỳ nơi nào, nhưng để thuận tiện cho việc tính toán, nên đặt theo hướng tối ưu Điều này có nghĩa là hướng dọc trục và vị trí gốc tọa độ được chọn sao cho các thông số DH có nhiều giá trị bằng 0 nhất Quy ước này áp dụng cho cả khớp xoay và khớp lăng trụ.
Bước 4: Trục zᵢ: Trục z của khớp thứ i được chọn theo hướng của chuyển động khớp tại vị trí đó Cụ thể:
Với khớp quay (revolute joint), trục zᵢ là trục mà quanh đó khớp quay.
Với khớp tịnh tiến (prismatic joint), trục zᵢ là trục mà dọc theo đó khớp di chuyển.
Trục xᵢ dọc đường vuông góc chung chiều đến
Trục y được xác định theo quy tắc bàn tay phải, dựa trên hướng của các trục xᵢ và zᵢ Mặc dù không xuất hiện trong bảng MDH, trục y vẫn đóng vai trò quan trọng trong mô hình không gian ba chiều.
Bước 7: Đặt hệ trục cho End Effector :
Nếu khớp thứ n là khớp quay, chiều của được chọn dọc theo chiều của khi và điểm gốc của hệ trục {n} được chọn tại giao điểm của với để
Thiết lập bảng DH
Trong đó: i là vị trí của khớp; αi−1 là góc giữa trục Zi và Zi+1 được tính toán quanh trục
Khoảng cách giữa các trục tọa độ được xác định bởi các thông số Xi, αi−1, di và θi Cụ thể, αi−1 là khoảng cách giữa trục Zi và Zi+1 theo trục Xi, di là khoảng cách giữa trục Xi−1 và Xi dọc theo trục Zi, và θi là góc giữa trục Xi−1 và Xi quanh trục Zi Từ đó, ta có thể thiết lập ma trận chuyển đổi giữa hệ trục tọa độ {i-1} và {i}.
Tính các ma trận chuyển đổi
Tính toán ma trận chuyển đổi đồng nhất từ hệ {n} về [4]:
Ma trận chuyển đổi đồng nhất tổng quát chuyển từ hệ trục i về hệ trục i-1 :
414\* MERGEFORMAT (.) Đặt , tượng tự với và
Ma trận chuyển đổi đồng nhất từ hệ tọa độ thứ 1 về hệ tọa độ thứ 0:
Ma trận chuyển đổi đồng nhất từ hệ tọa độ thứ 2 về hệ tọa độ thứ 1:
Ma trận chuyển đổi đồng nhất từ hệ tọa độ thứ 3 về hệ tọa độ thứ 2:
Ma trận chuyển đổi đồng nhất từ hệ tọa độ thứ 4 về hệ tọa độ thứ 3:
Ma trận chuyển đổi đồng nhất từ hệ tọa độ thứ 4 về hệ tọa độ thứ 0:
Tính toán bằng MATLAB function T = Foward (anpha,a,d,theta)
T =[cos(theta) -sin(theta) 0 a; sin(theta)*cosd(anpha) cos(theta)*cosd(anpha) - sind(anpha) -sind(anpha)*d; sin(theta)*sind(anpha) cos(theta)*sind(anpha) cosd(anpha) cosd(anpha)*d;
Kết quả : Ma trận chuyển đổi hệ trục tọa độ {1} sang hệ trục tọa độ {0}:
Kết quả : Ma trận chuyển đổi hệ trục tọa độ {2} sang hệ trục tọa độ {1}:
Kết quả : Ma trận chuyển đổi hệ trục tọa độ {3} sang hệ trục tọa độ {2}
Kết quả : Ma trận chuyển đổi từ trục tọa độ {2} sang trục tọa độ {0}:
Kết quả : Ma trận chuyển đổi từ trục tọa độ {3} sang trục tọa độ {0}:
Tính toán vị trí và hướng của điểm đầu cuối
Từ công thức 19 ta suy được vị trí và hướng của điểm đầu cuối:
Vậy tọa độ x, y, z của điểm đầu cuối:
Kết quả : Vị trí điểm đầu cuối EE trong hệ trục tọa độ {0}
Kết quả : Vị trí điểm đầu cuối EE trong hệ trục tọa độ {S}
ĐỘNG HỌC NGHỊCH
Trình bày cách tiệp cận bài toán
Xác định mô hình động học thuận:
Để bắt đầu, cần thiết lập mô hình động học thuận cho hệ thống, xác định mối liên hệ giữa các biến khớp như góc và độ dài với vị trí và hướng của điểm cuối.
Mô hình này thường được biểu diễn dưới dạng phương trình hình học hoặc phương trình ma trận (ví dụ: sử dụng phương pháp Denavit-Hartenberg).
Thiết lập phương trình động học nghịch:
Sử dụng mô hình động học thuận để thiết lập các phương trình động học nghịch nhằm tìm các giá trị của biến khớp Mục tiêu cuối cùng là đạt được vị trí và hướng mong muốn cho điểm cuối.
Các phương trình này thường không có dạng đóng, dẫn đến việc có thể xuất hiện nhiều nghiệm khác nhau, do có nhiều cấu hình khớp tương ứng với cùng một vị trí hoặc hướng của điểm cuối.
Giải phương trình động học nghịch:
Phương pháp giải tích: Nếu hệ thống đơn giản, có thể giải trực tiếp phương trình động học nghịch để tìm các nghiệm của các biến khớp.
Trong trường hợp các hệ thống phức tạp, phương pháp giải tích thường không khả thi, vì vậy người ta thường áp dụng các phương pháp số như thuật toán Newton-Raphson, gradient descent và các phương pháp tối ưu hóa khác để tìm kiếm nghiệm.
Bài toán động học nghịch thường có nhiều nghiệm hoặc không có nghiệm Cần phải xác định nghiệm nào là phù hợp với ứng dụng cụ thể.
Các tiêu chí như tối ưu hóa năng lượng, tránh va chạm, hoặc tối ưu hóa về độ linh hoạt có thể được áp dụng để lựa chọn nghiệm.
Kiểm tra và điều chỉnh:
Sau khi xác định được nghiệm, việc kiểm tra tính khả thi của chúng trong thực tế là rất quan trọng, đặc biệt khi hệ thống có các ràng buộc về vật lý hoặc cơ học.
Nếu cần, thực hiện điều chỉnh để đảm bảo hệ thống hoạt động chính xác.
Bài này ta sử dụng phương pháp đại số
Mô hình cánh tay robot ba bậc tự do với ba khớp xoay nối tiếp cho phép tính toán các góc cần thiết khi biết tọa độ x, y, z của điểm cuối Để xác định các góc này, chúng ta áp dụng phương pháp đại số, bắt đầu bằng việc sử dụng phương trình và Đặc biệt, hàm atan2 được sử dụng để tính toán góc, giúp tránh sai sót trong quá trình tính toán.
Tiếp theo, chúng ta sẽ xem xét mối quan hệ giữa các phương trình và xác định giá trị cần tìm bằng cách bình phương hai vế Sau khi có giá trị này, chúng ta có thể xác định các giá trị khác dựa vào mối quan hệ của các phương trình đã được thiết lập.
Lời giải bài toán động học nghịch
Ta lấy 112 * -113* ta được phương trình sau :
Ta lấy 112 * +113* được phương trình như sau:
Kết hợp với phương trình 114 ta được:
22222\* MERGEFORMAT (.) Đặt ta tìm được :
Ta tìm được 2 nghiệm như sau:
Từ phương trình 222, ta tách , theo công thức lượng giác ta được:
Từ phương trình 226 ta được:
Thay thế và lẫn nhau ở 2 phương trình, ta được:
Từ phương trình 229 ta tìm được theta 2:
Như vậy, ta đã tìm được các góc theta thông qua vị trí của EE:
Ta có được 4 bộ nghiệm:
QUY HOẠCH QUỸ ĐẠO
Giới thiệu
Quy hoạch quỹ đạo là một yếu tố quan trọng trong lĩnh vực dẫn đường và quy hoạch chuyển động Hệ thống cấp bậc trong quy hoạch chuyển động thường được cấu trúc theo một mô hình điển hình, giúp tối ưu hóa quá trình di chuyển.
Task planning – Thiết kế một vài mục tiêu (Ví dụ như nhặt đồ vật trước mặt bạn lên, gắp vật, thả vật, hàn, )
Path planning – Đưa ra đường dẫn khả thi từ điểm đầu đến điểm cuối Một đường dẫn luôn bao gồm tập hợp của các điểm tham chiếu.
Lập kế hoạch quỹ đạo là quá trình thiết lập lịch trình di chuyển theo thời gian, đảm bảo tuân thủ các ràng buộc như vị trí, vận tốc và gia tốc Việc này giúp tối ưu hóa hành trình, đảm bảo an toàn và hiệu quả trong quá trình di chuyển.
Trajectory following – Khi kế hoạch đã được lên thì cần một hệ thống đều khiển để cho kế hoạch có thể di chuyển đấy đủ và chính xác nhất.
Quy hoạch quỹ đạo ở không gian khớp và không gian làm việc
Quy hoạch quỹ đạo trong không gian làm việc liên quan đến việc xác định các điểm tham chiếu và nội suy trên trục tọa độ Descartes, nhằm xác định vị trí và hướng của một điểm cụ thể trên cánh tay robot, thường là ở khâu cuối (end effector).
Quy hoạch quỹ đạo trong không gian khớp liên quan đến việc xác định các điểm tham chiếu và nội suy trực tiếp trên các vị trí khớp Hình 4-2 minh họa hai quỹ đạo ở các không gian khác nhau, mỗi không gian đều có những ưu nhược điểm riêng Thông tin chi tiết về các ưu nhược điểm này được trình bày trong Bảng 3.
Không gian làm việc Không gian khớp Ưu điểm Chuyển động có thể dự đoán.
Có khả năng tránh chướng ngại vật và va chạm tốt hơn.
Tốc độ thực hiện nhanh hơn.
Chuyển động của cơ cấu truyền động trơn tru và dễ xác định hơn.
Nhược điểm Thực hiện chậm hơn.
Chuyện động của cơ cấu truyền động không trơn tru và khó xác định hơn.
Các điểm trung gian không được đảm bảo tuân theo các giới hạn chung hoặc va chạm.
Các kiểu quy hoạch quỹ đạo
Quỹ đạo vận tốc hình thang là quỹ đạo mà tại đó gia tốc giữ hằng số và vận tốc có hình dạng giống như hình thang Sự kết hợp này tạo ra một cấu hình vị trí đường cong s đặc trưng.
Ta có thể nội suy giữa 2 điểm dùng đa thức có các bậc khác nhau nhưng ta thường sử dụng
+ Cubic (đa thức bậc 3): Yêu cầu 4 điều kiện biên: Vị trí và vận tốc.
+ Quintic (đa thức bậc 5): Yêu cầu 6 điều kiện biên: Vị trí, vận tốc và gia tốc.
Quỹ đạo với các bậc cao hơn có thể được áp dụng để kết nối các vi phân bậc cao của vị trí tại điểm waypoint Trong báo cáo này, chúng tôi đã sử dụng quy hoạch quỹ đạo với đa thức bậc 3.
Quy hoạch quỹ đạo robot 3 bậc dùng đa thức bậc 3
Ta có các điều kiện ràng buộc:
37337\* MERGEFORMAT (.) Đa thức bậc ba: \* MERGEFORMAT
384Equation Section (Next)3934Equation Chapter 4 Section
3 4015Equation Chapter (Next) Section 1412Equation Section (Next)423Equation Section (Next)4333Equation Chapter 3 Section 3 44344\* MERGEFORMAT (.)
Từ các điều kiện ràng buộc ta có:
Ta đưa về dạng ma trận:
Do đó các tham số của đa thức bậc 3 có thể được tính bằng công thức sau:
Các nghiệm có thể được tính như sau:
Chúng ta có thể áp dụng phương trình (4.7) hoặc (4.8) để xác định các hệ số (i = 0, 1, 2, 3) Qua đó, ta có khả năng quy hoạch quỹ đạo theo đường thẳng mong muốn sử dụng hàm bậc 3.
3.4.1 Quy hoạch quỹ đạo qua hai điểm (1 lần)
Để lập quỹ đạo chuyển động cho robot giữa hai điểm A và B, chúng ta cần xác định tọa độ và thời gian của các điểm này Điểm A có tọa độ (10,10) và thời gian là 0 giây, trong khi điểm B có tọa độ (-20,-20) và thời gian là 2 giây Vận tốc của robot khi di chuyển qua các điểm A và B cần được tính toán để đảm bảo quỹ đạo di chuyển hợp lý.
Ta cần quy hoạch quỹ đạo di chuyển từ điểm A đến điểm B theo đa thức bậc 3 với thời gian là
Hình 3 1 Lưu đồ quy hoạch quỹ đạo qua hai điểm
3.4.2 Quy hoạch quỹ đạo qua ba điểm (1 lần)
Bài toán yêu cầu lập quỹ đạo chuyển động cho robot giữa ba điểm A, B và C với tọa độ và thời gian cụ thể: A(10,10) tại thời điểm 0 giây, B(-20,-20) tại 2 giây, C(10,-20) tại 4 giây, và quay trở lại A(10,10) tại 6 giây Vận tốc di chuyển qua các điểm A, B và C cũng cần được xác định.
Ta cần quy hoạch quỹ đạo di chuyển từ điểm A đến điểm B, điểm B đến C, điểm C về A theo đa thức bậc 3 với thời gian là
Hình 3 2 Lưu đồ quy hoạch quỹ đạo qua 3 điểm
3.4.3 Quy hoạch quỹ đạo theo đường thẳng (5 lần)
Để lập quỹ đạo chuyển động cho robot giữa hai điểm A và B, với tọa độ A(10,10) và B(-20,20), cần xác định vận tốc di chuyển tại các điểm này Quá trình di chuyển sẽ được thực hiện trong 5 chu kỳ, mỗi chu kỳ kéo dài 2 giây, bao gồm việc di chuyển từ A đến B và ngược lại.
Chúng ta cần lập kế hoạch cho quỹ đạo di chuyển từ điểm A đến điểm B bằng đa thức bậc 3 trong khoảng thời gian xác định, sau đó sẽ quay trở lại từ điểm B về điểm A cũng theo đa thức bậc 3 trong thời gian tương ứng.
Chúng tôi sẽ thực hiện 5 chu kỳ di chuyển giữa điểm A và điểm B, mỗi chu kỳ kéo dài 2 giây Sau khi hoàn thành quy hoạch, chúng tôi sẽ dừng lại tại điểm A.
Hình 3 3 Lưu đồ quy hoạch quỹ đạo qua hai điểm 5 lần
3.4.4 Quy hoạch quỹ đạo theo hình tam giác (5 lần)
Bài toán đặt ra là thiết lập quỹ đạo chuyển động cho robot đi qua ba điểm A, B và C với tọa độ A(10,10), B(-20,-20), C(10,-20) và thời gian di chuyển từ A đến C là 6 giây Vận tốc của robot khi di chuyển qua các điểm A, B và C cần được xác định để đảm bảo quỹ đạo chính xác.
Quỹ đạo chuyển động thực hiện trong 5 lần, mỗi chu kì di chuyển từ A đến B, B đến C, C về A và ngược lại là 2s.
Chúng ta cần lập kế hoạch cho quỹ đạo di chuyển từ điểm A đến B, từ B đến C, và từ C trở về A bằng đa thức bậc 3 trong khoảng thời gian nhất định Quy trình này sẽ được lặp lại 5 lần, và kết thúc tại điểm A sau khi hoàn tất quy hoạch Mỗi chu kỳ di chuyển từ A đến B, B đến C, và C trở về A sẽ diễn ra trong thời gian 2 giây.
Hình 3 4 Lưu đồ quy hoạch quỹ đạo qua 3 điểm 5 lần
KIỂM CHỨNG
Mô phỏng động học nghịch
Lấy kết quả vị trí x,y,z tính được từ khối động học thuận để kiểm chứng ngược lại các góc
Chương trình MATLAB trong khối IK_New: function [q1,q2,q3] = IK_New(x,y,z,solve)
%% Program by Tran Cong Minh, Mai Xuan Dinh l0; l1 ; l2; l3;
%% Dat cac bien lien quan den tinh goc
F = z-l1; a = -2*l2*F; b = -2*l2*E; d = l3*l3 - E*E - F*F - l2*l2; e = sqrt(a*a + b*b); if abs(d) == round(e * 10) / 10 h = 1; else h = d*d/(e*e); end alpha = atan2d((-2*l2*F)/e, (-2*l2*E)/e);
%% Bộ nghiệm thứ 1 if (solve ==1) q2 = atan2d(sqrt(1-h), d/e) + alpha
%% Bo nghiem thu 2 else q2 = atan2d(-sqrt(1-h), d/e) + alpha
Hình 4 5 Kết quả kiểm chứng động học nghịch
Hình 4 6 Kết quả kiểm chứng động học nghịch
Hình 4 7 Kiểm chứng động học nghịch
Mô phỏng quy hoạch quỹ đạo
4.3.1 Quy hoạch quỹ đạo qua hai điểm (1 lần)
Dựa trên yêu cầu của bài toán ở mục 3.4.2 và lưu đồ, chúng ta sẽ tiến hành mô phỏng đồ thị x,y để thể hiện mối quan hệ giữa các điểm A và B qua các mốc thời gian đã được quy hoạch.
Hình 4 8 Quy hoạch quỹ đạo qua hai điểm
Trong khối ma trận 2*5 trong hàm Constant, mỗi hàng của ma trận (1*5) nó sẽ biểu diễn thông tin của mỗi điểm
Tương ứng từ trái sang phải của hàng ma trận nó biểu thị thời gian, vị trí x, vị trí y, vận tốc x, vận tốc y của điểm đó.
Chương trình MATLAB trong khối Trajectory_Planning: function [x,y] = Trajectory_Planning(t,P)
%% Program by Tran Cong Minh, Mai Xuan Dinh ta=P(1,1); xa=P(1,2); ya=P(1,3); vxa=P(1,4); vya=P(1,5);
%%% tb=P(2,1); xb=P(2,2); yb=P(2,3); vxb=P(2,4); vyb=P(2,5); if t
