Phân tích động học thuận Robot bằng phương pháp ma trận DenavitHartenberg

Đồ án tin học trong cơ học cos 0 AYn ( ) = sin 0 cos sin AZn ( ) = 0 0 Nguyễn Hữu Dĩnh lớp Cơtin-A 0 sin 1 0 0 cos 0 0 sin cos 0 0 0 0 1 0 0 0 0 1 0 0 0 1 Tơng tự nh đối với phép quay Euler, ta cũng có ma trận chuyển hệ qui chiếu nh sau CC SS R ( , , ) = S 0 ( CSC SC ) ( CSC + SC ) CC 0 CC 0 ( SSSS + CC ) ( SSSS CC ) 0 0 0 1 kí hiệu: Cx = cos(x), Sx = sin(x); IV. Hệ phơng trình động học Robot. 4.1. Đặt vấn đề. Robot là một chuỗi các khâu, chúng đợc nối với nhau bằng các khớp quay, khớp tịnh tiến. Điểm tác động cuối cùng E thực hiện chuyển động dễ dàng linh hoạt theo một quĩ đạo nào đó. Tuy nhiên điểm tác động cuối không những phải bám theo một quĩ đạo nhất định mà còn phải giữ đợc một hớng nhát định để đảm bảo việc tiếp cận với đối tợng công tác. Vậy điều khiển Robot là điều khiển điểm tác động cuối E di chuyển theo một quĩ đạo cho trớc và đảm bảo hớng của điểm tác động cuối. Trong giới hạn của đồ án này, chúng ta chỉ đi sâu vào việc thành lập hệ phơng trình động học của robot khi biết các biến khớp, ta sẽ xác định đợc vị trí và hớng của điểm tác động cuối (trạng thái của điểm tác động cuối). 4.2. Ma trận quan hệ. Chọn hệ toạ độ cố định gắn liền với giá đỡ và các hệ hệ toạ động gắn với từng khâu động. Ký hiệu các hệ toạ độ này từ 0 đến n, kể từ giá trị cố định trở đi. Một điểm bất kỳ nào đó trong không gian đợc xác định trong hệ toạ độ thi bằng bán kính véctơ ri và trong hệ toạ độ cố định x 0, y0, z0 đợc xác định bằng bán kính véctơ r0. r0 = A1 . A2 ... Ai ri (4.1) hoặc: r0 = Ti .ri (4.2) với Ti = A1 . A2 ... Ai ; i = 1,2,...., n (4.3) 11 Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A Trong đó ma trận Ai mô tả vị trí và hớng của khâu đầu tiên, ma trận A2 mô tả vị trí hớng của khâu thứ hai so với khâu đầu; ma trận Ai mô tả vị trí hớng của khâu thứ i so với khâu thứ i-1. Nh vậy, tích của các ma trận Ai là ma trận Ti mô tả vị trí và hớng của khâu thứ i so với gía cố định. Thờng ký hiệu ma trận T với 2 chỉ số: trên và dới. Chỉ số dới để chỉ khâu đang xét còn chỉ số trên để chỉ toạ độ đợc dùng để đối chiếu. ví dụ biểu thức (4.3) có thể viết lại là: (4.4) 1 Ti = 0 Ti = A1 .Ti với: 1 (4.5) Ti = A2 . A3 .... Ai là ma trận mô tả vị trí và hớng của khâu thứ i so với khâu thứ nhất. Trong ký hiệu thờng bỏ qua chỉ số trên nếu chỉ số đó bằng 0. 4.3. Bộ thông số động học Denavit-Hartenberg và ma trận Denavit-Hartenberg. a. Các tham số động học Denavit-Hartenberg. Nh đã giới hạn, trong đồ án này chỉ xét hệ các vật rắn nối ghép với nhau bằng các khớp quay và các khớp tịnh tiến. Khi đó quan hệ giữa hai khâu kkế tiếp nhau có thể xác định bởi hai tham số khớp, nh hình vẽ dới đây: Trên hình trên khâu thứ i -1 nối với khâu i bằng khớp thứ i. Trục z i-1 đợc chọn là trục khớp của khớp thứ i. Tham số thứ nhất i , đợc gọi là góc khớp, là góc quay trục xi-1 quanh trục zi-1 đến trục xi// xi. Tham số thứ hai d i, là khoảng cách giữa hai trục x i và trục xi. Nếu khớp i là khớp quay thì i là biến còn di là hằng số. Nếu i là khớp tịnh tiến thì khoảng cách di là biến còn i là hằng số. Đối với các robot công nghiệp Denavit-Hertenberg đa ra cách chọn hệ trục toạ độ nh sau: 1. Trục zi-1đợc chọn dọc theo hớng của trục khớp động thứ i. 2. Trục xi-1 đợc chọn dọc theo đờng vuông góc chung của hai trục z i-2 và zi-1, hớng đi từ trục zi-2 đến zi-1. nếu trục zi-1 cắt trục zi-2 thì hớng của trục xi-1 đợc chọn tuỳ ý. 12 Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A 3. Gốc toạ độ đợc chọn trụcại giao điểm trục xi-1 và trục zi-1. 4. Trục yi-1 đợc chọn sao cho hệ toạ độ (Oxyz)i-1 là hệ qui chiếu thuận. Với cách chọn hệ toạ độ nh trên, nhiều khi hệ toạ độ khâu (Oxyz) i-1 không đợc xác định một cách duy nhất. Vì vậy ta cần có một số bổ sung thích hợp nh sau: 5. Đối với hệ toạ độ (Oxyz)0 theo qui ớc trên thì chỉ chọn đợc trục z0, ta có thể chọn trục x0 tuỳ ý. 6. Đối với hệ toạ độ (Oxyz) n do không co khớp thứ n+1, nên theo qui ớc trên ta không xác định đợc trục zn. Trục zn không đựoc xác định một cách duy nhất, trong khi trục xn lại đợc chọn theo pháp tuyến trục z n-1. Ngoài ra có thể chọn tuỳ ý soa cho hợp lý. 7. Khi hai trục zi-2 và trục zi-1 song song với nhau, giữa hai trục có nhiều đờng pháp tuyến chung, ta có thể chọn trục xi-1 theo hớng pháp tuyến chung nào cũng đợc. 8. Khi khớp thứ i là khớp tịnh tiến, về nguyên tắc ta có thể chọn trục z i-1 một cách tuỳ ý. Tuy nhiên, trong nhièu trờng hợp ngời ta thờng chọn trục zi-1 dọc theo trục của khớp tịnh tiến này. Vị trí của hệ toạ độ khâu (Oxyz) i đối với hệ toạ độ khâu (Oxyz) i-1 đợc xác định bởi bốn tham số Denavit Hertenberg i , di, ai, i nh sau: i : góc quay trục xi-1 quanh trục zi-1 đến trục xi (xi // xi). di: dịch chuyển tịnh tiến dọc theo trục z i-1 đến gốc toạ độ Oi-1 chuyển đến Oi, giao điểm trục xi và trục zi-1. ai: dịch chuyển tịnh tiến dọc theo trục xi để điểm Oi chuyển đến điểm Oi. i : góc quay quanh trục xi sao cho trục zi-1 chuyển đến trục zi. Dới đây là hình minh hoạ cách chọn các hệ toạ độ theo ý tởng của Denavit Hertenberg: 13 Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A Trong bốn tham số trên, các tham số ai 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ự ghép nối giữa các khâu thứ i và khâu thứ i1. Hai tham số còn lại là di và i , một là hằng số, một là biến phụ thuộc vào khớp i là khớp tịnh tiến hay khớp quay. Khi khớp i là tịnh tiến thì d i là biến số, còn i là hằng số. Khi khớp i là quay thì i là biến số, còn di là hằng số. b. Ma trận Denavit-Hartenberg. Ta có thể chuyển toạ độ khâu (Oxyz) i-1 sang hệ toạ độ khâu (Oxyz) i bằng bốn phép biến đổi cơ bản nh sau: Quay quanh trục zi-1 một góc i . Dịch chuyển tịnh tiến dọc trục zi-1 một đoạn di. 14 Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A Dịch chuyển tịnh tiến dọc trục xi một đoạn ai. Quay quanh trục xi một góc i . Ma trận của phép biến đổi, kí hiệu là Trục T i, là tích của bốn ma trận biến đổi có bản và có dạng nh sau: cos i sin i RZ ( i ) = 0 0 sin i 0 1 0 cos i R X ( i ) = 0 sin i 0 0 0 sin i cos i 0 cos i 0 0 0 1 0 0 0 , Tz ( d i ) = 0 1 0 0 0 1 0 0 0 , 0 1 1 0 Tx ( a i ) = 0 0 0 1 0 0 0 1 0 0 0 0 0 0 1 di 0 1 0 ai 0 0 1 0 0 1 Khi đó ta có: Ti = RZ ( i ) ì Tz ( d i ) ì Tx ( ai ) ì R X ( i ) cos i sin i Ti = 0 0 sin i cos i cos i cos i sin i 0 sin i sin i cos i sin i cos i 0 a i cos i ai sin i di 1 Định nghĩa: Ma trận Ti đợc xác định bởi công thức trên đợc gọi là ma trận Denavit-Hartenberg. Đó chính à ma trân côsin chỉ phơng của hệ qui chiếu (Oxyz)i-1 đối với hệ qui chiếu (Oxyz)i . Chính xác hơn ta phải ký hiệu ma trận này bằng Tii-1 , để đơn giản ta sử dụng ký hiệu Ti với nghĩa Tii-1, còn Ai đợc dùng với nghĩa Ti0. 4.4. Phơng trình xác định vị trí khâu thao tác(bàn kẹp) của robot. áp dụng liên tục phép biến đổi đối với hai khâu liên tiếp đối với robot n khâu ta đợc phơng trình xác định vị trí khâu thao tác (bàn kẹp) của robot. An = Tn ( 0) ( 0) ( 1) ( 2) = T1 T2 T3 ...Tn ( n 1) = T1T2T3 ...Tn . Phần II: Phần lập trình Trong bản đồ án tin này ta sử dụng phần mềm Maple để thực hiện việc xây dựng chơng trình tính. Trong chơng trình này các thông số đầu vào là các bộ thông số Denavit-Hertenberg và máy tính sẽ tự động tính toán và cho ra kết quả là các phơng trình động học của robot. Dới đây là chơng trình tính toán -Xây dựng module giải bài toán động học thuận của Robot công nghiệp bằng Maple Robot:=module() 15 Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A #option package,''copyrigh(C) noname''; export DH,vantoc,giatoc; #Tinh ma tran cua khau thu i DH:=proc(x,y,z,t) local a11,a12,a13,a14, a21,a22,a23,a24, a31,a32,a33,a34, a41,a42,a43,a44, A; a11:=cos(x); a12:=-sin(x)*cos(z); a14:=t*cos(x); a13:=sin(x)*sin(z); a21:=sin(x); a22:=cos(x)*cos(z); a23:=-cos(x)*sin(z); a24:=t*sin(x); a31:=0; a32:=sin(z); a33:=cos(z); a34:=y; a41:=0; a42:=0; a43:=0; a44:=0; A:=array([[a11,a12,a13,a14],[a21,a22,a23,a24],[a31,a32,a33,a34], [a41,a42,a43,a44]]); print("ma tran Denavit-Hartenberg la:"); print(A); RETURN(A); end: # ham tinh van toc cua diem thao tac vantoc:=proc(A) local i,j,px,py,pz,c1,c2,c3,Vp; a:=array(1..4,1..4); for i to 4 do for j to 4 do 16