给定一组一阶隐式连续微分方程

\[\mathbf{f}(\dot{\mathbf{x}}(t), \mathbf{x}(t), \mathbf{u}(t), \mathbf{p}, t)=\mathbf{0}\]

以单摆为例，可被描述为一阶常微分方程

\[\mathbf{f}\left(\left[\begin{array}{c} \dot{\theta} \\ \dot{\omega} \end{array}\right],\left[\begin{array}{c} \theta \\ \omega \end{array}\right],[T],\left[\begin{array}{c} I \\ m \\ g \\ l \end{array}\right]\right)=\left[\begin{array}{c} \dot{\theta}-\omega \\ I \dot{\omega}+m g l \sin \theta-T \end{array}\right]=\mathbf{0}\]

用 SymPy 实现：

import sympy as sm
sm.init_printing(use_latex='mathjax')

I, m, g, l, t = sm.symbols('I, m, g, l, t')
theta, omega, T = sm.symbols('theta, omega, T', cls=sm.Function)
f = sm.Matrix([theta(t).diff() - omega(t), I*omega(t).diff() + m*g*l*sm.sin(theta(t)) - T(t)])

代价函数表示为：

\[J\left(\mathbf{x}, \mathbf{u}, \mathbf{p}\right)=\int L(\mathbf{x}(t), \mathbf{u}(t), \mathbf{p}) d t\]

状态量、输入量和参数量的约束表示为

\[\begin{aligned} \mathbf{x}^{L} & \leq \mathbf{x} \leq \mathbf{x}^{U} \\ \mathbf{u}^{L} & \leq \mathbf{u} \leq \mathbf{u}^{U} \\ \mathbf{p}^{L} & \leq \mathbf{p} \leq \mathbf{p}^{U} \end{aligned}\]

Direct collocation 方法把最优控制问题和状态估计问题离散转换为非线性优化(NLP)问题。

Opty 目前只支持两种一阶积分方法：

Backward Euler method

\[\begin{aligned} \frac{d\mathbf{x}}{dt} & \approx & \frac{\mathbf{x}_i - \mathbf{x}_{i-1}}{h} \\ \mathbf{x}(t_i) & = & \mathbf{x}_i \\ \mathbf{u}(t_i) & = & \mathbf{u}_i \end{aligned}\]

对应的微分方程离散化为

\[\mathbf{f}_i = \mathbf{f}\left(\frac{\mathbf{x}_i - \mathbf{x}_{i-1}}{h}, \mathbf{x}_i, \mathbf{u}_i, \mathbf{p}, t_i\right) = 0\]

Midpoint method

\[\begin{aligned} \frac{d\mathbf{x}}{dt} & \approx & \frac{\mathbf{x}_{i+1} - \mathbf{x}_{i}}{h} \\ \mathbf{x}(t_i) & = & \frac{\mathbf{x}_i + \mathbf{x}_{i+1}}{2} \\ \mathbf{u}(t_i) & = & \frac{\mathbf{u}_i + \mathbf{u}_{i+1}}{2} \end{aligned}\]

对应的微分方程离散化为

\[\mathbf{f}_i = \mathbf{f}\left(\frac{\mathbf{x}_{i+1} - \mathbf{x}_{i}}{h}, \frac{\mathbf{x}_i + \mathbf{x}_{i+1}}{2}, \frac{\mathbf{u}_i + \mathbf{u}_{i+1}}{2}, \mathbf{p}, t_i\right) = \mathbf{0}\]

定义

\[\mathbf{X}_i = [\mathbf{x}_i \quad \mathbf{u}_{i} \quad \mathbf{p}_{i}]^T\]

则 NLP 问题为：

\[\begin{aligned} & \underset{\mathbf{X}_i \in \mathbb{R}^{n + q + r}} {\text{min}} & & J(\mathbf{X}_i) \\ & \text{s.t.} & & \mathbf{f}_i = \mathbf{0} \\ & & & \mathbf{X}_i^L \leq \mathbf{X}_i \leq \mathbf{X}_i^U \end{aligned}\]

Opty 基于 SymPy 采用符号计算稀疏 Jacobian 矩阵

\[\frac{\partial\mathbf{f}_i}{\partial\mathbf{X}_i}\]

值得注意是 Opty 目前不支持对代价函数求导

\[\frac{\partial J}{\partial\mathbf{X}_i}\]

需要用户自己编码实现。

API of opty.direct_collocation.Problem

Parameters

Parameters Function
obj 代价函数
obj_grad 代价函数导数
equations_of_motion 运动方程约束
state_symbols  
num_collocation_nodes  
node_time_interval  
known_parameter_map(optional)  
known_trajectory_map(optional)  
instance_constraints(optional)  
time_symbol(optional)  
tmp_dir(optional)  
integration_method(optional)  
parallel(optional)  
bounds(optional)  

References: