Quadcopter Minimum Time Trajectory#

Mathematical Formulation#

Problem Statement#

Find the optimal motor speeds \(\omega_i(t)\) for \(i = 1,2,3,4\) that minimize the flight time:

\[J = t_f + \int_0^{t_f} (\omega_1^2 + \omega_2^2 + \omega_3^2 + \omega_4^2) dt\]

Subject to the quadcopter dynamics with Newton-Euler equations:

\[\frac{d\mathbf{r}}{dt} = \mathbf{v}\]
\[\frac{d\mathbf{v}}{dt} = \frac{1}{m}(\mathbf{R}_b^g \mathbf{F}_T + \mathbf{F}_{\text{drag}}) + \mathbf{g}\]
\[\frac{d\boldsymbol{\Theta}}{dt} = \mathbf{W}(\boldsymbol{\Theta}) \boldsymbol{\omega}_b\]
\[\frac{d\boldsymbol{\omega}_b}{dt} = \mathbf{J}^{-1}(\boldsymbol{\tau} - \boldsymbol{\omega}_b \times \mathbf{J}\boldsymbol{\omega}_b - \mathbf{J}_r \boldsymbol{\omega}_b \times \mathbf{e}_z \Omega_{\text{prop}})\]

Where:

  • \(\mathbf{F}_T = [0, 0, -F_T]^T\) with \(F_T = K_T(\omega_1^2 + \omega_2^2 + \omega_3^2 + \omega_4^2)\)

  • \(\mathbf{F}_{\text{drag}} = -[K_{dx} \dot{X}, K_{dy} \dot{Y}, K_{dz} \dot{Z}]^T\)

  • \(\boldsymbol{\tau} = [l_{\text{arm}} K_T(\omega_1^2 - \omega_3^2), l_{\text{arm}} K_T(\omega_2^2 - \omega_4^2), K_d(\omega_1^2 - \omega_2^2 + \omega_3^2 - \omega_4^2)]^T\)

  • \(\Omega_{\text{prop}} = \omega_1 - \omega_2 + \omega_3 - \omega_4\)

Attitude Kinematics#

\[\frac{d\phi}{dt} = p + \sin\phi \tan\theta \cdot q + \cos\phi \tan\theta \cdot r\]
\[\frac{d\theta}{dt} = \cos\phi \cdot q - \sin\phi \cdot r\]
\[\frac{d\psi}{dt} = \frac{\sin\phi \cdot q + \cos\phi \cdot r}{\cos\theta}\]

Danger Zone Constraint#

\[(X - X_{\text{danger}})^2 + (Y - Y_{\text{danger}})^2 \geq (R_{\text{danger}} + R_{\text{safety}})^2\]

Boundary Conditions#

  • Initial conditions: \(\mathbf{r}(0) = [1, 1, 1]^T\) m, \(\mathbf{v}(0) = \mathbf{0}\), \(\boldsymbol{\Theta}(0) = \mathbf{0}\), \(\boldsymbol{\omega}_b(0) = \mathbf{0}\)

  • Final conditions: \(\mathbf{r}(t_f) = [5, 5, 5]^T\) m, \(\mathbf{v}(t_f) = \mathbf{0}\), \(\boldsymbol{\Theta}(t_f) = \mathbf{0}\), \(\boldsymbol{\omega}_b(t_f) = \mathbf{0}\)

  • Control bounds: \(0 \leq \omega_i \leq 1500\) rad/s for \(i = 1,2,3,4\)

  • State bounds: \(|\phi|, |\theta| \leq 60°\), \(Z \geq 0\)

Physical Parameters#

  • Vehicle mass: \(m = 0.92\) kg (DJI Mavic 3)

  • Inertia matrix: \(\mathbf{J} = \text{diag}(0.0123, 0.0123, 0.0246)\) kg⋅m²

  • Rotor inertia: \(J_r = 3 \times 10^{-5}\) kg⋅m²

  • Arm length: \(l_{\text{arm}} = 0.19\) m

  • Thrust coefficient: \(K_T = 2.5 \times 10^{-6}\) N⋅s²/rad²

  • Drag coefficient: \(K_d = 6.3 \times 10^{-8}\) N⋅m⋅s²/rad²

  • Linear drag: \(K_{dx} = K_{dy} = 0.15\) N⋅s/m, \(K_{dz} = 0.20\) N⋅s/m

  • Danger zone: Center \((3, 3)\) m, radius \(0.4\) m

State Variables#

  • \(X(t), Y(t), Z(t)\): Position coordinates (m)

  • \(\dot{X}(t), \dot{Y}(t), \dot{Z}(t)\): Velocity components (m/s)

  • \(\phi(t), \theta(t), \psi(t)\): Euler angles (roll, pitch, yaw) (rad)

  • \(p(t), q(t), r(t)\): Angular velocities (body frame) (rad/s)

Control Variables#

  • \(\omega_1(t), \omega_2(t), \omega_3(t), \omega_4(t)\): Motor angular speeds (rad/s)

Notes#

This problem demonstrates minimum-time quadcopter trajectory optimization with obstacle avoidance. Numerical scaling is applied for improved conditioning.

Running This Example#

cd examples/quadcopter
python quadcopter.py
python quadcopter_animate.py

Code Implementation#

examples/quadcopter/quadcopter.py#
  1import casadi as ca
  2import numpy as np
  3
  4import maptor as mtor
  5
  6
  7# ============================================================================
  8# Physical Constants and Parameters
  9# ============================================================================
 10
 11# Vehicle parameters (DJI Mavic 3 specifications)
 12m = 0.92
 13g = 9.81
 14l_arm = 0.19
 15
 16# Inertia parameters (kg⋅m²)
 17Jx = 0.0123
 18Jy = 0.0123
 19Jz = 0.0246
 20Jr = 0.000030
 21
 22# Aerodynamic coefficients
 23K_T = 2.5e-6
 24K_d = 6.3e-8
 25
 26# Drag coefficients (linear damping)
 27K_dx = 0.15
 28K_dy = 0.15
 29K_dz = 0.20
 30
 31# Motor constraints
 32omega_max = 1500.0
 33omega_min = 0.0
 34
 35# Danger zone parameters (infinite height cylinder)
 36DANGER_ZONE_CENTER_X = 3.0
 37DANGER_ZONE_CENTER_Y = 3.0
 38DANGER_ZONE_RADIUS = 0.4
 39
 40
 41# ============================================================================
 42# Scaling Factors
 43# ============================================================================
 44
 45POS_SCALE = 10.0
 46VEL_SCALE = 10.0
 47ANG_SCALE = 1.0
 48OMEGA_B_SCALE = 10.0
 49OMEGA_M_SCALE = 1000.0
 50TIME_SCALE = 1.0
 51
 52
 53# ============================================================================
 54# Problem Setup
 55# ============================================================================
 56
 57problem = mtor.Problem("Quadrotor Minimum Time Trajectory")
 58phase = problem.set_phase(1)
 59
 60
 61# ============================================================================
 62# Variables
 63# ============================================================================
 64
 65t = phase.time(initial=0.0)
 66
 67# Position states (scaled)
 68X_s = phase.state("X_scaled", initial=1.0 / POS_SCALE, final=5.0 / POS_SCALE)
 69Y_s = phase.state("Y_scaled", initial=1.0 / POS_SCALE, final=5.0 / POS_SCALE)
 70Z_s = phase.state("Z_scaled", initial=1.0 / POS_SCALE, final=5.0 / POS_SCALE, boundary=(0.0, None))
 71
 72# Velocity states (scaled)
 73X_dot_s = phase.state("X_dot_scaled", initial=0.0, final=0.0)
 74Y_dot_s = phase.state("Y_dot_scaled", initial=0.0, final=0.0)
 75Z_dot_s = phase.state("Z_dot_scaled", initial=0.0, final=0.0)
 76
 77# Attitude states (scaled Euler angles)
 78phi_s = phase.state(
 79    "phi_scaled", initial=0.0, final=0.0, boundary=(-np.pi / 3 / ANG_SCALE, np.pi / 3 / ANG_SCALE)
 80)
 81theta_s = phase.state(
 82    "theta_scaled", initial=0.0, final=0.0, boundary=(-np.pi / 3 / ANG_SCALE, np.pi / 3 / ANG_SCALE)
 83)
 84psi_s = phase.state("psi_scaled", initial=0.0, boundary=(-np.pi / ANG_SCALE, np.pi / ANG_SCALE))
 85
 86# Angular rate states (scaled)
 87p_s = phase.state(
 88    "p_scaled", initial=0.0, final=0.0, boundary=(-10.0 / OMEGA_B_SCALE, 10.0 / OMEGA_B_SCALE)
 89)
 90q_s = phase.state(
 91    "q_scaled", initial=0.0, final=0.0, boundary=(-10.0 / OMEGA_B_SCALE, 10.0 / OMEGA_B_SCALE)
 92)
 93r_s = phase.state(
 94    "r_scaled", initial=0.0, final=0.0, boundary=(-5.0 / OMEGA_B_SCALE, 5.0 / OMEGA_B_SCALE)
 95)
 96
 97# Control inputs (scaled motor speeds)
 98omega1_s = phase.control(
 99    "omega1_scaled", boundary=(omega_min / OMEGA_M_SCALE, omega_max / OMEGA_M_SCALE)
100)
101omega2_s = phase.control(
102    "omega2_scaled", boundary=(omega_min / OMEGA_M_SCALE, omega_max / OMEGA_M_SCALE)
103)
104omega3_s = phase.control(
105    "omega3_scaled", boundary=(omega_min / OMEGA_M_SCALE, omega_max / OMEGA_M_SCALE)
106)
107omega4_s = phase.control(
108    "omega4_scaled", boundary=(omega_min / OMEGA_M_SCALE, omega_max / OMEGA_M_SCALE)
109)
110
111
112# ============================================================================
113# Dynamics
114# ============================================================================
115
116# Convert scaled variables to physical units
117X_phys = X_s * POS_SCALE
118Y_phys = Y_s * POS_SCALE
119Z_phys = Z_s * POS_SCALE
120X_dot_phys = X_dot_s * VEL_SCALE
121Y_dot_phys = Y_dot_s * VEL_SCALE
122Z_dot_phys = Z_dot_s * VEL_SCALE
123phi_phys = phi_s * ANG_SCALE
124theta_phys = theta_s * ANG_SCALE
125psi_phys = psi_s * ANG_SCALE
126p_phys = p_s * OMEGA_B_SCALE
127q_phys = q_s * OMEGA_B_SCALE
128r_phys = r_s * OMEGA_B_SCALE
129omega1_phys = omega1_s * OMEGA_M_SCALE
130omega2_phys = omega2_s * OMEGA_M_SCALE
131omega3_phys = omega3_s * OMEGA_M_SCALE
132omega4_phys = omega4_s * OMEGA_M_SCALE
133
134# Total thrust and gyroscopic effect from motor speeds
135F_T = K_T * (omega1_phys**2 + omega2_phys**2 + omega3_phys**2 + omega4_phys**2)
136omega_prop = omega1_phys - omega2_phys + omega3_phys - omega4_phys
137
138# Trigonometric functions for rotation matrices
139cos_phi, sin_phi = ca.cos(phi_phys), ca.sin(phi_phys)
140cos_theta, sin_theta = ca.cos(theta_phys), ca.sin(theta_phys)
141cos_psi, sin_psi = ca.cos(psi_phys), ca.sin(psi_phys)
142tan_theta = ca.tan(theta_phys)
143
144# Position dynamics (simple integration of global velocities)
145X_dynamics_phys = X_dot_phys
146Y_dynamics_phys = Y_dot_phys
147Z_dynamics_phys = Z_dot_phys
148
149# Translational dynamics (global frame accelerations)
150X_dot_dynamics_phys = (1 / m) * (
151    -(cos_phi * cos_psi * sin_theta + sin_phi * sin_psi) * F_T - K_dx * X_dot_phys
152)
153Y_dot_dynamics_phys = (1 / m) * (
154    -(cos_phi * sin_psi * sin_theta - cos_psi * sin_phi) * F_T - K_dy * Y_dot_phys
155)
156Z_dot_dynamics_phys = (1 / m) * (-cos_phi * cos_theta * F_T - K_dz * Z_dot_phys) + g
157
158# Attitude dynamics
159cos_theta_safe = ca.fmax(ca.fabs(cos_theta), 1e-6)
160sec_theta = 1.0 / cos_theta_safe
161
162phi_dynamics_phys = p_phys + sin_phi * tan_theta * q_phys + cos_phi * tan_theta * r_phys
163theta_dynamics_phys = cos_phi * q_phys - sin_phi * r_phys
164psi_dynamics_phys = (sin_phi * q_phys + cos_phi * r_phys) * sec_theta
165
166# Angular acceleration dynamics
167p_dynamics_phys = (1 / Jx) * (
168    (Jy - Jz) * q_phys * r_phys
169    - Jr * q_phys * omega_prop
170    + l_arm * K_T * (omega1_phys**2 - omega3_phys**2)
171)
172q_dynamics_phys = (1 / Jy) * (
173    (Jz - Jx) * p_phys * r_phys
174    + Jr * p_phys * omega_prop
175    + l_arm * K_T * (omega2_phys**2 - omega4_phys**2)
176)
177r_dynamics_phys = (1 / Jz) * (
178    (Jx - Jy) * p_phys * q_phys
179    + K_d * (omega1_phys**2 - omega2_phys**2 + omega3_phys**2 - omega4_phys**2)
180)
181
182phase.dynamics(
183    {
184        X_s: X_dynamics_phys / POS_SCALE,
185        Y_s: Y_dynamics_phys / POS_SCALE,
186        Z_s: Z_dynamics_phys / POS_SCALE,
187        X_dot_s: X_dot_dynamics_phys / VEL_SCALE,
188        Y_dot_s: Y_dot_dynamics_phys / VEL_SCALE,
189        Z_dot_s: Z_dot_dynamics_phys / VEL_SCALE,
190        phi_s: phi_dynamics_phys / ANG_SCALE,
191        theta_s: theta_dynamics_phys / ANG_SCALE,
192        psi_s: psi_dynamics_phys / ANG_SCALE,
193        p_s: p_dynamics_phys / OMEGA_B_SCALE,
194        q_s: q_dynamics_phys / OMEGA_B_SCALE,
195        r_s: r_dynamics_phys / OMEGA_B_SCALE,
196    }
197)
198
199
200# ============================================================================
201# Constraints
202# ============================================================================
203
204# Cylindrical danger zone constraint (scaled coordinates)
205danger_zone_center_x_scaled = DANGER_ZONE_CENTER_X / POS_SCALE
206danger_zone_center_y_scaled = DANGER_ZONE_CENTER_Y / POS_SCALE
207danger_zone_radius_scaled = DANGER_ZONE_RADIUS / POS_SCALE
208
209safety_distance_scaled = 0.5 / POS_SCALE
210
211distance_squared_to_danger = (X_s - danger_zone_center_x_scaled) ** 2 + (
212    Y_s - danger_zone_center_y_scaled
213) ** 2
214
215phase.path_constraints(
216    distance_squared_to_danger >= danger_zone_radius_scaled**2 + safety_distance_scaled**2
217)
218
219
220# ============================================================================
221# Objective
222# ============================================================================
223
224omega_reg_scaled = omega1_s**2 + omega2_s**2 + omega3_s**2 + omega4_s**2
225omega_integral_scaled = phase.add_integral(omega_reg_scaled)
226problem.minimize(t.final + omega_integral_scaled)
227
228
229# ============================================================================
230# Mesh Configuration and Initial Guess
231# ============================================================================
232
233phase.mesh([8, 8, 8], [-1.0, -1 / 3, 1 / 3, 1.0])
234
235states_guess = []
236controls_guess = []
237
238for N in [8, 8, 8]:
239    tau = np.linspace(-1, 1, N + 1)
240    t_norm = (tau + 1) / 2
241
242    # Linear trajectory guess (scaled) - avoiding danger zone
243    X_vals_scaled = (1.0 + 4.0 * t_norm) / POS_SCALE
244    Y_vals_scaled = (1.0 + 4.0 * t_norm) / POS_SCALE
245    Z_vals_scaled = (1.0 + 4.0 * t_norm) / POS_SCALE
246
247    # Smooth velocity profile (scaled)
248    X_dot_vals_scaled = (5.0 * np.sin(np.pi * t_norm)) / VEL_SCALE
249    Y_dot_vals_scaled = (5.0 * np.sin(np.pi * t_norm)) / VEL_SCALE
250    Z_dot_vals_scaled = (3.0 * np.sin(np.pi * t_norm)) / VEL_SCALE
251
252    # Small attitude variations (scaled)
253    phi_vals_scaled = (0.1 * np.sin(2 * np.pi * t_norm)) / ANG_SCALE
254    theta_vals_scaled = (0.1 * np.cos(2 * np.pi * t_norm)) / ANG_SCALE
255    psi_vals_scaled = (0.2 * t_norm) / ANG_SCALE
256
257    # Angular rates near zero (scaled)
258    p_vals_scaled = np.zeros(N + 1)
259    q_vals_scaled = np.zeros(N + 1)
260    r_vals_scaled = np.zeros(N + 1)
261
262    states_guess.append(
263        np.vstack(
264            [
265                X_vals_scaled,
266                Y_vals_scaled,
267                Z_vals_scaled,
268                X_dot_vals_scaled,
269                Y_dot_vals_scaled,
270                Z_dot_vals_scaled,
271                phi_vals_scaled,
272                theta_vals_scaled,
273                psi_vals_scaled,
274                p_vals_scaled,
275                q_vals_scaled,
276                r_vals_scaled,
277            ]
278        )
279    )
280
281    # Hover motor speed baseline (scaled)
282    hover_speed = np.sqrt(m * g / (4 * K_T))
283    omega_vals_scaled = np.full(N, hover_speed / OMEGA_M_SCALE)
284
285    controls_guess.append(
286        np.vstack([omega_vals_scaled, omega_vals_scaled, omega_vals_scaled, omega_vals_scaled])
287    )
288
289phase.guess(
290    states=states_guess,
291    controls=controls_guess,
292    terminal_time=8.0,
293)
294
295
296# ============================================================================
297# Solve
298# ============================================================================
299
300solution = mtor.solve_adaptive(
301    problem,
302    error_tolerance=1e-2,
303    max_iterations=30,
304    min_polynomial_degree=3,
305    max_polynomial_degree=8,
306    ode_solver_tolerance=1e-3,
307    nlp_options={
308        "ipopt.max_iter": 1000,
309        "ipopt.tol": 1e-6,
310        "ipopt.constr_viol_tol": 1e-4,
311        "ipopt.linear_solver": "mumps",
312        "ipopt.print_level": 3,
313        "ipopt.mu_strategy": "adaptive",
314        "ipopt.acceptable_tol": 1e-4,
315        "ipopt.acceptable_iter": 5,
316    },
317)
318
319
320# ============================================================================
321# Results
322# ============================================================================
323
324if solution.status["success"]:
325    flight_time = solution.status["objective"]
326    print(f"Objective: {flight_time:.3f} seconds")
327
328    # Convert scaled final state to physical for verification
329    X_final = solution["X_scaled"][-1] * POS_SCALE
330    Y_final = solution["Y_scaled"][-1] * POS_SCALE
331    Z_final = solution["Z_scaled"][-1] * POS_SCALE
332    position_error = np.sqrt((X_final - 5.0) ** 2 + (Y_final - 5.0) ** 2 + (Z_final - 5.0) ** 2)
333
334    print(f"Final position: ({X_final:.3f}, {Y_final:.3f}, {Z_final:.3f}) m")
335    print(f"Position error: {position_error:.3f} m")
336
337    # Flight envelope analysis (convert scaled to physical)
338    X_dot_phys = solution["X_dot_scaled"] * VEL_SCALE
339    Y_dot_phys = solution["Y_dot_scaled"] * VEL_SCALE
340    Z_dot_phys = solution["Z_dot_scaled"] * VEL_SCALE
341    max_speed = max(np.sqrt(X_dot_phys**2 + Y_dot_phys**2 + Z_dot_phys**2))
342
343    phi_phys = solution["phi_scaled"] * ANG_SCALE
344    theta_phys = solution["theta_scaled"] * ANG_SCALE
345    max_phi = max(np.abs(phi_phys)) * 180 / np.pi
346    max_theta = max(np.abs(theta_phys)) * 180 / np.pi
347
348    print(f"Maximum speed: {max_speed:.2f} m/s")
349    print(f"Maximum roll angle: {max_phi:.1f}°")
350    print(f"Maximum pitch angle: {max_theta:.1f}°")
351
352    # Motor usage analysis (convert scaled to physical)
353    omega1_phys = solution["omega1_scaled"] * OMEGA_M_SCALE
354    omega2_phys = solution["omega2_scaled"] * OMEGA_M_SCALE
355    omega3_phys = solution["omega3_scaled"] * OMEGA_M_SCALE
356    omega4_phys = solution["omega4_scaled"] * OMEGA_M_SCALE
357
358    avg_motor_speed = (omega1_phys + omega2_phys + omega3_phys + omega4_phys) / 4
359    max_motor_speed = max(np.maximum.reduce([omega1_phys, omega2_phys, omega3_phys, omega4_phys]))
360
361    print(f"Average motor speed: {np.mean(avg_motor_speed):.1f} rad/s")
362    print(f"Maximum motor speed: {max_motor_speed:.1f} rad/s")
363    print(f"Motor utilization: {max_motor_speed / omega_max * 100:.1f}%")
364
365    # Danger zone verification
366    X_traj_phys = solution["X_scaled"] * POS_SCALE
367    Y_traj_phys = solution["Y_scaled"] * POS_SCALE
368    min_distance_to_danger = min(
369        np.sqrt(
370            (X_traj_phys - DANGER_ZONE_CENTER_X) ** 2 + (Y_traj_phys - DANGER_ZONE_CENTER_Y) ** 2
371        )
372    )
373    print(
374        f"Minimum distance to danger zone: {min_distance_to_danger:.3f} m (required: {DANGER_ZONE_RADIUS:.1f} m)"
375    )
376
377    solution.plot()
378
379else:
380    print(f"Optimization failed: {solution.status['message']}")