With a given linear drive system

Needs["VariationalMethods"]
params = {jb -> 870 10^-5 + 75 10^-6, p -> 2, mw -> 0.75, mb -> 10,
r1 -> 0.000001, r2 -> 0.000001,
jm -> 7800 0.002 \[Pi] (0.165/2)^4 1/2, r3 -> 0.165/2, k1 -> 1,
k2 -> 1};
displacement = 0;

ke = 1/2 mw (p/(2 \[Pi]) \[Alpha]'[t])^2 + 1/2 jm \[Alpha]'[t]^2  +
1/2 mb (p/(2 \[Pi]) \[Alpha]'[t])^2;
kp = mb g h1 + mb g (h1 + h2);
l = ke - kp;
eqns = EulerEquations[l, {\[Alpha][t]}, t] // FullSimplify;
deqns = {First@eqns[[1]] == -r1 \[Alpha]'[t] - f[t] + 4.3 u[t]};
MatrixForm[deqns]

symmodelqr =
StateSpaceModel[
deqns, {{\[Alpha][t], 0}, {\[Alpha]'[t], 0}}, {u[t],
f[t]}, {\[Alpha]'[t] 2 \[Pi]/60, f[t]}, t] // FullSimplify;
model = symmodelqr /. params


Where $$x(t) = \frac{\alpha(t)}{2\pi}$$, $$u(t)$$ is an input between 1-10 to control a servo drive and $$f(t)$$ is a force disturbance.

I am attempting to design a linear quadratic integrator force controller,

In the same form as above.

I first designed a lqr controller on the system...a disturbance force will be measured, and with a given motor input u(t) the system should attempt to slow down the linear drive (or back off entirely), to reduce the forces, which appears to work well.

init = {0, 0};
times = RandomInteger[{10, 60}, 3];
disturb = (*{0,Table[1 UnitBox[(t-times[[i]])/
25]\[ExponentialE]^(-1(t-times[[i]])^2),{i,1,Length@
times}]};*)
{0, 1 UnitBox[(t - 15)/25] - 1 UnitBox[(t - 40)/25]};
q = DiagonalMatrix[{1, 10}];
r = 1 {{1}};
lineargains =
Join[Last@CoefficientArrays[LQRegulatorGains[{model, 1}, {q, r}]] //
Normal, {ConstantArray[0, 2]}];
First@lineargains
controlmodel = SystemsModelStateFeedbackConnect[model, lineargains];
StateResponse[{controlmodel, init}, disturb, {t, 60}];
Plot[{alphadot, force, disturb[[2]] }, {t, 0, 60},
PlotLegends -> "Expressions", PlotRange -> All, FrameStyle -> Black,
Frame -> True]
(*{1., 5.47913}*)


I now have a specific speed profile, or rather, a force profile that I want to give as a reference signal $$r$$ Where the drive should attempt to keep a quadratically growing force reference tracked, again lowering or increasing the drive speed to keep this in check.

feedback = StateSpaceModel[{{}, {}, {{}}, {{1, -1}}}];
integrator = TransferFunctionModel[1/s, s];
tracker =
SystemsModelMerge@
SystemsConnectionsModel[{feedback, integrator,
model}, {{1, 1} -> {2, 1}, {2, 1} -> {3, 1}, {3, 1} -> {1,
2}}, {{1, 1}, {3, 2}}, {{3, 1}, {3, 2}}]


My attempt at this however appears to produce nonsense.

init2 = {0, 25, 25};
times2 = RandomInteger[{10, 60}, 3];
disturb2 = {1 UnitStep[t],
0 UnitBox[(t - 10)/5] - 0 UnitBox[(t - 40)/5]};
q2 = DiagonalMatrix[{1000, 10, 100}];
r2 = 1 {{1}};
lineargains =
Join[Last@
CoefficientArrays[LQRegulatorGains[{tracker, 1}, {q2, r2}]] //
Normal, {ConstantArray[0, 3]}];
First@lineargains
controlmodel2 = SystemsModelStateFeedbackConnect[tracker, lineargains];
StateResponse[{controlmodel2, init2}, disturb2, {t, 100}];
Plot[{int, alphadot2, force2, disturb2 }, {t, 0, 100},
PlotLegends -> "Expressions", PlotRange -> All, FrameStyle -> Black,
Frame -> True, ImageSize -> Large]
(*{31.7673, 3.16228, 45.8435}*)


The Integrator, the way I understand it, should give and keep a tracked signal to the input of the SSM $$u(t)$$ To keep disturbance forces constant. However, it appears to not do this at all.

Clearly something is wrong. My suspicion is that I've either designed the wrong Statespacemodel in general for force control, or my inputs/outputs are incorrect, or that my SystemsConnectionModel is built incorrectly, However, I can't seem to get anything that makes sense. Are there better eyes out there than mine?

What is the correct way to build an LQI in mma?

(I am completely open to suggestions on how to do it differently or better, however ideally an LQI implementation would be preferred)

• What is model and jm? I can't reproduce what 'appears to work well' for you. Could you fix the question. Sep 8, 2020 at 17:08
• Oh sorry! Will do as soon as possible, i‘m unfortunately still at work Sep 8, 2020 at 17:09
• @SubaThomas I've updated the code, and retested..it should be working now...let me know if it's not...thanks for having a look! Sep 8, 2020 at 21:39
• LQRegulatorGains[{model, 1}, {q, r}] gives the LQRegulatorGains::idim3 message because q is a 2x2 matrix, but model only has 1 state variable. Sep 8, 2020 at 22:07
• @SubaThomas ...when running this code exactlyin version 12.0.0 it gives me gains and etc and the plots as in the question.... lizard-truth.com/wp-content/uploads/controller.nb here is the notebook. I will however re-copy and paste the code again. My apologies. Sep 8, 2020 at 22:20

For this model it's not possible to design a controller that will track $$\alpha '$$.

{aa, bb, cc, dd} = Normal[model];
StateSpaceModel[{ArrayFlatten[{{0, {{0, -2 π/60}}}, {0, aa}}], ArrayFlatten[{{0}, {bb}}]}]
ControllableModelQ[%]


However a controller can be designed that tracks $$\alpha$$.

StateSpaceModel[{ArrayFlatten[{{0, {{1, 0}}}, {0, aa}}], ArrayFlatten[{{0}, {bb}}]}]
ControllableModelQ[%]
`

Because the controller is using state feedback you will also need an observer. An example of how to do all this can be seen in the 'Differential input controller design' section of the documentation for a path following robot. There the reference is $$0$$ for the robot to stay on course and the disturbance is the path.

• Ahh, $\dot{\alpha}$ is the state that should be modified to track a profile for $f[t]$....$f[t]$ is the disturbance and the value that needs to be kept in check...as in, I will give a $\dot{\alpha}$ initially... the model will eventually get a disturbance $f[t]$and it should modify $\dot{\alpha}$ to ensure that f[t] stays at a specific level. I would like to develop a force controller. f[t] will eventually not just be a constant value like 0 in the example robot..., but a specific profile. going between 25-40N. I actually don't want to track $\dot{\alpha}$ I'm sorry if that wasn't clear. Sep 9, 2020 at 15:46
• Also to be clear, I can directly measure all states....$\dot{\alpha}$, $\alpha$, and $f[t]$ including torque of motors, electricity and feedforces. Sep 9, 2020 at 15:51
• Sorry, I do not follow you. A disturbance is something outside your control. There is nothing an aircraft controller can do to make the wind gust go away or behave in a certain way. What is can do is use the control inputs available to mitigate the effects of the gust, by regulating variables like the flight path, roll, etc. In the robot example the disturbance is not 0, but the line. It can be any arbitrary profile. Sep 9, 2020 at 16:14
• Perhaps my vocabulary is incorrect...When cutting through a material of any kind with any sort of tool for example when milling, there is a reactionary force directly dependant on the feedrate at which i cut into said material. This reactionary force is a combination of tool pressure, and torques. This is the 'disturbance' And this can be kept in track if I reduce the feedrate in the cutting direction. This is no different to slowing down a circular saw when I try to cut through a piece of wood too fast and the motor RPMS drop...and it stops cutting...I reduce my pushing speed. Sep 9, 2020 at 16:56
• And the motor revs back up again. This is several years old in machining a tracking of cutting force and controlling feedrate...by Siemens AMC controllers cache.industry.siemens.com/dl/files/996/75338996/att_102241/v1/… and by toolscope cuttingtools.ceratizit.com/gb/en/services/toolscope.html Perhaps the description of this reactionary feedforce as a disturbance is incorrect. My apologies if this is confusing. Sep 9, 2020 at 16:58