# Постановка задачи оптимального управления

$\dot{x} = f(x,u,t), \qquad  x \in \mathbb{R}^n, \qquad  u \in \mathbb{U}^m$

$x$ - вектор фазовых переменных, $u$ - вектор управляющих параметров.

Начальные и терминальные условия условия:

$$x(t_0) \in A = \left\{
    \begin{array}\\
       a_1(x,t) = 0 \\
        \dots \\
        a_l(x,t) = 0
    \end{array}   
\right. \qquad
x(t_f) \in B = \left\{
    \begin{array}\\
       b_1(x,t) = 0 \\
        \dots \\
        b_r(x,t) = 0
    \end{array}   
\right.$$


Функционал
$$\Phi(x(t_f), t_f) \rightarrow \max_u$$

# Вспомогательные конструкции

Вектор сопряжённых переменных:

$$p \in \mathbf{R}^n$$

Функция Гамильтона-Понтрягина (гамильтониан):

$$H(x,p,u,t) = p'\cdot f$$

Дифференциальные уравнения для фазовых и сопряжённых переменных:

$$\begin{equation}\tag{1}
 \left\{
    \begin{array}\\
       \dot{x} = \frac{\partial H}{\partial p} \\
       \dot{p} = -\frac{\partial H}{\partial x} 
    \end{array}   
\right.
\end{equation}$$

Расширенный вектор фазовых переменных:
$$P = \left( 
    \begin{array}\\
       p \\
       -H 
    \end{array}
\right)$$

Расширенное терминальное многообразие:
$$B^* = B \cap (\Phi - \Phi^* = 0)$$

Условия трансверсальности:
$$\begin{equation}\tag{2}
P(t_0) \perp A, \qquad P(t_f) \perp B^*
\end{equation}$$

# Принцип маскимума Понтрягина

Для того чтобы некоторое управление $u(t)$ было оптимальным, необходимо, чтобы на соответствующей ему траектории $x(t), p(t)$ для системы (1) с граничными условиями (2) выполнялось условие:
$$H(x,p,u,t) = \max_{v\in\mathbb{U}}H(x,p,v,t), \quad t \in [t_0,t_f]$$

Примечание: в задаче с закреплённым левым концом (старт из фиксированной точки) и свободным левым (терминальное многообразие определяется только заданным моментом времени $t_f$) условия трансверсальности выглядят следующим образом:
$$p(t_f) = \frac{\partial \Phi(x,t)}{\partial x}$$

# Пример (управление одномерным осциллятором)

Уравнения модели:
$$m\cdot \ddot{x} + k\cdot x = F(t) \qquad \rightarrow \qquad  
\left\{
    \begin{array}\\
       \dot{x} = v \\
       \dot{v} = -\omega^2 \cdot x + u
    \end{array}   
\right. 
\qquad \omega^2 = \frac{k}{m}, \qquad \frac{F(t)}{m} = u, \qquad |u| \leq u^{*}
$$

Начальное многообразие:
$$    \begin{array}\\
       x(0) = 0 \\
       v(0) = 0
    \end{array}  
\qquad \rightarrow \qquad   
x(t_0) \in A = \left\{
    \begin{array}\\
       x = 0 \\
       v = 0 \\
       t = 0
    \end{array}   
\right.
$$

Требуется обеспечить максимальное значение координаты $x$ к моменту времени $t_f$, таким образом, функционал задачи:
$$\Phi(x(t_f), t_f) = x(t_f) \rightarrow \max_{u}$$

Функция Гамильтона-Понтрягина:
$$H(x,p,u,t) = p_x \cdot v + p_v \cdot (-\omega^2 \cdot x + u)$$

Управление $u$ входит в $H$ линейно, а значит, чтобы удовлетворить условиям принципа максимума
$$u(t) = u^{*} \cdot sign(p_v)$$

Дифференциальные уравнения на сопряженные переменные:
$$
\left\{
    \begin{array}\\
       \dot{p_x} = -\frac{\partial H}{\partial x} = \omega^2 p_v \\
       \dot{p_v} = -\frac{\partial H}{\partial v} = - p_x
    \end{array}   
\right. 
$$

Условия трансверсальности на левом конце:
$$P(t_0) = \lambda_x \cdot\frac{\partial x}{\partial \left(
    \begin{array}\\
       x \\
       v \\
       t
    \end{array}   
\right)} +
\lambda_v \cdot\frac{\partial v}{\partial \left(
    \begin{array}\\
       x \\
       v \\
       t
    \end{array}   
\right)} +
\lambda_t \cdot\frac{\partial t}{\partial \left(
    \begin{array}\\
       x \\
       v \\
       t
    \end{array}   
\right)}=
\left(
    \begin{array}\\
       \lambda_x \\
       \lambda_v \\
       \lambda_t
    \end{array}   
\right),
$$

то есть точке ортогонален любой вектор и никаких дополнительных ограничений на векотр сопряжённых переменных на левом конце ($t = 0$) не возникает.

Многообразие $B^{*}$ определяется как
$$B^{*} = \left\{
    \begin{array}\\
       t = t_f \\
       x - x^{*} = 0
    \end{array}   
\right. ,$$
где $x^{*}$ неизвестное оптимальное значение функционала.

Условие трансверсальности на правом конце (заметьте, что множитель Лагранжа при функционале равен 1):
$$P(t_f) = \frac{\partial \Phi}{\partial 
\left(
    \begin{array}\\
       x \\
       v \\
       t
    \end{array}   
\right)} + 
\lambda_{tf} \cdot \frac{\partial t}{\partial 
\left(
    \begin{array}\\
       x \\
       v \\
       t
    \end{array}   
\right)} = 
\left(
    \begin{array}\\
       1 \\
       0 \\
       \lambda_{tf}
    \end{array}   
\right).$$

Таким образом, уравнения на сопряжённые переменные имеют следующие решения:
$$
    \begin{array}\\
       p_x(t) = \cos{(\omega \cdot (t - t_f))} \\
       p_v(t) = - \frac{1}{\omega} \sin{(\omega \cdot (t - t_f))} 
    \end{array}
$$

Синтезированное оптимальное управление:
$$ u(t) = u^{*} \cdot sign(-\sin{(\omega \cdot (t - t_f))})$$.


```python
import numpy as np
from numpy import cos, sin, tan
import scipy.integrate as sci
import matplotlib.pyplot as plt
```


```python
class Parameters(object):
    pass


# Physical parameter
param = Parameters()  # instance of class Parameters
param.m = 1      # mass
param.k = 2      # spring constant
param.f_max = 1  # max force
param.u_max = param.f_max / param.m 
param.omega = np.sqrt(param.k / param.m)
param.omega2 = param.k / param.m

# Simulation parameter
sim_param = Parameters()  # instance of class Parameters
sim_param.t0 = 0          # start time
sim_param.tf = 10         # final time
sim_param.dt = 0.01       # step-size
```


```python
def rhs(t, x, p):
    dxdt = np.zeros(2)
    
    r = x[0]   # position vector
    v = x[1]   # velocity vector 
    
    u = control(t, p)  # control vector
    
    # dxdt = f(x, u, t):
    dxdt[0] = v
    dxdt[1] = - p.omega2 * r + u

    # return state derivative
    return dxdt
```


```python
def control(t, p):
    
    u = p.u_max * np.sign(-np.sin(p.omega * (t - sim_param.tf)))

    return u
```


```python
def plot_data(x, u, t, fig_width, fig_height):
    """Plotting function of simulated state and actions
    Args:
        x(ndarray) : state-vector trajectory
        u(ndarray) : control vector trajectory
        t(ndarray) : time vector
        fig_width  : figure width in cm
        fig_height : figure height in cm
        save (bool): save figure (default: False)
    Returns: None
    """
    # creating a figure with 2 subplots, that share the x-axis
    fig1, (ax1, ax2) = plt.subplots(2)


    # plot coords in subplot 1
    ax1.plot(t, x[:, 0], color='r')

    # plot velocities in subplot 2
    ax2.plot(t, np.rad2deg(u[:]), label=r'$u(t)$', lw=1, color='b')

    # Grids
    ax1.grid(True)
    ax2.grid(True)
    
    # set the labels on the x and y axis and the titles
    ax1.set_title('Position')
    ax1.set_ylabel(r'm')
    ax1.set_xlabel(r't in s')
    ax2.set_title('Control')
    ax2.set_ylabel(r'm/s^2')
    ax2.set_xlabel(r't in s')
    

    # automatically adjusts subplot to fit in figure window
    plt.tight_layout()
    
    return None
```


```python
# time vector
tt = np.arange(sim_param.t0, sim_param.tf, sim_param.dt)

# initial state
x0 = [0, 0]

# simulation
sol = sci.solve_ivp(lambda t, x: rhs(t, x, param), (sim_param.t0, sim_param.tf), x0, t_eval=tt)
y_traj = sol.y.T
u_traj = control(tt, param)

# plot
plot_data(y_traj, u_traj, tt, 12, 16)
plt.show()
```

# Численный метод решения задач оптимального управления

Формула для малых приращений функционала задачи оптимального управления:
$$
\begin{equation}\tag{3}
\Delta \Phi(x(t_f), t_f) = p'(t_0)\cdot \Delta x(t_0) + \int_{t_0}^{t_f} (H(x,p,u+\Delta u,t) - H(x,p,u,t)) dt + O(\varepsilon^2),
\end{equation}$$
где $\Delta x(t_0) = O(\varepsilon)$ и $\Delta u(t) = O(\varepsilon)$.

На основе этой формулы представляется возможным реализовать следующую итеративную процедуру (на примере задачи с фиксированным левым и свободным правым концом):
1. зафиксировать некоторое управление $u(t)$
2. решить задачу Коши с уравнениями модели и получить $x(t)$
3. решить задачу Коши с уравнениями для сопряжённых переменных и условиями трансверсальности на правом конце и получить 
$p(t)$
4. разбить интервал $[t_0, t_f]$ на равные достаточно малые подинтервалы и выбрать тот из них, для которого изменение $u(t)$, оставляющее $u$ в области допустимых управлений, обеспечивает максимальное приращение функционала в силу формулы (3)
5. изменить управление $u(t)$ в соответствии с п.4 и повторить пп. 2-4

Процедура повторяется, пока удаётся обеспечивать приращение функционала, то есть улучшать управление в смысле поставленной задачи.

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


```python
# функция, возвращающая непрерывное по времени управление, на основе определенного массива u(t)
def u(t):
    if t > sim_param.tf:
        raise ValueError
    i = int((t - sim_param.t0)//sim_param.dt)
    return u0[i]

#функция, вычисляющая правую часть уравнений движения осциллятора
def rhs_x(t, x, params):
    dxdt = np.zeros(2)
    dxdt[0] = x[1]
    dxdt[1] = u(t) - params.omega2 * x[0]
    return dxdt

#функция, вычисляющая правую часть уравнений на сопряжённые переменные
def rhs_p(t, p, params):
    dpdt = np.zeros(2)
    dpdt[0] = params.omega2 * p[1]
    dpdt[1] = -p[0]
    return dpdt

#функция Гамильтона-Понтрягина
def H(t, x, p, u, params):
    return p[:, 0] * x[:, 1] + p[:, 1]*(u - params.omega2 * x[:, 0])
```


```python
time = np.arange(sim_param.t0, sim_param.tf + sim_param.dt, sim_param.dt)
u0 = np.zeros(time.shape[0])
x0 = [0, 0]
pf = [1, 0]

#интегрируем уравнения движения слева направо
sx = sci.solve_ivp(lambda t, x: rhs_x(t, x, param), (sim_param.t0, sim_param.tf), x0, t_eval = time)
x = sx.y.T

#интегрируем уравнения движения в обращенном времени (от правого конца к левому, 
#т.к. условия трансверсальности даны на правом конце)
sp = sci.solve_ivp(lambda t, p: rhs_p(t, p , param), (sim_param.tf, sim_param.t0), pf, t_eval = sim_param.tf - time) 
p = sp.y.T[::-1] # получен в обратном порядке
```


```python
u_min = -np.ones(time.shape[0]) * param.u_max
u_max = np.ones(time.shape[0]) * param.u_max
```


```python
#Вычисляем гамильтониан на полученной для фиксированного управления u0(t) траектории x(t), p(t)
H0 = H(time, x, p, u0, param)

#а также гамильтонианы на той же траектории, но для управлений принадлежащих верхней (u_max) и нижней (u_min) границам
H_min = H(time, x, p, u_min, param)
H_max = H(time, x, p, u_max, param)
```


```python
# вычисляем интервал на котором изменение управления с фиксированного u0 на максимальное 
# даст наибольшее локальное приращение гамильтониану, а значит и функционалу задачи
i = np.argmax(H_max - H0)

if H_max[i] - H0[i] > 0:
    u0[i] = param.u_max

# то же для изменения с u0 на u_min
j = np.argmax(H_min - H0)

if H_min[i] - H0[i] > 0:
    u0[j] = -param.u_max
    
i, j
```




    (445, 223)



Так выглядела одна итерация вычислительной процедуры. В следующей ячейке все описанные выше операции повторяются в цикле. Цикл может содержать некоторое наперед заданное число итераций или строиться по условию while (пока удаётся увеличивать функционал).


```python
F = []

for i in range(u0.shape[0]):
    sx = sci.solve_ivp(lambda t, x: rhs_x(t, x, param), (sim_param.t0, sim_param.tf), x0, t_eval = time)
    x = sx.y.T
    
    F.append(x[-1, 0])
    
    sp = sci.solve_ivp(lambda t, p: rhs_p(t, p , param), (sim_param.tf, sim_param.t0), pf, t_eval = sim_param.tf - time) 
    p = sp.y.T[::-1] # получен в обратном порядке
    
    H0 = H(time, x, p, u0, param)
    H_min = H(time, x, p, u_min, param)
    H_max = H(time, x, p, u_max, param)
    
    i = np.argmax(H_max - H0)
    u0[i] = param.u_max
    if H_max[i] - H0[i] > 0:
        u0[i] = param.u_max
    
    j = np.argmax(H_min - H0)
    if H_min[j] - H0[j] > 0:
        u0[j] = -param.u_max
```

Строим графики переменной $x$, траектории системы на фазовой плоскости ${x,v}$, полученного закона управления $u(t)$ и график изменения функционала $\Phi$ с числом итераций


```python
plt.plot(time, x[:, 0])
```


```python
plt.plot(x[:, 0], x[:, 1])
```


```python
plt.plot(time, u0)
```


```python
plt.plot(F)
```


```python

```

76a424908ca9a69c2dc11f41a222f8f7272b422e
ipynb
Jupyter Notebook
Lecture Jupyter Notebooks/Lecture 11. Pontryagin's Maximum Principle.ipynb
DPritykin/Control-Theory-Course
f27c13cd0bf9671518c78414f8c3963c7cb870d6
MIT
2022-02-21T06:42:30.000Z
2022-03-14T05:18:00.000Z
Lecture Jupyter Notebooks/Lecture 11. Pontryagin's Maximum Principle.ipynb
DPritykin/Control-Theory-Course
f27c13cd0bf9671518c78414f8c3963c7cb870d6
MIT
Lecture Jupyter Notebooks/Lecture 11. Pontryagin's Maximum Principle.ipynb
DPritykin/Control-Theory-Course
f27c13cd0bf9671518c78414f8c3963c7cb870d6
MIT
2022-03-07T16:25:30.000Z
2022-03-07T16:25:30.000Z
Qwen/Qwen-72B
1. YES
2. YES