---
id: wiki-2026-0508-autonomous-vehicle-path-planning
title: Autonomous Vehicle Path Planning
category: 10_Wiki/Topics
status: verified
canonical_id: self
aliases: [AV Path Planning, Self-Driving Planning, Motion Planning]
duplicate_of: none
source_trust_level: A
confidence_score: 0.94
verification_status: applied
tags: [robotics, autonomous-driving, motion-planning, mpc, av]
raw_sources: []
last_reinforced: 2026-05-10
github_commit: pending
tech_stack:
  language: Python/C++
  framework: Apollo/Autoware/OpenPlanner
---

# Autonomous Vehicle Path Planning

## 매 한 줄
> **"매 perception 의 X — 매 prediction + decision + trajectory 의 closed-loop."**. AV path planning 의 perception output (objects, lanes, drivable area) → prediction (other agents) → behavior decision (lane change, yield) → trajectory (smooth, kinodynamic) → control. 매 2026 의 Tesla FSD v13 (end-to-end NN), Waymo (modular), Wayve LINGO (VLM-based), 모두 의 hybrid trend.

## 매 핵심

### 매 Architecture (Modular)
1. **Mission planner**: route (A→B) over road graph.
2. **Behavior planner**: discrete decision (FSM / POMDP / RL).
3. **Local planner / motion**: collision-free trajectory (Frenet, lattice, sampling, optimization).
4. **Trajectory tracker**: MPC / pure pursuit → steering + throttle.

### 매 Algorithms
- **Search**: A*, Hybrid A* (kinematic), RRT*, RRT-Connect.
- **Sampling**: lattice planner (predefined motion primitives).
- **Optimization**: iLQR, MPC, CILQR (cost = comfort + safety + progress).
- **Frenet frame**: lateral + longitudinal decoupling.
- **Learning-based**: ChauffeurNet, MotionLM, end-to-end (Tesla FSD v13).
- **Foundation model**: Wayve LINGO-2 / GAIA-2 — VLM + driving.

### 매 Safety
- ISO 26262 / ISO 21448 (SOTIF).
- RSS (Responsibility-Sensitive Safety, Mobileye).
- Formal verification of decision layer.
- Out-of-distribution detection.

### 매 응용
1. L4 robotaxi (Waymo, Cruise relaunch, Apollo Go).
2. L2+ ADAS (Tesla FSD, BYD, NIO Pilot).
3. Truck platooning (Aurora, Plus).
4. Last-mile delivery (Nuro).

## 💻 패턴

### Pattern 1 — Frenet trajectory generation

```python
import numpy as np

def frenet_quintic(s0, sd0, sdd0, s1, sd1, sdd1, T):
    # solve quintic polynomial coeffs for s(t)
    A = np.array([[T**3, T**4, T**5],
                  [3*T**2, 4*T**3, 5*T**4],
                  [6*T, 12*T**2, 20*T**3]])
    b = np.array([s1 - s0 - sd0*T - 0.5*sdd0*T*T,
                  sd1 - sd0 - sdd0*T,
                  sdd1 - sdd0])
    a3, a4, a5 = np.linalg.solve(A, b)
    return [s0, sd0, sdd0/2, a3, a4, a5]
```

### Pattern 2 — Hybrid A* (sketch)

```python
def hybrid_a_star(start, goal, grid, motion_primitives):
    open_set = PriorityQueue()
    open_set.put((0, start))
    came_from = {}
    g = {start: 0}
    while not open_set.empty():
        _, cur = open_set.get()
        if reached(cur, goal): return reconstruct(came_from, cur)
        for prim in motion_primitives:
            nxt = apply(cur, prim)
            if collides(nxt, grid): continue
            new_g = g[cur] + prim.cost
            if new_g < g.get(nxt, 1e18):
                g[nxt] = new_g
                f = new_g + reeds_shepp_heuristic(nxt, goal)
                open_set.put((f, nxt))
                came_from[nxt] = (cur, prim)
```

### Pattern 3 — MPC trajectory tracking (acados / casadi)

```python
import casadi as ca
N = 20  # horizon
dt = 0.1
opti = ca.Opti()
X = opti.variable(4, N+1)   # [x, y, theta, v]
U = opti.variable(2, N)     # [steer, accel]
cost = 0
for k in range(N):
    cost += ca.sumsqr(X[:2, k] - ref[:2, k]) + 0.1 * ca.sumsqr(U[:, k])
    opti.subject_to(X[:, k+1] == bicycle_model(X[:, k], U[:, k], dt))
opti.minimize(cost)
opti.solver("ipopt")
sol = opti.solve()
```

### Pattern 4 — RSS (longitudinal safe distance)

```python
def rss_safe_distance(v_rear, v_front, a_max_accel, a_max_brake, a_min_brake, rho=0.1):
    return max(0,
        v_rear * rho
        + 0.5 * a_max_accel * rho**2
        + (v_rear + a_max_accel * rho)**2 / (2 * a_min_brake)
        - v_front**2 / (2 * a_max_brake))
```

### Pattern 5 — Behavior FSM

```python
class State(str, Enum): KEEP="keep"; LEFT="left"; RIGHT="right"; STOP="stop"

def transition(s, perception):
    if perception.front_blocked and perception.left_clear: return State.LEFT
    if perception.red_light: return State.STOP
    return State.KEEP
```

## 매 결정 기준

| 상황 | Approach |
|---|---|
| Highway lane change | Frenet + lattice + MPC |
| Parking | Hybrid A* + Reeds-Shepp |
| Urban intersection | POMDP / behavior tree + RSS check |
| Off-road / unstructured | RRT* + sampling MPC |
| End-to-end product (Tesla) | NN policy + safety guard |

**기본값**: Frenet planning + MPC tracking + RSS safety check + rule-based behavior FSM.

## 🔗 Graph
- 부모: [[Robotics]] · [[Motion-Planning]] · [[Optimal-Control-Theory]]
- 응용: [[Robotaxi]]
- Adjacent: [[Kalman-Filter-and-State-Tracking|Kalman-Filter]]

## 🤖 LLM 활용
**언제**: scenario synthesis (corner cases), behavior reasoning prototype (LINGO-style), code generation for ROS / Apollo modules, log triage.
**언제 X**: real-time control loop (latency, safety cert), final RSS verification (formal methods).

## ❌ 안티패턴
- **Greedy lane change**: no comfort cost → jerky.
- **No prediction uncertainty**: 매 single mode 의 future — multi-modal essential.
- **Skipping kinodynamic check**: A* path 의 robot 의 unfollowable.
- **End-to-end without guard**: NN failure mode → safety violation.

## 🧪 검증 / 중복
- Verified (Apollo, Autoware open-source, Mobileye RSS paper, Waymo safety report).
- 신뢰도 A.

## 🕓 Changelog
| 날짜 | 변경 |
|---|---|
| 2026-05-08 | Phase 1 |
| 2026-05-10 | Manual cleanup — FULL content (Frenet, Hybrid A*, MPC, RSS) |