Chief Delphi

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)
-   Extra Discussion (http://www.chiefdelphi.com/forums/forumdisplay.php?f=68)
-   -   paper: Driving a Robot — Fastest Path from A to B (http://www.chiefdelphi.com/forums/showthread.php?t=130301)

Mr. N 15-08-2014 23:27

paper: Driving a Robot — Fastest Path from A to B
 
Thread created automatically to discuss a document in CD-Media.

Driving a Robot — Fastest Path from A to B by Mr. N

RyanCahoon 15-08-2014 23:28

Re: paper: Driving a Robot — Fastest Path from A to B
 
What if you have a trajectory of three (or more) poses? It seems the simplifying assumption that "the robot reaches top speed very quickly" may create a false conclusion

MARS_James 15-08-2014 23:42

Re: paper: Driving a Robot — Fastest Path from A to B
 
I have such a strong desire to print this out and walk around pits handing it to other teams for two reasons one is because it honestly is an interesting read(I am a nerd leave me be) the other is because if it convinces teams this is true then defense became a whole lot easier.

BBray_T1296 15-08-2014 23:47

Re: paper: Driving a Robot — Fastest Path from A to B
 
This is very interesting. I'm glad someone took the time to analyze this mathematically. Thanks

Michael Hill 15-08-2014 23:51

Re: paper: Driving a Robot — Fastest Path from A to B
 
Interesting read. A couple things I would point out: 1.) There is another scenario to consider: Drive, turn, drive (Back up to a point coincident with a point on your final pose vector, turn to face the vector, then drive forward to the point), and 2.) This doesn't take into account real accelerations. There is an underlying assumption that speed is instant, meaning it takes 0 time to get to speed/reverse directions. With a spline path, there are no reversals in direction, so it might be something worth looking at.

BBray_T1296 15-08-2014 23:53

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by MARS_James (Post 1396619)
I have such a strong desire to print this out and walk around pits handing it to other teams for two reasons one is because it honestly is an interesting read(I am a nerd leave me be) the other is because if it convinces teams this is true then defense became a whole lot easier.

Well, with defense this whole calculation can be thrown out the window. Since there is a moving and smart (ie non arbitrary/random) blockade you end up in a situation where there is many "pose" A, B, C, D, ..., Z which is constantly changing to path around said blockade.

This is simply analyzing a situation where there is no interference.

MARS_James 15-08-2014 23:58

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by BBray_T1296 (Post 1396622)
Well, with defense this whole calculation can be thrown out the window. Since there is a moving and smart (ie non arbitrary/random) blockade you end up in a situation where there is many "pose" A, B, C, D, ..., Z which is constantly changing to path around said blockade.

This is simply analyzing a situation where there is no interference.

Oh I know, it was a joke, though you know if someone read this and just read the final result atleast someone wouldn't take into account defense and just start driving "tetris style" as my old driver use to say

Mr. N 16-08-2014 11:58

Re: paper: Driving a Robot — Fastest Path from A to B
 
Thanks for reading the paper --- and for your feedback. You raise some very good points. Here are some additional thoughts in response to these points:

1) Acceleration

Very true, the analysis assumes instantaneous accelerations to make the problem more tractable. However, the assumption is based on models developed from speed trial data using our 2013 robot. In a straight-line test, the robot reached 75% of maximum speed within the following times/distances:

- In lo-speed gear (24:1): within 0.2 seconds/20 cm of travel
- In hi-speed gear (9.4:1): within 0.7 seconds/140 cm of travel

FRC robots, going full throttle from a standing start, do virtually all of their accelerating in a faction of a second.

So, the assumption is reasonable when talking about paths lasting several seconds covering several meters. Conversely, it is a poor assumption for very short, very quick paths.

2) Reversing to Get from A to B

This is a great point. When I developed a game simulator for the 2014 game using the "turn-straight-turn" strategy, I realized that the "delta theta" terms (i.e., the changes in heading) had an ambiguity for angles greater than 180 degrees. For example, you get to the same heading by turning +270 or -90. Clearly the second turn is faster. But it also requires the robot to travel backwards during its straight-line motion.

Although not explicitly mentioned in the paper, this result is entirely consistent with the conclusion. Consider the 2013 game: it is faster to travel in reverse from the shooting position at the pyramid to the feeding station, rather than turning 180 degrees first.

3) Multi-Segmented Paths

Given the qualifiers outline in (1), I would argue it is more efficient to plan a trip around a mid-field obstacle (e.g., a pyramid) as a series of straight segments rather than one curved, sweeping path.

4) Impact on Strategy vs. Tactics

Clearly, the findings are more relevant when planning overall game strategy that involves moving between key locations ("way points" or "way poses"), as opposed to tactical moves on the field when contending with defenders.

James Kuszmaul 16-08-2014 12:51

Re: paper: Driving a Robot — Fastest Path from A to B
 
Even assuming that, when accelerating as hard as possible, that acceleration is insignificant*, it should be noted that a human driver still has to control the robot for the in-place turn. And I doubt there are any drivers out there that can time it such that they switch from full acceleration to full deceleration at just the right time to turn a precise angle. The hardest part isn't accelerating as fast as possible; it's being as accurate as possible while moving at speed.
This still leaves the question of what the fastest path would be in autonomous, accounting for acceleration. Turn-Straight-Turn is certainly the simplest and reasonably fast if you get your control loops right.

*Keep in mind that if each acceleration takes a fraction of a second, you still have to accelerate/decelerate three times in a given maneuver, which can add up to a couple seconds over the course of a maneuver and even a second per maneuver adds up quickly when doing multiple maneuvers per cycle and multiple cycles per match.

Jared 16-08-2014 13:09

Re: paper: Driving a Robot — Fastest Path from A to B
 
Looks pretty cool.

That said, I don't agree. I have done a ton of work this summer with this sort of curve generation, and curve cannot be simplified to simple cubic curves. A trajectory can contain a large number of parametric quintic curves, which can have specified end headings, specified end heading derivatives, as well as (because they're parametric) dy/dt, which affects how sharp the curves are. If these are optimized, I believe it is faster to drive in a curve.

It is also not accurate to disregard robot acceleration.

I am also taking into account an actual robot's velocity, acceleration (both acceleration and deceleration, a robot can decelerate faster than it can accelerate), and jerk limits, as well as the actual speed the robot can take a turn at.

[url=https://imgflip.com/gif/b7pa0]
This path takes 1.77 seconds to drive, and travels 5 feet down and 5 feet to the left.

Using the same exact acceleration, velocity, and jerk limits as well as the same code to generate the path, I made this path:

[url=https://imgflip.com/gif/b7pex]

It takes 1.67 to drive, and ends up in the same location.

The straight path must do two 45 degree turns in addition to the straight line. This leaves .05 second for the robot to rotate 45 degrees. A 2 ft wide robot turns in a circle with a radius of 1 foot, so the distance the wheel must travel is 2*pi*1/4 = pi/2 feet = 1.571 feet in .05 seconds = 31 fps, with instantaneous acceleration. Not possible for a robot.

What if I do something like this? (Total distance 60 feet, average speed 5.5 feet per second)

To stop and turn at each waypoint would be really slow.

Aren Siekmeier 16-08-2014 13:52

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by Jared (Post 1396652)
...

To further back this up. Even if you are going to do a simple turn-straight-turn, it's going to be faster to turn about one drive side rather than the center of the robot because this moves your center further along the path, and doesn't require direction reversals for either drive side.

The curvilinear path, when considering the speeds of each side, is generalizing this by allowing each side of the drive to stay at a positive speed and minimized their accelerations while still accomplishing the heading changes and displacements needed.

Mr. N 16-08-2014 14:07

Re: paper: Driving a Robot — Fastest Path from A to B
 
Very nice work!

I think, in fact, we're in good agreement. Here are some points of clarification:

Quote:

Originally Posted by Jared (Post 1396652)

...curve cannot be simplified to simple cubic curves. A trajectory can contain a large number of parametric quintic curves, which can have specified end headings, specified end heading derivatives, as well as (because they're parametric) dy/dt, which affects how sharp the curves are. If these are optimized, I believe it is faster to drive in a curve.

Yes, a trajectory can be defined by an arbitrary parametric curve. I was merely pointing out that the lowest order polynomial spline that satisfies the end conditions is a cubic. However, the analysis is generalized for any function that has either (1) no inflection points or (2) one inflection point (a curve with more inflection points has superfluous arc length and is always longer). So, within the context of the original assumptions, the conclusions hold true for any spline.

Quote:

It is also not accurate to disregard robot acceleration.
Agreed. However, in my earlier post, I attempted to set out general boundaries for when the assumption is still valid. For longer paths --- on the order of many meters --- there is closer agreement to real results than for shorter paths. It also depends on what gear you're in. The assumption is closer to reality for low-speed gears.

Quote:

...a robot can decelerate faster than it can accelerate...
Very true, but a faster deceleration is in fact closer to my "instantaneous" assumption.

Quote:

...and jerk limits...
This I'm really curious about. In my dynamic model, the velocity/acceleration limits are set by the motor characteristics. Speed and torque are coupled. However, I don't see how third-order derivatives (jerk) enter into it.

Quote:

...This path takes 1.77 seconds to drive, and travels 5 feet down and 5 feet to the left...
In your example, the total path length is a little over 7 feet or about 2.2 meters. In my previous post, I point out that, in hi-speed gear, it takes about 1.4 meters to accelerate (and likely something similar to decelerate). So, this example is what I would classify as a "short" path (where acceleration definitely matters).

Can you re-run your example for 25x25 foot run (e.g., a cross-field maneuver) with similar rates of turn (remembering that the speed constraint must be imposed on the outermost bank of wheels during a turn, not the centroid).

Quote:

What if I do something like this? To stop and turn at each waypoint would be really slow.
Agreed , but is this representative of a real game strategy? I would argue that a typical game involves significantly fewer way points per scoring cycle that what you've shown.

--- --- ---

All that said, it looks like your research and programming has led to a very powerful and useful modelling tool. Congrats!

Mr. N 16-08-2014 14:16

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by compwiztobe (Post 1396655)
To further back this up. Even if you are going to do a simple turn-straight-turn, it's going to be faster to turn about one drive side rather than the center of the robot because this moves your center further along the path.

In fact, the analysis takes this into account.

Turning about a point other than the center does indeed made the center of the robot move faster. However, it simultaneously creates a longer path in every case. This is the central question: Does the increased speed compensate for the extra length? The answer is no (within the context of the working assumptions).

Jared 16-08-2014 15:05

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by Mr. N (Post 1396656)
Very nice work!

I think, in fact, we're in good agreement. Here are some points of clarification:

Me too. After reading this post, I agree with what you've said, that curves and drive-then-turn both have their uses, especially considering that curves are much more difficult to implement. For a single spline, the time difference is pretty small. Once you start joining many splines, or using weirder splines, it becomes a different problem.
[quote]

Quote:

This I'm really curious about. In my dynamic model, the velocity/acceleration limits are set by the motor characteristics. Speed and torque are coupled. However, I don't see how third-order derivatives (jerk) enter into it.
I implement the jerk limit not because it's needed, but because it makes the path a little smoother. It prevents the motor power from going full forward to full reverse instantly, preventing tripped breakers and slipping wheels. It can also stop a top heavy robot from doing little tips as it stops. It has a very small effect on time.

Quote:

In your example, the total path length is a little over 7 feet or about 2.2 meters. In my previous post, I point out that, in hi-speed gear, it takes about 1.4 meters to accelerate (and likely something similar to decelerate). So, this example is what I would classify as a "short" path (where acceleration definitely matters).
True. The longer the lines are, the faster they are, compared to curves. For something with one or two lines with small turns, lines could be faster. For something with 5 or 6 curves that are pretty short, curves could be faster.

Quote:

Can you re-run your example for 25x25 foot run (e.g., a cross-field maneuver) with similar rates of turn (remembering that the speed constraint must be imposed on the outermost bank of wheels during a turn, not the centroid).
Yes. The speed constraint problem is one I don't have a great solution for. Currently, I generate the curve through the points, calculate speeds/accelerations at each point for the center, then the two sides where the wheels are, and if any point is too high, it adjusts a parameter and tries again, and repeats until it's within range. The iterating solver tries to adjust the "curviness" of the splines as well as adding a factor to cheat and increase the calculated radius of curvature at each point, which is used to limit the speed through curves. The end result is that the robot will slow down more on tighter curves, thinking they are tighter than they really are, but driving on curves/straight parts with a large radius of curvature is not slowed down.

Quote:

Agreed , but is this representative of a real game strategy? I would argue that a typical game involves significantly fewer way points per scoring cycle that what you've shown.
Probably not.


Here's my setup for the bigger run
Start Point: (0,0)
End Point: (25, -25)
Max Velocity: 10 ft/s
Max Acceleration: 10 ft/s^2
Max Deceleration: 15 ft/s^2
Robot Width: 2 feet

Trial 1:
Start Heading: 0 (facing right)
End Heading: 0
Path Type: Quintic Parametric
Results:
Code:

    Time: 4.784024461201024
    Distance: 37.531625864027085
    Average Speed: 7.845199406569258

Trial 2:
Start Heading: -pi/4 (diagonal down and to the right)
End Heading: - pi/4
Path Type: Line

Results:
Code:

    Time: 4.520517763673839
    Distance: 35.3518031718333
    Average Speed: 7.820299580706168




A great example of where a line would be much faster:


Code:


    Time: 4.142848102721899
    Distance: 23.413771473152483
    Average Speed: 5.6516123431533405

A line for the same path:

Code:

    Time: 1.5516701850414147
    Distance: 6.099594833091013
    Average Speed: 3.9309866825392485


Mr. N 16-08-2014 15:49

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by Jared (Post 1396659)
A great example of where a line would be much faster:

Ha! Agreed!

Thanks very much for the detailed answer. Great work!

One thing: The acceleration limit you used (10 ft/s/s) may be little low. Based on our speed trials, we were getting something more like 22 ft/s/s (Supershifter, hi-speed gear).

I worked up a similar example using the math from my paper (I probably should have included this in the paper itself). I used a 25x25 foot path with a similarly shaped curve. I set my upper speed at 10ft/s and robot wheel base to 2 ft.

Although I don't take acceleration into account, keep in mind that (a) the path is long enough that the effects of acceleration are minimized, and (b) both the linear and curved path benefit from the "instantaneous" acceleration assumption -- they are both slightly faster than real life, by about the same amount.

Here are my results:

- Turn-Straight-Turn: 3.69 seconds, total arc length = 35.36 feet, total heading adjustment = 90 degrees

- Curve: 4.00 seconds, total arc length = 37.79, total heading adjustment = 126 degrees

The curved path takes 8.3% (or 0.31 seconds) longer to execute.

Although this doesn't sound like a lot, over 6 scoring cycles, that 1.84 seconds --- almost 2 seconds from optimizing just one part of a path.

Michael Hill 16-08-2014 16:40

Re: paper: Driving a Robot — Fastest Path from A to B
 
Swerve drive teams are laughing at this thread :p

Andrew Lawrence 16-08-2014 18:23

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by Michael Hill (Post 1396667)
Swerve drive teams are laughing at this thread :p

I wouldn't say that. A large part of this is conservation of momentum, and the best swerve teams know that utilizing a robot's momentum to its fullest will provide the smoothest and most advantageous maneuverability the swerve design can offer. The same is true with any robot drivetrain.

Jared Russell 17-08-2014 12:32

Re: paper: Driving a Robot — Fastest Path from A to B
 
What you have brought up is a special case of a well known theorem from optimal control that says (essentially) that you always want to be saturating your inputs to get to the final state in a time optimal way. Intuitively, if your left and right motors are always going full speed in the right direction towards your setpoints (both position and angular, and you've found the right turn-straight-turn policy that ensures that this is true), you will always get there faster than if they are not going full speed all the time.

This property holds with an infinite acceleration limit and, indeed even with limits on acceleration, jerk, etc. - except in the latter case, you always want to be accelerating/jerking at the limit. The complication is that once you introduce higher order constraints, the "order of operations" policy becomes difficult to discover because of the nonlinear mapping between motor speeds and position/rotation. Once you have higher order constraints, finding a time-optimal path becomes a more difficult process, typically requiring iteration and search in a non-convex space.

If you look at 254's acceleration profile generation code from this year, you'll see this concept in action - using a triple integrator with an input that always switches between +1/-1, we obtain a limited-jerk, limited-acceleration, limited-velocity trajectory. We then applied this acceleration profile over a spatial spline, which of course broke the time optimality but was good enough in practice (with some safety margin to prevent saturation).

So...Yours is the right conclusion if you ignore practicalities like acceleration limits, wheel slip, battery usage, dynamics/momentum, and jerk to the robot and any load(s) it is carrying. Empirically, these are quite significant factors for FRC driving:

* Robots that have high enough maximum accelerations to allow you to assume instantaneous acceleration will frequently also be capable of slipping their wheels, harming control, acceleration, and accurate distance measurement. Moreover, by the time you are talking about full-weight, 15+fps robots, the infinite acceleration assumption breaks down severely.

* A robot that moves from one point to another along an arc does not need to repeatedly accelerate and decelerate the drive motors as in a turn-straight-turn case. This saves battery life, heat, and wear-and-tear over the course of a match. The battery life savings can be significant enough that it allows you to gear your robot faster overall.

* Momentum is a HUGE part of FRC driving at high speeds. If I take a 254 robot at 19fps and want to turn, I can do so on a dime simply by slowing down one side of the drive and letting the momentum whip the back end of the robot around the corner. Maintaining momentum through maneuvers is generally preferential to coming to a stop.

* Even if you have an infinitely accelerating robot that doesn't slip its wheels, has a perfect battery, and doesn't care about momentum, if you are carrying a game piece externally, you may want to reduce the accelerations and jerks experienced by them. Case in point: The balls we held on our bumpers during auto mode this year could be dislodged by a very abrupt stop or quick turn in place.

Jared 17-08-2014 13:06

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by Jared Russell (Post 1396715)
If you look at 254's acceleration profile generation code from this year, you'll see this concept in action - using a triple integrator with an input that always switches between +1/-1, we obtain a limited-jerk, limited-acceleration, limited-velocity trajectory. We then applied this acceleration profile over a spatial spline, which of course broke the time optimality but was good enough in practice (with some safety margin to prevent saturation).

One quick question- how does the code (or does it even do it?) limit acceleration, jerk, and velocity if the path goes straight long enough for the robot to reach full speed, then suddenly turn sharply? Do you go through the turn too quickly, ignore acceleration limits and stop too quickly, or do you somehow begin decelerating before you get there?

Jared Russell 17-08-2014 14:13

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by Jared (Post 1396717)
One quick question- how does the code (or does it even do it?) limit acceleration, jerk, and velocity if the path goes straight long enough for the robot to reach full speed, then suddenly turn sharply? Do you go through the turn too quickly, ignore acceleration limits and stop too quickly, or do you somehow begin decelerating before you get there?

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.

Jared 17-08-2014 17:18

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by Jared Russell (Post 1396722)
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.

Mr. N 17-08-2014 17:32

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by Jared Russell (Post 1396715)
What you have brought up is a special case of a well known theorem from optimal control that says (essentially) that you always want to be saturating your inputs to get to the final state in a time optimal way.

Aw, man... you made me dig out my old Optimal Control textbook (thanks, actually):)

Indeed, if the question had been posed as an optimal control problem, Pontryagin's Minimum Principle leads to the conclusion that "bang-bang" control inputs (where the motors are always at + or - max speed) are the optimal solution, where the cost functional is simply total time (and the upper limits of the inputs are constrained). This implies that the robot must either be in straight-line motion (both motors +) or pure rotation (one + and the other -), which corresponds to a "turn-straight-turn" approach. Any other motor state generates a curvilinear path, and would be sub-optimal (for this specific cost functional).

There's a nice description here:
http://www.cds.caltech.edu/~murray/b...al_04Jan10.pdf

Quote:

This property holds with an infinite acceleration limit and, indeed even with limits on acceleration, jerk, etc.
Interesting.

Quote:

If you look at 254's acceleration profile generation code from this year...
I'll definitely have a look.

Quote:

...if you ignore practicalities...
Certainly, this analysis was performed in an idealized context, with a highly simplified cost functional. Your points are well taken.

DampRobot 17-08-2014 18:33

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by Mr. N (Post 1396662)
Ha! Agreed!

Thanks very much for the detailed answer. Great work!

One thing: The acceleration limit you used (10 ft/s/s) may be little low. Based on our speed trials, we were getting something more like 22 ft/s/s (Supershifter, hi-speed gear).

I worked up a similar example using the math from my paper (I probably should have included this in the paper itself). I used a 25x25 foot path with a similarly shaped curve. I set my upper speed at 10ft/s and robot wheel base to 2 ft.

Although I don't take acceleration into account, keep in mind that (a) the path is long enough that the effects of acceleration are minimized, and (b) both the linear and curved path benefit from the "instantaneous" acceleration assumption -- they are both slightly faster than real life, by about the same amount.

Here are my results:

- Turn-Straight-Turn: 3.69 seconds, total arc length = 35.36 feet, total heading adjustment = 90 degrees

- Curve: 4.00 seconds, total arc length = 37.79, total heading adjustment = 126 degrees

The curved path takes 8.3% (or 0.31 seconds) longer to execute.

Although this doesn't sound like a lot, over 6 scoring cycles, that 1.84 seconds --- almost 2 seconds from optimizing just one part of a path.

For this simulation, are you taking into account the time needed to turn, or assuming that the robot starts and ends on the same heading it travels?

Mr. N 17-08-2014 18:45

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by DampRobot (Post 1396733)
For this simulation, are you taking into account the time needed to turn, or assuming that the robot starts and ends on the same heading it travels?

Yes, the time to change heading is taken into account. The start and end headings can be arbitrary.

Mr. N 17-08-2014 19:18

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by Jared (Post 1396730)
Do you think you could help me figure out a good way to do this?

I realize this query wasn't directed at me, but perhaps I can contribute in a small way. Also, since I'm approaching this somewhat cold, apologies for any naοve suggestions.

Problem Statement (as I understand it):

You have defined a path for the center of the robot to follow, using a parametric curve that satisfies certain end constraints (position, heading). The path may have an arbitrary number of curved segments. You wish to find control inputs (i.e., left and right motor speeds) that will cause the robot to execute that path in minimum time, while adhering to speed, acceleration and jerk constraints imposed on the motors.

Is this correct?

Partition the Path

You can partition the path into segments between inflection points (i.e., when the curve transitions from positive to negative curvature).

In the absence of acceleration and jerk limits, you would assign Vmax to either the left or right motor, correct? That is: if curvature > 0 --> Set Vright = Vmax; if curvature < 0 --> Set Vleft = Vmax; if curvature = 0, Set both to Vmax.

The opposite motor will be set at whatever speed is necessary to achieve the desired curvature.

So, for each path segment, you know which motor should ideally be running at max speed.

Define a "Transition Zone" Based on Acceleration/Jerk Limits

In reality, at each inflection point, the motors cannot transition between speed settings instantaneously because of acceleration/jerk limits.

In the worst case, a motor would be commanded to go from +Vmax to -Vmax. Given the acceleration/jerk limits, it is possible to calculate the minimum arc length over which this transition can happen.

Use this arc length to define transition zones on either side of each inflection point.

Interpolate between Segments

For each motor, create a piece-wise continuous speed curve based on segments and transition zones:

There are two cases to consider:

1. Transition Zones do not overlap: In this case, for each motor join the "before" and "after" speeds with a transition curve (a piece-wise quadratic) that joins the two segments.

2. Transition Zones overlap (i.e., segment arc length is shorter than transition arc length). In such a case, the max speed you originally set for that segment is impossible to achieve. (Well, since this is based on the worst case, this isn't strictly true in all cases --- but close enough). In this case, either scale the max speed down for this segment or alter the path to give it a gentler curve.

Note: This algorithm will give a sub-optimal result in terms of time, but it should be close enough.

Does this help at all?

artK 17-08-2014 22:47

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by Jared (Post 1396659)
A great example of where a line would be much faster:


Code:


    Time: 4.142848102721899
    Distance: 23.413771473152483
    Average Speed: 5.6516123431533405

A line for the same path:

Code:

    Time: 1.5516701850414147
    Distance: 6.099594833091013
    Average Speed: 3.9309866825392485


I look at this and have to ask one question Jared: if you were to add PI to both of the headings, thus creating a new spline that should model a robot driving backwards, what do the numbers come out to?

Jared 18-08-2014 08:57

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by artK (Post 1396750)
I look at this and have to ask one question Jared: if you were to add PI to both of the headings, thus creating a new spline that should model a robot driving backwards, what do the numbers come out to?

Unfortunately, I don't remember exact what numbers I used for that example, but here are two path with their times. For all these, I'm assuming I have a maximum acceleration of 10, maximum deceleration of 15, maximum speed of 10, and a robot width of 2 feet.
The lower point is 8 feet away from the upper point.
This takes 3.926 seconds to drive, averaging 5.86 feet per second, on a path that is 23.00 feet long.



After adding pi to both headings, the path was only 7.2 feet long, took 1.573 seconds to drive, and averaged 4.57 feet per second.

-Note- the acceleration counts on the top of these images are not correct, as there's some lag when using an online GIF maker, and these values are calculated in real time for the preview, just to make sure I didn't mess up calculating the values for the path file. In the actual path file, acceleration never goes below -15.
Quote:

Originally Posted by Mr. N (Post 1396741)
I realize this query wasn't directed at me, but perhaps I can contribute in a small way. Also, since I'm approaching this somewhat cold, apologies for any naοve suggestions.

Problem Statement (as I understand it):

You have defined a path for the center of the robot to follow, using a parametric curve that satisfies certain end constraints (position, heading). The path may have an arbitrary number of curved segments. You wish to find control inputs (i.e., left and right motor speeds) that will cause the robot to execute that path in minimum time, while adhering to speed, acceleration and jerk constraints imposed on the motors.

Is this correct?

Partition the Path

You can partition the path into segments between inflection points (i.e., when the curve transitions from positive to negative curvature).

In the absence of acceleration and jerk limits, you would assign Vmax to either the left or right motor, correct? That is: if curvature > 0 --> Set Vright = Vmax; if curvature < 0 --> Set Vleft = Vmax; if curvature = 0, Set both to Vmax.

The opposite motor will be set at whatever speed is necessary to achieve the desired curvature.

So, for each path segment, you know which motor should ideally be running at max speed.

Define a "Transition Zone" Based on Acceleration/Jerk Limits

In reality, at each inflection point, the motors cannot transition between speed settings instantaneously because of acceleration/jerk limits.

In the worst case, a motor would be commanded to go from +Vmax to -Vmax. Given the acceleration/jerk limits, it is possible to calculate the minimum arc length over which this transition can happen.

Use this arc length to define transition zones on either side of each inflection point.

Interpolate between Segments

For each motor, create a piece-wise continuous speed curve based on segments and transition zones:

There are two cases to consider:

1. Transition Zones do not overlap: In this case, for each motor join the "before" and "after" speeds with a transition curve (a piece-wise quadratic) that joins the two segments.

2. Transition Zones overlap (i.e., segment arc length is shorter than transition arc length). In such a case, the max speed you originally set for that segment is impossible to achieve. (Well, since this is based on the worst case, this isn't strictly true in all cases --- but close enough). In this case, either scale the max speed down for this segment or alter the path to give it a gentler curve.

Note: This algorithm will give a sub-optimal result in terms of time, but it should be close enough.

Does this help at all?

Thanks for the suggestions- I like the idea of calculating acceleration/velocity limits on the wheel that is travelling the fastest.
I also really like dividing based on inflection points, instead of the sharpest corner of the curve.
I do see three potential issues/questions with this approach-
1. I do not always want to be accelerating/jerking at the maximum amount. If I am coming out of a tight turn, full acceleration may end up making me go too fast for the turn. Ideally, the path is straight enough that this isn't a problem, or we can just use the turn's acceleration limit in place of the robot's acceleration limit in the case of the turn's acceleration limit being lower.

2. When creating a quadratic to interpolate speeds between transitions zones, I am not sure if it can be done with just a quadratic. I need to make sure that acceleration will be a continuous function, so when interpolating, I need to be able to specify the location and first derivative of the endpoints. For this, I need at least a cubic. I also need to make sure that my acceleration and jerk never exceed certain numbers. I have no idea how I'd interpolate a curve like this, and to make it more complicated, it's more than just finding a curve whose first and second derivative are smaller than some numbers because it's a velocity vs. distance graph, so the derivatives aren't acceleration and jerk. This is the main problem I was struggling with in my second attempt.

3. When the transition zones do intersect, it becomes a very time consuming process for the computer to figure out what values will result in a slow enough turn, as it has to generate the entire path many times in a row. It is also a challenge to slow down just one part of the path, and have the robot behave following acc. and jerk limits.

I can get pretty close by always picking the smallest velocity in the overlap zone (as soon as we go too fast to make the second turn, start following the second turn's velocity curve), but this ignores the jerk limit, and acceleration jumps.


To be honest, I think that solving this problem with acceleration and jerk limits is beyond my ability. I think I'll just settle with ignoring the jerk limit between curves. I'll still be able to follow all the curves while keeping in mind acceleration and velocity limits, of both the robot and the curves on the path, but jerk will be forgotten at some points between tight curves.

Jared Russell 18-08-2014 13:58

Re: paper: Driving a Robot — Fastest Path from A to B
 
If you want to make sure that your path is both velocity and acceleration limited, here is a simple and widely used method:

1. For each point along the path, compute the maximum allowable velocity. For straight segments, this is simply the maximum motor velocity. For points along curves, you can compute the radius of curvature and from that derive the maximum linear speed that would let you follow the curve (based on setting the outside wheel's speed to the maximum).

You now have a discontinuous velocity profile that might look something like this:
Code:

Initial maximum velocities
Plot of waypoint # (x) vs. maximum velocity (y)

----      -----      ---------  (fast)
    ------           
              -------          (slow)

2. Now iterate through the path (beginning to end) and for each point, set max_velocity[i] = min(max_velocity[i], max_velocity[i - 1] + max_accel_over_distance_between_points (see NOTE) ). You will now have a profile that looks something like:

Code:

Intermediate maximum velocities
Plot of waypoint # (x) vs. maximum velocity (y)

----      ----        -------  (fast)
    ------/            /
              -------/          (slow)

3. Iterate through the path from end to beginning, and for each point, set max_velocity[i] = min(max_velocity[i], max_velocity[i + 1] + max_decel_over_distance_between_points). Your final profile will look like:

Code:

Final maximum velocities respecting acceleration limits
Plot of waypoint # (x) vs. maximum velocity (y)

---        --          -------  (fast)
  \------/  \        /
              \-------/          (slow)

NOTE: Generally you want to sample your path so that the points are relatively close together and uniformly spaced in distance (since this is probably what your controller will want as well). To figure out the maximum allowable velocity, use the equations d = v0 * t + 1/2 * a * t^2 and v = v0 + a * t.

Jared 18-08-2014 14:43

Re: paper: Driving a Robot — Fastest Path from A to B
 
Quote:

Originally Posted by Jared Russell (Post 1396776)
If you want to make sure that your path is both velocity and acceleration limited, here is a simple and widely used method:

1. For each point along the path, compute the maximum allowable velocity. For straight segments, this is simply the maximum motor velocity. For points along curves, you can compute the radius of curvature and from that derive the maximum linear speed that would let you follow the curve (based on setting the outside wheel's speed to the maximum).

You now have a discontinuous velocity profile that might look something like this:
Code:

Initial maximum velocities
Plot of waypoint # (x) vs. maximum velocity (y)

----      -----      ---------  (fast)
    ------           
              -------          (slow)

2. Now iterate through the path (beginning to end) and for each point, set max_velocity[i] = min(max_velocity[i], max_velocity[i - 1] + max_accel_over_distance_between_points). You will now have a profile that looks something like:

Code:

Intermediate maximum velocities
Plot of waypoint # (x) vs. maximum velocity (y)

----      ----        -------  (fast)
    ------/            /
              -------/          (slow)

3. Iterate through the path from end to beginning, and for each point, set max_velocity[i] = min(max_velocity[i], max_velocity[i + 1] + max_decel_over_distance_between_points). Your final profile will look like:

Code:

Final maximum velocities respecting acceleration limits
Plot of waypoint # (x) vs. maximum velocity (y)

---        --          -------  (fast)
  \------/  \        /
              \-------/          (slow)


Thanks! This works perfectly for acceleration and velocity limits, even with fast full speed straight segments followed by extremely sharp turns. My issue was that I wasn't looking at the minimum- I was always looking at values that followed acceleration rules. Adding jerk will be more tricky, but I have an idea.


All times are GMT -5. The time now is 23:29.

Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi