Đề tài Điều khiển chuyển động của Robot theo phương pháp Jacobian xấp xỉ khi động học và động lực học không biết chính xác

thanh nối kề trước nó. A1 mô tả vị trí và hướng của thanh nối đầu tiên, A2 mô tả vị trí và hướng của thanh nối thứ hai so với thanh nối thứ nhất. Như vậy vị trí và hướng của thanh nối thứ hai so với hệ toạ độ gốc được biểu diễn bởi ma trận : T2 = A1.A2 A3 mô tả vị trí và hướng của thanh nối thứ ba so với thanh nối thứ hai thì vị trí của thanh nối thứ 3 so với toạ độ gốc là: T3 = A1.A2.A3 Nếu một robot có 6 thanh nối, ta có ma trận biểu diễn vị trí và hướng của thanh nối cuối so với hệ tọa độ gốc: T6 = A1.A2.A3.A4.A5.A6 (2-10) Một robot 6 thanh nối tức là có 6 bậc tự do vì vậy có thể xác định được vị trí và hướng của tay robot (hand), 3 bậc tự do xác định vị trí và 3 bậc tự do khác xác định hướng. T6 sẽ là ma trận biểu diễn cả hướng và vị trí của tay robot (hand) so với khung tọa độ gốc. Gắn một khung tọa độ lên tay robot (hand) và tìm phép biến đổi biểu diễn khung tọa độ tay trong khung tọa độ gốc. Từ đó sẽ xây dựng được hệ phương trình động học thuận biểu diễn quan hệ giữa vị trí và hướng của tay so với các biến khớp. Hình 2.7. Khung tọa độ tay trong khung tọa độ gốc Hình 2.5 biểu diễn khung tọa độ tay trong khung tọa độ gốc. Gốc của khung tọa độ tay đặt tại điểm giữa của các ngón tay, được biểu diễn bởi vectơ trong khung tọa độ gốc. Ba vectơ đơn vị mô tả hướng của bàn tay được xác định như sau : - Vectơ có hướng mà theo đó bàn tay sẽ tiếp cận đến đối tượng gọi là vectơ (approach). - Vectơ có hướng mà theo đó các ngón tay của bàn tay nắm vào nhau khi cầm, nắm đối tượng gọi là vectơ ( orientation ). - Vectơ cuối cùng là vectơ pháp tuyến ( normal ). Như vậy ta có : Hệ toạ độ tay của Robot được biểu diễn bằng một ma trận 4x4: (2-11) Hay TE có dạng: (2-12) trong đó R(3x3) là ma trận chỉ hướng của bàn tay Robot (2-13) P(3x1) là ma trận biều diễn vị trí của tay Robot (2-14) 2.3. Bài toán động lực học thuận Với một robot đã biết cấu hình như: độ dài các thanh nối và góc quay các khớp hoặc độ dịch chuyển của các khớp tịnh tiến, bài toán động lực học thuận là tính toán vị trí và hướng của tay robot tương ứng với cấu hình robot xác định. Ngược lại, động học ngược sẽ tính toán các góc quay của khớp quay hoặc độ dịch chuyển của các khớp tịnh tiến tương ứng với vị trí và hướng của tay robot, nghĩa là tính toán các giá trị biến khớp cần thiết để đặt tay robot đúng vị trí mong muốn. Phương pháp chung để thiết lập phương trình động học thuận robot là sử dụng công cụ ma trận để miêu tả: vị trí, hướng và chuyển động sau đó sử dụng phép biểu diễn Denavit Hartenberg. 2.3.1. Phương pháp thiết kế khung tọa độ - Phép biểu diễn Danevit - Hartenberg 2.3.1.1. Tham số của thanh nối và khớp Xét 3 trục khớp trong không gian như hình sau: Hình 2.8. Thiết kế khung tọa độ thanh nối * an : Độ dài pháp tuyến chung của trục khớp n và n+1, chính là độ dài thanh nối n. * dn: Khoảng cách giữa hai chân pháp tuyến chung của trục n. * : Góc giữa hai trục của khớp n và khớp n+1. * : Góc giữa hai pháp tuyến chung của trục khớp n. 2.3.1.2. Nguyên tắc thiết kế khung tọa độ Theo phương pháp biểu điễn Danevit - Hartenberg (D-H), khung tọa độ thanh nối n được xây dựng theo nguyên tắc sau: + Gốc của khung toạ độ thanh nối n đặt tại giao điểm của pháp tuyến an với trục khớp thứ n+1. Trường hợp hai trục khớp cắt nhau, gốc khung toạ độ sẽ đặt tại chính giao điểm và trục x được đặt dọc theo đường vuông góc với mặt phẳng chứa hai trục z đó. Nếu các trục khớp song song với nhau, sẽ có nhiều pháp tuyến chung. Khi đó sẽ chọn pháp tuyến chung trùng với pháp tuyến chung của khớp trước. Gốc khung tọa độ chọn sao cho dn nhỏ nhất. + Trục zn của khung toạ độ thanh nối n đặt theo phương của trục khớp n+1. + Trục xn thường được đặt dọc theo pháp tuyến chung của trục n và n+1 theo hướng từ trục n đến n+1. + Đối với khớp tịnh tiến thì khoảng cách dn là biến khớp, hướng của trục khớp trùng với hướng di chuyển. Khi đó chiều dài an không có ý nghĩa nên đặt an = 0. Gốc tọa độ đặt trùng với gốc thanh nối tiếp theo. + Đối với khớp quay thì là biến khớp và dn = const. Trục xn được chọn sao cho thực hiện được phép quay từ zn-1 đến zn. Các thông số an, , dn và được gọi là bộ thông số DH. 2.3.1.3. Quan hệ giữa hai khung tọa độ n-1 và n Một cách tổng quát, quan hệ giữa hai khung tọa độ n và n-1 được xác định bằng các phép biến đổi theo thứ tự sau đây: - Quay quanh trục zn-1 một góc sao cho trục xn-1 trùng với phương của trục xn. - Tịnh tiến dọc theo trục zn-1 một khoảng dn để gốc khung tọa độ mới trùng với chân pháp tuyến chung của trục khớp n và trục khớp n+1. - Tịnh tiến dọc theo trục xn-1 (phương pháp tuyến chung) một đoạn an. - Quay quanh trục xn-1 một góc sao cho trục zn-1 trùng với trục zn. Các phép biến đổi trên được thực hiện so với khung tọa độ hiện tại (khung tọa độ ngay trước đó). Do đó phép biến đổi kết hợp được xác định như sau: (2-15) 2.3.2. Phương trình động học thuận của robot Gryphon Trình tự thiết lập hệ phương trình động học của robot Để thiết lập hệ phương trình động học của robot, ta tiến hành theo các bước sau: 1. Chọn hệ toạ độ cơ sở, gắn các hệ toạ độ mở rộng lên các thanh nối. 2. Lập bảng thông số DH (Denavit Hartenberg). 3. Dựa vào các thông số DH xác định các ma trận An 4. Tính ma trận T và viết các phương trình động học của robot Robot Gryphon có 5 bậc tự do nhưng trong đồ án này ta chỉ xét đến bậc thứ 3 (3 thanh nối). Khi áp dụng phương pháp Denavit-Hartenberg gắn các hệ trục toạ độ vào các khâu, ta thu được sơ đồ động học của Robot Gryphon như hình 2.9. Hình 2.9. Khung tọa độ thanh nối Theo thuật toán D-H ta có bảng tham số D-H ứng với sơ đồ động học trên: Trục a d 1 90 0 2 0 0 3 0 0 Bảng 2: Bảng tham số D-H Các ma trận chuyển tương ứng: (2-16) (2-17) (2-18) Vị trí và hướng của thanh nối thứ hai so với hệ toạ độ gốc được biểu diễn: (2-19) Phương trình động học thuận biểu diễn quan hệ giữa vị trí, hướng của tay và vị trí của các khớp. (2-20) Ma trận biểu diễn hướng của tay Robot: (2-21) Ma trận biểu diễn vị trí của tay Robot: (2-22) Để tìm miền làm việc của Robot ta dựa vào phương trình điểm tác động cuối: (2-23) 2.3.3. Ma trận Jacobian ý nghĩa của ma trận Jacobian là: - Biễu diễn quan hệ giữa tốc độ của tay và tốc độ của khớp: (2-24) - Biễu diễn quan hệ giữa dịch chuyển vi sai của tay và dịch chuyển vi sai của khớp: (2-25) Do đó ma trận Jacobian thu được bằng cách lấy vi phân phương trình động học thuận. (2-26) Hàng 1 của ma trận Jacobien (2-27) Tính hàng 2 của ma trận Jacobien (2-28) (2-29) Tính hàng 3 của ma trận Jacobien (2-30) (2-31) Như vậy ta có ma trận Jacobien: và: (2-32) Nếu chia cả 2 vế cho dt sẽ nhận được phương trình quan hệ tốc độ tay Robot và tốc độ khớp. Chương 3 động lực học robot gryphon Nội dung chương 3: -Bài toán động lực học. - Phương trình Lagrange. - Thành lập phương trình động lực học robot Gryphon. 3.1. Bài toán động lực học Động học thuận robot đã được nghiên cứu (ở mục 2.3.2) đã nghiên cứu mối quan hệ giữa vị trí, hướng, tốc độ của tay và vị trí, tốc độ của khớp nhưng chưa xem xét đến các lực gây ra các chuyển động. Trong quá trình di chuyển, robot tiếp xúc với môi trường, cần phải sinh ra một lực cần thiết để di chuyển vật và thực hiện công việc. Để làm khớp robot di chuyển tịnh tiến hoặc quay, cơ cấu chấp hành cần sinh một lực hoặc mômen đủ lớn. Mối quan hệ giữa lực, mômen của các khớp với vị trí, tốc độ và gia tốc được biểu diễn trong phương trình chuyển động, còn gọi là phương trình động lực học. Trong phương trình động lực học, lực và mômen là các tín hiệu vào. Dựa vào phương trình động lực học, sẽ tính được lực, mômen cần thiết để khớp robot có thể chuyển động được với tốc độ và gia tốc mong muốn. Phương trình động lực học cũng được sử dụng cho đánh giá ảnh hưởng của khối lượng tải đối với các chuyển động của robot. Giải phương trình động lực học sẽ nhận được các chuyển động của robot với các đầu vào là lực và mômen của các khớp, là cơ sở cho thiết kế hệ thống điều khiển robot. Trong phần này, sẽ trình bày phương pháp Lagrange tổng quát sau đó áp dụng để xây dựng phương trình động lực học của robot Gryphon. 3.2. Phương trình Lagrange Phương trình động lực học robot được thiết lập từ phương trình Lagrange: (3-1) trong đó: với là momen khớp i. (3-2) với: là hàm Lagrange là hàm tổng động năng, là động năng của thanh nối i là hàm tổng thế năng, là thế năng của thanh nối i Thế (2-) vào (2-) ta được: (3-3) (vì ) Dạng tương đương của phương trình (2-21): (3-4) với i = 1, 2, 3. 3.3. Phương trình động lực học của Robot Gryphon 3.3.1.Động năng và thế năng của các thanh nối 3.3.1.1. Động năng và thế năng của thanh nối 1: (3-5) Từ (2-16) ta có (3-6) (3-7) Nên (3-8) (3-9) 3.3.1.2. Động năng và thế năng của thanh nối 2: (3-10) (3-11) (3-12) (3-13) 3.3.1.3. Động năng và thế năng của thanh nối 3: (3-14) (3-15) (3-16) (3-17) 3.3.1.4. Tổng động năng và thế năng: (3-18) (3-19) (3-20) 3.3.2. Phương trình động lực học viết cho các thanh nối 3.3.2.1.Phương trình động lực học viết cho thanh nối 1 (3-21) (3-22) (3-23) (3-24) Phương trình động học viết cho thanh nối 1: (3-25) Với (3-26) (3-27) (3-28) (3-29) (3-30) 3.3.2.2. Phương trình động lực học viết cho thanh nối 2 (3-31) (3-32) (3-33) (3-34) Phương trình động học viết cho thanh nối 2: (3-35) Với (3-36) (3-37) (3-38) (3-39) (3-40) 3.3.2.3.Phương trình động lực học viết cho thanh nối 3 (3-41) (3-42) (3-43) (3-44) Phương trình động học viết cho thanh nối 3: (3-45) Với (3-46) (3-47) (3-48) (3-49) 3.3.2.4. Phương trình động lực học tổng quát của robot Gryphon Kết hợp 3 phương trình (3-25), (3-35), (3-45) thu được phương trình động lực học tổng quát của robot Gryphon có dạng: (3-50) trong đó: M() là ma trận dương đối xứng, , , Biểu diễn: thì (3-50) trở thành: (3-51) là ma trận 3x3, ma trận này không tồn tại duy nhất tuy nhiên ta cần tìm ma trận sao cho là ma trận 3x3 đối xứng ngược thỏa mãn: với hay Theo [TL - 4], có 1 cách tính để tìm ra được ma trận như sau: (3-52) với , i, j, k = 1, 2, 3 (3-53) Theo (3-52) và (3-53) sẽ tính được các phần tử của ma trận C(): (3-54) (3-55) (3-56) (3-57) (3-58) (3-59) (3-60) (3-61) (3-62) Kiểm tra lại công thức ta thấy thỏa mãn. S được tính theo theo biểu thức: S= - (3-63) Trước hết tính từ M(): (3-64) (3-65) (3-66) (3-67) (3-68) (3-69) Thay và vào (3-63) tính được S(): S11 = 0, S22 = 0, S33 = 0 (3-70) (3-71) (3-72) (3-73) Thay C() = vào (3-51) thì phương trình động lực học trở thành: (3-74) CHƯƠNG 4 ĐIềU KHIểN CHUYểN Động robot Gryphon với động học và động lực học chính xác Nội dung chương 4: - Phân loại yêu cầu điều khiển chuyển động. - Phân loại các hệ thống điều khiển chuyển động. - Hệ thống điều khiển phản hồi bù trọng lực. - Hệ thống điều khiển mômen tính toán. - Hệ thống điều khiển ma trận Jacobian chuyển vị và Jacobian nghịch đảo. 4.1. Khái quát 4.1.1. Phân loại yêu cầu điều khiển chuyển động Phân loại thành: điều khiển vị trí và điều khiển bám quỹ đạo. 4.1.1.1. Điều khiển vị trí Bài toán điều khiển vị trí là bài toán phải đảm bảo tay robot (end effector) di chuyển tới vị trí mong muốn mà không cần quan tâm tới quỹ đạo chuyển động từ vị trí ban đầu tới vị trí mong muốn. Hình 4.1. Tay robot chuyển động từ điểm đầu đến điểm mong muốn 4.1.1.2. Điều khiển bám quỹ đạo Bài toán điều khiển bám quỹ đạo là bài toán phải đảm bảo tay robot bám theo một đường cong (hay quỹ đạo) liên tục mong muốn từ vị trí đầu đến vị trí cuối. Bài toán điều khiển bám quỹ đạo thông thường gồm 3 giai đoạn: lập phương án chuyển động (motion planing), thiết kế quỹ đạo (trajectory generation) và thiết kế bộ điều khiển bám quỹ đạo (control design). Trong giai đoạn lập phương án chuyển động, bằng kinh nghiệm và hiểu biết của mình, người thiết kế lập ra quỹ đạo cho tay máy từ vị trí đầu đến vị trí cuối sao cho tránh được các chướng ngại vật mà chưa cần quan tâm đến thời gian chuyển động. Sang giai đoạn thiết kế quỹ đạo, người thiết kế phải lập quỹ đạo chuyển động theo đường chuyển động mong muốn đã chọn ở giai đoạn trước với vận tốc và gia tốc chuyển động là hàm của thời gian. Cuối cùng là thiết kế bộ điều khiển bám theo quỹ đạo chuyển động mong muốn. Trong đồ án này, coi như đã làm xong 2 giai đoạn đầu, nhiệm vụ đặt ra là thiết kế bộ điều khiển bám theo quỹ đạo chuyển động mong muốn. Hình 4.2. Tay robot bám theo quỹ đạo chuyển động mong muốn 4.1.2. Phân loại các hệ thống điều khiển chuyển động Hệ thống điều khiển chuyển động có chức năng đảm bảo tay robot (end effector) chuyển động bám theo quỹ đạo đặt trước trong môi rường làm việc (không gian làm việc). Chuyển động của tay robot được thực hiện nhờ các hệ thống truyền động khớp robot. Ta có thể phân loại các hệ thống điều khiển chuyển động như sau: 4.1.2.1. Phân loại theo không gian điều khiển, ta có hệ thống điều khiển không gian khớp và hệ thống điều khiển không gian làm việc. ** Với hệ thống điều khiển không gian khớp, đại lượng được điều khiển là vị trí của khớp robot (góc quay đối với khớp quay, độ dịch chuyển thẳng đối với khớp tịnh tiến). Vị trí đặt của khớp được tính toán từ lượng đặt vị trí của tay robot trong không gian làm việc thông qua khâu tính toán động học ngược. Động học ngược Bộ điều khiển BBĐ Động cơ Cơ cấu robot Cảm biến Xd qd X q Hình 4.3. Sơ đồ khối hệ thống điều khiển ở không gian khớp Trong sơ đồ trên thì Xd, X tương ứng là vectơ vị trí đặt và vectơ vị trí thực của tay robot; qd, q tương ứng là vectơ vị trí đặt và vectơ vị trí thực của khớp robot. ** Với hệ thống điều khiển ở không gian làm việc, lượng đặt là vị trí đặt tay trong không gian làm việc và lượng phản hồi là vị trí thực của tay. Khâu tính toán động học ngược sẽ thuộc mạch vòng điều khiển phản hồi. Bộ điều khiển BBĐ Động cơ Cơ cấu robot Cảm biến Xd X Hình 4.4. Sơ đồ khối hệ thống điều khiển ở không gian làm việc 4.1.2.2. Phân loại theo mức độ ràng buộc của robot, ta có hệ thống điều khiển phân tán và hệ thống điều khiển tập trung. Đối với các robot có tỉ số truyền của bộ truyền lớn, có thể coi hệ thống robot n bậc tự do sẽ gồm n hệ thống độc lập 1 đầu vào/ 1 đầu ra (SISO) và sự ràng buộc giữa các khớp được coi là thành phần nhiễu, có thể bỏ qua. Bởi vậy bộ điều khiển của các khớp được thiết kế độc lập với nhau. Đó chính là hệ thống điều khiển phân tán. Hệ thống điều khiển tập trung được xây dựng cho các robot có tỉ số truyền của bộ truyền nhỏ, khi đó robot là 1 hệ thống có tính ràng buộc và phi tuyến cao gồm nhiều đầu vào và nhiều đầu ra (MIMO). Đồ án này sẽ đi sâu nghiên cứu hệ thống điều khiển tập trung và không quan tâm tới hệ thống điều khiển phân tán. 4.1.2.3. Phân loại theo sự thay đổi tham số, ta có hệ thống điều khiển không thích nghi, hệ thống điều khiển thích nghi và hệ thống điều khiển bền vững. Hệ thống điều khiển không thích nghi là hệ thống có tham số bộ điều khiển cố định, được áp dụng khi tham số robot không thay đổi. Hệ thống điều khiển thích nghi là hệ thống mà khi tham số robot không được xác định chính xác hoặc biến đổi thì bộ điều khiển sẽ tự hiệu chỉnh tham số cho phù hợp. Hệ thống này đòi hỏi phải có khâu nhận dạng thích nghi (tức là phải có luật thích nghi). Hệ thống này có thể điều khiển chính xác nhưng mà chậm. Hệ thống điều khiển bền vững là hệ thống điều khiển cũng được áp dụng khi tham số robot biến đổi tuy nhiên hệ thống này không đòi hỏi khâu nhận dạng. Đặc điểm của hệ thống điều khiển bền vững là tham số của bộ điều khiển sẽ bền vững trong trong một giới hạn của các tham số robot. Tuy nhiên cấu trúc bộ điều khiển sẽ biến đổi cho phù hợp với sự thay đổi tham số dựa theo nguyên lý điều khiển trượt hoặc nguyên lý điều khiển mờ. 4.2. Một số bộ điều khiển 4.2.1. Hệ thống điều khiển trong không gian khớp Với hệ thống điều khiển không gian khớp, đại lượng được điều khiển là vị trí của khớp robot (góc quay đối với khớp quay, độ dịch chuyển thẳng đối với khớp tịnh tiến). Trong đồ án này ta coi robot là một hệ thống phi tuyến và ràng buộc vì vậy ta chỉ quan tâm tới các hệ thống điều khiển tập trung. Khi thiết kế hệ thống điều khiển tập trung thì có thể bỏ qua động học của cơ cấu chấp hành bao gồm quán tính của động cơ và bộ biến đổi. Như vậy chức năng của bộ điều khiển là tạo ra một mômen cần thiết để truyền động khớp robot đảm bảo khớp robot luôn bám theo vị trí đặt. 4.2.1.1. Hệ thống điều khiển phản hồi PD bù trọng lực 4.2.1.1.1. Cơ sở lý thuyết Bộ điều khiển Cơ cấu robot Cảm biến qd q Hình 4.5. Sơ đồ khối tổng quát của hệ thống điều khiển phản hồi Trong sơ đồ trên, qd là vectơ tín hiệu đặt vị trí của các khớp (qd = đối với khớp quay và qd = rd đối với khớp tịnh tiến), q là vectơ vị trí thực của các khớp robot tương ứng là với khớp quay và r với khớp tịnh tiến, là vectơ mômen đối với khớp quay và lực đối với khớp tịnh tiến. Phương trình động lực học tổng quát của robot có dạng: (4-1) hoặc (4-2) trong đó q = [q1, q2, , qn]T , ; là ma trận dương đối xứng; , , là ma trận đối xứng ngược. Từ chương này, ta sẽ dùng q thay cho và qi thay cho để kí hiệu cho các biến khớp nhằm làm tiện lợi hơn trong việc lập trình và mô phỏng với Matlab và Simulink. Luật điều khiển có cấu trúc dạng tỷ lệ - đạo hàm (PD): +) (4-3) +) (4-4) trong đó: Kp = diag(kp1, kp2, , kpn) là ma trận đường chéo các hệ số khuếch đại của từng khớp riêng biệt. Kd = diag(kd1, kd2, , kdn) là ma trận đường chéo các hệ số đạo hàm của từng khớp riêng biệt. là sai số vị trí của khớp robot, với qd = [qd1, qd2, , qdn]T là vectơ vị trí đặt của các khớp robot. là sai số tốc độ khớp robot. Hệ thống điều khiển với cấu trúc điều khiển (4-3), (4-4) đã được chứng minh là ổn định tuyệt đối xung quanh điểm cân bằng, không phụ thuộc vào khối lượng thanh nối và tải dựa vào lý thuyết ổn định Lyapunov. Thật vậy, sau đây ta sẽ trình bày cách chứng minh đối với luật điều khiển (4-3). Đặt biến trạng thái của hệ thống là: . Chọn hàm Lyapunov có dạng: VL = (4-5) Hàm VL biểu thị tổng năng lượng của hệ thống robot: thành phần biểu thị thế năng tích lũy trong hệ thống có hệ số tỷ lệ là Kp, thành phần là động năng robot. Do Kp, M(q) là các ma trận đối xứng dương nên VL > 0 với Tính đạo hàm cấp 1 của VL ta được: (4-6) Do qd là hằng số nên . Vì Kp, M(q) là các ma trận đối xứng dương nên: và Sử dụng các ràng buộc trên, phương trình (4-6) trở thành: (4-7) Cân bằng phương trình luật điều khiển (4-3) và phương trình động lực học robot (4-2): (4-8) Thay (4-8) vào (4-7) được: (4-9) Vì S(q,) là ma trận đối xứng ngược nên do đó (4-9) trở thành: (4-10) Từ (4-5) và (4-10), theo tiêu chuẩn ổn định Lyapunov, ta có khi t. Như vậy ở trạng thái xác lập, các thành phần tốc độ và gia tốc đều bằng 0 do đó thay vào phương trình hệ thống kín (4-8) sẽ có: 4.2.1.1.2. Thiết kế bộ điều khiển phản hồi PD bù trọng lực cho robot Gryphon + Phương trình động học và động lực học của robot Gryphon đã được thiết lập trong chương 2 và chương 3. Các tham số của robot: m1 = 15 kg, m2 = 12 kg, m3 = 8 kg; l1 =0.135 m; l2 = l3 =0.225 m; lg1 = l1/2, lg2 = l2/2, lg3 = l3/2, mômen quán tính các thanh nối: + Hệ phương trình động lực học của robot có dạng tổng quát: (4-11) trong đó ; M(q) , V(,) và G(q) đã được xác định trong chương 3. + Luật điều khiển có dạng (4-3): (4-12) với qd = [qd1, qd2, qd3]T ; Kp , Kd là các ma trận đường chéo dương. + Cân bằng (4-11) và (4-12) ta được: (4-13) Từ (4-13) ta xây dựng sơ đồ mô phỏng trong Simulinhk như hình 4.6. Các file có đuôi “.m” đi kèm theo phục vụ cho việc mô phỏng được trình bày trong phần phụ lục. Hình 4.6. Sơ đồ mô phỏng hệ điều khiểnphản hồi PD bù trọng lực trong không gian khớp + Để điều khiển điểm đặt với vectơ vị trí đặt của các khớp qd = [qd1, qd2, qd3]T = [1, 1.5, 1.3]T, thay đổi kp trong khoảng [200 , 300] và thay đổi kd trong khoảng [20 , 50], ta chọn được các ma trận hệ số Kp và Kd cho chất lượng hệ thống tốt nhất: và Sau đây là kết quả mô phỏng các góc quay của các khớp 1, 2 và 3: Hình 4.7. Góc quay khớp 1 Hình 4.8. Góc quay khớp 2 Hình 4.9. Góc quay khớp 3 Nhận xét kết quả mô phỏng: +) Từ các hình 4.7, 4.8, 4.9 ta thấy hệ thống điều khiển phản hồi PD bù trọng lực trong không gian khớp đã thiết kế là ổn định với các chỉ tiêu chất lượng sau: - Không có độ quá điều chỉnh. - Sai lệch tĩnh bằng 0. - Thời gian quá độ tương đối nhỏ (nhỏ hơn 1s). Đó là vì: ta đã bỏ qua các lực ma sát trong hệ thống cơ khí của robot, bỏ qua quán tính của cơ cấu chấp hành, bỏ qua nhiễu, , coi đáp ứng mô men là tức thời. Bởi vậy trong thực tế, chắc chắn thời gian quá độ sẽ lớn hơn. +) ảnh hưởng của ma trận hệ số Kp, Kd tới chất lượng của hệ thống điều khiển: - Giữ nguyên kp: khi tăng kd trong khoảng [10 , 25] thì độ quá điều chỉnh giảm, thời gian quá độ giảm và khi giảm kd thì ngược lại. - Giữ nguyên kd: khi tăng kp trong khoảng [100 , 200] thì thời gian quá độ giảm và khi giảm kp thì ngược lại. 4.2.1.2. Hệ thống điều khiển mômen tính toán Phương pháp này còn được gọi là phương pháp điều khiển động lực học ngược hoặc phương pháp điều khiển theo mô hình. a. Nguyên lý điều khiển Vì robot là 1 hệ thống ràng buộc và phi tuyến cao nên ta sẽ lựa chọn luật điều khiển sao cho khử được các thành phần phi tuyến của phương trình động lực học robot và phân ly đặc tính động lực học các thanh nối. Do đó ta sẽ nhận được 1 hệ thống tuyến tính, từ đó dễ dàng thiết kế theo các phương pháp kinh điển của hệ thống tuyến tính đảm bảo độ chính xác chuyển động yêu cầu. Sau đây là sơ đồ khối hệ thống: ĐK2 ĐK1 RB U q qd, q, Hình 4.10. Sơ đồ khối hệ thống điều khiển mômen tính toán trong sơ đồ trên thì q , tương ứng là vectơ vị trí thực và vectơ tốc độ thực của các khớp robot; qd , tương ứng là vectơ vị trí đặt và vectơ tốc độ đặt của các khớp robot; là mô men ở các khớp của robot; U là vectơ tín hiệu điều khiển phụ. Bộ ĐK1 là bộ điều khiển phi tuyến có tác dụng khử tính phi tuyến và ràng buộc của hệ thống robot. Bộ ĐK2 là bộ điều khiển tuyến tính. b. Phương pháp thiết kế bộ điều khiển mômen tính toán Bộ ĐK1: Dựa vào phương trình động lực học robot (4-1), giả thiết các thông số robot đã biết hoặc được xác định chính xác (tức là M(q), V(q), G(q) đã được xác định chính xác), luật điều khiển của bộ ĐK1 được chọn như sau: (4-14) trong đó U là vectơ tín hiệu điều khiển phụ. Cân bằng hai phương trình (4-1) và (4-14), do M(q) là ma trận thực dương có thể lấy nghịch đảo nên ta thu được phương trình vi phân tuyến tính cấp 2: (4-15) (4-15) là hệ phương trình vi phân tuyến tính cấp 2 độc lập với từng khớp. Do đó có thể thiết kế các bộ điều khiển độc lập có cấu trúc PD cho từng khớp. Bộ ĐK2: Từ lập luận ở trên ta xây dựng luật điều khiển phụ U có cấu trúc PD như sau: U = (4-16) với ; e = qd - q, tương ứng là sai lệch vị trí khớp và sai lệch tốc độ khớp; Kp , Kd là các ma trận đường chéo dương. Các ma trận hệ số Kp, Kd được xác định như sau: Thay (4-15) vào (4-16) ta được: (4-17) Trong đó: là sai số gia tốc khớp. Viết cho khớp thứ i: (4-18) Phương trình đặc tính viết ở dạng toán tử Laplace: (4-19) Các hệ số kpi, kdi được tính toán theo các tiêu chuẩn ổn định và hội tụ. c. Ưu, nhược điểm của hệ thống điều khiển mômen tính toán: Ưu điểm: - Khử được tính phi tuyến và ràng buộc của hệ thống robot. - Khử được sai lệch vị trí khớp Nhược điểm: - Khối lượng tính toán lớn. - Hệ thống điều khiển mômen tính toán chỉ xây dựng được khi biết chính xác các tham số của robot. 4.2.2. Hệ thống điều khiển trong không gian làm việc ở hệ thống điều khiển không gian làm việc, tín hiệu đặt trực tiếp là quỹ đạo chuyển động mong muốn của tay robot trong không gian làm việc, lượng phản hồi sẽ được tính từ vị trí của khớp thông qua khâu động học thuận. Hai hệ thống điều khiển điển hình là: Hệ thống điều khiển ma trận Jacobian chuyển vị và hệ thống điều khiển ma trận Jacobian nghịch đảo. 4.2.2.1. Hệ thống điều khiển ma trận Jacobian chuyển vị 4.2.2.1.1. Cơ sở lý thuyết Lực cần thiết để di chuyển tay theo quỹ đạo đặt trong không gian làm việc được xác định từ sai lệch vị trí và sai lệch tốc độ trong không gian làm việc theo luật điều khiển phản hồi PD kinh điển: (4-20) với: F là lực cần thiết để tay robot di chuyển theo quỹ đạo và tốc độ đặt trước. Xd , X tương ứng là vectơ vị trí đặt và vectơ vị trí thực của tay robot. , tương ứng là vectơ tốc độ đặt và vectơ tốc độ thực của tay robot. Kp = diag(kp1, kp2, , kpn) là ma trận đường chéo các hệ số khuyếch đại. Kd = diag(kd1, kd2, , kdn) là ma trận đường chéo các hệ số đạo hàm. Công thực hiện ở tay được tính theo công thức: FT . (với F là lực tác dụng ở tay, là vectơ dịch chuyển nhỏ của tay). Công thực hiện ở khớp được tính theo công thưc: . (với là mômen hoặc lực ở khớp, là vectơ dịch chuyển nhỏ của khớp). Công thực hiện ở tay sẽ cân bằng với công thực hiện ở khớp: FT . = . (4-21) Mặt khác l