变惯量机械臂最短时间控制¶

背景¶

该基准问题描述一根长度为五米、支点可沿杆长方向滑动的机械臂。支点移动会同时改变两个方向的转动惯量。因此，时间最优解并不只是旋转机械臂：它会先让支点向内移动，在较小惯量下完成姿态变化，再将支点送回指定的终端位置。

这个模型适合学习如何在 Pockit 的单个阶段中同时表达自由终止时间、随状态变化的等效惯量和多个饱和执行器。它是一个有意采用解耦形式的教学基准，并非完整的刚体机械臂模型。具体而言，模型省略了 \(\dot I(q)\dot q\) 一类惯量变化率动量项、科里奥利项以及完整刚体质量矩阵中的其余耦合项。

问题定义¶

令

\[ \boldsymbol{x} = \begin{bmatrix} r & v_r & \psi & \omega_\psi & \phi & \omega_\phi \end{bmatrix}^{\mathsf T}, \qquad \boldsymbol{u} = \begin{bmatrix} u_r & u_\psi & u_\phi \end{bmatrix}^{\mathsf T}. \]

其中，\(r\) 是支点在长度 \(L=5\) 的机械臂上的位置，\(\psi\) 是绕固定轴的方位角，\(\phi\) 是从该轴量起的极角。尺度化的极角方向转动惯量和方位角转动惯量为

\[ I_\phi(r)=\frac{(L-r)^3+r^3}{3}, \qquad I_\psi(r,\phi)=I_\phi(r)\sin^2\phi. \]

动力学方程为

\[ \begin{aligned} \dot r &= v_r, & \dot v_r &= \frac{u_r}{L},\\ \dot\psi &= \omega_\psi, & \dot\omega_\psi &= \frac{u_\psi}{I_\psi(r,\phi)},\\ \dot\phi &= \omega_\phi, & \dot\omega_\phi &= \frac{u_\phi}{I_\phi(r)}. \end{aligned} \]

目标是最小化时间：

\[ \min_{\boldsymbol{x},\boldsymbol{u},t_f} \quad t_f =\int_0^{t_f}1\,\mathrm dt. \]

广义坐标必须处于所用坐标图的物理定义域内：

\[ 0\le r\le L, \qquad \delta\le\phi\le\pi-\delta, \qquad \delta=10^\circ. \]

极角裕度使 \(I_\psi\) 远离 \(\phi=0\) 和 \(\phi=\pi\) 处的坐标奇异点。三个执行器指令都受到约束：

\[ -1\le u_r,u_\psi,u_\phi\le1. \]

端点条件为

\[ \boldsymbol{x}(0) = \begin{bmatrix} 4.5&0&0&0&\pi/4&0 \end{bmatrix}^{\mathsf T}, \]

\[ \boldsymbol{x}(t_f) = \begin{bmatrix} 4.5&0&2\pi/3&0&\pi/4&0 \end{bmatrix}^{\mathsf T}. \]

初始时间固定为零，\(t_f\) 是优化变量。

变量与单位¶

该基准采用类似 SI 的坐标以及经过尺度化的惯量和执行器指令。

符号	含义	单位
\(t\)	时间	s
\(r\)	支点沿机械臂的位置	m
\(v_r\)	支点速度	m/s
\(\psi,\phi\)	方位角和极角	rad
\(\omega_\psi,\omega_\phi\)	角速度	rad/s
\(I_\psi,I_\phi\)	尺度化转动惯量	尺度化
\(u_r,u_\psi,u_\phi\)	有界执行器指令	-

建模选择¶

状态按照“广义坐标、对应速度”交替排列，使等效模型的二阶结构更加直观。极角是状态，而不是固定参数：虽然它的初值和终值都是 \(\pi/4\)，优化器仍可在中间过程改变它，从而调整方位角方向的转动惯量。不能把这些方程理解为真实滑动支点机械臂的动量一致推导；后者必须包含这里省略的 \(\dot I\dot q\) 和刚体耦合项。

该阶段采用 160 个 Lobatto 网格区间，每个区间使用 2 个点。因此，每个控制量在区间内都是分段线性的，只要两个端点满足执行器边界，整个区间也必然满足；高阶控制多项式则可能在受约束节点之间过冲。求解时关闭 Ipopt 的边界松弛，并使用 V_x 和 V_u 在 10,001 个时刻独立检查返回的状态与控制。只要出现非有限值，或物理坐标域、执行器边界被违反，验证就会失败。只有执行器限制被标记为 bang-bang 约束。

初始猜测令 \(t_f=10.5\ \mathrm{s}\)，让方位角力矩在过程过半时反向，并建议支点先向内移动再返回。这样可为 Ipopt 提供有效的尺度，同时不会预先指定最终解。

运行示例¶

在仓库根目录运行：

python -m examples.robot_arm

如需保存图片且不打开交互窗口：

python -m examples.robot_arm --save robot-arm.png --no-show

关键实现¶

import numpy as np
import sympy as sp

from pockit.lobatto import System

system = System(0)
phase = system.new_phase(
    [
        "pivot_position",
        "pivot_speed",
        "azimuth",
        "azimuth_rate",
        "polar_angle",
        "polar_angle_rate",
    ],
    ["pivot_force", "azimuth_torque", "polar_torque"],
)
r, v_r, _, omega_psi, phi, omega_phi = phase.x
u_r, u_psi, u_phi = phase.u

I_phi = ((5.0 - r) ** 3 + r**3) / 3.0
I_psi = I_phi * sp.sin(phi) ** 2
phase.set_dynamics(
    [v_r, u_r / 5.0, omega_psi, u_psi / I_psi, omega_phi, u_phi / I_phi]
)
phase.set_integral([1.0])
polar_margin = np.deg2rad(10.0)
phase.set_phase_constraint(
    [r, phi, u_r, u_psi, u_phi],
    [0.0, polar_margin, -1.0, -1.0, -1.0],
    [5.0, np.pi - polar_margin, 1.0, 1.0, 1.0],
    [False, False, True, True, True],
)
phase.set_boundary_condition(
    [4.5, 0.0, 0.0, 0.0, 0.25 * np.pi, 0.0],
    [4.5, 0.0, 2.0 * np.pi / 3.0, 0.0, 0.25 * np.pi, 0.0],
    0.0,
    None,
)
phase.set_discretization(160, 2)
system.set_phase([phase])
system.set_objective(phase.I[0])

完整脚本还会构造初始猜测，以 bound_relax_factor=0.0 求解，检查 Ipopt 返回状态和端点数值；在 10,001 个状态或控制样本中，只要发现非有限值或变量离开相应定义域，就拒绝该解。

验证结果¶

使用默认网格和 Ipopt 设置，示例收敛到

\[ t_f=9.141679261\ \mathrm{s}. \]

支点的最小位置为 \(3.455698\ \mathrm{m}\)。它没有到达最小惯量位置 \(L/2=2.5\ \mathrm{m}\)：继续向内移动虽然能减小转动惯量，却会增加平移所需时间。执行器曲线显示了预期的饱和弧段以及它们之间的线性过渡。在 10,001 点检查网格上，完整插值控制范围为 \([-0.999999996,0.999999996]\)，位于要求的 \([-1,1]\) 边界内。最小极角为 \(31.099876^\circ\)，与 \(10^\circ\) 的奇异面裕度之间仍有充分距离；在输出精度下，物理定义域的最大违反量为零。

变惯量机械臂的最优状态与执行器历程

源代码¶

完整可运行示例见： examples/robot_arm.py。