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

h 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<