Đề tài 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

ọ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: QuyDao:=[5*sin(2*t),15+5*cos(2*t),t]; - Bước thời gian: buoc:=0.05; - Thời gian vẽ đồ thị: thoigian:=4; - Bước điều chỉnh: alpha:=0.01; - Ma trận KL 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: Đồ 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 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â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 3.4.5 Robot RRRP2 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 d1 0 0 q4 0 a2 0 0 Pi/2 0 -Pi/2 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 20 0 Pi/2 2 q2 0 20 0 3 q3 0 0 -Pi/2 4 0 q4 0 0 - Cấu hình ban đầu: dkdau:=q1(0)=Pi/6,q2(0)=1.22026,q3(0)=-1.39333,q4(0)=-5; - Quỹ đạo bàn kẹp: QuyDao:=[5+4*sin(t),-1+4*cos(t),34+t/5]; - Bước thời gian: buoc:=0.05; - Thời gian vẽ đồ thị: thoigian:=4; - Bước điều chỉnh: alpha:=0.01; - Ma trận KL 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: Đồ 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 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 3.5 SO SÁNH PHƯƠNG PHÁP KHALIL VÀ PHƯƠNG PHÁP REDUCED - GRADIENT - Do sử dụng các tiêu chuẩn tối ưu khác nhau để đưa ra những ràng buộc khác nhau đối với các tọa độ khớp dư của bài toán động học ngược Robot dư dẫn động nên mỗi phương pháp cho ta kết quả khác nhau đối với quy luật chuyển động của mỗi khâu, tuy nhiên cả hai phương pháp đều đưa lại cho chúng ta quỹ đạo bàn kẹp là như nhau. - Trong 2 phương pháp thì khối lượng tính toán của phương pháp Reduced-Gradient ít hơn so với phương pháp Khalil hay thuật toán phương pháp Khalil phức tạp hơn phương pháp Reduced- Gradient. - Ở phương pháp Khalil do chúng ta sử dụng hàm tối ưu đối với toàn bộ vận tốc các khớp, còn phương pháp Reduced- Gradient thì chỉ tối ưu hàm H’ đối với vector vận tốc cho nên ta tìm được quy luật chuyển động của các khâu của phương pháp Khalil hợp lý hơn. 3.6 GIỚI THIỆU VỀ MAPLE VÀ CÁCH SỬ DỤNG MAPLE ĐỂ GIẢI BÀI TOÁN ĐỘNG HỌC ROBOT DƯ DẪN ĐỘNG 3.6.1 Giới thiệu về phần mềm Maple Maple là một phần mềm tính toán rất mạnh, ngày càng thân thiện với người sử dụng được xây dựng ở trường đại học Waterloo(Canada) từ năm 1990 hiện đang được dùng rất phổ biến ở các trường đại học và trung học trên thế giới. Maple là một môi trường tính toán số, chữ và các ứng dụng đồ họa của toán học. Do đó việc sử dụng nó không đòi hỏi người dùng phải có kỹ năng lập trình giỏi (như đòi hỏi ở một số ngôn ngữ lập trình C++, visual C++,..) mà chỉ yêu cầu họ nắm vững các kiến thức lý thuyết cơ bản. Tuy nhiên Maple cũng là một ngôn ngữ lập trình bậc cao rất mạnh và cũng dễ tiếp cận. Về lập trình tính toán nó vượt xa các ngôn ngữ lập trính thông thường khác trên cả hai phương diện: mạnh và đơn giản, bởi vì mỗi hàm của nó tương đương với cả một gói chương trình con. Maple có thể giải quyết rất nhiều vấn đề của toán học như đại số ma trận, vector, giải hệ phương trình phi tuyến tính, hệ phương trình vi phân, tích phân…cho phép chúng ta có thể giải quyết bài toán một cách cực kỳ linh hoạt và phong phú. Cửa sổ làm việc chính của maple 9.0 Từ dấu nhắc lệnh ta có thể gõ bất kỳ lệnh nào của Maple 9.0 và sẽ nhận được kết quả một cách nhanh chóng. Trong Maple có hàng ngàn lệnh, tên các lệnh này mang tính rất dễ nhớ. Do đó ta không phải nhớ hàng ngàn câu lệnh đó mà chỉ cần nhấn phím F1 hoặc sử dụng ?command để tìm hiểu cú pháp từng câu lệnh. 3.6.2 Sử dụng Maple 9.0 để giải bài toán động học Robot dư dẫn động 3.6.2.1 Tính toán động học thuận cho Robot dư dẫn động Chương trình được trình bày ở phần phụ lục. Trong chương trình chúng ta sủ dụng các thủ tục sau: - BangDH(“duong dan”): nhập thông số Denavit- Hartenberg với tham số truyền vào là đường dẫn tới File chứa thông số Robot cần tính. - InTranDH(A): tính ma trận chuyển DH với tham số nhập vào ma trận chứa thông số DH - vitri(T): tính vị trí bàn kẹp với tham số truyền vào là ma trận chuyển DH T. - vantoc(p): tính vận tốc bàn kẹp với tham số truyền vào là vị trí bàn kẹp p. - giatoc(v): tính gia tốc bàn kẹp với tham số truyền vào là vận tốc bàn kẹp v. - vantocgoc(T): tính vận tốc góc bàn kẹp với tham số truyền vào là ma trận chuyển DH - giatocgoc(vg): tính gia tốc góc bàn kẹp với tham số truyền vào là vận tốc góc bàn kẹp - quydao(p): vẽ quỹ đạo chuyển động của bàn kẹp với tham số truyền vào là vi trí bàn kẹp 3.6.2.2 Chương trình tính toán động học ngược Robot dư dẫn động Chương trình chính là thủ tục hàm main( ) vời tham số truyền vào là tên phương pháp được sử dụng để tính toán động học ngược Robot dư dẫn động. Trong đó ta sử dụng các thủ tục sau: - JCB(p,q): tính ma trận Jacobi với tham số truyền vào là vị trí bàn kẹp p và ma trận tọa độ khớp q. - INV_Khalil(J,P,delta,t): giải bài toán động học ngược bằng phương pháp Khalil với tham số truyền vào là ma trận Jacobi J, quỹ đạo chuyển động của bàn kẹp P, bước và khoảng thời gian tính toán. - INV_RG(J,P,alpha,B,delta,t): giải bài toán động học ngược bằng phương pháp Reduced- Gradient với tham số truyền vào gồm có: ma trận Jacobi, quỹ đạo bàn kẹp, bước điều chỉnh, ma trận khối lượng quán tính, bước và khoảng thời gian tính toán. 2.3 Các thủ tục tính ma trận tựa ngịch đảo - MP_Inverse(A): đưa ra kết qủa ma trận tựa nghịch đảo của ma trận A bằng phương pháp Moore- Penrose, tham số truyền vào chính là ma trận A. - GRE_Inverse(A): đưa ra kết qủa ma trận tựa nghịch đảo của ma trận A bằng phương pháp Greville, tham số truyền vào chính là ma trận A. - KHALIL_Inverse(A): đưa ra kết qủa ma trận tựa nghịch đảo của ma trận A bằng phương pháp Khalil, tham số truyền vào chính là ma trận A. KẾT LUẬN Tại sao việc sử dụng Robot dư dẫn động vào trong thực tế sản xuất công nghiệp và các lĩnh vực khác ngày càng lan rộng và tăng nhanh. Điều đó được bắt nguồn từ khả năng khéo léo, linh hoạt của chúng, khả năng tránh cấu hình kỳ dị, tránh chướng ngại vật, giới hạn mômen khớp, cực tiểu động học…. Nhiều nhà khoa học bắt đầu chú ý nhiều đến lĩnh vực khoa học này . Trong đồ án bước đầu tìm hiểu về vấn đề này. * Những vấn đề đã được trình bày trong đồ án này: + Khái quát về ma trận Denavit- Hartenberg và cách thiết lập ma trận Denavit- Hartenberg là cơ sở xây dựng và viết chương trình tính toán động học thuận của Robot dư dẫn động về vị trí, vận tốc, gia tốc, vận tốc góc, gia tốc góc và vẽ được quỹ đạo chuyển động của bàn kẹp. Đồng thời nó cũng là đầu vào cho bài toán động học ngược . + Đưa ra định nghĩa ma trận tựa nghịch đảo và trình bày ba phương pháp xác định ma trận tựa nghịch đảo của ma trận bất kỳ. Nêu lên ứng dụng của các phương pháp đó trong việc giải hệ phương trình đại số tuyến tính từ đó so sánh sự khác nhau của các phương pháp cũng như đánh giá được ưu nhược điểm của từng phương pháp. + Giải pháp chung cho bài toán động học ngược Robot dư dẫn động và trình bày cụ thể hai phương pháp tính toán động học ngược mà không cần phải tính ma trận tựa nghịch đảo của ma trận Jacobi: đó là phương pháp Khalil và Reduced- Gradient, vấn đề phát sinh trong mỗi phương pháp và cách khắc phục chúng. Giải bài toán động học ngược đối với một số Robot dư dẫn động cụ thể. So sánh hai phương pháp. Đồng thời trình bày khái quát phần mềm Maple và sử dụng nó một cách hiệu quả xây dựng chương trình tính toán động học Robot dư dẫn động. * Hướng tiếp tục phát triển của đồ án là: + Cần kết hợp với các ngôn ngữ lập trình khác (OpenGL) để có thể mô phỏng cho ta một cách nhìn trực quan về chuyển động của Robot. + Tiếp tục nghiên cứu các phương pháp khác để xác định vector z trong công thức (3.4) để thoả mãn vấn đề tránh cấu hình kỳ dị, chướng ngại vật… đồng thời tạo sự khéo léo linh hoạt hơn cho Robot. + Cần xây dựng chương trình có giao diện thuận tiện cho việc người dùng nhập các thông số tính toán cho Robot. PHỤ LỤC 1. CÁC THỦ TỤC GIẢI QUYẾT BÀI TOÁN ĐỘNG HỌC THUẬN Tạo ma trận DH: DH:=proc(theta,d,a,alpha) local A; A:=matrix(4,4); A[1,1]:=cos(theta); A[1,2]:=-sin(theta)*cos(alpha); A[1,3]:=sin(theta)*sin(alpha); A[1,4]:=a*cos(theta); A[2,1]:=sin(theta); A[2,2]:=cos(theta)*cos(alpha); A[2,3]:=-cos(theta)*sin(alpha); A[2,4]:=a*sin(theta); A[3,1]:=0; A[3,2]:=sin(alpha); A[3,3]:=cos(alpha); A[3,4]:=d; A[4,1]:=0; A[4,2]:=0; A[4,3]:=0; A[4,4]:=1; return(A); end proc: Xác định ma trận chuyển Tn: TranDH:=proc(A) local i,n,temp1,temp2,temp3,temp4,tempM,T; n:=rowdim(A); tempM:=matrix(4,4); T:=matrix(4,4,0); T[1,1]:=1;T[2,2]:=1;T[3,3]:=1;T[4,4]:=1; for i from 1 by 1 to n do temp1:=A[i,1]; temp2:=A[i,2]; temp3:=A[i,3]; temp4:=A[i,4]; tempM:=DH(temp1,temp2,temp3,temp4); T:=multiply(T,tempM); end do; return(T); end proc: InTranDH:=proc(A) local B; print(`Ma tran chuyen DH :`); B:=TranDH(A); map(combine,B,trig); end proc: Thủ tục dạo hàm ma trận: daoham:=proc(A,t) local rA,cA,i,j,B; if type(A,matrix)=true then rA:=rowdim(A): cA:=coldim(A): B:=matrix(rA,cA): for i from 1 to rA do for j from 1 to cA do B[i,j]:=diff(A[i,j],t): od:od: return(B): elif type(A,vector)=true then cA:=vectdim(A); B:=vector(cA): for i from 1 to cA do B[i]:=diff(A[i],t): od: return(B): elif type(A,vector)=true then rA:=op(1,eval(A)): B:=vector(rA): for i from 1 to rA do B[i]:=diff(A[i],t): od: return(B): else error"nhap sai ma tran hoac vector hay nhap lai" fi: end: Vị trí bàn kẹp: vitri:=proc(A) local i,B,p: B:=vector(3): for i from 1 to 3 do B[i]:=map(combine,A[i,4],trig): od: for i from 1 to 3 do print(p||i=B[i]): print(plot(B[i],t=0..10)): od: return(B): end: Vận tốc bàn kẹp: vantoc:=proc(C) local i,vtoc,v: vtoc:=vector(3): vtoc:=daoham(C,t): for i from 1 to 3 do print(v||i=map(combine,vtoc[i],trig)): print(plot(vtoc[i],t=0..10)): od: return(vtoc): end: Gia tốc bàn kẹp: giatoc:=proc(v) local a,i,gtoc: gtoc:=vector(3): gtoc:=daoham(v,t): for i from 1 to 3 do print(a||i=map(combine,gtoc[i],trig)): print(plot(gtoc[i],t=0..10)): od: end: Vận tốc góc bàn kẹp: vantocgoc:=proc(A) local i,omega,vtgoc,temp,A_dham,A_ndao: vtgoc:=vector(3): A_dham:=daoham(A,t): A_ndao:=inverse(A): temp:=multiply(A_dham,A_ndao): vtgoc[1]:=temp[3,2]: vtgoc[2]:=temp[1,3]: vtgoc[3]:=temp[2,1]: for i from 1 to 3 do print(omega||i=map(combine,vtgoc[i],trig)): print(plot(vtgoc[i],t=0..10)): od: return(vtgoc): end: Gia tốc góc bàn kẹp: giatocgoc:=proc(vtgoc) local i, gtgoc,epsilon: gtgoc:=daoham(vtgoc,t): for i from 1 to 3 do print(epsilon||i=map(combine,gtgoc[i],trig)): print(plot(gtgoc[i],t=0..10)): od: end: Quỹ đạo bàn kẹp: quydao:=proc(vtri) local buoc,i,sym,x,y,z: x:=vector(1000): y:=vector(1000): z:=vector(1000): buoc:=0: for i from 1 to 1000 do buoc:=buoc+0.001: x[i]:=subs(t=buoc,vtri[1]): y[i]:=subs(t=buoc,vtri[2]): z[i]:=subs(t=buoc,vtri[3]): od: sym:={seq([x[i],y[i],z[i]],i=1..1000)}: print(`quy dao ban kep`): print(pointplot3d(sym,color=black,axes=none,thickness=2)): end: 2. CÁC THỦ TỤC XÁC ĐỊNH MA TRẬN TỰA NGHỊCH ĐẢO Phương pháp Moore-Penrose: MP_Inverse:=proc(A) local B: B:=matrix(coldim(A),rowdim(A)): if rowdim(A)<coldim(A) then B:=multiply(transpose(A),inverse(multiply(A,transpose(A)))): elif rowdim(A)=coldim(A) then B:=inverse(A): else B:=multiply(inverse(multiply(transpose(A),A)),transpose(A)): fi: print(simplify(B)): end: Phương pháp Greville: GRE-Inverse:=proc(A) local a,Ac,c,d,n,i,tg1,b,tg2,m,cs1,bs: n:=coldim(A): m:=rowdim(A): for i from 1 to n do a||i:=matrix(m,1); A||i:=matrix(m,i); a||i:=submatrix(A,1..m,i..i); A||i:=submatrix(A,1..m,1..i); od: for i from 1 to n-1 do if i=1 then Ac||i:=NDVT(a||i): fi: d||(i+1):=multiply(Ac||i,a||(i+1)): c||(i+1):=evalm(a||(i+1)-multiply(A||i,d||(i+1))): b:=matrix(i+1,1,0): if iszero(c||(i+1)) then tg1:=multiply(transpose(d||(i+1)),d||(i+1)): tg2:=multiply(transpose(d||(i+1)),Ac||i): bs||(i+1):=evalm((1+tg1)^(-1)*tg2): else bs||(i+1):=NDVT(c||(i+1)): fi: cs1:=evalm(Ac||i-multiply(d||(i+1),bs||(i+1))): Ac||(i+1):=blockmatrix(2,1,[cs1,bs||(i+1)]): od: end: Phương pháp Khalil: KHALIL_Inverse:=proc(J) global m,n,Jm,JJm,Jc,Em,Enm,JJ,k,C,C1,C1t,hs,CS1,CS2,CC: m:=rowdim(J): n:=coldim(J): Jm:=submatrix(J,1..m,1..m): if det(Jm=0) then break; fi: JJm:=inverse(Jm): Em:=band([1],m): Enm:=band([1],n-m): for k from m+1 to n do c||k:=multiply(JJm,col(J,k)); od: C1:=blockmatrix(1,n-m,[seq(c||k,k=m+1..n)]): C:=blockmatrix(1,2,[Em,C1]): C1t:=transpose(C1): hs:=evalm(Enm+multiply(C1t,C1)): CS1:=evalm(Em-multiply(C1,inverse(hs),C1t)): CS2:=multiply(hs,C1t): CC:=blockmatrix(2,1,[CS1,CS2]): Jc:=multiply(CC,JJm): end: 2. CÁC THỦ TỤC GIẢI QUYẾT BÀI TOÁN ĐỘNG HỌC NGƯỢC Thủ tục xác định ma trận Jacobi JCB:=proc(p,q) local j,i,temp,n,v: v:=convert(p,vector); n:=rowdim(q): temp:=jacobian(v,[seq(q||i,i=1..n)]); for i from 1 to 3 do if v[i]=0 then j:=delrows(temp,i..i): return(j): fi: od: return(temp) end: Thủ tục đổi chỗ các cột của ma trận doicho:=proc(A,k) local B,i,tg,n,m: m:=rowdim(A): n:=coldim(A): tg:=col(A,1): col(A,1):=col(A,k): col(A,k):=tg: B:=blockmatrix(1,n,[seq(col(A,i),i=1..n)]): end: Thủ tục giải bài toán động học ngược của phương pháp Khalil INV_KHALIL:=proc(J,P,delta,T) local m,n,k,X,h,Y,g,Jm,tg,J_check,qqq,Jm_INV,J_sub,Enm,c,s,dq,C1,C1_trans,cons,dq_p,dq_c,dQ, nghiem,eqns,sys,tgian,i,w,dq_m,xx,yy,zz,toado: m:=rowdim(J): n:=coldim(J): s:=1: J_sub:=J: Jm:=submatrix(J_sub,1..m,1..m): while det(Jm)=0 do J_sub:=doicho(J_sub,m+s): Jm:=submatrix(J_sub,1..m,1..m): print("Doi cho q1 va", q||(m+s)); s:=s+1: od: Jm_INV:=inverse(Jm): Enm:=band([1],n-m): for k from m+1 to n do c||k:=multiply(Jm_INV,col(J_sub,k)); od: C1:=blockmatrix(1,n-m,[seq(c||k,k=m+1..n)]): C1_trans:=transpose(C1): cons:=evalm(Enm+multiply(C1_trans,C1)): dq_p:=multiply(Jm_INV,daoham(P,t)): dq_c:=multiply(inverse(cons),C1_trans,dq_p): dq_m:=evalm(dq_p-multiply(C1,dq_c)): dQ:=blockmatrix(2,1,[dq_m,dq_c]): for i from 1 to n do dq||i:=subs({seq(q||k=q||k(t),k=1..n)},dQ[i,1]): pt||i:=D(q||i)(t)=dq||i: od: eqns:=seq(pt||i,i=1..n): sys:={eqns,dkdau}; w:=T/delta: tgian:=[seq(k*delta,k=0..w)]: nghiem:=dsolve(sys,{seq(q||i(t),i=1..n)},type=numeric,output=array(tgian)); for i from 1 to n do X||i:=seq([k*delta,nghiem[2,1][k+1,i+1]],k=0..w): h||i:=plot([X||i],color=red): Y||i:=seq([k*delta,(nghiem[2,1][k+2,i+1]-nghiem[2,1][k+1,i+1])/delta],k=0..w-1); g||i:=listplot([Y||i],color=red); od: for i from 1 to n do Z||i:=seq([k*delta,(Y||i[k+2,2]-Y||i[k+1,2])/delta],k=0..w-2); l||i:=plot([Z||i],color=red); od: for i from 1 to n do print(`vi tri khau`,i): print(h||i); print(`van toc khau`,i): print(g||i); print(`gia toc khau`,i): print(l||i); od: xx:=vector(200): yy:=vector(200): zz:=vector(200): J_check:=submatrix(J,1..m,1..m): qqq:=matrix(5,150): for i from 1 to n do for k from 1 to w do qqq[i,k]:=nghiem[2,1][k,i+1]: od: od: if det(J_check)=0 then for k from 1 to w do tg:=qqq[1,k]: qqq[1,k]:=qqq[m+s-1,k]: qqq[m+s-1,k]:=tg: od: fi: for k from 1 to w do xx[k]:=evalf(subs({seq(q||i=qqq[i,k],i=1..n)},p1[1]),10): yy[k]:=evalf(subs({seq(q||i=qqq[i,k],i=1..n)},p1[2]),10): zz[k]:=evalf(subs({seq(q||i=qqq[i,k],i=1..n)},p1[3]),10): od: toado:={seq([xx[k],yy[k],zz[k]],k=1..w)}: print(`quy dao ban kep`); print(pointplot3d(toado,color=black,axes=none,thickness=1)); end: Thủ tục giải bài toán động học ngược bằng phương pháp Reduced- Gradient: INV_RG:=proc(J,B,alpha,P,delta,T) local m,n,JA,JB,JA_INV,J_sub,JR,dp,s,zezo,tg,qqq,k,J_check,J1,J_tp1,delta_H,i,CS1,JR_trans,J2,Enm,CS2,J_tp2,dQ,sys,eqns,w,tgian,nghiem,H,xx,yy,zz,toado: m:=rowdim(J): n:=coldim(J): J_sub:=J: delta_H:=matrix(n,1): JA:=submatrix(J_sub,1..m,1..m): s:=1; while det(JA)=0 do J_sub:=doicho(J_sub,m+s): JA:=submatrix(J_sub,1..m,1..m): print("Doi cho q1 va", q||(m+s)); s:=s+1: od: H:=det(multiply(J_sub,B,transpose(J))); for i from 1 to n do delta_H[i,1]:=diff(H,q||i): od: JB:=submatrix(J_sub,1..m,m+1..n): JA_INV:=inverse(JA): JR:=multiply(JA_INV,JB): dp:=daoham(P,t): zezo:=matrix(n-m,m,0): J1:=blockmatrix(2,1,[JA_INV,zezo]): J_tp1:=multiply(J1,dp): JR_trans:=transpose(JR): CS1:=multiply(JR,JR_trans): Enm:=band([1],n-m): J2:=blockmatrix(2,2,[CS1,-JR,-JR_trans,Enm]): CS2:=multiply(J2,delta_H): J_tp2:=scalarmul(CS2,alpha): dQ:=evalm(J_tp1+J_tp2): for i from 1 to n do dq||i:=subs({seq(q||k=q||k(t),k=1..n)},dQ[i,1]): pt||i:=D(q||i)(t)=dq||i: od: eqns:=seq(pt||i,i=1..n): sys:={eqns,dkdau}: w:=T/delta: tgian:=[seq(k*delta,k=0..w)]: nghiem:=dsolve(sys,{seq(q||i(t),i=1..n)},type=numeric,output=array(tgian)): for i from 1 to n do X||i:=seq([k*delta,nghiem[2,1][k+1,i+1]],k=0..w): h||i:=plot([X||i],color=red): Y||i:=seq([k*delta,(nghiem[2,1][k+2,i+1]-nghiem[2,1][k+1,i+1])/delta],k=0..w-1); g||i:=listplot([Y||i],color=red); od: for i from 1 to n do Z||i:=seq([k*delta,(Y||i[k+2,2]-Y||i[k+1,2])/delta],k=0..w-2); l||i:=plot([Z||i],color=red); od: for i from 1 to n do print(`vi tri khau`,i): print(h||i); print(`van toc khau`,i): print(g||i); print(`gia toc khau`,i): print(l||i); od: xx:=vector(200): yy:=vector(200): zz:=vector(200): J_check:=submatrix(J,1..m,1..m): qqq:=matrix(5,150): for i from 1 to n do for k from 1 to w do qqq[i,k]:=nghiem[2,1][k,i+1]: od: od: if det(J_check)=0 then for k from 1 to w do tg:=qqq[1,k]: qqq[1,k]:=qqq[m+s-1,k]: qqq[m+s-1,k]:=tg: od: fi: for k from 1 to w do xx[k]:=evalf(subs({seq(q||i=qqq[i,k],i=1..n)},p1[1]),10): yy[k]:=evalf(subs({seq(q||i=qqq[i,k],i=1..n)},p1[2]),10): zz[k]:=evalf(subs({seq(q||i=qqq[i,k],i=1..n)},p1[3]),10): od: toado:={seq([xx[k],yy[k],zz[k]],k=1..w)}: print(`quy dao ban kep`); print(pointplot3d(toado,color=black,axes=none,thickness=1)); end: Chương trình chính tính toán động học thuận Robot dư dẫn động: restart; with(linalg): with(plots): with(plottools): read(“ C:/dong_hoc_Robot.txt”);# Đọc các thủ tục cần thiết T:=InTranDH(“C:/TSRobot/3khauphang.txt”); p:=vitri(T); v:=vantoc(p); gt:=giatoc(v); vtgoc:=vantocgoc(T); gtgoc:=giatocgoc(vtgoc); quydaobankep(p); Thủ tục chính tính toán động học ngược Robot dư dẫn động main:=proc(phuongphap) local A,B,T,q,J,P,choose; global p1: A:=matrix(n,4,DenHat): T:=InTranDH(A): p1:=vitri(T): q:=matrix(n,1,NameVar): J:=JCB(p1,q): P:=vector(QuyDao): choose:=phuongphap; if choose=Khalil then INV_KHALIL(J,P,buoc,thoigian); elif choose=RG then B:=matrix(n,n,MT_quantinh): INV_RG(J,B,alpha,P,buoc,thoigian); fi: end: Chương trình chính: restart; with(linalg); with(plots); with(plottools); with(DEtools); read(“C:/TSRobot_nguoc/3khauphang.txt”);# Đọc File dữ liệu cho Robot 3 khâu phẳng main(Khalil); TÀI LIỆU THAM KHẢO 1. Nguyễn Văn Khang: Bài giảng động lự học hệ nhiều vật. Đại học Bách Khoa Hà Nội 2003 2. Nguyễn Văn Khang: Cơ sở học kỹ thuật, Nhà xuất bản đại học quốc gia Hà Nội 2003 3. GS.TSKH Nguyễn Thiện Phúc: Robot công nghiệp. Nhà xuất bản khoa học và kỹ thuật2002. 4. Phạm Huy Điển: Tính toán, lập trình và giảng dạy toán học trên Maple. Nhà xuất bản khoa học và kỹ thuật Hà Nội 2002. 5. Đỗ Thành Trung: Bài giảng Maple cho sinh viên. - Bộ môn cơ học ứng dụng trường Đại Học Bách Khoa Hà Nội. 6. Hà Đức Hiệp: “Bài toán động học thuận và động học ngược Robot dư dẫn động”. Đồ án tốt nghiệp, Đại học Bách khoa Hà nội 2004 7. Nguyễn Nhật Lệ: Tối ưu hóa ứng dụng, Nhà xuất bản KH và kỹ thuật 8. C.Chevallereau and Khalil: “A new method for the solution of the inverse kinematics of redundant Robot”.. Laboratoire d’Automatique de la noe. 9. “Kinematic Resolution of Redundancy via Joint- Space Decomposition”. A.De luca, G.Oriolo. Dipartimento di Informatica, Univeresita degli Studi di Roma “Sapienza”, Via Eudossiana 18,00184 Roma, Italy. 10. C.Chevallereau and Khalil: “ Efficient method for the calculation of the pseudo inverse kinematic problem”. Proc. IEEE conf. on Robotics and Automation, Raleigh, 1987,pp.1842-1848. 11. Ben- Israel, T.N.E. Greville (1974). Generalized Inverse: Theory and Applications Wiley – Interscience, New York.. 12. C.A. Klein, C.Huang: “Review of pseudoinverse control for use with kinematicall redundant manipulators”, IEEE trans. on Syst, Man and Cybernetics. Vol, SMC – 13, 1983, pp.245-250. 13. A.De Luca and G. Oriolo: “ The reduced Gradient method fod solving redundancy in Robot arms”. 11th IFAC World Congress ( Tallinn, USSR, Aug 13-17, 1990) MỤC LỤC Lời nói đầu 1 Chương 1.Tính toán động học thuận Robot dư dẫn động 3 1.1 Bộ thông số DH và ma trận Denavit- Hartenberg 3 1.1.1 Bộ thông số DH 3 1.1.2 Ma trận Denavit- Hartenberg 5 1.1.3 Phương trình xác định vị trí khâu thao tác (bàn kẹp) của Robot 6 1.1.4 Giải quyết bài toán động học thuận 6 1.2 Tính toán động học thuận của một số Robot 7 1.2.1 Bài toán động học thuận của Robot 3 khâu phẳng 7 1.2.2 Bài toán động học thuận của Robot 4 khâu phẳng 10 1.2.3 Bài toán động học thuận của Robot 5 khâu phẳng 14 1.2.4 Bài toán động học thuận của Robot PPR phẳng 18 1.2.5 Bài toán động học thuận của Robot RRRP1 21 1.2.6 Bài toán động học thuận của Robot RRRP2 25 Chương 2. Nghịch đảo ma trận chữ nhật, ma trận tựa nghịch đảo 31 2.1 Định nghĩa ma trận tựa nghịch đảo 31 2.2 Phương pháp Moore- Penrose 32 2.2.1 Cơ sở lý thuyết 32 2.2.2 Sơ đồ khối của phương pháp Moore- Penrose 33 2.3 Phương pháp Greville 34 2.3.1 Cơ sở lý thuyết 34 2.3.2 Sơ đồ khối của phương pháp Greville 35 2.4 Phương pháp phân tích cân xứng Khalil 36 2.4.1 Cơ sở lý thuyết 36 2.4.2 Sơ đồ khối của phương pháp Khalil 37 2.5 Một số ví dụ cụ thể 38 2.6 So sánh các phương pháp xác định ma trận tựa nghịch đảo 40 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 41 3.1 Giải pháp đưa ra cho mô hình động học ngược 41 3.2 Phương pháp Khalil giải bài toán động học ngược Robot dư dẫn động 45 3.2.1 Phân tích ma trận Jacobi 45 3.2.2 Giải quyết theo tựa nghịch đảo 46 3.3 Phương pháp Reduced- Gradient (RG) giải bài toán động học ngược Robot dư dẫn động 49 3.4 Ứng dụng 2 phương pháp trên tính toán cho các Robot dư dẫn động cụ thể 51 3.4.1 Robot 3 khâu phẳng 51 3.4.2 Robot 4 khâu phẳng 55 3.4.3 Robot 5 khâu phẳng 59 3.4.4 Robot PPR phẳng 64 3.4.5 Robot RRRP1 68 3.4.6 Robot RRRP2 72 3.5 So sánh phương pháp Khalil và phương pháp Reduced- Gradient 76 3.6 Giới thiệu phần mềm Maple và cách sử dụng Maple để giải bài toán động học Robot dư dẫn động 77 3.6.1 Giới thiệu phần mềm Maple 77 3.6.2 Sử dụng Maple 9.0 để giải bài toán động học Robot dư dẫn động 78 Kết luận 80 Phụ lục 82 Tài liệu tham khảo 92