search

HKUST-Aerial-Robotics / Fast-Planner

A Robust and Efficient Trajectory Planner for Quadrotors
GNU General Public License v3.0
2.4k stars 665 forks source link

Question about the smoothness Cost #58

Closed taec1227 closed 4 years ago

taec1227 commented 4 years ago

Hi, I'm looking at the bspline optimization part in your project, and have a question about the calcSmoothnessCost function.

In the paper, the cost is formulated as: Screenshot from 2020-09-24 17-05-28

However, from the code, it shows like jerk = q[i + 3] - 3 q[i + 2] + 3 q[i + 1] - q[i]; cost += jerk.squaredNorm();

  1. Is the function in paper wrong? If so, what is the meaning of the code one?
  2. I found that when optimize the control points, there is only a hard coded bound on position, how to guarantee the safety of the optimized trajectory since the cost function is formulated with soft constraints?(Shouldn't the bound be a set of free corridors to guarantee the safety?)
ZbyLGsc commented 4 years ago
  1. Cost in the paper corresponds to second order derivative of B-spline, while that in the code corresponds to third order one (jerk). Both of them can be used.
  2. There is no 'hard coded bound', we penalize points that are close to distance. The distance is queried from ESDF.
taec1227 commented 4 years ago

Thanks for your reply, and there is another function named 'evaluateDeBoor' which confused me a lot. I know that the deBoor is the recursion of the b-spline basis, however, the 'evaluateDeBoor' seems to use control points to do a recursion, I didn't find any theory to support it, do I misunderstand any useful formulation?

ZbyLGsc commented 4 years ago

What you describe is called Cox-de Boor recursion formula, which evaluates the basis function explicitly. de Boor's algorithm is different. It is based on an equivalent recursive formula that does not evaluate the basis function.