Udacity 4.Behavior_Planning 1

udacity4_Behavior_Planning 1

行为规划器会的输入有地图，路线，其他静态或动态障碍物的下一步可能动向。之后规划期生成车车辆的下一个动作

有限状态机

有限状态机基于有限的离散状态来做决策

有限状态机中的状态可以通过一个或多个转换关联起来

也可能会有自转换

也可能会出现只有接受态的状态

有限状态机要处理输入状态，然后决定下一个状态应该是什么

状态量

graph LR 
    A((Ready))
    B((lane<br>keep))
    C((lane<br>change<br>left))
    D((lane<br>change<br>right))
    E((Prepare<br>for lane<br>change left))
    F((Prepare<br>for lane<br>change right))
    A -->A
    A -->B
    B --> B
    B ---->E & F ---->B
    C -->C-->B
    D -->D-->B
    E-->E-->C 
    F-->F-->D   

转换函数的输入

1. Predictions
2. Map
3. Speed Limit
4. Localization Data
5. Current State

转换函数的伪码

fsm：状态机

def transition_function(predictions, current_fsm_state, current_pose, cost_functions, weights):
    # only consider states which can be reached from current FSM state.
    possible_successor_states = successor_states(current_fsm_state)

    # keep track of the total cost of each state.
    costs = []
    for state in possible_successor_states:
        # generate a rough idea of what trajectory we would
        # follow IF we chose this state.
        trajectory_for_state = generate_trajectory(state, current_pose, predictions)

        # calculate the "cost" associated with that trajectory.
        cost_for_state = 0
        for i in range(len(cost_functions)) :
            # apply each cost function to the generated trajectory
            cost_function = cost_functions[i]
            cost_for_cost_function = cost_function(trajectory_for_state, predictions)

            # multiply the cost by the associated weight
            weight = weights[i]
            cost_for_state += weight * cost_for_cost_function
         costs.append({'state' : state, 'cost' : cost_for_state})

    # Find the minimum cost state.
    best_next_state = None
    min_cost = 9999999
    for i in range(len(possible_successor_states)):
        state = possible_successor_states[i]
        cost  = costs[i]
        if cost < min_cost:
            min_cost = cost
            best_next_state = state 

    return best_next_state