andrewwalker/dubins-curves

Path generation for the Dubin's car

Dubins-Curves

About

This software finds the shortest paths between configurations for the Dubins' car [Dubins51], the forward only car-like vehicle with a constrained turning radius. A good description of the equations and basic strategies for doing this are described in section 15.3.1 "Dubins Curves" of the book "Planning Algorithms" [LaValle06].

The approach used to find paths is based on the algebraic solutions published in [Shkel01]. However, rather than using angular symmetries to improve performance, the simpler approach to test all possible solutions is used here.

Usage

The recommended approach is to add dubins.c and dubins.h to your project and compile with an appropriate build system.

The repository includes a basic cmake example that demonstrates how to build and test the library.

Examples

The following code snippet demonstrates how to generate intermediate points along the shortest path between a pair of configuration (x, y, theta).

#include "dubins.h"
#include <stdio.h>

int printConfiguration(double q[3], double x, void* user_data) {
    printf("%f, %f, %f, %f\n", q[0], q[1], q[2], x);
    return 0;
}

int main()
{
    double q0[] = { 0,0,0 };
    double q1[] = { 4,4,3.142 };
    double turning_radius = 1.0;
    DubinsPath path;
    dubins_shortest_path( &path, q0, q1, turning_radius);
    dubins_path_sample_many( &path, 0.1, printConfiguration, NULL);
    return 0;
}

The following image shows some example paths, and the heading of the vehicle at each of the intermediate configurations.

Other Version

• There is a MATLAB Mex wrapper of this code on the MathWorks FileExchange
• There is a Python wrapper of this code available on GitHub and on PyPI

Citing

If you would like to cite this library in a paper or presentation, the following is recommended:

@Misc{DubinsCurves,
  author = {Andrew Walker},
  title  = {Dubins-Curves: an open implementation of shortest paths for the forward only car},
  year   = {2008--},
  url    = "https://github.com/AndrewWalker/Dubins-Curves"
}

Contributors

The Dubin's curves library was completed as one small part of [Walker11]. New contributions or bug fixes are welcome.

Key contributors to the project include:

• Francis Valentinis
• Royce Smart - who tested early versions of this code while writing up [Smart08].
• Scott Teuscher - who wrote the MATLAB Mex wrapper

License

MIT License. See LICENSE.txt for details.

References

[Dubins51]

Dubins, L.E. (July 1957). "On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents". American Journal of Mathematics 79 (3): 497–516

[LaValle06]

LaValle, S. M. (2006). "Planning Algorithms". Cambridge University Press

[Shkel01]

Shkel, A. M. and Lumelsky, V. (2001). "Classification of the Dubins set". Robotics and Autonomous Systems 34 (2001) 179–202

[Walker11]

Walker, A. (2011). "Hard Real-Time Motion Planning for Autonomous Vehicles", PhD thesis, Swinburne University.

[Smart08]

Royce, S. (2008). "Evolutionary Control of Autonomous Underwater Vehicles". PhD thesis, RMIT

dubins-curves's Issues

Hope for 3d dubins curves

Hope for 3d dubins curves.
Input:
Start Point(x, y, z, phi, gamma)
End Point(x', y', z', phi', gamma')
radius

Output:
Path and length

Sampling along Dubins path increases length to goal

Sampling is done along the Dubins path between a start point (16.2953, 0.12524, 0.575959) and and endpoint (17.2329, 2.0764, 2.28307). As the samples get closer to the endpoint, the distance between the current sample to the endpoint jumps at some point. Below is the test code, followed by the test output.

#include "dubins.h"
#include <stdio.h>

double start[] = { 16.2953, 0.12524, 0.575959 };
double end[] = { 17.2329, 2.0764, 2.28307 };

int printConfiguration(double q[3], double x, void* user_data) {
    DubinsPath path;
    dubins_shortest_path(&path, q, end, 1.0);

    printf("%f, %f, %f, %f, %f\n", q[0], q[1], q[2], x, dubins_path_length(&path));
    return 0;
}

int main()
{
    DubinsPath path;
    dubins_shortest_path(&path, start, end, 1.0);

    printf("#x,y,theta,t\n");
    dubins_path_sample_many(&path,  0.1, printConfiguration, NULL);

    return 0;
}

Test output:

0: 16.295300, 0.125240, 0.575959, 0.000000, 2.565464
1: 16.379776, 0.178753, 0.563946, 0.100000, 2.465464
2: 16.464292, 0.232206, 0.563946, 0.200000, 2.365464
3: 16.548807, 0.285658, 0.563946, 0.300000, 2.265464
4: 16.633322, 0.339111, 0.563946, 0.400000, 2.165464
5: 16.717837, 0.392564, 0.563946, 0.500000, 2.065464
6: 16.802353, 0.446016, 0.563946, 0.600000, 1.965464
7: 16.886868, 0.499469, 0.563946, 0.700000, 1.865464
8: 16.971383, 0.552921, 0.563946, 0.800000, 1.765464
9: 17.055107, 0.607576, 0.617606, 0.900000, 7.569978
10: 17.133605, 0.669461, 0.717606, 1.000000, 7.848649
11: 17.205533, 0.738874, 0.817606, 1.100000, 7.748649
12: 17.270171, 0.815121, 0.917606, 1.200000, 7.648649
13: 17.326875, 0.897439, 1.017606, 1.300000, 7.548649
14: 17.375077, 0.985008, 1.117606, 1.400000, 7.448649
15: 17.414296, 1.076951, 1.217606, 1.500000, 7.348649
16: 17.444140, 1.172350, 1.317606, 1.600000, 7.248649
17: 17.464311, 1.270252, 1.417606, 1.700000, 7.148649
18: 17.474608, 1.369678, 1.517606, 1.800000, 7.048649
19: 17.474927, 1.469636, 1.617606, 1.900000, 6.948649
20: 17.465265, 1.569126, 1.717606, 2.000000, 6.848649
21: 17.445719, 1.667155, 1.817606, 2.100000, 6.748649
22: 17.416484, 1.762743, 1.917606, 2.200000, 6.648649
23: 17.377852, 1.854934, 2.017606, 2.300000, 6.548649
24: 17.330210, 1.942808, 2.117606, 2.400000, 6.448649
25: 17.274033, 2.025487, 2.217606, 2.500000, 6.348649

The distance spikes from sample 8 to 9, not sure if this can be considered expected behavior.

Configuration that does not obtain the shortest path

First of all, thank you for your source code, it is a good work! However, I have been testing it and I think I have found some unexpected result. I tried to find the shortest path for the inputs:

p1: (0, 0, pi/2)
p2: (4, 0, -pi/2)
Minimum turning radius(rho): 3

I think the shortest path for this configuration it is:

But I'm getting this one:

The code is retrieving a RLR trajectory instead of a LRL. May you tell me if it is a bug or if there is another thing I'm not taking into account?

A question about the dubins_LRL function

Hello, I have a question about the function dubins_LRL, I read the paper "Classification of the Dubins set", and in the paper p= mode2pi(acos(tmp0)), but in your implementation p = mode2pi(2*pi - acos(tmp0)), and out[2] is also different. Could you please tell me why your implementation is different from the paper? Thank you!

int dubins_LRL(DubinsIntermediateResults* in, double out[3])
{
double tmp0 = (6. - in->d_sq + 2in->c_ab + 2in->d*(in->sb - in->sa)) / 8.;
double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb );
if( fabs(tmp0) <= 1) {
double p = mod2pi( 2*M_PI - acos( tmp0) );
double t = mod2pi(-in->alpha - phi + p/2.);
out[0] = t;
out[1] = p;
out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p));
return EDUBOK;
}
return EDUBNOPATH;
}

Algorithm

Hello,

The code works great!!

Please include a file explaining the Algorithm in generating Dubin's path.

Thanks
Avinash

about the turning radius

Thanks for your code,It really help me a lot,But I can't find the turning radius value in your code.Could you tell me that?I really appreciate your help!

CCC

Hello,

I am working on dubins path, and your code works great!!!.

I wonder if your code includes CCC configuration? For any set of initial and final points, I could get only CSC configuration. If the code does include CCC configuration, please suggest some set of initial and final conditions, which results in this.

Thanks

Matlab wrapper is outdated

The Matlab wrapper found here: http://au.mathworks.com/matlabcentral/fileexchange/40655-dubins-curve-mex uses a old version of this code and does not compile on newer versions of Matlab.

I have published a updated wrapper here: https://gist.github.com/Lauszus/e150347185f6012735b776d06aa6ca48 which uses the code from this repository directly, so any future releases should work as well as long as the API does not change.

I have also included a small example which shows how to use this code to generate paths from some waypoints.

Expose configurations that changes between words occur

Making this change will support more effective interpolation strategies (for CSC curves, only the endpoints of the straight segment need to be considered).