Tính toán động học và mô phỏng 3D robot doc

Tính toán động học và mô phỏng 3D rôbốt Gryphon Đinh Văn Phong * , Đỗ Sanh * , Nguyễn Trọng Thuần * , Đỗ Đăng Khoa ** *Trờng Đại học Bách Khoa Hà nội, **Đề tài 3.312.01 Tóm tắt: Bài báo đề cập đến bài toán điều khiển động học của rôbốt và áp dụng vào rôbốt Gryphon. Nội dung của bài báo nhằm giải quyết bài toán thuận, bài toán ngợc và mô phỏng hoạt động của rô bốt trong không gian đồ hoạ ba chiều. Đồng thời trong bài báo cũng giới thiệu hớng nghiên cứu việc điều khiển rô bốt Gryphon bằng máy tính. 1. Mở đầu Trong bài báo này khảo sát bài toán điều khiển động học rô bốt, chủ yếu tập trung vào hai bài toán : Bài toán thuận : xác định vị trí điểm cuối và hớng tay kẹp mà tay máy đạt đợc khi qui luật thay đổi theo thời gian của các thông số định vị của tay máy là hàm đã biết. Bài toán này nhằm phục vụ bài toán xác định phạm vi hoạt động của tay máy, bài toán thuận trong động lực học tay máy. Bài toán ngợc : xác định qui luật thay đổi theo thời gian của tay máy để nó nắm bắt đợc vị trí đã cho của đối tợng theo một hớng định trớc của tay kẹp. Bài toán này nhằm phục vụ các bài toán điều khiển quĩ đạo, các bài toán điều khiển tối u Bài báo dới đây sẽ trình bày cơ sở lý thuyết của cả hai bài toán và minh hoạ qua việc áp dụng để mô phỏng một rô bốt cụ thể. 2. Cơ sở lý thuyết. 2.1 Công thức xác định vị trí Trong bài báo này chúng ta sẽ sử dụng phơng pháp ma trận Denavit-Hartenberg. gắn các hệ trục tọa độ vào từng khâu của tay máy (rôbốt). Nhờ đó mối quan hệ về toạ độ của cùng một điểm trên hai hệ trục kế tiếp nhau sẽ đợc xác định bằng một ma trận chuyển 4x4.,xem tr 53 [3]. Gọi j j -1 H là ma trận chuyển toạ độ một điểm từ hệ j ( Ox j y j z j ) về hệ j-1 ( Ox j-1 y j-1 z j-1 ), ta có: cos( ) cos( ).sin( ) sin( ).sin( ) .cos( ) sin( ) cos( ).cos( ) -sin( ).cos( ) .sin( ) 0sin() cos() 00 0 1 jjj jjkj jjj jjkj jjj a a d = j j-1 H (2.1) trong đó các tham số có ý nghĩa nh sau: j là góc quay trục x j-1 đến trục x j quanh trục z j-1 , d j là đoạn dịch trục x j-1 đến trục x j dọc trục z j-1 , a j là đoạn dịch trục z j-1 đến trục z j dọc trục x j-1 , j là góc quay trục z j-1 đến trục z j quanh trục x j-1 . Thông qua việc sử dụng liên tiếp các hệ toạ độ, ma trận chuyển hệ toạ độ gắn vào tay kẹp về hệ toạ độ gắn vào đế của rô bốt có dạng: ( ) ( ) ( ) ( ) 12 n qq q tay kep 1 2 n n de 0 1 n-1 0 H = H .H H = H q ( ) n 0 Hq có dạng: ( ) ( ) 1 Rq pq 0 (2.2) trong đó hệ toạ độ đế rô bốt kí hiệu là 0, hệ toạ độ tay kẹp rô bốt kí hiệu là n, q là ma trận nx1 của các toạ độ suy rộng,chúng là các thông số định vị của rôbốt. R(q) là ma trận 3x3 xác định hớng của tay kẹp, p(q) là vectơ 3x1 xác định vị trí đầu bàn kẹp so với hệ đế. Ba cột của ma trận R tơng ứng với hớng của ba vectơ đơn vị trên hệ gắn với tay kẹp so với hệ đế rôbốt (hệ toạ độ nền). Vị trí của một điểm P thuộc tay kẹp đợc xác định theo công thức : ( ) 0 n pp r=H qr (2.3) trong đó p r là vectơ định vị điểm P thuộc tay kẹp so với hệ toạ độ nền, p r là vectơ định vị điểm P trong hệ toạ độ gắn vào tay kẹp rôbốt. 2.2 Công thức xác định vận tốc. Từ công thức (2.3) ta dễ dàng xác định đợc vận tốc điểm P gắn vào tay kẹp rôbốt so với hệ toạ độ nền : pp np 0 v=r=H.r & & (2.4) Để tiến hành điều khiển hoạt động của từng khâu rôbốt, ta tiến hành biểu diễn ma trận n 0 H & dới dạng sau : n n(i) 0i i=1 H= Hq & & (2.5) trong đó H (i) là ma trận ứng với khớp i và chỉ phụ thuộc vào biến khớp, p r & là véc tơ vận tốc của điểm cuối tay kẹp so với hệ toạ độ nền, n 0 H & là ma trận đạo hàm của ma trận Denavit- Khâu j -2 Khâu j -1 Khâu j Khâu j +1 z j-2 d j z j-1 y j-1 x j-1 j x j y j z j a j a j-1 x j Hình 1 z j Khớp j -1 Khớp j Khớp j +1 j Hartenberg giữa hệ toạ độ nền với hệ toạ độ gắn vào tay kẹp, i q & là vận tốc của chuyển động tại khớp thứ i. 3. Khảo sát động học của rô bốt Gryphon. 3.1 Giới thiệu mô hình và nguyên tắc hoạt động. Rô bốt Gryphon do hãng FeedBack của Anh sản xuất phục vụ cho mục đích nghiên cứu. Đây là một rôbốt năm trục kèm theo bàn kẹp nh hình 2 Trục hông (waist axis). Trục vai (shoulder axis). Trục khuỷu tay (elbow axis). Trục cổ tay (tool pitch axis). Trục cổ tay (tool roll axis). Bàn kẹp (gripper). Hình 2 Những u điểm nổi bật của rôbốt là chuyển động nhanh, chính xác và mềm mại. Rôbốt đợc điều khiển bởi bốn vi xử lý cho phép điều khiển đặt vật chính xác. Mỗi trục của rôbốt đợc điều khiển bởi một động cơ bớc với bộ mã hoá phản hồi. Trong bộ điều khiển, một vi xử lý sẽ giám sát vị trí của các trục. Hai cái khác sẽ quản lý các động cơ và cái còn lại sẽ giám sát cả ba cái trên đồng thời làm nhiệm vụ giao tiếp với máy chủ. 3.2 Công thức xác định vị trí Khi áp dụng phơng pháp Denavit-Hartenberg gắn các hệ trục toạ độ vào các khâu x 0 y 0 z 0 a 1 1 z 1 x 1 y 1 a 2 a 4 a 5 a 6 z 5 y 5 x 5 a 3 x 4 y 4 z 4 z 3 x 3 y 3 z 2 4 5 2 x 2 y 2 3 Hình 3 ta thu đợc sơ đồ động học của rôbốt Gryphon nh hình 3. Bảng tham số Denavit-Hartenberg: Theo thuật toán Denavit-Hartenberg ta có bảng tham số Denavit-Hartenberg ứng với sơ đồ động học trên : Trục D a 1 1 a 1 /2 a 2 2 2 00 a 4 3 3 00 a 5 4 4 -a 3 /2 0 5 5 a 6 0 0 Xác định các ma trận chuyền. Đây là các ma trận chuyển thành chuyển các hệ toạ độ trên sơ đồ động học (hình 3). 1121 1121 1 0 0 01 0 00 0 1 CSaC SCaS a = 1 0 H , 22 42 22 42 0 0 0010 0001 CS aC SC aS = 2 1 H , 33 53 33 53 0 0 0010 0001 CS aC SC aS = 3 2 H , 44 44 3 00 00 01 0 00 0 1 CS SC a = 4 3 H , 55 55 6 00 00 001 0001 CS SC a = 5 4 H (3.1) Ma trận chuyển toàn hệ ta y ke p de H là ma trận truyền thuần nhất giữa hệ toạ độ nền và hệ toạ độ gắn vào tay kẹp. taykep 1 2 3 4 5 de 0 1 2 3 4 H =H.H.H.H.H ( ) () 1 234 5 1 5 1 234 5 1 5 1 234 1 234 6 23 5 2 4 2 1 3 1 234 5 1 5 1 234 5 1 5 1 234 1 234 6 23 5 2 4 2 1 3 234 5 234 5 234 234 6 23 5 2 4 1 000 1 CC C SS CC S SC CS C S a C a C a a Sa SC C CS SC S CC SS S S a C a Ca a Ca SC SS C Ca Sa Sa a ++ +++ ++++ = +++ taykep de H (3.2). Để viết đơn giản ta sử dụng các kí hiệu sau: C 1 =cos( 1 ), C 234 =cos( 2 + 3 + 4 ), S 1 =sin( 1 ), S 234 =sin( 2 + 3 + 4 ). Ma trận R 1 234 5 1 5 1 234 5 1 5 1 234 1 234 5 1 5 1 234 5 1 5 1 234 234 5 234 5 234 CC C SS CC S SC CS SC C CS SC S CC SS SC SS C + + = R (3.3) Vectơ p () () 1 234 6 23 5 2 4 2 1 3 1 234 6 23 5 2 4 2 1 3 234 6 23 5 2 4 1 CS a Ca Ca a Sa SSa Ca Ca a Ca Ca Sa Sa a +++ =++++ +++ p (3.4) Xem [1]. 3.3 Bài toán thuận Ta có thể xác định vị trí và vận tốc nh sau: Trong bài toán thuận ( ) n 0 Hq từ (2.3) là hàm đã biết của thời gian, do đó ta tính đợc ( ) t= pp rr Trong công thức (2.5) vì ( ) ( ) ,tt==qq qq && do đó (i) H là hàm đã biết của t, từ (2.4) ta tính đợc ( ) t pp v=v . 3.4 Bài toán ngợc 3.4.1 Bài toán vị trí Mục đích bài toán nhằm xác định các góc (1,5) i i = để tay máy nắm bắt đợc đối tợng tại một vị trí đã cho và theo một hớng xác định. Do đó dữ liệu vào là vị trí đầu bàn kẹp T xyz ppp = T p và hớng tay kẹp R. Từ các công thức (3.3) và (3.4) ta nhận thấy rằng cột thứ 3 của ma trận R và p không phụ thuộc vào 5 mà chỉ phụ thuộc vào các góc 1 , 2 , 3 , 4 do đó các góc này sẽ đợc xác định theo vectơ cấu hình tay kẹp : w=[p x , p y , p z , R 13 , R 23 , R 33 ]. Nói cách khác ta thu đợc hệ phơng trình sau: ( ) () 1 12346 235 24 2 13 2 1 234 6 23 5 2 4 2 1 3 3 234 6 23 5 2 4 1 413 1234 523 1 234 633 234 w w w w w w w x y z p CS a Ca Ca a Sa p SSa Ca Ca a Ca p Ca Sa Sa a R CS R SS R C +++ ++++ +++ === (3.5) thực hiện các phép biến đổi giải tích, ta thu đợc kết quả sau: Góc quay ở khớp hông 1 đợc tính nh sau Đặt 222 123 wwba= + 1213123 atan2(w b-w a ,w b+w a )q= (3.6) Góc quay ở khớp khuỷu 3 đựoc tính nh sau: ( ) ( ) 1 1 13 1 2 13 1 2346 2 11 21 2346 2 2312346 ww ww w bSaCCaSSaa CSSaa baCa =+ + =+ =+ với 234 1 4 1 5 6 atan2( ,-w )qCwSw=+ 2222 1245 3 45 2 bbaa q aa + = (3.7) Góc quay ở khớp vai 2 đợc tính nh sau: ( ) ( ) ( ) ( ) ( ) 224531531453253 atan2 ,qbaaCbaSbaaCbaS=+ ++ (3.8) Góc quay ở khớp pitch 4 đợc tính nh sau: 423423 qq qq= (3.9) Góc quay ở khớp roll 5 đợc tính nh sau: ( ) 5 111 121 112 122 atan2 ,qSRCRSRCR= (3.10) Xem [1]. 3.4.2 Bài toán vận tốc. Khi cần di chuyển vật kẹp với một vận tốc cho trớc, ta phải tính vận tốc quay tại các khớp nhờ các công thức (2.4) và (2.5). Muốn vậy ta phải tính đợc các ma trận (i) H trong (2.5). Đối với rôbốt Gryphon ta nhận đợc: 5 234 1 1 5 1 5 234 1 5 1 234 1 6 234 5 23 4 2 2 1 3 1 5 234 1 5 1 5 234 1 5 1 234 1 6 234 5 23 4 2 2 1 3 () () 000 0 000 0 CC S CS SSC CC SS S aS aC aC a Ca CCC SS CSC SC CC C a S aC aC a Sa + + +++ + + +++ = (1) H 5 234 1 1 5 234 1 234 1 6 234 5 23 4 2 1 5 234 1 5 234 1 234 1 6 234 5 23 4 2 5 234 5 234 234 234 6 23 5 2 4 () () 000 0 CS C CSS CC C aC aS aS SCS SS S SC S aC aS a S CC SC S S a C a Ca = ++ (2) H 5 234 1 1 5 234 1 234 1 6 234 5 23 1 5 234 1 5 234 1 234 1 6 234 5 23 5 234 5 234 234 234 6 23 5 () () 000 0 CS C CSS CC C aC aS SCS SS S SC S aC aS CC SC S S a C a = + (3) H 5 234 1 1 5 234 1 234 1 6 234 1 5 234 1 5 234 1 234 1 6 234 5 234 5 234 234 234 6 0000 CS C CSS CC CaC SCS SS S SC SaC CC SC S S a = (4) H 5 234 1 1 5 1 5 234 1 5 15234 15 15234 15 5 234 5 234 00 00 00 0000 SC C SC CCC SS SSC CC SCC CS SS CS + + = (2) H (3.11) Sử dụng (3.11) có thể giải quyết bài toán điều khiển vận tốc. Tuy nhiên vấn đề này sẽ đề cập trong một công trình khác. 4. Chơng trình mô phỏng. 4.1 Giới thiệu chơng trình. Đây là chơng trình máy tính viết bằng ngôn ngữ C++ sử dụng công cụ lập trình Visual C++ 6.0 có sự hỗ trợ của th viện đồ hoạ OpenGL. Chơng trình này làm nhiệm vụ giải bài toán động học thuận và bài toán động học ngợc của rôbốt Gryphon và thể hiện chuyển động của rôbốt theo các bộ số liệu góc khớp. 4.2 Th viện đồ hoạ OpenGL. The Open Graphics Library (OpenGL) là một th viện đồ hoạ tốc độ cao, độc lập với các hệ thống giao diện của hệ điều hành. Th viện này đợc hãng Silicon Graphics Inc phát triển cho các workstation đồ hoạ tốc độ cao từ năm 1982 dới cái tên IRIS GL. Đây là một th viện đồ hoạ có thể triển khai trên nhiều hệ thống khác nhau nh : Microsoft Windows 95/98/NT/2000. Đồng thời các ứng dụng đồ hoạ OpenGL cũng có thể viết trên nhiều ngôn ngữ lập trình khác nhau nh : C/C++, Fortran, Ada, Java Về cơ bản OpenGL là một th viện gồm tập hợp của khoảng 150 hàm hỗ trợ các thao tác nh sau, xem [2], [8] : Thể hiện các đối tợng cơ bản nh điểm, đờng, đa giác, và từ đó có thể tạo ra những đối tợng đồ hoạ phức tạp hơn nh mặt cầu, mặt nón, mặt hình trụ hoặc các đối tợng do lập trình viên tạo ra; Quan sát đối tợng : các đối tợng sau khi đợc vẽ ra có thể đợc quan sát từ những góc nhìn khác nhau thông qua các phép biến hình (transformation); Định màu sắc : các đối tợng có thể đợc thể hiện với màu sắc bằng nhiều cách chỉ định màu khác nhau: RGBA hay color-index ; Sử dụng ánh sáng : ánh sáng có thể đợc sử dụng để tạo ra những cảnh "thật" từ những đối tợng đã có. OpenGL cho phép sử dụng nhiều loại nguồn sáng khác nhau nh phát xạ (emitted), bao quanh(ambient), phân tán (diffuse) và phản chiếu (specular); Các kỹ thuật tăng chất lợng hiện thị ảnh nh chống răng ca (anti-aliasing), trộn màu (blending), sơng khói trong ảnh (fog) ; Thao tác trên các ảnh bitmap : lập trình viên có thể "dán" các ảnh của cảnh thật lên trên bề mặt các đối tợng tạo ra bằng OpenGL ; 4.3 Giao diện chính của chơng trình mô phỏng. Phần bên trái của giao diện là các nút điều khiển từng khớp của rô bốt, phần bên phải là thể hiện hình ảnh ba chiều của rô bốt tơng ứng với sự điều khiển 4.4 Các menu điều khiển của chơng trình. 4.5 Các thanh công cụ của chơng trình. Thanh công cụ thay đổi vị trí vật Thanh công cụ thay đổi hớng nhìn vật B ậ t tắt thanh côn g c ụ chuẩn B ậ t tắt thanh tr ạ n g thái B ậ t tắt thanh côn g c ụ tha y đổi v ị trí v ật B ậ t tắt thanh côn g c ụ tha y đổi hớn g nhin v ật B ậ t tắt thanh côn g c ụ đ ặ t chế đ ộ Q ua y v ậ t theo chiều dơn g tr ụ c x Q ua y v ậ t theo chiều âm tr ụ c x Q ua y v ậ t theo chiều dơn g tr ụ c y Q ua y v ậ t theo chiều âm tr ụ c y Q ua y v ậ t theo chiều dơn g tr ụ c z Q ua y v ậ t theo chiều âm tr ụ c z T ị nh tiến v ậ t san g bên trái T ị nh tiến v ậ t san g bên p hải T ị nh tiến v ậ t xuốn g dới T ị nh tiến v ậ t lên trên Thu nhỏ v ật Phón g to v ật Đa v ậ t về v ị trí ban đầu Thôn g tin về tác g iả Đ ặ t g iá tr ị bớc g óc cho từn g khớ p Đ ặ t hi ệ u ứn g ánh sán g Thanh công cụ thay chế độ điều khiển 5. Kết luận. Bài báo đã trình bày phơng pháp khảo sát bài toán điều khiển động học rôbốt và áp dụng vào một ví dụ cụ thể ,rôbốt Gryphon cùng với một chơng trình máy tính mô phỏng sinh động dễ dùng. Bài báo cũng đặt cơ sở cho việc tính toán động lực học rôbốt. Công trình này đã đợc tài trợ của Hội đồng khoa học tự nhiên của Việt nam. Summary: Kinematic calculations and 3D simulation of the Gryphon robot. In the paper, a method of investigating the problem of kinematic controlling a robot is presented. The results obtained are applied to the Gryphon robot and used to construct a 3D simulation software. The results are also basic to the dynamic simulation. Tài liệu tham khảo 1. Đồ án tôt nghiệp :Tính toán động học và mô phỏng 3D rô bốt Gryphon, Đỗ Đăng Khoa. 2. Báo cáo nội bộ: OpenGL và ứng dụng, Phan Mạnh Dần và nhóm nghiên cứu. 3. Fundamentals of Robotíc Analysis and Control , Robert. J. Schilling. 4. Lập trình Window dùng Visual C++ 5.0 và MFC, Dơng Quang Thiện. 5. Computer Aided Kinematics and Dynamics of Mechanical Systems, Hang. EJ, Volume 1:Basic method, Alyn and Bacon, New York (1989). Hớn g nhìn từ trên xuốn g Hớn g nhìn từ dới lên Hớn g nhìn từ trái san g Hớn g nhìn từ p hải san g Hớn g nhìn từ p hía trớc Hớn g nhìn từ p hía sau Hớn g nhìn tâ y nam Hớn g nhìn đôn g nam Hớn g nhìn đôn g bắc Hớn g nhìn tâ y bắc Dừn g chu y ển độn g theo q uá trình Bắt đầu chu y ển độn g theo q uá trinh Chế độ động học thuận rôbốt Chế độ độn g học n g ợc rôbốt Thiết kế q uĩ đạo rôbố t 6. Computer Aided Analysis of Mechanical Systems, Nikravesh P.E, Printice-Hall, New Jersey. 7. Dynamics of Multibody Systems, Shabana, Wiley, New York. 8. OpenGL Programming Guide , Mason Woo, Jackie Neider, Tom Davis. . and 3D simulation of the Gryphon robot. In the paper, a method of investigating the problem of kinematic controlling a robot is presented. The results obtained are applied to the Gryphon robot. used to construct a 3D simulation software. The results are also basic to the dynamic simulation. Tài liệu tham khảo 1. Đồ án tôt nghiệp :Tính toán động học và mô phỏng 3D rô bốt Gryphon,. Tính toán động học và mô phỏng 3D rôbốt Gryphon Đinh Văn Phong * , Đỗ Sanh * , Nguyễn Trọng Thuần * , Đỗ Đăng Khoa **