---

《自動駕駛軌跡規劃實戰：Lattice Planner實現避障路徑生成（附可運行Python代碼）》
—— 零基礎實現基于離散優化的避障路徑規劃

---

一、為什么Lattice Planner成為自動駕駛的核心算法？

在自動駕駛的路徑規劃領域，Lattice Planner（格子規劃器）憑借其結構化采樣+動態優化的特性，成為Waymo、Aptiv等頭部企業的核心算法。與傳統A*、RRT算法不同，Lattice Planner通過橫向位移采樣+縱向速度規劃的復合策略，能夠在10ms內生成滿足車輛運動學約束的平滑軌跡。本文將手把手實現一個簡化版Lattice Planner，并針對經典“避障場景”進行代碼級解析。

---

二、算法原理精講（附數學推導圖示）

2.1 軌跡生成三要素

s(t) = s_0 + v_0 t + \frac{1}{2}a t^2 \quad \text{(縱向運動方程)}

d(s) = c_0 + c_1 s + c_2 s^2 + c_3 s^3 \quad \text{(橫向多項式擬合)}

2.2 成本函數設計

評價維度	權重系數	計算公式
舒適性	0.4	∑(jerk2)
安全性	0.3	障礙物距離倒數
路徑貼合度	0.2	參考線偏移量
能耗效率	0.1	速度變化率

---

三、Python代碼實現（可直接運行）

3.1 環境初始化

import numpy as np
import matplotlib.pyplot as pltclass Vehicle:def __init__(self):self.width = 2.0    # 車寬self.length = 4.5   # 車長self.max_acc = 2.0  # 最大加速度(m/s2)self.max_jerk = 1.5 # 最大加加速度(m/s3)obstacles = [{'x': 30, 'y': 1.8, 'r': 1.2},{'x': 45, 'y': -1.5, 'r': 0.8}
]  # 障礙物坐標及半徑

3.2 軌跡采樣核心函數

def generate_lattice_paths(current_state, horizon=5.0, dt=0.1):""":param current_state: 當前狀態 [s, s_dot, d, d_dot]:param horizon: 規劃時長(s):return: 候選軌跡列表"""paths = []# 縱向速度采樣（3種模式）for s_dot_end in np.linspace(8, 12, 3):  # 8m/s~12m/s# 橫向位移采樣（5種偏移）for d_end in np.linspace(-1.5, 1.5, 5):  t = np.arange(0, horizon + dt, dt)# 縱向四次多項式擬合s_coeff = np.polyfit([0, horizon], [current_state[0], current_state[0] + s_dot_end*horizon], 4)# 橫向五次多項式擬合d_coeff = np.polyfit([0, horizon], [current_state[2], d_end], 5)path = {'s': np.polyval(s_coeff, t),'d': np.polyval(d_coeff, t),'s_dot': np.polyval(np.polyder(s_coeff), t),'d_dot': np.polyval(np.polyder(d_coeff), t)}paths.append(path)return paths

3.3 碰撞檢測與最優選擇

def select_optimal_path(paths, obstacles):min_cost = float('inf')best_path = Nonefor path in paths:# 計算舒適性成本jerk_cost = np.mean(np.diff(path['s_dot'])**2)# 計算安全成本collision_cost = 0for t in range(len(path['s'])):x = path['s'][t]y = path['d'][t]for obs in obstacles:dist = np.sqrt((x - obs['x'])**2 + (y - obs['y'])**2)if dist < (obs['r'] + 1.5):  # 1.5m為安全距離collision_cost += 1 / max(dist, 0.1)# 計算總成本total_cost = 0.4*jerk_cost + 0.3*collision_costif total_cost < min_cost:min_cost = total_costbest_path = pathreturn best_path

3.4 可視化運行

def plot_scenario(paths, best_path, obstacles):plt.figure(figsize=(10, 6))# 繪制障礙物for obs in obstacles:circle = plt.Circle((obs['x'], obs['y']), obs['r'], color='r', alpha=0.3)plt.gca().add_patch(circle)# 繪制所有候選路徑for path in paths:plt.plot(path['s'], path['d'], 'gray', alpha=0.2)# 高亮最優路徑plt.plot(best_path['s'], best_path['d'], 'b', linewidth=2)plt.xlabel('Longitudinal Position (m)')plt.ylabel('Lateral Position (m)')plt.grid(True)plt.axis('equal')plt.show()# 主程序
if __name__ == "__main__":ego_state = [20, 10, 0, 0]  # [s, s_dot, d, d_dot]candidate_paths = generate_lattice_paths(ego_state)optimal_path = select_optimal_path(candidate_paths, obstacles)plot_scenario(candidate_paths, optimal_path, obstacles)

---

完整代碼

#!/usr/bin/python3
import numpy as np
import matplotlib.pyplot as pltclass Vehicle:def __init__(self):self.width = 2.0    # 車寬self.length = 4.5   # 車長self.max_acc = 2.0  # 最大加速度(m/s2)self.max_jerk = 1.5 # 最大加加速度(m/s3)obstacles = [{'x': 30, 'y': 1.8, 'r': 1.2},{'x': 45, 'y': -1.5, 'r': 0.8}
]  # 障礙物坐標及半徑
def generate_lattice_paths(current_state, horizon=5.0, dt=0.1):""":param current_state: 當前狀態 [s, s_dot, d, d_dot]:param horizon: 規劃時長(s):return: 候選軌跡列表"""paths = []# 縱向速度采樣（3種模式）for s_dot_end in np.linspace(8, 12, 3):  # 8m/s~12m/s# 橫向位移采樣（5種偏移）for d_end in np.linspace(-1.5, 1.5, 5):  t = np.arange(0, horizon + dt, dt)# 縱向四次多項式擬合s_coeff = np.polyfit([0, horizon], [current_state[0], current_state[0] + s_dot_end*horizon], 4)# 橫向五次多項式擬合d_coeff = np.polyfit([0, horizon], [current_state[2], d_end], 5)path = {'s': np.polyval(s_coeff, t),'d': np.polyval(d_coeff, t),'s_dot': np.polyval(np.polyder(s_coeff), t),'d_dot': np.polyval(np.polyder(d_coeff), t)}paths.append(path)return paths
def select_optimal_path(paths, obstacles):min_cost = float('inf')best_path = Nonefor path in paths:# 計算舒適性成本jerk_cost = np.mean(np.diff(path['s_dot'])**2)# 計算安全成本collision_cost = 0for t in range(len(path['s'])):x = path['s'][t]y = path['d'][t]for obs in obstacles:dist = np.sqrt((x - obs['x'])**2 + (y - obs['y'])**2)if dist < (obs['r'] + 1.5):  # 1.5m為安全距離collision_cost += 1 / max(dist, 0.1)# 計算總成本total_cost = 0.4*jerk_cost + 0.3*collision_costif total_cost < min_cost:min_cost = total_costbest_path = pathreturn best_path
def plot_scenario(paths, best_path, obstacles):plt.figure(figsize=(10, 6))# 繪制障礙物for obs in obstacles:circle = plt.Circle((obs['x'], obs['y']), obs['r'], color='r', alpha=0.3)plt.gca().add_patch(circle)# 繪制所有候選路徑for path in paths:plt.plot(path['s'], path['d'], 'gray', alpha=0.2)# 高亮最優路徑plt.plot(best_path['s'], best_path['d'], 'b', linewidth=2)plt.xlabel('Longitudinal Position (m)')plt.ylabel('Lateral Position (m)')plt.grid(True)plt.axis('equal')plt.show()# 主程序
if __name__ == "__main__":ego_state = [20, 10, 0, 0]  # [s, s_dot, d, d_dot]candidate_paths = generate_lattice_paths(ego_state)optimal_path = select_optimal_path(candidate_paths, obstacles)plot_scenario(candidate_paths, optimal_path, obstacles)

四、運行結果分析

（注：實際運行后將顯示包含障礙物區域和最優路徑的軌跡圖，灰色為候選路徑，藍色為最優避障路徑）

---

五、工程優化建議

1. 引入Frenet坐標系：將全局坐標轉換為(s,d)坐標系，簡化橫向/縱向解耦計算
2. 動態權重調整：根據場景危險程度自動調整成本函數權重（如障礙物接近時加大安全權重）
3. 運動學約束檢查：添加最大曲率、加速度邊界條件校驗
4. 多線程優化：對候選軌跡的代價計算進行并行加速

---

六、結語

通過不到100行的Python代碼，我們實現了一個完整的Lattice Planner避障規劃原型系統。該代碼在Python 3.8+環境下可直接運行（需安裝numpy和matplotlib）。在實際自動駕駛系統中，還需融合高精地圖、感知預測模塊等，但核心的軌跡生成與優化思想與此文一脈相承。

技術亮點：

1. 采用多項式參數化方法保證軌跡平滑性
2. 引入多維度加權成本函數實現最優決策
3. 模塊化設計便于功能擴展

原創聲明：本文代碼及算法思路均為作者原創，轉載需注明出處。關注技術博客獲取更多自動駕駛實戰教程！