自动驾驶软件：Cruise自动驾驶二次开发_（7）.决策与规划模块开发
决策与规划模块开发
1. 决策与规划模块概述
决策与规划模块是自动驾驶系统中的核心部分，负责根据感知模块提供的环境信息和车辆状态信息，生成最优的行驶路径和决策策略。本节将详细介绍决策与规划模块的基本原理和开发流程，包括路径规划、行为决策和运动控制等方面的内容。
2. 路径规划
路径规划是决策与规划模块中的重要环节，负责生成从起点到终点的最优路径。路径规划通常分为全局路径规划和局部路径规划两部分。
2.1 全局路径规划
全局路径规划主要负责生成从起点到终点的宏观路径，通常基于地图信息和导航系统。常用的全局路径规划算法有A*算法、Dijkstra算法和RRT（Rapidly-exploring Random Trees）算法等。
2.1.1 A*算法
A*算法是一种启发式搜索算法，能够在图中找到从起点到终点的最短路径。算法通过评估函数来选择当前节点的下一个节点，评估函数通常包括两个部分：从起点到当前节点的实际代价（g(n)）和从当前节点到终点的估计代价（h(n)）。
评估函数公式：
f(n) = g(n) + h(n)
代码示例：
import heapq
def a_star_search(graph, start, goal):
"""
使用A*算法进行全局路径规划
:param graph: 图形表示的地图
:param start: 起点
:param goal: 终点
:return: 从起点到终点的最短路径
"""
open_set = []
heapq.heappush(open_set, (0, start))
came_from = {}
g_score = {node: float('inf') for node in graph}
g_score[start] = 0
f_score = {node: float('inf') for node in graph}
f_score[start] = heuristic(start, goal)
while open_set:
current = heapq.heappop(open_set)[1]
if current == goal:
return reconstruct_path(came_from, goal)
for neighbor in graph[current]:
tentative_g_score = g_score[current] + graph[current][neighbor]
if tentative_g_score < g_score[neighbor]:
came_from[neighbor] = current
g_score[neighbor] = tentative_g_score
f_score[neighbor] = g_score[neighbor] + heuristic(neighbor, goal)
if neighbor not in [node[1] for node in open_set]:
heapq.heappush(open_set, (f_score[neighbor], neighbor))
return None
def heuristic(node, goal):
"""
估计代价函数，这里使用欧几里得距离
:param node: 当前节点
:param goal: 终点
:return: 估计代价
"""
return ((node[0] - goal[0]) ** 2 + (node[1] - goal[1]) ** 2) ** 0.5
def reconstruct_path(came_from, current):
"""
重构路径
:param came_from: 记录每个节点的前驱节点的字典
:param current: 当前节点
:return: 从起点到当前节点的路径
"""
total_path = [current]
while current in came_from:
current = came_from[current]
total_path.append(current)
total_path.reverse()
return total_path
# 示例地图
graph = {
(0, 0): {(1, 0): 1, (0, 1): 1},
(1, 0): {(0, 0): 1, (1, 1): 1},
(0, 1): {(0, 0): 1, (1, 1): 1},
(1, 1): {(1, 0): 1, (0, 1): 1, (2, 1): 1},
(2, 1): {(1, 1): 1, (2, 2): 1},
(2, 2): {(2, 1): 1}
}
start = (0, 0)
goal = (2, 2)
path = a_star_search(graph, start, goal)
print(f"从 {start} 到 {goal} 的路径: {path}")
代码描述：
a_star_search函数实现了A*算法，输入为地图、起点和终点，输出为从起点到终点的最短路径。
heuristic函数用于计算从当前节点到终点的估计代价，这里使用欧几里得距离作为估计代价。
reconstruct_path函数用于从记录的前驱节点字典中重构路径。
2.2 局部路径规划
局部路径规划主要负责在当前环境和车辆状态的基础上，生成短时间内的行驶路径。常用的局部路径规划算法有DWA（Dynamic Window Approach）和势场法等。
2.2.1 DWA算法
DWA算法是一种基于动态窗口的路径规划方法，通过在动态窗口内搜索最优的速度和角速度，生成局部路径。DWA算法的主要步骤包括：
定义动态窗口，即可行的速度和角速度范围。
在动态窗口内生成候选路径。
评估候选路径，选择最优路径。
代码示例：
import numpy as np
def dwa_planning(x, u, config, goal, ob):
"""
使用DWA算法进行局部路径规划
:param x: 当前状态 (x, y, theta, v, omega)
:param u: 当前控制输入 (v, omega)
:param config: 配置参数
:param goal: 目标位置 (x, y)
:param ob: 障碍物位置列表 [(x, y), ...]
:return: 最优控制输入 (v, omega)
"""
# 定义动态窗口
vs = [v for v in np.arange(u[0] - config.max_accel * config.dt,
u[0] + config.max_accel * config.dt, config.v_resolution)]
omegas = [omega for omega in np.arange(u[1] - config.max_delta_yawrate * config.dt,
u[1] + config.max_delta_yawrate * config.dt, config.yawrate_resolution)]
# 评估候选路径
best_trajectory = None
best_u = None
best_cost = float('inf')
for v in vs:
for omega in omegas:
trajectory = calc_trajectory(x, v, omega, config)
if not check_collision(trajectory, ob, config):
cost = calc_final_cost(trajectory, goal, config)
if cost < best_cost:
best_trajectory = trajectory
best_u = [v, omega]
best_cost = cost
return best_u, best_trajectory
def calc_trajectory(x, v, omega, config):
"""
计算候选路径
:param x: 当前状态 (x, y, theta, v, omega)
:param v: 速度
:param omega: 角速度
:param config: 配置参数
:return: 候选路径
"""
x_new = np.array(x)
trajectory = np.array(x_new)
time = 0
while time <= config.predict_time:
x_new = motion(x_new, [v, omega], config.dt)
trajectory = np.vstack((trajectory, x_new))
time += config.dt
return trajectory
def motion(x, u, dt):
"""
车辆运动模型
:param x: 当前状态 (x, y, theta, v, omega)
:param u: 控制输入 (v, omega)
:param dt: 时间步长
:return: 下一状态
"""
x_new = np.zeros_like(x)
x_new[0] = x[0] + u[0] * np.cos(x[2]) * dt
x_new[1] = x[1] + u[0] * np.sin(x[2]) * dt
x_new[2] = x[2] + u[1] * dt
x_new[3] = u[0]
x_new[4] = u[1]
return x_new
def check_collision(trajectory, ob, config):
"""
检查路径是否与障碍物碰撞
:param trajectory: 候选路径
:param ob: 障碍物位置列表
:param config: 配置参数
:return: 是否碰撞
"""
for i in range(len(trajectory)):
for j in range(len(ob)):
d = np.hypot(trajectory[i][0] - ob[j][0], trajectory[i][1] - ob[j][1])
if d <= config.robot_radius:
return True
return False
def calc_final_cost(trajectory, goal, config):
"""
计算路径的最终代价
:param trajectory: 候选路径
:param goal: 目标位置
:param config: 配置参数
:return: 代价
"""
# 到目标的距离代价
dist_cost = np.hypot(trajectory[-1][0] - goal[0], trajectory[-1][1] - goal[1])
# 速度代价
speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1][3])
# 角速度代价
omega_cost = config.omega_cost_gain * abs(trajectory[-1][4])
return dist_cost + speed_cost + omega_cost
# 配置参数
class Config:
def __init__(self):
self.max_speed = 1.0 # 最大速度 [m/s]
self.min_speed = -1.0 # 最小速度 [m/s]
self.max_accel = 0.5 # 最大加速度 [m/ss]
self.max_delta_yawrate = 40.0 * np.pi / 180.0 # 最大角速度变化 [rad/s]
self.v_resolution = 0.1 # 速度分辨率 [m/s]
self.yawrate_resolution = 10.0 * np.pi / 180.0 # 角速度分辨率 [rad/s]
self.dt = 0.1 # 时间步长 [s]
self.predict_time = 3.0 # 预测时间 [s]
self.robot_radius = 0.5 # 机器人半径 [m]
self.speed_cost_gain = 0.1 # 速度代价增益
self.omega_cost_gain = 0.1 # 角速度代价增益
config = Config()
# 当前状态
x = [0.0, 0.0, 0.0, 0.0, 0.0] # (x, y, theta, v, omega)
u = [0.0, 0.0] # (v, omega)
# 目标位置
goal = [5.0, 5.0]
# 障碍物位置
ob = [(2.0, 2.0), (3.0, 3.0)]
best_u, best_trajectory = dwa_planning(x, u, config, goal, ob)
print(f"最优控制输入: {best_u}")
print(f"最优路径: {best_trajectory}")
代码描述：
dwa_planning函数实现了DWA算法，输入为当前状态、当前控制输入、配置参数、目标位置和障碍物位置，输出为最优控制输入和最优路径。
calc_trajectory函数用于计算候选路径，输入为当前状态、速度、角速度和配置参数，输出为候选路径。
motion函数定义了车辆的运动模型，输入为当前状态、控制输入和时间步长，输出为下一状态。
check_collision函数用于检查路径是否与障碍物碰撞，输入为候选路径、障碍物位置和配置参数，输出为是否碰撞。
calc_final_cost函数用于计算路径的最终代价，输入为候选路径、目标位置和配置参数，输出为代价。
3. 行为决策
行为决策模块负责根据环境信息和车辆状态，决定车辆的行为。常见的行为决策包括跟随、避障、变道和停车等。
3.1 跟随行为
跟随行为是指自动驾驶车辆在前车的带领下行驶。通常通过PID控制器来实现跟随行为。
代码示例：
import time
class PIDController:
def __init__(self, Kp, Ki, Kd, dt):
"""
初始化PID控制器
:param Kp: 比例增益
:param Ki: 积分增益
:param Kd: 微分增益
:param dt: 时间步长
"""
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.dt = dt
self.integral = 0.0
self.previous_error = 0.0
def update(self, error):
"""
更新PID控制器
:param error: 误差
:return: 控制输出
"""
self.integral += error * self.dt
derivative = (error - self.previous_error) / self.dt
self.previous_error = error
return self.Kp * error + self.Ki * self.integral + self.Kd * derivative
# 配置参数
Kp = 1.0
Ki = 0.1
Kd = 0.05
dt = 0.1
# 初始化PID控制器
pid = PIDController(Kp, Ki, Kd, dt)
# 模拟前车和跟随车辆的位置
lead_car_position = [0.0, 0.0]
follow_car_position = [0.0, 0.0]
# 模拟前车的运动
for t in range(100):
lead_car_position[0] += 0.5 # 前车以0.5m/s的速度前进
error = lead_car_position[0] - follow_car_position[0] # 计算误差
control_output = pid.update(error) # 更新PID控制器
follow_car_position[0] += control_output # 跟随车辆根据控制输出前进
print(f"时间: {t * dt}s, 前车位置: {lead_car_position[0]}, 跟随车辆位置: {follow_car_position[0]}")
time.sleep(dt)
代码描述：
PIDController类定义了一个PID控制器，包含比例增益（Kp）、积分增益（Ki）、微分增益（Kd）和时间步长（dt）等参数。
update方法用于更新PID控制器的输出，输入为误差，输出为控制信号。
模拟了前车和跟随车辆的位置变化，通过PID控制器控制跟随车辆的前进速度，使其跟随前车。
3.2 避障行为
避障行为是指自动驾驶车辆在检测到障碍物时进行避让。通常通过势场法或模型预测控制（MPC）来实现避障行为。
3.2.1 势场法
势场法通过构建吸引场和排斥场来生成避障路径。吸引场吸引车辆向目标位置移动，排斥场排斥车辆远离障碍物。
代码示例：
import numpy as np
def potential_field_planning(x, y, goal, ob, Kp, Kr, eta, grid_size):
"""
使用势场法进行避障路径规划
:param x: 当前x位置
:param y: 当前y位置
:param goal: 目标位置 (x, y)
:param ob: 障碍物位置列表 [(x, y), ...]
:param Kp: 吸引场增益
:param Kr: 排斥场增益
:param eta: 避障阈值
:param grid_size: 网格大小
:return: 下一步的x和y位置
"""
# 计算吸引场力
force_to_goal = Kp * np.array([goal[0] - x, goal[1] - y])
# 计算排斥场力
total_repulsive_force = np.zeros(2)
for (ox, oy) in ob:
d = np.hypot(x - ox, y - oy)
if d <= eta:
repulsive_force = Kr * (1.0 / d - 1.0 / eta) * (1.0 / d**2) * np.array([x - ox, y - oy])
total_repulsive_force += repulsive_force
# 总力
total_force = force_to_goal + total_repulsive_force
# 计算下一步的位置
dx = total_force[0] * grid_size
dy = total_force[1] * grid_size
return x + dx, y + dy
# 当前位置
x = 0.0
y = 0.0
# 目标位置
goal = [5.0, 5.0]
# 障碍物位置
ob = [(2.0, 2.0), (3.0, 3.0)]
# 势场法参数
Kp = 1.0 # 吸引场增益
Kr = 100.0 # 排斥场增益
eta = 1.0 # 避障阈值
grid_size = 0.1 # 网格大小
# 模拟避障路径规划
for t in range(100):
x, y = potential_field_planning(x, y, goal, ob, Kp, Kr, eta, grid_size)
print(f"时间: {t * 0.1}s, 当前位置: ({x}, {y})")
代码描述：
potential_field_planning函数实现了势场法避障路径规划，输入为当前位置、目标位置、障碍物位置、吸引场增益、排斥场增益、避障阈值和网格大小，输出为下一步的位置。
force_to_goal计算了吸引场力，该力将车辆吸引向目标位置。
total_repulsive_force计算了排斥场力，该力将车辆推开远离障碍物。
total_force是吸引场力和排斥场力的矢量和。
dx和dy是总力在x和y方向上的分量，乘以网格大小后得到下一步的位置变化。
模拟了避障路径规划，每一步更新当前位置并打印结果。
3.2.2 模型预测控制（MPC）
模型预测控制（MPC）是一种基于模型的控制方法，通过预测未来的状态来优化当前的控制输入。MPC通常用于处理复杂的动态系统，如自动驾驶车辆的避障行为。
代码示例：
import numpy as np
from scipy.optimize import minimize
def mpc_planning(x, goal, ob, config):
"""
使用模型预测控制进行避障路径规划
:param x: 当前状态 (x, y, theta, v, omega)
:param goal: 目标位置 (x, y)
:param ob: 障碍物位置列表 [(x, y), ...]
:param config: 配置参数
:return: 最优控制输入 (v, omega)
"""
def cost_function(u):
# 模拟未来的轨迹
trajectory = simulate_trajectory(x, u, config)
# 计算代价
cost = calc_mpc_cost(trajectory, goal, ob, config)
return cost
# 初始控制输入
u0 = [0.0, 0.0]
# 约束条件
bounds = [(config.min_speed, config.max_speed), (config.min_omega, config.max_omega)]
# 优化控制输入
result = minimize(cost_function, u0, bounds=bounds)
return result.x
def simulate_trajectory(x, u, config):
"""
模拟未来的轨迹
:param x: 当前状态 (x, y, theta, v, omega)
:param u: 控制输入 (v, omega)
:param config: 配置参数
:return: 未来的轨迹
"""
trajectory = np.array(x)
for t in np.arange(0, config.predict_time, config.dt):
x = motion(x, u, config.dt)
trajectory = np.vstack((trajectory, x))
return trajectory
def calc_mpc_cost(trajectory, goal, ob, config):
"""
计算路径的代价
:param trajectory: 未来的轨迹
:param goal: 目标位置
:param ob: 障碍物位置列表
:param config: 配置参数
:return: 代价
"""
dist_cost = 0.0
collision_cost = 0.0
for i in range(len(trajectory)):
# 计算到目标的距离代价
dist_cost += np.hypot(trajectory[i][0] - goal[0], trajectory[i][1] - goal[1])
# 计算碰撞代价
for (ox, oy) in ob:
d = np.hypot(trajectory[i][0] - ox, trajectory[i][1] - oy)
if d < config.robot_radius:
collision_cost += 1.0 / d
return dist_cost + config.collision_cost_gain * collision_cost
# 配置参数
class Config:
def __init__(self):
self.max_speed = 1.0 # 最大速度 [m/s]
self.min_speed = -1.0 # 最小速度 [m/s]
self.max_omega = 1.0 # 最大角速度 [rad/s]
self.min_omega = -1.0 # 最小角速度 [rad/s]
self.dt = 0.1 # 时间步长 [s]
self.predict_time = 3.0 # 预测时间 [s]
self.robot_radius = 0.5 # 机器人半径 [m]
self.collision_cost_gain = 10.0 # 碰撞代价增益
config = Config()
# 当前状态
x = [0.0, 0.0, 0.0, 0.0, 0.0] # (x, y, theta, v, omega)
# 目标位置
goal = [5.0, 5.0]
# 障碍物位置
ob = [(2.0, 2.0), (3.0, 3.0)]
# 进行MPC规划
best_u = mpc_planning(x, goal, ob, config)
print(f"最优控制输入: {best_u}")
代码描述：
mpc_planning函数实现了模型预测控制，输入为当前状态、目标位置、障碍物位置和配置参数，输出为最优控制输入。
cost_function定义了代价函数，用于评估控制输入生成的轨迹的代价。
simulate_trajectory函数模拟了未来的轨迹，输入为当前状态、控制输入和配置参数，输出为未来的轨迹。
calc_mpc_cost函数计算了路径的代价，包括到目标的距离代价和碰撞代价。
Config类定义了MPC的配置参数，包括最大速度、最小速度、最大角速度、最小角速度、时间步长、预测时间、机器人半径和碰撞代价增益。
模拟了当前状态、目标位置和障碍物位置，通过MPC规划得到最优控制输入并打印结果。
4. 运动控制
运动控制模块负责将路径规划和行为决策的结果转化为具体的控制指令，控制车辆的运动。常用的运动控制算法有PID控制、模型预测控制（MPC）和模糊控制等。
4.1 PID控制
PID控制是一种经典的控制方法，通过比例、积分和微分项来调整控制输出，使车辆沿预定路径行驶。
代码示例：
import time
class PIDController:
def __init__(self, Kp, Ki, Kd, dt):
"""
初始化PID控制器
:param Kp: 比例增益
:param Ki: 积分增益
:param Kd: 微分增益
:param dt: 时间步长
"""
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.dt = dt
self.integral = 0.0
self.previous_error = 0.0
def update(self, error):
"""
更新PID控制器
:param error: 误差
:return: 控制输出
"""
self.integral += error * self.dt
derivative = (error - self.previous_error) / self.dt
self.previous_error = error
return self.Kp * error + self.Ki * self.integral + self.Kd * derivative
# 配置参数
Kp = 1.0
Ki = 0.1
Kd = 0.05
dt = 0.1
# 初始化PID控制器
pid = PIDController(Kp, Ki, Kd, dt)
# 模拟路径跟踪
path = [(0.0, 0.0), (1.0, 1.0), (2.0, 2.0), (3.0, 3.0), (4.0, 4.0), (5.0, 5.0)]
current_position = [0.0, 0.0] # (x, y)
for t in range(100):
# 计算到下一个路径点的误差
if t < len(path) - 1:
next_path_point = path[t + 1]
else:
next_path_point = path[-1]
error_x = next_path_point[0] - current_position[0]
error_y = next_path_point[1] - current_position[1]
error = np.hypot(error_x, error_y)
# 更新PID控制器
control_output = pid.update(error)
# 更新当前位置
current_position[0] += control_output * np.cos(t * np.pi / 18) * dt
current_position[1] += control_output * np.sin(t * np.pi / 18) * dt
print(f"时间: {t * dt}s, 当前位置: ({current_position[0]}, {current_position[1]})")
time.sleep(dt)
代码描述：
PIDController类定义了一个PID控制器，包含比例增益（Kp）、积分增益（Ki）、微分增益（Kd）和时间步长（dt）等参数。
update方法用于更新PID控制器的输出，输入为误差，输出为控制信号。
模拟了路径跟踪，通过PID控制器调整控制输出，使车辆沿预定路径行驶。
每一步计算当前位置与下一个路径点之间的误差，更新PID控制器的输出，并根据输出更新当前位置。
4.2 模型预测控制（MPC）
模型预测控制（MPC）是一种基于模型的控制方法，通过预测未来的状态来优化当前的控制输入。MPC可以处理复杂的动态系统，如自动驾驶车辆的路径跟踪和避障。
代码示例：
import numpy as np
from scipy.optimize import minimize
def mpc_control(x, path, config):
"""
使用模型预测控制进行路径跟踪
:param x: 当前状态 (x, y, theta, v, omega)
:param path: 预定路径 [(x, y), ...]
:param config: 配置参数
:return: 最优控制输入 (v, omega)
"""
def cost_function(u):
# 模拟未来的轨迹
trajectory = simulate_trajectory(x, u, config)
# 计算代价
cost = calc_mpc_cost(trajectory, path, config)
return cost
# 初始控制输入
u0 = [0.0, 0.0]
# 约束条件
bounds = [(config.min_speed, config.max_speed), (config.min_omega, config.max_omega)]
# 优化控制输入
result = minimize(cost_function, u0, bounds=bounds)
return result.x
def simulate_trajectory(x, u, config):
"""
模拟未来的轨迹
:param x: 当前状态 (x, y, theta, v, omega)
:param u: 控制输入 (v, omega)
:param config: 配置参数
:return: 未来的轨迹
"""
trajectory = np.array(x)
for t in np.arange(0, config.predict_time, config.dt):
x = motion(x, u, config.dt)
trajectory = np.vstack((trajectory, x))
return trajectory
def calc_mpc_cost(trajectory, path, config):
"""
计算路径的代价
:param trajectory: 未来的轨迹
:param path: 预定路径
:param config: 配置参数
:return: 代价
"""
path_cost = 0.0
for i in range(len(trajectory)):
min_dist = float('inf')
for (px, py) in path:
dist = np.hypot(trajectory[i][0] - px, trajectory[i][1] - py)
if dist < min_dist:
min_dist = dist
path_cost += min_dist
return path_cost
# 配置参数
class Config:
def __init__(self):
self.max_speed = 1.0 # 最大速度 [m/s]
self.min_speed = -1.0 # 最小速度 [m/s]
self.max_omega = 1.0 # 最大角速度 [rad/s]
self.min_omega = -1.0 # 最小角速度 [rad/s]
self.dt = 0.1 # 时间步长 [s]
self.predict_time = 3.0 # 预测时间 [s]
config = Config()
# 当前状态
x = [0.0, 0.0, 0.0, 0.0, 0.0] # (x, y, theta, v, omega)
# 预定路径
path = [(0.0, 0.0), (1.0, 1.0), (2.0, 2.0), (3.0, 3.0), (4.0, 4.0), (5.0, 5.0)]
# 进行MPC控制
for t in range(100):
best_u = mpc_control(x, path, config)
x = motion(x, best_u, config.dt)
print(f"时间: {t * config.dt}s, 当前状态: {x}")
代码描述：
mpc_control函数实现了模型预测控制，输入为当前状态、预定路径和配置参数，输出为最优控制输入。
cost_function定义了代价函数，用于评估控制输入生成的轨迹的代价。
simulate_trajectory函数模拟了未来的轨迹，输入为当前状态、控制输入和配置参数，输出为未来的轨迹。
calc_mpc_cost函数计算了路径的代价，包括路径跟踪代价。
Config类定义了MPC的配置参数，包括最大速度、最小速度、最大角速度、最小角速度、时间步长和预测时间。
模拟了当前状态和预定路径，通过MPC控制得到最优控制输入并更新当前位置。
5. 总结
决策与规划模块是自动驾驶系统中的关键部分，负责生成最优的行驶路径和决策策略。路径规划分为全局路径规划和局部路径规划，行为决策包括跟随、避障、变道和停车等。运动控制则负责将路径规划和行为决策的结果转化为具体的控制指令，控制车辆的运动。本节详细介绍了A*算法、DWA算法、PID控制器和MPC等常用算法的实现，并提供了相应的代码示例。这些算法和方法为自动驾驶车辆的决策与规划提供了坚实的基础。
6. 未来展望
随着自动驾驶技术的不断发展，决策与规划模块也在不断优化和创新。未来的研究方向包括：
多目标优化 ：同时考虑多个目标，如安全性、舒适性和效率。
动态环境建模 ：更准确地建模动态环境，提高路径规划的实时性和鲁棒性。
深度学习应用 ：利用深度学习技术提升决策与规划的智能化水平，如通过神经网络预测其他车辆的行为。
多传感器融合 ：结合多种传感器的信息，提高环境感知的准确性和可靠性。
通过这些研究方向，自动驾驶系统的决策与规划模块将变得更加智能和高效，为实现更高级别的自动驾驶提供支持。