Robot Arm Problem

问题描述

系统仅包含一个阶段。有 \(n_x = 6\) 个状态变量和 \(n_u = 3\) 个控制变量。

动力学方程为

$$ \begin{align*} \dot{x}_1(t) &= x_2(t) \\ \dot{x}_3(t) &= x_4(t) \\ \dot{x}_5(t) &= x_6(t) \\ \dot{x}_2(t) &= u_1(t) / L \\ \dot{x}_4(t) &= u_2(t) / {I_\theta(t)} \\ \dot{x}_6(t) &= u_3(t) / I_\phi(t) \end{align*} $$

其中

$$ \begin{align*} L &= 5\\ I_\phi(t) &= \frac 13\left(\left(L - x_1\right)^3 + x_1^3\right)\\ I_\theta(t) &= I_\phi(t)\sin^2x_5 \end{align*} $$

系统有阶段约束

$$ \begin{equation*} -1 \leq u_i(t) \leq 1, \quad i = 1, 2, 3 \end{equation*} $$

以及单个积分（时间区间长度）

$$ \begin{equation*} \mathbb{I} = \int_{t_0}^{t_f} 1 \mathrm dt \end{equation*} $$

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

$$ \begin{align*} \boldsymbol{x}_0 &= \left[9 / 2, 0, 0, 0, \pi / 4, 0\right]^\mathrm{T}\\ \boldsymbol{x}_f &= \left[9 / 2, 0, 2 \pi / 3, 0, \pi / 4, 0\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, linear_guess

S = System(0)
P = S.new_phase(6, 3)
y_1, y_2, y_3, y_4, y_5, y_6 = P.x
u_1, u_2, u_3 = P.u
L = 5
I_theta = ((L - y_1) ** 3 + y_1 ** 3) / 3 * sp.sin(y_5) ** 2
I_phi = ((L - y_1) ** 3 + y_1 ** 3) / 3
P.set_dynamics([y_2, u_1 / L, y_4, u_2 / I_theta, y_6, u_3 / I_phi])
P.set_integral([1])
P.set_boundary_condition([9 / 2, 0, 0, 0, np.pi / 4, 0], [9 / 2, 0, 2 * np.pi / 3, 0, np.pi / 4, 0], 0, None)
P.set_phase_constraint([u_1, u_2, u_3], [-1, -1, -1], [1, 1, 1], True)
P.set_discretization(20, 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-12, 'acceptable_tol': 1e-18})
print(info['status_msg'].decode())
print(info['obj_val']) # 9.140966690377573

fig = plt.figure()
gs = fig.add_gridspec(3, 2)
ax = []
name = [r'$x_1$', r'$x_2$', r'$x_3$', r'$x_4$', r'$x_5$', r'$x_6$']
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()

计算结果