Luận văn Điều khiển phi tuyến hệ AGV

kế dùng đáp ứng backstepping. Tiến hành mô phỏng trên máy tính bằng phần mềm Matlab nghiên cứu trên robot di động hai bánh đã cho kết quả tốt. Cơ sở cho chúng ta tiến hành áp dụng thực tế cho 15 robot di động khác. Hướng phát triển tiếp theo sẽ làm mô hình robot thật và đưa phương pháp này vào áp dụng cho robot di động. [4] Trong bài báo này trình bày thiết kế bộ điều khiển mobile robot nonholonomic. Trên cơ sở phương trình động học của robot, một robot điều khiển được thiết kế sao cho bám theo quỹ đạo tham chiếu tùy ý với vận tốc được xác định trước. Các thuật toán điều khiển được thiết kế chứng minh ổn định cho WMR. Luật điều khiển được chứng minh trên một ví dụ quỹ đạo đơn giản, tuy nhiên đối với một ứng dụng tổng quát hơn một thuật toán lập kế hoạch chuyển động theo thời gian tối ưu với ràng buộc gia tốc được trình bày là tốt. Hình 2.14 Sơ đồ khối vòng điều khiển mobile robot. Hình 2.15 cho thấy kết quả mô phỏng vòng kín trong đó robot có trạng thái sai số ban đầu (không bắt đầu với góc định hướng và vị trí đúng), các tín hiệu ngõ vào và ngõ ra có nhiễu tác động. Bộ điều khiển triệt tiêu các trạng thái sai số ban đầu và robot đã bám theo quỹ đạo tham chiếu với độ chính xác đạt yêu cầu. 16 17 Hình 2.15 Điều khiển vòng kín với ߞ = 0.6 ,߱௡ = 2 để mô phỏng robot (reference --, robot path -). Quỹ đạo robot tín hiệu ngõ ra với nhiễu và tín hiệu ngõ vào (feedforward: - -, feedforward and closed-loop: -). Kết quả điều khiển vòng kín của robot thực nghiệm được thể hiện trong hình 2.16. Tín hiệu ngõ vào của robot là vận tốc tuyến tính ݑଵ và vận tốc góc ݑଶ. Robot được điều khiển bởi hai động cơ DC cho mỗi bánh xe, từ ݑଵ ,ݑଶ và các tín hiệu từ encoder tác giả sử dụng bộ điều khiển PID để đảm bảo robot đạt được vận tốc mong muốn ݑଵ ,ݑଶ. 18 Hình 2.16 Điều khiển vòng kín với ߞ = 0.6 ,߱௡ = 2 để mô phỏng robot thực nghiệm (reference - - , robot path -). Quỹ đạo robot tín hiệu ngõ ra với nhiễu và tín hiệu ngõ vào (feedforward: - - , feedforward and closed-loop: -). Nghiên cứu được trình bày cũng như ứng dụng thực tế chứng minh rằng mobile robot có thể bám theo quỹ đạo tham chiếu mong muốn với vận tốc quy định và độ chính xác đạt yêu cầu. Một thuật toán tối ưu hóa thiết lập chuyển động cho robot được trình bày. Tối ưu hóa là công việc mất nhiều thời gian và trong tương lai một giải pháp với những quỹ đạo tối ưu được xác định trước cho một mạng lưới các vị trí bắt đầu sẽ được dùng để thay thế. [5] Nghiên cứu này trình bày một quỹ đạo hoàn hảo cho WMR được phát triển. Phương pháp hồi tiếp tuyến tính hóa (FL) có thể được sử dụng để WMR đạt được vị trí mong muốn và góc định hướng quỹ đạo. Do sự thể hiện không ổn định từ các tín hiệu bên ngoài nên bộ điều khiển FL được thay thế bằng bộ điều khiển trượt (SMC). SMC không thể loại bỏ được chattering do đó bộ điều khiển trượt mờ (FSMC) được đề xuất để giảm chattering. Tính ưu việt của FSMC được trình bày thông qua kết quả mô phỏng. Hình 2.17 Sơ đồ khối SMC. 19 Hình 2.18 Sơ đồ khối FSMC. Bảng 2.1 Luật điều khiển mờ. 20 Hình 2.19 Hàm membership của ngõ vào-ngõ ra ݏ, ̇ݏ,ݑ௙. Hình 2.20 WMR bám theo quỹ đạo tham chiếu. 21 Hình 2.21 Sai số ܺ (ܺௗ − ܺ). Hình 2.22 Sai số ܻ ( ௗܻ − ܻ). 22 Hình 2.23 Tín hiệu điều khiển torque cho bánh phải. Hình 2.24 Sai số góc định hướng ߰ௗ − ߰ . 23 Hình 2.25 WMR bám theo quỹ đạo tham chiếu: (a) FL; (b) đường chấm chấm-SMC, đường liền-FSMC. Hình 2.26 Sai số ܺ (ܺௗ − ܺ): (a) FL; (b) đường chấm chấm-SMC, đường liền-FSMC. 24 Hình 2.27 Sai số ܻ ( ௗܻ − ܻ): (a) FL; (b) đường chấm chấm-SMC, đường liền-FSMC. Hình 2.28 Sai số góc định hướng ߰ௗ − ߰: (a) FL; (b) đường chấm chấm-SMC, đường liền-FSMC. 25 Hình 2.29 Tín hiệu điều khiển torque cho bánh phải: (a) FL; (b) đường chấm chấm- SMC, đường liền-FSMC. Từ hình 2.20–2.24 là kết quả mô phỏng bộ điều khiển được thiết kế chưa chưa có nhiễu tác động từ bên ngoài. Trong hình 2.20 với tất cả các bộ điều khiển WMR bám theo quỹ đạo tham chiếu với độ chính xác đáng kể. Có thể thấy được từ hình 2.21–2.24 bộ điều khiển FL cho hiệu suất tốt hơn cho vị trí và góc định hướng so với SMC và FSMC. Tuy nhiên, trong hình 2.23 độ lớn moment xoắn khi sử dụng FL là lớn, FSMC có kết quả sai số nhỏ hơn so với SMC. Trong hình 2.25 bộ điều khiển FL rất nhạy với nhiễu đo được trong khi đó SMC thể hiện tương đối tốt với nhiễu tác động. Từ hình 2.26–2.28 sai số vị trí và góc định hướng của SMC và FSMC tốt hơn so với FL. Hơn nữa FSMC có hiệu suất đáng tin cậy hơn so với SMC. Một sự vượt trội khác liên quan đến FL , SMC có tín hiệu điều khiển moment xoắn nhỏ điều này được thể hiện trong hình 2.29. Khi có các tác động bên ngoài FL có hiệu suất không tốt với sai số ở tần số cao. Bằng cách sử dụng SMC kết hợp với một mặt trượt PID hiệu suất của WMR được cải thiện. FSMC được đề xuất tiếp tục nâng cao hiệu suất của WMR loại bỏ được nhiễu vì vậy nó sẽ phù hợp hơn trong hệ thống WMR thực. 26 [6] Trong bài báo này, trình bày phương pháp thiết kế bộ điều khiển cho xe tự hành sử dụng kỹ thuật tuyến tính hồi tiếp. Các mô hình động lực học của hệ thống với nhiễu bên ngoài được trình bày. Véc tơ điều khiển đầu vào được thiết kế dùng cho kỹ thuật tuyến tính hồi tiếp. Véc tơ điều khiển đầu vào chuyển đổi toàn bộ hệ thống thành hai hệ thống con tuyến tính của hệ thống bao gồm sai số vị trí và sai số vận tốc. Dựa trên hai hệ thống con tuyến tính, véc tơ đầu vào mới của xe tự hành được thiết kế. Véc tơ điều khiển đầu vào mới đảm bảo rằng các véc tơ sai số hội tụ theo hàm mũ về không. Hình 2.30 Lưu đồ giải thuật điều khiển tuyến tính hồi tiếp. Hình 2.31 Quỹ đạo mong muốn của AGV có dạng đường thẳng (y = x). 27 Bảng 2.2 Giá trị thông số của AGV. Bảng 2.3 Giá trị khởi tạo ban đầu. 28 Hình 2.32 Quỹ đạo của AGV ở thời gian ban đầu. Từ hình 2.32 cho thấy: quỹ đạo mong muốn của AGV và quỹ đạo tham chiếu trùng với nhau sau khi AGV di chuyển một đoạn đường rất ngắn. Điều này cho thấy AGV đã bám theo quỹ đạo mong muốn rất chính xác. Hình 2.33 Sai lệch vị trí trong toàn thời gian. Hình 2.33 biểu diễn sự sai lệch vị trí giữa điểm bám P và điểm tham chiếu R trong toàn thời gian đi hết quỹ đạo tham chiếu của AGV. Từ hình 2.33 cho thấy các sai 29 lệch vị trí hội tụ về 0 nhanh trước 8 giây khi AGV bám theo quỹ đạo tham chiếu là đường thẳng và ổn định ở 0 trong toàn thời gian. Hình 2.34 Vận tốc tuyến tính của AGV trong toàn thời gian. Hình 2.34 biểu diễn vận tốc tuyến tính của AGV sau thời gian rất ngắn khoảng 3 giây rồi tiến đến bằng với vận tốc đặt trước ݎௗଶ = 0.05 [݉/ݏ] và ổn định ở giá trị này trong suốt thời gian chuyển động của AGV. Hình 2.35 Vận tốc góc của bánh phải và bánh trái đối với quỹ đạo là đường thẳng. 30 Hình 2.35 biểu diễn vận tốc góc bánh phải (wrw) và bánh trái (wlw) của AGV đối với quỹ đạo tham chiếu là đường thẳng. Nó cho thấy rằng vận tốc góc của bánh xe trái và phải thay đổi một cách nhanh chóng vào thời điểm bắt đầu và có cùng một giá trị 0.32 [rad / s] theo đường thẳng sau khoảng 7 giây. Hình 2.36 Véc tơ điều khiển đầu vào ߬. Hình 2.36 biểu diễn véc tơ điều khiển đầu vào bánh phải (trw) và bánh trái (tlw) của AGV đối với quỹ đạo tham chiếu là đường thẳng. Nó cho thấy rằng véc tơ điều khiển bánh xe trái và phải thay đổi một cách nhanh chóng vào thời điểm bắt đầu và có cùng một giá trị [−5.2; 5.2]ݎܽ݀/ݏଶ sau khoảng thời gian 4 giây. Hình 2.37 Véc tơ điều khiển đầu vào u. 31 Hình 1.33 biểu diễn véc tơ điều khiển đầu vào ݑଵvà ݑଶ của AGV đối với quỹ đạo tham chiếu là đường thẳng. Nó cho thấy rằng véc tơ điều khiển đầu vào thay đổi một cách nhanh chóng vào thời điểm bắt đầu và có cùng một giá trị [−7.6; 7.6]ܰ݉ sau khoảng thời gian 4 giây. Hình 2.38 Véc tơ điều khiển đầu vào mới ߟ. Hình 2.38 biểu diễn véc tơ điều khiển đầu vào mới ߟଵ và ߟଶ của AGV đối với quỹ đạo tham chiếu là đường thẳng. Nó cho thấy rằng véc tơ điều khiển đầu vào mới ߟଵ thay đổi một cách nhanh chóng vào thời điểm bắt đầu và có giá [−0.1; 0.1]݉/ݏଶ Và véc tơ điều khiển đầu vào mới ߟଶ giảm dần từ thời điểm khi bắt đầu tiến về 0 trong toàn thời gian quỹ đạo của AGV. Trong bài báo này, một bộ điều khiển tuyến tính hồi tiếp được đề xuất dựa trên mô hình động lực học của AGV với những nhiễu từ bên ngoài. Sử dụng kỹ thuật tuyến tínhhồi tiếp, véc tơ điều khiển đầu vào chuyển đổi toàn bộ hệ thống thành hai hệ thống con tuyến tính của hệ thống bao gồm sai số vị trí và sai số vận tốc. Dựa trên hai hệ thống con tuyến tính, véc tơ đầu vào mới của xe tự hành được thiết kế. Véc tơ điều khiển đầu vào mới đảm bảo rằng các véc tơ sai số hội tụ theo hàm mũ về không. Các kết quả mô phỏng được trình bày để minh họa hiệu quả cho bộ điều khiển bám cho AGV sử dụng thuật toán tuyến tính hồi tiếp. 32 2.4 Nhận xét và hướng tiếp cận Nhận xét: qua các công trình nghiên cứu của các tác giả đã nêu trên, ta nhận thấy rằng có rất nhiều phương pháp để giải quyết vấn đề đó là định vị và điều khiển bám cho mobile robot theo quỹ đạo mong muốn trên một mặt phẳng hai chiều. Tuy nhiên, mỗi phương pháp đều có ưu, khuyết điểm được thể hiện qua kết quả mô phỏng mà các bài báo cũng đã chỉ ra. Hướng tiếp cận: xây dựng thuật toán điều khiển dựa trên phương pháp Lyapunov cho WMR nhằm điều khiển WMR bám theo quỹ đạo đã được định trước, sau đó tiếp cận vị trí mong muốn. Cuối cùng, áp dụng thuật toán điều khiển này trên mô hình WMR bao gồm : - Khung xe. - Hai bánh chủ động hay còn gọi là bánh lái điều khiển nằm ở phía sau xe. Hai bánh chủ động này được điều khiển bằng hai động cơ điện một chiều để đạt được các chuyển động và định hướng. - Một bánh thụ động ở phía trước xe bánh này giúp cho xe cân bằng. 33 Chương 3 MÔ HÌNH TOÁN HỌC CỦA AGV 3.1 Cấu trúc AGV AGV trong đề tài này dựa trên mô hình của WMR, được thể hiện như trên hình 3.1. Hai bánh xe được cố định ở mỗi bên khung của AGV, được điều khiển độc lập bằng hai động cơ DC có trang bị encoder và một bánh xe đa hướng ở phía trước cần thiết cho việc ổn định hướng. AGV có thể di chuyển qua lại và thay đổi góc định hướng thông qua việc điều khiển vận tốc của hai bánh lái. Cả hai bánh xe có bán kính như nhau là r và khoảng cách nhau với khoảng cách là 2b. Hình 3.1 Mô hình động học của WMR. 34 Do AGV là một hệ thống phi tuyến nên việc lập kế hoạch đường đi và điều khiển bám theo quỹ đạo trở nên rất khó khăn để tối ưu quá trình điều khiển và ổn định hệ thống. Để tránh bánh xe bi trượt và những tác động ảnh hưởng của cơ khí thì việc chuyển động trơn tru và ổn định là rất quan trọng. 3.2 Xây dựng phương trình động học Trong hình 3.1 thể hiện AGV trong hệ trục tọa độ Oxy cũng chính là không gian nhà kho của AGV , CXY là hệ tọa độ cục bộ gắn trên AGV. Vị trí của AGV tại điểm C trong hệ tọa độ toàn cục hoàn toàn được xác định bởi tọa độ tổng suy rộng ܙ =[ݔ௖ ݕ௖ Θ௖]் , trong đó ݔ஼ ,ݕ஼ là tọa độ điểm C, Θ௖ là góc định hướng của AGV. Giả sử bánh xe không trượt và robot phải chuyển động với ràng buộc về tốc độ như sau: ̇ݔ௖ݏ݅݊Θ௖ − ̇ݕ௖ܿ݋ݏΘ௖ = 0 (3.1) Ràng buộc có thể được viết lại như sau: ۯ(ܙ)̇ܙ = 0 ܣ(ݍ) = [−ݏ݅݊Θ௖ ܿ݋ݏΘ௖ 0] = [ܽଵ௜] ݅ = 1,2,3 ̇ܙ = ۸(ܙ)ܢ ⇔ ̈ܙ = ۸(ܙ)̇ܢ + ۸̇(ܙ)ܢ ۸(ܙ) = ൥ܿ݋ݏΘ 0ݏ݅݊Θ 00 1൩ , ܢ = ቂݒ௖௖߱ቃ Trong đó ܬ(ݍ) là ma trận Jacobi phải thỏa ۸܂(ܙ)ۯ܂(ܙ) = 0, ݖ là vector vận tốc, ݒ௖, ௖߱ là vận tốc tuyến tính và vận tốc góc tương ứng của AGV. Với điều kiện ràng buộc này chuyển động của AGV được mô tả như sau: ቐ ̇ݔ௖ = ݒ௖ܿ݋ݏΘ௖ ̇ݕ௖ = ݒ௖ݏ݅݊Θ௖ Θ̇௖ = ௖߱ (3.2) 35 Mối quan hệ giữa ݒ௖,߱௖ và vận tốc góc của hai bánh xe: ቂ ߱௥௪ ߱௟௪ ቃ = ⎣ ⎢ ⎢ ⎢ ⎡ 1 ݎൗ ܾ ݎൗ1 ݎൗ − ܾ ݎൗ ⎦ ⎥ ⎥ ⎥ ⎤ ቂ ݒ௖ ߱௖ ቃ (3.3) Trong đó ߱௥௪,߱௟௪ lần lượt là vận tốc góc bánh phải và bánh trái của AGV. Phương trình động học tham chiếu của AGV: ቐ ̇ݔ௥ = ݒ௥ܿ݋ݏΘ௥ ̇ݕ௥ = ݒ௥ݏ݅݊Θ௥ Θ̇௥ = ߱௥ (3.4) Trong đó ݔ௥ , ݕ௥ ,Θ௥ là vị trí mong muốn của AGV. ݒ௥ ,߱௥ lần lượt là vận tốc tuyến tính và vận tốc góc mong muốn. Muốn AGV bám theo quỹ đạo mong muốn ta phải tìm một luật điều khiển sao cho lim௧→ஶ ݍ(ݐ) = ݍ௥ . Vector sai số được thể hiện tương đối so với tọa độ cục bộ được cố định trên robot như sau: ൥ ݁ଵ ݁ଶ ݁ଷ ൩ = ൥ ܿ݋ݏΘ௖ ݏ݅݊Θ௖ 0−ݏ݅݊Θ௖ ܿ݋ݏΘ௖ 00 0 1൩ ൥ݔ௥ − ݔ௖ݕ௥ − ݕ௖Θ௥ − Θ௖൩ (3.5) Đạo hàm bậc 1 sai số theo thời gian: ൝ ݁̇ଵ = ߱௖݁ଶ − ݒ௖ + ݒ௥ܿ݋ݏ݁ଷ ݁̇ଶ = −߱௖݁ଵ + ݒ௥ݏ݅݊݁ଷ ݁̇ଷ = ߱௥ − ߱௖ (3.6) 36 Hình 3.2 Khái niệm về AGV bám theo quỹ đạo tham chiếu. Đối với điều khiển AGV việc khắc phục sai số tích lũy là rất quan trọng, thêm vào đó cần phải có giới hạn về động lực học. ൜ |ݒ௖| ≤ ݒ௠௔௫ |߱௖| ≤ ߱௠௔௫ 3.3 Xây dựng quỹ đạo đường đi cho AGV Xây dựng quỹ đạo nhằm mục đích tìm ra một quỹ đạo trơn nhằm đảm bảo điều kiện ràng buộc về (3.1) cho hệ thống robot (3.2). Từ kết quả [2] ta có thể tạo một đường cong nội suy của hàm đa thức bậc 7 ߟଷ − ݏ݌݈݅݊݁ đi qua hai điểm A(ݔ஺, ݕ஺)và B(ݔ஻, ݕ஻) được tính như sau: P(s , η) = ൣ݌௫(ݏ), ݌௬(ݏ)൧் (3.7) ݌௫(ݏ) = ߙ଴ + ߙଵݏ + ߙଶݏଶ + ߙଷݏଷ + ߙସݏସ + ߙହݏହ + ߙ଺ݏ଺ + ߙ଻ݏ଻ (3.8) ݌௬(ݏ) = ߚ଴ + ߚଵݏ + ߚଶݏଶ + ߚଷݏଷ + ߚସݏସ + ߚହݏହ + ߚ଺ݏ଺ + ߚ଻ݏ଻ (3.9) ݏ ߳ [0 1] tham số quãng đường đã được chuẩn hóa. Tại thời điểm ban đầu. 37 ݏ = 0 , ݌௫(ݏ) = ݔ஺ ,݌௬(ݏ) = ݕ஺ Tại thời điểm kết thúc. ݏ = 1 , ݌௫(ݏ) = ݔ஻ , ݌௬(ݏ) = ݕ஻ Các hệ số của ݌௫(ݏ) được tính như sau: ߟ̅ = ‖ݔ஺ − ݔ஻ ,ݕ஺ − ݕ஻‖ = ඥ(ݔ஺ − ݔ஻)ଶ + (ݕ஺ − ݕ஻)ଶ ߙ଴ = ݔ஺ ߙଵ = ̅ߟܿ݋ݏߠ஺ , ߛଵ஺ = ߟ̅ܿ݋ݏߠ஻ ߙଶ = 0 , ߛଶ஺ = 0 ߙଷ = 0 , ߛଷ஺ = 0 ߙସ = 35(ݔ஻ − ݔ஺) − 20ߙଵ − 10ߙଶ − 4ߙଷ − 15ߛଵ஺ + 5ߛଶ஺ − ߛଷ஺ ߙହ = −84(ݔ஻ − ݔ஺) + 45ߙଵ + 20ߙଶ + 6ߙଷ + 39ߛଵ஺ − 14ߛଶ஺ + 3ߛଷ஺ ߙ଺ = 70(ݔ஻ − ݔ஺) − 36ߙଵ − 15ߙଶ − 4ߙଷ − 34ߛଵ஺ + 13ߛଶ஺ − 3ߛଷ஺ ߙ଻ = −20(ݔ஻ − ݔ஺) + 10ߙଵ + 4ߙଶ + ߙଷ + 10ߛଵ஺ − 4ߛଶ஺ + ߛଷ஺ Các hệ số của đa thức ݌௬(ݏ) được tính như sau: ߚ଴ = ݕ஺ ߚଵ = ̅ߟݏ݅݊ߠ஺ , ߛଵ஻ = ̅ߟݏ݅݊ߠ஻ ߚଶ = 0 ,ߛଶ஻ = 0 ߚଷ = 0 ,ߛଷ஻ = 0 ߚସ = 35(ݔ஻ − ݔ஺) − 20ߚଵ − 10ߚଶ − 4ߚଷ − 15ߛଵ஻ + 5ߛଶ஻ − ߛଷ஻ ߚହ = −84(ݔ஻ − ݔ஺) + 45ߚଵ + 20ߚଶ + 6ߚଷ + 39ߛଵ஻ − 14ߛଶ஻ + 3ߛଷ஻ ߚ଺ = 70(ݔ஻ − ݔ஺) − 36ߚଵ − 15ߚଶ − 4ߚଷ − 34ߛଵ஻ + 13ߛଶ஻ − 3ߛଷ஻ ߚ଻ = −20(ݔ஻ − ݔ஺) + 10ߚଵ + 4ߚଶ + ߚଷ + 10ߛଵ஻ − 4ߛଶ஻ + ߛଷ஻ 38 Các điểm trên quỹ đạo G3 gọi là way point (WP), AGV sẽ đi qua các điểm này để đến điểm đích đã được xác định trước. Số điểm WP có thể tùy chọn và nằm trong dải giới hạn, quá trình chuyển động sử dụng ߟଷ − ݏ݌݈݅݊݁ phù hợp để AGV tiếp cận pallet. Hình 3.3 Quỹ đạo G3 đi qua hai điểm A và B. 39 Chương 4 THIẾT KẾ BỘ ĐIỀU KHIỂN VÀ HỆ THỐNG ĐO LƯỜNG 4.1. Cơ sở lý thuyết 4.1.1 Giới thiệu phương pháp Lyapunov - Phương pháp Lyapunov cung cấp đủ điều kiện đủ để đánh giá tính ổn định của hệ phi tuyến. - Có thể áp dụng cho hệ phi tuyến bậc cao bất kỳ. - Có thể dùng phương pháp Lyapunov để thiết kế các bộ điều khiển phi tuyến. - Hiện nay phương pháp Lyapunov là phương pháp được sử dụng rộng rãi nhất để phân tích và thiết kế hệ phi tuyến. 4.1.2 Điểm cân bằng của hệ phi tuyến Xét hệ phi tuyến bất biến theo thời gian mô tả bởi phương trình trạng thái sau: ̇ݔ = ݂(ݔ,ݑ) (4.1) Một điểm ݔ௘ ∈ ܀୬ được gọi là điểm cân bằng của hệ thống nếu như hệ đang ở trạng thái ݔ௘ và không có tác động bên ngoài thì hệ sẽ nằm nguyên tại đó. Dễ thấy điểm cân bằng phải là nghiệm của phương trình: ݂(ݔ,ݑ)|௫ୀ௫೐ ,௨ୀ଴ = 0 (4.2) Hệ phi tuyến có thể có nhiều điểm cân bằng hoặc không có điểm cân bằng nào. Điều này hoàn toàn khác so với hệ tuyến tính, hệ tuyến tính luôn luôn có một điểm cân bằng là ݔ௘ = 0. 40 Ví dụ: Xét hệ con lắc mô tả bởi PTVP: ݈݉ଶ̈ߠ(ݐ) + ܤ̇ߠ(ݐ) + ݈݉݃ݏ݅݊ߠ = ݑ(ݐ) Xác định điểm cân bằng (nếu có). Giải quyết: Thành lập PTTT. Đặt ൜ ݔଵ(ݐ) = ߠ(ݐ) ݔଶ(ݐ) = ̇ߠ(ݐ) PTTT mô tả hệ con lắc: ̇ݔ(ݐ) = ࢌ(ݔ(ݐ),ݑ(ݐ)) Trong đó: ࢌ(ݔ,ݑ) = ቈ ݔଶ(ݐ) − ௚ ௟ ݏ݅݊ݔଵ(ݐ) − ஻௠௟మ ݔଶ(ݐ) + ଵ௠௟మ ݑ(ݐ)቉ Điểm cân bằng phải là nghiệm của phương trình: ࢞ ̇ = ࢌ(ݔ,ݑ)|௫ୀ௫೐ ,௨ୀ଴ = 0 ⇒ ቊݔଶ௘ = 0 − ௚ ௟ ݏ݅݊ݔଵ௘ − ஻ ௠௟మ ݔଶ௘ = 0 ⇒ ൜ݔଵ௘ = ݇ߨݔଶ௘ = 0 41 Kết luận: Hệ con lắc có vô số điểm cân bằng. 4.1.3 Ổn định tại điểm cân bằng 4.1.3.1 Định nghĩa Một hệ thống được gọi là ổn định tại điểm cân bằng ݔ௘ nếu như có một tác động tức thời đánh bật hệ ra khỏi ݔ௘ và đưa đến điểm ݔ௢ thuộc lân cận nào đó của ݔ௘ thì sau đó hệ có khả năng tự quay được về điểm cân bằng ݔ௘ ban đầu. Chú ý: Tính ổn định của hệ phi tuyến chỉ có nghĩa khi đi cùng với điểm cân bằng. Có thể hệ ổn định tại điểm cân bằng này nhưng không ổn định tại điểm cân bằng khác. Ví dụ: Con lắc đơn. 4.1.3.2 Ổn định và ổn định tiệm cận Lyapunov Cho hệ phi tuyến không kích thích mô tả bởi PTTT: ̇ݔ = ݂(ݔ,ݑ)|௨ୀ଴ (4.3) Giả sử hệ thống có điểm cân bằng ݔ௘ = 0. - Hệ thống được gọi là ổn định Lyapunov tại điểm cân bằng ݔ௘ = 0 nếu với ߝ > 0 bất kỳ bao giờ cũng tồn tại ߜ phụ thuộc ߝ sao cho nghiệm ݔ(ݐ) của phương trình (4.3) với điều kiện đầu ݔ(0) thỏa mãn: ‖ݔ(0)‖ < ߜ ⇒ ݔ(ݐ) < ߝ , ∀ݐ ≥ 0 42 - Hệ thống được gọi là ổn định tiệm cận Lyapunov tại điểm cân bằng ݔ௘ = 0 nếu với ߝ > 0 bất kỳ bao giờ cũng tồn tại ߜ phụ thuộc ߝ sao cho nghiệm ݔ(ݐ) của phương trình (4.3) với điều kiện đầu ݔ(0) thỏa mãn: ‖ݔ(0)‖ < ߜ ⇒ lim ௧→ஶ ݔ(ݐ) = 0 4.1.4 Phương pháp tuyến tính hóa Lyapunov Giả sử xung quanh điểm cân bằng ݔ௘, hệ thống phương trình (4.1) có thể tuyến tính hóa về dạng: ݔ෤̇ = ܣݔ෤ + ܤݑ෤ (4.4) Định lý : - Nếu hệ thống tuyến tính hóa (4.4) ổn định thì hệ phi tuyến (4.1) ổn định tiệm cận tại điểm cân bằng ݔ௘. - Nếu hệ thống tuyến tính hóa (4.4) không ổn định thì hệ phi tuyến (4.1) không ổn định tiệm cận tại điểm cân bằng ݔ௘. 43 - Nếu hệ thống tuyến tính hóa (4.4) ở biên giới ổn định thì không kết luận được gì về tính ổn định của hệ phi tuyến (4.1) tại điểm cân bằng ݔ௘. Ví dụ : Xét hệ con lắc mô tả bởi PTTT: ̇ݔ(ݐ) = ࢌ(ݔ(ݐ),ݑ(ݐ)) Trong đó : ࢌ(ݔ,ݑ) = ቈ ݔଶ(ݐ) − ௚ ௟ ݏ݅݊ݔଵ(ݐ) − ஻௠௟మ ݔଶ(ݐ) + ଵ௠௟మݑ(ݐ)቉ Xét tính ổn định của hệ thống tại điểm cân bằng. Giải quyết: - Mô hình tuyến tính quanh điểm cân bằng: ݔ௘ = [0 0]் . ݔ෤̇ = ࡭ݔ෤ + ࡮ݑ෤ ܽଵଵ = డ௙భడ௫భቚ(௫ୀ଴,௨ୀ଴) = 0 ܽଵଶ = డ௙భడ௫మቚ(௫ୀ଴,௨ୀ଴) = 1 ܽଶଵ = డ௙మడ௫భቚ(௫ୀ଴,௨ୀ଴) = − ௚௟ ܿ݋ݏݔଵ(ݐ)ቚ(௫ୀ଴,௨ୀ଴) = − ௚௟ ܽଶଶ = డ௙మడ௫మቚ(௫ୀ଴,௨ୀ଴) = − ஻௠௟మ 44 ⇒ ܣ = ቈ 0 1− ௚ ௟ − ஻ ௠௟మ ቉ ⇒ PTĐT: det(ݏܫ − ܣ) = det ൥ ݏ −1݃ ݈ ݏ + ܤ ݈݉ଶ ൩ = 0 ⇔ ݏଶ + ܤ ݈݉ଶ ݏ + ݃ ݈ = 0 Kết luận: Hệ thống ổn định (theo hệ quả tiêu chuẩn Hurwitz). - Mô hình tuyến tính quanh điểm cân bằng: ݔ௘ = [ߨ 0]். ݔ෤̇ = ࡭ݔ෤ + ࡮ݑ෤ ܽଵଵ = డ௙భడ௫భቚቀ௫ୀቂగ଴ቃ,௨ୀ଴ቁ = 0 ܽଵଶ = డ௙భడ௫మቚቀ௫ୀቂగ଴ቃ,௨ୀ଴ቁ = 1 ܽଶଵ = డ௙మడ௫భቚቀ௫ୀቂగ଴ቃ,௨ୀ଴ቁ = − ௚௟ ܿ݋ݏݔଵ(ݐ)ቚቀ௫ୀቂగ଴ቃ,௨ୀ଴ቁ = ௚௟ ܽଶଶ = డ௙మడ௫మቚቀ௫ୀቂగ଴ቃ,௨ୀ଴ቁ = − ஻௠௟మ ⇒ ܣ = ቈ0 1௚ ௟ − ஻ ௠௟మ ቉ ⇒ PTĐT det(ݏܫ − ܣ) = det ൥ ݏ −1 − ݃ ݈ ݏ + ܤ ݈݉ଶ ൩ = 0 ⇔ ݏଶ + ܤ ݈݉ଶ ݏ − ݃ ݈ = 0 Kết luận: Hệ thống không ổn định (PTĐT không thỏa điều kiện cần). 4.1.5 Phương pháp trực tiếp Lyapunov 4.1.5.1 Định lý ổn định Lyapunov: cho hệ phi tuyến không kích thích mô tả bởi PTTT (4.3). Giả sử hệ thống có điểm cân bằng ݔ௘ = 0. Nếu tồn tại một hàm ܸ(ݔ) sao cho: - ܸ(ݔ) ≥ 0 ,∀ݔ - ܸ(0) = 0 - ܸ̇(ݔ) ≤ 0 ,∀ݔ ≠ 0 Thì hệ thống (4.3) ổn định Lyapunov tại điểm ݔ௘. Nếu ܸ̇(ݔ) < 0 ,∀ݔ ≠ 0 thì hệ thống (4.3) ổn định tiệm cận Lyapunov tại điểm ݔ௘. Chú ý: Hàm ܸ(ݔ) thường được chọn là hàm toàn phương theo biến trạng thái. 45 4.1.5.2 Định lý không ổn định Lyapunov: cho hệ phi tuyến không kích thích mô tả bởi PTTT (4.3). Giả sử hệ thống có điểm cân bằng ݔ௘ = 0. Nếu tồn tại một hàm ܸ(ݔ) sao cho: - ܸ(ݔ) ≥ 0 ,∀ݔ - ܸ(0) = 0 - ܸ̇(ݔ) > 0 ,∀ݔ ≠ 0 Thì hệ thống (4.3) không ổn định Lyapunov tại điểm ݔ௘. Ví dụ 1 : Cho hệ thống mô tả bởi PTTT: ൜ ̇ݔଵ = −ݔଵ + ݔଶ + ݔଶ(ݔଵଶ + ݔଶଶ) ̇ݔଶ = −ݔଵ − ݔଶ − ݔଵ(ݔଵଶ + ݔଶଶ) Xác định trạng thái cân bằng của hệ thống và đánh giá tính ổn định của hệ thống tại trạng thái cân bằng. Giải quyết: Trạng thái cân bằng là nghiệm của phương trình: ൜ −ݔଵ + ݔଶ + ݔଶ(ݔଵଶ + ݔଶଶ) = 0 −ݔଵ − ݔଶ − ݔଵ(ݔଵଶ + ݔଶଶ) = 0 ⇔ ൜ݔଵ௘ = 0ݔଶ௘ = 0 Đánh giá tính ổn định: Chọn hàm Lyapunov ܸ(ݔ) = 12 (ݔଵଶ + ݔଶଶ) Ta có: - ܸ(ݔ) > 0 ,∀ݔ ≠ 0 - ܸ(0) = 0 ܸ̇(ݔ) = ݔଵ̇ݔଵ + ݔଶ̇ݔଶ = ݔଵ[−ݔଵ + ݔଶ + ݔଶ(ݔଵଶ + ݔଶଶ)] + ݔଶ[−ݔଵ − ݔଶ − ݔଵ(ݔଵଶ + ݔଶଶ)] = −ݔଵଶ − ݔଶଶ ⇒ ܸ̇(ݔ) < 0 ,∀ݔ ≠ 0 46 Hệ thống ổn định tiệm cận Lyapunov tại điểm cân bằng. Ví dụ 2: Cho hệ thống mô tả bởi PTTT: ൜ ̇ݔଵ = ݔଶ + ݔଵ(ݔଵଶ + ݔଶସ) ̇ݔଶ = −ݔଵ + ݔଶ(ݔଵଶ + ݔଶସ) Xác định trạng thái cân bằng của hệ thống và đánh giá tính ổn định của hệ thống tại trạng thái cân bằng. Giải quyết: Trạng thái cân bằng là nghiệm của phương trình: ൜ ݔଶ + ݔଵ(ݔଵଶ + ݔଶସ) = 0 −ݔଵ + ݔଶ(ݔଵଶ + ݔଶସ) = 0 ⇔ ൜ݔଵ௘ = 0ݔଶ௘ = 0 Đánh giá tính ổn định: Chọn hàm Lyapunov ܸ(ݔ) = 12 (ݔଵଶ + ݔଶଶ) Ta có : - ܸ(ݔ) > 0 ,∀ݔ ≠ 0 - ܸ(0) = 0 ܸ̇(ݔ) = ݔଵ̇ݔଵ + ݔଶ̇ݔଶ = ݔଵ[ݔଶ + ݔଵ(ݔଵଶ + ݔଶସ)] + ݔଶ[−ݔଵ + ݔଶ(ݔଵଶ + ݔଶସ)] = (ݔଵଶ + ݔଶଶ)(ݔଵଶ + ݔଶସ) ⇒ ܸ̇(ݔ) > 0 ,∀ݔ ≠ 0 Hệ thống không ổn định tại điểm cân bằng. 47 4.2. Thiết kế bộ điều khiển Mục tiêu của đề tài này là thiết kế bộ điều khiển sao cho AGV bám theo quỹ đạo tham chiếu ߟଷ − ݏ݌݈݅݊݁ được xây dựng trong chương 3. Nói cách khác AGV sẽ lần lượt đi qua tập hợp điểm G3 với một vận tốc mong muốn không đổi ݒ௥. Khi đến đích AGV tiếp cận pallet và nâng pallet đến vị trí mong muốn. Bộ điều khiển được thiết kế dựa vào phương pháp Lyapunov. Theo [7] từ (3.5) chọn hàm Lyapunov như sau: ܸ = ଵ ଶ (݁ଵଶ + ݁ଶଶ) + ଶ௞మ ݏ݅݊ଶ ቀ௘యଶ ቁ (4.5) ܸ̇ = ݁ଵ݁̇ଵ + ݁ଶ݁̇ଶ + ௘̇య௞మ ݏ݅݊݁ଷ (4.6) Thay (3.6) vào (4.6) ta có: ܸ̇ = ݁ଵ(߱௖݁ଶ − ݒ௖ + ݒ௥ܿ݋ݏ݁ଷ) + ݁ଶ(−߱௖ ଵ݁ + ݒ௥ݏ݅݊݁ଷ) + (߱௥ −߱௖)݇ଶ ݏ݅݊݁ଷ = −ݒ௖݁ଵ + ଵ݁ݒ௥ܿ݋ݏ݁ଷ + ݁ଶݒ௥ݏ݅݊݁ଷ − ఠ೎௞మ ݏ݅݊݁ଷ + ఠೝ௞మ ݏ݅݊݁ଷ (4.7) Để ܸ̇ mang giá trị âm ta chọn: ൜ ݒ௖ = ݒ௥ܿ݋ݏ݁ଷ + ݇ଵ݁ଵ ߱௖ = ߱௥ + ݇ଶ݁ଶݒ௥ + ݇ଷݏ݅݊݁ଷ (4.8) Trong đó ݇ଵ,݇ଶ, ݇ଷ là những giá trị dương. Thay (4.8) vào (4.7) ta có: ܸ̇ = −݇ଵ ଵ݁ଶ − ௞య௞మ ݏ݅݊ଶ݁ଷ < 0 (4.9) Từ (4.5) và (4.9) chứng minh rằng các sai số ଵ݁, ݁ଶ, ݁ଷ hội tụ về không khi thời gian t tiến ra vô cùng. Do đó bộ điều khiển (4.8) đáp ứng được yêu cầu đặt ra. 48 4.3. Hệ thống đo lường Mô hình toán học của AGV được thành lập dựa trên một số giả định như AGV di chuyển trên một mặt phẳng không trượt và bỏ qua ma sát trên các bánh xe. Chuyển động của AGV khi bám theo quỹ đạo mong muốn được đo bằng encoder trang bị trên hai bánh xe chủ động thì vận tốc quay có thể được tính như sau: ݒ௖_ݎ݈݁ܽ = ௥(ఠೝೢ_௥௘௔௟ାఠ೗ೢ_௥௘௔௟)ଶ (4.10) ߱௖_ݎ݈݁ܽ = ௥(ఠೝೢ_௥௘௔௟ିఠ೗ೢ_௥௘௔௟)ଶ௕ (4.11) Trong đó ߱௥௪_ݎ݈݁ܽ và ߱௟௪_ݎ݈݁ܽ là vận tốc góc của bánh phải và bánh trái đo được từ encoder. Vị trí của AGV được ước lượng dựa theo (3.2). ቐ ݔ௖ = ݔ௖௣ + ݒ௖_ݎ݈݁ܽ. ௦ܶܿ݋ݏΘ௖௣ ݕ௖ = ݕ௖௣ + ݒ௖_ݎ݈݁ܽ. ௦ܶݏ݅݊Θ௖௣ Θ௖ = Θ௖௣ + ௦ܶ ௖߱_ݎ݈݁ܽ (4.12) ௦ܶ là thời gian lấy mẫu và ݔ௖ ௣, ݕ௖௣,Θ௖௣ là tọa độ của AGV ở thời điểm trước đó. Từ các công thức này để điều khiển AGV theo mục tiêu của đề tài gồm các bước theo sơ đồ khối hình 4.1. 49 Hình 4.1 Sơ đồ khối của vòng điều khiển AGV bám quỹ đạo G3. Hình 4.1 Sơ đồ khối của vòng điều khiển AGV bám quỹ đạo G3. Path Planing Trajectory G3 Eq (3.7), (3.8), (3.9) Path Tracking Wheel Control AGV Controller Eq (4.8) Eq (3.3) ݒ ߱ Right Wheel Controller Left Wheel Controller Right DC Motor Left DC Motor ݑ௥௪ ݑ௟௪ Position Estimation Right Encoder Left Encoder Eq (4.10), (4.11) Eq (4.12) ݒ௖,߱௖ ݔ௥ ,ݕ௥,Θ௥, ݒ௥,߱௥ ݔ௖ , ݕ௖,Θ௖, ݒ௖,߱௖ ߱௥௪_ݎ݈݁ܽ ߱௟௪_ݎ݈݁ܽ 50 Trong hình 4.1 gồm bốn phần: - Path Planing: Thiết lập quỹ đạo đường đi cho AGV, quỹ đạo này chính là G3. - Path Tracking: Dùng bộ điều khiển (4.8) kiểm soát đường đi cho AGV. Từ (4.8) và (3.3) tính được vận tốc góc mong muốn cho bánh xe phải và trái. - Wheel Control: Bộ điều khiển động cơ được thiết kế riêng biệt cho hai bánh xe nhằm điều khiển vận tốc góc của bánh xe theo vận tốc góc tham chiếu ở phần Path Tracking. Để di chuyển chính xác dọc theo quỹ đạo G3, AGV phải biết vị trí và góc định hướng thực tế, chúng được ước tính bằng phương pháp đo sử dụng encoder. - Position Estimation: Ước lượng vị trí theo (4.10), (4.11), (4.12). Từ các vị trí này sẽ hồi tiếp về Path Tracking. 51 Chương 5 THIẾT KẾ THI CÔNG AGV VÀ MÔ PHỎNG 5.1 Thiết kế mô hình Mô hình AGV bao gồm: - Khung xe với chiều dài khoảng cách cho hai bánh xe là 20 cm và sử dụng mica 5 mm để làm khung. - Hai bánh chủ động nằm ở phía sau xe có đường kính 100 mm. Hình 5.1 Bánh xe dùng cho động cơ có bộ giảm tốc đường kính 100 mm. - Động cơ DC Tsukasa TG-35F-AG-18-A275 với điện áp 24 VDC, tốc độ không tải 7400 RPM, được trang bị encoder với độ phân giải 90 xung/vòng, được gắn trực tiếp trên bánh xe. Hình 5.2 Động cơ DC Tsukasa TG-35F-AG-18-A27. 52 - Một bánh xe omni nhôm nằm phía trước xe, bánh này giúp cho xe được cân bằng. Hình 5.3 Bánh xe omni nhôm. Thiết kế mô hình được mô phỏng như sau: Hình 5.4 Mô hình AGV mô phỏng. 53 5.2 Thiết kế mạch điện 5.2.1 Phương án thiết kế Hình 5.5 Cấu hình hệ thống điều khiển. Chức năng từng khối: - Master bao gồm một vi điều khiển pic 18F4550 dùng để tính toán giải thuật điều khiển bám cho AGV và truyền tín hiệu điều khiển xuống khối slave bằng giao thức I2C, tọa độ của AGV lần lượt sẽ được hiển thị trên LCD.