Đề 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

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ại có: (với J là ma trận Jacobian) (4-22) Thay (4-22) vào (4-21) thu được: (4-23) Thay (4-20) vào (4-23) xác định được vectơ mômen truyền động khớp như sau: (4-24) Để bù được thành phần trọng lực của robot thì phải dùng luật điều khiển sau: (4-25) Từ (4-25) ta xây dựng được sơ đồ khối hệ thống điều khiển vị trí tay robot sử dụng ma trận Jacobian chuyển vị như sau: KP Kd RB ĐHT J Xd F q X JT + + Kp G(q) RB + Tính G(q) G(q) Hình 4.11. Sơ đồ khối hệ thống điều khiển ma trận Jacobian chuyển vị 4.2.2.1.2. Thiết kế bộ điều khiển ma trận Jacobian chuyển vị cho robot Gryphon + Các thông số của robot vẫn giữ nguyên như trong mục 4.2.1.1.2. + Luật điều khiển có dạng (4-25): với là vectơ mô men ở các khớp; J là ma trận Jacobian, đã xác định trong chương 2; Kp , Kd là các ma trận đường chéo dương; 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; q là vị trí thực của các khớp robot; G(q) là thành phần trọng lực robot, đã xác định trong chương 3. Sơ đồ mô phỏng trong Simulink như hình 4.12 (các file có đuôi “.m” đi kèm được trình bày trong phần phụ lục). Hình 4.12. Sơ đồ Simulink mô phỏng hệ thống điều khiển ma trận Jaccobian chuyển vị + Để điều khiển điểm đặt với vectơ trị đặt của tay Xd = = [0.3, 0.16, 0.25]T, thay đổi hệ số kp trong khoảng [100 , 200] và thay đổi hệ số kd trong khoảng [5 , 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à Kết quả mô phỏng tọa độ x, y và z của điểm tác động cuối của tay so với phương ngang như sau: Hình 4.13. Tọa độ x của điểm tác động cuối Hình 4.14. Tọa độ y của điểm tác động cuối Hình 4.15. Tọa độ z của điểm tác động cuối Nhận xét kết quả mô phỏng: +) Từ các hình 4.13, 4.14, 4.15 ta thấy hệ thống điều khiển ma trận Jacobian chuyển vị đã thiết kế là ổn định với các chỉ tiêu chất lượng: - Không có độ quá điều chỉnh. - Sai lệch tĩnh bằng 0. - Thời gian quá độ tương đối nhỏ (khoảng 1s). Giải thích như trong mục 4.2.1.1.2. +) ảnh hưởng của các ma trận hệ số Kp, Kd tới chất lượng hệ thống: - 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. - Giữ nguyên kp: khi tăng kd trong khoảng [5 , 50] thì thời gian quá độ giảm, độ quá điều chỉnh giảm và khi giảm kd thì ngược lại. 4.2.2.2. Hệ thống điều khiển ma trận Jacobian nghịch đảo 4.2.2.2.1. Cơ sở lý thuyết Sai lệch vị trí và sai lệch tốc độ của tay robot được định nghĩa như sau: , trong đó 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. Khi coi sai lệch vị trí và sai lệch tốc độ của tay robot là các đại lượng nhỏ thì sai lệch vị trí khớp và sai lệch tốc độ khớp robot được xác định bởi phương trình sau: trong đó tương ứng là sai lệch vị trí khớp và sai lệch tốc độ khớp robot; J là ma trận Jacobian. Tương tự như luật phản hồi PD kinh điển (trong hệ thống điều khiển robot tập trung), vectơ mômen cần thiết truyền động cho khớp robot được xác định như sau: (4-26) trong đó Kp , Kd là các ma trận đường chéo dương. Do đó phương trình (4-26) được viết lại: (4-27) Để bù được thành phần trọng lực của robot thì phải dùng luật điều khiển sau: + G(q) (4-28) KP Kd RB ĐHT J Xd F q X J-1 + + Kp G(q) RB + G(q) Hình 4.16. Sơ đồ khối hệ thống điều khiển ma trận Jacobian nghịch đảo 4.2.2.2.2. Thiết kế bộ điều khiển ma trận Jacobian nghịch đảo cho robot Gryphon + Các thông số robot vẫn giữ nguyên như trong mục 4.2.1.1.2. + Luật điều khiển có dạng (4-28): + G(q) với là vectơ mô men ở các khớp; J là ma trận Jacobian, đã xác định trong chương 2; Kp , Kd là các ma trận đường chéo dương; 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; q là vị trí thực của các khớp robot; G(q) là thành phần trọng lực robot, đã xác định trong chương 2. Sơ đồ mô phỏng trong Simulink như hình 4.17 nhưng luật điều khiển thì khác (các file có đuôi “.m” đi kèm theo được trình bày trong phần phụ lục). Hình 4.17. Sơ đồ Simulink mô phỏng hệ thống điều khiển ma trận Jaccobian nghịch đảo + Để điều khiển điểm đặt với vectơ vị trí đặt của tay Xd = = [0.3, 0.16, 0.25]T, thay đổi hệ số kp trong khoảng [50 , 150] và thay đổi hệ số kd trong khoảng [5 , 30], 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à Kết quả mô phỏng tọa độ x, y và z như sau: Hình 4.18. Tọa độ x của điểm tác động cuối Hình 4.19. Tọa độ y của điểm tác động cuối Hình 4.20. Tọa độ z của điểm tác động cuối Nhận xét kết quả mô phỏng: +) Từ các hình 4.18, 4.19, 4.20 ta thấy hệ thống điều khiển ma trận Jacobian nghịch đảo là ổn định với các chỉ tiêu chất lượng: - 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). Giải thích như trong mục 4.2.1.1.2. +) ảnh hưởng của các ma trận hệ số Kp và Kd tới chất lượng hệ thống: - Giữ nguyên kd: khi tăng kp trong khoảng [50 , 120] thì thời gian quá độ giảm và khi tăng kp trong khoảng [120 , 150] thì xảy ra hiện tượng quá điều chỉnh. - Giữ nguyên kp: khi tăng kd trong khoảng [5 , 15] thì độ quá điều chỉnh giảm, thời gian quá độ giảm và khi tăng kd trong khoảng [15 , 30] thì thời gian quá độ tăng. 4.3. Nhận xét Trong các hệ thống điều khiển ở trên khi thiết kế đều đòi hỏi phải biết chính xác các thông số động học của robot tức là ma trận Jacobian phải được xác định chính xác. Khi động học robot là không chính xác thì không thể thu được góc quay mong muốn của các khớp từ quỹ đạo tay mong muốn bằng cách giải bài toán động học ngược. Hơn nữa ma trận Jacobian cũng không thể xác định chính xác nếu động học robot là không chính xác. Do đó, với các hệ thống điều khiển ở trên, khi động học thu được là không chính xác thì robot không thể thực hiện thao tác đưa vật thể tới một vị trí mong muốn. Thật vậy, quay trở lại mục 4.2.2.1, bộ điều khiển ma trận Jacobian chuyển vị đã được thiết kế cho robot Gryphon với giả thiết là biết chính xác các thông số của robot. Giả sử chiều dài thực của các thanh nối là: l1 =0.135 m, l2 =0.225 m, l3 = 0.225 m. Nhưng thực tế đo được chiều dài các thanh nối có giá trị: = 0.14 m, = 0.25 m, = 0.25 m và giả sử các thông số khác vẫn biết chính xác. Khi đó ma trận Jacobian trong luật điều khiển được tính toán với các giá trị đo được (không chính xác). Sơ đồ mô phỏng trong Simulink như trong hình 4.20. Các file có đuôi “.m” đi kèm theo được trình bày trong phần phụ lục. Hình 4.21. Sơ đồ mô phỏng hệ điều khiển Jacobian chuyển vị với động học không chính xác Các ma trận hệ số Kp và Kd vẫn giữ nguyên như trong mục 4.2.2.1. Ta có kết quả mô phỏng: Hình 4.22. Tọa độ x của điểm tác động cuối Hình 4.23. Tọa độ y của điểm tác động cuối Hình 4.24. Tọa độ z của điểm tác động cuối Từ kết quả mô phỏng trên các hình 4.22,4.23, 4.24, ta thấy khi các thông số động học robot không thể xác định chính xác thì sẽ có sai lệch tĩnh là không nhỏ (khác 0) và nếu sai số của các thông số động học đo được là càng lớn thì sai lệch tĩnh sẽ càng lớn. Bởi vậy tay robot không thể chuyển động tới vị trí mong muốn. Chương 5 điều khiển chuyển động robot Gryphon theo phương pháp jacobiAn xấp xỉ Nội dung chương 5: - Đặt vấn đề. - Điều khiển điểm đặt theo phương pháp Jacobian xấp xỉ. - Điều khiển bám quỹ đạo theo phương pháp Jacobian xấp xỉ thích nghi. 5.1. Đặt vấn đề Trong lĩnh vực điều khiển robot, hầu hết các nghiên cứu đã thừa nhận động học của robot là biết chính xác do đó ma trận Jacobian được xác định chính xác. Nhưng trên thực tế thì không có một thông số vật lý nào có thể thu được một cách chính xác. Thêm nữa, khi robot gắp các vật thể có chiều dài khác nhau, có hướng và điểm kẹp chưa biết chính xác thì động học robot thay đổi bởi vậy rất khó để thu được động học chính xác. Khi động học robot là không chính xác thì không thể tìm được góc quay mong muốn của các khớp từ quỹ đạo tay mong muốn bằng cách giải bài toán động học ngược. Hơn nữa ma trận Jacobian cũng không thể xác định chính xác nếu động học robot là không chính xác. Với các phương pháp điều khiển trong không gian khớp thì cần phải giải bài toán động học ngược để tìm ra vị trí khớp mong muốn. Với các phương pháp điều khiển trong không gian Đề các thì các luật điều khiển vẫn đòi hỏi ma trận Jacobian chính xác hay động học thuận chính xác. Do đó hệ thống điều khiển robot không thể thích nghi với sự không biết chính xác động học hoặc những thay đổi không biết trước nếu việc điều khiển chuyển động robot đòi hỏi phải biết chính xác. Bởi vậy, robot không thể thực hiện thao tác đưa vật thể tới một vị trí mong muốn nếu chiều dài hoặc điểm kẹp của vật thể là không biết chính xác. Khi bài toán điều khiển được mở rộng cho điều khiển bàn tay robot có nhiều ngón tay thì sự thừa nhận động học chính xác sẽ giới hạn khả năng ứng dụng của robot vì động học thường là không biết chính xác trong nhiều ứng dụng của bàn tay robot. Ví dụ, các điểm tiếp xúc của các ngón tay robot thường là không biết chính xác và thay đổi khi robot thực hiện thao tác. Từ đó, đồ án này sẽ đi sâu tìm hiểu một vài luật điều khiển ma trận Jacobian xấp xỉ khi mà không biết chính xác động học và động lực học robot. Bằng cách phản hồi vị trí của tay robot (end effector) thì tay robot có thể bám theo quỹ đạo chuyển động mong muốn khi không biết chính xác động học và động lực học robot. Như vậy, áp dụng lý thuyết điều khiển này sẽ làm cho robot có một mức độ linh hoạt cao trong việc xử lý những thay đổi không biết trước và sự không biết chính xác động học và động lực học của nó. 5.2. Điều khiển điểm đặt với phương pháp Jacobian xấp xỉ 5.2.1. Nhắc lại phương trình động lực học và phương trình động học tổng quát Phương trình động lực học tổng quát của robot n bậc tự do biểu diễn trong không gian khớp q = [q1, q2, , qn]T có dạng: (5-1) trong đó: M(q) là ma trận quán tính dương đối xứng, là mômen tại các khớp của robot, G(q) là thành phần trọng lực của robot, S(q,) là ma trận đối xứng ngược (tức là = 0 với ). Phương trình động học thuận robot có dạng: X = h(q) (5-2) trong đó X biểu diễn vị trí và hướng của cơ cấu tác động cuối (end effector). Phương trình biểu diễn quan hệ giữa tốc độ tay và tốc độ khớp : (5-3) trong đó J(q) là ma trận Jacobian. 5.2.2. Điều khiển điểm đặt theo phương pháp Jacobian xấp xỉ Mục này sẽ trình bày bộ điều khiển phản hồi Jacobian xấp xỉ dùng cho điều khiển điểm đặt của robot với động học và động lực học không biết chính xác. Quá trình thiết kế bộ điều khiển được chia thành 2 giai đoạn: trước hết bỏ qua thành phần trọng lực (giả sử đã bù được) sau đó lại xét đến thành phần trọng lực. 5.2.2.1. Bộ điều khiển điểm đặt Jacobian xấp xỉ bù trọng lực 5.2.2.1.1. Cơ sở lý thuyết Luật điều khiển [TL - 1]: (5-4) trong đó được chọn lựa sao cho: (5-5) và e = X-Xd = [e1, e2, , en]T là sai lệch khỏi vị trí mong muốn Xd , p là 1 hằng số xác định dương, s(e) = [s1(e1), , sn(en)]T , Kp = kp I, Bv = bv I với I là ma trận đơn vị, kp là tín hiệu phản hồi thu được từ sai lệch vị trí, bv là tín hiệu phản hồi thu được từ vận tốc khớp. si(ei) với i = 1, , n là các hàm bão hòa sẽ định nghĩa ngay sau đây. X là vị trí của điểm tác động cuối được đo bởi hệ thống quan sát hoặc hệ thống cảm biến vị trí Hàm Si(ei) trong đó là 1 hàm vô hướng. Hàm Si(ei) và đạo hàm của nó có các tính chất sau: * Si(ei) > 0 với ei 0 và Si(ei) = 0. * Si(ei) có khả năng vi phân 2 lần liên tiếp và đạo hàm là hàm tăng dần trong khoảng nghĩa là: si(ei) = s0 khi ei và si(ei) = -s0 khi ei - trong đó s0 là 1 hằng số dương. * Tồn tại 1 hằng số ci sao cho: với ei 0 (5-6) Thay thế (5-4) vào (5-1) thu được: (5-7) Để thực hiện phân tích ổn định cho hệ thống kín với bộ điều khiển Jacobian xấp xỉ, chọn hàm Lyapunov sau: V = (5-8) Có: = trong đó và là trị riêng lớn nhất của ma trận A. Do đó từ (5-8) có: trong đó và được lựa chọn sao cho: > 0 (5-9) Tiếp theo ta sẽ chứng minh đạo hàm theo thời gian của hàm Lyapunov là xác định âm. Đạo hàm V theo thời gian ta được: Từ (5-7) rút ra: . Thay vào (5-10): Ta có: (5-11) (5-12) Vì s(e) bị chặn nên tồn tại 1 hằng số c0 sao cho: (5-13) Thay thế (4-13) vào (4-12) thu được: (5-14) trong đó là giá trị riêng nhỏ nhất của . Để ý rằng: (5-15) Thay (5-15) vào (5-14) thu được: (5-16) trong đó:, , (5-17) Từ đó ta thấy nếu: min> p (5-18) và > . Khi đó > 0 và > 0 W là xác định dương. W là xác định dương là xác định âm. Kết luận: - Với Kp và Bv được lựa chọn thỏa mãn điều kiện (5-9), (5-18) và được lựa chọn thỏa mãn điều kiện (5-5) thì các hàm V và W là xác định dương 0. Do đó, theo tiêu chuẩn ổn định Lyapunov, hệ thống kín được mô tả bởi phương trình (5-7) là ổn định tiệm cận tại điểm cân bằng nghĩa là: Xd - X , khi t . - Nhìn vào điều kiện (5-18) ta thấy kp và đóng vai trò rất quan trọng trong việc ổn định hệ thống. Nếu p lớn hơn thì yêu cầu bv lớn hơn, nếu p nhỏ hơn thì thì có thể chọn bv nhỏ hơn. Nếu p lớn hơn thì yêu cầu nhỏ hơn, nếu p nhỏ hơn thì có thể chọn lớn hơn. Do đó để ổn định hệ thống thì cần phải chỉnh định kp và cho thích hợp. Bởi vậy, thực tế, ta làm ổn định hệ thống bằng cách tăng hoặc giảm hệ số . 5.2.2.1.2. Thiết kế bộ điều khiển điểm đặt Jacobian xấp xỉ bù trọng lực cho robot Gryphon + Các phương trình động học thuận và động lực học robot Gryphon đã được thiết lập trong chương 2 và chương 3. - Phương trình động lực học dạng (5-1): trong đó q= [q1, q2, q3]T , tương ứng là các vectơ vị trí, tốc độ, gia tốc của các khớp robot; M(q) , S(), G(q) đã được xác định. - Quan hệ giữa tốc độ tay và tốc độ khớp dạng (5-3): trong đó là tốc độ tay robot, J(q) là ma trận Jacobian. + Giả sử các thông số thực tế của robot là: 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, . + Chọn luật điều khiển: (5-19) trong đó là ma trận Jacobian ước lượng, được tính toán theo các thông số động học ước lượng: ; s(e) = [s1(e1), s2(e2), s3(e3)]T ; Kp , Bv là các ma trận đường chéo dương. Hàm bão hòa si(ei) (i = 1, 2, 3) được chọn với: s0 = = 0.02 Sơ đồ mô phỏng trong Simulink như hình 5.1. 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 5.1. Sơ đồ mô phỏng hệ điều khiển điểm đặt Jacobian xấp xỉ bù trọng lực + Để điều khiển điểm đặt với vectơ vị trí đặt của tay Xd = =[0.33, 0.17, 0.2]T, thay đổi hệ số kp trong khoảng [2500 , 4000] và thay đổi hệ số bv trong khoảng [5 , 15], ta chọn được các ma trận hệ số Kp và Bv cho chất lượng hệ thống tốt nhất: và + Kết quả mô phỏng như sau: Hình 5.2. Tọa độ x của điểm tác động cuối Hình 5.3. Tọa độ y của điểm tác động cuối Hình 5.4. Tọa độ z của điểm tác động cuối Nhận xét kết quả mô phỏng: +) Từ các hình 5.2, 5.3, 5.4 ta thấy hệ điều khiển điểm đặt Jacobian xấp xỉ bù trọng lực đã thiết kế là ổn định với các chỉ tiêu chất lượng: - Sai lệch tĩnh bằng 0. - Thời gian quá độ khoảng 2.5 s. +) ảnh hưởng của hàm bão hòa si(ei) và các ma trận hệ số Kp, Kd tới chất lượng hệ thống. - Giữ nguyên bv: khi tăng kp trong khoảng [2500 , 4000] thì thời gian quá độ giảm, độ quá điều chỉnh tăng và khi giảm kp thì ngược lại. - Giữ nguyên kp: khi tăng bv trong khoảng [5 , 15] thì thời gian quá độ tăng, độ quá điều chỉnh giảm và khi giảm bv thì ngược lại. - Khi chọn các hằng số s0 và của hàm bão hòa si(ei) sao cho s0 = nhỏ hơn sai lệch e ban đầu (thời điểm t = 0) thì sẽ làm giảm độ quá điều chỉnh. 5.2.2.2. Bộ điều khiển điểm đặt Jacobian xấp xỉ thích nghi với trọng lực không biết chính xác. 5.2.2.2.1. Cơ sở lý thuyết Thành phần trọng lực có thể viết ở dạng sau: (5-20) với I(q) và D = [D1, , Dp]T chứa các thông số không biết chính xác. Luật điều khiển thích nghi [TL -1]: ` (5-21) trong đó: là vectơ ước lượng của vectơ D. Thay (5-20) và (5-21) vào (5-1) ta thu được phương trình hệ thống kín: (5-22) trong đó: Chọn hàm Lyapunov: V1 = V + (5-23) trong đó V là hàm xác định dương ở (5-8) trong mục (5.2.2.1.1) và A-1 là ma trận đường chéo dương. Do đó V1 cũng là hàm xác định dương. Đạo hàm V1 theo thời gian: (5-24) Từ (5-22) rút ra: . Thay vào (5-24) ta được: = Nếu ta chọn: = 0 (5-25) thì (5-26) Từ (5-23) và (5-26), theo tiêu chuẩn ổn định Lyapunov ta có: khi t Lại có: (5-27) Kết hợp (5-25) và (5-27) ta thu được luật nhận dạng thích nghi: (5-28) Kết luận: Với Kp, Bv và được lựa chọn như trong mục (5.2.2.1.1) thì hệ thống kín mô tả bởi phương trình (5-22) và (5-28) là ổn định tiệm cận tại điểm cân bằng nghĩa là: Xd - X , khi t . 5.2.2.2.2. Thiết kế bộ điều khiển điểm đặt Jacobian xấp xỉ thích nghi cho robot Gryphon. + Các thông số thực của robot như trong mục 5.2.2.1.2. + Ta có: G(q) = I(q)D (5-29) với D = [m2lg2, m2l2, m3lg3, m3l2]T chứa các thông số không biết chính xác và I(q) là ma trận 3x6 như sau: + Chọn luật điều khiển thích nghi (5-21) và (5-28) trong đó và hàm si(ei) được chọn như trong mục 5.2.2.1.2: Chọn hằng số và A là ma trận đường chéo dương, các phần tử đường chéo đều bằng 0.21 . + Sơ đồ mô phỏng trong Simulink (các file có đuôi “.m” được trình bày trong phụ lục): Hình 5.5. Sơ đồ mô phỏng hệ điều khiển điểm đặt Jacobian xấp xỉ thích nghi + Để điều khiển điểm đặt với vectơ vị trí đặt của tay Xd = = [3.5, 2, 2]T, chọn các ma trận hệ số Kp và Bv như trong mục 5.2.2.1.2. và + Kết quả mô phỏng như sau: Hình 5.6. Tọa độ x của điểm tác động cuối Hình 5.7. Tọa độ y của điểm tác động cuối Hình 5.8. Tọa độ z của điểm tác động cuối Nhận xét kết quả mô phỏng: Từ các hình 5.6, 5.7, 5.8 ta thấy hệ thống điều khiển điểm đặt Jacobian xấp xỉ thích nghi đã thiết kế là ổn định và có chất lượng điều chỉnh gần giống với hệ thống điều khiển điểm đặt Jacobian xấp xỉ bù trọng lực đã thiết kế trong mục 5.2.2.1.2. ảnh hưởng của hàm bão hòa si(ei) và các ma trận hệ số Kp, Bv tới chất lượng hệ thống cũng tương tự như thế. 5.3. Điều khiển bám quỹ đạo với phương pháp Jacobian xấp xỉ thích nghi 5.3.1. Cơ sở lý thuyết Mục này sẽ trình bày bộ điều khiển bám quỹ đạo cho robot theo phương pháp Jacobian thích nghi khi động học và động lực học không biết chính xác. ý tưởng chính là đưa vào 1 vectơ trượt thích nghi sử dụng tốc độ tay ước lượng. Các thông số động học không chính xác của tốc độ tay ước lượng và ma trận Jacobian được cập nhật (update) bởi 1 luật cập nhật thông số động học. Đặt (5-30) trong đó: X hay X-Xd được đo bởi 1 sensor vị trí, Xd là quỹ đạo chuyển động đặt của tay và là tốc độ đặt của tay. Đạo hàm (5-30) ta được: (5-31) trong đó là tốc độ thực của tay và là gia tốc đặt của tay. Ta có: (5-32) trong đó L chứa các thông số động học, J(q,L) là ma trận Jacobian và . Khi các thông số động học không biết chính xác thì ta có: (5-33) trong đó là tốc độ ước lượng của tay, là ma trận Jacobian xấp xỉ và chứa các thông số động học chưa biết. sẽ được cập nhật bởi 1 luật cập nhật thông số sẽ được định nghĩa sau. Ta định nghĩa vectơ trượt thích nghi : = (5-34) Tiếp theo, đặt: (5-35) ở đây, ta thừa nhận là robot làm việc trong một không gian hữu hạn sao cho ma trận Jacobian xấp xỉ không suy biến. Từ (5-35) có: (5-36) trong đó: . Ta định nghĩa vectơ trượt s trong không gian khớp: s = = (5-37) và (5-38) Thay (5-37) và (5-38) vào phương trình (5-1), ta được: (5-39) Tổng 4 số hạng cuối của phương trình (5-39) có thể viết ở dạng: (5-40) trong đó chứa các thông số động lực học chưa biết của robot, . Thay (5-40) vào (5-39) thu được: (5-41) Luật điều khiển thích nghi trên cơ sở ma trận Jacobian xấp xỉ [TL - 1]: (5-42) trong đó: ; Kd, Kp, K là các ma trận đường chéo n x n có các hệ số dương. Các thông số động học ước lượng của ma trận Jacobian được cập nhật (update) bởi luật sau: (5-43) và các thông số động lực học ước lượng được cập nhật bởi luật sau: (5-44) trong đó là các ma trận đường chéo có các hệ số dương. Thay (5-42) vào (5-41) thu được: (5-45) ở đây: . Xét hàm Lyapunov sau: (5-46) ở đây: . Đạo hàm phương trình (4-46) theo thời gian thu được: (5-47) Thay từ phương trình (5-45), từ phương trình (5-43) và từ phương trình (5-44) vào phương trình (5-47), sử dụng thuộc tính 2 của phương trình động lực học và phương trình (5-37) thu được: (5-48) Từ các phương trình (5-32), (5-30), (5-34), có: (5-49) trong đó: (5-50) Thay (5-49) vào (5-48) thu được: (5-51) Từ (5-46) và (5-51), theo tiêu chuẩn ổn định Lyapunov, ta rút ra kết luận sau: Trong 1 không gian làm việc hữu hạn sao cho ma trận Jacobian xấp xỉ là không suy biến thì luật điều khiển thích nghi Jacobian xấp xỉ (5-42) và các luật cập nhật thông số (5-43) và (5-44) dùng cho hệ thống robot (5-1) sẽ làm hội tụ vị trí và sai số bám tốc độ. Nghĩa là: và khi . Ngoài ra, tốc độ ước lượng của tay robot cũng hội tụ về tốc độ thực của tay, nghĩa là: khi . 5.3.2. Thiết kế bộ điều khiển bám quỹ đạo Jacobian xấp xỉ thích nghi cho robot Gryphon. + Các thông số thực 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: + Phương trình động lực học tổng quát của robot Gryphon có dạng: (5-52) hoặc (5-53) trong đó q= [q1, q2, q3]T , tương ứng là các vectơ vị trí, tốc độ, gia tốc của các khớp robot; M(q) , S(), V(), G(q) đã được xác định; là vectơ mômen ở các khớp. + Phương trình biểu diễn quan hệ giữa tốc độ tay và tốc độ khớp: (5-54) trong đó là tốc độ tay và tốc độ khớp; J(q,L) là ma trận Jacobian; L = [l1, l2, l3]T là các thông số động học của robot. (5-55) Viết lại phương trình (5-54) dưới dạng khác: (5-56) với: (5-57) + Các vectơ được định nghĩa như ở 5.3.1. + Biểu diễn (5-58) trong đó chứa các thông số động lực học chưa biết của robot, . Để tìm được và U, ta thay thế các ma trận M(q), đã xác định được ở trong chương 3 vào phương trình (5-58). Sau một số phép biến đổi tìm được: , là ma trận khá phức tạp nên sẽ được trình bày trong phụ lục. + Dùng luật điều khiển đã nêu trong mục 5.3.1: (5-59) (5-60) (5-61) trong đó và , 5 ma trận đều là ma trận đường chéo dương; là ma trận Jacobian xấp xỉ ; là vectơ ước lượng của vectơ ; là vectơ ước lượng của vectơ L. + Sau đây là các sơ đồ mô phỏng: Hình 5.9. Sơ đồ mô phỏng hệ điều khiển bám quỹ đạo Jacobian xấp xỉ thích nghi Hình 5.10. Sơ đồ cấu trúc khối Subsystem1 Hình 5.11. Sơ đồ cấu trúc khối Subsystem2 + Quỹ đạo chuyển động mong muốn: + Để điều khiển tay robot bám theo quỹ đạo mong muốn trên, thay đổi hệ số kp trong khoảng [1000 , 2000] và thay đổi hệ số kd trong khoảng [5 , 15], 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: Các ma trận K, R, N được chọn như sau: N là ma trận đường chéo có các phần tử đường chéo bằng 0.1. + Kết quả mô phỏng: Hình 5.12. Tọa độ x của điểm tác động cuối Hình 5.13. Tọa độ y của điểm tác động cuối Hình 5.14. Tọa độ z của điểm tác động cuối Nhận xét kêt quả mô phỏng: +) Từ các hình 5.12, 5.13, 5.14 ta thấy hệ thống điều khiển bám quỹ đạo Jacobian xấp xỉ thích nghi đã thiết kế là tương đối ổn định. Các tín hiệu vị trí thực của tay theo toạ độ x và y hội tụ về các tín hiệu vị trí đặt với sai số bám nhỏ tuy nhiên tín hiệu vị trí thực của tay theo toạ độ z vẫn chưa được như mong muốn. +) ảnh hưởng của các ma trận hệ số Kp, Kd tới chất lượng hệ thống: - Giữ nguyên kd: khi tăng kp trong khoảng [1000 , 2000] thì sai số bám giảm và thời gian quá độ giảm và khi giảm kp thì ngược lại. - Giữ nguyên kp: khi tăng kd trong khoảng [1 , 10] thì sai số bám tăng, thời gian quá độ giảm và khi giảm kd thì ngược lại. Kết luận Luận văn đã nghiên cứu các hệ thống điều khiển Robot, mô phỏng với ba hệ thống điều khiển cụ thể: hệ thống điều khiển phản hồi bù trọng lực, 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. Kết quả mô phỏng cho thấy các hệ thống đảm bảo tay robot (end effector) di chuyển tới vị trí mong muốn khi thông số động học của Robot xác định chính xác. Nhưng khi động học Robot là không chính xác thì không thể thu được góc quay mong muốn của các khớp từ quỹ đạo tay mong muốn bằng cách giải bài toán động học ngược. Hơn nữa ma trận Jacobian cũng không thể xác định chính xác nếu động học robot là không chính xác. Và kết quả mô phỏng hệ điều khiển Jacobian chuyển vị với động lực không chính xác đã thể hiên điều đó. Với hệ điều khiển chuyển động robot theo phương pháp Jacobian xấp xỉ, dù động học và động lực học không biết chính xác nhưng kết quả vẫn cho thấy tính ổn định của hệ thống. Điều đó chỉ ra rằng điểm tác động cuối cùng có thể di chuyển theo chuyển động mong muốn thậm chí khi động học và động lực học không biết chính xác. Điều này cho Robot một khả năng xử lý tinh vi ở cấp cao với sự thay đổi không xác định trước và không chắc chắn về động học và động lực học, điều mà tương tự như chuyển động “với tay” và vận hành công cụ của con người. Mặc dù luận văn chưa có kết quả thực nghiệm song những kết quả mô phỏng thu được tạo cơ sở tốt cho việc thiết kế một hệ thống điều khiển robot có khả năng linh hoạt hơn trong thực tế. Tài liệu tham khảo Tài liệu tiếng việt Phạm Công Ngô (2001), Lý thuyết điều khiển tự động, Nhà xuất bản khoa học và kỹ thuật, Hà nội. TS. Nguyễn Mạnh Tiến, (2007) Điều khiển robot công nghiệp, Nhà xuất bản khoa học và kỹ thuật, Hà nội. Nguyễn Phùng Quang (2005), Matlab và Simulink dành cho kỹ sư điều khiển tự động, Nhà xuất bản khoa học và kỹ thuật. Tài liệu tiếng anh Chien Chhern Cheah (2005), Approximate Jacobian Control for Robot Manipulators, School of Electrical and Electronic Engineering, Singapore Paul Richard (1982), Robot Manipulator, The Massachusetts Institute of Technology, United State of America. Tsuneo Yoshikawa (1990), Foundation of Robotics Analysis and Control, The Massachusetts Institute of Technology, United State of America. PHụ LụC Phụ lục chương 4 PL1. Các file có đuôi “.m” đi kèm với sơ đồ Simulink hình 4.6 để mô phỏng hệ thống điều khiển phản hồi PD bù trọng lực trong không gian khớp. % File “parasyst.m” global l1 l2 l3 m1 m2 m3 ; % cac thong so cua robot m1=15; m2=12; m3=8; l1=0.135; l2=0.225; l3=0.225; % File “M_term.m” function out= M_term(q1,q2,q3); % ham tinh ma tran M(q) global m1 m2 m3 l1 l2 l3; lg1=l1/2; lg2=l2/2;lg3=l3/2; % khoang cach tu dau thanh noi den tam khoi r=0.03; J1=m1*r1*r1; J2=m2*l2*l2/12; J3=m3*l3*l3/12; % momen quan tinh cac thanh noi qk11=m2*lg2^2*cos(q2) ^2+J1; qk12=m3*(lg3^2*cos(q2+q3) ^2+2*lg3*lg2* cos(q2) *cos(q2+q3)+ l2*cos(q2) ^2; out(1,1)=qk11+qk12; % phan tu M11 out(1,2)=0; % phan tu M12 out(1,3)=0; % phan tu M13 out(2,1) =out(1,2); % phan tu M21 out(2,2) =m2*lg2^2+J2+ m3*(lg3^2+2*m3*l2*lg3*cos(q3)); % phan tu M22 out(2,3)=m3*lg3^2+J3+m3*l2*lg3*cos(q3); % phan tu M23 out(3,1)=out(1,3); % phan tu M31 out(3,2)=out(2,3); % phan tu M32 out(3,3)=m3*lg3^2+J3; % phan tu M33 % File “Vg.m” function out = V(u); % ham tinh V(q,) global l1 l2 l3 m1 m2 m3; lg1=l1/2; lg2=l2/2; lg3=l3/2; q1=u(1); q2=u(2); q3=u(3); % vi tri khop dq1=u(4); dq2=u(5); dq3=u(6); % toc do khop lsq31=-2*m2*lg2^2*sin(q2)*cos(q2)*dq2*dq1; lsq32=2*m3*(lg3*cos(q2+q3) +l2*cos(q2))*(lg3*sin(q2+q3)+l2*sin(q2))*dq1*dq2; lsq33=-2*m3*(lg3*cos(q2+q3) +l2*cos(q2))+l2*cos(q2)*lg3*sin(q2+q3*dq1*dq3; out(1)=lsq31+lsq32+lsq33; % phan tu V1 lsq41=m2*lg2^2*sin(q2)*cos(q2)*dq1^2*; lsq42= -m3*dq1^2*(lg3*cos(q2+q3) +l2*cos(q2))*(lg3*sin(q2+q3)+l2*sin(q2)); lsq43=-m3*lg3*l2*sin(q3)*(2*dq2*dq3+dq3); out(2)=lsq41+lsq42+lsq43; % phan tu V2 lsq51= m3*lg3*sin(q2+q3) *dq1^2*(lg3*cos(q2+q3) +l2*cos(q2)); lsq52= m3*lg3*l2*sin(q3)*^2dq2; out(3)=lsq51+lsq52; % phan tu V3 % File “G.m” function out =G(u); % ham tinh G(q) global l1 l2 l3 m1 m2 m3; lg1=l1/2; lg2=l2/2; lg3=l3/2; q1=u(1); q2=u(2); q3=u(3); % vi tri khop out(1)=0; % phan tu G1(q) out(2)=(m2*lg2*cos(q2)+m3*l2*cos(q2)+m3*lg3*cos(q2+q3))*9.8; % G2(q) out(3)=m3*lg3*cos(q2+q3)*9.8; % G3(q) % File “con_para.m” Kp=[200 0 0;... 0 200 0;... 0 0 200]; % ma tran he so Kp Kd=[25 0 0;... 0 25 0;... 0 0 25]; % ma tran he so Kd qd=[1;1.5;1.3]; % vecto vi tri dat cua cac khop % File “con_in” function out = con_in(u); % ham tinh momen theo luat dieu khien con_para; q= [u(1);u(2);u(3)]; delq= qd-q; % sai lech vi tri khop deldq=[u(4);u(5);u(6)]; % sai lech toc do khop T=Kp*delq-Kd*deldq; % luat dieu khien phan hoi PD trong khong gian khop out=[T;qd]; % File “dyna.m” function out = dyna(u); % tinh gia toc khop q1=u(4); q2=u(5);q3=u(6); M=M_term(q1,q2,q3); TU=[u(1);u(2);u(3)]; out=M\TU; PL2. Các file có đuôi “.m” đi kèm với sơ đồ Simulink hình 4.12 để mô phỏng hệ thống điều khiển ma trận Jaccobian chuyển vị. Các file “parasyst.m”, “M_term.m”, “Vg.m”, “G.m”, “dyna.m” giống như trong PL1. % File “con_para.m” Kp=[200 0 0;... 0 200 0;... 0 0 200]; Kd=[50 0 0;... 0 50 0;... 0 0 50]; Xd=[0.3;0.16;0.25]; % vecto dat vi tri cua tay % File “J_term.m” function out= J_term(q1,q2,q3); % ham tinh ma tran Jacobian global l1 l2 l3; out(1,1)= -l2*sin(q1)*cos(q2)-l3*sin(q1)*cos(q2+q3); out(1,2)= -l2*cos(q1)*sin(q2)-l3*cos(q1)*sin(q2+q3); out(1,3)= -l3*cos(q1)*sin(q2+q3); out(2,1)= l2*cos(q1)*cos(q2)+l3*cos(q1)*cos(q2+q3); out(2,2)= l2*sin(q1)*sin(q2)-l3*sin(q1)*sin(q2+q3); out(2,3)= -l3*sin(q1)*sin(q2+q3); out(3,1)= 0; out(3,2)= l2*cos(q2)+l3*cos(q2+q3); out(3,3)= l3* cos(q2+q3); % File “X.m” function out= X(u); % ham tinh X tu q qua phuong trinh dong hoc thuan global l1 l2 l3; q1= u(1); q2= u(2); q3= u(3); out(1)=l2 * cos(q1) * cos(q2) + l3* cos(q1) * cos(q2+q3); out(2)=l2 * sin(q1) * cos(q2) + l3* sin(q1) * cos(q2+q3); out(3)= l1+ l2*sin(q2) + l3* sin(q2+q3); % File “Xdot.m” function out= Xdot(u); % ham tinh toc do cua tay q1= u(1); q2= u(2); q3= u(3); J= J_term(q1,q2,q3); out= J*[u(4);u(5);u(6)]; % File “con_in.m” function out = con_in(u); % ham tinh momen theo luat dieu khien Jacobian chuyen vi con_para; X= [u(1);u(2);u(3)]; delX= Xd-X; % sai lech vi tri cua tay deldX= [u(4);u(5);u(6)]; % sai lech toc do cua tay q1= u(7); q2= u(8); q3= u(9); J= J_term(q1,q2,q3); T= J'*(Kp*delX-Kd*deldX); % luat dieu khien Jacobian chuyen vi out=[T;Xd]; PL3. Các file có đuôi “.m” đi kèm với sơ đồ Simulink hình 4.17 để mô phỏng hệ thống điều khiển ma trận Jaccobian nghịch đảo. Tất cả các file đều giống như trong mục PL2 trừ 2 file “con_para.m” và “con_in.m”. % File “con_para.m” Kp=[120 0 0;... 0 120 0;... 0 0 120]; Kd=[25 0 0;... 0 25 0;... 0 0 25]; Xd=[0.3;0.16;0.25]; % File “con_in.m” function out = con_in(u); % ham tinh momen theo luat dieu khien Jacobian nghich dao con_para; X= [u(1);u(2);u(3)]; delX= Xd-X; deldX= [u(4);u(5);u(6)]; q1= u(7); q2= u(8); q3= u(9); J= J_term(q1,q2,q3); T= inv(J)*(Kp*delX-Kd*deldX); % luat dieu khien Jacobian nghich dao out=[T;Xd]; PL4. Các file có đuôi “.m” đi kèm với sơ đồ Simulink hình 4.21 để mô phỏng hệ điều khiển Jacobian chuyển vị với động học không chính xác. Các file giống như trong mục PL2: “con_para.m”, “dyna.m”, “G.m”, “M_term.m”, “Vg.m”, “X.m”. Các file khác: % File “parasyst.m” global l1 l2 l3 m1 m2 m3 l1e l2e l3e; m1=15; m2=12; m3=8; l1=0.135; l2=0.225; l3=0.225; % cac thong so thuc cua robot l1e=0.137; l2e=0.227; l3e=0.222; % cac thong so do duoc (khong chinh xac) % File “Je_term.m” function out= Je_term(q1,q2,q3); % ham tinh ma tran Jacobian theo cac thong so do duoc global l1e l2e l3e; out(1,1)= -l2e*sin(q1)*cos(q2)-l3e*sin(q1)*cos(q2+q3); out(1,2)= -l2e*cos(q1)*sin(q2)-l3e*cos(q1)*sin(q2+q3); out(1,3)= -l3e*cos(q1)*sin(q2+q3); out(2,1)= l2e*cos(q1)*cos(q2)+l3e*cos(q1)*cos(q2+q3); out(2,2)= l2e*sin(q1)*sin(q2)-l3e*sin(q1)*sin(q2+q3); out(2,3)= -l3e*sin(q1)*sin(q2+q3); out(3,1)= 0; out(3,2)= l2e*cos(q2)+l3e*cos(q2+q3); out(3,3)= l3e* cos(q2+q3); % File “G3e.m” function out =G3e(q1,q2,q3); % ham tinh G(q) theo cac thong so do duoc (khong chinh xac) global l1e l2e l3e m1 m2 m3; lg1e=l1e/2; lg2e=l2e/2; lg3e=l3e/2; % khoang cach tu dau thanh noi den tam khoi out(1)=0; % phan tu G1(q) out(2)=(m2*lg2e*cos(q2)+m3*l2e*cos(q2)+m3*lg3e*cos(q2+q3))*9.8; % G2(q) out(3)=m3*lg3e*cos(q2+q3)*9.8; % G3(q) out=[out1;out2;out3]; % File “con_in.m” function out = con_in3(u); % ham tinh momen theo luat dieu khien Jacobian chuyen % vi nhung dong hoc khong chinh xac con_para; X= [u(1);u(2);u(3)]; delX= Xd-X; deldX= [u(4);u(5);u(6)]; q1= u(7); q2= u(8); q3= u(9); Ja= Ja_term(q1,q2,q3); Gee= G3e(q1,q2,q3); T= Je'*(Kp*delX-Kd*deldX)+Gee; out=[T;Xd]; Phụ lục chương 5 PL5. Các file có đuôi “.m” đi kèm với sơ đồ Simulink hình 5.1 để mô phỏng hệ thống điều khiển điểm đặt Jacobian xấp xỉ bù trọng lực. Các file giống như trong mục PL4: “dyna.m”, “G.m”, “M_term.m”, “Vg.m”, “X.m”, “Je_term.m”. Các file khác: % File “parasyst.m” global l1 l2 l3 m1 m2 m3 l1e l2e l3e; m1=15;m2=12;m3=8; l1=0.135;l2=0.225;l3=0.225; l1e=0.137; l2e=0.25; l3e=0.25; % cac thong so dong hoc uoc luong % File “con_para.m” Kp=[3000 0 0;... 0 3000 0;... 0 0 3000]; Bv=[12 0 0;... 0 12 0;... 0 0 12]; Xd=[0.33;0.17;0.2]; % File “con_in.m” function out = con_in (u); % tinh momen theo luât dieu khien Jacobian xap xi bu trong luc con_para; s1= u(1); s2= u(2); s3= u(3); % cac ham bao hoa si(ei) s= [s1;s2;s3]; dq= [u(7);u(8);u(9)]; q1= u(4); q2= u(5); q3= u(6); Je= Je_term(q1,q2,q3); T= -Je'*Kp*s-Bv*dq; out= [T;Xd]; PL6. Các file có đuôi “.m” đi kèm với sơ đồ Simulink hình 5.5 mô phỏng hệ thống điều khiển Jacobian xấp xỉ thích nghi với trọng lực không biết chính xác. Tất cả các file đều giống như trong mục PL5 trừ file “con_in.m”. Ngoài ra có thêm các file “I3x4.m” và “De_dot.m”. % File “I3x4.m” function out = I3x4(q1,q2,q3); % ham tinh ma tran 3x6 c2= cos(q2); c23= cos(q2+q3); g= 9.8; out= [ 0 0 0 0 ;... g*c2 0 g*c23 g*c2 ;... 0 0 g*c23 0 ]; % File “De_dot.m” function out = De_dot(u); % Tinh dao ham vecto uoc luong cac thong so robot s1= u(1); s2= u(2);s3= u(3); s= [s1;s2;s3]; q1= u(4); q2= u(5); q3= u(6); dq= [u(7);u(8);u(9)]; I= I3x4(q1,q2,q3); Je= Je_term(q1,q2,q3); A= [0.21 0 0 0 ;... 0 0.21 0 0 ;... 0 0 0.21 0 ;... 0 0 0 0.21]; a= 0.5; out= -A*I'*(dq+a*Je'*s); % File “con_in.m” function out = con_in(u); % tinh momen theo luat dieu khien diem dat % Jacobian xap xi thich nghi con_para; s1= u(1); s2= u(2); s3= u(3); s= [s1;s2;s3]; dq= [u(7);u(8);u(9)]; % vecto toc do khop q1= u(4); q2= u(5); q3= u(6); Je= Je_term(q1,q2,q3); I= I3x4(q1,q2,q3); De= [u(10);u(11);u(12);u(13)]; % vecto uoc luong cac thong so robot T= -Je'*Kp*s-Bv*dq+I*De; out=[T;Xd]; PL7. Các file có đuôi “.m” đi kèm với sơ đồ Simulink hình 5.9 để mô phỏng hệ thống điều khiển bám quỹ đạo Jacobian xấp xỉ thích nghi. Các file giống như trong mục PL2: “parasyst.m”, “dyna.m”, “G.m”, “M_term.m”, “Vg.m”, “X.m”. Các file khác: % File “con_para.m” Kp=[2000 0 0;... 0 2000 0;... 0 0 2000]; Kd=[2 0 0;... 0 2 0;... 0 0 2]; K = [155 0 0;... 0 155 0;... 0 0 155]; % File “Je_term.m” function out= Je_term(q1,q2,q3,La1,La2,La3); % ham tinh ma tran Jacobian xap xi out(1,1)= -La2*sin(q1)*cos(q2)-La3*sin(q1)*cos(q2+q3); out(1,2)= -La2*cos(q1)*sin(q2)-La3*cos(q1)*sin(q2+q3); out(1,3)= -La3*cos(q1)*sin(q2+q3); out(2,1)= La2*cos(q1)*cos(q2)+La3*cos(q1)*cos(q2+q3); out(2,2)= La2*sin(q1)*sin(q2)-La3*sin(q1)*sin(q2+q3); out(2,3)= -La3*sin(q1)*sin(q2+q3); out(3,1)= 0; out(3,2)= La2*cos(q2)+La3*cos(q2+q3); out(3,3)=La3* cos(q2+q3); % File “Y_term.m” function out = Y_term(q1,q2,q3,dq1,dq2,dq3); % ham tinh ma tran Y(q,) out(1,1)= 0; out(1,2)= -sin(q1)*cos(q2)*dq1+cos(q1)*sin(q2)*dq2; out(1,3)= -sin(q1)*cos(q2+q3)*dq1-sin(q2+q3)*cos(q1)*dq2+cos(q1)*sin(q2+q3)*dq3; out(2,1)= 0; out(2,2)= cos(q1)*cos(q2)*dq1 - sin(q1)*sin(q2)*dq2; out(2,3)= cos(q1)*cos(q2+q3)*dq1-sin(q1)*sin(q2+q3)*dq2-sin(q2+q3)*sin(q1)*dq3; out(3,1)= 0; out(3,2)= cos(q2)*dq2; out(3,3)= cos(q2+q3)*dq2+cos(q2+q3)*dq3; % File “qr_dot.m” function out = qr_dot(u); % ham tinh q1= u(1); q2= u(2); q3= u(3); La1= u(7); La2= u(8); La3= u(9); dXr= [u(10);u(11);u(12)]; Je= Je_term3(q1,q2,q3,La1,La2,La3); out= inv(Je)*dXr; % File “Xe_dot.m” function out = Xe_dot(u); % ham tinh q1= u(1); q2= u(2); q3= u(3); dq1= u(4); dq2= u(5); dq3= u(6); La= [u(7);u(8);u(9)]; Y= Y_term(q1,q2,q3,dq1,dq2,dq3); out= Y*La; % File “Z3x10.m” function out= Z3x18(q1,q2,q3,dq1,dq2,dq3,dqr1,dqr2,dqr3,ddqr1,ddqr2,ddqr3); % ham tinh ma tran Z() g= 9.8; c2= cos(q2); c3= cos(q3); c23= cos(q2+q3); s2= sin(q2); s3= sin(q3); s23= sin(q2+q3); out(1,1)= ddqr1; out(1,2)= 0; out(1,3)= 0; out(1,4)= 0; out(1,5)= c2^2*ddqr1-s2*c2*(dq2*dqr1+ dq1*dqr2); out(1,6)= 0; out(1,7)= c2^2*ddqr1-s2*c2*(dq2*dqr1+ dq1*dqr2); out(1,8)= 2*c2*c23*ddqr1-c2*s23*(dqr2*dq1+dqr1*dq2+dqr3*dq1) -c23*s2*(dq2*dqr1+dq1*dqr2); out(1,9)= 0; out(1,10)= c23^2*ddqr1-c23*s23*(dqr2*dq1+dqr1*dq2+dqr3*dq1); out(2,1)= 0; out(2,2)= ddqr2; out(2,3)= ddqr3+ddqr2; out(2,4)= g*c2; out(2,5)= ddqr2 + c2*s2*dq1*dqr1; out(2,6)= g*c2; out(2,7)= c2*s2*dq1*dqr1; out(2,8)= 2*c3*ddqr2+c3*ddqr3+dqr1*dq1*(c23*s2 + c2*s23)- s3*(dqr2*dq3+dqr3*dq2+dq3*dqr3); out(2,9)= g*c23; out(2,10)= ddqr2+ddqr3+c23*s23*dq1*dqr1; out(3,1)= 0; out(3,2)= 0; out(3,3)= ddqr2+ddqr3; out(3,4)= 0; out(3,5)= 0; out(3,6)= 0; out(3,7)= 0; out(3,8)= c3*ddqr2+c2*s23*dq1*dqr1+ s3*dq2*dqr3-; out(3,9)= g*c23; out(3,10)= ddqr2+ddqr3+c23*s23*dqr1*dq1; % File “La_dot.m” function out = La_dot(u); % tinh dao ham vecto cac thong so %dong hoc uoc luong () con_para; q1= u(1); q2= u(2); q3= u(3); dq1= u(4); dq2= u(5); dq3= u(6); delX= [u(7);u(8);u(9)]; deldX= [u(10);u(11);u(12)]; Y= Y_term(q1,q2,q3,dq1,dq2,dq3); R= [0.1 0 0;... 0 0.1 0;... 0 0 0.1]; out= R*Y'*(Kd*deldX+Kp*delX); % File “Ua_dot.m” function out= Ue_dot(u); % tinh vecto q1= u(1); q2= u(2); q3= u(3); dq1= u(4); dq2= u(5); dq3= u(6); dqr1= u(7); dqr2= u(8); dqr3= u(9); ddqr1= u(10); ddqr2= u(11); ddqr3= u(12); dq= [dq1;dq2;dq3]; dqr= [dqr1;dqr2;dqr3]; s= dq-dqr; Z= Z3x10(q1,q2,q3,dq1,dq2,dq3,dqr1,dqr2,dqr3,ddqr1,ddqr2,ddqr3); N= [0.1 0 0 0 0 0 0 0 0 0 ;... 0 0.1 0 0 0 0 0 0 0 0 0 ;... 0 0 0.1 0 0 0 0 0 0 0 0 ;... 0 0 0 0.1 0 0 0 0 0 0 0 ;... 0 0 0 0 0.1 0 0 0 0 0 0 ;... 0 0 0 0 0 0.1 0 0 0 0 0 ;... 0 0 0 0 0 0 0.1 0 0 0 0 ;... 0 0 0 0 0 0 0 0.1 0 0 0 ;... 0 0 0 0 0 0 0 0 0.1 0 0 ;... 0 0 0 0 0 0 0 0 0 0.1 0 ;... 0 0 0 0 0 0 0 0 0 0 0.1]; out= -N*Z'*s; % File “con_in.m” function out = con_in(u); % tinh momen theo luat dieu khien bam quy dao % Jacobian xap xi thich nghi con_para; delX= [u(1);u(2);u(3)]; deldX= [u(4);u(5);u(6)]; La1= u(7); La2= u(8); La3= u(9); sx= [u(10);u(11);u(12)]; q1= u(13); q2= u(14); q3= u(15); dq1= u(16); dq2= u(17); dq3= u(18); dqr1= u(19); dqr2= u(20); dqr3= u(21); ddqr1= u(22); ddqr2= u(23); ddqr3= u(24); Ue= [u(25);u(26);u(27);u(28);u(29);u(30);u(31);u(32);u(33);u(34)]; Je= Je_term3(q1,q2,q3,La1,La2,La3); Z= Z3x10(q1,q2,q3,dq1,dq2,dq3,dqr1,dqr2,dqr3,ddqr1,ddqr2,ddqr3); out= -Je'*(Kd*deldX+Kp*delX)-Je'*K*sx+Z*Ue;