行为规划伪代码

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

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

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

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

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

posted on 2018-05-05 16:39  未完代码  阅读(335)  评论(0编辑  收藏  举报