ПИД-регулятор — это весело / Хабр

ПИД-регулятор — один из самых распространённых механизмов обратной связи. Он есть буквально везде: нажимаете ручку газа на электросамокате - ПИД, когда выставляете температуру на обогревателе - ПИД, и даже когда сливаете воду из бачка…

Начнём с как раз с этого примера

Здесь уровень воды должен быть «полным». Если есть ненулевая ошибка

$e(t) = h_{\text{full}} - h_{\text{water}}(t),$

то клапан приоткрывается и подаёт управляющий сигнал , пропорциональный этой ошибке:

Разумеется, в реальной жизни не может быть отрицательным и всегда ограничен максимальным расходом воды, который вообще доступен в системе.

Уже здесь видно главное: мы измеряем отклонение от цели и воздействуем на систему так, чтобы это отклонение уменьшить.

История с обогревателем

Теперь возьмём пример поинтереснее - обогреватель.

Проблема тут в том, что тепло не доходит до термометра мгновенно. Воздух имеет конечную теплопроводность, нагревательный элемент тоже не магический, и вся система реагирует с запаздыванием.

Такие процессы описываются уравнением теплопроводности - параболическим уравнением в частных производных:

$\frac{\partial w}{\partial t} = \Delta w$

Попробуем решить его для двумерного материала, который нагревается в одной точке :

$\frac{\partial w}{\partial t} = \Delta w + f(x,y,t),$

где — функция нагрева.

sol = NDSolveValue[
  {
    D[w[x, y, t], t] == Laplacian[w[x, y, t], {x, y}] + 
      If[(x + 2)^2 + y^2 < 0.1 && t > 0.0, 100.0, 0],
    w[x, y, 0] == 0
  },
  w,
  {x, y} ∈ Rectangle[{-2, -1}, {2, 1}],
  {t, 0, 10}
];

Визуализируем распределение температуры во времени:

Table[
  DensityPlot[
    sol[x, y, t],
    {x, -2, 2}, {y, -1, 1},
    Epilog -> {
      Text["🔥", {-2, 0}],
      Text[Style["A", White, FontSize -> 14], {1.6, 0.}, {0, 0}]
    },
    PlotLabel -> ("t = " <> ToString[t]),
    ColorFunctionScaling -> False,
    ImageSize -> 200
  ],
  {t, {0.2, 1, 10}}
] // Row

Теперь посмотрим, как ведёт себя температура в фиксированной точке A:

Plot[
  sol[1.6, 0., t],
  {t, 0, 10},
  AxesLabel -> {"time", "Temperature"},
  PlotLabel -> "A",
  PlotRange -> Full
]

Вот это и есть реакция нашей системы. Она запаздывает… ~~или нет~~

В отличие от электромагнитных или упругих волн, уравнение теплопроводности описывает диффузионные волны. Их скорость зависит от времени и постепенно падает. Можно даже сказать, что это предельно “задавленная” потерями версия обычной волны.

И вот здесь начинается интересное: контроллер нельзя просто так «вклеить» в это уравнение. Получается петля обратной связи, с которой NDSolve и другие встроенные решатели работают уже не так охотно. Поэтому приходится переходить к дискретизации.

Метод FDTD

Один из естественных путей - дискретизировать уравнение и по времени, и по пространству.

Обычно проще работать с уравнениями первого порядка, но для начала давайте просто напишем самый базовый численный шаг для двумерного уравнения теплопроводности. Оператор Лапласа в дискретном виде довольно известен:

solveHeat[w_, f_, dt_: 0.0025, dx_: 0.1] := Table[
  If[i > 1 && i < 50 && j > 1 && j < 50,
    w[[i, j]] + dt (
      w[[i - 1, j]] + w[[i, j - 1]] - 4 w[[i, j]] + 
      w[[i, j + 1]] + w[[i + 1, j]]
    )/dx^2 + dt f[i, j],
    w[[i, j]]
  ],
  {i, 50}, {j, 50}
];

Поставим источник тепла в тот же угол:

Module[{w = Table[0., {50}, {50}]},
  FixedPoint[
    solveHeat[
      #,
      Function[{i, j}, If[Max[Abs[{i, j} - {25, 2}]] < 1, 1.0, 0.0]]
    ] &,
    w,
    20
  ] // ListDensityPlot
]

А теперь посмотрим на эволюцию по времени, если мы сначала включим, а потом через 100 отсчётов выключим нагреватель:

Module[{w = Table[0., {50}, {50}]},
  Table[
    With[{heater = If[steps > 100 && steps < 300, 100.0, 0.0]},
      w = solveHeat[
        w,
        Function[{i, j}, If[Max[Abs[{i, j} - {25, 2}]] < 1, heater, 0.0]]
      ];
      {{steps, w[[25, 25]]}, {steps, Clip[heater, {0, 0.002}]}}
    ],
    {steps, 1, 1000}
  ]
];
ListLinePlot[
  % // Transpose,
  Filling -> 0,
  PlotLegends -> {"Temperature (A)", "Heater"}
]

Для устойчивости здесь действует условие CFL (Критерий Куранта-Фридрихса-Леви):

$\delta t / \delta x^2 \le 0.25$

Пропорциональный регулятор P

А теперь подключим контроллер.

Module[{w = Table[0., {50}, {50}]},
  Table[
    With[{error = (0.0022 - w[[25, 25]])},
      {heater = 100000.0 Clip[error, {0, Infinity}]},
      w = solveHeat[
        w,
        Function[{i, j}, If[Max[Abs[{i, j} - {25, 2}]] < 1, heater, 0.0]]
      ];
      {{steps, w[[25, 25]]}, {steps, heater/30000.0}}
    ],
    {steps, 1, 3000}
  ]
];
ListLinePlot[
  % // Transpose,
  Filling -> 0,
  PlotLegends -> {"Temperature (A)", "Heater"},
  Epilog -> {Red, Dashed, Line[{{0, 0.0022}, {5000, 0.0022}}]},
  PlotRange -> {Automatic, {0, 0.0022}},
  PlotLabel -> "1x heater power"
]

И тут видно неприятную вещь: возникает систематическая ошибка. Температура не доходит до целевого значения.

Попробуем раскачать систему сильнее:

Module[{w = Table[0., {50}, {50}]},
  Table[
    With[{error = (0.0022 - w[[25, 25]])},
      {heater = 100000.0 Clip[error, {0, Infinity}]},
      w = solveHeat[
        w,
        Function[{i, j}, If[Max[Abs[{i, j} - {25, 2}]] < 1, heater, 0.0]]
      ];
      {{steps, w[[25, 25]]}, {steps, heater/30000.0}}
    ],
    {steps, 1, 3000}
  ]
];
ListLinePlot[
  % // Transpose,
  Filling -> 0,
  PlotLegends -> {"Temperature (A)", "Heater"},
  Epilog -> {Red, Dashed, Line[{{0, 0.0022}, {5000, 0.0022}}]},
  PlotRange -> {Automatic, {0, 3 0.0022}},
  PlotLabel -> "10x heater power"
]

Смотрится уже бодрее, но теперь система начинает жить собственной жизнью: появляются колебания, а систематическая ошибка всё ещё никуда не делась.

Интегральная часть (I)

До сих пор мы игрались только с пропорциональной частью PID. Но вторая по важности штука - это интеграл, который накапливает ошибку со временем.

Ровно так же, как мы до этого делали интегрирование уравнения теплопроводности, можно копить и ошибку.

Module[{w = Table[0., {50}, {50}], accError = 0.0},
  Table[
    With[{error = (0.0022 - w[[25, 25]])},
      {
        heater = 100000.0 Clip[error + 0.001 accError, {0, Infinity}]
      },
      accError += error;

      w = solveHeat[
        w,
        Function[{i, j}, If[Max[Abs[{i, j} - {25, 2}]] < 1, heater, 0.0]]
      ];
      {{steps, w[[25, 25]]}, {steps, heater/30000.0}}
    ],
    {steps, 1, 3000}
  ]
];
ListLinePlot[
  % // Transpose,
  Filling -> 0,
  PlotLegends -> {"Temperature (A)", "Heater"},
  Epilog -> {Red, Dashed, Line[{{0, 0.0022}, {5000, 0.0022}}]},
  PlotRange -> {Automatic, {0, 3 0.0022}},
  PlotLabel -> "10x heater power + I"
]

Ура! Базовая линия медленно подползает к цели.

Уменьшим общую мощность нагревателя и посмотрим ещё раз:

Module[{w = Table[0., {50}, {50}], accError = 0.0},
  Table[
    With[{error = (0.0022 - w[[25, 25]])},
      {
        heater = 20000.0 Clip[error + 0.001 accError, {0, Infinity}]
      },
      accError += error;

      w = solveHeat[
        w,
        Function[{i, j}, If[Max[Abs[{i, j} - {25, 2}]] < 1, heater, 0.0]]
      ];
      {{steps, w[[25, 25]]}, {steps, heater/30000.0}}
    ],
    {steps, 1, 3000}
  ]
];
ListLinePlot[
  % // Transpose,
  Filling -> 0,
  PlotLegends -> {"Temperature (A)", "Heater"},
  Epilog -> {Red, Dashed, Line[{{0, 0.0022}, {5000, 0.0022}}]},
  PlotRange -> {Automatic, {0, 3 0.0022}},
  PlotLabel -> "2x heater power + I"
]

Уже хорошо.

Но можно ли сделать ещё лучше?

Дифференциальная часть (D)

Можно реагировать ещё и на то, на сколько быстро меняется ошибка. Это позволяет заранее подтормаживать систему и гасить нарастающие колебания.

Module[{g = 0, w = Table[0., {50}, {50}], accError = 0.0, prevError = 0.0022},
  Table[
    With[{error = (0.0022 - w[[25, 25]])},
      {
        heater = 100000.0 Clip[
          0.9 error + 0.001 accError + 100 (error - prevError),
          {0, Infinity}
        ]
      },
      accError += error;
      prevError = error;

      w = solveHeat[
        w,
        Function[{i, j}, If[Max[Abs[{i, j} - {25, 2}]] < 1, heater, 0.0]]
      ];
      w = solveHeat[
        w,
        Function[{i, j}, If[Max[Abs[{i, j} - {25, 2}]] < 1, heater, 0.0]]
      ];

      {{2 steps, w[[25, 25]]}, {2 steps, heater/30000.0}}
    ],
    {steps, 1, 3000/2}
  ]
];
ListLinePlot[
  % // Transpose,
  Filling -> 0,
  PlotLegends -> {"Temperature (A)", "Heater"},
  Epilog -> {Red, Dashed, Line[{{0, 0.0022}, {5000, 0.0022}}]},
  PlotRange -> {Automatic, {0, 3 0.0022}},
  PlotLabel -> "10x heater power + I + D"
]

Мы заметно уменьшили время нарастания, но за это пришлось заплатить небольшими колебаниями из-за слишком большого коэффициента при D.

Подбор PID-коэффициентов - это вообще отдельное ремесло и наука, и здесь мы этого вряд ли раскроем…

Для сравнения — вариант без D:

В итоге получился отличный виртуальный полигон: FDTD-модель обогревателя очень наглядно показывает, зачем в PID нужны P, I и D.

Вот такой небольшой аттракцион, и мы только только добрались до определения ПИД.

Формальное определение ПИД

Ну что ж, теперь уже можно честно записать ПИД в общем виде:

$u(t) = K_p e(t) + K_i \int_0^t e(\tau)\, d\tau + K_d \frac{de(t)}{dt}$

Где:

— управляющий сигнал;
— ошибка, то есть разница между желаемым и измеренным значением;
, , — коэффициенты настройки.

Регулятор положения грузика

Теперь давайте перейдём к примеру по-проще и по-чище: стабилизации положения грузика в заданной точке.

Чтобы решить задачу аналитически (и позже численно), удобно продифференцировать выражение для ПИД и избавиться от интеграла:

$u(t) = K_p e(t) + K_i \int_0^t e(\tau)\, d\tau + K_d e'(t)$

Дифференцируем:

Но при этом надо не забыть начальное условие:

Жизнь становится проще, когда в уравнении остаются только производные.

Физическая система

Представим себе грузик , который движется по направляющей, а толкает его маленький ракетный двигатель, управляемый ПИД-регулятором.

Система уравнений тогда будет такой:

system = {
  x''[t] == u[t]/m,
  x'[0] == 0,
  x[0] == 0
};

Проверка

Прежде чем прикручивать PID, проверим, что система вообще ведёт себя как должна.

Подадим постоянную силу:

DSolve[system /. {u[t_] :> 1}, x, t]

Получим:

$x(t) = \frac{t^2}{2m}$

Ровно та самая парабола из школьной механики.

Function[{t}, t^2/(2 m)] /. {m -> 1.0};
Plot[
  %[t],
  {t, 0, 10},
  AxesLabel -> {"t", "x"}
]

Подключаем П-контроллер

Теперь свяжем управляющий сигнал с положением через ошибку:

Схема выглядит так

А система уравнений следующая

system = {
  u[t] == Kp e[t],
  x''[t] == u[t]/m + 1,
  x'[0] == 0,
  x[0] == 0,

  e[t] == (a - x[t])
};

Решим её аналитически:

sol = DSolve[system, {u, x, e}, t] // Flatten;
sol = Simplify[sol, Assumptions -> {Element[a, Reals], m > 0, Kp > 0}];

Построим графики для регулируемого и почти нерегулируемого случая:

x /. sol /. {m -> 1.0, a -> 1.0} /. {{Kp -> 1}, {Kp -> 0.01}};
Plot[
  #[t] & /@ % // Evaluate,
  {t, 0, 10},
  Epilog -> {Dashed, Red, Line[{{0, 1.0}, {10, 1.0}}]},
  PlotLegends -> Placed[{"Regulated", "Unregulated"}, {0.15, 0.5}]
]

И сразу видно главное: одного P здесь маловато. Система перелетает цель и начинает осциллировать.

Именно в этот момент в игру и входят I и D.

Расширяем до ПИД

Аналитически это уже решать не так удобно, поэтому перейдём к численным методам NDSolve

system = {
  (u')[t] == e[t] Ki + Kp (e')[t] + Kd (e'')[t],
  u[0] == Kp (a - x[0]),
  x''[t] == u[t]/m + 1.0,
  x'[0] == 0.,
  x[0] == 0.,

  e[t] == (a - x[t])
};

ClearAll[sol];
sol[p_, i_, d_] := Flatten @ NDSolve[
  system /. {
    m -> 5.0,
    a -> 2.5,
    Kp -> p,
    Ki -> i p,
    Kd -> d p
  },
  {u, x, e},
  {t, 0, 200}
];

Сделаем интерактивно

Manipulate[
  With[
    {
      eqs = {Clip[x[t], {-0.5, 4}] + 0.05, Clip[0.01 u[t], {-0.5, 4}]} /. sol[10.0, i, d]
    },
    Plot[
      eqs,
      {t, 0, 50},
      Frame -> True,
      PlotLegends -> {"x[t]", "u[t]"},
      FrameLabel -> {"t (time)", "value"},
      Epilog -> {Dashed, Red, Line[{{0, 2.5}, {50, 2.5}}]},
      PlotRange -> {{-1, 50}, {-1, 4.5}}
    ]
  ],
  {{i, 0, "I"}, 0, 0.1, 0.025},
  {{d, 0, "D"}, 0, 1.0, 0.25},
  ContinuousAction -> True,
  Appearance -> None
]

Картина получается ровно такая, какую и хочется увидеть:

D гасит колебания,
I убирает систематическую ошибку.

Ещё более интерактивная версия

Чтобы обновлять систему совсем вживую, например, мышкой, снова переходим к итерационной схеме, так как NDSolve тут не поможет

Исходный PID:

$u(t) = K_p e(t) + K_i \int_0^t e(\tau)\,d\tau + K_d e'(t)$

Поднимаем интеграл в производную:

Введём вспомогательную переменную:

Тогда:

Дальше дискретизируем правилами замены (ничего не делаем вручную):

{
  h[t] == e'[t],
  u'[t] == Kp h[t] + Ki e[t] + Kd h'[t]
} /. {
  (e_)'[t] :> (e[n] - e[n - 1])/\[Delta]t,
  e_[t] :> e[n]
};

dsol = Solve[%, {h[n], u[n]}] // Flatten;

Определим конструктор дискретного PID-контроллера:

createPID := Module[{uState, hState, eState},
  With[
    {
      rules1 = dsol,
      rules2 = {
        e[n - 1] -> Indexed[eState, 2],
        e[n] -> Indexed[eState, 1],

        h[n - 1] -> Indexed[hState, 2],
        h[n] -> Indexed[hState, 1],

        u[n - 1] -> Indexed[uState, 2],
        u[n] -> Indexed[uState, 1],

        \[Delta]t -> #5,
        Kd -> #4,
        Kp -> #2,
        Ki -> #3
      }
    },
    Hold@Module[{},
      uState = {0, 0};
      hState = {0, 0};
      eState = {0, 0};

      (
        eState[[2]] = eState[[1]];
        eState[[1]] = #1;

        hState[[2]] = hState[[1]];
        hState[[1]] = h[n];

        uState[[2]] = uState[[1]];
        uState[[1]] = u[n]
      ) &
    ] /. rules1 /. rules2
  ]
] // ReleaseHold;

Здесь мы использовали Indexed вместо [[]] для того, чтобы WL не ругался на то, что наши символы пока ещё не являются массивами.

Создадим экземпляр:

pid = createPID;

Проверим:

pid[0.1, 0.1, 0.1, 0.1, 0.1]

В принципе, каждый шаг интегрирования здесь можно было бы свернуть до простого умножения на матрицу - коэффициенты ведь постоянны.

Теперь напишем тестовую систему с грузиком и ракетным двигателем:

{
  x''[t] == u[t]/m,
  x'[0] == 0,
  x[0] == 0,

  e[t] == (a - x[t])
}

Переведём её в дискретную форму:

{
  v'[t] == u[t]/m,
  x'[t] == v[t],
  e[t] == (a - x[t])
} /. {
  v'[t] -> (v[n] - v[n - 1])/\[Delta]t,
  v[t] -> v[n],
  x'[t] -> (x[n] - x[n - 1])/\[Delta]t,
  x[t] -> x[n],
  e[t] -> e[n]
} // Simplify;

Solve[%, {v[n], x[n], e[n]}] // Flatten // TableForm

А теперь вручную соберём совсем простую модель:

testSystem[initialPos_, mass_] := Module[
  {stateV = {0., 0.}, stateX = {1., 1.} initialPos},
  Function[{u, target, dt},
    stateV[[2]] = stateV[[1]];
    stateV[[1]] = If[
      Abs[stateX[[1]]] > 1,
      -stateV[[1]],
      stateV[[2]] + (dt/mass) u
    ];
    stateX[[2]] = stateX[[1]];
    stateX[[1]] = Clip[stateX[[2]] + dt stateV[[1]], {-0.9, 0.9}, {-0.88, 0.88}];

    {stateX[[1]], target - stateX[[1]]}
  ]
];

Не забудем о стенках: пусть блок отскакивает и не улетает в космос - для этого Clip.

Собираем всё вместе

Теперь цель уже не фиксированная, а управляется движением мыши в реальном времени.

Схема примерно такая:

Код (сработает только в WLJS Notebook, но не в Mathematica):

pid = createPID;
sys = testSystem[0., 1.0];

Module[
  {
    value = 0.0,
    error = 0.0,
    target = 0.5,
    control = 0.0,
    gravity = 0.25,
    history = Table[{i, 0.}, {i, 300}],
    p = <|"P" -> 1.0, "I" -> 0.0, "D" -> 0.0|>,
    handler
  },
  handler = Function[Null,
    {value, error} = sys[
      Clip[pid[error, p["P"], p["I"], p["D"], 0.1], {-10, 10}] - gravity,
      target,
      0.1
    ];
    history[[1, 2]] = error;
    history = Transpose[{history[[All, 1]], RotateLeft[history[[All, 2]]]}];
  ];

  Row[{
    EventHandler[
      Graphics[
        {
          Red, Line[{{-1, target}, {1, target}}],
          Blue, Line[{{-1, value}, {1, value}}],
          EventHandler[AnimationFrameListener[value], handler]
        },
        PlotRange -> {{-1, 1}, {-1, 1}},
        ImageSize -> {100, 300}
      ],
      {
        "mousemove" -> Function[xy, target = Clip[xy[[2]], {-0.8, 0.8}]]
      }
    ],
    Graphics[
      Line[history],
      PlotRange -> {{1, 300}, {-1, 1}},
      ImageSize -> {200, 300},
      Axes -> True,
      AxesOrigin -> {0, 0.00001},
      Ticks -> {False, True},
      Frame -> True,
      PlotLabel -> "Error"
    ],
    EventHandler[
      InputGroup[<|
        "P" -> InputRange[0, 2.0, 0.01, p["P"], "Label" -> "P"],
        "I" -> InputRange[0, 2.0, 0.01, p["I"], "Label" -> "I"],
        "D" -> InputRange[0, 2.0, 0.01, p["D"], "Label" -> "D"]
      |>],
      Function[data, p = data]
    ]
  }]
]

Если поиграться руками, очень быстро становится видно: D нужен, чтобы успокаивать колебания, а I — чтобы компенсировать постоянные смещения, вроде той же «гравитации» в нашей игрушечной модели.

Самобалансирующийся робот

Ну а теперь - к сложному!

Прежде чем собирать целого робота, давайте дойдём до него через несколько более простых систем.

Второй закон Ньютона для вращения

Начнём с маятника

Для вращательного движения имеем:

$\hat{I}\,\dot{\vec{L}} = [\vec{l} \times \vec{g}]\,m$

Если оставить только одну координату, получим:

$\ddot{\theta} = (g/l)\sin(\theta)$

А если перейти в ускоряющуюся систему отсчёта, появится фиктивный момент силы:

$\ddot{\theta} = (g/l)\sin(\theta) + (1/l)\cos(\theta)\,a'_x$

Если добавить трение, получится:

$\ddot{\theta} = (g/l)\sin(\theta) + (1/l)\cos(\theta)\,a'_x - d\dot{\theta}$

Соберём численную модель маятника:

createPendulum[iθ_, g_, l_, d_: 0.1] := Module[
  {ω = {0., 0.}, θ = {1., 1.} iθ},
  Function[{ax, dt},
    ω[[2]] = ω[[1]];
    ω[[1]] =
      (1.0 - d) ω[[2]] +
      (dt g/l) Sin[θ[[1]]] +
      (dt ax/l) Cos[θ[[1]]];
    θ[[2]] = θ[[1]];
    θ[[1]] = θ[[2]] + dt ω[[1]];

    θ[[1]]
  ]
];

Посмотрим, насколько это вообще нестабильная штука:

pen = createPendulum[0.01, 9.9, 1.0, 0.003];
Module[{pos = {0, 1.0}, t = 0.0},
  Animate[
    t = pen[0.0, 0.1];
    pos = {Sin[t], Cos[t]};
    Graphics[
      {Line[{{0, 0}, pos}], Disk[pos, 0.05]},
      PlotRange -> {{-1, 1}, {-1, 1}},
      AxesOrigin -> {0.00001, 0},
      Axes -> True,
      Ticks -> None
    ],
    {dummy, 0, 0.5, 0.01},
    Appearance -> None
  ]
]

Колёса для робота

Что делать с платформой? PID, скорее всего, будет управлять либо напряжением, либо током на моторе.

Для электрических моторов, особенно DC, напряжение обычно определяет скорость, а ток - вращательный момент.

Пойдём по пути напряжения. Вращение колёс будем переводить напрямую в координату (сферический конь):

createPlatform[initialPos_, mass_] := Module[
  {stateV = {0., 0.}, stateX = {1., 1.} initialPos},
  Function[{u, dt},
    stateV[[2]] = stateV[[1]];
    stateV[[1]] = If[Abs[Abs[stateX[[1]]] - 2.0] < 0.01, 0, u];
    stateX[[2]] = stateX[[1]];
    stateX[[1]] = Clip[stateX[[2]] + dt stateV[[1]], {-2, 2}];

    {stateX[[1]], (stateV[[1]] - stateV[[2]])/dt}
  ]
];

Мы сделали также простое ограничение: если платформа упёрлась слишком далеко, моторы больше не тянут.

Проверим, как это всё выглядит без регулятора:

Module[
  {
    platform = createPlatform[0.0, 3.0],
    pen = createPendulum[0.00, 9.9, 1.0, 0.003],
    x = .0,
    ac = .0,
    voltage = .0,
    pos = {0., 0.},
    angle = 0.0,
    handler
  },

  handler = Function[Null,
    {x, ac} = platform[voltage, 0.01];
    angle = pen[-ac, 0.01];
    pos = {Sin[angle], Cos[angle]};
  ];

  EventHandler[
    Graphics[
      {
        Translate[
          {
            {Brown, Disk[{0, -0.8 + 1.0}, 0.1]},
            Rectangle[{-0.4, -1.0 + 0.2 + 1.0}, {0.4, -1.0 + 0.4 + 1.0}],
            Red, Line[{{0, 0.4}, pos}], Disk[pos, 0.05]
          },
          {x, -0.1 - 1.0}
        ],
        Line[{{-2.0, -1.0}, {2, -1.0}}],
        EventHandler[AnimationFrameListener[x], handler]
      },
      PlotRange -> {{-2, 2}, {-1, 1}},
      ImageSize -> {400, 200}
    ],
    {
      "mousemove" -> Function[xy, voltage = 2.0 xy[[1]]]
    }
  ]
]

Мышкой это стабилизировать всё еще можно, если постараться.

Добавляем PID

Теперь добавим PID по углу маятника. Он будет управлять напряжением моторов так, чтобы угол стремился к нулю

Но чтобы ещё и двигать робота к нужной позиции, добавим смещение по положению:

Соберём всё вместе:

Module[{p = <|"P" -> 11.71, "I" -> 36.56, "D" -> 0|>},
  Module[
    {
      platform = createPlatform[0.0, 1.0],
      pid = createPID,
      pid2 = createPID,
      pen = createPendulum[0.01, 9., 1.0, 0.03],
      x = .0,
      ac = .0,

      target = 0.0,
      voltage = .0,
      pos = {0., 0.},
      angle = 0.0,
      history = Table[{i, 0.}, {i, 300}],
      handler
    },

    handler = Function[Null,
      voltage = Clip[pid[angle, p["P"], p["I"], p["D"], 2 0.02], {-15, 15}];
      voltage = Clip[pid[angle, p["P"], p["I"], p["D"], 2 0.02], {-15, 15}];
      {x, ac} = platform[voltage + 2.0 (x - target), 2 0.02];
      angle = pen[-ac, 2 0.02];
      {x, ac} = platform[voltage + 2.0 (x - target), 2 0.02];
      angle = pen[-ac, 2 0.02];
      pos = {Sin[angle], Cos[angle]};
      history[[1, 2]] = voltage;
      history = Transpose[{history[[All, 1]], RotateLeft[history[[All, 2]]]}];
    ];

    Row[{
      EventHandler[
        Graphics[
          {
            Translate[
              {
                {Brown, Disk[{0, -0.8 + 1.0}, 0.1]},
                Rectangle[{-0.4, -1.0 + 0.2 + 1.0}, {0.4, -1.0 + 0.4 + 1.0}],
                Red, Line[{{0, 0.4}, pos}], Disk[pos, 0.05]
              },
              {x, -0.1 - 1.0}
            ],
            Line[{{-2.0, -1.0}, {2, -1.0}}],
            EventHandler[AnimationFrameListener[pos], handler],
            Green, Arrow[{{target, -1.2}, {target, -1.0}}]
          },
          PlotRange -> {{-2, 2}, {-1.2, 0.8}},
          ImageSize -> {400, 200}
        ],
        {
          "mousemove" -> Function[xy, target = 0.6 target + 0.4 xy[[1]]]
        }
      ],

      Graphics[
        Line[history],
        PlotRange -> {{1, 300}, {-2, 2}},
        ImageSize -> {200, 300},
        Axes -> True,
        AxesOrigin -> {0, 0.00001},
        Ticks -> {False, True},
        Frame -> True,
        PlotLabel -> "Voltage"
      ],

      Column[{
        Button[
          "Reset",
          platform = createPlatform[0.0, 1.0];
          pid = createPID;
          pen = createPendulum[0.01, 9., 1.0, 0.03];
          ac = .0;
          voltage = .0;
          angle = 0.0;
        ],
        EventHandler[
          InputGroup[<|
            "P" -> InputRange[0, 40.0, 0.01, p["P"], "Label" -> "P"],
            "I" -> InputRange[0, 40.0, 0.01, p["I"], "Label" -> "I"],
            "D" -> InputRange[0, 10.0, 0.01, p["D"], "Label" -> "D"]
          |>],
          Function[data, p = data]
        ]
      }]
    }]
  ]
]

И вот тут уже видно особенно хорошо: в этой системе критичны прежде всего P и I.

Система сильно нелинейная: вращение маятника связано с линейным движением платформы через моторы, поэтому влияние I, D здесь может показаться неочевидным. Это уже совсем не тот уютный обогреватель и не тот блок на рельсе, который мы прогали в начале.

Вместо вывода

За самыми обычными вещами — обогревателем, мотором, ручками самоката - часто стоят удивительно красивые идеи.

PID - это одна из них.

Ссылки

Про ПИД без математики
Пост на буржуйском
Среда песочница, где были построены все примеры