第三期 行为规划——7.状态转移方程的伪代码

行为规划伪代码

实现转换函数的一种方法是为每个可访问的“下一个状态”生成粗略轨迹，然后找到最佳状态。为了“找到最好的”，我们通常使用成本函数。

然后我们可以计算出每条粗略轨迹的代价是多大，然后选择成本轨迹最低的状态。

稍后我们会更详细地讨论这一点，但首先仔细阅读下面的伪代码，以更好地了解转换函数的工作原理。

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

显然，我们在这里重申了一些重要的细节。即：什么是这些成本函数以及我们如何建立呢？接下来我们将讨论这个问题！