Advanced Search

Hi,

I had some real problems related to the clothoid fit.

It turned out eq.45 in the abstract is partially wrong.
In the abstract page.7 there is stated : DeltaTheta = Thetai4 - Thetai0.

Now this is most off time ok. But not alway's ok. The problem lies in the way DeltaTheta is calculated.
The input theta's are in range [0,2*pi], [0,-2*pi], derived from atan2(x,y). This are results of decomposed 3d vectors,
as the clothoid uses inputs for xy and z seperatly.

eq.45 Delta Theta [dt] uses - or + for direction. This is ok, so we know we must use - and +.
Simply substracting the theta's may lead's to incorrect values, wich results in a self intersecting clothoid for example,
caused by a extra + or - pi rotations.

I had to think a while how to solve this problem. To solve it, i calculated the theta's back into xy vectors and then
calculated the signed angle between these two xy vectors. Here is the solution in code.
So far above problem seems to be solved now.
I send a email to the abstract creator's and informed them about the problem.

Furthermore, the exact path deviation is used to fit the clothoid.

In total for one clothoid fit, ceres non-linear solver is used 3 times.

1. Ceres finds the clothoid xyz endpoint, given the 3 unknowns y11, y21, s1.
2. Ceres finds the closest point in space to the clothoid curve. (helper function to calculate path deviation).
3. Ceres finds the inperpolated points on curveA & curveB given the max path deviation.

The performance is about ~25ms to fit the clothoid.

Some fit clothoid curve fitting example's i used for validation :

// Line line fit.

// Arc line fit.

// Arc arc fit.

// No deviation is used here.

If you have absolute encoders, I think the best solution is to buy new servo drives and motors. Some good brands are Yaskawa, Delta, and CTB. I personally chose Adtech