How to efficiently evaluate or approximate a road Clothoid?

Question

I'm facing the problem of computing values of a clothoid in C in real-time.

First I tried using the Matlab coder to obtain auto-generated C code for the quadgk-integrator for the Fresnel formulas. This essentially works great in my test scnearios. The only issue is that it runs incredibly slow (in Matlab as well as the auto-generated code).

Another option was interpolating a data-table of the unit clothoid connecting the sample points via straight lines (linear interpolation). I gave up after I found out that for only small changes in curvature (tiny steps along the clothoid) the results were obviously degrading to lines. What a surprise...

I know that circles may be plotted using a different formula but low changes in curvature are often encountered in real-world-scenarios and 30k sampling points in between the headings 0° and 360° didn't provide enough angular resolution for my problems.

Then I tried a Taylor approximation around the R = inf point hoping that there would be significant curvatures everywhere I wanted them to be. I soon realized I couldn't use more than 4 terms (power of 15) as the polynom otherwise quickly becomes unstable (probably due to numerical inaccuracies in double precision fp-computation). Thus obviously accuracy quickly degrades for large t values. And by "large t values" I'm talking about every point on the clothoid that represents a curve of more than 90° w.r.t. the zero curvature point.

For instance when evaluating a road that goes from R=150m to R=125m while making a 90° turn I'm way outside the region of valid approximation. Instead I'm in the range of 204.5° - 294.5° whereas my Taylor limit would be at around 90° of the unit clothoid.

I'm kinda done randomly trying out things now. I mean I could just try to spend time on the dozens of papers one finds on that topic. Or I could try to improve or combine some of the methods described above. Maybe there even exists an integrate function in Matlab that is compatible with the Coder and fast enough.

This problem is so fundamental it feels to me I shouldn't have that much trouble solving it. any suggetions?

Answer 1

about the 4 terms in Taylor series - you should be able to use much more. total theta of 2pi is certainly doable, with doubles.

you're probably calculating each term in isolation, according to the full formula, calculating full factorial and power values. that is the reason for losing precision extremely fast.

instead, calculate the terms progressively, the next one from the previous one. Find the formula for the ratio of the next term over the previous one in the series, and use it.

For increased precision, do not calculate in theta by rather in the distance, s (to not lose the precision on scaling).

your example is an extremely flat clothoid. if I made no mistake, it goes from (25/22) pi =~ 204.545° to (36/22) pi =~ 294.545° _{(why not include these details in your question?)}. Nevertheless it should be OK. Even 2 pi = 360°, the full circle (and twice that), should pose no problem.

given: r = 150 -> 125,  90 degrees turn :

  r s = A^2 = 150 s = 125 (s+x)  

  =>  1+(x/s) = 150/125 = 1 + 25/125       x/s = 1/5  

  theta = s^2/2A^2 = s^2 / (300 s) = s / 300  ;   = (pi/2) * (25/11)  = 204.545°
  theta2 = (s+x)^2/(300 s) = (6/5)^2 s / 300  ;   = (pi/2) * (36/11)  = 294.545°
  theta2 - theta = ( 36/25 - 1 ) s / 300 == pi/2

  =>  s = 300 * (pi/2) * (25/11) = 1070.99749554    x = s/5 = 214.1994991

      A^2 = 150 s = 150 * 300 * (pi/2) * (25/11) 

      a = sqrt (2 A^2) = 300 sqrt ( (pi/2) * (25/11) ) = 566.83264608

The reference point is at r = Infinity, where theta = 0.

we have x = a INT[u=0..(s/a)] cos(u^2) d(u) where a = sqrt(2 r s) and theta = (s/a)^2. write out the Taylor series for cos, and integrate it, term-by-term, to get your Taylor approximation for x as function of distance, s, along the curve, from the 0-point. that's all.

next you have to decide with what density to calculate your points along the clothoid. you can find it from a desired tolerance value above the chord, for your minimal radius of 125. these points will thus define the approximation of the curve by line segments, drawn between the consecutive points.

Answer 2

I am doing my thesis in the same area right now.

My approach is the following.

1. at each point on your clothoid, calculate the following (change in heading / distance traveled along your clothoid), by this formula you can calculate the curvature at each point by this simple equation.

2. you are going to plot each curvature value, your x-axis will be the distance along the clothoid, the y axis will be the curvature. By plotting this and applying very easy linear regression algorithm (search for Peuker algorithm implementation in your language of choice)

3. you can easily identify where are the curve sections with value of zero (Line has no curvature), or linearly increasing or decreasing (Euler spiral CCW/CW), or constant value != 0 (arc has constant curvature across all points on it).

I hope this will help you a little bit.

You can find my code on github. I implemented some algorithms for such problems like Peuker Algorithm.