Dynamic Obstacle Avoidance#

Mathematical Formulation#

Problem Statement#

Find the optimal control inputs \(a(t)\) and \(\delta(t)\) that minimize the transit time with control effort penalty:

\[J = t_f + 0.01 \int_0^{t_f} (a^2 + \delta^2) dt\]

Subject to the bicycle model dynamics:

\[\frac{dx}{dt} = u \cos\phi - v \sin\phi\]
\[\frac{dy}{dt} = u \sin\phi + v \cos\phi\]
\[\frac{d\phi}{dt} = \omega\]
\[\frac{du}{dt} = a + v \omega - \frac{F_{Y1} \sin\delta}{m}\]
\[\frac{dv}{dt} = -u \omega + \frac{F_{Y1} \cos\delta + F_{Y2}}{m}\]
\[\frac{d\omega}{dt} = \frac{l_f F_{Y1} \cos\delta - l_r F_{Y2}}{I_z}\]

Where the tire forces are:

\[\alpha_f = \frac{v + l_f \omega}{u} - \delta\]
\[\alpha_r = \frac{v - l_r \omega}{u}\]
\[F_{Y1} = -k_f \alpha_f\]
\[F_{Y2} = -k_r \alpha_r\]

And the collision avoidance constraint:

\[(x - x_{\text{obs}}(t))^2 + (y - y_{\text{obs}}(t))^2 \geq d_{\min}^2\]

Boundary Conditions#

  • Initial conditions: \(x(0) = 0\), \(y(0) = 0\), \(\phi(0) = \frac{\pi}{3}\), \(u(0) = 5.0\), \(v(0) = 0\), \(\omega(0) = 0\)

  • Final conditions: \(x(t_f) = 20\), \(y(t_f) = 20\), others free

  • Control bounds: \(-8.0 \leq a \leq 8.0\) m/s², \(-0.5 \leq \delta \leq 0.5\) rad

  • State bounds: \(0.05 \leq u \leq 20\) m/s, \(-15 \leq v \leq 15\) m/s, \(-3 \leq \omega \leq 3\) rad/s, \(-40 \leq x,y \leq 40\) m

Physical Parameters#

  • Vehicle mass: \(m = 1412\) kg

  • Yaw inertia: \(I_z = 1536.7\) kg⋅m²

  • Front axle distance: \(l_f = 1.06\) m

  • Rear axle distance: \(l_r = 1.85\) m

  • Front cornering stiffness: \(k_f = 128916\) N/rad

  • Rear cornering stiffness: \(k_r = 85944\) N/rad

  • Vehicle radius: \(r_v = 1.5\) m

  • Obstacle radius: \(r_o = 2.5\) m

  • Minimum separation: \(d_{\min} = r_v + r_o = 4.0\) m

  • Minimum velocity: \(u_{\min} = 0.05\) m/s

Obstacle Trajectory#

The obstacle follows a predetermined path with waypoints:

  • \((5.0, 5.0)\) at \(t = 0\) s

  • \((12.0, 12.0)\) at \(t = 3\) s

  • \((15.0, 15.0)\) at \(t = 6\) s

  • \((20.0, 20.0)\) at \(t = 12\) s

Linear interpolation is used between waypoints with time clamping at boundaries.

State Variables#

  • \(x(t)\): Vehicle x-position (m)

  • \(y(t)\): Vehicle y-position (m)

  • \(\phi(t)\): Vehicle heading angle (rad)

  • \(u(t)\): Longitudinal velocity (m/s)

  • \(v(t)\): Lateral velocity (m/s)

  • \(\omega(t)\): Yaw rate (rad/s)

Control Variables#

  • \(a(t)\): Longitudinal acceleration (m/s²)

  • \(\delta(t)\): Front wheel steering angle (rad)

Notes#

This problem demonstrates collision avoidance with a moving obstacle using a full dynamic bicycle model.

Running This Example#

cd examples/dynamic_obstacle_avoidance
python dynamic_obstacle_avoidance.py
python dynamic_obstacle_avoidance_animate.py

Code Implementation#

examples/dynamic_obstacle_avoidance/dynamic_obstacle_avoidance.py#
  1import casadi as ca
  2import numpy as np
  3
  4import maptor as mtor
  5
  6
  7# Obstacle trajectory definition
  8OBSTACLE_WAYPOINTS = np.array(
  9    [
 10        [5.0, 5.0, 0.0],
 11        [12.0, 12.0, 3.0],
 12        [15.0, 15.0, 6.0],
 13        [20.0, 20.0, 12.0],
 14    ]
 15)
 16
 17# Create module-level interpolants for obstacle trajectory
 18_times = OBSTACLE_WAYPOINTS[:, 2]
 19_x_coords = OBSTACLE_WAYPOINTS[:, 0]
 20_y_coords = OBSTACLE_WAYPOINTS[:, 1]
 21
 22_x_interpolant = ca.interpolant("obs_x_interp", "linear", [_times], _x_coords)
 23_y_interpolant = ca.interpolant("obs_y_interp", "linear", [_times], _y_coords)
 24
 25
 26def obstacle_position(current_time):
 27    """Get obstacle position at given time with clamped interpolation."""
 28    t_clamped = ca.fmax(_times[0], ca.fmin(_times[-1], current_time))
 29    return _x_interpolant(t_clamped), _y_interpolant(t_clamped)
 30
 31
 32# Vehicle physical parameters
 33m = 1412.0
 34I_z = 1536.7
 35l_f = 1.06
 36l_r = 1.85
 37k_f = 128916.0
 38k_r = 85944.0
 39
 40# Safety parameters
 41vehicle_radius = 1.5
 42obstacle_radius = 2.5
 43min_separation = vehicle_radius + obstacle_radius
 44u_min = 0.05
 45
 46# Problem setup
 47problem = mtor.Problem("Dynamic Bicycle Model - Forward Motion Only")
 48phase = problem.set_phase(1)
 49
 50# Variables
 51t = phase.time(initial=0.0)
 52x = phase.state("x_position", initial=0.0, final=20.0)
 53y = phase.state("y_position", initial=0.0, final=20.0)
 54phi = phase.state("heading", initial=np.pi / 3)
 55u = phase.state("longitudinal_velocity", initial=5.0, boundary=(u_min, 20.0))
 56v = phase.state("lateral_velocity", initial=0.0, boundary=(-15.0, 15.0))
 57omega = phase.state("yaw_rate", initial=0.0, boundary=(-3.0, 3.0))
 58a = phase.control("acceleration", boundary=(-8.0, 8.0))
 59delta = phase.control("steering_angle", boundary=(-0.5, 0.5))
 60
 61# Dynamics
 62u_safe = ca.if_else(u < u_min, u_min, u)
 63alpha_f = (v + l_f * omega) / u_safe - delta
 64alpha_r = (v - l_r * omega) / u_safe
 65F_Y1 = -k_f * alpha_f
 66F_Y2 = -k_r * alpha_r
 67
 68phase.dynamics(
 69    {
 70        x: u * ca.cos(phi) - v * ca.sin(phi),
 71        y: u * ca.sin(phi) + v * ca.cos(phi),
 72        phi: omega,
 73        u: a + v * omega - (F_Y1 * ca.sin(delta)) / m,
 74        v: -u * omega + (F_Y1 * ca.cos(delta) + F_Y2) / m,
 75        omega: (l_f * F_Y1 * ca.cos(delta) - l_r * F_Y2) / I_z,
 76    }
 77)
 78
 79# Constraints
 80obs_x, obs_y = obstacle_position(t)
 81distance_squared = (x - obs_x) ** 2 + (y - obs_y) ** 2
 82
 83phase.path_constraints(
 84    distance_squared >= min_separation**2,
 85    x >= -40.0,
 86    x <= 40.0,
 87    y >= -40.0,
 88    y <= 40.0,
 89)
 90
 91# Objective
 92control_effort = phase.add_integral(a**2 + delta**2)
 93problem.minimize(t.final + 0.01 * control_effort)
 94
 95# Mesh and guess
 96phase.mesh([8, 8, 8], [-1.0, -1 / 3, 1 / 3, 1.0])
 97
 98# Solve
 99solution = mtor.solve_adaptive(
100    problem,
101    error_tolerance=1e-1,
102    max_iterations=30,
103    min_polynomial_degree=3,
104    max_polynomial_degree=10,
105    ode_solver_tolerance=1e-1,
106    nlp_options={
107        "ipopt.max_iter": 2000,
108        "ipopt.tol": 1e-8,
109        "ipopt.constr_viol_tol": 1e-4,
110        "ipopt.linear_solver": "mumps",
111        "ipopt.print_level": 5,
112    },
113)
114
115# Results
116if solution.status["success"]:
117    print(f"Forward-only model objective: {solution.status['objective']:.6f}")
118    print(f"Mission time: {solution.status['total_mission_time']:.6f} seconds")
119
120    x_final = solution[(1, "x_position")][-1]
121    y_final = solution[(1, "y_position")][-1]
122    u_final = solution[(1, "longitudinal_velocity")][-1]
123    v_final = solution[(1, "lateral_velocity")][-1]
124    omega_final = solution[(1, "yaw_rate")][-1]
125
126    print("Final state verification:")
127    print(f"  Position: ({x_final:.2f}, {y_final:.2f}) m")
128    print("  Target position: (20.0, 20.0) m")
129    print(f"  Position error: {np.sqrt((x_final - 20.0) ** 2 + (y_final - 20.0) ** 2):.3f} m")
130    print(f"  Final velocities: u = {u_final:.2f} m/s, v = {v_final:.2f} m/s")
131    print(f"  Final yaw rate: ω = {omega_final:.2f} rad/s")
132
133    solution.plot()
134
135else:
136    print(f"Optimization failed: {solution.status['message']}")