行为规划伪代码
实现转换函数的一种方法是为每个可访问的“下一个状态”生成粗略轨迹,然后找到最佳状态。为了“找到最好的”,我们通常使用成本函数。
然后我们可以计算出每条粗略轨迹的代价是多大,然后选择成本轨迹最低的状态。
稍后我们会更详细地讨论这一点,但首先仔细阅读下面的伪代码,以更好地了解转换函数的工作原理。
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
显然,我们在这里重申了一些重要的细节。即:什么是这些成本函数以及我们如何建立呢?接下来我们将讨论这个问题!