Xây dựng phần mềm thiết kế bộ điều khiển số lqr/lqg

ầy đầy đủ các bước để thực hiện chuyển từ bài toán tương tự sang bài toán điều khiển số, mà chúng em chỉ sử dụng các kết quả đã có của bài toán biến đổi đó để tìm hiểu và xây dựng bộ điều khiển số nhằm điều khiển đối tượng dựa trên ngôn ngữ lập trình C/C++. Nội dung của bản báo cáo được chia thành các phần sau: Mục 1. Nội dung thực tập Mục 2. Nội dung phương pháp Bài toán điều khiển tối ưu LQR 2.2 Bài toán điều khiển tối ưu LQG Cả 2 mục trên này sẽ trình bầy nội dung, ưu nhược điểm và các ứng dụng của bài toán này cho các đối tượng thực tế. Mục 3. Bài toán điều khiển bám Nếu bài toán điều khiển chỉ đơn thuần là bài toán LQR/LQG thì chất lượng điều khiển thu được có thể chưa được tốt như mong muốn, ví dụ như hệ thống không có khả năng kháng được nhiễu hay vẫn còn tồn tại sai lệch tĩnh ở tín hiệu đầu ra. Do vậy phần này trình bầy về những vấn đề bám bằng cách đưa thêm vào 2 bài toán điều khiển ở phần trên các khâu lọc trước và khâu tích phân để nâng cao chất lượng của hệ thống. Mục 4. Thực hiện chương trình điều khiển viết bằng ngôn ngữ lập trình C++/Fmol Từ lý thuyết đã được tìm hiểu ở phần trước ta đi đến xây dựng phần mềm điều khiển cho các đối tượng thực. Mục 5. Ví dụ chương trình thiết kế và mô phỏng Phần này kiểm chứng lại độ chính xác của phần mềm thông qua mô phỏng và các ví dụ tường minh. 2. Nội dung phương pháp Cho hệ thống MIMO có mô hình plant K u y Hình 2.1: Mô hình bộ điều khiển phản hồi trạng thái (2.1) Trong đó AR, BR, CR, D u tín hiệu điều khiển y tín hiệu ra của đối tượng K bộ điều khiển phản hồi trạng thái Bài toán có nhiệm vụ xác định bộ điều khiển K sao cho hàm mục tiêu là nhỏ nhất = (2.2) Bài toán có tên là LQR (Linear Quadratic Regulator), nếu vector tín hiệu trạng thái x phản hồi là không đo được, song đối tượng là quan sát được thì ta có thể thiết kế thêm bộ quan sát trạng thái nhằm xác định giá trị x(t) thông qua việc đo những tín hiệu vào ra u(t), y(t) trong khoảng thời gian T hữu hạn, nếu ta dùng bộ quan sát trang thái Kalman thì bài toán đó được gọi là nài toán LQG (Linear Quadratic Gaussian). Do ta dùng máy tính để điều khiển hệ thống nên ta phải chuyển các phương trình liên tục sang dạng rời rạc, các phương trình có dạng như sau Mô hình rời rạc đối tượng (2.3) trong đó Mô hình hàm mục tiêu J= (2.4) với Q1, Q2 là ma trận trọng số do người thiết kế đưa vào dựa vào mối quan hệ giữa trạng thái các biến x và bộ điều khiển. Để tìm được bộ điều khiển ta phải giải phương trình Riccati, do việc tìm phương trình Riccati tương đối phức tạp nên ta chỉ đưa ra kết quả (2.5) Thường viết dưới dạng (2.6) với Cuối cùng ta đạt được (2.7) với (2.8) Phần tiếp theo ta sẽ tìm hiểu về bài toán LQR, LQG xác định bộ điều khiển tối ưu, phần mà ta sẽ thực hiện xuyên suốt trong bài thực tập. 2.1 Bài toán điều khiển tối ưu LQR Trong phần trước chúng ta xây dựng bộ điều khiển tối ưu bằng cách tối thiểu hàm mục tiêu, ta thấy rằng kết quả của K(k) thay đổi trong từng bước tính, do vậy để thuận tiện trong việc điều khiển hệ thống ta xác định bộ điều khiển tối ưu K là không đổi, trong thực tế đây là cách giải quyết tối ưu, ta gọi đó là bài toán Linear Quadratic Gaussian (LQR). Trong phần này ta sẽ tìm ra bộ điều khiển LQR (hay K) và hiệu quả của nó thông qua các ví dụ. Như ta đã nói ở phần trước để xác định K ta phải dựa vào phương trình Riccati, và tại trạng thái ổn định, S(k) trở thành S(k+1) (cả hai được gọi là S), phương trình Riccati trở thành. (2.10) Có nhiều cách để tính S, tuy nhiên ta sử dụng phương pháp eigenvector composition, đây là phương pháp phần lớn các phần mềm sử dụng. Nó dựa trên công thức Hamilton (2.11) và ma trận Hamilton (2.12) Với là vector Lagrange của mỗi giá trị k Giải phương trình Hamilton đã chứng minh được rằng hệ có 2n nghiệm, trong đó có n nghiệm là ổn định và n nghiệm không ổn định. Hơn thế nửa n nghiệm ổn định là tối ưu, làm cho bộ khuếch đại không đổi trong hệ thống kín. Chúng ta sẽ đưa ma trận Hamilton về dạng đường chéo, với cách giải quyết này ta sẽ đạt được bộ điều khiển tối ưu. Chúng ta phân chia như sau, một nữa nghiệm của hệ thống ở bên trong đường tròn và một nửa nghiệm ở bên ngoài đường tròn. Khi ấy trở thành (2.13) Với là ma trận đường chéo của quỹ đạo nghiêm không ổn định (|z|>1) và là ma trận đường chéo của quỹ đạo nghiệm ổn định (|z|<1). đạt được bằng cách biến đổi (2.14) Trong đó (2.15) với là ma trận eigenvector kết hợp với ma trận Hamilton bên ngoài đường tròn đơn vị và là ma trận eigenvector kết hợp với ma trận Hamilton bên trong đường tròn đơn vị. Sau khi giải phương trình Riccati kết quả ta có (2.16) và luật điều khiển (2.17) K được tính như sau = (2.18) Vậy bài toán LQR được thực hiện qua các bước sau. +Xác định ma trận Hamilton +Xác định ma trận kết hợp với ma trận Hamilton có quỹ đạo nghiệm bên trong đường tròn đơn vị. +Xác định K từ (2.16) và (2.18) Ta có dùng phần mền Matlab để tính giá trị của bộ điều khiển K lệnh như sau: [K,S]=dlqr(A,B,Q1,Q2) Trong quá trình xây dựng phần mềm em sử dụng phần mềm FMOL [1] để tính giá trị bộ điều khiển lệnh như sau: [K,S]=dare(A,B,Q1,Q2) Ví dụ: Cho đối tượng có các ma trận sau A= B= C = Ma trận hàm mục tiêu 1 Lệnh thực hiện trên Matlab [K,S]=dlqr(A,B,Q1,Q2) ; Kết quả như sau 2.2 Bài toán điều khiển tối ưu LQG Phương pháp quan sát trạng thái tối ưu rất hấp dẫn bởi nó sử dụng được cho hệ thống MIMO rất dễ dàng, và cho phép người thiết kế nhanh chóng xác định được ma trận quan sát trạng thái L. Trong phần này ta sẽ sử dụng bộ lọc Kalman đễ xây dựng thuật toán bởi bộ lọc này loại bỏ một cách triệt để ảnh hưởng của nhiễu vào hệ thống và một điều lý thú là thuật toán xác định bộ quan sát Kalman hoàn toàn giống như việc thiết kế bộ điều khiển tối ưu phản hồi trạng thái (LQR). Với cách giải quyết tương tự như với bài toán LQR, ta sẽ xây dựng cách giải quyết bộ quan sát trạng thái tối ưu với bộ điều khiển L. Mô hình rời rạc của đối tượng. (2.19) Với nhiễu quá trình và nhiễu đo đầu ra , giá trị nhiễu được xác định bởi (2.20) Tương tự như phần trước ta củng có kết quả như sau. (2.21) Từ kết quả này ta thấy có sự tương đồng giữa hai công thức do vậy ta có bảng tương đương như sau. Control Estimation Khi ấy ma trận Hamilton có dạng sau. (2.22) Giá trị của ma trận M như sau (2.23) với là ma trận kết hợp với ma trận nằm trong vòng tròn đơn vị, kết quả ma trận khuếch đại trạng thái ổn định Kalman-filter. (2.24) Cũng thực hiện tương tự như phần xây dựng bộ khuếch đại tối ưu, ta được bộ khuếch đại ổn định trạng thái của bộ lọc Kalman. Kết hợp cả hai phần một và hai ta có mô hình tổng quát của hệ thống điều khiển LQG, LQR với sự tồn tại của nhiễu trong hệ thống. Đây là mô hình đạt hiệu quả cao không chỉ cho hệ MIMO mà còn cho cả hệ thống SISO. LQG K Plant r yr - Estimator w LQR v Hình 2.2: Mô hình bài toán LQG Lệnh trong Matlab để tính ma trận L L=dlqr(A’,C’,S,T) Tương tự lệnh trong quá trình viết phần mềm L=dare(A’,B’,S,T) Ví dụ: Cho đối tượng có các ma trận sau A= B= C = Ma trận hàm tổn hao 1 Lệnh thực hiện trên Matlab L=lqr(A’,C’,S,T) ; Kết quả như sau L=[1.0536 -0.3899] 3. Bài toán điều khiển bám ở phần trước ta đã phân tích hai bài toán LQR, LQG việc tồn tại sai lệch tĩnh là không thể tránh khỏi, điều này gây khó chịu cho người sử dụng là tín hiệu r(t) được đưa vào dưới dang một hằng số, song hệ thống lại không cho ra tín hiệu mong muốn. Để loại bỏ sai lệch tĩnh ta phải bỗ xung thêm một khâu tiền xử lý. Tuỳ vào điều kiện cụ thể mà ta thêm khâu tiền xử lý dưới là hằng số hay là một khâu tích phân, đối với khâu tích phân thường dùng khi hệ thống có nhiễu can thiệp. Lúc này ta có thể phân tích toàn bộ hệ thống gồm đối tượng, khâu tiền xử lý và bộ điều khiển thành mô hình có dạng tổng quát như sau: C P r uc yr xp Hình 3.1: Mô hình rút gọn của hệ thống trong đó C (Controller) bộ điều khiển thực hiện thuật toán điều khiển số thu được từ mỗi mô hình tương ứng P (Plant) đối tượng điều khiển r giá trị đặt của tín hiệu điều khiển xp biến trạng thái của đối tượng yr đầu ra của đối tượng Như vậy bộ điều khiển C ta có thể coi là một đối tượng điều khiển có hai đầu vào là r và xp còn tín hiệu ra là uc được đưa vào để điều khiển đối tượng P. Phương trình trạng thái rời rạc mô tả bộ điều khiển C có dạng (3.1) ở đây xc là biến trạng thái của bộ điều khiển C v là tín hiệu vào của bộ điều khiển C AcR, BcR, CcR, Dc là các ma trận có vai trò tương tự như các ma trận của đối tượng ở trên. Nhiệm vụ của ta là trong mỗi trường hợp cụ thể ta phải xác định các ma trận Ac, Bc, Cc, Dc của bộ điều khiển từ đó làm cơ sở để xây dựng thuật toán điều khiển số bằng ngôn ngữ C/C++. 3.1 Bộ điều khiển phản hồi trạng thái, có khâu lọc trước Mô hình bài toán Nu Nx K Plant Hr Controller r xp xr uss yr - uc Hình 3.1: Sơ đồ khối hệ thống có khâu lọc trước và phản hồi Các ma trận Nx R, Nu R chính là các ma trận của khâu lọc trước, Nx có tác dụng tạo ra tín hiệu ra xr vào đầu vào của đối tượng để đạt được giá trị mong muốn. Thông thường người thiết kế có đủ những hiểu biết về đối tượng, nên biết được trạng thái nào để có thể duy trì trạng thái đầu ra mong muốn. Trong trường hợp đối tượng là phức tạp, chúng ta sẽ gặp rất nhiều khó khăn để xác định trạng thái đó, do vậy ta xây dựng một bộ điều khiển trạng thái làm cân bằng với đầu vào, tức là khi đầu ra ở trạng thái mong muốn thì tín hiệu điều khiển uc luôn được duy trì ở một giá trị nào đó chứ nó không hôi tụ về 0, Nu là bộ điều khiển có tác dụng như vậy. Phương trình trạng thái rời rạc của bộ điều khiển C lúc này thu được từ mô hình trên có dạng sau (3.2) hay (3.3) và mô hình của đối tượng (3.4) Từ (3.3) và coi các biến trạng thái của bộ điều khiển trong trường hợp này là bằng không. Các ma trận của bộ điều khiển C trong trường hợp này như sau: Ac=, Bc=, Cc= Dc= (3.5) với là ma trận gồm các phần tử có hệ số 0 Biến trạng thái xP của đối tượng sẽ tìm được theo phương trình trạng thái quen thuộc sau (3.6) Các ma trận Nx, Nu sẽ tìm được qua việc biến đổi tương đương các phương trình sau ở trạng thái xác lập ta có Vậy với và ta thu được hay Cuối cùng ta có (3.7) mặt khác Và ở trạng thái xác lập (3.8) kết hợp (3.6), (3.7) ta có (3.9) ở đây các ma trận R, R, IR,I1R Sau khi xác định được Nx, Nu ta xác định K tương tự như phần trên ta đã xét Ví dụ: Do trong phần thực tập này chúng em chưa có điều kiện mô phỏng nên từ ví dụ này trở đi chỉ đưa ra kết quả của đầu ra y(k) trong các bước tính. Cho đối tượng MIMO có mô hình như sau. Kết quả chạy trên phần mềm mô phỏng như sau Sau 12 bước tính ta có kết quả đầu ra y(12) và uc(k) như sau Với tín hiệu đặt hai đầu vào r là [2 1] ta thấy kết quả tương đối chính xác, tuy nhiên khi cho nhiễu vào kết quả không được chính xác như trước. 3.2 Bộ điều khiển có khâu lọc trước và có khâu quan sát trạng thái Mô hình bài toán Nu Nx K Plant Controller r xr uss yr - Estimator uc Hình 3.1: Sơ đồ khối hệ thống có khâu lọc trước và phản hồi Khi tín hiệu trả về không đo được ta cần phải thêm một khâu quan sát trạng thái để quan sát gía trị xP (k). Tương tự như các phần trước thuật toán tìm ma trận phản hồi trạng thái K không thay đổi, thuật toán này còn được áp dụng để tìm ma trận LP của khâu quan sát trạng thái Kalman. Nhưng ở đây khi tìm LP ta cần thay thế tương đương và , một điều cần chú ý là số chiều của ma trận LP lúc này sẽ là LP R, các ma trận Nx, Nu vẫn được tìm theo phương pháp trên. LP=dlqr(A’,C’,S,T) S, T ma trận trọng số của khâu lọc Kalman Phương trình trạng thái rời rạc của bộ điều khiển C lúc này thu được từ mô hình trên có dạng sau = (3.10) Phương trình của biến trạng thái khi có bộ quan sát trạng thái là (3.11) thay thế biểu thức uc vừa tìm được ở trên ta có (3.12) với tín hiệu đặt của bộ điều khiển là giá trị đặt r và biến trạng thái thì từ (3.10) và (3.12) ta tìm được các ma trận tương ứng của bộ điều khiển C như sau: (3.13) Với các ma trận của bộ điều khiển tìm được trên ta có thể dễ dàng sử dụng nó cho việc lập trình. Ví dụ: Dùng lại ví dụ ở mục 1 với ma trận trọng số khâu lọc Kalman được bổ xung kết quả mô phỏng như sau. Sau 13 bước tính kêt quả như sau và Nhược điểm của 2 bài toán trên là hệ thống rất nhậy với nhiễu của ma trận K do vây ta có thể thêm khâu tích phân vào hệ thống để hy vọng khử được hoàn toàn ảnh hưởng của ma trận K cũng như sai lệch tĩnh sủa hệ thống. ở hai phần trên chúng ta đã cùng nhau xem xét ảnh hưởng của mô hình có Nx, Nu đối với một đối tượng bất kỳ và đi đến nhận xét rằng chất lượng của hệ thống được nâng lên rõ rệt. Đáp ứng đầu ra luôn thực hiện được như giá trị đặt yêu cầu. Tuy nhiên với hai mô hình trên vấn đề kháng nhiễu vẫn chưa được đề cập đến. Nói một cách khác hệ thống sẽ không giữ được giá trị đặt như yêu cầu nếu có nhiễu bên ngoài tác động vào hệ thống. Bởi vậy dưới đây ta sẽ trình bầy về hai mô hình trong đó có đưa thêm vào hệ thống khâu tích phân có nhiệm vụ khử nhiễu nhằm nâng cao chất lượng hệ thống hơn. 3.3 Bộ điều khiển có khâu tích phân, không có khâu quan sát Mô hình bài toán lúc này được cộng thêm vào các biến trạng thái ở khâu tích phân mà ta đã thêm vào hệ thống nhằm khắc phục nhiễu Controller Nx K Plant Hr r xp xr yr - uc w - Hình 3.1: Sơ đồ khối hệ thống có khâu tích phân không có khâu quan sát Đối tượng vẫn có mô hình trạng thái (3.14) biến trạng thái xI lúc này thu được vẫn là Cuối cùng ta được mô hình đối tượng có số bậc tăng lên như sau (3.15) Luật điều khiển lúc này sẽ là (3.16) Như vậy ta sẽ sử dụng mô hình này để tìm ma trận phản hồi . Thuật toán tìm K1 vẫn không thay đổi theo các phương pháp đã trình bầy ở trên. Để thuận tiện cho việc lập trình ta vẫn tìm cách biểu diễn bộ điều khiển C theo phương trình trạng thái rời rạc như đã nói ở trên Từ mô hình ta có thể coi xI là biến trạng thái của bộ điều khiển, với (3.17) và tín hiệu ra của bộ điều khiển là (3.18) Kết hợp (3.17) và (3.18) thì các ma trận trạng thái tương ứng của bộ điều khiển C như sau (3.19) Ví dụ: Sử dụng lại ví dụ 1 bổ xung thêm vécto đây là vector của ma trận đường chéo ghép với ma trân Q1 thu được ma trận mới là ma trận trọng số của hệ mới. Kết quả như sau Ma trận trọng số của hệ mới là Với nhiễu là kết quả sau 35 bước tính Kết quả cho ta thấy nhiễu không ảnh hưởng đến hệ thống, thực tế khi cho nhiễu lớn hơn nữa thì kết quả đầu ra vẫn hội tụ ở giá trị mong muốn khi tăng bước tính lên, đây là kết quả mà các phần trước không có. 3.4 Bộ điều khiển có khâu tích phân và có khâu quan sát Khi tín hiệu trả về không đo được chúng ta phải xây dựng thêm khâu quan sát để xác định của đối tượng thay vì các giá trị được giả thiết là đã biết như ở phần trên Mô hình bài toán Controller Nx K Plant Hr r xr yr - uc w - Estimator Hình 3.1: Sơ đồ khối hệ thống có khâu tích phân và khâu quan sát trạng thái Với mô hình mới này thuật toán tìm ma trận phản hồi trạng thái K1 vẵn không thay đổi tức là K1 vẫn là nghiệm của hàm dare() với các ma trận ở mô hình mở rộng như đã nói ở trên. Nhưng ma trận quan sát trạng thái LP trong trường hợp này vẫn sử dụng đối tượng cũ. Phương trình rời rạc của bộ điều khiển C được tìm như sau: Từ mô hình trên ta có tín hiệu ra từ khâu tích phân (3.20) Khi có khâu quan sát trạng thái thì biến trạng thái của đối tượng bằng (3.21) Với việc coi các biến trong trường hợp này là và ta được (3.22) Tín hiệu ra uc của bộ điều khiển bằng (3.23) Kết hợp (3.22) và (3.23) ta thu được ma trận của bộ điều khiển C có dạng (3.24) Ví dụ : Dùng đối tượng như các ví dụ trước ta có Với nhiễu đầu vào ta có kết quả sau 40 bước tính và Trong phần này ta đã xây dựng các thuật toán điều khiển nhằm nâng cao chất lượng hệ thống và các tham số của bộ điều khiển cũng như những ví dụ minh hoạ tường minh, phần tiếp theo ta sẽ tiến hành xây dựng phần mềm thực hiện các thuật toán trên bằng ngôn ngữ C/C++ 4. Thực hiện chương trình điều khiển viết bằng ngôn ngữ lập trình C++/Fmol Ngôn ngữ C++ là ngôn ngữ rất phổ biến trong việc xây dựng phần mềm điều khiển tự động, bởi nó thuận tiện cho người sử dụng (không như ngôn ngữ máy hay Assembler) và đảm bảo tính thời gian thực rất cao. Để chương trình điều khiển được rõ ràng, dễ hiểu trước tiên chúng em xin trình bầy khái quát về một số lớp trong thư viện Fmol mà sẽ sử dụng trong chương trình sau này. + Lớp Matrix được thực hiện trong file “Matrix.h”, gồm nhiều tính năng như khai báo một ma trận, cộng trừ nhân chia ma trận…đễ hiểu rõ hơn ta đọc file “doc” trong thư viện FMOL [1]. + Lớp Vector được thực hiện trong file ”Vector.h”, nó cũng có tính năng tương tự như lớp ma trận. + Ngoài ra ta còn sử dụng thư viện “Control.lib” để tính bộ điều khiển tối ưu (K=dare(A,B,Q,R) ta đã đề cập đến ở phần trước), để sử dụng thư viện này ta cần khai báo các lệnh sau trong hàm main(). using namespace fmol; using namespace fmol::control; + Lớp DSS về cơ bản định nghĩa các hàm để tính toán đưa ra các ma trận trạng thái A, B, C, D khi ta đưa vào hàm này đối số là số biến trạng thái, số tín hiệu đầu vào, số tín hiệu đầu ra, và thời gian trích mẫu Td. Ví dụ: DSS plant(2,2,2,0.1,0); // khai báo đối tượng có 2 biến trạng thái, 2 đầu vào, 2 đầu ra, thời gian trích mẫu 0.1 sec, không có trễ. Vector u(2); plant.A // gọi ma trận A của đối tượng plant (có kiểu DSS) Vector y = plant.step(u) // tính đầu ra của đối tượng y khi cho vector đầu vào u (lệnh này ta có sử dụng trong chương trình). Tiếp theo xây dựng một thư viện có tên “lq.h” gồm 4 hàm sau: Trả về bộ điều khiển dưới dạng phương trình trạng thái A, B, C, D trong trường hợp bài toán LQR, các đối của hàm đều để ở dạng tham chiếu để ta không thể thay đổi nội dung của đối, plant là mô hình của đối tượng cho dưới dạng phương trình trạng thái gồm các ma trận A, B, C, D. Các ma trận trọng số Q, R của hàm mục tiêu đều có kiểu ma trận, các hàm sau đều khai báo theo chuẩn như vậy. DSS lqr(const DSS& plant,const Matrix& Q,const Matrix& R); - Trả về bộ điều khiển trong bài toán LQG, trong đó ma trận S, T là ma trận của khâu lọc Kalman. DSS lqg(const DSS& plant,const Matrix& Q,const Matrix& R, const Matrix& S,const Matrix& T); Có khâu tích phân, không có khâu quan sát. Trong đó vector qi là vector đường chéo của ma trận chéo ghép với ma trân Q tạo thành ma trận mới để tính K. Ví dụ: vector mới có dạng K=dare(A,B,Q’,R) DSS lqri(const DSS& plant,const Matrix Q,const Matrix R, const Vector& qi); - Có khâu tích phân, có khâu quan sát. DSS lqgi(const DSS& plant,const Matrix& Q,const Matrix& R, const Vector& qi, const Matrix& S,const Matrix& T); Ngoài ra để cho chương trình không dài dòng ta xây dựng thêm một hàm có tác dụng tính Nx, Nu, hàm này có đối là đối tượng đưa vào và trả về hai giá trị là Nx, Nu. pair Nxnu(const DSS& plant); Bốn hàm này được khai báo ở header file “lq.h” như sau // lq.h: interface for the lq class. ////////////////////////////////////////////////// #if !defined(AFX_LQ_H__ADA7F2E3_26FF_11D7_913B_D04105F11C69__INCLUDED_) #define AFX_LQ_H__ADA7F2E3_26FF_11D7_913B_D04105F11C69__INCLUDED_ #if _MSC_VER > 1000 #pragma once #endif // _MSC_VER > 1000 #include #include namespace fmol { namespace control { pair Nxnu(const DSS& plant); // bo dieu khien phan hoi trang thai, co khau loc truoc DSS lqr(const DSS& plant,const Matrix& Q,const Matrix& R); // khong co khau I va co khau quan sat va khau loc truoc DSS lqg(const DSS& plant,const Matrix& Q,const Matrix& R, const Matrix& S,const Matrix& T); // co khau I va khong co quan sat DSS lqri(const DSS& plant,const Matrix Q,const Matrix R, const Vector& qi); // co khau I va co khau quan sat DSS lqgi(const DSS& plant,const Matrix& Q,const Matrix& R, const Vector& qi, const Matrix& S,const Matrix& T); } } #endif // !defined(AFX_LQ_H__ADA7F2E3_26FF_11D7_913B_D04105F11C69__INCLUDED_) Sau đó thực hiện các hàm trên ở file có đuôi .cpp “lq.cpp”. // lq.cpp: implementation of the lq class. ////////////////////////////////////////////////// #include "lq.h" #include #include #include ////////////////////////////////////////////////// // Construction/Destruction ////////////////////////////////////////////////// namespace fmol { namespace control { pair Nxnu(const DSS& plant) { //Nx, Nu duoc tinh theo cong thuc (3.8) int m,n;Matrix Nx,Nu; m=plant.A.nrows(); n=plant.B.ncols(); Matrix I(m,m),I2(n,n),M,O(n,n),I1(m+n,n),H; M=colx(rowx(plant.A-I.eye(m),plant.C),rowx(plant.B,O)); I1(m+1,m+n,1,n)=I2.eye(n); H=inv(M)*I1; Nx=H(1,m,1,n); Nu=H(m+1,m+n,1,n); return make_pair(Nx,Nu); } // bo dieu khien phan hoi trang thai, co khau loc truoc DSS lqr(const DSS& plant,const Matrix& Q,const Matrix& R) { //Bo dieu khien da duoc xay dung trong (3.5) int n = plant.nX(); int m = plant.nI(); int p = plant.nO(); DSS ctrl(0,p+n,m,plant.Ts); Matrix Nx, Nu, K, X; (Nx,Nu) = Nxnu(plant); (X,K) = dare(plant.A,plant.B,Q,R); cout<<"K:"<<K; ctrl.D = colx(K*Nx+Nu,-K); return ctrl; } // khong co khau I va co khau quan sat va khau loc truoc DSS lqg(const DSS& plant,const Matrix& Q,const Matrix& R, const Matrix& S,const Matrix& T) { //Bo dieu khien da duoc xay dung trong (3.13) int n = plant.nX(); int m = plant.nI(); int p = plant.nO(); DSS ctrl(n,p+n,m,plant.Ts); Matrix Nx, Nu, K, X,L,V(m,n); (Nx,Nu) = Nxnu(plant); (X,K) = dare(plant.A,plant.B,Q,R); (X,L) = dare(plant.A.t(),plant.C.t(),S,T); cout<<"L:"<<L; ctrl.A=(plant.A-L.t()*plant.C-plant.B*K); ctrl.B=colx(plant.B*(Nu+K*Nx),L.t()*plant.C); ctrl.C=-K; ctrl.D=colx(Nu+K*Nx,V); return ctrl; } // co khau I va khong co quan sat DSS lqri(const DSS& plant,const Matrix Q,const Matrix R, const Vector& qi) { int n = plant.nX(); int m = plant.nI(); int p = plant.nO(); DSS ctrl(p,p+n,m,plant.Ts); Matrix Nx, Nu, K1(m,n),Ki(m,m),K, X,I(p,p),Ad, J(m,m),O(n,m),Bd,Qd(m+n,m+n),Qi(m,m); Ad=colx(rowx(J.eye(m),O),rowx(plant.C,plant.A)); //Bo dieu khien da duoc xay dung trong (3.17) Bd=rowx(O,plant.B); Qi.diag()=qi; Qd(1,n,1,n)=Q(1,n,1,n); Qd(n+1,n+m,n+1,m+n)=Qi(1,m,1,m); (Nx,Nu) = Nxnu(plant); (X,K) = dare(Ad,Bd,Qd,R); Ki(1,m,1,m)=K(1,m,1,m); K1(1,m,1,n)=K(1,m,m+1,m+n); cout<<"Ki=:"<<Ki; cout<<"K1=:"<<K1; ctrl.A=I.eye(m); ctrl.B=colx(Ki,-Ki*plant.C); ctrl.C=I.eye(m); ctrl.D=colx(K1*Nx,-K1); return ctrl; } // co khau I va co khau quan sat DSS lqgi(const DSS& plant,const Matrix& Q,const Matrix& R, const Vector& qi, const Matrix& S,const Matrix& T) { //Bo dieu khien da duoc xay dung trong (3.24) int n = plant.nX(); int m = plant.nI(); int p = plant.nO(); DSS ctrl(n+p,p+n,m,plant.Ts); Matrix Nx, Nu, K1(m,n), Ki(m,m), K, X, I(m,m), L,Ad,Bd,Qd(m+n,m+n),Qi(m,m),O(n,m),O1(m,n),J(m,m); Ad=colx(rowx(J.eye(m),O),rowx(plant.C,plant.A)); Bd=rowx(O,plant.B); Qi.diag()=qi; Qd(1,n,1,n)=Q(1,n,1,n); Qd(n+1,n+m,n+1,m+n)=Qi(1,m,1,m); (Nx,Nu) = Nxnu(plant); (X,K) = dare(Ad,Bd,Qd,R); (X,L) = dare(plant.A.t(),plant.C.t(),S,T); Ki(1,m,1,m)=K(1,m,1,m); K1(1,m,1,n)=K(1,m,m+1,m+n); cout<<"K:"<<K; cout<<"L:"<<L; ctrl.A=colx(rowx(I.eye(m),plant.B),rowx(O1,pla nt.A-L.t()*plant.C-plant.B*K1)); ctrl.B=colx(rowx(Ki,plant.B*K1*Nx),rowx(- Ki*plant.C,L.t()*plant.C)); ctrl.C=colx(I.eye(m),-K1); ctrl.D=colx(K1*Nx,O1); return ctrl; } } } 5. Ví dụ chương trình thiết kế và mô phỏng ** Để thử kết quả nhận được ta xây dựng 4 file ví dụ có tên lq_test.cpp #include "fmol\matrix.h" #include "fmol\vector.h" #include "fmol\control\ssmodels.h" #include #include "lq.h" using namespace fmol; using namespace fmol::control; void main() { Matrix Q(2,2),R(2,2),Nx,Nu; DSS plant(2,2,2,1); Q(1,1)=1; R(1,1)=1;R(2,2)=1; plant.A(1,1)=1.5;plant.A(1,2)=1; plant.A(2,1)=-.5;plant.A(2,2)=0; plant.B(1,1)=1 ;plant.B(1,2)=2; plant.B(2,1)=0 ;plant.B(2,2)=1.5; plant.C(1,1)=1 ;plant.C(1,2)=0; plant.C(2,1)=0 ;plant.C(2,2)=1; (Nx,Nu)=Nxnu(plant); cout<<"Nx:"<<Nx; cout<<"Nu:"<<Nu; DSS ctrl=lqr(plant,Q,R) ; int s; cout<<"nhap so lan tinh\n"; cin>>s; Vector v(2,1.0),u(2),v1(4,1.0),e; v1(1)=2; for(int i=1;i<=s;++i) { v=plant.step(u); e=plant.x; v1(3,4)=e(1,2); u=ctrl.step(v1); cout<<v<<"\t "<<u<<"\n"; } } lqg_test.cpp #include "fmol\matrix.h" #include "fmol\vector.h" #include "fmol\control\ssmodels.h" #include #include "lq.h" using namespace fmol; using namespace fmol::control; using namespace std; void main() { Matrix Q(2,2),R(2,2),S(2,2),T(2,2),Nx,Nu; DSS plant(2,2,2,1); Q(1,1)=1; R(1,1)=1;R(2,2)=1; plant.A(1,1)=1.5;plant.A(1,2)=1; plant.A(2,1)=-.5;plant.A(2,2)=0; plant.B(1,1)=1 ;plant.B(1,2)=2; plant.B(2,1)=0 ;plant.B(2,2)=1.5; plant.C(1,1)=1 ;plant.C(1,2)=0; plant.C(2,1)=0 ;plant.C(2,2)=1; S(1,1)=0.5; T(1,1)=1;T(2,2)=1; (Nx,Nu)=Nxnu(plant); cout<<"Nx:"<<Nx; cout<<"Nu:"<<Nu; DSS ctrl=lqg(plant,Q,R,S,T) ; int s; cout<<"nhap so lan tinh\n"; cin>>s; Vector v,u(2),v1(4,1.0),e,w(2,0.5); v1(1)=2; for(int i=1;i<=s;++i) { v=plant.step(u); e=plant.x; v1(3,4)=e(1,2); u=ctrl.step(v1); cout<<v<<"\t "<<u<<"\n"; } } lqri_test.cpp #include "fmol\matrix.h" #include "fmol\vector.h" #include "fmol\control\ssmodels.h" #include #include "lq.h" using namespace fmol; using namespace fmol::control; using namespace std; void main() { Matrix Q(2,2),R(2,2),Nx,Nu; Vector Qi(2); DSS plant(2,2,2,1); Qi(1)=1;Qi(2)=1; Q(1,1)=1;Q(2,2)=1; R(1,1)=1;R(2,2)=1; plant.A(1,1)=1.5;plant.A(1,2)=1; plant.A(2,1)=-0.5;plant.A(2,2)=0; plant.B(1,1)=1 ;plant.B(1,2)=2; plant.B(2,1)=0 ;plant.B(2,2)=1.5; plant.C(1,1)=1 ;plant.C(1,2)=0; plant.C(2,1)=0 ;plant.C(2,2)=1; (Nx,Nu)=Nxnu(plant); cout<<"Nx:"<<Nx; cout<<"Nu:"<<Nu; DSS ctrl=lqri(plant,Q,R,Qi) ; int s; cout<<"nhap so lan tinh\n"; cin>>s; Vector v,u(2),w(2,0.5),v1(4,1.0),e; v1(1)=1.5; for(int i=1;i<=s;++i) { v=plant.step(u); e=plant.x; v1(3,4)=e(1,2); u=ctrl.step(v1)+w; cout<<v<<"\t "<<u<<"\n"; } } lqgi_test.cpp. #include "fmol\matrix.h" #include "fmol\vector.h" #include "fmol\control\ssmodels.h" #include #include "lq.h" using namespace fmol; using namespace fmol::control; using namespace std; void main() { Matrix Q(2,2),R(2,2),S(2,2),T(2,2),Nx,Nu; Vector Qi(2); DSS plant(2,2,2,1); Qi(1)=1;Qi(2)=1; Q(1,1)=1;Q(2,2)=1; R(1,1)=1;R(2,2)=1; plant.A(1,1)=1.5;plant.A(1,2)=1; plant.A(2,1)=-0.5;plant.A(2,2)=0; plant.B(1,1)=1 ;plant.B(2,1)=2; plant.B(2,1)=0 ;plant.B(1,2)=1.5; plant.C(1,1)=1 ;plant.C(1,2)=0; plant.C(2,1)=0 ;plant.C(2,2)=1; S(1,1)=0.5;S(2,2)=0; T(1,1)=1;T(2,2)=1; (Nx,Nu)=Nxnu(plant); cout<<"Nx:"<<Nx; cout<<"Nu:"<<Nu; DSS ctrl=lqgi(plant,Q,R,Qi,Q,R) ; int s; cout<<"nhap so lan tinh\n"; cin>>s; Vector v,u(2),w(2,.5),v1(4,1.0),e; v1(1)=1.5; for(int i=1;i<=s;++i) { v=plant.step(u); e=plant.x; v1(3,4)=e(1,2); u=ctrl.step(v1)+w; cout<<v<<"\t "<<u<<"\n"; } }  Tài liệu tham khảo [1] TS. Hoàng Minh Sơn: Thư viện phần mềm FMOL. [2] David .J Franklin: Digital Control of Dynamic System. [3] Nguyễn Doãn Phước: Lý thuyết điều khiển tuyến tính. ._.