无人驾驶学习笔记--行为规划 【Behavior Planning】

无人驾驶学习笔记–行为规划 【Behavior Planning】

作为无人驾驶最顶层的决策模块,获取各方信息汇总后做出行为决策给路径规划执行,最终传给底层控制模块从而驱动车辆。
无人驾驶学习笔记--行为规划 【Behavior Planning】_第1张图片

I. 有限状态机 Finite State Machine

设计有限个数的状态来限制车辆的行为,其中

  1. 接受状态: 没有状态传递给其他状态
  2. 传递函数: 使用输入值来决定传递方向

优点:

  1. 易执行
  2. 逻辑简单,直接由逻辑状态转换为物理状态

缺点:

  1. 可能存在未考虑到的状态,在之后追加过程中会有大改动
  2. 大环境下不易执行

II. 无人驾驶中可能使用的状态

  • 加速、减速、停车
  • 保持当前车速、保持目标速度
  • 跟车、超车
  • 当前车道行驶、准备左右换车道、左右换车道
行为 位置 速度 信号
保持当前车道 保持在车道中间位置 在可行范围内保持目标速度
左右换车道 想左右移动 在可行范围内保持目标速度
准备左右换车道 保持在当前车道中间位置 尝试匹配换道位置和速度 打转向灯

III. 行为控制伪代码

def transition_func(predictions, current_fsm_state, current_pose, cost_functions, weights):
	# 只考虑车辆可以完成的状态
	possible_success_states = success_states(current_fsm_state)
	
	# 对每一个状态进行cost判断
	costs = []
	for state in possible_success_states:
		trajectory = generate_trajectory(state, current_pose, predictions)

		# 计算cost
		cost = 0
		for i in range(len(cost_functions)):
			cost_function = cost_functions[i]
			function_cost = cost_function(trajectory, predictions)

			# 增加权重
			weight = weights[i]
			cost += weight * function_cost
		costs.append({'state' : state, 'cost' : cost})

	# 取最小cost的状态
	best_next_state = None
	min_cost = 99999999
	for i in range(len(possible_success_states)):
		state = possible_success_states[i]
		cost = costs[i]
		if cost < min_cost:
			min_cost = cost
			best_next_state = state

	return best_next_state

IV. 成本函数 Cost Functions

各考虑因素的权重:可行性 > 安全性 > 合法性 > 舒适度 > 效率

位置 速度 加速度
可行性 避障 加速度是否可执行
安全性 缓冲距离 速度~=交通速度
合法性 是否在车道上 没有超速
舒适度 在车道中央行驶 加速度变化jerk
效率 目标车道上 速度~=限速

不同的判定条件,对应的成本函数不尽相同,如下为几个成本函数设置的例子:

  1. 惩罚没有在车道中央的行为: 1 1 + e − ( d − d _ l a n e _ c e n t e r ) 2 \frac{1}{1+e^{-(d-d\_lane\_center)^2} } 1+e(dd_lane_center)21

  2. 惩罚加速度超过车辆能力的行为:
    c o s t = { 1 s ¨ ≥ a _ m a x 0 s ¨   < a _ m a x cost=\left\{ \begin{array}{rcl} 1 & & {\ddot{s} \ge {a\_max}}\\ 0 & & {\ddot{s} \ < {a\_max}}\\ \end{array} \right. cost={10s¨a_maxs¨ <a_max

  3. 奖励靠近目标车道的行为 ( l a n e _ n u m b e r − t a r g e t _ l a n e _ n u m b e r ) 2 (lane\_number - target\_lane\_number)^2 (lane_numbertarget_lane_number)2

  4. 惩罚驶出车道的行为 c o s t = { 1 d ≥ d _ m a x 1 d ≤ d _ m i n 0 d _ m i n < d < d _ m a x cost=\left\{ \begin{array}{rcl} 1 & & {d \ge {d\_max}}\\ 1 & & {d \le {d\_min}}\\ 0 & & {{d\_min} < d < {d\_max}}\\ \end{array} \right. cost=110dd_maxdd_mind_min<d<d_max

  5. 惩罚超速行为 c o s t = { 1 s ˙ ≥ v _ s p e e d _ l i m i t 0 s ˙   < v _ s p e e d _ l i m i t cost=\left\{ \begin{array}{rcl} 1 & & {\dot{s} \ge {v\_speed\_limit}}\\ 0 & & {\dot{s} \ < {v\_speed\_limit}}\\ \end{array} \right. cost={10s˙v_speed_limits˙ <v_speed_limit

你可能感兴趣的:(自动驾驶)