MPC Swing-up

[BystrickyK]

Tak překvapivě se mi povedlo udělat ten nelineární MPC, který zvládá i swing-up. Kroky však počítá dost pomalu, zvlášť se kousá ve fázi kdy je kyvadlo v horizontální poloze. Taky limitaci vstupu (napětí na motor) jsem mu musel výrazně zvýšit na +- 25V, protože pro naši limitaci +- 12V asi potřebuje udělat na začátku několik kyvů, a na to mu nestačí prediction horizont. Překvapivě když se vstup vůbec neomezí a jediná limitace je poloha vozítka, tak se MPC naprosto podělá a začne posílat vstupy kolem 500V a dokonce prolomí omezení polohy vozítka.
Ve skriptu taky podvádím a MPCčku posílám stav přímo, místo abych používal estimátor; rozšířený KF mi na první pokus nefungoval tak jsem ho prozatím vynechal. Všechno je ve skriptu MPC_Controlled.m, taky jsem udělal skript visualizeData.m na vizualizaci výsledků po simulaci, která běží plynuleji než simulace která běží ve for smyčce se soustavou.

[BystrickyK]

Doladil jsem vahy a uz swing-up zvlada i s +-12V a omezenim Xc na +-0.5. Priznam se vsak ze mi to pripada jako magie, musim se vice podivat na teorii. Taky mi přijde blbé že kontroler rozeznává mezi polohou pi a -pi -> když se zadá pi jako požadovaná poloha, tak umožňuje swing-up pouze counter-clockwise, přestože by clockwise z určité polohy bylo mnohem jednodušší.

Dalo by se to vyřešit custom váhovou funkcí?

EDIT1: Asi jsem byl mimo, už jsem to částečně vyřešil změnou definice výstupu úhlu jako mod(X(2), 2*pi), swing-up už je pěkný, ale zase se rozbil swing-down. Z nějakého důvodu MPC předpovídá X(2) trajektorii až někam do -30, asi je třeba předefinovat rovnici pro X(2) ať je tam modulo zahrnuto
**EDIT2: Pro definici výstupů [ X(1), sawtooth( X(2), 1/2) ] už to funguje jak jsem chtěl, byl asi problém s nespojitostí modulo funkce. Nejdřív jsem to zkoušel s cos( X(2) ), ale ta se moc pomalu mění v úvratích a kyvadlo pak mělo problém s ustálením. Trojuhelníková vlna funguje docela dobře, optimizer si už nějak s nespojitostí derivace poradil.

Edit: Přidal jsem vizualizaci predikce kontroleru v každém vzorku dt. Docela mi to pomohlo pro ladění a pochopení fungování.

[BystrickyK]

Se vzorkovací periodou 0.05s, PH 20 a CH 5. Výpočty však trvaly něco kolem 0.2s .
Výstupní fce [ X(1); X(2) ]

To samé, ale výstupní fce [ X(1); mod(X(2), 2*pi) ]. Při swing-downu dostane kyvadlo křeče, výpočty trvají dlouho a hodnoty predikce alpha jsou mimo možný výstupní interval. Ale swing-up je pěkný.

Předpověď na počátku swing-down manévru:

Zvládne swing-up i s výstupem +-5V, dokonce jsem zkrátil PH na 8 a CH na 5 -> jde tady vidět že občas se splete a snaží se ustabilizovat některé neustabilizovatelné kyvy, když vidí jen 0.4s dopředu. Zase s použitím modulo výstupní fce:

Swing-up a swing-down s +-5V, PH8 CH5 dt0.05, výstupní fce [ X(1), sawtooth( X(2), 1/2 ) ]:

+-10V, PH8 CH5 dt0.1, výstupní fce [ X(1), sawtooth( X(2), 1/2 ) ]

Průběh výpočetních časů MPC při omezení iterací na 20:

ale dá se ukrojit omezením iterací na ~12, MPC to pořád zvládá, toto je průběh pro předchozí
animaci

Ještě poslední simulace s +-20V