Behavior planning--13. Implement a cost function in C + +

Source: Internet
Author: User

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

COST=1?E?? ? ? ∣δd∣ ?? < Span class= "Fontsize-ensurer reset-size5 size5" > ?? /? δs

Here, δwas the lateral distance between the goal lane and the final chosen Lane, and δwas the longitudinal d Istance from the vehicle to the goal.

In this quiz, we ' d like your 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 o R PLCL), and the cost function should incorporate this information. We'll 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 is the one lane over from the current lane.
    • Final lane:the Immediate resulting lane of the given behavior. For LCR and LCL, this would is one lane over.
    • Theδs distance to the goal.
    • The goal lane.

Your task in the implementation is to modifyδD in the equation above so it satisifes:

    • ΔD is smaller as both intended lane and final lane were 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 is in the range 0 to 1.

You can implement your solution in cost.cpp below.

Cost.cpp

floatGoal_distance_cost (intGoal_lane,intIntended_lane,intFinal_lane,floatdistance_to_goal) {    /*The cost increases with both the distance of intended lane from the goal and the distance of the final lane fro M the goal.    The cost of being out of the goal lane also becomes larger as vehicle approaches the goal. */    intDelta_d =2.0*goal_lane-intended_lane-Final_lane; floatCost =1-Exp (-(ABS (DELTA_D)/distance_to_goal)); returnCost ;}

Behavior planning--13. Implement a cost function in C + +

Contact Us

The content source of this page is from Internet, which doesn't represent Alibaba Cloud's opinion; products and services mentioned on that page don't have any relationship with Alibaba Cloud. If the content of the page makes you feel confusing, please write us an email, we will handle the problem within 5 days after receiving your email.

If you find any instances of plagiarism from the community, please send an email to: info-contact@alibabacloud.com and provide relevant evidence. A staff member will contact you within 5 working days.

A Free Trial That Lets You Build Big!

Start building with 50+ products and up to 12 months usage for Elastic Compute Service

  • Sales Support

    1 on 1 presale consultation

  • After-Sales Support

    24/7 Technical Support 6 Free Tickets per Quarter Faster Response

  • Alibaba Cloud offers highly flexible support services tailored to meet your exact needs.