平面四旋翼避障¶

背景¶

四旋翼穿越有障碍物的区域时，需要协调平移与姿态：倾斜机体才能产生水平加速度，同时总推力仍需抵抗重力。本例规划一条固定五秒的飞行轨迹，从 \((0,0)\) 出发，在圆形障碍物上方通过，抵达 \((5,0)\ \mathrm m\)，并以水平姿态静止。

这个例子展示了如何用紧凑的轨迹优化模型同时表达非线性动力学、执行器边界和几何路径约束。由于只考虑平面运动，一个俯仰角就能完整描述姿态。

问题定义¶

状态向量和控制向量为

\[ \boldsymbol x=[x,z,v_x,v_z,\theta,\omega]^{\mathsf T}, \qquad \boldsymbol u=[T,\tau]^{\mathsf T}, \]

其中，\(T\) 是总推力，\(\tau\) 是俯仰力矩。俯仰角逆时针为正。质量 \(m=1.20\ \mathrm{kg}\)，俯仰转动惯量 \(I=0.025\ \mathrm{kg\,m^2}\)，重力加速度 \(g=9.81\ \mathrm{m/s^2}\)，动力学为

\[ \begin{aligned} \dot x &= v_x, & \dot z &= v_z,\\ \dot v_x &= -\frac{T}{m}\sin\theta, & \dot v_z &= \frac{T}{m}\cos\theta-g,\\ \dot\theta &= \omega, & \dot\omega &= \frac{\tau}{I}. \end{aligned} \]

因此，负俯仰角会产生正向水平加速度。固定的端点条件为

\[ \boldsymbol x(0)=[0,0,0,0,0,0]^{\mathsf T}, \qquad \boldsymbol x(5)=[5,0,0,0,0,0]^{\mathsf T}. \]

障碍物中心为 \((2.5,0.8)\ \mathrm m\)，半径为 \(0.8\ \mathrm m\)，因此障碍物与地面相切。计入 \(0.12\ \mathrm m\) 的机体净空后，路径约束为

\[ (x-2.5)^2+(z-0.8)^2\ge0.92^2. \]

其余边界为

\[ \begin{aligned} -0.2&\le x\le5.2, & 0&\le z\le3.0,\\ -65^\circ&\le\theta\le65^\circ, & -3&\le\omega\le3\ \mathrm{rad/s},\\ 0&\le T\le2.2mg, & -0.25&\le\tau\le0.25\ \mathrm{N\,m}. \end{aligned} \]

固定时间目标对控制消耗、角速度和多余高度进行正则化：

\[ J=\int_0^5\left[ 0.025\left(\frac{T-mg}{mg}\right)^2 +0.012\left(\frac{\tau}{0.25}\right)^2 +0.002\omega^2 +0.004z^2 \right]\,\mathrm dt. \]

变量与单位¶

符号	含义	单位
\(t\)	时间	s
\(x,z\)	水平位置和高度	m
\(v_x,v_z\)	水平速度和垂直速度	m/s
\(\theta\)	俯仰角	rad
\(\omega\)	俯仰角速度	rad/s
\(T\)	总推力	N
\(\tau\)	俯仰力矩	N m

建模选择¶

平面旋转只有一个自由度。若采用四元数，会额外引入四个姿态变量和一个单位模约束。使用单个角度 \(\theta\) 是最简表示，没有冗余约束，还可以直接得到推力方向。

初值采用五阶光滑水平进度曲线和正弦平方高度拱线。根据参考加速度估算俯仰角和推力，再根据俯仰角加速度估算力矩。这样既能越过障碍物，又能为优化器提供具有动力学意义的起点。

路径约束施加在配点节点上，但节点之间的多项式仍可能略微下探。因此，默认离散模型在物理安全半径 \(0.92\ \mathrm m\) 之外再加入 \(0.004\ \mathrm m\) 的小保护量，并加入端点兼容的内部地面保护：

\[ z(t)\ge 16\delta\left(\frac{t}{5}\right)^2 \left(1-\frac{t}{5}\right)^2, \qquad \delta=0.03\ \mathrm m. \]

该保护函数在两个固定的地面端点处都为零，仅在飞行中点达到 \(0.03\ \mathrm m\)。它不会改变端点，却能避免端点插值产生极小的负高度。求解后，脚本在 4,001 个等间隔时刻重构全部状态和控制，并检查物理状态边界、执行器边界、障碍物净空和端点。默认 Lobatto 网格使用 14 个区间、每个区间六个配点；快速模式使用 \(8\times4\) 网格。

运行示例¶

在仓库根目录执行经过验证的默认求解：

python -m examples.planar_quadrotor

使用较小的离散问题进行冒烟测试：

python -m examples.planar_quadrotor --quick

保存默认结果且不打开绘图窗口：

python -m examples.planar_quadrotor --save planar-quadrotor.png --no-show

核心实现¶

system = System(0, fastmath=True)
phase = system.new_phase(
    ["x", "z", "velocity_x", "velocity_z", "pitch", "pitch_rate"],
    ["thrust", "torque"],
)
x, z, velocity_x, velocity_z, pitch, pitch_rate = phase.x
thrust, torque = phase.u

phase.set_dynamics(
    [
        velocity_x,
        velocity_z,
        -thrust * sp.sin(pitch) / MASS,
        thrust * sp.cos(pitch) / MASS - GRAVITY,
        pitch_rate,
        torque / PITCH_INERTIA,
    ]
)
phase.set_integral(
    [
        0.025 * ((thrust - MASS * GRAVITY) / (MASS * GRAVITY)) ** 2
        + 0.012 * (torque / MAX_TORQUE) ** 2
        + 0.002 * pitch_rate**2
        + 0.004 * z**2
    ]
)
distance_squared = (x - 2.5) ** 2 + (z - 0.8) ** 2
normalized_time = phase.t / HORIZON
ground_guard = (
    16.0
    * GROUND_GUARD_HEIGHT
    * normalized_time**2
    * (1.0 - normalized_time) ** 2
)
phase.set_phase_constraint(
    [x, z, pitch, pitch_rate, thrust, torque,
     distance_squared, z - ground_guard],
    [-0.2, 0.0, -MAX_PITCH, -3.0, 0.0, -0.25,
     0.924**2, 0.0],
    [5.2, 3.0, MAX_PITCH, 3.0, MAX_THRUST, 0.25,
     np.inf, np.inf],
)
phase.set_boundary_condition(
    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    [5.0, 0.0, 0.0, 0.0, 0.0, 0.0],
    0.0,
    5.0,
)

完整脚本还会构造光滑初值、调用 Ipopt 求解、执行密集验证，并绘制路径、姿态和控制量。

验证结果¶

默认网格的目标函数值为 \(0.01739641\)。相对所需机体安全半径，密集采样得到的最小净空为 \(0.003627\ \mathrm m\)，输出精度下的最大端点误差为 \(3.828\times10^{-13}\)。最大的密集物理路径边界违约为 \(4.431\times10^{-17}\)，属于浮点舍入量级。密集历史中的峰值推力为 \(13.619567\ \mathrm N\)，峰值力矩绝对值为 \(0.022751\ \mathrm{N\,m}\)，均明显低于边界。

平面四旋翼路径、姿态、速度、推力和俯仰力矩

源代码¶

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