June 29, 2021

A Recipe for Dubins Paths

Six paths diverged in a yellow wood, and I— / I took the one least length implied, / And that has made all the difference.

Intro

Dubins' paths are a useful tool for robot path planning, in part because they're so simple. They're the shortest paths for a robot with a minimum turning radius (like your car), with the added constraint that the robot can only move forward. This article presents geometry and code to construct Dubins paths between two robot poses in 2d.

Dubins paths solve the following problem: Given the (x,y,θ) state of your robot, an (x,y,θ) goal state, and your robot's minimum turning radius R, construct the shortest driveable path from start to goal. Assume your robot can only move forward at constant speed.

In 1957, Lester Eli Dubins proved the solutions to this problem can be constructed using only two types of geometric primitives: (1) circles with minimum turning radius and (2) straight lines.

Depending on the start pose, the goal pose, and the turning radius, the shortest path will always take one of six forms. The six types of Dubins path are named LRL, LSL, LSR, RLR, RSR, and RSL, where R stands for "right turn", L stands for "left turn", and S stands for "straight line".

In the diagrams above, black arrows indicate start and goal poses. Red arcs indicate right turns, blue arcs indicate left turns, and green lines indicate straight segments.

LSL, LSR, RSL, and RSR paths are composed of two circular arcs connected by a shared tangent line segment. Collectively, they are referred to as CSC paths—Circle, Straight, Circle. The remaining two path types, LRL and RLR, are grouped under the label CCC—Circle, Circle, Circle (duh!).

One of the six paths will always be the shortest possible, but which one? To keep things simple, I'll show you how to generate all six paths and their lengths. Then, you can pick the shortest option. Soueres and Boissonnat derive rules to analytically select the shortest path type without generating all six paths, but on modern computers the potential speedup (probably) isn't worth the added complexity.

"Keep moving forward."

-L. E. Dubins (maybe)

Dubins Path Ingredients

Dubins paths are constructed from state vectors, tangent circles, and tangent lines.

Dubins State

The dubins car is a rigid body that rotates and translates in the euclidean plane. Its state is an (x, y, h) coordinate triple containing its heading and 2D position.


              typedef struct DubinsState {
                double x;    // X coordinate
                double y;    // Y coordinate
                double h;    // Robot heading (radians)
              } DubinsState;
            

The input to the dubins path generator is an initial and a final state. The goal of the path generator is to generate a dubins curve connecting the initial state to the final state.

State Tangent Circles

Dubins paths begin and end with arc segments lying tangent to the rover's initial and final state. The radius of the state tangent circles is equal to the car's minimum turning radius, or the inverse of its maximum curvature.


              typedef struct DubinsCircle {
                double x;
                double y;
                double r;
                DubinsDirection direction;
              } DubinsCircle;
            

To find the center of the state tangent circles, construct a unit vector pointing in the direction of the car's heading. Scale the vector by the minimum turning radius, and rotate it 90 degrees to either the left or the right.


          static void jsf__state_tangent_circle(const DubinsState *state, double radius,
            const DubinsDirection direction, DubinsCircle *tangent_circle) {
            // Rotate CCW for the LEFT tangent circle, CW for the right tangent circle.
            double alpha = (direction == DUBINS_LEFT) ? M_PI / 2.0 : -M_PI / 2.0;
            tangent_circle->x = state->x + radius * cos(alpha + state->h);
            tangent_circle->y = state->y + radius * sin(alpha + state->h);
            tangent_circle->r = radius;
            tangent_circle->direction = direction;
          }
        

It is helpful to record the turning direction of each tangent circle. Later on, it will be important to know which circles represent a left turn and which circles represent a right turn.

Tangent Lines

The LSL, LSR, RSL, and RSR path types connect the state tangent circles with tangent line segments. Here, we represent those line segments as a pair of (x, y) coordinates.


          typedef struct DubinsLine {
            double x0, y0;
            double x1, y1;
          } DubinsLine;
        

There are two types of tangent lines connecting any pair of non-overlapping circles, interior and exterior. Interior tangent lines cross between the circles; exterior tangent lines do not.

Warning: The tangent-finding routines presented here are only appropriate for connecting two circles with equal radius. In the Dubins case, this is guaranteed, but other use-cases will likely require a more general approach.

Exterior Tangent Lines

Etiam non dignissim mauris, vitae euismod tortor. Praesent in felis enim. Morbi mollis faucibus scelerisque. Suspendisse at finibus diam. Cras sodales, magna ac iaculis efficitur, neque nunc pharetra justo, et blandit elit massa ac erat. Curabitur vestibulum neque in sem mollis convallis nec id neque. Proin rhoncus sapien urna, in tempus tortor maximus sit amet. Duis sed consectetur lectus.


          static bool jsf__tangent_line(DubinsCircle *c0, DubinsCircle *c1,
                                        DubinsLine *tangent_line) {
            // This method requires both circles to have the same radius.
            if (fabs(c0->r - c1->r) > DUBINS_EPSILON) {
              return false;
            }

            // Compute the distance from c0 to c1.
            double c0c1_x = c1->x - c0->x;
            double c0c1_y = c1->y - c0->y;
            double D = jsf__norm(c0c1_x, c0c1_y);

            // Normalize the vector from the center of c0 to c1.
            double c0c1_hat_x = c0c1_x / D;
            double c0c1_hat_y = c0c1_y / D;

            if (c0->direction == c1->direction) {
              // Construct an exterior tangent line.

              // If the circles are identical, there is no unique tangent line.
              if (fabs(D) < DUBINS_EPSILON) {
                return false;
              }

              // Connect two LEFT circles by rotating clockwise 90 degrees.
              // Connect two RIGHT circles by rotating counter-clockwise 90 degrees.
              double rot_x, rot_y;
              if (c0->direction == DUBINS_LEFT) {
                rot_x =  c0c1_hat_y;
                rot_y = -c0c1_hat_x;
              } else {
                rot_x = -c0c1_hat_y;
                rot_y =  c0c1_hat_x;
              }

              // Find the tangent points on c0 and c1 that create the tangent line.
              tangent_line->x0 = c0->x + c0->r * rot_x;
              tangent_line->y0 = c0->y + c0->r * rot_y;
              tangent_line->x1 = tangent_line->x0 + c0c1_x;
              tangent_line->y1 = tangent_line->y0 + c0c1_y;
            } else {
              // Construct an interior tangent line... (see below)
            }
            return true;
          }
        

Etiam non dignissim mauris, vitae euismod tortor. Praesent in felis enim. Morbi mollis faucibus scelerisque. Suspendisse at finibus diam. Cras sodales, magna ac iaculis efficitur, neque nunc pharetra justo, et blandit elit massa ac erat. Curabitur vestibulum neque in sem mollis convallis nec id neque. Proin rhoncus sapien urna, in tempus tortor maximus sit amet. Duis sed consectetur lectus.

Interior Tangent Lines

Etiam non dignissim mauris, vitae euismod tortor. Praesent in felis enim. Morbi mollis faucibus scelerisque. Suspendisse at finibus diam. Cras sodales, magna ac iaculis efficitur, neque nunc pharetra justo, et blandit elit massa ac erat. Curabitur vestibulum neque in sem mollis convallis nec id neque. Proin rhoncus sapien urna, in tempus tortor maximus sit amet. Duis sed consectetur lectus.

Etiam non dignissim mauris, vitae euismod tortor. Praesent in felis enim. Morbi mollis faucibus scelerisque. Suspendisse at finibus diam. Cras sodales, magna ac iaculis efficitur, neque nunc pharetra justo, et blandit elit massa ac erat. Curabitur vestibulum neque in sem mollis convallis nec id neque. Proin rhoncus sapien urna, in tempus tortor maximus sit amet. Duis sed consectetur lectus.

Etiam non dignissim mauris, vitae euismod tortor. Praesent in felis enim. Morbi mollis faucibus scelerisque. Suspendisse at finibus diam. Cras sodales, magna ac iaculis efficitur, neque nunc pharetra justo, et blandit elit massa ac erat. Curabitur vestibulum neque in sem mollis convallis nec id neque. Proin rhoncus sapien urna, in tempus tortor maximus sit amet. Duis sed consectetur lectus.


          static bool jsf__tangent_line(DubinsCircle *c0, DubinsCircle *c1,
                                        DubinsLine *tangent_line) {
            // This method requires both circles to have the same radius.
            if (fabs(c0->r - c1->r) > DUBINS_EPSILON) {
              return false;
            }

            // Compute the distance from c0 to c1.
            double c0c1_x = c1->x - c0->x;
            double c0c1_y = c1->y - c0->y;
            double D = jsf__norm(c0c1_x, c0c1_y);

            // Normalize the vector from the center of c0 to c1.
            double c0c1_hat_x = c0c1_x / D;
            double c0c1_hat_y = c0c1_y / D;

            if (c0->direction == c1->direction) {
              // Construct an exterior tangent line... (see above)
            } else {
              // Construct an interior tangent line.

              // If the circles overlap, there is no interior tangent line.
              if (fabs(D) < 2 * c0->r) {
                return false;
              }

              // Compute alpha, the angle between the c0c1 vector and the tangent points.
              double alpha = acos(c0->r / (0.5 * D));

              // Rotate by alpha. CW for LEFT to RIGHT and CCW for RIGHT to LEFT.
              double rot_x, rot_y;
              double c = cos(alpha);
              double s = sin(alpha);
              if (c0->direction == DUBINS_LEFT) {
                rot_x =  c * c0c1_hat_x + s * c0c1_hat_y;
                rot_y = -s * c0c1_hat_x + c * c0c1_hat_y;
              } else {
                rot_x = c * c0c1_hat_x - s * c0c1_hat_y;
                rot_y = s * c0c1_hat_x + c * c0c1_hat_y;
              }

              tangent_line->x0 = c0->x + c0->r * rot_x;
              tangent_line->y0 = c0->y + c0->r * rot_y;

              tangent_line->x1 = c1->x - c1->r * rot_x;
              tangent_line->y1 = c1->y - c1->r * rot_y;
            }
            return true;
          }
        

Etiam non dignissim mauris, vitae euismod tortor. Praesent in felis enim. Morbi mollis faucibus scelerisque. Suspendisse at finibus diam. Cras sodales, magna ac iaculis efficitur, neque nunc pharetra justo, et blandit elit massa ac erat. Curabitur vestibulum neque in sem mollis convallis nec id neque. Proin rhoncus sapien urna, in tempus tortor maximus sit amet. Duis sed consectetur lectus.

Circle-Connecting Circles

Etiam non dignissim mauris, vitae euismod tortor. Praesent in felis enim. Morbi mollis faucibus scelerisque. Suspendisse at finibus diam. Cras sodales, magna ac iaculis efficitur, neque nunc pharetra justo, et blandit elit massa ac erat. Curabitur vestibulum neque in sem mollis convallis nec id neque. Proin rhoncus sapien urna, in tempus tortor maximus sit amet. Duis sed consectetur lectus.


          static bool jsf__circle_tangent_circle(const DubinsCircle *c0,
                                                 const DubinsCircle *c1,
                                                 DubinsCircle *tangent_circle) {
            // Input circle radii must match.
            if (fabs(c0->r - c1->r) > DUBINS_EPSILON) {
              return false;
            }

            // Input circle directions must match.
            if (c0->direction != c1->direction) {
              return false;
            }

            // Compute the distance from c0 to c1.
            double c0c1_x = c1->x - c0->x;
            double c0c1_y = c1->y - c0->y;
            double D = jsf__norm(c0c1_x, c0c1_y);

            // No tangent circle (with same radius as c0 and c1)
            // is possible because c0 and c1 are too far apart.
            if (D > 4 * c0->r) {
              return false;
            }

            // Normalize the vector from the center of c0 to c1.
            double c0c1_hat_x = c0c1_x / D;
            double c0c1_hat_y = c0c1_y / D;

            // Compute the angle by which to rotate.
            double alpha = acos(D / 4 * (c0->r));
            alpha *= (c0->direction == DUBINS_RIGHT) ? -1.0 : 1.0;

            // Rotate c0c1_hat by alpha.
            double c = cos(alpha);
            double s = sin(alpha);
            double rot_x = c * c0c1_hat_x - s * c0c1_hat_y;
            double rot_y = s * c0c1_hat_x + c * c0c1_hat_y;

            // Compute the center of the tangent circle.
            tangent_circle->x = c0->x + 2 * c0->r * rot_x;
            tangent_circle->y = c0->y + 2 * c0->r * rot_y;

            // Fill in everything else.
            if (c0->direction == DUBINS_RIGHT) {
              tangent_circle->direction = DUBINS_LEFT;
            } else {
              tangent_circle->direction = DUBINS_RIGHT;
            }
            tangent_circle->r = c0->r;

            return true;
          }
        

Path Construction

Etiam non dignissim mauris, vitae euismod tortor. Praesent in felis enim. Morbi mollis faucibus scelerisque. Suspendisse at finibus diam. Cras sodales, magna ac iaculis efficitur, neque nunc pharetra justo, et blandit elit massa ac erat. Curabitur vestibulum neque in sem mollis convallis nec id neque. Proin rhoncus sapien urna, in tempus tortor maximus sit amet. Duis sed consectetur lectus.


          typedef struct DubinsPath {
            double curvatures[3];
            double arclengths[3];
            DubinsPathType type;
          } DubinsPath;
        

Etiam non dignissim mauris, vitae euismod tortor. Praesent in felis enim. Morbi mollis faucibus scelerisque. Suspendisse at finibus diam. Cras sodales, magna ac iaculis efficitur, neque nunc pharetra justo, et blandit elit massa ac erat. Curabitur vestibulum neque in sem mollis convallis nec id neque. Proin rhoncus sapien urna, in tempus tortor maximus sit amet. Duis sed consectetur lectus.


          static double jsf__angle_between(double x0, double y0, double x1, double y1,
                                           DubinsDirection direction) {
            double n0 = jsf__norm(x0, y0);
            double n1 = jsf__norm(x1, y1);

            double arg = (x0 * x1 + y0 * y1) / (n0 * n1);

            // Clamp to [-1, +1] to avoid domain errors caused by rounding.
            arg = fmin(fmax(arg, -1.0), 1.0);

            double angle = acos(arg);

            // The sign of this determinant gives the
            // direction of rotation from (x0,y0) to (x1, y1).
            double cross2d = x0 * y1 - x1 * y0;
            if (direction == DUBINS_LEFT && cross2d < 0) {
              angle = 2 * M_PI - angle;
            }
            if (direction == DUBINS_RIGHT && cross2d > 0) {
              angle = 2 * M_PI - angle;
            }
            // Round anything close to 2pi to 0.
            // Otherwise, paths will do full circles when they shouldn't.
            if (fabs(angle - 2 * M_PI) < DUBINS_EPSILON) {
              angle = 0.0;
            }
            return angle;
          }
        

Circle Straight Circle (CSC)

Etiam non dignissim mauris, vitae euismod tortor. Praesent in felis enim. Morbi mollis faucibus scelerisque. Suspendisse at finibus diam. Cras sodales, magna ac iaculis efficitur, neque nunc pharetra justo, et blandit elit massa ac erat. Curabitur vestibulum neque in sem mollis convallis nec id neque. Proin rhoncus sapien urna, in tempus tortor maximus sit amet. Duis sed consectetur lectus.


          static void jsf__construct_csc(const DubinsState *s0, const DubinsState *s1,
                                         const DubinsCircle *c0, const DubinsLine *line,
                                         const DubinsCircle *c1, DubinsPath *path) {

            // Arc from s0, around c0, to line0
            {
              double v0x =    s0->x - c0->x; double v0y =    s0->y - c0->y;
              double v1x = line->x0 - c0->x; double v1y = line->y0 - c0->y;
              path->arclengths[0] =
                  jsf__angle_between(v0x, v0y, v1x, v1y, c0->direction) * c0->r;
              path->curvatures[0] =
                  (c0->direction == DUBINS_LEFT) ? 1.0 / c0->r : -1.0 / c0->r;
            }

            // Straight line from line0 to line1
            {
              path->arclengths[1] = jsf__norm(line->x1 - line->x0, line->y1 - line->y0);
              path->curvatures[1] = 0.0;
            }

            // Arc from line1, around c1, to s1
            {
              double v0x = line->x1 - c1->x; double v0y = line->y1 - c1->y;
              double v1x =    s1->x - c1->x; double v1y =    s1->y - c1->y;
              path->arclengths[2] =
                  jsf__angle_between(v0x, v0y, v1x, v1y, c1->direction) * c1->r;
              path->curvatures[2] =
                  (c1->direction == DUBINS_LEFT) ? 1.0 / c1->r : -1.0 / c1->r;
            }
          }
        

Circle Circle Circle (CCC)

Etiam non dignissim mauris, vitae euismod tortor. Praesent in felis enim. Morbi mollis faucibus scelerisque. Suspendisse at finibus diam. Cras sodales, magna ac iaculis efficitur, neque nunc pharetra justo, et blandit elit massa ac erat. Curabitur vestibulum neque in sem mollis convallis nec id neque. Proin rhoncus sapien urna, in tempus tortor maximus sit amet. Duis sed consectetur lectus.


          static void jsf__construct_ccc(const DubinsState *s0, const DubinsState *s1,
                                         const DubinsCircle *c0, const DubinsCircle *c1,
                                         const DubinsCircle *c2, DubinsPath *path) {
            // Tangent point 0 is the average of the centers of c0 and c1.
            double tp0x = 0.5 * (c0->x + c1->x);
            double tp0y = 0.5 * (c0->y + c1->y);

            // Tangent point 1 is the average of the centers of c1 and c2.
            double tp1x = 0.5 * (c1->x + c2->x);
            double tp1y = 0.5 * (c1->y + c2->y);

            // Arc from s0, around c0, to tp0
            {
              double v0x = s0->x - c0->x; double v0y = s0->y - c0->y;
              double v1x =  tp0x - c0->x; double v1y =  tp0y - c0->y;
              path->arclengths[0] =
                  jsf__angle_between(v0x, v0y, v1x, v1y, c0->direction) * c0->r;
              path->curvatures[0] =
                  (c0->direction == DUBINS_LEFT) ? 1.0 / c0->r : -1.0 / c0->r;
            }

            // Arc from tp0, around c1, to tp1
            {
              double v0x = tp0x - c1->x; double v0y = tp0y - c1->y;
              double v1x = tp1x - c1->x; double v1y = tp1y - c1->y;
              path->arclengths[1] =
                  jsf__angle_between(v0x, v0y, v1x, v1y, c1->direction) * c1->r;
              path->curvatures[1] =
                  (c1->direction == DUBINS_LEFT) ? 1.0 / c1->r : -1.0 / c1->r;
            }

            // Arc from tp1, around c2, to s1
            {
              double v0x =  tp1x - c2->x; double v0y =  tp1y - c2->y;
              double v1x = s1->x - c2->x; double v1y = s1->y - c2->y;
              path->arclengths[2] =
                  jsf__angle_between(v0x, v0y, v1x, v1y, c2->direction) * c2->r;
              path->curvatures[2] =
                  (c2->direction == DUBINS_LEFT) ? 1.0 / c2->r : -1.0 / c2->r;
            }
          }
        

Etiam non dignissim mauris, vitae euismod tortor. Praesent in felis enim. Morbi mollis faucibus scelerisque. Suspendisse at finibus diam. Cras sodales, magna ac iaculis efficitur, neque nunc pharetra justo, et blandit elit massa ac erat. Curabitur vestibulum neque in sem mollis convallis nec id neque. Proin rhoncus sapien urna, in tempus tortor maximus sit amet. Duis sed consectetur lectus.

Path Selection

Etiam non dignissim mauris, vitae euismod tortor. Praesent in felis enim. Morbi mollis faucibus scelerisque. Suspendisse at finibus diam. Cras sodales, magna ac iaculis efficitur, neque nunc pharetra justo, et blandit elit massa ac erat. Curabitur vestibulum neque in sem mollis convallis nec id neque. Proin rhoncus sapien urna, in tempus tortor maximus sit amet. Duis sed consectetur lectus.

Etiam non dignissim mauris, vitae euismod tortor. Praesent in felis enim. Morbi mollis faucibus scelerisque. Suspendisse at finibus diam. Cras sodales, magna ac iaculis efficitur, neque nunc pharetra justo, et blandit elit massa ac erat. Curabitur vestibulum neque in sem mollis convallis nec id neque. Proin rhoncus sapien urna, in tempus tortor maximus sit amet. Duis sed consectetur lectus.

Path Sampling

You can find the state of a Dubins car at a given distance along a Dubins path by forward simulation.


          DubinsState jsf_dubins_path_sample(const DubinsState *initial_state,
                                             const DubinsPath *path,
                                             const double arclength) {

            const int num_arcs = sizeof(path->arclengths) / sizeof(path->arclengths[0]);

            // Record the index of the sample-containing arc.
            int arc_idx = 0;                // Record the index of the sample-containing arc.
            double arc_prefix_length = 0.0; // Record the length of the path before the start of the sample-containing arc.

            // Seek forward to find the arc that contains this sample.
            while (arc_idx < num_arcs &&
                   arc_prefix_length + path->arclengths[arc_idx] < arclength) {
              arc_prefix_length += path->arclengths[arc_idx];
              arc_idx++;
            }

            // Find the state at the start of the arc containing the sample.
            DubinsState state = *initial_state;
            for (int i = 0; i < num_arcs && i <= arc_idx; ++i) {
              double k = path->curvatures[i];
              double s = path->arclengths[i];

              // If this arc contains the sample,
              // only travel partially along it to find the sample state.
              if (i == arc_idx) {
                s = arclength - arc_prefix_length;
              }

              // Don't try to sample before the beginning of the path.
              s = fmax(0.0, s);

              // Arc is a straight line.
              if (k == 0.0) {
                state.x += s * cos(state.h);
                state.y += s * sin(state.h);
              } else {
                // Compute the radius of this arc.
                double r = 1.0 / k;

                // Find the tangent circle to the current state in the direction of this arc.
                DubinsCircle circle;
                {
                  DubinsDirection dir = (k > 0.0) ? DUBINS_LEFT : DUBINS_RIGHT;
                  jsf__state_tangent_circle(&state, fabs(r), dir, &circle);
                }

                double dh = s * k;
                state.x = circle.x + r * cos(state.h - M_PI / 2.0 + dh);
                state.y = circle.y + r * sin(state.h - M_PI / 2.0 + dh);
                state.h += dh;
              }
            }
            return state;
          }
        

Wait, back up.

Dubins paths are great, but what if your robot can drive backwards? In that case, the shortest paths are known as Reeds-Shepp paths. Reeds-Shepp paths are constructed using the same circle and line primitives, but because the robot can change direction, there are 46 path types instead of 6. Reeds-Shepp is a lot more complicated—a topic for another post!

"Take two steps forward, one step back."

-J.A. Reeds & L.A. Shepp

More Resources

Planning Algorithms, by Steven M. Lavalle
A Step-by-Step Tutorial to Computing Dubin's Paths, by Andy G. (gieseanw)
Isochrons for a Dubins Car, from the Wolfram Demonstrations Project
Shortest Path for the Dubins Car, from the Wolfram Demonstrations Project