Động học robot potx

3./Động Học Thuận: Qui Tắc Denavit-Hartenberg Bước 1: Xác định các trục khớp và đặt tên tương ứng 10 −n zz  . Bước 2: Xác lập hệ tọa độ nền. Đặt gốc của hệ tọa độ này tại bất kỳ điểm nào trên trục 0 z . Các trục 0 x và 0 y được chọn thỏa qui tắc tam diện thuận. Lặp 1, ,1 −= ni lần thực hiện bước 3 đến bước 5. Bước 3: Xác định các gốc i O là giao điểm của đường vuông góc chung giữa i z và 1−i z với i z . Nếu i z giao với 1−i z , đặt i O tại giao điểm này. Nếu i z song song với 1−i z , đặt i O tại bất kỳ vị trí nào trên i z sao cho thuận tiện. Bước 4: Xác định i x dọc theo đường vuông góc chung giữa 1−i z và i z đi qua i O , hoặc theo hướng vuông góc với mặt phẳng tạo bởi 1−i z và i z nếu 1−i z và i z giao nhau. Bước 5: Xác định i y thỏa qui tắc tam diện thuận. Bước 6: Xác định hệ tọa độ tác động cuối nnnn zyxo . Giả sử khớp n là khớp quay, đặt az n = dọc theo hướng 1−n z . Xác định gốc n O bất kỳ trên n z sao cho thuận tiện, thường là tâm của bộ kẹp hay tại đầu dụng cụ mà tay máy phải mang. Đặt sy n = theo hướng kẹp và đặt n x theo as x . Nếu dụng cụ kẹp không đơn giản thì đặt n x và n y tạo thành tam diện thuận. Bước 7: Lập bảng tham số ch các khâu trên robot i a : khoảng cách theo phương i x từ i O đến giao điểm của các trục i x và 1−i z . i d : khoảng cách theo phương 1−i z từ 1−i O đến giao điểm của các trục i x và 1−i z , i d thay đổi khi khớp i là khớp trượt. i α : là góc quay quanh trục i x từ 1−i z đến i z . i θ : là góc quay quanh trục 1−i z từ 1−i x đến i x . Bước 8: Từ các ma trận biến đổi thuần nhất i A bằng cách thay các tham số trên vào. Bước 9: Tính nn AAT  1 0 = . Ma trận này cho ta biết được vị trí và hướng đối với hệ tọa độ nền của dụng cụ gắn trên khâu cuối. Ví dụ 3.1: 1 Hình 3.1: Tay máy hai khâu phẳng. Bảng tham số khâu cho robot 2 khâu đồng phẳng Khâu i a i α i d i θ 1 1 a 0 0 1 θ 2 2 a 0 0 2 θ             − =             − = 1000 0100 0 0 1000 0100 0 0 2222 2222 2 1111 1111 1 sacs casc A sacs casc A Ma trận biến đổi thuần nhất             + +− == 1000 0100 0 0 122111212 122111212 21 0 2 sasacs cacasc AAT Lưu ý rằng 2 thành phần đầu của cột cuối cùng của 0 2 T là vị trí x và y của 2 O . 12211 12211 sasay cacax += += 2 Phần quay của 0 2 T cho hướng của 2222 zyxo đối với hệ tọa độ nền. Ví dụ 3.2: Cổ tay khớp cầu (Spherical Wrist) Trục 543 ,, zzz đồng quy tại điểm O . Tay máy Stanford có cổ tay thuộc dạng này. Hình 3.2: Gán hệ trục tọa độ cho cổ tay khớp cầu. Bảng 3.2: Tham số DH cho cổ tay khớp cầu. Khâu i a i α i d i θ 4 0 -90 0 4 θ 5 0 90 0 5 θ 6 0 0 6 d 6 θ Ta thấy rằng ba biến khớp cuối 654 ,, θθθ là các góc Euler ψθφ ,, tương ứng đối với hệ tọa độ 3333 zyxO . Ta có             − − = 1000 0010 00 00 44 44 4 cs sc A ;             − − = 1000 0010 00 00 55 55 5 cs sc A ;             − = 1000 100 00 00 6 66 66 6 d cs sc A             − +−+ −− =       == 1000 10 6556565 654546465464654 6545465464654 3 6 3 6 654 3 6 dccsscs dssssccscsscccs dscscsccssccc OR AAAT So sánh phần ma trận quay 3 6 R của 3 6 T với phép biến đổi góc Euler. Điều đó cho thấy rằng vai trò 654 ,, θθθ hoàn toàn giống với các góc Euler ψθφ ,, đối với hệ tọa độ 3333 zyxO . NL: Các góc Euler Xét hệ tọa độ cố định 0000 zyxO và hệ tọa độ quay 1111 zyxO nhận được bởi việc thực hiện 3 phép quay sau: 3 (1) Quay quanh trục z một góc φ ; (2) Quay quanh trục y hiện hành một góc θ ; (3) Quay quanh trục z hiện hành một góc ψ Hình 3.3: Sự biểu diễn các góc Euler Ma trận biến đổi           − +−+ −−− =           −           −           − = = θψθψθ θφψφψθφψφψθφ θφψφψθφψφψθφ ψψ ψψ θθ θθ φφ φφ ψθφ csscs ssccscsscccs sccssccssccc cs sc cs sc cs sc RRRR zyz 100 0 0 0 010 0 100 0 0 ,,, 0 1 Bây giờ xét bài toán xác định các góc ψθφ ,, khi cho trước ma trận quay           = 333231 232221 131211 rrr rrr rrr R Giả sử rằng cả hai phần tử 2313 ,rr đều không bằng 0. Có nghĩa là 0 ≠ θ s và vì thế cả 3231 ,rr đều không bằng 0. Nếu cả 2313 ,rr đều không bằng 0 thì 1 33 ±≠r và ta có 33 rc = θ , 2 33 1 rs −±= θ , như vậy ta có ( ) 2 3333 1,tan rrA −= θ (3.1) hay ( ) 2 3333 1,tan rrA −−= θ (3.2) Tùy vào dấu của các tham số mà hàm Atan sẽ chọn góc phần tư cho góc θ . Nếu cả hai tham số bằng 0, thì hàm Atan không xác định. Nếu 0 2313 == rr , thì 1 33 ±=r và 0 3231 == rr . Vì vậy R có dạng 4           ± = 100 0 0 2221 1211 rr rr R Nếu 1 33 =r thì 1 = θ c và 0 = θ s , kết quả là 0 = θ . Trong trường hợp này 0 1 R trở thành           =           ++ +−+ =           +−+ −−− = 333231 232221 131211 0 1 100 0)()( 0)()( 100 0 0 rrr rrr rrr cs sc ccsssccs csscsscc R ψφψφ ψφψφ ψφψφψφψφ ψφψφψφψφ ),tan(),tan( 12112111 rrArrA −==+⇒ ψφ Có vô số nghiệm trong trường hợp này. Ta có thể lấy 0= φ , và xác định ψ . Nếu 1 33 −=r , thì 1 −= θ c và 0 = θ s , kết quả là πθ = . Ta có           =           −− −−−− = 333231 232221 131211 0 1 100 0)()( 0)()( rrr rrr rrr cs sc R ψφψφ ψφψφ ),tan(),tan( 12112111 rrArrA −−=−−=−⇒ ψφ Cũng có vô số nghiệm trong trường hợp này. 0 4./Động học nghịch (Inverse kinematics) 5 Phát biểu bài toán Cho trước một ma trận biến đổi homogenous 4x4 sau:       = 10 OR H (4.1) Tìm một hay tất cả nghiệm của phương trình sau HqqT nn =),,( 1 0  (4.2) trong đó )()(),,( 111 0 nnnn qAqAqqT  = (4.3) và H biểu diễn vị trí và hướng mong muốn của cơ cấu tác động cuối, và ta phải tìm giá trị các góc khớp n qq ,, 1  sao cho HqqT nn =),,( 1 0  . Phương trình (4.2) đưa đến việc giải 12 phương trình phi tuyến với n ẩn sau: ijnij hqqT == ),,( 1  , 3,2,1=i , 4, ,1=j (4.4) trong đó ijij hT , tương ứng là 12 phần tử có giá trị của 0 n T và H (vì hàng cuối cùng của cà 0 n T và H đều là )1,0,0,0( , nên 4 trong số 16 phương trình từ (4.2) sẽ không có giá trị). Ví dụ 4.1: Khảo sát tay máy Stanford. 6 Hình 4.1: Tay máy Stanford Giả sử rằng vị trí và hướng của hệ tọa độ trên cơ cấu tác động cuối được cho trước như sau: Để tìm các biến khớp 54321 ,,,, θθθθ d ta phải giải tập phương trình lượng giác phi tuyến sau: 7 Dĩ nhiên, các phương trình trên rất khó giải trực tiếp, và vấn đề này cũng xảy ra đối với hầu hết các tay máy robot. Vì thế, ta cẩn phải tìm các kỹ thuật hiệu quả có hệ thống để khai thát cấu trúc động học đặc biệt của từng tay máy. Trong khi vấn đề động học thuận luôn có lời giải duy nhất, chỉ đơn giản thay các giá trị biến khớp vào các phương trình động học thuận, thì vấn đề động học ngược có thể không có lời giải. Ngay cả khi tồn tại một lời giải, nó cũng có thể không là lời giải duy nhất. Hơn nữa, vì các phương trình động học thuận nói chung là các hàm phi tuyến phức tạp đối với biến khớp, nghiệm cho bài toán động học ngược có thể rất khó giải được ngay cả khi nghiệm tồn tại. Để giải bài toán động học ngược ta quan tâm nhất việc tìm nghiệm dạng đóng hơn là tìm nghiệm bằng phương pháp số vì hai lý do. Thứ nhất, trong một số ứng dụng, như đi theo vết đường hàn được cung cấp bởi hệ thống vision, các phương trình động học ngược phải được giải với tốc độ nhanh, khoảng 20 mili giây, và các biểu thức đóng cho nghiệm trực tiếp thì thực tế hơn là dùng phương pháp số. Thứ hai, các phương trình động học nói chung có nhiều nghiệm. Tìm lời giải bằng biểu thức đóng cho phép ta đưa ra các qui tắc chọn nghiệm đặc biệt trong một số nghiệm khả dĩ. Chọn nghiệm cho bài toán động học ngược phụ thuộc vào yếu tố toán học và công nghệ. Ví dụ, chuyển động của khớp quay có thể bị giới hạn nhỏ hơn một vòng quay 0 360 đưa đến việc không phải tất cả nghiệm của phương trình động học đều có được vị trí vật lý thật tương ứng trên tay máy. Ta sẽ giả sử rằng với vị trí và hướng cho trước sao cho phương trình (4.2) tồn tại nghiệm. Khi một nghiệm được xác định bởi phương trình toán học, nó phải được kiểm tra thêm xem có thỏa mãn mọi rằng buộc trên khoảng chuyển động khả dĩ của khớp. 4.2 Tách động học Dù bài toán động học ngược là rất khó, đối với tay máy 6 khớp, có ba khớp cuối đồng quy tại một điểm, ta có thể tách bài toán động học ngược thành hai bài toán đơn giản 8 hơn là động học ngược vị trí và động học ngược hướng. Cụ thể, đối với tay máy 6 bậc tự do với cổ tay khớp cầu, bài toán động học ngược có thể tách thành hai bài toán, đó là tìm vị trí giao điểm các trục cổ tay (tâm cổ tay), và sau đó tìm hướng của cổ tay. Ta biểu diễn (4.2) thành hai hệ phương trình như sau: OqqO RqqR = = ),,( ),,( 61 0 6 61 0 6   (4.5) trong đó O và R là hướng và vị trí của dụng cụ, được biểu diễn đối với hệ tọa độ cố định bên ngoài (world coordinate system). Ta phải giải bài toán trên đối với các ẩn 61 ,, qq  . Giả sử các trục khớp cầu 43 , zz và 5 z giao nhau tại c O và vì vậy gốc 4 O và 5 O gán theo qui tắc D-H sẽ luôn ở tâm cổ tay c O . Thường 3 O cũng ở tại c O , nhưng điều này không nhất thiết là như vậy đối với phương pháp này. Điểm quan trọng đối với động học ngược là chuyển động của ba khâu quanh 3 trục không làm thay đổi vị trí c O , và như vậy, vị trí tâm cổ tay là một hàm của chỉ ba biến khớp đầu tiên. Giải thuật: Đối với các tay máy thuộc loại này bài toán động học ngược có thể giải theo giải thuật sau: Bước 1: Tìm 321 ,, qqq sao cho tâm cổ tay c O có tọa độ cho trước như sau:           −= 1 0 0 6 0 RdOO c (4.6) Bước 2: dùng các biến khớp đã tìm được trong bước 1, thay vào 0 3 R . Bước 3: Tìm các góc Euler ứng với ma trận quay RRRRR T )()( 0 3 10 3 3 6 == − (4.7) 4.3 Động học ngược vị trí: giới thiệu một phương pháp hình học 9 Ta giới hạn phương pháp động học ngược vào phương pháp hình học vì hai lý do. Thứ nhất, hầu hết các thiết kế tay máy hiện hành thì đơn giản về mặt động học, thường năm loại cơ bản và có cổ tay khớp cầu. Thực tế, một phần là do khó khăn trong việc tìm giải thuật chung cho bài toán động học ngược; thứ hai, có ít kỹ thuật có thể xử lý bài toán động học ngược đối với các vị trí tùy ý. 4.3.1 Tay máy 3 khớp bản lề Hình 4.2: Tay máy 3 khớp quay (khỉu tay) Hình 4.3: Chiếu tâm cổ tay lên mặt phẳng x0-y0 Từ phép chiếu trên hình 4.2, ta có ),tan( 1 cc yxA= θ (4.8) xác định với mọi )0,0(),( ≠ cc yx và có duy nhất nghiệm sao cho 2222 sin;cos yx y yx x + = + = θθ (4.9) Nghiệm thứ 2 là ),tan( 1 cc yxA+= πθ , sẽ dẫn đến có nghiệm khác cho 32 , θθ Các nghiệm 1 θ đều hợp lệ ngoại trừ 0== cc yx . Trong trường hợp này, (4.8) không xác định và tay máy ở vị trí kỳ dị (singular configuration), trên hình 4.3 10 [...]... chuyển động của khâu 2 và 3 trong mặt phẳng (còn tiếp, trình bày sau) Hình 4.8: Chiếu lên mặt phẳng tạo bởi khâu 2 và 3 Hình 4.9: Bốn lời giải cho động học nghịch vị trí tay máy PUMA 13 Hình 4.2: Các động cơ cổ tay được bố trí cân bằng với hệ thống và truyền động bằng các ống đồng trục Hình 4.3: Cơ cấu cổ tay đồng quy tại một điểm (tâm cổ tay) 4.4 Động học nghịch hướng Ta đã dùng phương pháp hình học. .. nghịch hướng Ta đã dùng phương pháp hình học để giải bài toán vị trí nghịch Phương pháp này cho ta giá trị của ba biến khớp đầu tiên θ1 , θ 2 và θ 3 ứng với một vị trí tâm cổ tay cho trước Bài toán động học ngược hướng bây giờ trở thành bài toán tìm giá trị của ba biến khớp cuối cùng ứng với hướng cho trước đối với hệ tọa độ o3 x3 y3 z3 Đối với cổ tay khớp cầu, bài toán này trở thành bài toán tìm . bài toán động học ngược thành hai bài toán đơn giản 8 hơn là động học ngược vị trí và động học ngược hướng. Cụ thể, đối với tay máy 6 bậc tự do với cổ tay khớp cầu, bài toán động học ngược. định bởi phương trình toán học, nó phải được kiểm tra thêm xem có thỏa mãn mọi rằng buộc trên khoảng chuyển động khả dĩ của khớp. 4.2 Tách động học Dù bài toán động học ngược là rất khó, đối. trình động học thuận, thì vấn đề động học ngược có thể không có lời giải. Ngay cả khi tồn tại một lời giải, nó cũng có thể không là lời giải duy nhất. Hơn nữa, vì các phương trình động học thuận