Free-Flying Robot

问题描述

系统只有一个阶段。有 \(n_x=6\) 个状态变量 \(\boldsymbol{x} = \left[x, y, v_x, v_y, \theta, \omega\right]^\mathrm{T}\) 和 \(n_u=4\) 个控制变量 \(\boldsymbol{u} = \left[u_1, u_2, u_3, u_4\right]^\mathrm{T}\)。

定义 \(T_1 = u_1 - u_2,T_2 = u_3 - u_4\)，有动力学方程

$$ \begin{align*} \dot x &= v_x\\ \dot y &= v_y\\ \dot{v}_x &= (T_1 + T_2)\cos\theta\\ \dot{v}_y &= (T_1 + T_2)\sin\theta\\ \dot\theta &= \omega\\ \dot\omega &= \alpha T_1 - \beta T_2 \end{align*} $$

其中 \(\alpha = \beta = 0.2\)。系统有 4 个阶段约束

$$ 0 \leq u_i \leq 1, \quad i = 1, 2, 3, 4 $$

以及单个积分

$$ \mathbb I = \int_{t_0}^{t_f} u_1 + u_2 + u_3 + u_4\, \mathrm dt $$

其中时间区间 \(t_0 = 0, t_f = 12\) 固定。边界条件为

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

在系统层面，有目标函数

$$ F = \mathbb I $$

计算程序

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

alpha = 0.2
beta = 0.2

S = System(0)
P = S.new_phase(6, 4)
x, y, v_x, v_y, theta, omega = P.x
u_1, u_2, u_3, u_4 = P.u
T_1 = u_1 - u_2
T_2 = u_3 - u_4
P.set_dynamics([v_x, v_y, (T_1 + T_2) * sp.cos(theta), (T_1 + T_2) * sp.sin(theta), omega, alpha * T_1 - beta * T_2])
P.set_integral([u_1 + u_2 + u_3 + u_4])
P.set_boundary_condition([-10, -10, 0, 0, np.pi / 2, 0], [0, 0, 0, 0, 0, 0], 0, 12)
P.set_phase_constraint([u_1, u_2, u_3, u_4], [0, 0, 0, 0], [1, 1, 1, 1], [True, True, True, True])
P.set_discretization(10, 10)
S.set_phase([P])
S.set_objective(P.I[0])

v = linear_guess(P)
v, info = ipopt.solve(S, v, optimizer_options={'tol': 1e-9, 'acceptable_tol': 1e-18, 'max_iter': 8000, 'linear_solver': 'mumps'})
print(info['status_msg'].decode())
print(info['obj_val']) # 7.9103620992230095

fig = plt.figure()
gs = fig.add_gridspec(3, 2)
ax = []
name = ['x', 'y', 'v_x', 'v_y', r'\theta', r'\omega']
for i in range(3):
    for j in range(2):
        ax.append(fig.add_subplot(gs[i, j]))
for i, ax_ in enumerate(ax):
    ax_.plot(v.t_x, v.x[i], zorder=1)
    ax_.set_ylabel('${}$'.format(name[i]))
ax[4].set_xlabel('$t$')
ax[5].set_xlabel('$t$')
for i in range(3):
    ax[2 * i + 1].yaxis.tick_right()
    ax[2 * i + 1].yaxis.set_label_position('right')
for i in range(4):
    ax[i].set_xticklabels([])
fig.suptitle("state")
fig.tight_layout()
plt.show()

for i in range(len(v.u)):
    plt.plot(v.t_u, v.u[i], label=r"$u_{}$".format(i + 1))
plt.legend()
plt.xlabel('$t$')
plt.title("control")
plt.show()

计算结果