最速降线

问题描述

最速降线是最著名的变分问题。

系统仅包含一个阶段。有 \(n_x = 3\) 个状态变量（水平位移 \(x\)，垂直位移 \(y\)，速度 \(v\)）和 \(n_u = 1\) 个控制变量（曲线切线的角度 \(\theta\)）。

动力学方程为

$$ \dot x=v\sin\theta,\quad\dot y=v\cos\theta,\quad\dot v=g\cos \theta,\quad g=9.81 $$

系统有单个积分（时间区间长度）

$$ \mathbb{I} = \int_{t_0}^{t_f} 1 \mathrm dt $$

初始时间固定 \(t_0 = 0\)，末端时间 \(t_f\) 自由，为优化变量。系统有边界条件

$$ \begin{align*} \boldsymbol{x}_0 &= \left[0,0,0\right]^\mathrm{T}\\ \boldsymbol{x}_f &= \left[2,2,\text{free}\right]^\mathrm{T} \end{align*} $$

系统层面，有目标函数

$$ \begin{equation*} F = \mathbb I \end{equation*} $$

计算程序

import numpy as np
import sympy as sp
import matplotlib.pyplot as plt
from pockit.optimizer import ipopt
from pockit.lobatto import System, constant_guess

g = 9.81

S = System(0)
P = S.new_phase(3, 1)
x, y, v = P.x
theta, = P.u
P.set_dynamics([v * sp.sin(theta), v * sp.cos(theta), g * sp.cos(theta)])
P.set_integral([1])
P.set_boundary_condition([0, 0, 0], [2, 2, None], 0, None)
P.set_phase_constraint([theta], [-np.pi], [np.pi])
P.set_discretization(10, 8)
S.set_phase([P])
S.set_objective(P.I[0])

v = constant_guess(P)
v, info = ipopt.solve(S, v)
print(info['status_msg'].decode())
print(info['obj_val']) # 0.8243386694213402

plt.plot(v.t_x, v.x[0])
plt.plot(v.t_x, v.x[1])
plt.plot(v.t_x, v.x[2])
plt.title('state')
plt.legend(['x', 'y', 'v'])
plt.xlabel('t')
plt.show()

plt.plot(v.t_u, v.u[0])
plt.title('control')
plt.legend([r'$\theta$'])
plt.xlabel('t')
plt.show()

计算结果