Autonomous
Vehicle
Navigation
Path Planning & Navigation


Contact Information

Roger Lopez
Manager
Autonomous Systems & Controls
(210) 522-3832
rlopez@swri.org

For a driverless vehicle to autonomously negotiate traffic, navigation tasks range from steering and speed control to negotiating complex urban environments with stop signs, traffic lights, other vehicles, and pedestrians. Southwest Research Institute (SwRI) retrofitted a commercially available 2006 Ford® Explorer with the sensors, actuators, and computing hardware needed to sense the state of the world and make intelligent decisions based on that information, including:

  • A modular architecture for real-time navigation control software
  • Safe, comfortable speed profiles for autonomous vehicles
  • Global route mapping to find the best route from point A to B, minimizing travel time and/or fuel consumption.
  • Separating the desired travel route into manageable segments using path segmentation
  • Dynamic recalculation of path segments to allow lane changes or obstacle avoidance

Path Planning

Navigation, or path planning, for nonholonomic systems has been a highly researched topic since the field of robotics became popular in the mid-1980s. Much of this path planning work has focused on car-like vehicles, similar to the Mobile Autonomous Robotics Technology Initiative (MARTI®) formerly the Southwest Safe Transport Initiative (SSTI) Ford Explorer, but used a simplified model of the car. While simple line segments and arcs of circles are convenient and are still widely used today, they do not transition well to passenger vehicles due to the discontinuity in curvature (i.e., steering angle) between path segments. These discontinuities lead to a rapid steering rate and a decrease in passenger comfort. More recent research has led to paths that are more comfortable to passengers, but are computationally intensive due to the fact that these paths cannot be solved in closed form.

Global Route Planning

In addition to planning a local path for the vehicle to follow in the near-term, the autonomous vehicle’s navigation system must be able to plan the complete route from the vehicle’s starting position to a destination. This type of global route planning is in such frequent use today (e.g., Mapquest® or Google® Maps, in-vehicle global positioning systems (GPS), etc.).

MARTI has implemented the 4D/RCS reference model architecture to modularize the components performing the tasks of an autonomous vehicle navigation system. This modular approach allows the slower, more deliberative software components that are computing the long-term route to be separated from the faster, reactive software components that are quickly changing the vehicle's local path to perform actions like avoiding obstacles.

Image: In addition to planning a local path for the vehicle to follow in the near-term, the autonomous vehicle’s navigation system must be able to plan the complete route from the vehicle’s starting position to a destination. Chart above shows navigation system nodes.

In addition to planning a local path for the vehicle to follow in the near-term, the autonomous vehicle’s navigation system must be able to plan the complete route from the vehicle’s starting position to a destination. Chart above shows navigation system nodes.

Navigation System Nodes

Route Management

The Route Management Node’s primary responsibility is to provide an interface to the user allowing the user to choose an origin and a destination for the vehicle, either through a map interface, or by providing a Mission Data File (a file type used in the DARPA Urban Challenge). It will then calculate the route based on a priori information (e.g. data from a Route Network Definition File, an online mapping program, or an on-board database). The route data, which includes information about additional passing lanes and locations of intersections, will then be sent to the Advanced Navigation Node.

Advanced Navigation

The Advanced Navigation Node receives the route information from the Route Management Node. It uses this route data along with high-level object data to generate high-level behaviors that will instruct the vehicle to safely continue along the route while responding to vehicles, pedestrians, and other objects. It also includes behaviors to navigate in areas where route data does not exist, as well as instruct the vehicle to park itself. The commands generated by this node will be sent to the Basic Mobility Node.

Basic Mobility

The Basic Mobility Node receives the commands from the Advanced Navigation Node, which includes information about the route. It uses data about the route along with sensor information specific to vehicles, objects, and the road around it. It generates behaviors that will allow the vehicle to drive in a specific lane, change lanes, make turns, go around objects, follow other vehicles, and stop at stop signs. The commands generated from these behaviors will be sent to the Vehicle Path Node.

Vehicle Path

The Vehicle Path Node receives commands from the Basic Mobility Node, which includes information about the near-term desired route and speed the vehicle should travel. It creates the local path segments, which are compact representations of the local route that can be interpreted by the real-time controller. It sends these path segment commands to the Vehicle Controller Node. It also generates desired states of the vehicle’s auxiliary functions such as the turn signals and shifter. It sends these commands to the Vehicle Auxiliary Functions Node.

Vehicle Controller

The Vehicle Controller Node receives commands from the Vehicle Path Node, which contains information about the desired local path segments and speed profile of the vehicle. It uses this information to generate desired steering and speed commands at each point along the path. This node also uses vehicle and sensor state information to perform safety checks and generate safety behaviors. It sends the steering information to the Steering Controller and the speed information to the Speed Controller.

Vehicle Auxiliary

The Vehicle Auxiliary Node receives commands from the Vehicle Path Node, which contains information about the desired state of the auxiliary functions of the vehicle, such as the turn signals and the shifter. It then sends commands to the Shifting Controller and Signal Controller.

Navigation System on the MARTI Vehicle

The navigation and path planning system implemented on the MARTI vehicle includes:

Route Planning

This topic focuses on determining the global route, or how the vehicle will traverse from its present location to a destination, which could be very far away.

Image: Route planning focuses on determining the global route, or how the vehicle will traverse from its present location to a destination, which could be very far away.

Route planning focuses on determining the global route, or how the vehicle will traverse from its present location to a destination, which could be very far away.

Path Planning

This topic discusses creating a series of path segments that describe the path directly in front of (or behind) the vehicle.

Image: Path planning discusses creating a series of path segments that describe the path directly in front of (or behind) the vehicle.

Path planning discusses creating a series of path segments that describe the path directly in front of (or behind) the vehicle.

Speed Planning

The speed profile of the vehicle is created to set a safe and comfortable speed of the vehicle along the path.

Lane Changing/Obstacle Avoidance

The path segments need to be recomputed dynamically when the vehicle needs to pass another vehicle or avoid an obstacle.

Image: Lane changing/obstacle avoidance: The path segments need to be recomputed dynamically when the vehicle needs to pass another vehicle or avoid an obstacle.

Lane changing/obstacle avoidance: The path segments need to be recomputed dynamically when the vehicle needs to pass another vehicle or avoid an obstacle.

A framework for navigation and path planning has been successfully implemented on the MARTI vehicle using the 4D/RCS model architecture. The system consists of a long-term route planner to create a route to the destination, and a short-term path planner that considers passenger comfort and safety. Tying the two planners together is a supervisory system that switches the behavior of the vehicle based on situational input to perform basic on-road driving maneuvers. Future work includes adding advanced vehicle behaviors like parking lot navigation, responding to accidents, and adding a fuel conservation mode.

Related Terminology

autonomous vehicle navigation  •  driverless vehicle  •  path planning  •  global route planning  •  navigation system  •  navigation system nodes  •  obstacle avoidance  •  autonomous lane changing

Benefiting government, industry and the public through innovative science and technology
Southwest Research Institute® (SwRI®), headquartered in San Antonio, Texas, is a multidisciplinary, independent, nonprofit, applied engineering and physical sciences research and development organization with 10 technical divisions.

04/15/14