Used Det(ΔXi) instead of Det(Xi) in the 'p1' & 'p2' formulas.

[santhosh46]

I found two mistakes in the code, I'm not sure whether you were aware of it,

1. As per the Dolgov Et al. paper, the formula for 'p1' in final optimization is, Xi.ort(-Xi1)/(Det(Xi)Det(Xi1)), but in the Smoother::curvatureTerm function you used Det(ΔXi)Det(ΔXi1).
2. As per the Dolgov Et al. paper, the σκ(∆φi/|∆xi | -kmax) is a quadratic penalty function which after differentiation, you get 2(∆φi/|∆xi | -kmax)*(dki/dxi), instead you just considered the dki/dxi for correction. I believe if you correct these parameters the algorithm should work fine with non-zero curvature weight.

[hect1995]

@santhosh46 I do not understand why he does: gradient = wCurvature (0.25 kim1 + 0.5 ki + 0.25 kip1); And still after correcting what you mentioned I have the same problem, did you solve it? And I dont understand the penalization they do over kappa in the original paper. In my case I woul like kappa to be near 0 the whole time in order to make the curve smoother but with what they do, the further away you are from k_max, the bigger will be the gradient and the closer to kappa_max the smaller it will be which should be the opposite

[LawranceXcl]

@santhosh46 The second mistake you said is not the mistake. The (∆φi/|∆xi | -kmax) is constant.

[karlkurzer]

As I am not actively working on the project any longer I probably won't fix the smoother/curvature terms. If one of you guys wants to create a pull request however I'd be happy to incorporate the correct solution.

[santhosh46]

@hect1995 Hello,

Sorry for the too late reply. If u still have that problem it's not something u did, but the formula itself causes the behaviour, the curvature term is very unstable, the values changes very rapidly, no matter what values u tune for w_curv it'll behave the same. So is the reason I believe that weight is 0 in the repo. For overcoming this, I used a different method, the objective of curvature term is to stay between the max and min curvature limits for vehicle feasibility, so i smoothened the whole trajectory using Pure Pursuit Controller as in kuwatatcst09 paper (search for the exact keyword for the paper, it's an MIT motion planner fpr darpa. For every 10 iterations I'm smoothening the trajectory durimg optimization. So the resultant traj will be far from obstacles and it has feasibility too, the exact purpose of the Stanford paper, try that, it works like a charm. Hope this helps :)

[santhosh46]

@hect1995 Forgot to mention, gradient = wCurvature (0.25 kim1 + 0.5 ki + 0.25 kip1), u get this formula by finding the gradient of the curvature term in the objective function; it's not something he's using it random, that's the exact formula u will get by deriving it, try finding the gradient from objective function, it should give u the same. Correct me if I'm wrong.

[santhosh46]

@LawranceXcl Hello,

If that term is constant the derivative would have been zero, it's like, (d/dx)(f^2) = 2f*df/dx, if f is constant, df/dx is 0. In this case f = ki = delta(phi)/delta(xi). Correct me if I'm wrong.