Smooth Path Generator for RoboRio 2015

[faust1706]

I am no expert in data science, or computational mathematics, it was an amateur question. Since his program is iterative, there does exist a point when taking the inverse of a matrix is faster than n interations, I believe.

[GuyM142]

Correct me if I'm wrong:
The code generates an array of velocities which the robot has to reach at specific times of the auto (each side independently) and with encoders on each side and a PID loop I need to make the wheels actually get to that velocity.
Did I get it right?

If so, what is the best way to use PID for velocity control?

[artK]

Kevin, can you talk a little more about the convergence of Alpha and Beta combinations? How did you determine whether or not certain parameters converge? And would convergence be related with the original path (Like would a path with an right or acute angle between segments have different convergence properties than a closer to straight path)?

[Jared Russell]

Originally Posted by GuyM142

Correct me if I'm wrong:
The code generates an array of velocities which the robot has to reach at specific times of the auto (each side independently) and with encoders on each side and a PID loop I need to make the wheels actually get to that velocity.
Did I get it right?

If so, what is the best way to use PID for velocity control?

As Art said there are many ways to implement this. The easiest velocity loops (IMO) heavily utilize feedforward terms.

// For each side of the drive...
while (following) {
  // desired_position, desired_velocity, and desired_acceleration are all generated by your profile.
  position_error =  desired_position[i] - actual_position;
  command[i] = Kv * desired_velocity[i] + Ka * desired_acceleration[i] + Kp * position_error;
}

You will find that it is very easy to tune with the following procedure. Before doing this, it is REALLY helpful to have graphs of position vs. time and/or velocity vs. time in Smart Dashboard (or an equivalent plotting tool).

1) Generate an impulse response. From a stop, slam the sticks forward until you max out at your full robot speed. You may require a long distance to get up to speed, so plan accordingly. Stop the robot once it hits full speed (...or the back wall of your shop).

2) You can now analyze your charts to obtain a couple useful quantities:
max_speed: The maximum speed (maximum slope) of the position vs. time plot.
accel_time: How long from when you started until you hit the maximum speed (it is a bit of a judgement call since the curve is smooth, but try to get it in the right ballpark). max_accel ~= max_speed / accel_time

4) Set Kp = Ka = 0, and Kv = 1/max_speed.

5) Generate a motion profile with a maximum velocity of about 80% of your max speed and a maximum acceleration of about 80% of your max acceleration. The 80% is to be conservative and robust to batteries, etc.

6) Attempt to follow the profile with the gains from step 4. Watch your plots vs. the profile (yep...also plot the commanded positions/velocities). It is likely that you lag the profile initially, and lead it (or overshoot) at the end due to the robot's inertia.

7) Next, we will tune Ka to have the robot compensate for this lag/lead. The idea is that when you are accelerating heavily, you can add or subtract from your command to compensate for the robot's inertia. Tune Ka by adjusting and repeating the experiment in 6 until you are following the profile as close as possible. The right value of Ka will be between 0 and 1/max_accel. 1/(2*max_accel) is a reasonable first guess.

8) At this point, you should be following profiles reasonably well, but it is not perfectly repeatable, probably doesn't drive straight, etc. Let's add some feedback. The units of Kp are in (% of power per unit of error). If you have tuned Kv and Ka pretty well, you can get away with a Kp on the order of 1 or 2 if your units are meters (1/3 to 2/3 if using feet, etc).

9) You should follow the trajectories really, really well now. They should be somewhat straighter and quite repeatable.

At this point, things are probably working decently well. Here are a few more options if you want to keep tweaking:

1) Add an integral term based on the sum of position error to get even more precision in final distance. We found this totally unnecessary last year, but YMMV.

2) Add a gyro to help stay straight. Each side of the drive is totally independent, so you can add a simple gyro controller that adds or subtracts from the velocity command being fed to each in order to compensate for accumulated errors.

3) Add trajectory replanning so that if you get bumped to the side, you instantly regenerate a new trajectory to smoothly get you back on track. If necessary for our strategy, 254 might do this in 2015

4) Better characterize your drive train. The calculated left and right velocities are actually assuming a two wheeled vehicle with no slip...4, 6, and 8 wheel drives always have some scrub that results in the vehicle turning less than commanded. You can attempt to measure this and adjust your trajectories accordingly.

[yash101]

This software is quite similar to what I have been planning to implement. I think I will try to decompose this code and rewrite it so I can learn how it works. I have been working on a way to generate a path on the field. The problem with my approach is that no algorithm is perfect. Even though I am using greedy's algorithm because it seems like it will work perfectly for an FRC setup, it every once in a while will generate some sudden turn, often for no purpose. This type of algorithm would help smoothen it out so It can be transformed into motor powers to control the robot.

Kevin,
Thank you for sharing this with me as now I have an almost identical piece of software that works, so I can have an example to learn off of.

[NotInControl]

Originally Posted by faust1706

Kevin, +1 for using gradient descent. I love that algorithm. Noob question: why not use the normal equation: (XX')^-1 X'y to solve for you thetas?

The algorithm is modified for the purpose of what we are trying to do here. Its actually a combination of 2 separate gradient functions playing tug of war against each other, one pushing each point and the other pulling each point. Alpha and Beta control who is "stronger" in the tug. One pulling the point away from its origin point, the other pushing it back. A happy medium generates smooth transitions. If we didn't modify the approach and used a single gradient method to try to minimize the distance between all points, the solution will always be a straight line from the first node to the last node. You can achieve this result, by making Alpha zero, which essentially takes one half of the algorithm out of the tug of war and thus with only Beta active, will pull the solution to a straight line. This doesn't work for us. Another reason why I avoid the format you posted, is based on my own best practice that I try to live by, is avoid having to calculate the inverse of a variable because can lead to problems dealing with singularizes and crashes when you do not control the dataset, as in most algorithm cases.

Originally Posted by faust1706

You mentioned this was part of you PhD thesis (first of all, that's really impressive. Some of my lab mates are working on theirs and it looks incredibly stressful). For your project of "developing a controller for an Autonomous Car," are you given a path already? I'm curious because I wrote a pathfinding algorithm (really just an adaption of a*) over the summer and I'm taking the path generated from that and putting it into my program that generates velocity profiles of the left and right side for the robot. I am just wondering if you're doing a similar thing.

We have no apriori knowledge other than map definition data (gps location of streets, 1-way, or 2-way, sidewalks, stores, etc) and a destination. We also have speed limit definitions for each roads.

Originally Posted by faust1706

"If the algorithm does not converge, the program will simply never finish, so it is pretty easy to identify." Luckily for an autonomous period, everything will be calculated long before the match starts, unless you develop a new path moments before the match, so you'll have ample time investigate why it fails to converge.

This is true for if you only plan to calculate the path once before the match. The reason I made that statement is subtle, but it alludes to a teams desire to calculate new paths in real-time. This would be useful for example if you got nudged in a match, and wanted to course correct. The primary reason for this approach to the problem was to support real-time or near real-time path re-planning on an embedded device. However, this is where you need to pay very close attention to the parameters, and do a lot of testing for convergence and other problems. If I have a chance, I will post some test results, I can't post anything related to my PhD study, because that is proprietary.

Originally Posted by artK

Kevin, can you talk a little more about the convergence of Alpha and Beta combinations? How did you determine whether or not certain parameters converge? And would convergence be related with the original path (Like would a path with an right or acute angle between segments have different convergence properties than a closer to straight path)?

Majority of these values were determined imperially during my previous study. I provide them here, because they still make sense, and are still valid. The default parameters have the best convergence ratio based on the algorithm structure itself. While I can not guarantee convergence for anything, I am fairly confident you will not have issues if you stay within the bounds of the table. It doesn't mean values outside of it won't work for your particular test set. If anyone runs into a convergence issue, I would like to know, because it helps everyone.

Let me think more about your second question. My hunch right now is that valid path data regardless of its actual shape should have no affect on the convergence rate of a set of parameters. But let me think about it some more, and possibly do some calcs on it and get back to you. I would assume we are talking about valid, realistic paths only, and nothing which is intentionally malformed.

Originally Posted by GuyM142

Correct me if I'm wrong:
The code generates an array of velocities which the robot has to reach at specific times of the auto (each side independently) and with encoders on each side and a PID loop I need to make the wheels actually get to that velocity.
Did I get it right?

If so, what is the best way to use PID for velocity control?

Yes that is correct. Jared, posted some good points in the previous post. I have also had a good conversation with Austin from Team 971 on this exact topic. We share how we approach controls on each respective team, and I think a lot of useful information is in that thread. If you have a chance, give it a read. http://www.chiefdelphi.com/forums/sh...d.php?t=129574

Originally Posted by yash101

Kevin,
Thank you for sharing this with me as now I have an almost identical piece of software that works, so I can have an example to learn off of.

No problem, let me know how it works for you. I wish I could release this under a beerware license, but this is a HS community lol.

Regards,
Kevin

[faust1706]

Sorry to revive an rather old thread, but since this is a continuation of @notincontrol's work, I felt it needed to be tied with it.

Here is my attempt at converting his code to work with swerve: https://github.com/faust1706/Smooth-Swerve

I based the calculations off Ether's materials, but that doesn't mean I miss typed or something.

How to use: define set of waypoints, x and y, and then a set of angles you want the robot to be facing at each way point.

A known thing that is broken is the output velocity per motor.

[Ether]

Originally Posted by faust1706

Since his program is iterative, there does exist a point when taking the inverse of a matrix is faster than n interations, I believe.

Please provide a more detailed explanation what you mean. I'd rather not guess.