Auto Park

Highway driving is almost trivial in a planning sense: stay in the lane, never go backwards, useful trajectories live in a narrow envelope. Parking violates all of that.

A car is non-holonomic. Velocity has to point along the heading; you can't strafe. Configuration space is 3D (x, y, θ) but control space is 2D (steering, throttle). Reachable states form a thin curved manifold in SE(2), not a ball. In a tight bay the goal pose lives somewhere no forward-only trajectory can touch. You have to weave and reverse.

Clearances are tight. A standard bay is ~2.5 m wide. The EV is 1.85 m. The lateral margin is roughly the localization error. Oblique detection is ugly. Approach at 30° and the bay opening is foreshortened, the divider lines are half-occluded by parked cars, depth gets noisy at grazing incidence. Reverse is the common case. Production stacks (Apollo, Autoware) treat it as a corner case. For parking it's most of the path. Jerk is rubric-policed. Anything above 2 m/s³ on engagement loses points, so I can't just stomp the brake the moment a path lands.

The "easy for humans, brutal for planners" regime. Everything below is an attempt to make the planner a little less brutalized.

The stack is eight ROS 2 nodes on a Jetson Orin, talking over DDS with intra-process zero-copy where I could get it.

Sensors run at their own cadence. Planner re-plans at 2 Hz unless an obstacle delta exceeds 0.3 m, in which case it preempts. Control is the only thing on a real-time scheduler.

I tried three approaches before fusion stuck.

Pure lidar. Clean geometry, semantically blind. A bay is a flat patch of asphalt; so is a driving lane. I could see that there is room, not that it is a bay.

Pure RGB. Monocular YOLO finds bay lines beautifully in good light but has no metric scale. A 30-pixel line could be 3 m or 12 m away. Useless for planning.

Fusion. YOLOv8-s^[5] on a custom dataset (~14k frames, ~9k synthesized in CARLA with randomized lighting, pavement, and bay orientation). Detector outputs four bay corners plus obstacle boxes. Stereo depth (Semi-Global Matching with lidar as a sparse prior to stabilize textureless asphalt) gives metric z per pixel. Fusion lifts each detection into a 3D polygon and rasterizes everything into a 20 cm occupancy grid with three channels: free, occupied, bay_candidate.

What the planner sees: a costmap and a list of candidate goal poses, one per bay, with confidence and orientation.

Grid A* fails for car-like robots because nodes don't carry heading. You get a path through cells, not a drivable one. The polyline ignores turning radius and demands heading changes the steering rack physically can't do.

Hybrid A*^[1] lets nodes carry continuous state. Each node is (x, y, θ); expansions are short kinematic arcs at a fixed set of steering inputs ({−δ_max, −δ_max/2, 0, +δ_max/2, +δ_max}, forward and reverse). Children land off-grid; the discretization is only used for the closed set. Search is discrete, states are continuous.

The cost at each node:

g(n) is arc length plus penalties (1.5x on reverse, 2.0x on direction changes — flipping gear is what hurts passengers most). h(n) is the larger of two:

h_nh, the non-holonomic-without-obstacles heuristic — length of the shortest Reeds-Shepp^[2] curve to the goal, ignoring the costmap. Captures kinematics. h_ho, the holonomic-with-obstacles heuristic — length of the shortest grid-A* path, ignoring kinematics. Captures geometry. The max stays admissible and dominates either alone. Straight out of Dolgov et al.; I didn't invent it, just verified it works.

Near the goal, expanding kinematic arcs gets wasteful — you almost never land on the goal pose exactly. Instead I shoot a Reeds-Shepp analytic expansion directly to the goal and accept it as the tail if it's collision-free. Typical searches close 1,200 to 4,000 nodes for a parallel park versus 40k+ without.

The tracker is intentionally boring. Three PIDs at 50 Hz.

Cross-track drives lateral error e_y to zero, summed with a feedforward steering command from path curvature. Heading drives heading error e_ψ to zero; output sums into the same steering command. Longitudinal drives speed error to zero, with the setpoint from a precomputed velocity profile (slower at high curvature, zero at direction-change cusps).

Cross-track error against the closest path point:

n̂ is the left-hand normal so sign carries which side of the path we're on.

The tuning story. Ziegler-Nichols on a flat straight gave me K_p = 0.8, K_i = 0.05, K_d = 0.12. Forward looked great. The moment I asked it to track a reverse arc it oscillated. Twice I almost blamed the planner before I noticed: the cross-track sign convention flips in reverse (the front axle becomes the "back" of the kinematic model). ZN had tuned for a regime that no longer applied.

Hand-tuned a second set for reverse (K_p = 0.45, K_i = 0.02, K_d = 0.18 — softer P, more D for damping) and switch on commanded gear. ZN is great when your plant is stationary; for parking the plant changes character every shift.

Built a scenario harness on CARLA 0.9.15^[4] with a small YAML DSL.

# scenarios/oblique_30deg_wet_pedestrian.yaml
map: Town04
weather:
  precipitation: 0.6
  road_wetness: 0.8
  cloudiness: 0.7
ego:
  spawn: {x: 142.3, y: -88.1, yaw: 30.0}
  initial_speed: 2.0     # m/s, rolling start
target_bay:
  id: "bay_07"
  occupancy: empty
  neighbors: [occupied, occupied]
actors:
  - type: pedestrian
    spawn: {x: 138.0, y: -90.5}
    behavior: cross_at_t=4.0s
  - type: vehicle
    spawn: {x: 130.0, y: -85.0}
    behavior: static
metrics:
  - final_pose_error
  - max_lateral_deviation
  - max_jerk
  - planning_latency_p99

Across 52 scenarios — bay angles of 0, 15, 30, 45°, parallel and perpendicular, wet and dry, three pedestrian profiles — I got 8.1 cm RMSE on final pose translation and 2.3° on heading. p99 planning latency 187 ms, control jitter under 2 ms.

Honest caveat: the 8 cm is the mean across successful completions. Three scenarios timed out, all at 45° approach on a wet shoulder. Not in the average. Fixing them before competition.

Most useful thing I learned, and it has nothing to do with algorithms.

Reverse-into-bay maneuvers kept failing at the end. Car would get 80% in, planner would emit an infeasible path, tracker would freeze or back out. I spent a week on Hybrid A* expansion granularity, a week on the cost function, a week suspecting localization drift.

The bug was in perception. Once the car was deep in the bay, the stereo cameras (A-pillar mounted, forward-biased FoV) lost sight of the rear and side markings. Fusion dutifully marked the bay low confidence. The planner — which I'd set to re-plan on every confidence drop — would invalidate the in-flight path and search from a mostly-uninformed costmap. It usually found something worse.

The fix was not algorithmic. I added a commit point: once the planner is within 1.5 m of the goal with heading error below 10°, the path is frozen and the tracker runs it open-loop, with lidar emergency-stop as the only override. An ugly finite-state-machine hack. Works perfectly.

Three things on the roadmap.

Real sensor noise. CARLA stereo is too clean. Collecting ZED 2i data in the UWAFT lot to retrain the YOLO head and resample the depth-prior noise.

Latency budget on Orin. Fusion is 28 ms p50 on a desktop RTX 3070. I have 80 ms total at 10 Hz, which means TensorRT INT8 with QAT on the YOLO backbone and rewriting the rasterizer in CUDA.

Snow. Competition is in May, so probably fine, but the parking-line detector falls to 0.4 mAP under partial cover. Future me's problem.

Full stack runs at the SAE Autonomous Challenge in May 2027. I'll write a follow-up once we have real numbers — I expect 8 cm to roughly double once sensor noise, suspension compliance, and tire slip get involved. Still inside the 25 cm rubric.