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
233# Mesh and guess
234num_interval = 16
235degree = [3]
236final_mesh = degree * num_interval
237phase.mesh(final_mesh, np.linspace(-1.0, 1.0, num_interval + 1))
238
239states_guess = []
240controls_guess = []
241for N in final_mesh:
242    tau = np.linspace(-1, 1, N + 1)
243    t_norm = (tau + 1) / 2
244
245    # Linear trajectory guess (scaled) - avoiding danger zone
246    X_vals_scaled = (1.0 + 4.0 * t_norm) / POS_SCALE
247    Y_vals_scaled = (1.0 + 4.0 * t_norm) / POS_SCALE
248    Z_vals_scaled = (1.0 + 4.0 * t_norm) / POS_SCALE
249
250    # Smooth velocity profile (scaled)
251    X_dot_vals_scaled = (5.0 * np.sin(np.pi * t_norm)) / VEL_SCALE
252    Y_dot_vals_scaled = (5.0 * np.sin(np.pi * t_norm)) / VEL_SCALE
253    Z_dot_vals_scaled = (3.0 * np.sin(np.pi * t_norm)) / VEL_SCALE
254
255    # Small attitude variations (scaled)
256    phi_vals_scaled = (0.1 * np.sin(2 * np.pi * t_norm)) / ANG_SCALE
257    theta_vals_scaled = (0.1 * np.cos(2 * np.pi * t_norm)) / ANG_SCALE
258    psi_vals_scaled = (0.2 * t_norm) / ANG_SCALE
259
260    # Angular rates near zero (scaled)
261    p_vals_scaled = np.zeros(N + 1)
262    q_vals_scaled = np.zeros(N + 1)
263    r_vals_scaled = np.zeros(N + 1)
264
265    states_guess.append(
266        np.vstack(
267            [
268                X_vals_scaled,
269                Y_vals_scaled,
270                Z_vals_scaled,
271                X_dot_vals_scaled,
272                Y_dot_vals_scaled,
273                Z_dot_vals_scaled,
274                phi_vals_scaled,
275                theta_vals_scaled,
276                psi_vals_scaled,
277                p_vals_scaled,
278                q_vals_scaled,
279                r_vals_scaled,
280            ]
281        )
282    )
283
284    # Hover motor speed baseline (scaled)
285    hover_speed = np.sqrt(m * g / (4 * K_T))
286    omega_vals_scaled = np.full(N, hover_speed / OMEGA_M_SCALE)
287
288    controls_guess.append(
289        np.vstack([omega_vals_scaled, omega_vals_scaled, omega_vals_scaled, omega_vals_scaled])
290    )
291
292phase.guess(
293    states=states_guess,
294    controls=controls_guess,
295    terminal_time=8.0,
296)
297
298
299# ============================================================================
300# Solve
301# ============================================================================
302
303solution = mtor.solve_adaptive(
304    problem,
305    error_tolerance=1e-2,
306    max_iterations=30,
307    min_polynomial_degree=3,
308    max_polynomial_degree=8,
309    ode_solver_tolerance=1e-3,
310    nlp_options={
311        "ipopt.max_iter": 1000,
312        "ipopt.tol": 1e-6,
313        "ipopt.constr_viol_tol": 1e-4,
314        "ipopt.linear_solver": "mumps",
315        "ipopt.print_level": 3,
316        "ipopt.mu_strategy": "adaptive",
317        "ipopt.acceptable_tol": 1e-4,
318        "ipopt.acceptable_iter": 5,
319    },
320)
321
322
323# ============================================================================
324# Results
325# ============================================================================
326
327if solution.status["success"]:
328    flight_time = solution.status["objective"]
329    print(f"Objective: {flight_time:.3f} seconds")
330
331    # Convert scaled final state to physical for verification
332    X_final = solution["X_scaled"][-1] * POS_SCALE
333    Y_final = solution["Y_scaled"][-1] * POS_SCALE
334    Z_final = solution["Z_scaled"][-1] * POS_SCALE
335    position_error = np.sqrt((X_final - 5.0) ** 2 + (Y_final - 5.0) ** 2 + (Z_final - 5.0) ** 2)
336
337    print(f"Final position: ({X_final:.3f}, {Y_final:.3f}, {Z_final:.3f}) m")
338    print(f"Position error: {position_error:.3f} m")
339
340    # Flight envelope analysis (convert scaled to physical)
341    X_dot_phys = solution["X_dot_scaled"] * VEL_SCALE
342    Y_dot_phys = solution["Y_dot_scaled"] * VEL_SCALE
343    Z_dot_phys = solution["Z_dot_scaled"] * VEL_SCALE
344    max_speed = max(np.sqrt(X_dot_phys**2 + Y_dot_phys**2 + Z_dot_phys**2))
345
346    phi_phys = solution["phi_scaled"] * ANG_SCALE
347    theta_phys = solution["theta_scaled"] * ANG_SCALE
348    max_phi = max(np.abs(phi_phys)) * 180 / np.pi
349    max_theta = max(np.abs(theta_phys)) * 180 / np.pi
350
351    print(f"Maximum speed: {max_speed:.2f} m/s")
352    print(f"Maximum roll angle: {max_phi:.1f}°")
353    print(f"Maximum pitch angle: {max_theta:.1f}°")
354
355    # Motor usage analysis (convert scaled to physical)
356    omega1_phys = solution["omega1_scaled"] * OMEGA_M_SCALE
357    omega2_phys = solution["omega2_scaled"] * OMEGA_M_SCALE
358    omega3_phys = solution["omega3_scaled"] * OMEGA_M_SCALE
359    omega4_phys = solution["omega4_scaled"] * OMEGA_M_SCALE
360
361    avg_motor_speed = (omega1_phys + omega2_phys + omega3_phys + omega4_phys) / 4
362    max_motor_speed = max(np.maximum.reduce([omega1_phys, omega2_phys, omega3_phys, omega4_phys]))
363
364    print(f"Average motor speed: {np.mean(avg_motor_speed):.1f} rad/s")
365    print(f"Maximum motor speed: {max_motor_speed:.1f} rad/s")
366    print(f"Motor utilization: {max_motor_speed / omega_max * 100:.1f}%")
367
368    # Danger zone verification
369    X_traj_phys = solution["X_scaled"] * POS_SCALE
370    Y_traj_phys = solution["Y_scaled"] * POS_SCALE
371    min_distance_to_danger = min(
372        np.sqrt(
373            (X_traj_phys - DANGER_ZONE_CENTER_X) ** 2 + (Y_traj_phys - DANGER_ZONE_CENTER_Y) ** 2
374        )
375    )
376    print(
377        f"Minimum distance to danger zone: {min_distance_to_danger:.3f} m (required: {DANGER_ZONE_RADIUS:.1f} m)"
378    )
379
380    solution.plot()
381
382else:
383    print(f"Optimization failed: {solution.status['message']}")