Tính toán động học thuận robot dư dẫn động bằng phương pháp khalil và phương pháp gradient thu gọn

LỜI NÓI ĐẦU Trong sự nghiệp công nghiệp hoá, hiện đại hoá đất nước, vấn đề tự động hoá sản xuất có một vai trò đặc biệt quan trọng. Nhu cầu nâng cao năng suất và chất lượng sản phẩm ngày càng đòi hỏi ứng dụng rộng rãi các phương tiện tự động hóa sản xuất. Xu hướng đó tạo ra những dây chuyền về thiết bị tự động có tính linh hoạt cao. Vì vậy nhu cầu ứng dụng Robot để tạo ra hệ thống sản xuất linh hoạt ngày càng tăng nhanh. Khi xét về vấn đề Robot chúng ta đặt ra các bài toán: động học Robot, động lực học Robot và điều khiển Robot. Bài toán động học Robot là cơ sở, đầu vào cho các bài toán động lực học Robot và điều khiển Robot. Bài toán động học Robot gồm có động học thuận và động học ngược. Với những Robot có số bậc tự do bằng kích thước không gian làm việc thì tọa độ các khâu hoàn toàn xác định và là duy nhất. Trường hợp Robot có số bậc tự do lớn hơn kích thước không gian làm việc người ta gọi là Robot dư dẫn động, loại Robot này có tính mềm dẻo và linh hoạt cao đang được ứng dụng rộng rãi. Robot dư dẫn động được tạo ra nhằm thoả mãn các yêu cầu như: tránh cấu hình kỳ dị, tránh chướng ngại vật, tạo ra sự khéo léo cho Robot, giới hạn mômen khớp, cực tiểu động học v.v…. Để thấy được các ưu điểm của Robot dư dẫn động, đồ án tốt nghiệp này tập trung nghiên cứu bài toán động học thuận và động học ngược của Robot dư dẫn động. Đồ án gồm các chương sau: Chương 1: Dựa trên lý thuyết cơ bản về ma trận Denavit- Hartenberg để giải bài toán động học thuận: xác định vị trí bàn kẹp, vận tốc bàn kẹp, gia tốc bàn kẹp v.v….Quan trọng nhất của phần này là xác định được vị trí bàn kẹp là đầu vào để giải quyết bài toán động học ngược được trình bày ở phần sau. Chương 2: Đưa ra một số phương pháp tìm nghịch đảo ma trận chữ nhật là cơ sở để giải quyết bài toán động học ngược Robot. Đặc biệt là phương pháp phân tích cân xứng của Khalil. Chương 3: Dựa trên phương pháp tính ma trận tựa nghịch đảo trình bày ở chương hai, chúng ta tập trung nghiên cứu và đưa ra 2 phương pháp: phương pháp Khalil và phương pháp Reduced _ Gradient để giải quyết bài toán động học ngược Robot dư dẫn động. Trong quá trình thực hiện đồ án này, do kiến thức về Robot có hạn, thời gian ít và tài liệu bằng tiếng Việt chưa có nên đồ án của em vẫn còn nhiều thiếu xót, em kính mong được sự chỉ bảo, hướng dẫn của các thầy để ngày càng hoàn thiện mình hơn. Em xin chân thành cảm ơn GS.TSKH Nguyễn Văn Khang, Th.S Đỗ Thành Trung đã tận tình chỉ bảo, hướng dẫn em hoàn thành bản đồ án này. Em cũng xin chân thành cảm ơn các thầy giáo trong bộ môn Cơ học ứng dụng đã tạo điều kiện giúp đỡ em trong quá trình làm đồ án tại bộ môn. tÝnh to¸n ®éng häc thuËn robot d­ dÉn ®éng b»ng ph­¬ng ph¸p khalil vµ ph­¬ng ph¸p gradient thu gän CHƯƠNG 1 TÍNH TOÁN ĐỘNG HỌC THUẬN RÔBỐT DƯ DẪN ĐỘNG 1.1 BỘ THÔNG SỐ DH VÀ MA TRẬN DENAVIT-HARTENBERG 1.1.1 Bộ thông số DH Khớp Khớp Khâu i-1 Khớp Khâu i Khâu i+1 Hình 1.1 Dưới đây là trình bày cách xây dựng hệ tọa độ đối với hai khâu liên tiếp i và i+1. Hình 1.1 là trường hợp 2 khớp động liên tiếp là khớp quay. Hình 1.2 là khớp tịnh tiến. Đối với Robot công nghiệp, Denavit-Hartenberg(1995) đã đưa ra cách chọn các hệ trục tọa độ như sau: Trục được chọn dọc theo hướng của trục khớp động thứ i Trục được chọn dọc theo đường vuông góc chung của hai trục và , hướng đi từ trục sang trục . Nếu trục cắt trục thì hướng của trục được chọn tuỳ ý. Gốc tọa độ được chọn tại giao điểm của trục và trục . Trục được chọn sao cho hệ là hệ quy chiếu thuận. Với cách chọn trên, đôi khi các hệ tọa độ khâu không được xác định một cách duy nhất. Vì vậy ta cần có một số bổ xung như sau: Khớp Khớp Khâu i-1 Khớp Khâu i Khâu i+1 Hình 1.2 Đối với hệ tọa độ theo quy ước trên ta mới chỉ chọn được trục , còn trục chưa có trong quy ước trên. Ta có thể chọn trục một cách tuỳ ý. Đối với hệ tọa độ do không có khớp n+1 nên ta theo quy ước trên không xác định được trục . Trục không xác định duy nhất trong khi trục lại được xác định theo pháp tuyến của trục . Trong trường hợp này nếu là khớp quay ta nên chọn trục song song với trục . Ngoài ra ta có thể chọn tuỳ ý sao cho hợp lý. Khi hai trục và song song với nhau thì giữa hai trục này có nhiều đường pháp tuyến chung, ta có thể chọn trục hướng theo pháp tuyến chung nào cũng được. Khi khớp thứ i là tịnh tiến, về nguyên tắc ta có thể chọn trục một cách tuỳ ý. Tuy nhiên trong nhiều trường hợp người ta thường chọn trục dọc theo trục của khớp tịnh tiến này. Vị trí của hệ tọa độ khâu đối với hệ tọa độ khâu được xác định bởi bộ thông số DH như sau: -: góc quay quanh trục để trục chuyển đến trục . -: khoảng cách đo dọc trục khớp động thứ i từ đường vuông góc chung giữa hai trục khớp động i+1 và i tới đường vuông góc chung giữa khớp động i và trục khớp động i-1. -: độ dài đường vuông góc chung giữa hai trục khớp động i+1 và i. -: Góc chéo giữa hai trục khớp động i+1 và i. Trong bốn tham số trên, các tham số và luôn luôn là các hằng số, độ lớn của chúng phụ thuộc vào hình dáng và sự kết nối các khâu thứ i-1 và khâu thứ i. Hai tham số còn lại và , một là hằng số một là biến số phụ thuộc vào khớp quay hay khớp tịnh tiến. Nếu khớp i là khớp quay thì là biến còn là hằng , nếu khớp i là tịnh tiến thì là biến còn là hằng số. 1.1.2 Ma trận Denavit-Hartenberg Trên cơ sở đã xây dựng các hệ tọa độ đối với hai khâu động liên tiếp như đã trình bày ở trên, ta có thể thiết lập mối quan hệ giữa hai hệ tọa độ liên tiếp theo bốn bước sau đây: Quay quanh trục một góc . Tịnh tiến dọc trục một khoảng . Tịnh tiến dọc trục một đoạn . Quay quanh trục một góc . Ma trận của phép biến đổi thuần nhất ký hiệu là Hi, là tích của bốn phép biến đổi cơ bản và có dạng như sau: (1.1) Ma trận gọi là ma trận Denavit-Hartenberg. Ma trận Denavit-Hartenberg là ma trận chuyển tọa độ của hệ quy chiếu đối với hệ quy chiếu. Chính xác hơn ta phải ký hiệu là . Đơn giản cách viết ta sử dụng ký hiệu thay cho . Còn được dùng với nghĩa . 1.1.3 Phương trình xác định vị trí khâu thao tác (bàn kẹp) của Robot Ma trận là tích các ma trận và là ma trận mô tả vị trí và hướng của hệ tọa độ gắn liền với khâu thứ i so với hệ tọa độ cố định. Trường hợp , với n là chỉ số chỉ hệ tọa gắn liền với bàn kẹp của Robot, từ đó ta có: (1.2) Trong đó ,, là tọa độ vị trí của khâu thao tác. 1.1.4 Giải quyết bài toán động học thuận của Robot Giải quyết bài toán động học thuận của Robot thực chất là chúng ta cho trước cấu hình của Robot và quy luật chuyển động của các khâu, từ đó chúng ta xác định quy luật chuyển động của bàn kẹp (điểm tác động cuối): vị trí bàn kẹp, vận tốc bàn kẹp, gia tốc chuyển động bàn kẹp, vận tốc góc, gia tốc góc v.v… 1.2 TÍNH TOÁN ĐỘNG HỌC THUẬN CỦA MỘT SỐ ROBOT 1.2.1 Bài toán động học thuận của Robot 3 khâu phẳng a. Hình vẽ và bảng thông số động học Denavit-hartenberg Hình 1.3 Trục θi di ai αi 1 2 3 q1 q2 q3 0 0 0 a1 a2 a3 0 0 0 b. Phương trình xác định vị trí bàn kẹp của Robot Áp dụng công thức (1.2) ta xác định được ma trận chuyển : Từ đó ta có được vi trí bàn kẹp trong hệ tọa độ cố định: c. Dữ liệu đầu vào của Robot 3 khâu phẳng Để tính toán cụ thể bài toán động học thuận ta phải cho trước quy luật chuyển động của các biến khớp theo thời gian và cho giá trị cụ thể của các khoảng cách . Ta chọn quy luật chuyển động của các biến khớp theo thời gian t như sau: ; ; Và các khoảng cách: , thay vào chương trình tính toán: - Vị trí: Đồ thị tọa độ bàn kẹp px(t) Đồ thị bàn kẹp py(t) Vận tốc: Đồ thị vận tốc bàn kẹp vy(t) Đồ thị vận tốc bàn kẹp vx(t) - Gia tốc: Đồ thị gia tốc bàn kẹp ay(t) Đồ thị gia tốc bàn kẹp ax(t) Quỹ đạo bàn kẹp 1.2.2 Bài toán động học thuận của Robot 4 khâu phẳng a. Hình vẽ và bảng thông số động học Denavit-Hartenberg Hình 1.4 Trục θi di ai αi 1 2 3 4 q1 q2 q3 q4 0 0 0 0 a1 a2 a3 a4 0 0 0 0 b. Phương trình xác định vị trí bàn kẹp của Robot Áp dụng công thức (1.2) ta xác định được ma trận chuyển : Ở đây do kết quả ma trận dài nên ta sử dụng kí hiệu: . Từ đó vị trí bàn kẹp trong hệ tọa độ cố định: c. Dữ liệu đầu vào cho Robot 4 khâu phẳng Chọn quy luật chuyển động các biến khớp theo thời gian: , , , Và các khoảng cách: Thay vào chương trình tính toán ta được kết quả: - Vị trí: Đồ thị tọa độ bàn kẹp Đồ thị tọa độ bàn kẹp - Vận tốc: Đồ thị vận tốc bàn kẹp Đồ thị vận tốc bàn kẹp - Gia tốc: Đồ thị gia tốc bàn kẹp Đồ thị gia tốc bàn kẹp Quỹ đạo bàn kẹp 1.2.3 Bài toán động học thuận của Robot 5 khâu phẳng Hnh 1.5 a. Hình vẽ và bảng thông số động học Denavit- Hartanberg Bảng thông số: Trục θi di ai αi 1 2 3 4 5 q1 q2 q3 q4 q5 0 0 0 0 0 a1 a2 a3 a4 a5 0 0 0 0 0 b. Phương trình xác định vị trí bàn kẹp của Robot: Từ biểu thức (1.2) ta xác định được ma trận : Ở đây ta sử dụng kí hiệu: Từ đó ta có được công thức tính vị trí bàn kẹp trong hệ tọa độ cố định: c. Dữ liệu đầu vào cho Robot 5 khâu phẳng Chọn quy luật chuyển động của các biến khớp như sau theo thời gian như sau: . Và các khoảng cách :. Thay vào chương trình tính toán ta được kết quả như sau: -Vị trí: Đồ thị tọa độ bàn kẹp Đồ thị tọa độ bàn kẹp - Vận tốc: Đồ thị vận tốc bàn kẹp Đồ thị vận tốc bàn kẹp - Gia tốc: Đồ thị gia tốc bàn kẹp Đồ thị gia tốc bàn kẹp Quỹ đạo bàn kẹp 1.2.4 Bài toán động học thuận của Robot PPR phẳng a. Hình vẽ và bảng thông số động học Denavit-Hartenberg Hình 1.6 Trục θi di ai αi 1 2 3 Pi/2 Pi/2 q3 q1 q2 0 0 0 a3 Pi/2 Pi/2 0 b. Vị trí bàn kẹp: Từ biểu thức (1.2) ta có thể xác định ma trận chuyển: Từ đó: c. Dữ liệu đầu vào cho Robot PPR phẳng Chọn quy luật chuyển động của các biến khớp theo thời gian như sau: Và khoảng cách: . Thay vào chương trình ta được: - Vị trí: Đồ thị tọa độ bàn kẹp Đồ thị tọa độ bàn kẹp - Vận tốc: Đồ thị vận tốc Đồ thị vận tốc - Gia tốc: Đồ thị gia tốc Đồ thị gia tốc Quỹ đạo bàn kẹp 1.2.5 Bài toán động học thuận của Robot RRRP1 a. Hình vẽ và bảng thông số động học Denavit-Hartenberg Hình 1.7 Bảng thông số: Trục θi di ai αi 1 2 3 4 q1 q2 q3 0 0 0 0 q4 a1 a2 a3 0 0 0 Pi 0 b. Phương trình xác định vị trí bàn kẹp của Robot Ở đây ta sử dụng kí hiệu: Vị trí bàn kẹp trong hệ tọa độ cố định là: c. Dữ liệu đầu vào cho rôbôt RRRP1 Ta chọn quy luật chuyển động của các biến khớp theo thời gian t như sau: , , , . Và các khoảng cách : , , Thay vào phương trình ta tính được kết quả như sau: - Vị trí: Đồ thị tọa độ bàn kẹp Đồ thị tọa độ bàn kẹp Đồ thị tọa độ bàn kẹp - Vận tốc: Đồ thị vận tốc bàn kẹp Đồ thị vận tốc bàn kẹp Đồ thị vận tốc bàn kẹp -Gia tốc: Đồ thị gia tốc bàn kẹp Đồ thị gia tốc bàn kẹp Quỹ đạo bàn kẹp 1.2.6 Bài toán động học thuận Robot RRRP2 a. Hình vẽ và bảng thông số động học Denavit-Hartenberg Hình 1.8 Bảng thông số: Trục θi di ai αi 1 2 3 4 q1 q2 q3 0 d1 0 0 q4 0 a2 0 0 Pi/2 0 -Pi/2 0 b. Phương trình xác định vị trí bàn kẹp Ở đây ta sử dụng kí hiệu: c. Dữ liệu đầu vào cho Rôbôt RRRP2 Ta chọn quy luật chuyển động cho các biến khớp theo thời gin như sau: . Và các khoảng cách , : . Thay vào chương trình tính toán ta được kết quả như sau: - Vị trí: Đồ thị tọa độ bàn kẹp Đồ thị tọa độ bàn kẹp Đồ thị tọa độ bàn kẹp - Vận tốc: Đồ thị vận tốc bàn kẹp Đồ thị vận tốc bàn kẹp Đồ thị vận tốc bàn kẹp - Gia tốc: Đồ thị gia tốc bàn kẹp Đồ thị gia tốc bàn kẹp Đồ thị gia tốc bàn kẹp - Vận tốc góc: Đồ thị vận tốc góc bàn kẹp Đồ thị vận tốc góc bàn kẹp Quỹ đạo bàn kẹp Đồ thị vận tốc góc bàn kẹp - Gia tốc góc: Đồ thị gia tốc góc bàn kẹp Đồ thị gia tốc góc bàn kẹp CHƯƠNG 2 NGHỊCH ĐẢO MA TRẬN CHỮ NHẬT MA TRẬN TỰA NGHỊCH ĐẢO Khi xét bài toán động học thuận ta có quan hệ như sau: (2.1) Có thể tìm q về mặt hình thức dưới dạng: (2.2) Đạo hàm hai vế của phương trình (2.1) ta được: (2.3) Trong đó là ma trận Jacobi cỡ Đối với các Robot dư dẫn động có số lượng các tọa độ khớp lớn hơn số lượng các tọa độ không gian của khâu thao tác, do đó ma trận Jacobi là ma trận chữ nhật. Để giải quyết bài toán động học ngược của Robot dư dẫn động ta cần giải bài toán nghịch đảo ma trận Jacobi, từ đó chúng ta đưa ra định nghĩa ma trận tựa nghịch đảo. 2.1 ĐỊNH NGHĨA MA TRẬN TỰA NGHỊCH ĐẢO Cho A là ma trận chữ nhật cỡ , , X là ma trận cỡ , . Các phương trình sau đây được sử dụng để định nghĩa ma trận nghịch đảo suy rộng, ma trận nghịch đảo suy rộng phản chiều và ma trận tựa nghịch đảo của ma trận A. (2.4) (2.5) (2.6) (2.7) Các phương trình (2.4) đến (2.7) được gọi là các điều kiện Penrose. Định nghĩa 1: Ma trận nghịch đảo suy rộng (Generalized Inverse) của ma trận là ma trận thoả mãn phương trình (2.4). Ta ký hiệu ma trận nghịch đảo suy rộng . Định nghĩa 2. Ma trận nghịch đảo suy rộng phản chiếu (Reflexive Generalized Inverse) của ma trận là ma trận thoả mãn các phương trình (2.4) và (2.5). Ký hiệu là . Định nghĩa 3. Ma trận tựa nghịch đảo (Pseudoinverse) của ma trận là ma trận thoả mãn bốn phương trình từ (2.4) đến (2.7). Ta ký hiệu là hoặc . Chú ý: Ma trận tựa nghịch đảo còn gọi là ma trận Moore- Penrose để kỷ niệm hai nhà bác học đã nghiên cứu loại ma trận này. Để giải quyết bài toán tìm ma trận nghịch đảo của ma trận chữ nhật ở chương này ta trình bày ba phương pháp tính ma trận tựa nghịch đảo: Phương pháp Moore-penrose Phương pháp Greville Phương pháp Khalil 2.2 PHƯƠNG PHÁP MOORE- PENROSE 2.2.1 Cơ sở lý thuyết Định lý 1: Tính ma trận nghịch đảo của ma trận A Cho A là ma trận cỡ . A+ là ma trận tựa nghịch đảo cỡ của A. Nếu A có hạng đầy đủ thì A+ được xác định như sau: (2.8) Như vậy ta có thể xây dựng thuật toán cho phương pháp này như sau: b1. Nhập vào ma trận A. b2. So sánh số hàng và số cột của ma trận A (tức là so sánh m và n ) rồi dựa vào công thức (2.1) để tính ma trận tựa nghịch đảo Chú ý: ma trận A có hạng đầy đủ tức là Rank( A)= m nếu Rank( A)= n nếu 2.2.2 Sơ đồ khối của phương pháp Moore- Penrose START Nhập vào So sánh m,n return() m<n m>n m=n END Dựa trên lý thuyết về nghịch đảo ma trận của Moore-Penrose và sơ đồ khối ta xây dựng thủ tục MP_Inverse() để tính ma trận tựa nghịch đảo của , mã nguồn được viết bằng phần mềm Maple 9.0, trình bày ở phần phụ lục. Phương pháp Moore – Penrose là phương pháp khá nổi tiếng tìm ma trận tựa nghịch đảo của ma trận chữ nhật. Tuy nhiên phương pháp này chỉ áp dụng được khi hạng của ma trận A là đầy đủ. Trong trường hợp hạng của ma trận A không đầy đủ, để tìm được ma trận tựa nghịch đảo của ma trận A ta sử dụng phương pháp khác đó là phương pháp Greville. 2.3 PHƯƠNG PHÁP GREVILLE 2.3.1 Cơ sở lý thuyết Phương pháp Greville cho phép tính toán ma trận tựa nghịch đảo A+(AÎCmxn ) bằng phương pháp lặp. Để xác định ma trận tựa nghịch đảo theo phương pháp này ta đưa vào một số ký hiệu sau: Gọi ma trận là phần của ma trận A. Trong đó là cột thứ k của A, cho k=1,..,n. Vector và được định nghĩa bởi: (2.9) (2.10) Định lý 2.2: Cho ma trận AÎCmxn . Nghịch đảo Greville của Ak(k=2..n) được xác định theo công thức sau: (2.11) trong đó: (2.12) (2.13) Chú ý: nếu cả hai và , khi đó và . Như vậy dựa trên cơ sở lý thuyết của phương pháp nghịch đảo ma trận Greville, ta có thể xây dựng thuật giải cho phương pháp này như sau: b1. Nhập ma trận A. b2. Cho k=2 khi đó ta tính được theo công thức: Sau đó ta tính được và theo (2.9) và (2.10) từ đó theo công thức (2.11) của định lý 2.2 ta tính được . b3. Cho cho đến khi thì ta thu được 2.3.2 Sơ đồ khối phương pháp Greville return( Nhập vào: i:=i+1 false END START true false true Dựa trên lý thuyết về ma trận nghịch đảo của Greville và sơ đồ khối trên ta xây dựng thủ tục GRE_Inverse() để tính ma trận tựa nghịch đảo của , mã nguồn được viết trên phần mềm Maple 9.0 trình bày ở phần phụ lục. 2.4 PHƯƠNG PHÁP KHALIL 2.4.1 Cơ sở lý thuyết Đối với phương pháp Khalil ta chỉ xét các ma trận chữ nhật A cỡ () và có hạng là m. Theo lý thuyết ma trận (F.R. Gantmacher: Matrazentheory, Springer Verlag, Berlin 1986), ta có thể phân tích A thành tích của hai ma trận như sau: (2.14) Ta có thể viết lại ma trận C như sau: (2.15) Trong đó: (2.16) : Ma trận đơn vị cấp m : cột thứ k của A : ma trận cỡ Do J là ma trận hạng m, C là ma trận có hạng m và bằng hạng của A nên ta có công thức xác định tựa nghịch đảo: (2.17) Trong đó C+ được xác định theo công thức: (2.18) (2.19) Với là ma trận 0 (). - Như vậy thuật toán của phương pháp này được đưa ra như sau: b1. Nhập vào ma trận A. b2. Phân tích A thành tích 2 ma trận và ma trận C với ma trận được lấy từ m cột đầu của ma trận A (là ma trận vuông).Ta tiến hành kiểm tra nếu thì dừng lại và thoát khỏi chương trình còn nếu thì tiếp tục chương trình. Ma trận C được tính theo công thức (2.15) và (2.16). b3. Sử dụng công thức để tính ma trận tựa nghịch đảo với ma trận được tính theo (2.18) hoặc (2.19). - Dựa trên lý thuyết về nghịch đảo ma trận theo phương pháp phân tích cân xứng và sơ đồ khối ta xây dựng thủ tục KHALIL_Inverse() tính ma trận tựa nghịch đảo của , trình bày ở cuối phụ lục. Return(A+) START END Nhập vào false true true false 2.4.2 Sơ đồ khối phương pháp Khalil 2.5 MỘT SỐ VÍ DỤ CỤ THỂ Ví dụ 1: Giải hệ hai phương trình 3 ẩn sau Ta nhận thấy: ; Sử dụng các thủ tục MP_Inverse() của phương pháp Moore-Penrose, thủ tục GRE_Inverse() của phương pháp Greville đều cho ta kết quả ma trận tựa nghịch đảo của là được tính như sau: Vậy nghiệm của hệ là: Nếu sử dụng thủ tục KHALIL_Inverse() của phương pháp Khalil thì kết quả ma trận tựa nghịch đảo là: Suy ra nghiệm như sau: Ví dụ 2: giải hệ 3 phương trình 5 ẩn sau Ta có: ; Sử dụng các thủ tục MP_Inverse() của phương pháp Moore-Penrose, thủ tục GRE_Inverse() của phương pháp Greville, cho ta kết quả ma trận tựa nghịch đảo của là được tính như sau: Nghiệm của hệ phương trình là: Nếu sử dụng thủ tục KHALIL_Inverse() tính được 2.6 SO SÁNH CÁC PHƯƠNG PHÁP XÁC ĐỊNH TỰA NGHỊCH ĐẢO - Đối với phương pháp Moore- Penrose: có thuật toán đơn giản nhất trong 3 phương pháp cho phép ta có thể xác định ma trận tựa nghịch đảo của ma trận A cỡ nhưng yêu cầu là hạng của ma trận A phải đầy đủ. Nếu A có hạng thì phương pháp này không xác định được ma trận tựa nghịch đảo. - Đối với phương pháp Greville: có thể tính toán tựa nghịch đảo ngay cả khi ma trận A có hạng không đầy đủ (tức là ). Trong trường hợp hạng của ma trận A là đầy đủ thì phương pháp này có cùng kết quả với phương pháp Moore- Penrose. - Đối với phương pháp Khalil: cho phép xác định ma trận tựa nghịch đảo của ma trận A cỡ với và hạng của ma trận A phải đầy đủ , nếu không thì phương pháp này cũng không thể áp dụng. Kết quả của phương pháp này cho ta hàng đầu của ma trận giống với 2 phương pháp trên, còn hàng sau thì có khác so với 2 phương pháp trên. CHƯƠNG 3 TÍNH TOÁN ĐỘNG HỌC NGƯỢC ROBOT DƯ DẪN ĐỘNG BẰNG PHƯƠNG PHÁP MA TRẬN JACOBI Để giải quyết bài toán động học ngược của Robot dư dẫn động, chúng ta có khá nhiều phương pháp . Trong chương này chúng ta chỉ tập trung nghiên cứu 2 phương pháp: phương pháp Khalil và phương pháp Reduced Gradient (RG). Mỗi phương pháp trên sử dụng các hàm tối ưu hoá khác nhau để đưa ra những ràng buộc đối với các tọa độ khớp dư nhằm giải quyết bài toán động học ngược của Robot dư dẫn động. 3.1 GIẢI PHÁP ĐƯA RA CHO MÔ HÌNH ĐỘNG HỌC NGƯỢC Giả sử là các tọa độ khớp, là các tọa độ xác định vị trí của khâu thao tác (bàn kẹp). Theo bài toán động học thuận ta có quan hệ như sau: (3.1) Đối với các Robot dư dẫn động ta có , khi đó số tọa độ trạng thái lớn hơn tọa độ của bộ phận thao tác. Bậc tự do dư để thoả mãn các yêu cầu sau: Tránh các chướng ngại vật Tránh cấu hình kỳ dị động học Giữ biến khớp trong giới hạn vật lý của chúng Tạo ra sự khéo léo cho Robot Giới hạn mô men khớp, cực tiểu động lực… Từ hệ thức (3.1) ta có biến đổi một cách hình thức: Đạo hàm hai vế của phương trình (3.1) theo q ta được: (3.2) Trong đó là ma trận Jacobi cỡ (3.3) Từ (3.2) suy ra: Do đó ta có: ở đây: Đặt: Từ đó ta có: (3.4) Trong đó: là ma trận tựa nghịch đảo của J là ma trận đơn vị cấp n z là một vector khớp bất kỳ được xác định bằng cách tối ưu hoá hoặc chọn một cách thích hợp. Để thực hiện ta đưa vào hàm , khi đó z được đưa ra trong [9,15]: ở đó: : Gradient của : hằng số dương nếu là hàm cực đại, hằng số âm nếu là cực tiểu. Sau khi thiết lập được công thức (3.4) ta sử dụng phương pháp số để tính toán vị trí, vận tốc, gia tốc các khâu ở từng thời điểm. Ở đây chúng ta sử dụng công thức sai phân hữu hạn: (3.5) Từ đó ta có: (3.6) (3.7) Thay (3.6), (3.7) vào (3.4) ta tìm được vị trí của các khâu tại thời điểm thứ k: (3.8) Hay (3.9) Việc xác định điều kiện đầu cho các toạ độ : Khi cho biết vị trí ban đầu của bàn kẹp ta có m vị trí , do đó ta phải cho trước vị trí ban đầu sau đó ta sử dụng phương trình xác định vị trí bàn kẹp để xác định m vị trí ban đầu còn lại. Vận tốc các khâu tại thời điểm thứ k được tính theo công thức (3.6). Để tính gia tốc các khâu tại thời điểm thứ k ta cũng sử dụng công thức sai phân hữu hạn: (3.10) Như vậy, một cách giải quyết đơn giản bài toán động học ngược Robot dư dẫn động là chúng ta chỉ cần chọn hàm z thích hợp và sử dụng các phương pháp được đưa ra ở chương hai để tính ma trận tựa nghịch đảo của ma trận Jacobi là J+ sau đó thay vào công thức (3.4). Tuy nhiên các phương pháp đó cho ta kết quả chưa thật sự chính xác. Dưới đây chúng ta sẽ nêu ra 2 phương pháp tính trực tiếp để giải quyết bài toán mà không cần phải tính J+. 3.2 PHƯƠNG PHÁP KHALIL GIẢI BÀI TOÁN ĐỘNG HỌC NGƯỢC ROBOT DƯ DẪN ĐỘNG 3.2.1 Phân tích ma trận Jacobi Xét ma trận Jacobi J kích thước có hạng là m. Khi đó ma trận J có thể được phân tích thành hai ma trận có hạng đầy đủ như sau: (3.11) trong đó: : ma trận lấy từ ma trận J C : ma trận Không làm giảm tính tổng quát, chúng ta giả sử rằng ma trận lấy từ m cột trái của ma trận J. Thưc tế để chính xác, chúng ta chọn ma trận Jm sao cho định thức lớn nhất trong các trị số đã có, m khớp đầu tiên gọi là các khớp chính và các khớp còn lại gọi là khớp phụ. Từ (3.11) theo tài liệu [10]: (3.12) Với: (313) : ma trận đơn vị cỡ : cột thứ k của ma trận J : ma trận cỡ Theo phương pháp này hạng của hai ma trận phân tích là đầy đủ dẫn đến biểu thức tính ma trận tựa nghịch đảo: (3.14) Với C+ được xác định theo công thức (trình bày ở chương 2): Hoặc (3.15) Theo công thức (3.2) ta có biến đổi sau: (3.16) Nếu ta đặt (3.17) Thì phương pháp giải các khớp cơ bản như sau: (3.18) Khi đó (3.17) tìm các khớp còn lại để thoả mãn tiêu chuẩn cực tiểu. Chú ý: Có một vấn đề phát sinh là khi ta lấy m cột trái của ma trận J rồi sau đó tính nghịch đảo ma trận . Trong một số bài toán mà ma trận là ma trận suy biến tức là không thể tiến hành nghịch đảo được ma trận đó. Một cách giải quyết được đưa ra là ta sẽ đảo vị trí các cột của ma trận J, thuật toán đổi chỗ các cột của ma trận J được tiến hành như sau: 1. Lấy m cột trái của ma trận J thành lập ma trận 2. Kiểm tra nếu thì ta tiến hành đổi chỗ cột 1 với cột i (i=m+1..n) của ma trận J sau đó ta quay về bước 1. Công việc kiểm tra dừng lại khi hoặc . Đồng thời trong quá trình đổi chỗ sẽ ghi lại việc đổi chỗ giữa cột 1 với cột bao nhiêu từ đó tương ứng với việc đổi chỗ vị trí giữa bậc tự do q. Thủ tục đổi chỗ giữa các cột của ma trận J được trình bày ở phần phụ lục. 3.2.2 Giải quyết theo tựa nghịch đảo Để tìm vector ta giải quyết như sau: Tìm và sau đó sử dụng (3.17), (3.18) (3.19) Khi tính toán có thể cân nhắc tới vấn đề tối ưu: Tối ưu sử dụng để giải quyết phần vận tốc khớp cơ bản , chúng ta cần cực tiểu nó. Ta có: (3.20) Trong đó (3.21) Ta biến đổi: (3.22) Với: : m thành phần đầu của : (n-m) thành phần sau của Sử dụng (3.21) và đã được xác định theo (3.17) chúng ta có thể tính theo tự do. Chúng ta cần cực tiểu chuẩn: (3.23) Đạo hàm (3.23) theo : Dựa vào công thức đạo hàm vector: ta có (3.24) (3.25) Thay vào biểu thức đạo hàm của (3.23): (3.26) Chuyển vị 2 vế của (3.26) ta được: (3.27) Từ đó: (3.28) Với ma trận hoàn toàn xác định và (3.28) chính là công thức giải quyết tối ưu của . Như vậy các bước giải quyết bài toán như sau: b1: tính sử dụng công thức (3.17) b2: tính sử dụng công thức (3.28) b3: tính sử dụng công thức (3.22) Dựa vào các bước giải bài toán động học ngược như trên ta xây dựng thủ tục INV_KHALIL() để tính toán vị trí và vận tốc các khâu của Robot dư dẫn động. Thủ tục được trình bày trong phần phụ lục. 3.3 PHƯƠNG PHÁP REDUCED GRADIENT (RG) GIẢI BÀI TOÁN ĐỘNG HỌC NGƯỢC RÔBỐT DƯ DẪN ĐỘNG Trong đồ án này ta xét ma trận Jacobi là ma trận chữ nhật có kích thước . Giả sử rằng hạng của ma trận Jacobi J bằng số hạng của ma trận này Từ công thức ta biến đổi như sau: (3.29) Trong đó: (3.30) Công thức (3.35) được viết lại như sau: (3.31) , với và với Từ đó ta có: (3.32) trong đó là ma trận vuông không suy biến. Tọa độ khớp q được chia làm hai phần , tương ứng với biến cơ bản và biến độc lập. Do vậy, được xác định theo hàm ngược hình thức trình bày trong [9]. Biểu thức này được xuất phát từ định lý hàm ẩn theo hướng động học. Từ đó, khi vận tốc chọn tuỳ ý, vận tốc khớp cơ bản tính theo công thức sau: (3.33) Việc chọn có thể xác định theo hàm chuẩn , trong trường hợp này có thể biến đổi thành hàm . Cụ thể, vector vận tốc sẽ được chọn để tối ưu hàm . Do vậy ta đạo hàm hàm theo thời gian: (3.34) sau đó vector vận tốc độc lập sẽ được cho bởi: (3.35) trong đó : : hàm tiêu chuẩn, trong đồ án này ta chọn hàm như sau: , trong đó B là ma trận khối lượng quán tính được chọn tuỳ ý có dạng ma trận đường chéo kích thước : (3.36) : gọi là bước điều chỉnh. Gradient của hàm theo q Đặt , giải pháp toàn bộ không gian khớp là: (3.37) Phương trình (3.37) là cách giải bài toán động học dư dẫn động gọi là phương pháp Reduced Gradient (RG). Chú ý: Trong trường hợp ma trận có định thức bằng 0 thì cách giải quyết cũng được đưa ra như cách giải quyết đã được trình bày trong phương pháp Khalil. Dựa trên lý thuyết về phương pháp Reduced- Gradient, chúng ta xây dựng thủ tục INV_RG() để giải quyết bài toán động học ngược của Robot dư dẫn động (được trình bày ở phần phụ lục) . 3.4 ỨNG DỤNG HAI PHƯƠNG PHÁP TRÊN TÍNH TOÁN CHO CÁC ROBOT DƯ DẪN ĐỘNG CỤ THỂ 3.4.1 Robot 3 trục phẳng a, Hình vẽ và bảng thông số Denavit- Hartenberg Hình 1.3 Trục θi di ai αi 1 2 3 q1 q2 q3 0 0 0 a1 a2 a3 0 0 0 b, File dữ liệu đầu vào cho bài toán động học ngược: - Thông số DH: khau theta d a alpha 1 q1 0 20 0 2 q2 0 20 0 3 q3 0 20 0 - Cấu hình ban đầu: dkdau:=q1(0)=Pi/6,q2(0)=1.8803,q3(0)=-1.2164; - Quỹ đạo bàn kẹp: QuyDao:=[10+6*sin(t),36+6*cos(t)]; - Bước thời gian: buoc:=0.05; - Thời gian vẽ đồ thị: thoigian:=7; - Bước điều chỉnh: alpha:=0.00001; - Ma trận quán tính: c, Kết quả bài toán động học ngược: Sử dụng phương pháp Khalil giải quyết theo tựa nghịch đảo: Đồ thị tọa độ, vận tốc, gia tốc khâu 1 Đồ thị tọa độ, vận tốc, gia tốc khâu 2 Đồ thị tọa độ, vận tốc, gia tốc khâu 3 Sử dụng phương pháp Reduced- Gradient Đồ thị tọa độ, vận tốc, gia tốc khâu1 Đồ thị tọa độ, vận tốc, gia tốc khâu 2 Đồ thị tọa độ, vận tốc, gia tốc khâu 3 3.4.2 Robott 4 trục phẳng a, Hình vẽ và bảng thông số Denavit- Hartenberg Hình 1.4 Trục θi di ai αi 1 2 3 4 q1 q2 q3 q4 0 0 0 0 a1 a2 a3 a4 0 0 0 0 b, Dữ liệu đầu vào cho bài toán động học ngược Robot - Thông số DH: khau theta d a alpha 1 q1 0 15 0 2 q2 0 15 0 3 q3 0 15 0 4 q4 0 15 0 - Cấu hình ban đầu: dkdau:=q1(0)=Pi/4,q2(0)=Pi/4,q3(0)=7.33712,q4(0)=-7.67822; - Quỹ đạo bàn kẹp: QuyDao:=[3+6*sin(t),42+6*cos(t)]; - Bước thời gian: buoc:=0.05; - Thời gian vẽ đồ thị: thoigian:=6; - Bước điều chỉnh: alpha:=0.00001; - Ma trận KL quán tính: c, Kết quả: Sử dụng phương pháp Khalil giải quyết theo tựa nghịch đảo Đồ thị tọa độ, vận tốc, gia tốc khâu1 Đồ thị tọa độ, vận tốc, gia tốc khâu 2 Đồ thị tọa độ, vận tốc, gia tốc khâu 3 Đồ thị tọa độ, vận tốc, gia tốc khâu 4 Sử dụng phương pháp Reduced- Gradient Đồ thị tọa độ, vận tốc, gia tốc khâu1 Đồ thị tọa độ, vận tốc, gia tốc khâu 2 Đồ thị tọa độ, vận tốc, gia tốc khâu 3 Đồ thị tọa độ, vận tốc, gia tốc khâu 4 3.4.3 Robot 5 khâu phẳng Hnh 1.5 a, Hình vẽ và bảng thông số DH Trục θi di ai αi 1 2 3 4 5 q1 q2 q3 q4 q5 0 0 0 0 0 a1 a2 a3 a4 a5 0 0 0 0 0 b, Dữ liệu đầu vào giải bài toán động học ngược Robot 5 trục phẳng - Thông số DH: khau theta d a alpha 1 q1 0 10 0 2 q2 0 10 0 3 q3 0 10 0 4 q4 0 10 0 5 q5 0 10 0 - Cấu hình ban đầu: dkdau:=q1(0)=Pi/4,q2(0)=Pi/6,q3(0)=-Pi/3,q4(0)=6.59406,q5(0)=-8.196947; - Quỹ đạo bàn kẹp: QuyDao:=[30-2*t,15+2*t]; - Bước thời gian: buoc:=0.05; - Thời gian vẽ đồ thị: thoigian:=6; - Bước điều chỉnh: alpha:=0.00001; - Ma trận KL quán tính: c, Kết quả Sử dụng phương pháp Khalil Đồ thị tọa độ, vận tốc, gia tốc khâu1 Đồ thị tọa độ, vận tốc, gia tốc khâu 2 Đồ thị tọa độ, vận tốc, gia tốc khâu 3 Đồ thị tọa độ, vận tốc, gia tốc khâu 4 Đồ thị tọa độ, vận tốc, gia tốc khâu 5 Sử dụng phương pháp Reduced- Gradient Đồ thị tọa độ, vận tốc, gia tốc khâu 1 Đồ thị tọa độ, vận tốc, gia tốc khâu 2 Đồ thị tọa độ, vận tốc, gia tốc khâu 3 Đồ thị tọa độ, vận tốc, gia tốc khâu 4 Đồ thị tọa độ, vận tốc, gia tốc khâu 5 3.4.3 Robot PPR phẳng a, Hình vẽ và bảng thông số DH Trục θi di ai αi 1 2 3 Pi/2 Pi/2 q3 q1 q2 0 0 0 a3 Pi/2 Pi/2 0 b, File dữ liệu đầu vào cho bài toán động học ngược Robot: - Thông số DH: khau theta d a alpha 1 Pi/2 q1 0 Pi/2 2 Pi/2 q2 0 Pi/2 3 q3 0 20 0 - Cấu hình ban đầu: dkdau:=q1(0)=5,q2(0)=6.795,q3(0)=Pi/3; - Quỹ đạo bàn kẹp: QuyDao:=[24+4*sin(t),6+9*cos(t)]; - Bước thời gian: buoc:=0.05; - Thời gian vẽ đồ thị: thoigian:=6; - Bước điều chỉnh: alpha:=0.02; - Ma trận KL quán tính: c, Kết quả: Sử dụng phương pháp Khalil: Đồ thị tọa độ, vận tốc, gia tốc khâu 1 Đồ thị tọa độ, vận tốc, gia tốc khâu 2 Đồ thị tọa độ, vận tốc, gia tốc khâu 3 Sử dụng phương pháp Reduced- Gradient: Đồ thị tọa độ, vận tốc, gia tốc khâu 1 Đồ thị tọa độ, vận tốc, gia tốc khâu 2 Đồ thị tọa độ, vận tốc, gia tốc khâu 3 3.4.4 Robot RRRP1 a, Hình vẽ và bảng thông số DH Trục θi di ai αi 1 2 3 4 q1 q2 q3 0 0 0 0 q4 a1 a2 a3 0 0 0 Pi 0 b, File dữ liệu đầu vào cho bài toán động học ngược: - Thông số DH: khau theta d a alpha 1 q1 0 10 0 2 q2 0 10 0 3 q3 0 10 Pi 4 0 q4 0 0 - Cấu hình ban đầu: dkdau:=q1(0)=0,q2(0)=1.047195,q3(0)=1.04717,q4(0)=Pi/6; - Quỹ đạo bàn kẹp: ._.