Quote:
Originally Posted by Jared Russell
In 2014, by having conservative limits on these quantities and not having lots of sharp turns. (Inelegant but good enough.)
For 2015 and beyond, by assigning maximum velocities to each point on the path based on curvature and working backwards to respect the other limits. This is the "correct" way to do it, and fairly straightforward for the limited acceleration/unlimited jerk case. But it can be very complicated for limited jerk control.
|
Do you think you could help me figure out a good way to do this? I've been struggling with this for a while. Currently, I've been able to calculate the radius of curvature at each point, and from that, I can calculate the maximum velocity at each point. If I calculate velocities starting at the beginning and moving to the end, I'll end up going too fast before the curve, and I won't be able to slow down to the maximum velocity I can take the turn at in time.
Currently, I have two approaches to the problem, neither of which seem to work very well.
My first approach was to take the data for maximum velocity and find all the local minimums. These are the points in the middle of the turns where the robot should decelerate as it is approaching, and accelerate as it is driving away. Then, I would do the calculations starting at each of the minimums and work away from them in both directions, calculating velocities that could be achieved when starting at this point traveling at the maximum curve velocity, while keeping in mind acceleration limits. Then, after I had many sets of velocities, I went through and picked the smallest velocity at each point. This way, I made sure I picked a velocity that allowed me to get through every single curve without going over the speed limit.
In the example graph, you can see what it would look like with a path with two minimums.
The red line represents the maximum velocity the robot can travel at that point in the curve.
The black line represents the velocities calculated starting from min 1 and working forward, using acceleration limits.
The green line represents velocities calculated starting from min 2 and working backward, using acceleration limits.
The dotted blue line is the function I'm choosing to use as velocity. Unfortunately, it has a corner at the red point where acceleration suddenly jumps, and jerk is massively large/undefined. It also takes a long time to calculate.
My second approach, which is a work in progress, tries to use curve fitting (the same quintic hermite parametric spline interpolation I used for the path) to create a velocity vs. time graph. I can set the second derivatives of the start and end of the spline equal to the jerk (2nd derivative of velocity), the first derivative of the start and end of the spline to the acceleration, and the function values at the start and end of the spline to the velocity.
This gives me a second derivative continuous velocity/time curve (jerk will always be finite), but jerk and acceleration may be too large.
Because the splines are parametric and calculated from two nonparametric splines, I don't have set values for the derivatives and second derivatives, I only have the ratio between dx/dt and dy/dt. By manually adjusting dx/dt (and changing dy/dt to keep the ratio the same), I can change how tight the curve is. I can find a curve that meets acceleration/jerk limits, but it is time consuming and must be done manually, and I am not sure it exists for all possible paths.