Robust Street Overtaking#

Mathematical Formulation#

Problem Statement#

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

\[J = \frac{t_f}{t_{\text{scale}}} + 0.02 \int_0^{t_f} \left[\left(\frac{a}{a_{\text{scale}}}\right)^2 + \left(\frac{\delta}{\delta_{\text{scale}}}\right)^2\right] 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_{\text{safe}}} - \delta\]
\[\alpha_r = \frac{v - l_r \omega}{u_{\text{safe}}}\]
\[F_{Y1} = -k_f \alpha_f\]
\[F_{Y2} = -k_r \alpha_r\]
\[u_{\text{safe}} = \max(u, u_{\min})\]

And dual collision avoidance constraints:

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

Street Geometry#

  • Street boundaries: \(x \in [-2.0, 20.0]\) m, \(y \in [-10.0, 50.0]\) m

  • Lane width: \(w_{\text{lane}} = 3.6\) m

  • Street center: \(x_{\text{center}} = 9.0\) m

  • Right lane center: \(x_{\text{right}} = 10.8\) m

  • Left lane center: \(x_{\text{left}} = 7.2\) m

Boundary Conditions#

  • Initial conditions: \(x(0) = 10.8\), \(y(0) = 0.0\), \(\phi(0) = \frac{\pi}{2}\), \(u(0) = 12.0\), \(v(0) = 0\), \(\omega(0) = 0\)

  • Final conditions: \(x(t_f) = 10.8\), \(y(t_f) = 50.0\), \(\phi(t_f) = \frac{\pi}{2}\), others free

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

  • State bounds: \(0.5 \leq u \leq 30\) m/s, \(-5 \leq v \leq 5\) m/s, \(-2.5 \leq \omega \leq 2.5\) rad/s

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 = 1.5\) m

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

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

Scaling Parameters#

  • Time scale: \(t_{\text{scale}} = 5.0\) s

  • Acceleration scale: \(a_{\text{scale}} = 10.0\) m/s²

  • Steering scale: \(\delta_{\text{scale}} = 0.3\) rad

Obstacle Trajectories#

Obstacle 1 (Right lane, moving forward):

  • \((10.8, 15.0)\) at \(t = 0\) s

  • \((10.8, 40.0)\) at \(t = 5\) s

Obstacle 2 (Left lane, moving backward):

  • \((7.2, 55.0)\) at \(t = 0\) s

  • \((7.2, 0.0)\) at \(t = 15\) s

Linear interpolation with time clamping is used between waypoints.

State Variables#

  • \(x(t)\): Lateral position (m)

  • \(y(t)\): Longitudinal 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)

Mesh Configuration#

The problem uses an adaptive mesh with 11 polynomial segments:

  • Polynomial degrees: \([4, 4, 4, 6, 7, 8, 6, 5, 4, 4, 4]\)

  • Mesh nodes: \([-1.0, -0.7, -0.4, -0.2, -0.1, 0.0, 0.2, 0.4, 0.6, 0.8, 0.9, 1.0]\)

This provides higher resolution during critical overtaking phases.

Notes#

This problem demonstrates complex street overtaking with two moving obstacles in opposing directions. The vehicle must coordinate lane changes while avoiding both a forward-moving obstacle in the right lane and a backward-moving obstacle in the left lane. The formulation includes realistic vehicle dynamics, street constraints, and collision avoidance with safety margins.

Running This Example#

cd examples/overtaking_maneuver
python overtaking_maneuver.py
python overtaking_maneuver_animate.py

Code Implementation#

examples/overtaking_maneuver/overtaking_maneuver.py#
  1import casadi as ca
  2import numpy as np
  3from scipy.interpolate import CubicSpline
  4
  5import maptor as mtor
  6
  7
  8# ============================================================================
  9# Street Geometry
 10# ============================================================================
 11
 12STREET_LEFT_BOUNDARY = -2.0
 13STREET_RIGHT_BOUNDARY = 20.0
 14STREET_BOTTOM = -10.0
 15STREET_TOP = 50.0
 16
 17LANE_WIDTH = 3.6
 18STREET_CENTER = (STREET_LEFT_BOUNDARY + STREET_RIGHT_BOUNDARY) / 2
 19RIGHT_LANE_CENTER = STREET_CENTER + LANE_WIDTH / 2
 20LEFT_LANE_CENTER = STREET_CENTER - LANE_WIDTH / 2
 21
 22
 23# ============================================================================
 24# Obstacle Trajectories
 25# ============================================================================
 26
 27AGENT_START = (RIGHT_LANE_CENTER, 0.0)
 28AGENT_END = (RIGHT_LANE_CENTER, 50.0)
 29
 30OBSTACLE_1_WAYPOINTS = np.array(
 31    [
 32        [RIGHT_LANE_CENTER, 15.0, 0.0],
 33        [RIGHT_LANE_CENTER, 40.0, 5.0],
 34    ]
 35)
 36
 37OBSTACLE_2_WAYPOINTS = np.array(
 38    [
 39        [LEFT_LANE_CENTER, 55.0, 0.0],
 40        [LEFT_LANE_CENTER, 0.0, 15.0],
 41    ]
 42)
 43
 44# Create interpolants for obstacle motion
 45_times_1 = OBSTACLE_1_WAYPOINTS[:, 2]
 46_x_coords_1 = OBSTACLE_1_WAYPOINTS[:, 0]
 47_y_coords_1 = OBSTACLE_1_WAYPOINTS[:, 1]
 48
 49_times_2 = OBSTACLE_2_WAYPOINTS[:, 2]
 50_x_coords_2 = OBSTACLE_2_WAYPOINTS[:, 0]
 51_y_coords_2 = OBSTACLE_2_WAYPOINTS[:, 1]
 52
 53_x_interpolant_1 = ca.interpolant("obs_x_interp_1", "linear", [_times_1], _x_coords_1)
 54_y_interpolant_1 = ca.interpolant("obs_y_interp_1", "linear", [_times_1], _y_coords_1)
 55_x_interpolant_2 = ca.interpolant("obs_x_interp_2", "linear", [_times_2], _x_coords_2)
 56_y_interpolant_2 = ca.interpolant("obs_y_interp_2", "linear", [_times_2], _y_coords_2)
 57
 58
 59def obstacle_1_position(current_time):
 60    """Get first obstacle position with clamped interpolation."""
 61    t_clamped = ca.fmax(_times_1[0], ca.fmin(_times_1[-1], current_time))
 62    return _x_interpolant_1(t_clamped), _y_interpolant_1(t_clamped)
 63
 64
 65def obstacle_2_position(current_time):
 66    """Get second obstacle position with clamped interpolation."""
 67    t_clamped = ca.fmax(_times_2[0], ca.fmin(_times_2[-1], current_time))
 68    return _x_interpolant_2(t_clamped), _y_interpolant_2(t_clamped)
 69
 70
 71# ============================================================================
 72# Vehicle Parameters
 73# ============================================================================
 74
 75m = 1412.0
 76I_z = 1536.7
 77l_f = 1.06
 78l_r = 1.85
 79k_f = 128916.0
 80k_r = 85944.0
 81
 82vehicle_radius = 1.5
 83obstacle_radius = 1.5
 84min_separation = vehicle_radius + obstacle_radius + 0.5
 85u_min = 0.5
 86
 87
 88# ============================================================================
 89# Problem Setup
 90# ============================================================================
 91
 92problem = mtor.Problem("Robust Street Overtaking")
 93phase = problem.set_phase(1)
 94
 95
 96# ============================================================================
 97# Variables
 98# ============================================================================
 99
100t = phase.time(initial=0.0)
101
102x = phase.state("x_position", initial=AGENT_START[0], final=AGENT_END[0])
103y = phase.state("y_position", initial=AGENT_START[1], final=AGENT_END[1])
104phi = phase.state("heading", initial=np.pi / 2, final=np.pi / 2)
105
106u = phase.state("longitudinal_velocity", initial=12.0, boundary=(u_min, 30.0))
107v = phase.state("lateral_velocity", initial=0.0, boundary=(-5.0, 5.0))
108omega = phase.state("yaw_rate", initial=0.0, boundary=(-2.5, 2.5))
109
110a = phase.control("acceleration", boundary=(-6.0, 6.0))
111delta = phase.control("steering_angle", boundary=(-0.4, 0.4))
112
113
114# ============================================================================
115# Dynamics
116# ============================================================================
117
118u_safe = ca.if_else(u < u_min, u_min, u)
119
120alpha_f = (v + l_f * omega) / u_safe - delta
121alpha_r = (v - l_r * omega) / u_safe
122
123F_Y1 = -k_f * alpha_f
124F_Y2 = -k_r * alpha_r
125
126phase.dynamics(
127    {
128        x: u * ca.cos(phi) - v * ca.sin(phi),
129        y: u * ca.sin(phi) + v * ca.cos(phi),
130        phi: omega,
131        u: a + v * omega - (F_Y1 * ca.sin(delta)) / m,
132        v: -u * omega + (F_Y1 * ca.cos(delta) + F_Y2) / m,
133        omega: (l_f * F_Y1 * ca.cos(delta) - l_r * F_Y2) / I_z,
134    }
135)
136
137
138# ============================================================================
139# Constraints
140# ============================================================================
141
142# Collision avoidance
143obs_x_1, obs_y_1 = obstacle_1_position(t)
144obs_x_2, obs_y_2 = obstacle_2_position(t)
145
146distance_squared_1 = (x - obs_x_1) ** 2 + (y - obs_y_1) ** 2
147distance_squared_2 = (x - obs_x_2) ** 2 + (y - obs_y_2) ** 2
148
149phase.path_constraints(
150    distance_squared_1 >= min_separation**2 - 1.0,
151    distance_squared_2 >= min_separation**2 - 1.0,
152)
153
154# Workspace bounds
155phase.path_constraints(
156    x >= STREET_LEFT_BOUNDARY,
157    x <= STREET_RIGHT_BOUNDARY,
158    y >= STREET_BOTTOM,
159    y <= STREET_TOP,
160)
161
162
163# ============================================================================
164# Objective
165# ============================================================================
166
167time_scale = 5.0
168control_scale = 10.0
169
170normalized_time = t.final / time_scale
171normalized_control = phase.add_integral((a / control_scale) ** 2 + (delta / 0.3) ** 2)
172
173problem.minimize(normalized_time + 0.02 * normalized_control)
174
175
176# ============================================================================
177# Mesh Configuration
178# ============================================================================
179
180POLYNOMIAL_DEGREES = [4, 4, 4, 6, 7, 8, 6, 5, 4, 4, 4]
181
182MESH_NODES = [-1.0, -0.7, -0.4, -0.2, -0.1, 0.0, 0.2, 0.4, 0.6, 0.8, 0.9, 1.0]
183
184phase.mesh(POLYNOMIAL_DEGREES, MESH_NODES)
185
186
187# ============================================================================
188# Initial Guess Generation
189# ============================================================================
190
191
192def _generate_robust_initial_guess():
193    """Generate weight-insensitive initial guess with conservative timing."""
194    waypoints_t = np.array([0.0, 0.2, 0.35, 0.5, 0.65, 0.8, 1.0])
195
196    waypoints_x = np.array(
197        [
198            RIGHT_LANE_CENTER,
199            RIGHT_LANE_CENTER,
200            (RIGHT_LANE_CENTER + LEFT_LANE_CENTER) / 2,
201            LEFT_LANE_CENTER,
202            (RIGHT_LANE_CENTER + LEFT_LANE_CENTER) / 2,
203            RIGHT_LANE_CENTER,
204            RIGHT_LANE_CENTER,
205        ]
206    )
207
208    waypoints_y = np.array([0.0, 10.0, 20.0, 30.0, 40.0, 45.0, 50.0])
209    waypoints_phi = np.array(
210        [np.pi / 2, np.pi / 2, np.pi / 2 + 0.1, np.pi / 2, np.pi / 2 - 0.1, np.pi / 2, np.pi / 2]
211    )
212    waypoints_u = np.array([12.0, 14.0, 16.0, 18.0, 16.0, 14.0, 12.0])
213
214    spline_x = CubicSpline(waypoints_t, waypoints_x, bc_type="natural")
215    spline_y = CubicSpline(waypoints_t, waypoints_y, bc_type="natural")
216    spline_phi = CubicSpline(waypoints_t, waypoints_phi, bc_type="natural")
217    spline_u = CubicSpline(waypoints_t, waypoints_u, bc_type="natural")
218
219    states_guess = []
220    controls_guess = []
221
222    for N in POLYNOMIAL_DEGREES:
223        tau = np.linspace(-1, 1, N + 1)
224        t_norm = (tau + 1) / 2
225
226        x_vals = spline_x(t_norm)
227        y_vals = spline_y(t_norm)
228        phi_vals = spline_phi(t_norm)
229        u_vals = spline_u(t_norm)
230
231        v_vals = np.gradient(x_vals) * 1.5
232        omega_vals = np.gradient(phi_vals) * 1.0
233
234        v_vals = np.clip(v_vals, -10.0, 10.0)
235        omega_vals = np.clip(omega_vals, -2.0, 2.0)
236
237        states_guess.append(np.vstack([x_vals, y_vals, phi_vals, u_vals, v_vals, omega_vals]))
238
239        a_vals = np.gradient(u_vals)[:N] * 1.0
240        delta_vals = np.gradient(phi_vals)[:N] * 2.0
241
242        a_vals = np.clip(a_vals, -4.0, 4.0)
243        delta_vals = np.clip(delta_vals, -0.3, 0.3)
244
245        controls_guess.append(np.vstack([a_vals, delta_vals]))
246
247    return states_guess, controls_guess
248
249
250# ============================================================================
251# Initial Guess and Solve
252# ============================================================================
253
254states_guess, controls_guess = _generate_robust_initial_guess()
255
256phase.guess(
257    states=states_guess,
258    controls=controls_guess,
259    terminal_time=5.0,
260)
261
262solution = mtor.solve_adaptive(
263    problem,
264    error_tolerance=1e-3,
265    max_iterations=15,
266    min_polynomial_degree=3,
267    max_polynomial_degree=8,
268    ode_solver_tolerance=1e-1,
269    nlp_options={
270        "ipopt.max_iter": 1000,
271        "ipopt.tol": 1e-6,
272        "ipopt.constr_viol_tol": 1e-4,
273        "ipopt.linear_solver": "mumps",
274        "ipopt.print_level": 3,
275        "ipopt.mu_strategy": "adaptive",
276        "ipopt.acceptable_tol": 1e-4,
277        "ipopt.acceptable_iter": 5,
278    },
279)
280
281
282# ============================================================================
283# Results
284# ============================================================================
285
286if solution.status["success"]:
287    print(f"  Objective: {solution.status['objective']:.6f}")
288    print(f"  Mission time: {solution.status['total_mission_time']:.3f} seconds")
289
290    x_traj = solution["x_position"]
291    y_traj = solution["y_position"]
292    u_traj = solution["longitudinal_velocity"]
293    v_traj = solution["lateral_velocity"]
294
295    max_lateral_deviation = abs(x_traj - RIGHT_LANE_CENTER).max()
296    max_speed = u_traj.max()
297    max_lateral_velocity = abs(v_traj).max()
298
299    print(f"  Max lateral deviation: {max_lateral_deviation:.2f} m")
300    print(f"  Max speed: {max_speed:.1f} m/s")
301    print(f"  Max lateral velocity: {max_lateral_velocity:.2f} m/s")
302    print(f"  Successful overtaking: {'✓' if max_lateral_deviation > 3.0 else '✗'}")
303
304    x_final = solution[(1, "x_position")][-1]
305    y_final = solution[(1, "y_position")][-1]
306    position_error = np.sqrt((x_final - AGENT_END[0]) ** 2 + (y_final - AGENT_END[1]) ** 2)
307
308    print("\nFinal Position:")
309    print(f"  Reached: ({x_final:.2f}, {y_final:.2f}) m")
310    print(f"  Target: ({AGENT_END[0]:.2f}, {AGENT_END[1]:.2f}) m")
311    print(f"  Error: {position_error:.3f} m")
312
313    solution.plot()
314
315else:
316    print(f"✗ Optimization failed: {solution.status['message']}")
317
318
319# ============================================================================
320# Export for Animation
321# ============================================================================
322
323__all__ = [
324    "AGENT_END",
325    "AGENT_START",
326    "LANE_WIDTH",
327    "LEFT_LANE_CENTER",
328    "OBSTACLE_1_WAYPOINTS",
329    "OBSTACLE_2_WAYPOINTS",
330    "RIGHT_LANE_CENTER",
331    "STREET_BOTTOM",
332    "STREET_CENTER",
333    "STREET_LEFT_BOUNDARY",
334    "STREET_RIGHT_BOUNDARY",
335    "STREET_TOP",
336    "solution",
337]