Теория

Устройство маятника с маховиком

На рис. 8 показан однозвенный маятник с маховиком. Маховик 1 закреплен на стержне 4. Стержень способен совершать круговые движения в вертикальной плоскости. Его ось вращения 5 установлена на неподвижном основании. В этой оси отсутствует какой-либо привод.

Рисунок 8. Основные компоненты маятника с маховиком. 1 – маховик, 2 – электродвигатель, 3 – ось маховика, 4 – стержень, 5 – ось стержня, 6 – плата управления.

Ось вращения маховика 3 закреплена на стержне и параллельна его оси вращения. Маховик приводится во вращение бесколлекторным электродвигателем 2. Электропривод, как и маховик, смонтирован на стержне. Статор двигателя жестко закреплен на стержне, ось ротора жестко соединена с осью маховика. Плата управления 6 содержит микроконтроллер и драйвер двигателя. При управлении используется информация об угле поворота стержня относительно неподвижного основания и угле маховика относительно стержня. Эти углы измеряются магнитными датчиками углового положения, которые подключены к плате управления.

Динамика маятника

Прежде чем разрабатывать систему управления, нужно понять, как ведет себя сам объект. Дифференциальные уравнения позволяют описать его поведение во времени с учетом управляющих воздействий. Наличие динамической модели дает возможность предсказуемо влиять на поведение объекта и оценивать качество системы: устойчивость, быстродействие и точность. Разработка управления без учета динамики, как правило, приводит к ручному подбору коэффициентов обратной связи и, как следствие, к малонадежным, неэффективным и неуниверсальным решениям. Особенно это критично для неустойчивых объектов, где неконтролируемое поведение может привести к повреждению оборудования. Более того, наличие динамической модели позволяет произвести исследование системы и оценить её качество: устойчивость, быстродействие, точность и т.д. Таким образом, моделирование динамики является принципиально необходимым этапом при разработке систем управления.

На рис. 9 приведена схема маятника. Стержень $OB$ в точке $O$ шарнирно соединён с неподвижной опорой. Ось шарнира перпендикулярна плоскости качания маятника. Симметричный относительно своей оси вращения маховик смонтирован на маятнике так, что его центр расположен на конце стержня в точке $B$. Маховик может поворачиваться вокруг проходящей через точку $B$ горизонтальной оси, перпендикулярной плоскости качания стержня. Эта ось параллельна оси шарнира $O$. Ось вращения маховика является продолжением оси ротора электродвигателя. Все необходимые обозначения и параметры маятника приведены в таблице 1. Система имеет две степени свободы. Перевести систему из одного состояния в другое можно только при помощи момента, вырабатываемого электродвигателем.

Управление вращением двигателя осуществляется с помощью алгоритма векторного управления [9, с. 587]. Мы будем подавать на вход алгоритма желаемое напряжение, то есть квадратурное напряжение в терминах векторного управления, которое и станет управляющим параметром системы управления маятником. Чтобы найти зависимость между желаемым моментом и напряжением, в модель маятника будет включена упрощенная модель двигателя.

Рисунок 9. Схематичное изображение обратного маятника с маховиком.

Таблица 1. Параметры маятника

Параметр	Ед. изм.	Описание
$l_1$	м	Расстояние от точки $O$ до центра масс стержня
$l_2$	м	Расстояние от точки $O$ до маховика
$m_1$	кг	Масса стержня
$m_2$	кг	Общая масса маховика и ротора двигателя
$\theta$	рад	Угол поворота стержня относительно вертикальной оси
$\phi$	рад	Угол поворота маховика относительно продольной оси стержня
$I_1$	кг·м²	Момент инерции стержня
$I_2$	кг·м²	Момент инерции маховика и ротора двигателя
$g$	м/с²	Ускорение свободного падения
$\tau$	Н·м	Момент, вырабатываемый электродвигателем
$\mu_p$	Н·м·с/рад	Коэффициент вязкого трения в шарнире $O$
$\mu_m$	Н·м·с/рад	Коэффициент вязкого трения между статором и ротором двигателя

Динамическую модель будем строить при помощи метода Лагранжа [3]. Он является одним из классических подходов к получению уравнений движения механических систем с помощью уравнений Лагранжа второго рода. В отличие от второго закона Ньютона, метод Лагранжа оперирует энергетическими величинами: кинетической и потенциальной энергией.

Суть метода заключается в следующем: для системы с $n$ обобщёнными координатами $\eta_1, \eta_2, \dots, \eta_n$ строится лагранжиан $L = T - U$, где $T$ — кинетическая энергия, а $U$ — потенциальная энергия системы. Далее для каждой координаты $\eta_i$ записывается уравнение Лагранжа второго рода:

$$\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\eta}_i}\right) - \frac{\partial L}{\partial \eta_i} = \mathcal{Q}_i$$

где $\mathcal{Q}_i$ — обобщённая сила.

Подробно изучить принципы построения динамических уравнений можно в книге [3]. Напомним общий алгоритм построения модели методом Лагранжа:

1. Выбрать обобщённые координаты — минимальный набор независимых переменных, полностью описывающих положение системы.
2. Найти обобщённые силы.
3. Вычислить кинетическую энергию $T$ как сумму энергий всех подвижных масс, включая вращательное движение.
4. Вычислить потенциальную энергию $U$, которая зависит от положения подвижных масс в поле тяготения.
5. Построить лагранжиан $L = T - U$.
6. Построить уравнения Лагранжа второго рода для получения системы дифференциальных уравнений с учетом обобщённых сил $\mathcal{Q}_i$ при наличии внешнего управления или трения.

После построения системы нелинейных дифференциальных уравнений для упрощения анализа и синтеза управления линеаризуем её в окрестности верхнего положения равновесия, затем представим в пространстве состояний и переведем в дискретную форму.

Приступим к построению динамической модели маятника в соответствии с предложенным выше алгоритмом. В качестве обобщенных координат возьмем $\theta$ и $\phi$. С их помощью мы можем полностью описать положение системы.

На систему действуют три силы: сила вязкого трения $\mu_p \dot{\theta}$ в неподвижной точке крепления стержня $O$, сила вязкого трения $\mu_m \dot{\phi}$ между статором и ротором двигателя и крутящий момент $\tau$, создаваемый электродвигателем. Таким образом вектор обобщенных сил будет выглядеть как:

$$\mathbf{Q} = \begin{bmatrix} -\mu_p \dot{\theta} \\ \tau - \mu_m \dot{\phi} \end{bmatrix}.$$

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

$$T = \sum_{i=1}^{2} T_i$$

где

$$T_i = \frac{1}{2} m_i \dot{x}_i^2 + \frac{1}{2} m_i \dot{y}_i^2 + \frac{1}{2} I_i \dot{\alpha}_i^2$$

— формула кинетической энергии в общем виде, $m_i$ — масса $i$-го звена, $I_i$ — момент инерции $i$-го звена, $x_i$ и $y_i$ — положение центра масс $i$-го звена вдоль осей $X$ и $Y$, а $\alpha_i$ — абсолютный угол поворота $i$-го звена в неподвижной системе координат.

В нашем случае $x_i$, $y_i$, $\alpha_i$ равны:

$$\begin{aligned} x_1 &= l_1 \sin \theta, & y_1 &= l_1 \cos \theta, & \alpha_1 &= \theta. \end{aligned} \tag{1}$$ $$\begin{aligned} x_2 &= l_2 \sin \theta, & y_2 &= l_2 \cos \theta, & \alpha_2 &= \theta + \phi. \end{aligned} \tag{2}$$

Здесь $\alpha_2 = \theta + \phi$, так как $\phi$ — угол поворота маховика относительно стержня, а $\theta$ — угол поворота стержня в неподвижной системе координат. Чтобы найти абсолютный угол маховика относительно неподвижной системы координат, необходимо сложить оба этих угла. Их производные по времени примут следующий вид:

$$\begin{aligned} \dot{x}_1 &= l_1 \dot{\theta} \cos \theta, & \dot{y}_1 &= -l_1 \dot{\theta} \sin \theta, & \dot{\alpha}_1 &= \dot{\theta}. \end{aligned} \tag{3}$$ $$\begin{aligned} \dot{x}_2 &= l_2 \dot{\theta} \cos \theta, & \dot{y}_2 &= -l_2 \dot{\theta} \sin \theta, & \dot{\alpha}_2 &= \dot{\theta} + \dot{\phi}. \end{aligned} \tag{4}$$

Выражение $\dot{\alpha}_2 = \dot{\theta} + \dot{\phi}$ также может быть получено согласно теореме о сложении скоростей:

Теорема о сложении скоростей

Абсолютная скорость точки равна векторной сумме её переносной и относительной скоростей:

$$\mathbf{v}_a = \mathbf{v}_e + \mathbf{v}_r$$

где $\mathbf{v}_a$ — вектор абсолютной скорости, $\mathbf{v}_e$ — вектор переносной скорости, $\mathbf{v}_r$ — вектор относительной скорости.

В нашем случае маховик вращается относительно стержня, следовательно угловая скорость равна $\dot{\theta} + \dot{\phi}$. Более подробно о сложном движении точки в пространстве можно узнать из [3].

Построим уравнение кинетической энергии маятника, подставив в него полученные переменные $(3)$ и $(4)$:

$$T = \frac{1}{2}\Big( m_1 l_1^2 \dot{\theta}^2 \cos^2 \theta + m_1 l_1^2 \dot{\theta}^2 \sin^2 \theta + I_1 \dot{\theta}^2 + m_2 l_2^2 \dot{\theta}^2 \cos^2 \theta + m_2 l_2^2 \dot{\theta}^2 \sin^2 \theta + I_2 (\dot{\theta} + \dot{\phi})^2 \Big).$$

Упростив выражение, получим:

$$T = \frac{1}{2}(m_1 l_1^2 + m_2 l_2^2 + I_1 + I_2)\dot{\theta}^2 + I_2 \dot{\theta}\dot{\phi} + \frac{1}{2} I_2 \dot{\phi}^2. \tag{5}$$

Аналогично потенциальная энергия маятника равна сумме потенциальных энергий стержня и маховика:

$$U = m_1 g y_1 + m_2 g y_2.$$

Подставим сюда выражения $(1)$ и $(2)$:

$$U = (m_1 l_1 + m_2 l_2) g \cos \theta. \tag{6}$$

Для простоты дальнейших вычислений произведем замену переменных:

$$a = m_1 l_1^2 + m_2 l_2^2 + I_1, \qquad b = (m_1 l_1 + m_2 l_2)g.$$

Тогда уравнения $(5)$ и $(6)$ примут вид:

$$T = \frac{1}{2}(a + I_2)\dot{\theta}^2 + I_2 \dot{\theta}\dot{\phi} + \frac{1}{2} I_2 \dot{\phi}^2, \qquad U = b \cos \theta.$$

Теперь построим лагранжиан:

$$L = T - U = \frac{1}{2}(a + I_2)\dot{\theta}^2 + I_2 \dot{\theta}\dot{\phi} + \frac{1}{2} I_2 \dot{\phi}^2 - b \cos \theta. \tag{7}$$

Запишем уравнения Лагранжа второго рода с учетом обобщенных координат $\theta$, $\phi$ и вектора обобщенных сил:

$$\begin{aligned} \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}}\right) - \frac{\partial L}{\partial \theta} &= -\mu_p \dot{\theta}, \\ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\phi}}\right) - \frac{\partial L}{\partial \phi} &= \tau - \mu_m \dot{\phi}. \end{aligned} \tag{8}$$

Найдем все необходимые производные лагранжиана $(7)$:

$$\frac{\partial L}{\partial \theta} = b \sin \theta, \qquad \frac{\partial L}{\partial \dot{\theta}} = (a + I_2)\dot{\theta} + I_2 \dot{\phi}, \qquad \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}}\right) = (a + I_2)\ddot{\theta} + I_2 \ddot{\phi};$$ $$\frac{\partial L}{\partial \phi} = 0, \qquad \frac{\partial L}{\partial \dot{\phi}} = I_2 \dot{\theta} + I_2 \dot{\phi}, \qquad \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\phi}}\right) = I_2 \ddot{\theta} + I_2 \ddot{\phi}.$$

Подставим эти выражения в систему $(8)$:

$$\begin{aligned} (a + I_2)\ddot{\theta} + I_2 \ddot{\phi} - b \sin \theta &= -\mu_p \dot{\theta}, \\ I_2 \ddot{\theta} + I_2 \ddot{\phi} &= \tau - \mu_m \dot{\phi}. \end{aligned}$$

Преобразуем уравнения так, чтобы $\ddot{\theta}$ и $\ddot{\phi}$ лежали в левой части:

$$\begin{aligned} \ddot{\theta} &= -\frac{\mu_p}{a}\dot{\theta} + \frac{\mu_m}{a}\dot{\phi} + \frac{b}{a}\sin \theta + \frac{1}{a}\tau, \\ \ddot{\phi} &= -\frac{\mu_m}{I_2}\dot{\phi} + \frac{\mu_p}{a}\dot{\theta} - \frac{\mu_m}{a}\dot{\phi} - \frac{b}{a}\sin \theta + \left(\frac{1}{I_2} + \frac{1}{a}\right)\tau. \end{aligned} \tag{9}$$

Приведенная система дифференциальных уравнений описывает динамику механической части обратного маятника.

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

$$\begin{aligned} V &= L_m \frac{di}{dt} + R_m i + K_e \dot{\phi}, \\ \tau &= K_t i. \end{aligned}$$

Таблица 2. Параметры электродвигателя

Параметр или переменная	Ед. измерения	Описание
$V$	В	Подаваемое на двигатель напряжение
$i$	А	Ток, проходящий через обмотки двигателя
$L_m$	Гн	Индуктивность обмотки двигателя
$R_m$	Ом	Сопротивление обмотки двигателя
$K_e$	В·с/рад	Скоростная постоянная
$K_t$	Н·м/А	Моментная постоянная

Параметры $L_m$, $R_m$, $K_e$, $K_t$ обычно указываются в документации на двигатель либо находятся экспериментально. Индуктивность обмоток, как правило, намного ниже сопротивления, поэтому переменной $L_m$ можно пренебречь. Тогда соотношение между моментом на валу и напряжением можно выразить как:

$$\tau = \frac{K_t \bigl(V - K_e \dot{\phi}\bigr)}{R_m}.$$

Подставим это выражение в систему $(9)$:

$$\begin{aligned} \ddot{\theta} &= \frac{b}{a}\sin \theta - \frac{\mu_p}{a}\dot{\theta} + \frac{R_m \mu_m + K_t K_e}{a R_m}\dot{\phi} + \frac{K_t}{a R_m}V, \\ \ddot{\phi} &= -\frac{b}{a}\sin \theta + \frac{\mu_p}{a}\dot{\theta} - \frac{a + I_2}{a I_2}\left(\mu_m + \frac{K_t K_e}{R_m}\right)\dot{\phi} + \frac{K_t(a + I_2)}{a I_2 R_m}V. \end{aligned} \tag{10}$$

Мы получили математическую модель, полностью описывающую поведение обратного маятника. Теперь для синтеза управления и анализа на устойчивость нам необходимо её линеаризовать и привести к форме в пространстве состояний.

Желаемым состоянием маятника является верхнее неустойчивое положение равновесия, когда все скорости равны нулю, то есть $\theta = 0$, $\dot{\theta} = 0$, $\dot{\phi} = 0$. При этом нас не интересует значение угла $\phi$. Линеаризуем систему $(10)$ в этой окрестности. Единственным нелинейным членом в этой системе является $\sin \theta$. Разложив его в ряд Тейлора и взяв первый член, получим $\sin \theta \approx \theta$. Тогда система $(10)$ примет вид:

$$\begin{aligned} \ddot{\theta} &= \frac{b}{a}\theta - \frac{\mu_p}{a}\dot{\theta} + \frac{R_m \mu_m + K_t K_e}{a R_m}\dot{\phi} + \frac{K_t}{a R_m}V, \\ \ddot{\phi} &= -\frac{b}{a}\theta + \frac{\mu_p}{a}\dot{\theta} - \frac{a + I_2}{a I_2}\left(\mu_m + \frac{K_t K_e}{R_m}\right)\dot{\phi} + \frac{K_t(a + I_2)}{a I_2 R_m}V. \end{aligned}$$

Представим теперь эту систему в пространстве состояний матричным уравнением:

$$\dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u}. \tag{11}$$

В качестве вектора состояния выберем $\mathbf{x} = \begin{bmatrix}\theta & \dot{\theta} & \dot{\phi}\end{bmatrix}^T$, так как нас интересуют угловое положение стержня и угловые скорости стержня и маховика. В качестве управляющего воздействия возьмем $\mathbf{u} = \begin{bmatrix}V\end{bmatrix}$. Само уравнение примет вид:

$$\begin{bmatrix} \dot{\theta} \\ \ddot{\theta} \\ \ddot{\phi} \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 \\ \frac{b}{a} & -\frac{\mu_p}{a} & \frac{R_m \mu_m + K_t K_e}{aR_m} \\ -\frac{b}{a} & \frac{\mu_p}{a} & -\frac{a+I_2}{aI_2}\left(\mu_m + \frac{K_t K_e}{R_m}\right) \end{bmatrix} \begin{bmatrix} \theta \\ \dot{\theta} \\ \dot{\phi} \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{K_t}{aR_m} \\ \frac{K_t(a+I_2)}{aI_2R_m} \end{bmatrix} V. \tag{12}$$

Теперь переведем уравнение $(12)$ в дискретную форму. Существует множество методов преобразования непрерывных систем в дискретные. Мы рассмотрим метод Эйлера. От других он отличается простотой, но при этом обладает меньшей точностью при высоких значениях шага дискретизации. Чтобы метод был достаточно точным, шаг дискретизации нашей системы должен быть порядка 1 мс.

Представим $\dot{\mathbf{x}}$ в виде:

$$\dot{\mathbf{x}} = \frac{\mathbf{x}_{k+1} - \mathbf{x}_k}{\Delta t},$$

где $\Delta t$ — шаг дискретизации, а $\mathbf{x}_k$ — состояние системы на шаге $k$.

Подставим это выражение в уравнение $(11)$:

$$\frac{\mathbf{x}_{k+1} - \mathbf{x}_k}{\Delta t} = \mathbf{A}\mathbf{x}_k + \mathbf{B}\mathbf{u}_k.$$

Умножим уравнение на $\Delta t$ и перенесем $\mathbf{x}_k$ в правую часть:

$$\mathbf{x}_{k+1} = \mathbf{x}_k + \mathbf{A}\mathbf{x}_k \Delta t + \mathbf{B}\mathbf{u}_k \Delta t.$$

Это уравнение можно представить в виде:

$$\mathbf{x}_{k+1} = \mathbf{A}_d \mathbf{x}_k + \mathbf{B}_d \mathbf{u}_k. \tag{13}$$

где матрицы $\mathbf{A}_d$ и $\mathbf{B}_d$ описывают динамику системы и определяются как:

$$\mathbf{A}_d = \mathbf{I}_n + \mathbf{A}\Delta t, \tag{14}$$ $$\mathbf{B}_d = \mathbf{B}\Delta t. \tag{15}$$

Здесь $\mathbf{I}_n$ — единичная матрица размером $n \times n$. В итоге уравнение $(13)$ примет вид:

$$\begin{bmatrix} \theta_{k+1} \\ \dot{\theta}_{k+1} \\ \dot{\phi}_{k+1} \end{bmatrix} = \begin{bmatrix} 1 & \Delta t & 0 \\ \frac{b}{a}\Delta t & 1 - \frac{\mu_p}{a}\Delta t & \frac{R_m \mu_m + K_t K_e}{aR_m}\Delta t \\ -\frac{b}{a}\Delta t & \frac{\mu_p}{a}\Delta t & 1 - \frac{a+I_2}{aI_2}\left(\mu_m + \frac{K_t K_e}{R_m}\right)\Delta t \end{bmatrix} \begin{bmatrix} \theta_k \\ \dot{\theta}_k \\ \dot{\phi}_k \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{K_t}{aR_m}\Delta t \\ \frac{K_t(a+I_2)}{aI_2R_m}\Delta t \end{bmatrix} V_k. \tag{16}$$

Стабилизация маятника в верхнем неустойчивом положении равновесия

Очевидно, что рассматриваемая система неустойчива в вертикальном положении. Рассмотрим задачу стабилизации маятника в верхнем неустойчивом положении равновесия $\theta = 0$, $\dot{\theta} = 0$ в предположении, что в начале процесса стабилизации он уже находится в некоторой окрестности этого желаемого положения. Задача может быть решена с помощью управления по обратной связи, используя весь вектор состояния. Структурная схема такой системы управления показана на рисунке 10, где $\mathbf{K}$ — матрица весовых коэффициентов. Закон управления выглядит как:

$$\mathbf{u}_k = -\mathbf{K}\mathbf{x}_k. \tag{17}$$

При этом задача сводится к выбору таких коэффициентов обратной связи матрицы $\mathbf{K}$, чтобы система обеспечивала устойчивое вертикальное положение маятника.

Рисунок 10. Структурная схема системы управления с регулятором полного состояния.

Существует множество способов выбора коэффициентов обратной связи, или, по-другому, синтеза управления. Как отмечалось выше, коэффициенты можно выбрать эмпирическим путем, то есть подобрать вручную. Но такой подход может привести к неэффективному решению проблемы. В текущей работе мы рассмотрим вариант выбора коэффициентов с помощью решения задачи линейно-квадратичного управления. Далее по тексту регулятор, синтезированный с помощью решения этой задачи, будем называть LQR от английского Linear Quadratic Regulator.

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

$$J = \int_{0}^{\infty}\left(\mathbf{x}^T \mathbf{Q}\mathbf{x} + \mathbf{u}^T \mathbf{R}\mathbf{u}\right)\,dt.$$

Здесь $\mathbf{Q} \in \mathbb{R}^{n \times n}$ и $\mathbf{R} \in \mathbb{R}^{m \times m}$ — заданные симметричные положительно определенные матрицы весовых коэффициентов, $n$ - размерность вектора состояния, $m$ - размерность вектора управления. Здесь $\mathbf{Q}$ задаёт штраф за отклонение состояний от желаемого положения, а $\mathbf{R}$ — штраф за величину управляющего воздействия. Чем больше элементы матрицы $\mathbf{Q}$, тем сильнее регулятор стремится быстрее уменьшить отклонения состояний. Чем больше элементы матрицы $\mathbf{R}$, тем сильнее ограничивается управляющее воздействие, и тем более плавным, но обычно менее быстрым становится переходный процесс. Таким образом, подбор $\mathbf{Q}$ и $\mathbf{R}$ задаёт компромисс между точностью и быстродействием, с одной стороны, и затратами на управление — с другой.

Подбор весовых коэффициентов обычно ведут по следующим принципам:

• начинать нужно с диагональных единичных матриц;

$$\mathbf{Q} = \operatorname{diag}(q_1, q_2, \dots, q_n), \qquad \mathbf{R} = \operatorname{diag}(r_1, r_2, \dots, r_m).$$

• чем больше значение $q_i$, тем сильнее штрафуется отклонение состояния $x_i$ от нуля;
• значения $r_i$ увеличивают штраф за управление $u_i$, приводя к более плавному управлению.

В нашем случае используется дискретная система $(16)$. Функционал качества для дискретных систем имеет вид:

$$J = \sum_{k=0}^{\infty}\left(\mathbf{x}_k^T \mathbf{Q}\mathbf{x}_k + \mathbf{u}_k^T \mathbf{R}\mathbf{u}_k\right).$$

Закон управления по отрицательной обратной связи $(17)$ должен минимизировать этот функционал. Согласно утверждению из [5], для такого функционала с бесконечным временем процесса матрица $\mathbf{K}$ вычисляется как:

$$\mathbf{K} = \left(\mathbf{R} + \mathbf{B}_d^T \mathbf{P}\mathbf{B}_d\right)^{-1}\mathbf{B}_d^T \mathbf{P}\mathbf{A}_d. \tag{18}$$

где $\mathbf{P}$ — положительно определенная матрица, являющаяся решением дискретного алгебраического уравнения Риккати:

$$\mathbf{P} = \mathbf{A}_d^T \mathbf{P}\mathbf{A}_d - \mathbf{A}_d^T \mathbf{P}\mathbf{B}_d \left(\mathbf{R} + \mathbf{B}_d^T \mathbf{P}\mathbf{B}_d\right)^{-1} \mathbf{B}_d^T \mathbf{P}\mathbf{A}_d + \mathbf{Q}.$$

Это уравнение имеет единственное решение $\mathbf{P}$, если система управляема, а матрицы $\mathbf{Q}$ и $\mathbf{R}$ положительно определены.

Система называется управляемой, если существует такое допустимое управление $\mathbf{u}$, которое бы переводило начальное состояние $\mathbf{x}_0$ в конечное состояние $\mathbf{x}_f$ за конечный интервал времени $[t_0, t_f]$.

Критерий управляемости линейных стационарных систем

Линейный стационарный объект управляем тогда и только тогда, когда матрица управляемости

$$\mathbf{C} = \begin{bmatrix} \mathbf{B} & \mathbf{A}\mathbf{B} & \mathbf{A}^2\mathbf{B} & \dots & \mathbf{A}^{n-1}\mathbf{B} \end{bmatrix}$$

имеет максимальный ранг, то есть $\mathrm{rank}(\mathbf{C}) = n$.

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

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

Критерий устойчивости дискретных систем

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

Разберем подробнее смысл этого критерия. Мы имеем замкнутую систему вида:

$$\mathbf{x}_{k+1} = \mathbf{A}_d \mathbf{x}_k + \mathbf{B}_d \mathbf{u}_k. \tag{19}$$ $$\mathbf{u}_k = -\mathbf{K}\mathbf{x}_k. \tag{20}$$

Подставим $\mathbf{u}_k$ из уравнения $(20)$ в $(19)$, вынесем $\mathbf{x}_k$ за скобку и получим:

$$\mathbf{x}_{k+1} = \left(\mathbf{A}_d - \mathbf{B}_d \mathbf{K}\right)\mathbf{x}_k.$$

Характеристическое уравнение системы будет иметь вид:

$$det((\mathbf{A}_d - \mathbf{B}_d \mathbf{K}) - \mathbf{\lambda} \mathbf{I}) = 0.$$

Согласно критерию устойчивости, все собственные значения $\lambda_i$ матрицы $\mathbf{A}_d - \mathbf{B}_d \mathbf{K}$ должны удовлетворять условию $|\lambda_i| < 1$. Если хотя бы одно собственное значение $|\lambda_i| > 1$, то система будет неустойчива. Какое-либо значение $|\lambda_j| = 1$ при всех остальных $|\lambda_i| < 1$ определяет границу устойчивости дискретной системы.

Для решения алгебраического уравнения Риккати применяют итерационные методы, например метод Ньютона, а на практике чаще всего используют готовые средства Matlab или библиотеки Control Systems на Python. С их помощью находят матрицу $\mathbf{K}$ и анализируют управляемость и устойчивость системы. После этого полученные значения $\mathbf{K}$ применяются в управлении аппаратной платформой. Применительно к нашему обратному маятнику, $\mathbf{K}$ будет представлять собой вектор, так как вектор управления состоит всего из одного элемента. Описанный алгоритм стабилизации будет выглядеть так:

1. Получить с датчиков значения $\theta$ и $\phi$.
2. Вычислить угловые скорости $\dot{\theta}$ и $\dot{\phi}$:

$$\dot{\theta}_k = \frac{\theta_k - \theta_{k-1}}{\Delta t}, \qquad \dot{\phi}_k = \frac{\phi_k - \phi_{k-1}}{\Delta t}.$$

3. Вычислить желаемое значение напряжения на двигателе:

$$V = -\mathbf{K}\mathbf{x}_k = -(k_1 \theta + k_2 \dot{\theta} + k_3 \dot{\phi}).$$

где $k_i$ — $i$-й элемент вектора $\mathbf{K}$.

4. Передать значение $V$ в систему управления двигателем.

Перевод маятника из нижнего положения равновесия в верхнее

Изначально маятник покоится в нижнем положении равновесия. Для перехода в верхнее положение равновесия его нужно сначала раскачать, а затем после попадания в окрестность верхнего положения, стабилизировать [6].

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

$$E = \frac{1}{2}(m_1 l_1^2 + m_2 l_2^2 + I_1 + I_2)\dot{\theta}^2 + (m_1 l_1 + m_2 l_2)g \cos \theta.$$

Желаемая энергия маятника $E^\ast$, покоящегося в верхнем положении равновесия, равна $(m_1 l_1 + m_2 l_2)g$. Закон управления, который будет обеспечивать раскачивание маятника до тех пор, пока его энергия не достигнет величины $E^\ast$, выглядит как:

$$u = k(E^\ast - E)\,\operatorname{sign}(\dot{\theta}\cos \theta). \tag{21}$$

где $k > 0$ — коэффициент обратной связи. Множитель $\operatorname{sign}(\dot{\theta}\cos \theta)$ обеспечивает подкачку энергии в сторону равновесия. Такой метод называется управлением с формированием энергии. В иностранной литературе его обычно называют Energy-Shaping Control. Закон управления $(21)$ обеспечивает рост полной энергии системы до заданного значения, а знак угловой скорости стержня определяет направление вращения. По своей сути Energy-Based Control представляет собой П-регулятор, где в качестве ошибки выступает разница между желаемой и текущей энергией.

Слежение за заданным значением $E^\ast$ энергии в соответствии с законом управления $(21)$ прекращается, когда система преодолевает некоторый порог $\varepsilon$. После этого включается закон управления $(17)$, который уже доводит маятник в желаемое верхнее положение равновесия и стабилизирует его в нём.

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

$$E^\ast = -(m_1 l_1 + m_2 l_2)g.$$

Стоит отметить, что в настоящей работе не получено теоретическое доказательство работоспособности закона управления $(21)$. Этот закон является нелинейным, поэтому произвести синтез и анализ аналитическими методами не представляется возможным. При этом работоспособность построенного управления удаётся показать при помощи численных и экспериментальных исследований.