Planar Quadrotor Obstacle Avoidance

Background

A quadrotor crossing a cluttered area must coordinate translation and attitude: tilting creates horizontal acceleration, while the total thrust must still support the vehicle against gravity. This example plans a five-second flight from \((0,0)\) to \((5,0)\ \mathrm m\), passing above a circular obstacle and returning to rest with level attitude.

The purpose is to show a compact trajectory-optimization model with nonlinear dynamics, actuator bounds, and a geometric path constraint. It models planar motion, so one pitch angle describes attitude completely.

Problem formulation

The state and control vectors are

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

where \(T\) is total thrust and \(\tau\) is pitch torque. Positive pitch is counterclockwise. With mass \(m=1.20\ \mathrm{kg}\), pitch inertia \(I=0.025\ \mathrm{kg\,m^2}\), and gravity \(g=9.81\ \mathrm{m/s^2}\), the dynamics are

\[ \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} \]

Thus a negative pitch angle produces positive horizontal acceleration. The fixed endpoint conditions are

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

The obstacle has center \((2.5,0.8)\ \mathrm m\) and radius \(0.8\ \mathrm m\), so it is tangent to the ground. Adding \(0.12\ \mathrm m\) of vehicle clearance gives the path constraint

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

The remaining bounds are

\[ \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} \]

The fixed-time objective regularizes control effort, angular rate, and excess altitude:

\[ 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. \]

Variables and units

Symbol	Meaning	Unit
\(t\)	Time	s
\(x,z\)	Horizontal position and altitude	m
\(v_x,v_z\)	Horizontal and vertical velocity	m/s
\(\theta\)	Pitch angle	rad
\(\omega\)	Pitch rate	rad/s
\(T\)	Total thrust	N
\(\tau\)	Pitch torque	N m

Modeling choices

A quaternion would add four attitude variables plus a unit-norm constraint, even though planar rotation has only one degree of freedom. Using the single angle \(\theta\) is minimal, has no redundant constraint, and gives the thrust direction directly.

The initial guess follows a smooth fifth-order horizontal progress curve and a sine-squared altitude arch. Its pitch and thrust are estimated from the reference accelerations, and its torque is estimated from the pitch acceleration. This gives the optimizer a dynamically meaningful, obstacle-clearing starting point.

Path constraints are imposed at collocation nodes, while a polynomial can dip slightly between nodes. The default transcription therefore enforces a small \(0.004\ \mathrm m\) guard beyond the physical \(0.92\ \mathrm m\) safe radius. It also imposes the endpoint-compatible interior ground guard

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

This guard is zero at both fixed ground endpoints and reaches only \(0.03\ \mathrm m\) halfway through the flight. It prevents a tiny negative altitude caused by endpoint interpolation without changing either endpoint. After solving, the script evaluates every state and control at 4,001 evenly spaced times and checks the physical state bounds, actuator bounds, obstacle clearance, and endpoints. The default Lobatto mesh uses 14 intervals and six collocation points per interval; quick mode uses \(8\times4\).

Run the example

From the repository root, run the verified default solve:

python -m examples.planar_quadrotor

Run a smaller smoke-test transcription with:

python -m examples.planar_quadrotor --quick

To save the default result without opening a window:

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

Key implementation

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,
)

The complete script also constructs the smooth initial guess, solves with Ipopt, performs dense validation, and draws the path, attitude, and controls.

Verified result

With the default mesh, the objective is \(0.01739641\). The minimum dense clearance beyond the required vehicle radius is \(0.003627\ \mathrm m\), and the maximum endpoint error is \(3.828\times10^{-13}\). The largest dense physical path-bound violation is \(4.431\times10^{-17}\), at floating-point roundoff. Peak dense thrust is \(13.619567\ \mathrm N\), while peak dense absolute torque is \(0.022751\ \mathrm{N\,m}\), both comfortably inside their bounds.

Source code

See the complete runnable example: examples/planar_quadrotor.py.