Thiết kế quỹ đạo chuyển động cho robot

1 LỜI NÓI ĐẦU 3 CHƯƠNG I . GIẢI BÀI TOÁN ĐỘNG HỌC 4 1.1. Giải bài toán động học thuận 4 1.1.1. Cơ sở lý thuyết 4 1.1.2. Thiết lập phương trình động học thuận cho robot RRR. 9 a. Tìm các ma trận biến đổi 9 b. Xác định vị trí và hướng của bàn kẹp 10 c. Xác định vận tốc của điểm tác động cuối so với hệ cố định 11 d. Vận tốc góc của mỗi khâu so với hệ cố định 11 e. Các đồ thị thể hiện vị trí, vận tốc điểm tác động cuối E 11 1.2. Giải bài toán động học ngƣợc 12 1.2.1. Giải bài toán động học ngược bằng phương pháp giải tích 13 a. Cơ sở lý thuyết 13 b. Áp dụng giải bài toán ngược cho robot RRR 14 1.2.2. Giải bài toán ngược bằng phương pháp số 17 a. Cơ sở lý thuyết 17 b. Áp dụng giải bài toán cho robot RRR 19 CHƯƠNG II. GIẢI BÀI TOÁN TĨNH HỌC ROBOT 21 2.1. Cơ sở lý thuyết 21 2.2. Áp dụng cho Robot RRR 22 CHƯƠNG III. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CHO ROBOT 27 3.1 Giới thiệu và cơ sở thiết kế quỹ đạo 27 3.2. Tính toán thiết kế quỹ đạo chuyển động 27 3.2.1 Thiết kế quỹ đạo trong không gian khớp 27 3.2.2. Thiết kế quỹ đạo trong không gian làm việc 29 a. Quỹ đạo của điểm tác động cuối theo đường thẳng từ A đến B trong tc (s) 30 b. Thiết kế quỹ đạo điểm tác động tác động cuối di chuyển theo đường tròn từ A đến B trong  ct s lấy AB làm đường kính. 32 CHƯƠNG IV. ĐỘNG LỰC HỌC ROBOT 34 2 4.1. Cơ sở lý thuyết 34 4.2. Áp dụng tìm phƣơng trình vi phân cho Robot RRR 36 4.2.2. Bảng tham số động học 36 4.2.3. Các phương trình vi phân 36 III. ĐIỀU KHIỂN ROBOT 37 5.1. Điều khiển phản hồi và điều khiển vòng kín. 37 5.2. Thiết kế bộ điều khiển PID 38 5.3. Thiết kế bộ điều khiển trong không gian khớp 40 KẾT LUẬN 44 TÀI LIỆU THAM KHẢO 45 3 LỜI NÓI ĐẦU 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. Đây là những bước cơ sở ban đầu hết sức quan trọng trước khi thiết kế Robot. Với sự phát triển như vũ bão của công nghệ thông tin như ngày này, rất nhiều các lĩnh vực trong cơ khí đã tận dụng được sự phát triển này để tạo ra những bước nhảy vọt, rong đó có công nghiệp Robot. Trên cơ sở đó, môn học Robotics đã mang lại cho sinh viên những kiến thức vô cùng quan trọng cho sinh viên chúng em. Bên cạnh đó, nó cũng tạo ra một cơ hội để sinh viên được tiếp cận với những phần mềm tính toán, mô phỏng phổ biến trên thế giới hiện nay như Maple và Matlab. Để thực hiện được bài tập lớn này, em xin chân thành cảm ơn thầy giáo, PGS.TS Phan Bùi Khôi đã tận tình, chu đáo dạy học trên lớp. Em xin chân thành cảm ơn thầy. 4 CHƢƠNG I . GIẢI BÀI TOÁN ĐỘNG HỌC 1.1. Giải bài toán động học thuận 1.1.1. Cơ sở lý thuyết Vị trí mỗi khâu trong không gian được xác định bởi vị trí một điểm định vị và hướng của khâu đó đối với một hệ quy chiếu đã chọn. Điểm định vị là một điểm xác định nào đó của khâu, thông thường trong động lực học ta hay lấy khối tâm của khâu đó làm điểm định vị. Hướng của khâu được xác định bằng ma trận cosin chỉ hướng hoặc bằng các tọa độ suy rộng xác địnhvị trí của vật rắn quay quanh một điểm. Động học robot nghiên cứu chuyển động của các khâu của robot về phương diện hình học, không quan tâm đến các lực và momen gây ra chuyển động.Động học robot là bài toán qua trọng phục vụ tính toán và thiết kế robot.Nhiệm vụ chủ yếu của bài toán động học thuận là xác định vị trí và hương của bàn kẹp dưới dạng hàm của biến khớp. Các phương pháp ma trận 4x4 và các phương pháp ma trận 3x3 hay được sử dụng trong phân tích động học robot. Hai phương pháp ma trận 4x4 phổ biến là phương pháp ma trận Denavit-Hartenberg và phương pháp ma trận Craig. Trong báo cáo này chúng em trình bày và áp dụng phương pháp ma trận Denavit-Hartenberg để tính toán động học robot. Giải bài toán động học thuận robot công nghiệp bằng phương pháp ma trận Denavit- Hartenberg Cách xác định các trục cuả hệ tọa độ khớp. Đối với robot công nghiệp ,Denavit-Hartenberg đã đưa ra cách chọn các hệ trục tọa độ có gốc tại khớp thứ i như sau: Trục 1i z  được chọn dọc theo hướng của trục khớp động thứ i. Trục 1i x  được chọn dọc theo đường vuông góc chung của 2 trục 2i z  và 1i z  hương đi từ trục 2i z  sang trục 1i z  . Nếu trục 1i z  cắt trục 2i z  thì hướng của trục 1i x  được chọn tùy ý miễn là vuông góc với trục 1i z  .Khi 2 trục 2i z  và 1i z  song song với nhau, giữa 2 trục này có nhiều đường vuông góc chung , ta có thể chọn trục 1i x  hướng theo pháp tuyến chung nào cũng được. Gốc tọa độ 1i O  được chọn tại giao điểm cuả trục 1i x  và trục 1i z  . Trục 1i y  được chọn sao cho hệ 1 ( x ) i O yz  là hệ quy chiếu thuận. 5 Hệ tọa độ 1 ( x ) i O yz  được xác định như trên trong một số tài liệu được quy ước là hệ tọa độ khớp. Chú ý: Với cách chọn hệ tọa độ như trên , đôi khi hệ tọa độ khớp 1 ( x ) i O yz  không được một cách duy nhất. vì vậy, ta có một số bổ sung thích hợp như sau. Đối với hệ tọa độ 0 ( x )O yz theo quy ước trên ta mới chỉ chọn được trục 0 z , còn trục 0 x chưa có trong quy ước trên.Ta có thể chọn trục 0 x một cách tùy ý, miễn là 0 x vuông góc với 0 z . Đối với hệ tọa độ ( x ) n O yz , do không có khớp (n+1) nên theo quy ước trên ta không xác định n z . Trục n z không được xác định duy nhất, trong khi trục n x lại được chọn theo đường pháp tuyến của trục 1n z  . Trong trường hợp này, nếu khớp n là khớp quay, ta có thể chọn trục n z song song trục 1n z  . Ngòai ra ta có thể chọn tùy ý sao cho hợp lý. Khi khớp thứ i là tịnh tiến, về nguyên tắc ta có thể chọn trục 1i z  một cách tùy ý. Tuy nhiên trong nhiều trường hợp người ta thường chọn trục 1i z  dọc theo trục cuả khớp tịnh tiến này. Hình 1.1.diễn các thông số Denavit-Hartenberg giữa các trục hệ tọa độ Các tham số động học Denavit-Hartenberg 6 Vị trí của hệ toạ độ khớp (Oxyz)i đối với hệ tọa độ khớp (Oxyz)i-1 được 4 tham số Denavit-haartenderg di , 𝜃𝑖 , ai, 𝛼𝑖 như sau: i d dịch chuyển tịnh tiến dọc theo trục 1i z  để gốc tọa độ 1i O  chuyển đến ' i O giao điểm của trục i x và trục 1i z  . i θ : góc quay quanh trục 1i z  để trục 1i x  chuyển đến trục ' i x ( ' i x // i x ). ia : dịch chuyển tịnh tiến theo dọc trục i x để điểm ' i O chuyển đến điểm i O . iα : góc quay quanh trục i x sao cho trục ' 1i z  ( ' 1i z  // 1i z  ). Chuyển đến trục i z . Do hệ trục tọa độ 1 ( xyz) i O  gắn liền vào khâu thứ i-1 , còn hệ trục tọa độ ( xyz) i O gắn liền vào khâu thứ i , cho nên vị trí cuả khâu thứ i đối với khâu thứ i-1, được xác định bới 4 tham số Denavit-hartenberg. Trong 4 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ự ghép nối các khâu thứ i-1 và 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 i là khớp quay hay khớp tịnh tiến.Khi khớp i là khớp quay thì i θ là biến, còn i d là hằng số. Khi khớp i là khớp tịnh tiến thì i d là biến, còn i θ là hằng số. Chú ý về việc xác định hệ tọa độ khớp tại khớp tịnh tiến.Trong trường hợp khớp i là khớp tịnh tiến, về nguyên tắc ta có thể chọn trục 1i z  một cách tùy ý, do đó việc xác định các tham số Denavit-Hartenberg phụ thuộc vào việc chọn hệ trục tọa độ. Ma trận của phép biến đổi, kí hiệu là i H , là tích của 4 na trận biến đổi cơ bản,và có dạng như sau: cos( ) sin( )cos( ) sin( )sin( ) cos( ) sin( ) cos( )cos( ) cos( )sin( ) sin( ) 0 sin( ) cos( ) 0 0 0 1 i i i i i i i i i i i i i i i i i θ θ α θ α a θ θ θ α θ α a θ α α d              H (1.1) Ma trận i H được xác định bởi công thức (1.1) được gọi là ma trận Denavit-Hartenberg địa phương. Phương trình xác định vị trí khâu thao tác (bàn kẹp) cuả robot. Xét mô hình cơ học của một robot n khâu động như hình vẽ: 7 Hình 1.2. Robot n khâu Theo nguyên tắc nêu trên, ta thiết lập được hệ trục tọa độ gắn liền với giá cố định và hệ tọa độ gắn liền với các vật.Gọi 0 R là hệ quy chiếu 0 ( xyz)O gắn liền với giá cố định, hệ quy chiếu ( xyz) i i R O gắn liền với khâu thứ i.Ma trận 1i i H  cho ta biết vị trí và hướng của khâu i đối với hệ quy chiếu 1i R  gắn vào khâu thứ i-1. Từ đó suy ra ma trận Denavit-Hartenberg i H cho biết vị trí của hệ quy chiếu ( xyz) i i R O đối với hệ quy chiếu 1 1 ( xyz) i i R O    .Áp dụng liên tiếp các phép biến đổi đối với robot n khâu, ta có: 0 1 2 3 ... n  D H H H H (1.2)  0 0 1 n E Tn        A r D `(1.3) Ma trận n D cho biết vị trí của điểm tác động cuối E và hướng cuả khâu thao tác (bàn kẹp) của robot đối với hệ quy chiếu cố định 0 R Như vậy khi biết được các đặc tính hình học cuả các khâu và quy luật chuyển động của các khớp là ta có thể xác định được vị trí và hướng của bàn kẹp. Xác định vận tốc, gia tốc điểm tác động cuối và vận tốc góc, gia tốc góc các khâu cuả robot bằng phương pháp trực tiếp. Vận tốc và gia tốc dài của bàn kẹp có thể dễ dàng suy ra từ đạo hàm vector tọa độ (0) E r 8 Vận tốc điểm thao tác:                 0 0 0 0 0 0 0 0 0 , : E R E E Ex Ey E Ez E d x dtv d d hay v y dt dt v d z dt                         v r (1.4) Gia tốc điểm thao tác:                 0 0 0 0 0 0 0 0 0 , : ExR E Ey Ez Ex E Ey Ez v dta d d hay a v dt dt a d v dt                            a v (1.5) Ta có thể tính trực tiếp vận tốc góc khâu thứ i của robot dựa trên công thức tính vận tốc góc vật rắn thông qua ma trận cosin chỉ hướng như sau:       0 0 T i i i T i i i   ω A A ω A A   (1.6) Từ (1.6) suy ra biểu thức vận tốc góc khâu thứ i của robot. Áp dụng định nghĩa gia tốc góc của vật rắn, khi biết vận tốc góc của các khâi của robot ta có thể tính gia tốc góc các khâu của robot theo công thức sau: 0 0 0 R R B R Bd dt α ω   (1.7) 9 1.1.2. Thiết lập phƣơng trình động học thuận cho robot RRR. Hình 1.3. Đặt các trục tọa độ cho robot RRR Thiết lập bảng động học DH. Từ hình vẽ ta tìm được bảng động học: i d , i θ , i a , i α Khâu i θ i d i a i α 1 1 q 0 1 a 2 π 2 2 q 0 2 a 0 3 3 q 0 3 a 0 a. Tìm các ma trận biến đổi Dựa vào công thức (1.1) ở trên ta thiết lập các ma trận Denavit-Hartenberg Hi như sau: 1 1 1 1 1 1 1 1 1 0 0 0 1 0 0 0 0 0 1 C S a C S C a S             H 2 2 2 2 2 2 2 2 2 0 0 1 0 0 0 0 0 1 C S a C S C a S             H 3 3 3 3 3 3 3 3 3 0 0 0 1 0 0 0 0 0 1 C S a C S C a S             H 10 Dựa vào (1.2) ta có ma trận Denavit-Hartenberg của khâu thao tác đối với khâu 0 như sau: 1 23 1 23 1 1 3 23 2 2 1 1 23 1 23 1 1 3 23 2 2 1 3 23 23 3 23 2 2 ( ) ( ) 0 0 0 0 1 C C C S S C a C a C a S C S S C S a C a C a S C a S a S                  D (1.8) b. Xác định vị trí và hướng của bàn kẹp Vị trí của khâu thao tác (bàn kẹp) được xác định trong hệ tọa độ cố định R0 được xác định bởi điểm E (điểm tác động cuối) và hướng của khâu thao tác. Theo (1.8) ta có: 1 3 23 2 2 1 1 3 23 2 2 1 3 23 2 2 ( ) ( ) E C a C a C a S a C a C a a S a S             x (1.9) Ma trận cosin chỉ hướng (xác định hướng của bàn kẹp) xác định từ ma trận 3 T như sau: 1 23 1 23 1 3 1 23 1 23 1 23 23 0 C C C S S S C S S C S C            A (1.10) Xác định hướng của bàn kẹp: Sử dụng phép quay Roll-Pitch-Yaw trong phép quay hệ quy chiếu cố định 0 R sang hệ quy chiếu n R thì ta có ma trận cosin chỉ hướng RPY, như sau:                                                           cos cos cos sin sin sin cos cos sin cos sin sin sin cos sin sin sin cos cos sin sin cos cos sin sin cos sin cos cos φ θ φ θ ψ φ ψ φ θ ψ φ ψ φ θ φ θ ψ φ ψ φ θ ψ φ ψ θ θ ψ θ ψ               R 11 Hình 1.4. Phép quay Roll – Pitch – Yaw Đồng nhất các phần tử của 2 ma trận A3và A, ta tìm được góc φ , hướng của bàn kẹp như sau: 1 2 3 ; ( ); 2 π φ q θ q q ψ     c. Xác định vận tốc của điểm tác động cuối so với hệ cố định     1 1 3 23 2 2 1 1 3 23 2 3 2 2 2 1 0 0 1 1 3 23 2 2 1 1 3 23 2 3 2 2 2 1 3 23 2 3 2 2 2 0 . ( ) ( ( ) . ) . ( ) ( ( ) . ) ( ) R E E S q a C a C a C a S q q a C q a C q a a a S a S q q a C q a a C q q a C q d dt                         v r            (1.11) d. Vận tốc góc của mỗi khâu so với hệ cố định 1 2 1 2 1 3 1 2 1 2 3 1 2 1 3 1 1 1 0 0 ; ; S q S q S q ω ω C q ω C q C q q q q                                            (1.12) Với , 1..3iω i  lần lượt là vận tốc góc của khâu 1, 2, 3 so với hệ tọa độ cố định. e. Các đồ thị thể hiện vị trí, vận tốc điểm tác động cuối E Ta cho các số liệu đầu vào như sau: 1 2 30.4( ), 0.3( ), 0.3( )a m a m a m   Và giả sử các góc quay tại mỗi khớp là các hàm: 1 1 1 sin( ), sin( ), os( ) 30 2 30 π π π q t q t q c t   12 Với kết quả (1.9) và (1.11), sử dụng phần mềm Matlab ta có các đồ thị như sau: Hình 1.5. Vị trí điểm tác động cuối Hình 1.6. Vận tốc điểm tác động cuối so với hệ cố định 1.2. Giải bài toán động học ngƣợc Bài toán động học ngược có ý nghĩa đặc biệt quan trọng trong lập trình và điều khiển chuyển động của robot. Bởi lẽ, trong thực tế thường phải điều khiển robot sao cho bàn kẹp di chuyển tới các vị trí nhất định trong không gian thao tác theo một quy luật nào đó. Ta cần xác định các giá trị biến khớp tương ứng với vị trí và hướng của robot theo yêu cầu đó. Đây cũng chính là nội dung của bài toán động học ngược. 13 Từ bài toán thuận ta biết phương trình xác định vị trí bàn kẹp  x f q . Bây giờ giả sử x đã biết, cần tìm q một cách hình thức như sau:  -1=q f x Trong đó: 1[ ... ]Tnq qq là vectơ toạ độ suy rộng biến khớp 1[ ... ]Tnq qq là vectơ toạ độ suy rộng của khâu thao tác (bàn kẹp). Với n là số toạ độ suy rộng khớp (số bậc tự do của robot), m là số toạ độ suy rộng của bàn kẹp ( m = 6) Có 3 trường hợp xảy ra : - Khi m = n , robot có cấu trúc động học cân bằng hay cấu trúc chuẩn.Phương trình (2.1) có thể có nghiệm duy nhất phụ thuộc vào cấu trúc của robot. - Khi m < n , robot có cấu trúc dư dẫn động. Số toạ độ suy rộng khớp qi lớn hơn số tọa độ suy rộng khâu thao tác xj cần xác định. Bài toán có nhiều nghiệm, để giải bài toán có thể đưa thêm vào các điều kiện phụ như là: điều kiện về công nghệ, điều kiện về cơ học, các điều kiện về toán học…để đưa bài toán về bài toán cấu trúc động học cân bằng, giải bài toán bằng phương pháp ma trận tựa nghịch đảo, trong đó số phương trình nhỏ hơn số ẩn. - Khi m > n , robot có số toạ độ suy rộng khớp ít hơn số toạ độ suy rộng khâu thao tác, phương trình (2.1) không giải được. Để bài toán có nghiệm, ta cần đưa thêm vào các điều kiện ràng buộc, tuy nhiên đây là bài toán không có nhiều ý nghĩa trong thực tế. Các phương pháp giải bài toán động học ngược Việc đi tìm nghiệm của bài toán động học ngược có ý nghĩa rất quan trọng trong lập trình và điều khiển robot. Tuy nhiên, việc này khá khó khăn và hiện chưa có phương pháp tổng quát nào để giải quyết vấn đề này một cách thật hiệu quả. Có hai nhóm phương pháp hay được sử dụng là : - Nhóm phương pháp giải tích - Nhóm phương pháp số 1.2.1. Giải bài toán động học ngƣợc bằng phƣơng pháp giải tích a. Cơ sở lý thuyết Khi giải bài toán động học thuận bằng phương pháp ma trận Denavit-Hartenberg ta có ma trận biến đổi xác định vị trí thao tác là: 14 (0) 1 2 3 1 ( ) ( ) ( ) ( ) ... 1 n E n n n n T          A q r q T q D q H H H H H 0 (1.13) Từ đó ta xác định được ma trận cosin chỉ hướng khâu thao tác và vector điểm thao tác E là các hàm của tọa độ suy rộng. Mặt khác, từ nhiệm vụ công nghệ của robot ta có ma trận cấu hình khâu thao tác (ma trận cosin chỉ hướng của khâu thao tác và vector xác định vị trí điểm thao tác) dưới dạng hàm của các tọa độ thao tác: ( ) 0 0 0 1 x x x x y y y y n z z z z n s a p n s a p n s a p             T x (1.14) Từ đó ta có phương trình ma trận: ( ) ( )n nT q T x (1.15) Từ phương trình (1.15) sử dụng các phương pháp đại số và hình học ta có thể tìm được các hàm xác định tọa độ khớp  -1=q f x . b. Áp dụng giải bài toán ngược cho robot RRR Sử dụng kết quả (1.8) ta có ma trận Denavit-Hartenberg của khâu thao tác đối với khâu 0 như sau: 1 23 1 23 1 1 3 23 2 2 1 1 23 1 23 1 1 3 23 2 2 1 3 1 2 3 23 23 3 23 2 2 ( ) ( ) 0 0 0 0 1 C C C S S C a C a C a S C S S C S a C a C a S C a S a S                   D H H H (1.16) Cho các phần tử (1,4) và (2,4) của hai ma trận (1.14) và (1.16) bằng nhau ta được: 1 1 1 1 tan( ) atan2( , ) y x y x PS q C P q P P     (1.17) Từ phương trình ma trận (1.16) ta suy ra:   1 1 2 3 3  H H D H Ta có: 15   3 3 3 3 3 1 3 3 3 3 3 3 3 3 3 3 3 3 0 0 0 1 x x x x x x x x y y y y y y y y z z z z z z z z C n S s S n C s a a n a p C n S s S n C s a a n a p C n S s S n C s a a n a p                       D H (1.18) 1 2 1 2 1 1 2 2 1 1 2 1 2 1 1 2 2 1 1 2 2 2 2 2 ( ) ( ) 0 0 0 0 1 C C C S S C a C a S C S S C S a C a S C a S                H H (1.19) So sánh các phần tử (3,4) và (2,4) của hai ma trân (1.18) và (1.19) ta có: 3 2 2 z z za n a pS a    (1.20) 3 1 1 2 2 y y ya n a p a S C a     (1.21) 2 2 2atan2( , )q S C  (1.22) So sánh các phần tử (3,1) và (3,2) của hai ma trận (1.14) và (1.16) ta có: 23 23 ; z z S n C s  (1.23) Suy ra: 2 3 23 23atan2(S ,C )q q  (1.24) Vậy 3 23 23 2atan2(S ,C )q q  (1.25) Sử dụng các góc Roll-Pitch-Yall, với các thông số hướng của khâu thao tác cuối như sau: ; ; 6 3 3 π π π φ θ ψ   và khâu thao tác cuối di chuyển theo các phương trình 1+0.15(1+3sin3t)cost; 0.3(1+3sin3t)sin(t); =0.3+0.2costx y zp p p  . Từ các phương trình (1.17), (1.22) và (1.25) ta vẽ đồ thị gồm góc quay, tốc đọ góc và gia tốc góc tại mỗi khớp như sau: 16 Hình 1.7. Chuyển động khâu thác cuối trong không gian (hình hoa ba cánh) Hình 1.8. Đồ thị góc quay các biến khớp Hình 1.9. Đồ thị vận tốc góc quay các biến khớp 0 0.5 1 1.5 2 -2 -1 0 1 0.1 0.2 0.3 0.4 0.5 Phuong trinh diem thao tac cuoi trong khong gian 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 -1.2 -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 Phuong trinh diem thao tac cuoi trong khong gian 0 1 2 3 4 5 6 7 8 9 10 -4 -3 -2 -1 0 1 2 3 Thoi gian (s) Go c q ua y ( rad ) Do thi bien khop q1 q2 q3 0 100 200 300 400 500 600 700 800 900 1000 -0.03 -0.02 -0.01 0 0.01 0.02 0.03 Thoi gian (s) To c do g oc (r ad /s ) Van toc goc bien khop dq1 dq2 dq3 17 Hình 1.10. Đồ thị gia tốc góc của biến khớp 1.2.2. Giải bài toán ngƣợc bằng phƣơng pháp số a. Cơ sở lý thuyết Nhìn chung, các phương pháp số có thể giải được bài toán động học ngược một cách tổng quát, tính tự động hoá cao. Tuy nhiên, trong thực tế, việc tìm lời giải bằng phương pháp này lại gặp khó khăn như thời gian tính toán lâu do gặp phải hệ phương trình siêu việt, hoặc vì tính đa trị của lời giải,… Dưới đây, ta sẽ trình bày các nội dung chính để giải bài toán động học ngược của robot bằng phương pháp số. Phương trình xác định vị trí bàn kẹp ( )x f q (1.26) Đạo hàm hai vế phương trình (1.26) theo t, ta được       f x q J q q q   (1.27) trong đó :’ 0 100 200 300 400 500 600 700 800 900 1000 -1.5 -1 -0.5 0 0.5 1 1.5 x 10 -3 Thoi gian (s) G ia to c go c (ra d/ s2 ) Gia toc goc bien khop ddq1 ddq2 ddq3 18 1 1 1 1 2 1 2 ... ( ) ... ... ... ... ... n m m m n f f f q q q f f f q q q                            f J q q (1.28) -Khi m=n ta có bài toán có cấu trúc động học cân bằng. x, q, f 𝝐 R n . Từ (1.27) ta suy ra:       1t t tq J q x  (1.29) Đạo hàm (1.27) theo t, ta được:          t t t t t       x J q q J q q    (1.30) Từ (1.30) suy ra          t t t t t       J q q x J q q   (1.31) Từ (1.31) suy ra ta được :          1t t t t t          q J q x J q q   (1.32) Công thức (1.29) và (1.32) cho phép ta xác định được vectơ vận tốc suy rộng gia tốc suy rộng khi biết  tq tại thời điểm khảo sát và các quy luật      , ,t t tx x x  . Giả sử robot làm việc trong khoảng thời gian [0,T]. Ta chia khoảng thời gian đó thành n phần bằng nhau. h=∆t = T/n∗  n∗ =T/∆t=T/h Ta có:  *k+1 kt =t +Δt k=0,1,....,n Áp dụng khai triển Taylor hàm vectơ q  t  ở lân cận giá trị t =tk, ta có            2 k+1 k k k k 1 q t =q t +Δt =q t +q t Δt+ q t Δt ... 2  Bỏ qua các vô cùng bé bậc hai ta có:         1k kt t t t t   -1q q J q x với (k=0,1,…,n*) (1.33) 19 Nhận xét: Tính q(tk+1) theo (1.33) ta nhận được giá trị thô của q(t) tại thởi điểm t=tk, vì vậy cần nâng cao độ chính xác của  1kt q bằng việc tính q0. Thuật toán xác định  0 0tq q Ký hiệu t0=0 là thời điểm ban đầu. Ta có thể xác định vectơ gần đúng ban đầu 0 q của vectơ 0 q bằng phương pháp vẽ hoặc thực nghiệm. Áp dụng khai triển Taylor tìm nghiệm 0 q gần đúng như sau:  0 0 0 0t   q q q q (1.34) Theo phương trình (1.34) ta có:          0 0 0 0 0 0 0...t          f x f q f q q f q q q q   Từ đó, suy ra các công thức gần đúng:    0 0 0 0( )t  J q q x f q  (1.35) Giải phương trình gần đúng (1.35) ta có:    10 0 0 0( )t    q J q x f q  (1.36) Lấy 0 0 0  q q q  (1.37) Nếu 0 1 ε q thì ta lại thế phương trình (1.37) vào (1.34) rồi giải nó. Quy trình này dừng lại khi 0 1 ε q . Trong đó 𝜀1 là sai số dương bé chọn trước. Kết quả của quá trình này ta được 0 0 q q (1.38) Biết được 0 q thay vào (1.29) và (1.32) để tính 0 q , 0 q . b. Áp dụng giải bài toán cho robot RRR Sử dụng phương pháp lặp Newton-Raphson ta tìm giá trị của vectơ 0 q . Xem chương trình code viết trên MATLAB ở phần phụ lục. Lấy phương trình điểm tác động cuối như bài toán giải bằng phương pháp giải tích, ta có kết quả góc quay của các tọa độ suy rộng như sau: 20 Hình 1.11. Đồ thị mối quan hệ giữa các tọa độ suy rộng với thời gian 0 1 2 3 4 5 6 7 8 9 10 -10 -5 0 5 10 15 t (s) q (ra d) Do thi goc quay cac bien khop q1 q2 q3 21 CHƢƠNG II. GIẢI BÀI TOÁN TĨNH HỌC ROBOT 2.1. Cơ sở lý thuyết Cho trước các thông số: Coi các khâu là thanh đồng chất , tiết diện ngang không đáng kể, khối lượng các khâu là m1, m2, m3. Cho lực tác dụng vào khâu thao tác tại điểm E, 0 F = [ Fx, Fy, Fz ] T . Chọn một vị trí của robot theo bài toán động học: 1 2 3 70 0 15 90 ( ); 0 ( ); 14.5 ( ). 60 0 13 x E y E z F m F F N M Nm m m kg F m                                                      (2.1) Cho lực tác động vào khâu thao tác tại điểm E gồm véc tơ lực 𝐹𝐸 , và momen 𝑀𝐸 , tính lực (mô men) tác động vào các khớp đảm bảo Robot cân bằng tĩnh. Xét khâu thứ i: Ta có hệ phương trình cân bằng lực: , 1 1, , 1 1, , 1 i i i i i i i i i i i i Ci i F F P M M r F r P                        (2.2) Dạng ma trận trong hệ tọa độ cơ sở: 0 0 0 , 1 1, 0 0 0 0 0 0 , 1 1, , 1 i i i i i i i i i i i i Ci i F F P M M r F r P               (2.3) Dạng ma trận trong hệ tọa độ khâu i: , 1 1, , 1 1, , 1 i i i i i i i i i i i i i i i i i i i i i Ci i F F P M M r F r P               (2.4) Trong đó: ,i jF  : là lực do khâu j tác dụng lên khâu i. ,i jM  : là momen do khâu j tác dụng lên khâu i. iP  : là trọng lực tác dụng lên khâu i. ir  : vector 1i iO O   . Cir  : vector i iO C  22 2.2. Áp dụng cho Robot RRR  Xét khâu thứ 3:   0 0 3,4 0 0 3,4 3 3 3 3 3 3 , , , , ,0,0 ,0,0 2 T E x y z T E x y z T T C F F F F F M M M M M r a a r                     0 3 30 0 TP m g  Theo (2.3) ta có: 0 0 0 3,2 3,4 3 0 0 0 0 0 0 3,2 3,4 3 3,2 3 3C F F P M M r F r P          0 3,2 3 3 0 0 0 3,2 3 3 3 3 0 0 0 x x y y z z x x y y c z z F F F F F F m g F m g M F M M r F r M F m g m g                                                                           (2.5) Trong đó : 1 23 1 23 1 3 0 0 3 3 3 3 1 23 1 23 1 23 23 3 1 23 3 1 23 3 23 0 0 0 C C C S S a r R r S C S S C S C a C C a S C a S                                  3 23 3 1 23 0 3 3 23 3 1 23 3 1 23 3 1 23 0 0 0 a S a S C r a S a C C a S C a C C              (2.6) 23 3 1 23 1 23 1 0 0 3 3 3 3 1 23 1 23 1 23 23 3 1 23 3 1 23 3 23 2 0 0 0 2 2 2 C C a C C C S S r R r S C S S C S C a C C a S C a S                                       3 3 23 1 23 0 3 3 3 23 1 23 3 3 1 23 1 23 0 2 2 0 2 2 0 2 2 c a a S S C a a r S C C a a S C C C                   (2.7) Thay (2.6) và (2.7) vào (2.5) thay các thông số cho trước ta được: 0 3,2 3 0 3,2 ( ) ( ) T x y zF F F F m g N M Nm          Xét khâu thứ 2 Xét khâu thứ 2:  0 2 20 0 TP m g   2 2 2 2 2 2 ,0,0 ,0,0 2 T T C r a a r          Theo (2.3) ta có: 0 0 0 2,1 3,2 2 0 0 0 0 0 0 2,1 3,2 2 2,1 2 2c F F P M M r F r P          (2.8) Trong đó : 24 1 2 1 2 1 2 2 1 2 0 0 2 2 2 2 1 2 1 2 1 2 1 2 2 2 2 2 0 0 0 C C C S S a a C C r R r C S S S C a C S S C a S                                      2 2 2 1 2 0 2 2 2 2 1 2 2 1 2 2 1 2 0 0 0 a S a C S r a S a C C a C S a C C             (2.9) 2 2 1 2 1 2 1 2 1 0 0 2 2 2 2 2 1 2 1 2 1 1 2 2 2 2 2 2 2 0 2 0 0 2 C C a a C C C C C S S a r R r C S S S C C S S C a S                                         2 2 2 1 2 0 2 2 2 2 1 2 2 2 1 2 1 2 0 2 2 0 2 2 0 2 2 C a a S C S a a r S C C a a C S C C                     (2.10) Ta thay kết quả (2.9) và (2.10) vào (2.8) ta được: 0 2,1 3 2 ( ) T x y zF F F F m g m g N     0 2,1 ( )M Nm  Xét khâu thứ 1:  1 1 1 1 1 1 ,0,0 ,0,0 2 T T C r a a r           0 1 30,0, g TP m  Theo (2.3) ta có: 0 0 0 1,0 2,1 1 0 0 0 0 0 0 1,0 2,1 1 1,0 1 1c F F P M M r F r P          (2.11) Trong đó : 25 1 1 1 1 1 0 0 1 1 1 1 1 1 1 1 0 0 0 0 1 0 0 0 C S a a C r R r S C a S                                   Suy ra: 1 1 0 1 1 1 1 1 1 1 0 0 0 0 0 a S r a C a S a C           (2.12) 1 1 1 1 1 0 0 1 1 1 1 1 1 1 1 20 2 0 0 2 0 1 0 0 0 c c a a C C S a r R r S C S                                        Suy ra: 1 1 0 1 1 1 1 1 1 1 0 0 2 0 0 2 0 2 2 c a S a r C a a S C                   (2.13) 0 2,1 3 2 1 ( ) T x y zF F F F m g m g m g N      0 10 ( )M Nm Giả sử với các phương trình biến khớp như đã biết như ở bài toán thuận, và các số liệu đầu vào của bài toán tĩnh học như (2.1), ta tính độ lớn lực và vẽ đồ thị moment tác động lên các khớp như sau: 0 3,2 0 2,1 0 1,0 [70 90 187.53] [70 90 329.775] [70 90 476.925] T T T F F F    26 Hình 2.1. Moment tác động lên khớp động thứ nhất Hình 2.2. Moment tác động lên khớp động thứ 2 Hình 2.3. Moment tác động lên khớp động thứ 3 27 CHƢƠNG III. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CHO ROBOT Chọn 2 điểm A, B bất kỳ trong không gian làm việc của Robot. Thiết kế quỹ đạo chuyển động Robot ( có thể chọn quỹ đạo của đa thức bậc 1, 2, 3) 3.1 Giới thiệu và cơ sở thiết kế quỹ đạo Thiết kế quỹ đạo chuyển động của robot có liên quan mật thiết đến bài toán điều khiển robot di chuyển từ vị trí này đến vị trí khác trong không gian làm việc. Đường đi và quỹ đạo được thiết kế là các đại lượng đặt cho hệ thống điều khiển vị trí của robot. Do đó độ chính xác của quỹ đạo sẽ ảnh hưởng đến chất lượng di chuyển của robot. Yêu cầu thiết kế quỹ đạo chuyển động của robot là: + Khâu chấp hành phải đảm bảo đi qua lần lượt các điểm trong không gian làm việc hoặc di chuyển theo một quỹ đạo xác định. + Quỹ đạo của robot phải là đường liên tục về vị trí trong một khoảng nhất định. + Không có bước nhảy về vận tốc, gia tốc. Quỹ đạo là các đường cong dạng: + Đa thức bậc 2: 2( )x t a bt ct   + Đa thức bậc 3: 2 3( )x t a bt ct dt    + Đa thức bậc cao: x(t) = 2( ) .... nx t a bt ct kt     Ta sẽ sử dụng quỹ đạo dạng đa thức bậc 3 để thiết kế 3.2. Tính toán thiết kế quỹ đạo chuyển động 3.2.1 Thiết kế quỹ đạo trong không gian khớp Chọn 2 điểm A, B bất kỳ trong không gian làm việc, biết tọa độ (xE, yE, zE) và hướng của khâu thao tác. Thiết kế quỹ đạo chuyển động bất kỳ từ A đến B Theo bài toán động học ngược ta xác định được các biến khớp 1 2 3, ,   tại A và B. Chọn quỹ đạo thiết kế là đa thức hàm bậc 3 theo thời gian có dạng như sau: 2 3( )i i i i it a bt c t d t     (3.1) i= 1..3 tương ứng với 3 biến khớp Ta được hệ phương trình sau: 28 2 3 1 1 1 1 1 2 3 2 2 2 2 2 2 3 3 3 3 3 3 ( ) ( ) ( ) t a b t c t d t t a b t c t d t t a b t c t d t                   Giả sử thời gian robot đi từ điểm A đến điểm B là trong t(s) và vận tốc tại 2 điểm đó bằng 0. Từ đó ta có hẹ phương trình sau: 2 3 2 (0) (A) (t) (B) (0) (A) 0 ( ) (B) 2 0 • • 3 i i i i i i i i i i i i i i i i i a a b t c t d t b t b c t d t                                   2 3 (A) 0 3 (B) (A) 2 (B) (A) i i i i i i i i i a b c t d t                     (3.2)  Giả sử Robot RRR cần dịch chuyển từ vị trí A(0.1536036850, 0.2660493873, 0.2747058204) đến điểm B(0.1541938615, -0.1120283979, 0.4048039886) tương ứng với tọa độ khớp là 4 3 ( , , ) 3 4 5 π π π A đến 9 2 3 ( , , ) 5 9 5 π π π B  . Ta suy ra hệ số của phương trình quỹ đạo cho khâu 1 là:     1 1 1 1 1 1 2 2 1 1 1 3 3 4 ( ) 3 0 9 4 3 3 ( ) ( ) 75 3 5 125 9 4 2 2 ( ) ( ) 145 3 5 1875 a A b B A c t B A d t                                         Vậy 2 3 1 4 7 14 ( ) 0 3 125 1875 π π π θ t t t t    . 29 Ta tính tương tự cho các góc quay còn lại. Sau khi thiết kế xong, ta vẽ đồ thị của các biến khớp trên miền thời gian như sau: Hình 3.1. Đồ thị góc khớp thứ 1 Hình 3.2. Đồ thị góc khớp thứ 2 Hình 3.2. Đồ thị góc khớp thứ 2 3.2.2. Thiết kế quỹ đạo trong không gian làm việc Để phục vụ cho tín hiệu vào cho bộ điều khiển trong không gian làm việc nên chỉ xét quỹ đạo di chuyển của robot RR giữa hai điểm A(x0,y0,z0), B(xc,yc,zc ) là đường thẳng và đường tròn ( ta thiết kế ở đây đó là đường tròn nhận AB làm đường kính). 0 20 40 60 80 100 120 -2 -1 0 1 2 3 4 5 6 Do thi goc quay, van toc goc, gia toc goc theta 1 Thoi gian (s) q2 ,q 2d ot ,q 2d ot do t q1 q1d q1dd 0 20 40 60 80 100 120 -1 0 1 2 3 4 5 6 7 8 9 Do thi goc quay, van toc goc, gia toc goc theta 2 Thoi gian (s) q2 ,q 2d ot ,q 2d ot do t q2 q2d q2dd 0 20 40 60 80 100 120 -5 -4 -3 -2 -1 0 1 2 Thoi gian (s) q3 ,q 3d ot ,q 3d ot do t Do thi goc quay, van toc goc, gia toc goc theta 3 q3 q3d q3dd 30 a. Quỹ đạo của điểm tác động cuối theo đường thẳng từ A đến B trong tc (s) Ta có phương trình đường thẳng trong không gian làm là mặt phẳng giữa hai điểm A(x0,y0), B(xc,yc) là 0 0 0 0 0 0c c c x x y y z z x x y y z z         với (x,y) là tọa độ điểm tác động cuối. => 0 0 0 0 0 c c c c c y y y x y x y x x x x x       0 0 0 0 0 c c c c c z z z x z x z x x x x x       Cũng như cách thiết lập quỹ đạo góc khớp ta để thỏa mãn điều kiện về vận tốc đầu và cuối ta thiết lập quan hệ x=x(t) là đa thức bậc 3. 2 3 1 2 3( ) ox t a a t a t a t    (3.3) Cũng các điều kiện 0 0 0 (0) ( , , ) (0) 0 ( ) ( , , ) ( ) 0 o c c c c c c s A x y z v v s t B x y z v t v          mà quan hệ y =k.x+b => x=x(t) cũng có điều kiện như quỹ đạo. Vậy 0 0 0 (0) ( , , ) (0) 0 ( ) ( , , ) ( ) 0 o c c c c c c s A x y z v v s t B x y z v t v          Ta có các hệ số như sau: 0 0 1 0 2 2 0 3 3 0 3( ) 2( ) c c c c a x a x x a t x x a t             (3.4) Khi thiết kế x=x(t) và 0 0 0 0 0 ( ) ( )c c c c c y y y x y x y t x t x x x x       ; 0 0 0 0 0 ( ) ( )c c c c c z z z x z x z t t x x x x       31  Cần thiết kế quỹ đạo để Robot đi từ điểm (30, 0, 0) đến điểm (-25, -15, 10) trong 5(s). Dựa vào (3.4) và (3.3) ta tìm được các hệ số như sau: 2 3( ) 30 0 -6.6t 0.88t ; ( ) 0.273x(t)-8.0578; z(t)=-0.182x(t)+5.3719x t y t     Sử dụng các phương trình vừa tìm được, sử dụng Matlab ta vẽ các đồ thị sau: Hình 3.3. Phương trình x, y,z Hình 3.4. Vận tốc của điểm tác động cuối Hình 3.5. Phương trình x, y,z Hình 3.6. Quỹ đạo chuyển động 0 1 2 3 4 5 6 7 8 9 10 -50 0 50 100 150 200 250 fx fy fz 0 10 20 30 40 50 60 70 80 90 100 -4 -2 0 4 6 8 10 12 14 fxd fyd fzd 0 10 20 30 40 50 60 70 80 90 100 -0.2 -0.1 0 0.1 0.2 0.3 0.4 0.5 fxdd fydd fzdd -100 0 100 200 300 -50 0 50 100 -50 -40 -30 -20 -10 0 10 f 32 b. Thiết kế quỹ đạo điểm tác động tác động cuối di chuyển theo đường tròn từ A đến B trong  ct s lấy AB làm đường kính. Ta có phương trình đường tròn trong không gian làm là mặt phẳng giữa hai điểm    0 0, ,0 , , ,0c cA x y B x y lấy AB làm đường kính     2 2 2 i ix x y y R    với 0 0, , 0 2 2 c c i i x x y y x y z      ,     2 2 0 1 2 c o cR x x y y    Viết dưới dạng tham số như sau sin( ( )) cos( ( )) z 0 i i x x R a t y y R a t        (3.5) cũng để thỏa mãn điều kiện về vận tốc a(t) ta thiết kế cũng phải là bậc 3   2 31 2 3oa t a a t a t a t    Và phải thỏa mãn điều kiện:             0 0 0 0 , 0 0 , 0 c c c c c s A x y v v s t B x y v t v          Từ đó ta tìm được các hệ số 0 1 2 3, , ,a a a a như sau. 0 0 1arcsin , 0 ix xa a R        Đặt : 0arcsin arcsinc i i x x x x w R R               (3.6) 2 32 3 3 2 , c c w w a a t t     .  Giả sử cần tạo ra một quỹ đạo để Robot chuyển động từ điểm A(0.7, 0) đến B(- 0.7, 0). Dựa vào (3.5) và (3.6) ta tìm được các phương trình sau: 2 3( ) 1.570796326794897+0t+ 2.356194490192345 0.785398163397448 0 0.7sin( ( )) 0 0.7cos( ( )) 0 a t t t x a t y a t z           Sử dụng các phương trình vừa tìm được, sử dụng Matlab ta vẽ các đồ thị sau: 33 Hình 3.7. Quỹ đạo chuyển động của điểm cuối Hình 3.8. Vận tốc điểm tác động cuối Hình 3.9. Gia tốc điểm tác động cuối 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 Thoi gian (s) D ic h ch uy en (m ) Do thi quy dao chuyen dong diem tac dong cuoi fx fy -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 f 0 20 40 60 80 100 120 140 160 180 200 -0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15 Thoi gian (s) V an t oc ( m /s ) Do thi van toc diem tac dong cuoi fxd fyd 0 20 4 6 80 100 120 140 160 180 200 -0.02 -0.015 -0.01 -0.0 5 0 0.005 0.01 0.015 0.02 0.025 Thoi gian (s) G ia to c (m /s 2 ) D thi gia toc diem tac dong cuoi fxdd fydd 34 CHƢƠNG IV. ĐỘNG LỰC HỌC ROBOT 4.1. Cơ sở lý thuyết Trong tính toán động học robot, để xác định vị trí của các khâu ta chỉ cần sử dụng hệ tọa độ cố định và hệ tọa độ khớp. Trong bài toán động lực học robot ta cần thêm một hệ tọa độ nữa là hệ tọa độ khâu. Hệ tọa độ khâu là hệ quy chiếu gắn với vật rắn, thường có gốc trùng với khối tâm Ci của vật rắn, các trục hướng theo các trục quán tính chính của vật rắn. Trong hình (4.1) hệ i i i iO x y z là hệ tọa độ khớp, i i i iC  là hệ tọa độ khâu. Hình 4.1. Hệ tọa độ khâu Giả sử robot là hệ holonom có p vật rắn và r liên kết. Khi đó số bậc tự do của hệ là n = 6p – r. Kí hiệu các biến khớp của hệ là  1... T nq qq Vị trí khâu thứ i được xác định bởi: Tọa độ khối tâm của khâu:  ,Ci Ci tr r q Ma trận cosin chỉ hướng của khâu:  ,i i tA A q Xét robot có liên kết hôlônôm giữ và dừng, khi đó:  Ci Cirr q ,  i iA A q (4.1) Theo định nghĩa các ma trận Jacobi tịnh tiến và ma trận Jacobi quay được xác định bởi công thức: Ci Ti    r J q i Ci Ri       ω φ J q q (4.2) 35 Trong đó i là vectơ đại số ứng với góc quay i  của vật rắn thứ i, quay quanh trục quay tức thời. Vận tốc khối tâm và vận tốc góc của vật rắn được xác định bằng các công thức sau: • Ci Ci Ci Ti d dt      r r v q J q q   (4.3)  Ci CiCi Ri d dt      φ φ ω q J q q q   (4.4) Ta có phương trình vi phân chuyển động của Robot dạng ma trận như sau:        , t  M q q C q q q g q τ   Trong đó:   1 1 p p T T i Ti Ti Ri i Ri i i m     M q J J J I J (4.5) là ma trận vuông cấp n, và được gọi là ma trận khối lượng suy rộng của robot. Thế năng trọng lực của robot Thế năng trọng lực của mỗi khâu của rôbot được xác định bởi biểu thức: 0Ti i Cim  g r (4.6) ( ) T        π g q q (4.7) Trong đó: 0g T là vecto gia tốc trọng trường, xét trường hợp 0z là trục thẳng đứng, hướng lên, ta có:  0 0 0 T g g ;   T Ci Ci Ci Cix y zr là tọa độ khối tâm thứ i Ta định nghĩa: 1 2 ij jkik ijk k j i m mm h q q q            , (số hạng này có tên là Christoffel symbols ở dạng thứ nhất). Ta đưa vào kí hiệu:     1 , n ij ijk k k c h q  q q q  (4.8) 36 Suy ra: n n ijc    C  là ma trận li tâm – Coriolis. Thành phần  ,C q q q  đại diện cho lực quán tính ly tâm và quán tính Coriolis tác dụng lên robot. 4.2. Áp dụng tìm phƣơng trình vi phân cho Robot RRR 4.2.2. Bảng tham số động học Khâu Vị trí trọng tâm Khối lượng Ma trận mômen quán tính 𝑥𝑐 𝑦𝑐 𝑧𝑐 𝐼𝑥𝑥 𝐼𝑦𝑦 𝐼𝑧𝑧 𝐼𝑥𝑦 𝐼𝑦𝑧 𝐼𝑧𝑥 1 1C l 0 0 𝑚1 0 𝐼1𝑦 𝐼1𝑧 0 0 0 2 2C l 0 0 𝑚2 0 𝐼2𝑦 𝐼2𝑧 0 0 0 3 3C l 0 0 𝑚3 0 𝐼3𝑦 𝐼3𝑧 0 0 0 4.2.3. Các phƣơng trình vi phân 37 III. ĐIỀU KHIỂN ROBOT 5.1. Điều khiển phản hồi và điều khiển vòng kín. Khi xét bài toán điều khiển cho một tay máy trong công nghiệp nào đó, trước hết chúng ta sẽ phải mô hình hóa tay máy đó là một cơ cấu – đối tượng được điều khiển, trong đó các cảm biến (sensor) được đặt tại các khớp để giám sát trạng thái của các khớp và các cơ cấu dẫn động (Actuator) được gá tại các khớp để sinh lực (moment) dẫn động các khâu. Chúng ta luôn muốn điều khiển các khớp của robot bám theo đúng quỹ đạo đã thiết kế. Các phần tử dẫn động làm việc cách nhận lệnh và sinh lực (mô men). Do vậy chúng ta phải sử dụng các hệ điều khiển để tính toán các lệnh phù hợp nhất cho các phần tử này sao cho chúng thực hiện được các quy luật chuyển động mong muốn. Hình … mô tả quan hệ giữa quỹ đạo thiết kế và kết quả chuyển động theo quỹ đạo mong muốn đó. Từ các giá trị ban đầu là quy luật ( ) d tq , quy luật vận tốc ( ) d tq , quy luật gia tốc ( ) d tq mong muốn, bộ điều khiển có nhiệm vụ tính toán vector lực τ , là nguồn động lực để các hệ robot chuyển động với quy luật q và q . Nhờ các sensor bộ điều khiển có thể đọc được các giá trị này theo thời gian thực. Chú ý rằng toàn bộ các đường tín hiệu trên hình… đều gắn với vector n chiều với n là số khớp của robot. Có thể nhận thấy rằng, để tính toán mô men τ thì một khả năng rất khả quan là sử dụng phương trình động lực học, với đầu vào là ( ) d tq , ( ) d tq và ( ) d tq được tính toán trong phần thiết kế quỹ đạo. Do vậy, từ kết quả của phần tính toán động lực học, τ có thể được xác định theo công thức: ( ). ( , ). ( ) ( ). ( , ) ( )     τ M q q C q q q G q M q q V q q G q     (5.1) Trong đó: ( )M q là ma trận khối lượng suy rộng vuông cấp n. ( , )V q q là vector n chiều của các thành phần Coriolis và li tâm. ( )G q là vector n chiều các thành phần lực suy rộng của các lực không thế. τ được tính theo công thức (5.1) sẽ giúp hệ chuyển động theo đúng quỹ đạo mong muốn, nếu mô hình động lực là hoàn toàn đúng, hệ không chịu tác động của nhiễu. Nhìn chung cách duy nhất để xây dựng một hệ điều khiển có hiệu năng cao là phải sử dụng được 38 tín hiệu phản hồi như hình 5.1. Theo đó, tín hiệu phản hồi được sử dụng để tính toán sai lệch giữa vị trí e và sai lệch vận tốc e . , .   d d e = q q e = q q   (5.2) Đối với bài toán điều khiển tay máy, chúng ta xuất phát từ phương trình động lực học (5.1). Với cách tiếp cận điều khiển chia tách, trong trường hợp này ta có: τ ατ β  (5.3) với τ là vector n các thành phàn mô men dẫn động các khớp. Chúng ta chọn: ( ) ( , ) ( )    α M q β V q q G q (5.4) Với luật điều khiển phản hồi vòng kín như sau: d v p    τ q K E K E (5.5) Trong đó: d  E q q (5.6) 5.2. Thiết kế bộ điều khiển PID Hệ thống điều khiển nêu dưới đây sẽ thiết kế theo luật điều khiển PID. Khi thiết kế hệ thống điều khiển ta bỏ qua động học của cơ cấu chấp hành, quán tính động cơ. Như vậy chức năng của bộ điều khiển là tạo ra một moomen cần thiết để truyền động khớp robot đảm bảo khớp robot luôn bám theo vị trí đặt. Sơ đồ hệ thống điều khiển: 39 Hình 5.1: Sơ đồ cấu trúc hệ thông điều khiển robot với bộ điều khiển PID Hầu hết các bộ điều khiển robot công nghiệp hiện nay đều có cấu trúc, mà trong đó chúng có thể được mô tả:   α I β 0 (5.7) Trong đó I ma trận đơn vị cấp n. Luật điều khiển sẽ là: d v p i dt     τ q K E K E K E (5.8) Trong đó v K , p K và i K là các ma trận đường chéo tham số hằng số. Với luật điều khiển này đã giả thiết thành phần momen trọng lực G(p) đã được bù hoàn toàn. Hệ thống điều khiển với cấu trúc bộ điều khiển như trên, ổn định tuyệt đối toàn cục. Thực vậy chọn hàm Liapunov có dạng như sau: 1 1 2 2 T T P V e Me e K e (5.9) Vì ma trận M(q) đối xứng và xác định dương , ma trận KP được chọn là ma trận đối xứng, xác định dương nên V>0 với mọi e, q #0 Đạo hàm theo thời gian ta được: 1 2 T T T P  V q Mq q Mq e K q       ( ( ))  Mq τ Cq g q  40     1 2 1 2 0 2 T T T T P D P T T T D D              V q K e K q q Cq q Mq e K q q K q q M C q q K q               (5.10) Căn cứ vào biểu thức trên ta thấy hàm V giảm khi khi q #0 và robot sẽ đạt đến vị trí q=qd mong muốn. Hàm V =0 chỉ khi q =0 khi đó e=const Ta có : ( ) ( , ) ( ) ( )P D i dt      M q q C q q q g q K e K K E g q   (5.11) Tác động của việc tăng một thông số độc lập Thông số Thời gian khởi động. (RISE TIME) Quá độ OVERSHOOT Thời gian xác lập SETTLING TIME Sai số ổn định Độ ổn định Giảm Tăng Thay đổi nhỏ Giảm Giảm cấp Giảm Tăng Tăng Giảm đáng kể Giảm cấp Giảm ít Giảm ít Giảm Về lý thuyết không tác động Cải thiện nếu d K nhỏ 5.3. Thiết kế bộ điều khiển trong không gian khớp Giả sử bàn kẹp E đang ở một vị trí (xE,yE,zE) nào đó ứng với các tọa độ khớp 0 i q (i=1…n), nhiệm vụ của bộ điều khiển là phải đưa ra được điện áp đặt vào các động cơ để bàn kẹp di chuyển đến đúng hay gần đến vị trí mong muốn d E r với sai lệch cho phép, tương ứng vị trí mong muốn của bàn kẹp d E r là các tọa độ khớp d i q . Sau đây là xét luật điều khiển đơn giản nhất đáp ứng được yêu cầu trên, đó là luật điều khiển PID cùng với bù trọng lực. Lực điều khiển được tính từ các vị trí mong muốn (ứng với 41 tọa độ biến khớp d i q ) và trạng thái chuyển động của robot tại thời điểm hiện tại mà thu thập từ các sensor đặt tại các khớp. Sau khi tìm được phương trình vi phân chuyển động, ta thiết kế bộ điều khiển PID có bù trọng lượng trong Simulink như sau: Hình 5.2. Thiết kế sơ đồ khối trên Simulink Trong đó bọ điều khiển PID được thiết kế như sau: Hình 5.3. Thiết kế bộ điều khiển PID Sau khi hiệu chỉnh các thông số, ta thu được kết quả như sau: 42 Hình 5.4. Đồ thị biến khớp đặt và thực sau khi điều khiển Hình 5.5. Đồ thị vận tốc biến khớp đặt và thực sau khi điều khiển 0 1 2 3 4 5 6 7 8 9 10 -1 -0.5 0 0.5 1 1.5 2 2.5 3 3.5 Goc quay dat va thuc Thoi gian (s) G oc q ua y (ra d) q1 q2 q3 0 1 2 3 4 5 6 7 8 9 10 -25 -20 -15 -10 -5 0 5 10 15 20 Van toc goc dat va thuc Thoi gian (s) V an to c go c (ra d/ s) qd1 qd2 qd3 43 Hình 5.6. Đồ thị sai số của góc khớp Hình 5.6. Đồ thị sai số của vận tốc góc khớp Nhận xét: Nhìn vào các đồ thị ta có thấy được răng, nhờ bộ điều khiển mà tín hiệu ra đã bám khá tốt so với tín hiệu vào. Tuy nhiên, sai số như vậy vẫn còn có thể điều chỉnh được. Đặc biệt, đối với các góc khớp sai số vẫn còn khá lớn trong khi vận tốc góc bám tốt hơn. 0 1 2 3 4 5 6 7 8 9 10 -1 -0.5 0 0.5 1 1.5 2 2.5 Do thi sai so goc khop 0 1 2 3 4 5 6 7 8 9 10 -25 -20 -15 -10 -5 0 5 10 15 20 Do thi sai so van toc goc khop 44 KẾT LUẬN Bài tập lớn này đã tìm ra được kết quả như sau: Vị trí điểm thao tác E và hướng khâu thao tác. Tìm ra được các tọa độ suy rộng khi biết trước phương trình di chuyển của khâu thao tác. Thiết kế quỹ đạo trong không gian khớp và không gian làm việc. Tìm ra được phương trình vi phân chuyển động của Robot. Sử dụng bộ điều khiển PID để điều khiển trong không gian khớp. Tuy nhiên, vẫn còn nhiều khía cạnh mà bài tập lớn này bỏ qua vì thời gian và trình độ chưa cho phép. Các thông số bộ điều khiển còn phải điều chỉnh lại để được bộ tham số tối ưu hơn. Cuối cùng, em xin chân thành cảm ơn các thầy cô trong trường đã tạo ra được môi trường học tập tốt cho sinh viên, các thầy cô luôn hết sức hướng dẫn, truyền đạt cho sinh viên. Chân thành cảm ơn! 45 TÀI LIỆU THAM KHẢO 1. Phan Bùi Khôi, Bài giảng Robotics. Trường Đại học Bách khoa Hà Nội, 2009. 2. Nguyễn Thiện Phúc, Robot công nghiệp. NXB Khoa học và Kỹ thuật, 2002. 3. Nguyễn Văn Khang, Chu Anh Mỳ, Cơ sở Robot công nghiệp. NXB Giáo dục Việt Nam, 2011. 4. Lung Wen Tsai, Robot Analysis, The Machenics of Serial And Parallel Manipulator, NewYork, 1999. 5. Nguyễn Doãn Phước, Lý thuyết điều khiển tuyến tính. NXB Khao học và Kỹ thuật, Hà Nội, 2009. 6. Nguyễn Phùng Quang, Nguyễn Phùng Quang, Matlab & Simulink dành cho kỹ sư điều khiển tự động, NXB Khoa học kỹ thuật, 2003.