平面自由飞行机器人最省燃料控制¶

背景¶

自由飞行检查机器人或航天器无法借助环境支撑，必须独立完成平移和转动。在这个平面示例中，两个执行器模块沿机体轴线产生推力。每个模块包含一对方向相反的单向喷口，因此能够沿机体轴正向或反向施力；两个模块的推力差同时产生偏航力矩。

机器人需要在 12 秒内从 \((-10,-10)\) 移动到原点并停下，同时把航向角从 \(90^\circ\) 调整为零。优化目标是最小化一个归一化的推进剂消耗代理量。

问题定义¶

状态向量和执行器向量为

\[ \boldsymbol{x} = \begin{bmatrix} x&y&v_x&v_y&\theta&\omega \end{bmatrix}^{\mathsf T}, \]

\[ \boldsymbol{u} = \begin{bmatrix} u_{A+}&u_{A-}&u_{B+}&u_{B-} \end{bmatrix}^{\mathsf T}. \]

由两对对置喷口得到带符号的模块推力：

\[ T_A=u_{A+}-u_{A-}, \qquad T_B=u_{B+}-u_{B-}. \]

平移质量和偏航转动惯量经过归一化，力矩系数取 \(\alpha=\beta=0.2\)，动力学方程为

\[ \begin{aligned} \dot x &= v_x, & \dot v_x &= (T_A+T_B)\cos\theta,\\ \dot y &= v_y, & \dot v_y &= (T_A+T_B)\sin\theta,\\ \dot\theta &= \omega, & \dot\omega &= \alpha T_A-\beta T_B. \end{aligned} \]

每个喷口指令都是单向且有界的：

\[ 0\le u_{A+},u_{A-},u_{B+},u_{B-}\le1. \]

目标函数为

\[ \min_{\boldsymbol{x},\boldsymbol{u}} \quad \int_0^{12} \left(u_{A+}+u_{A-}+u_{B+}+u_{B-}\right)\,\mathrm dt. \]

固定端点条件为

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

\[ \boldsymbol{x}(12) = \begin{bmatrix} 0&0&0&0&0&0 \end{bmatrix}^{\mathsf T}. \]

变量与单位¶

长度和时间保留 SI 单位；推力、质量、偏航转动惯量和推进剂流量均已归一化，因此执行器指令和目标函数是尺度化量。

符号	含义	单位
\(t\)	时间	s
\(x,y\)	质心位置	m
\(v_x,v_y\)	惯性系速度	m/s
\(\theta\)	机体航向角	rad
\(\omega\)	偏航角速度	rad/s
\(u_{A+},u_{A-},u_{B+},u_{B-}\)	单向喷口指令	-
\(T_A,T_B\)	带符号的模块推力	尺度化
\(\alpha,\beta\)	偏航力矩系数	尺度化

建模选择¶

平面姿态属于 \(\mathrm{SO}(2)\)，因此一个连续航向角已经足够；使用四元数及其单位范数约束只会引入冗余变量。对置喷口的构造对目标函数也很重要：每个物理喷口指令始终非负，向任一方向喷射都会消耗推进剂。

该示例忽略外力，并对质量和转动惯量进行归一化，以便清楚展示制导问题的结构。它是轨迹优化模型，而不是高保真航天器推进模型。

该阶段采用 160 个 Lobatto 网格区间，每个区间使用 2 个点。这样每个喷口指令在区间内都是分段线性的：端点边界会在整个区间内成立，不会出现高阶控制多项式可能产生的节点间过冲。求解时关闭 Ipopt 的边界松弛，并使用 V_u 在 10,001 个时刻独立检查返回的指令。

初始猜测对位置和航向采用五次静止到静止轨迹，再把近似的轴向加速度和偏航加速度映射回四个单向喷口。

运行示例¶

在仓库根目录运行：

python -m examples.free_flying_robot

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

python -m examples.free_flying_robot --save free-flying-robot.png --no-show

关键实现¶

import numpy as np
import sympy as sp

from pockit.lobatto import System

system = System(0)
phase = system.new_phase(
    ["x", "y", "velocity_x", "velocity_y", "heading", "yaw_rate"],
    [
        "module_a_positive",
        "module_a_negative",
        "module_b_positive",
        "module_b_negative",
    ],
)
_, _, v_x, v_y, heading, yaw_rate = phase.x
u_ap, u_am, u_bp, u_bm = phase.u

thrust_a = u_ap - u_am
thrust_b = u_bp - u_bm
total_thrust = thrust_a + thrust_b
yaw_moment = 0.2 * thrust_a - 0.2 * thrust_b

phase.set_dynamics(
    [
        v_x,
        v_y,
        total_thrust * sp.cos(heading),
        total_thrust * sp.sin(heading),
        yaw_rate,
        yaw_moment,
    ]
)
phase.set_integral([sum(phase.u)])
phase.set_phase_constraint(list(phase.u), [0.0] * 4, [1.0] * 4, True)
phase.set_boundary_condition(
    [-10.0, -10.0, 0.0, 0.0, 0.5 * np.pi, 0.0],
    [0.0] * 6,
    0.0,
    12.0,
)
phase.set_discretization(160, 2)
system.set_phase([phase])
system.set_objective(phase.I[0])

完整脚本还会提供与动力学尺度一致的初始猜测，以 bound_relax_factor=0.0 求解，检查终端状态，只要 10,001 点 V_u 历程中有指令离开 \([0,1]\) 就拒绝该解，并绘制运动路径、若干机器人姿态以及执行器历程。

验证结果¶

默认求解成功终止，得到归一化推进剂消耗代理量

\[ J=7.915541559. \]

10,001 点插值指令范围为 \([0.000000000,0.999999997]\)，因此四个喷口在完整时域内都保持在 \([0,1]\) 中，而不只是配点处满足边界。脚本以 \(2\times10^{-7}\) 的绝对容差检查终点，数值解满足位置、速度、航向角和偏航角速度全部为零的要求。

平面自由飞行机器人的最优路径、采样姿态、状态和喷口指令

源代码¶

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