无人驾驶学习笔记–行为规划 【Behavior Planning】
作为无人驾驶最顶层的决策模块,获取各方信息汇总后做出行为决策给路径规划执行,最终传给底层控制模块从而驱动车辆。
I. 有限状态机 Finite State Machine
设计有限个数的状态来限制车辆的行为,其中
- 接受状态: 没有状态传递给其他状态
- 传递函数: 使用输入值来决定传递方向
优点:
- 易执行
- 逻辑简单,直接由逻辑状态转换为物理状态
缺点:
- 可能存在未考虑到的状态,在之后追加过程中会有大改动
- 大环境下不易执行
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 + 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−(d−d_lane_center)21
-
惩罚加速度超过车辆能力的行为:
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 -
奖励靠近目标车道的行为 ( 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_number−target_lane_number)2
-
惩罚驶出车道的行为 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=⎩⎨⎧110d≥d_maxd≤d_mind_min<d<d_max
-
惩罚超速行为 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