I'd like to program a trajectory planner, let's say for a robot, and I can pass acceleration commands to the robot, the robot can move in one dimension. The outcome of the planner is now a vector with accelerations, for example 10 accelerations, and since I set the accelerations with a frequency of 2, this will be sufficient to steer the robot for the next 5 seconds.The trajectory should be planned in a way that (a) minimizes jerk and (b) is as near at the desired velocity as possible. So I know what makes up my fitness function: sum over squared jerks and squared differences v, v_desired.So I want to minimize jerk. I thought I search for a jerk profile (that is, jerk values at 10 discrete times, like 0.5, 1, ..., 5, to be consistent with the example above), integrate this twice so that I also have the speed profile, and apply my fitness function. The jerk profile is searched with a particle swarm optimizer. The nice thing is, 0 is already a good initialization for the profile.Then I thought about approximating the jerk profile with a polynomial, and not to search for the profile directly, but for the polynomial parameters. This way, integration would be trivial, everything would be steady and smooth, and I could sample points at whatever times I'd like. At least I thought this was a good idea.So my question is: Are polynomials a good tool for this? I can't believe that estimating e.g. 5 parameters (and evaluating the resulting polynomial at e.g. 30 positions) will yield better results than searching for 30 points (the jerk) directly. My experiments from the last hours shows that the curves very quickly start looking like waves, and that's not what you want for a trajectory, especially not for the velocity profile ...Is my approach with estimate function parameters rather than sample points bad? Is a polynomial bad? Are there better functions, easily to extend in number of parameters and trivially to integrate?I hope my question is understandable, if not, please complain and I will try being more specific.UPDATEThe picture below shows the result of my first approach, with optimizing parameters for a 4th degree polynomial for jerk. This was integrated with the correct initial values for a, v and s in my one-dimensional world. The initial v was set to 5.0, v desired is 2.0, and initial acceleration was 2.0. I would expect the velocity to first increase (caused by the initial acceleration), and then decrease slowly to 2. At point number 10, you can see the overshoot of v and the wiggly behaviour mentioned in the first comment.
Trajectory Planning: Polynomials as universal, integrable approximation functions?
optimization;polynomials
Ok, I experimented a lot today and using splines definitely looks promising. Although a little bit more computational effort to setup the splines (inversion of a matrix and so on, but I used Eigen3 for that), integration is rather trivial. With the polynomial representation, I can do a lot analytically, for example, the costs for deviation in velocity will become a difference of polynomials (one of a piecewise aka. spline and one of zeroth degree, constant), and it's exactly one step to get the costs, no need to draw samples at discrete points and sum them up. Don't know if I could need that, but it has been a good training at least.However, this is the first result my optimizer came up with, I optimized the support points for the splines (5 in this example, just conincidence that it's also the chosen time horizon on the horizontal axis).Compared with the first version with the high degree polynomial, this looks promising!