I'm strugling on that problem for a while, so any help is welcome. I need a trajectory representation that is performant for optimization, i.e. I want something that computes quickly. The function I want to optimize takes into account the duration of the trajectory, and samples some points along it to compute some costs related to the environment (distance to obstacles, time to collision,...) and the trajectory (local speed, acceleration and jerk limitation,...). For now I'm using a method based on waypoints and a dynamic local method that is too heavy for my usage. My optimization variables are the waypoints (position and a few derivatives), and the local method computes a spline between each successive waypoint. I think I can make profit of a less accurate method: I don't need to control the exact speed of each waypoints, nor their exact position and timing.
To be more specific, here are a few constraint I can think of:
- the optimization algorithm can have some control on the speed along the trajectory (e.g. by constraining the duration on portions of the trajectory, or specifying speed vector on waypoints...). The speed may strongly vary along the trajectory;
- I need a quick answer about the feasibility of the trajectory (low CPU usage);
- In the case the method is based on waypoints, I can use only the waypoints to compute my cost, then the computation of a specific state along the trajectory can be slow, I don't care;
- Otherwise, I need to quickly access a certain number of states to sample costs along the trajectory.
- Start and final positions of the trajectory are fixed. I may need to specify their speeds also;
- The trajectory is continuous in position, speed and acceleration, and jerk is bounded;
- For now I'm focusing on holonomic problems: 2D/3D navigation, with orientation, arm motion...
I've been thinking of B-Splines, or Bézier curves, but I don't know much of their maths, and I'm not sure whether they fit my requirements...
Hope my question is clear enough, any advice is welcome!
Note: I'm not looking for a motion-planner, I want to optimize an existing trajectory to global/local optimum regarding some cost and constraints.
