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

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

作为无人驾驶最顶层的决策模块，获取各方信息汇总后做出行为决策给路径规划执行，最终传给底层控制模块从而驱动车辆。

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. 惩罚没有在车道中央的行为：$$\frac{1}{1+e^{-(d-d\_lane\_center{)}^2}}$$

2. 惩罚加速度超过车辆能力的行为：
   $$cost=\{\begin{array}{rcl}1& & \ddot{s}\ge a\_max\\ 0& & \ddot{s}<a\_max\end{array}$$

3. 奖励靠近目标车道的行为 $$(lane\_number-target\_lane\_number{)}^2$$

4. 惩罚驶出车道的行为 $$cost=\{\begin{array}{rcl}1& & d\ge d\_max\\ 1& & d\le d\_min\\ 0& & d\_min<d<d\_max\end{array}$$

5. 惩罚超速行为 $$cost=\{\begin{array}{rcl}1& & \dot{s}\ge v\_speed\_limit\\ 0& & \dot{s}<v\_speed\_limit\end{array}$$