• behavior planning——13. implement a cost function in C++


    In the previous quizzes, you designed a cost function to choose a lane when trying to reach a goal in highway driving:

    cost=1eΔd​​​​/​Δs

    Here, Δ was the lateral distance between the goal lane and the final chosen lane, and Δ was the longitudinal distance from the vehicle to the goal.

    In this quiz, we'd like you to implement the cost function in C++, but with one important change. The finite state machine we use for vehicle behavior also includes states for planning a lane change right or left (PLCR or PLCL), and the cost function should incorporate this information. We will provide the following four inputs to the function:

    • Intended lane: the intended lane for the given behavior. For PLCR, PLCL, LCR, and LCL, this would be the one lane over from the current lane.
    • Final lane: the immediate resulting lane of the given behavior. For LCR and LCL, this would be one lane over.
    • The Δdistance to the goal.
    • The goal lane.

    Your task in the implementation will be to modify Δd in the equation above so that it satisifes:

    • Δis smaller as both intended lane and final lane are closer to the goal lane.
    • The cost function provides different costs for each possible behavior: KL, PLCR/PLCL, LCR/LCL.
    • The values produced by the cost function are in the range 0 to 1.

    You can implement your solution in cost.cpp below.

     cost.cpp

    float goal_distance_cost(int goal_lane, int intended_lane, int final_lane, float distance_to_goal) {
        /*
        The cost increases with both the distance of intended lane from the goal
        and the distance of the final lane from the goal. The cost of being out of the 
        goal lane also becomes larger as vehicle approaches the goal.
        */
        int delta_d = 2.0*goal_lane - intended_lane - final_lane;
        float cost = 1 - exp(-(abs(delta_d) / distance_to_goal));
        return cost;
    }
  • 相关阅读:
    491 · 回文数
    936 · 首字母大写
    1343 · 两字符串和
    1535 · 转换成小写字母
    13 · 字符串查找
    146 · 大小写转换 II
    241 · 转换字符串到整数(容易版)
    46 · 主元素
    kotlin协程——>通道
    kotlin协程——>异步流
  • 原文地址:https://www.cnblogs.com/fuhang/p/8984638.html
Copyright © 2020-2023  润新知