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

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.

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. 1 Chng 2: a ra mt s phng phỏp tỡm nghch o ma trn ch nht l c s gii quyt bi toỏn ng hc ngc Robot. c bit l phng phỏp phõn tớch cõn xng ca Khalil. Chng 3: Da trờn phng phỏp tớnh ma trn ta nghch o trỡnh by chng hai, chỳng ta tp trung nghiờn cu v a ra 2 phng phỏp: phng phỏp Khalil v phng phỏp Reduced _ Gradient gii quyt bi toỏn ng hc ngc Robot d dn ng. Trong quỏ trỡnh thc hin ỏn ny, do kin thc v Robot cú hn, thi gian ớt v ti liu bng ting Vit cha cú nờn ỏn ca em vn cũn nhiu thiu xút, em kớnh mong c s ch bo, hng dn ca cỏc thy ngy cng hon thin mỡnh hn. Em xin chõn thnh cm n GS.TSKH Nguyn Vn Khang, Th.S Thnh Trung ó tn tỡnh ch bo, hng dn em hon thnh bn ỏn ny. Em cng xin chõn thnh cm n cỏc thy giỏo trong b mụn C hc ng dng ó to iu kin giỳp em trong quỏ trỡnh lm ỏn ti 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 2 Khớp 1 0 −i Khớp Khâu i-1 Khớp Khâu i Khâu i+1 2−i Z i Z 1−i Z 1 0 −i i a i X i Y 1− ′ i Z i 0 i α i 0 ′ i d 1−i Y ' i X 1−i X i θ i 0 Hình 1.1 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 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 1 − i z được chọn dọc theo hướng của trục khớp động thứ i • Trục 1 − i x được chọn dọc theo đường vuông góc chung của hai trục 2 − i z và 1 − i z , hướng đi từ trục 2 − i z sang trục 1 − i z . Nếu trục 1 − i z cắt trục 2 − i z thì hướng của trục 1 − i x được chọn tuỳ ý. 3 Khớp 1 0 −i Khớp Khâu i-1 Khớp Khâu i Khâu i+1 2−i Z i Z 1−i Z 1 0 −i i a i X i Y 1− ′ i Z i 0 i α i 0 ′ i d 1−i Y ' i X 1−i X i θ i 0 Hình 1.2 • Gốc tọa độ 1 − i O được chọn tại giao điểm của trục 1 − i x và trục 1 − i z . • Trục 1 − i y được chọn sao cho hệ ( ) 1 − i Oxyz là hệ quy chiếu thuận. Với cách chọn trên, đôi khi các hệ tọa độ khâu ( ) 1 − i Oxyz 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: • Đối với hệ tọa độ ( ) o Oxyz theo quy ước trên ta mới chỉ chọn được trục o z , còn trục o x chưa có trong quy ước trên. Ta có thể chọn trục o x một cách tuỳ ý. • Đối với hệ tọa độ ( ) n Oxyz do không có khớp n+1 nên ta theo quy ước trên không xác định được trục n z . Trục n z không xác định duy nhất trong khi trục n x lại được xác định theo pháp tuyến của trục 1 − n z . Trong trường hợp này nếu là khớp quay ta nên chọn trục n z song song với trục 1 − n z . Ngoài ra ta có thể chọn tuỳ ý sao cho hợp lý. 4 • Khi hai trục 1 − i z và 2 − i z 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 1 − i x 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 1 − i z một cách tuỳ ý. Tuy nhiên trong nhiều trường hợp người ta thường chọn trục 1 − i z 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 Oxyz đối với hệ tọa độ khâu ( ) 1 − i Oxyz được xác định bởi bộ thông số DH như sau: - i θ : góc quay quanh trục 1 − i z để trục 1 − i x chuyển đến trục ( ) iii xxx // ′′ . - i d : 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. - i a : độ dài đường vuông góc chung giữa hai trục khớp động i+1 và i. - 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ố i a và i α 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 i θ và i d , 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ì i θ là biến còn i d là hằng , nếu khớp i là tịnh tiến thì i d là biến còn i θ 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: 1. Quay quanh trục 1 − i z một góc i θ . 2. Tịnh tiến dọc trục 1 − i z một khoảng i d . 3. Tịnh tiến dọc trục 1 − i x một đoạn i a . 4. Quay quanh trục i x một góc i α . Ma trận của phép biến đổi thuần nhất ký hiệu là H i , là tích của bốn phép biến đổi cơ bản và có dạng như sau: 5             −                                     − = 1000 0cossin0 0sincos0 0001 1000 0100 0010 001 1000 100 0010 0001 1000 0100 00cossin 00sincos ii ii i i ii ii i a d αα ααθθ θθ H             − − = 1000 cossin0 sinsincoscoscossin cossinsincossincos iii iiiiiii iiiiiii i d a a αα θαθαθθ θαθαθθ H (1.1) Ma trận i H gọi là ma trận Denavit-Hartenberg. Ma trận Denavit-Hartenberg i H là ma trận chuyển tọa độ của hệ quy chiếu ( ) 1 − i Oxyz đối với hệ quy chiếu ( ) i Oxyz . Chính xác hơn ta phải ký hiệu là i i- H 1 . Đơn giản cách viết ta sử dụng ký hiệu i H thay cho i i- H 1 . Còn i D được dùng với nghĩa i o H . 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 i D là tích các ma trận i H 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 ni = , 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ó:             === − 1000 21 110 zzzz yyyy xxxx nn n pasn pasn pasn HHHHHHD n 21 (1.2) Trong đó x p , y p , z p 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 6 a. Hình vẽ và bảng thông số động học Denavit-hartenberg Trục θ i d i a i α i 1 2 3 q1 q2 q3 0 0 0 a 1 a 2 a 3 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 n D : 3 2 2 1 1 0 HHHD = n             +++++++++ +++++++−++ = 1000 0100 1sin)sin()321sin(0)cos()sin( 1cos)cos()321cos(0)sin()cos( 12123321321 12123321321 qaqqaqqqaqqqqqq qaqqaqqqaqqqqqq Từ đó ta có được vi trí bàn kẹp trong hệ tọa độ cố định: 112123213 cos)cos()cos( qaqqaqqqap x +++++= 112123213 sin)sin()sin( qaqqaqqqap y +++++= 0 = z p c. Dữ liệu đầu vào của Robot 3 khâu phẳng 3 y 0 y 1 x 1 y 2 y 0 x 3 x 1 q 1 a 2 x 2 a 3 a 2 q 3 q Hình 1.3 7 Để 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 i a . Ta chọn quy luật chuyển động của các biến khớp theo thời gian t như sau: tq 3 1 π = ; tq 2 2 π = ; tq π = 3 Và các khoảng cách: 15,15,15 321 === aaa , thay vào chương trình tính toán: - Vị trí: ) 3 cos(15) 6 5 cos(15) 6 11 cos(15 ttt p x πππ ++= ) 3 sin(15) 6 5 sin(15) 6 11 sin(15 ttt p y πππ ++= 0 = z p - Vận tốc: π π π π π π ) 3 sin(5) 6 5 sin( 2 25 ) 6 11 sin( 2 55 ttt v x −−−= π π π π π π ) 3 cos(5) 6 5 cos( 2 25 ) 6 11 cos( 2 55 ttt v y ++= 0 = z v Đồ thị tọa độ bàn kẹp p x (t) Đồ thị bàn kẹp p y (t) Đồ thị vận tốc bàn kẹp v y (t) Đồ thị vận tốc bàn kẹp v x (t) 8 Đồ thị gia tốc bàn kẹp a y (t) Đồ thị gia tốc bàn kẹp a x (t) - Gia tốc: 222 ) 3 cos( 3 5 ) 6 5 cos( 12 125 ) 6 11 cos( 12 605 π π π π π π ttt a x −−−= 222 ) 3 sin( 3 5 ) 6 5 sin( 12 125 ) 6 11 sin( 12 605 π π π π π π ttt a y −−−= 0 = z a Quỹ đạo bàn kẹp 9 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 Trục θ i d i a i α 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 n D :             +++ +++− = = ++++++++++++ ++++++++++++ 1000 0100 0 0 . 1121232134321443214321 1121232134321443214321 4 3 3 2 2 1 1 0 SaSaSaSaCS CaCaCaCaSC n HHHHD Ở đây do kết quả ma trận n D dài nên ta sử dụng kí hiệu: )sin(),cos(sin,cos , jijijijiiiii qqSqqCqSqC +=+=== ++ . Từ đó vị trí bàn kẹp trong hệ tọa độ cố định: 11212321343214 cos)cos()cos()cos( qaqqaqqqaqqqqap x +++++++++= 11212321343214 sin)sin()sin()sin( qaqqaqqqaqqqqap y +++++++++= 0 y 1 x 1 y 2 y 0 x 4 y 4 x 1 q 1 a 2 x 2 a 3 a 2 q 3 q 4 q 3 y 3 x 4 a Hình 1.4 10 [...]... 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 33 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: A = [ a 1 ,a 2 , ,a n ] = [ An −1 a n ] Gọi ma trận... 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ỡ m × n A+ là ma trận tựa nghịch đảo cỡ n × m của A Nếu... n (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 A + Chú ý: ma trận A có hạng đầy đủ tức là Rank( A)= m nếu m < n Rank( A)= n nếu n < m 32 2.2.2 Sơ đồ khối của phương pháp Moore- Penrose Nhập vào mn m=n A... , khi đó a k = 0 và b k = 0 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 A1+ theo công thức:    a1  + A1 =  m  2 ∑a [i,1]   i =1  T Sau đó ta tính được d 2 và c 2 theo (2.9) và (2.10) từ đó theo công thức (2.11) của định lý 2.2 ta tính được A2+ b3... A i + 1 +) 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 A m×n , 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ỡ m × n ( m < n ) và có hạng là m Theo lý thuyết ma trận (F.R Gantmacher:... )π 2 Đồ thị gia tốc Đồ thị gia tốc Quỹ đạo bàn kẹp 20 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 z2 z1 z0 q2 q1 a3 q3 x3 x0 x2 z3 x1 a2 a1 q4 x4 z4 Hình 1.7 Bảng thông số: Trục 1 θi q1 di 0 ai a1 αi 0 2 q2 0 a2 0 3 q3 0 a3 Pi 4 0 q4 0 0 b Phương trình xác định vị trí bàn kẹp của Robot D n =0 H 1 1 H 2 2 H 3 3 H 4 C1+2 +3 S1+2 +3 0 a 3 C1+2 +3... T A] A T −1 return() 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 A m×n , 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... 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: p =f (q ) (2.1) Có thể tìm q về mặt hình thức dư i dạng: q =f −1 (p) (2.2) Đạo hàm hai vế của phương trình (2.1) ta được:  p= Trong đó ∂ f   q =J (q )q ∂ q ∂ f J= ∂ q (2.3) là ma trận Jacobi cỡ m× n Đố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... 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ỡ m× n , A∈IR m×n , X là ma trận cỡ m× n , X∈IR m×n 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... gia tốc bàn kẹp Quỹ đạo bàn kẹp 12 13 1.2.3 Bài toán động học thu n của Robot 5 khâu phẳng a Hình vẽ và bảng thông số động học Denavit- Hartanberg x4 y5 x5 y4 y2 x2 q3 y0 y1 a5 q5 a3 a2 q2 a1 q1 a4 q4 x1 x3 x0 Hnh 1.5 Bảng thông số: Trục 1 θi q1 di 0 ai a1 αi 0 2 q2 0 a2 0 3 q3 0 a3 0 4 q4 0 a4 0 5 q5 0 a5 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