watonomous.github.io

[ Software Division : Path Planning Group ]

Created by [ Rowan Dempster], last modified by [ Ben Zhang] on Jan 18, 2020

Contents
Subpages
Mission Statement
Very Useful Links
Path Planning Overview:

Subpages

Mission Statement

The job of Path Planning is, as we understand it, "Given an accurate enough picture of the world around the vehicle, and an accurate description of the vehicle's current state, instruct the vehicle on how to move". This is a lot to cover, and we have a large team, so we decided to split our team into subteams that handle different parts of our architecture.

Very Useful Links

General:

Path Planning Objectives and Key Results: https://docs.google.com/document/d/1rkt8IM88ilq60LGbX8em9kbyAao1YGtyIgTIvwKArUc/edit?usp=sharing
Jira Workboard (Needs updating): https://jira.watonomous.ca/secure/RapidBoard.jspa?rapidView=2&projectKey=PP&view=planning.nodetail&epics=visible&issueLimit=100
Software Pipeline Documentation: https://docs.google.com/document/d/14kDOO9L3YU2OQEs0tR85pUxXs78bYIPO87eHWYtXIGo/edit#heading=h.g756mi2nxmkt
Software Requirements Documentation: https://docs.google.com/document/d/1VU0NWDou8glHfn-Y5G2p4WwfCui42pHw_v4QPVbhBZc/edit

Onboarding:

Path Planning Onboarding: Path Planning Onboarding
General Onboarding: Software Onboarding Tutorial
Software Pipeline Tutorial (Out of date as of Jan 13, 2020): Software Pipeline Integration Tutorial (OUT OF DATE)

Tutorials:

Rviz Tutorial: RViz Tool
Rosbag Tutorial: Rosbag Tutorial
Server Use: SSH Access to WATO machines
General Tutorials: General Tutorials

Path Planning Overview:

Architecture doc as of Nov 14, 2019: https://docs.google.com/document/d/11dzfnXUmhjTbYjIIyPo1pTQVJHtsKcRjvhW9IXq2DIA/edit

MPC Meeting - November 6th

[{.confluence-embedded-image height=”400”}]{.confluence-embedded-file-wrapper .confluence-embedded-manual-size}

Behavioural planner

The behavioural planner's job is to interpret and process semantic information about the car's state and the environment. This includes sending instructions about car speed, state (stopped, driving etc), and producing a list of lanes that the car can follow.

[{.confluence-embedded-image .confluence-thumbnail height=”250”}]{.confluence-embedded-file-wrapper .confluence-embedded-manual-size}

Inputs:

Environment
Next global mapping waypoint (not currently implemented)

Process:

From lane lines contained in the environment, create a list of valid trajectories that the car can follow
From the global mapping waypoint, get whether the next intersection is left, right, or straight. If a left turn is next, set the desired trajectory to the leftmost trajectory. If a right turn is next, set the desired trajectory to the rightmost trajectory. Otherwise, set the desired trajectory to the current trajectory.
Associate obstacles with stop lines to create the cutting layer.

Outputs:

Desired trajectory id (int)
Current trajectory id (int)
List of available trajectories ( vector<Polyline> )
Cutting later ( vector<LineSegments> )

Trajectory planning:

Decide on a Path that describes how the vehicle will move through its immediate environment towards a local Goal Pose.

[{.confluence-embedded-image height=”250”}]{.confluence-embedded-file-wrapper .confluence-embedded-manual-size}

**Path
**A Path is an ordered collection of Vehicle States. Each Vehicle State is a pose, and has a target velocity, and maybe even a target acceleration!

**Immediate Environment?
**The immediate environment of the vehicle is defined by the costmap provided by the Costmap subteam.

Inputs:

Desired trajectory id (int)
Current trajectory id (int)
List of available trajectories ( vector<Polyline> )
Cutting later ( vector<LineSegments> )

Process:

Stitch together portions of the available trajectories in order to create a polyline which changes lanes
Check to see if the cutting layer intersects the created polyline. If it does, select the last point before the intersection. Otherwise, select the last point in the trajectory.
Fit a curve to the polyline. NOTE: we still need to decide whether we do this in C++ or Python, and what method to use.
Give

Outputs:

Curve fit parameters
Final point

More on the Trajectory Planning document

Potential Field Module:

Potential fields are a representation of the car's environment as a series of continuous sigmoidal functions

[{.confluence-embedded-image .confluence-external-resource height=”250”}]{.confluence-embedded-file-wrapper .confluence-embedded-manual-size}

Inputs:

Environment

Process:

Potential field functions will be created for every required object in the environment

Reference Paper: https://www.mdpi.com/2076-3417/8/12/2592/htm
[Paper on Integration with MPC: ]A_Potential_Field-Based_Model_Predictive_Path-Planning_Controller_for_Autonomous_Road_Vehicles.pdf

MPC Feedback Controller:

[[[]{.companion-edit-button-placeholder .edit-button-overlay data-linked-resource-container-id=”1442247” data-linked-resource-id=”1442612” data-template-name=”companionEditIcon” data-source-location=”embedded-attachment”}]

(Yeah, that's autonomous btw)

MPC is our Year 3 Feedback Controler in charge of handling smooth steering, acceleration, and braking. While Trajectory creates an "ideal path" for the car to follow, MPC makes sure the car follows that path.

MPC uses Potential Fields to implement obstacle avoidance.

Inputs:

Curve fit parameters
Final point
Environment

Process:

The potential field will be created by using potential field module with the environment
Error function will minimize the distance to the final point, lateral distance to ideal trajectory curve, and potential field cost

Fun Fact: Our Technical Lead for MPC, Mohamed Daoud{.confluence-userlink .user-mention}, has a paper on MPC published in IEEE Explore!

[{.confluence-embedded-image height=”153”}]{.confluence-embedded-file-wrapper .confluence-embedded-manual-size}

Occupiable Objects

[[[]{.companion-edit-button-placeholder .edit-button-overlay data-linked-resource-container-id=”1442247” data-linked-resource-id=”1442605” data-template-name=”companionEditIcon” data-source-location=”embedded-attachment”}]

(Yeah, that's autonomous btw)

Mission Statement

Create and manage a special type of object within the cost map: Occupiable Objects.

An occupiable object any complex object where the physical region that the object occupies does not completely define its effect on the course of the vehicle. Some examples:

A stop sign is an occupiable object because it affects the road next to it (we have to stop when we reach that part of the road), not just the area is occupied on the side of the road.
The side of the road is not an occupiable object because it only affects the region of the Environment where is physically presides.
A pedestrian is an occupiable obstacle because its velocity vector is not always guaranteed to be zero (ie it moves), and thus it affects the area where its velocity vector is pointing towards.

Occupiable Objects are used by Behavioural Planner to determine where / when to stop. Each occupiable object in the environment has it's own state machine. For example, a pedestrian has its own state machine containing information about whether or not the pedestrian has crossed the road. A stop sign has a state machine determining if the car has stopped at the stop sign yet. Based on the internal state, an Occupiable Object will produce a "Stop Line", a line that the car should stop at if the car's path crosses that line.

Input:

Processing Environment message

Output:

List of stop lines (lines where the car should stop)

More on the Occupiable Objects Documentation

Costmap Subteam (Deprecated, replaced by Potential Fields)

Mission Statement

The Costmap module creates a cost based occupancy grid which encapsulates all static objects in the vehicle's immediate environment.

Objects?

What we mean by objects is everything from road lines to huge boulders in the middle of the road. In other words, objects are not just obstacles that block our path, but rather anything that impacts the course of the vehicle. All of these objects are sent to Path Planning from Processing using the Environment.msg.

Static?

What it means for an object to be static is that the obstacle does not have impact (for path planning purposes) any other part of the environment besides the physical space that it occupies. This is best understood by example:

A stop sign is not a static object because it affects the road next to it (we have to stop when we reach that part of the road).
The side of the road is a static object because it only affects the region of the Environment where is physically presides.
A pedestrian is not a static obstacle because its velocity vector is not always guaranteed to be zero (ie it moves).

The Output!

The output of the Costmap module is a matrix where each cell represents the cost (ie penalty) of the vehicle being inside that cell.
The trajectory planner uses this matrix to navigate its way through the environment is the "best" (defined by least cost) way possible to its next goal state.
The Cost Map should always be up to date and an accurate reflections of the vehicle's local environment.

More...

See more details in the Costmap subdocument: https://phabricator.watonomous.ca/w/software/path_planning/costmap/