Investigate Trajectory and Motion Control Improvements

JNeiger commented 5 years ago

So I talked to Professor Vela and we walked through the correct way to do the path planning and motion control. Sounds like he may have actually worked with CMU 5-10 years back or something. Anyways, this list can be followed up until we get reasonable results. It mostly just removes a bunch of assumptions that are "incorrect", but good enough. Things like how we limit world acceleration of the robots to prevent slippage, but it actually is the wheel accelerations that cause slippage etc.

Ideal Setup

Target Path

You would take a list of waypoints of the path returned by our obstacle avoidance. It's the exact path we are going to try to follow. It should also be smoothed so that you can follow the path at speed. Ideally, at max speed.

Figuring out what speed to follow the path

Given a time to reach the end of the path, you can easily calculate the speed along the path you need to reach to do that while following all acceleration constraints. We actually don't care what the accelerations of the robots are, they are not the true cause of slippage. It is specifically the acceleration of the wheels. So, we need to do the trajectory calculations in wheel space instead of world space to find out the target speeds at each point in the path.

On Robot Control

The target wheel velocities will then be sent to the robots. This target wheel velocity will be sent to the wheel torque controller as the feedforward term. The feedback term will be the error in trajectory compared with the current target trajectory point. The feedback term can be either before we send the target wheel velocitiess to the robot or on the robot themselves, it shouldn't make too much of difference.

Which improvements to do first

Torque controllers to reach wheel velocity setpoints
Total trajectory error feedback (Both velocity error and position error, think LQR K matrix, 4 input 2 output)
Do trajectory, but do it in world space (xy pos and vel) instead of in wheel space (omega1 - omega4)
Do trajectory in wheel space

How to do most of the math

Trajectory Calculations

This is actually somewhat easy. The position as a function of time can be calculated using a polynomial. It doesn't matter what power polynomial, but a cubic polynomial is normally used. The standard trapezoidal velocity curves is a quadratic polynomial, but it requires a piecewise function splitting at each acceleration discontinuity. Cubic polynomials create a continuous acceleration so you can describe the entire trajectory in a single formula.

p(t) = a1 + a2*t + a3*t^2 + a4*t^3

Given the starting and ending velocities, we can actually build a system of linear equations to find the parameters of the curve, a1 - a4. This is specifically for a single dimension, but for high dimensions, you can easily expand it since each direction can be considered independent.

p0 is the starting position, p1 end point. p0d is the starting vel, p1d is the ending vel. ti is the starting time, tf is the ending time. If you set up the matrix in the following format, you can solve a system of equations to find the parameters of the trajectory. The 4 equations are p(ti) = p0, p(tf) = p1, p_dot(ti) = p0d, and p_dot(tf) = p1d.

Ax = b
Ma = b (Better known in this form for trajectory problems)

[ 1, ti, ti^2,   ti^3; [a1  = [ p0;
  1, tf, tf^2,   tf^3;  a2      p1;
  0,  1, 2*ti, 3*ti^2;  a3     p0d;
  0,  1, 2*tf, 3*tf^2]  a4]    p1d];

If there are multiple points in this path and you don't want to deal with setting a predefined velocity at each point, use the continuity of accelerations p_doubledot(ti_2) = p_doubledot(tf_1) as an equation.

Take the inverse of A and you can solve for the a1 - a4's. This setup will not constrain acceleration at all though. You would need something else for that. To take into account acceleration limits, you will have to solve the system of linear equations with constraints.

arg min ( || Ma - b || ^2 )
    a

where
              Aa < b

A is the acceleration matrix. b is the max and min accel matrix

A = [0, 0,  2,  6*ti;
     0, 0, -2, -6*tf];

b = [max_accel;
     min_accel];

max_accel = -min_accel

To find the minimum a values, you have to do an optimization. Luckly, it's linear so you should only have a single min value. Additionally, you can find the derivatives and stuff really easily so things like gradient descent will be really easy.

This does everything in terms of max and min world accelerations. To make it in terms of wheel velocities, the derivatives of the position must be multiplied by the body to wheel matrix. This matrix can be found in firmware. It's basically a 4x3 where the x/y/omega multiply by some type of geometry matrix called the body to wheel matrix resulting in the wheel velocities of all the wheels. You would have to update the A matrix so it changes the a1 - a4 vector to be in the wheel velocities frame instead of world velocity frame. I think just multiplying the entire matrix by a modified body to wheel matrix will work. Care has to taken to make sure that you account for the two rows per velocity state variable. So it would become a 8 row matrix, 2 for each of the 4 wheels, compared to the 6 row matrix, 2 for each of the 3 velocities (X/Y/omega).

Wheel Controller

We are using BLDC motors with both encoders and current sensors. We should split it up into a trajectory builder, regulator and observer. The trajectory builder takes the velocity target and builds a torque profile to reach that quickly. Do simple trapezoidal or something for the feed forward path.

The regulator is there to make sure we are hitting that torque target. We will be getting the torque error and do LQI/LQR/PID to make that error hit 0. We'll also have a feed forward component from the trajectory builder to hit the target easier.

The observer will be trying to estimate the torque. You may be able to get away with using the 3 currents to get the total current and treat it like a brushed DC motor. You also may be able to treat it like an AC motor and it will turn into a DC motor when you look at it in the rotating from using iq and id.

Ideally, you would only need the feedforward path if we do things right so it won't matter too much if we are off

JNeiger commented 5 years ago

This also covers a lot of stuff in #2. It doesn't cover obstacles stuff or priority.

ashaw596 commented 5 years ago

If you want to try hacking a quick tracjectory thing (once you figure out wheel characteristics and limits) you could probs do a test with adding it to this function https://github.com/RoboJackets/robocup-software/blob/2f764db50e852ece683debb298bbeb3888c10797/soccer/planning/RRTPlanner.cpp#L541 .

You'll need to add actual rotation planning, but if you assume static rotation, you can probs do a one step calculation of wheel torque from the last target accel to the next(maybe. Wheel stuff seems complicated so I have real idea). It currently just uses the incorrect limiting of centrifugal body acceleration + velocity acceleration.

ashaw596 commented 5 years ago

Well the acceleration limits are calculated before, so here maybe https://github.com/RoboJackets/robocup-software/blob/2f764db50e852ece683debb298bbeb3888c10797/soccer/planning/RRTPlanner.cpp#L696 is where we are calculating the velocity profile for the bezier path.

ashaw596 commented 5 years ago

Hmm, for the directly using the acceleration of the cubic polynomial, that seems hard. A simple version is effectively what we're using actually to generate the smooth path. The matrix is set up and solved here. (I never commited the actual equations I gues...) But yeah it assumes acceleration and velocity and continuous and solves for cubic beziers connecting points given a start and end velocity. https://github.com/RoboJackets/robocup-software/blob/2f764db50e852ece683debb298bbeb3888c10797/soccer/planning/RRTPlanner.cpp#L763 But yeah. I couldn't figure out how to acceleration constraints so we use the velocity profile thing. Using approximate optimization seems like a cool idea! Might need to switch to 4th order beziers or remove the continuous acceleration constraints though.

JNeiger commented 5 years ago

I would need to play with it some to see how much it would replace. If it does the same curve smoothing stuff, it may be able to just straight up replace it, if not, we would probably just split the cubic bezier curve into a set of waypoints. It'll try to fit the curve as best as possible, but the optimization will account for the small errors.

This should also do angle planning too assuming you set a start and end. It probably won't be able to do any modulo stuff so you could have it do 50 rotations as it moves if you really wanted to

JNeiger commented 5 years ago

Should close #770 with the angle planning

kylestach commented 5 years ago

@JNeiger How different are the dynamical constraints and the slippage constraints? i.e., what are the constraints we get on acceleration when we put u=umax for a given wheel across the spectrum of velocity states compared to the slippage acceleration? IME, adding in a dynamics-based acceleration constraint can definitely improve tracking performance with this type of setup (otherwise, your FF profile following wheel acceleration constraints generates u-values that are out-of-range). (Note: my experience with this kind of setup was using brushed motors so I'm realizing I might be completely off-base here. I may need to go do some proper research on state-space modelling for BLDC first.)

JNeiger commented 5 years ago

Slippage constraints will always happen at a constant wheel acceleration while the dynamic constraints due to the motor is a function of velocity.

That's true with the performance. I'm hoping that BLDC motors are somewhat similar to AC synchronous motors where the max torque is a function of wheel velocity, I need to actually get around to double checking this. It should be non-linear, but I believe we can linearize it so the max acceleration is a linear function of the wheel velocity. If we play with the formulas a bit, we should be able to make it work so that it will never plan accelerations profiles that are too large to be reached using the current constraints on the optimization. A will basically be A - lambda * omega where omega can be calculated using p_dot(t). You should only need to check at the max/min accel values and maybe max velocity.

I'll have to actually do the math to see what wheel velocities we would have to hit that would cause a significant change in the acceleration limit. I believe that we will never hit the max speed where no torque can be produced, but if that does happen, we'll just add the max velocity constraints to the optimization. Based on the acceleration at the ti and tf we can algebraically find the max speed and check the constraint.

Additionally, the on robot torque planner will never be hitting the max torque either. It should never be hit so that when we are following a torque input, we have enough extra power to "catch up" to the command if we fall behind. I believe just a simple trapezoidal thing will work here with smooth acceleration curves and a constant jerk.

kylestach commented 5 years ago

Slippage constraints will always happen at a specific wheel acceleration while the dynamic constraints due to the motor is a function of velocity.

Right. Specifically, I was wondering about the numbers - if the max torque that the motor can give at any speed we care about is greater than the slippage torque, then we can ignore dynamics in planning obviously because that will never be the active constraint. Otherwise, it can be worth it to run some fancier dynamics-based acceleration limiting in conjunction with the slippage-based limiting depending on how big the disparity is.

JNeiger commented 5 years ago

Yeah, I'm leaning on the side of it hopefully won't matter too much and we'll be able to hand wave it away. It'll make things a bunch easier.

They're 60 watt motors on somewhat small field, so I can't see us really hitting those high speeds and even if we did, at the high speeds, we will probably hit some controllability problems due to a variety of other factors. It's something we'll just have to try and see. If it works, we're good, but we should definitely have a plan for what to do when it doesn't.

kylestach commented 5 years ago

Ah, thanks for clearing that up. I haven't really built an intuition yet about how fast these things go compared to their dynamical limits.

kylestach commented 5 years ago

I wrote up an implementation of this at https://github.com/RoboJackets/robocup-analysis/tree/planner-qp. After thinking about it and running it on some sample problems, I've got some thoughts:

For non-degenerate cases (where the minimum cost as defined above will be zero, and a trajectory can be found that exactly hits the target positions/velocities in the correct amount of time), this is exactly equivalent to cubic Hermite splines. For cases where we're acceleration-limited (the target specified isn't feasible in the given time), it will give curves that don't start or end at the right position or velocity - a "best-fit" for the cost given.
- One way to approach this would be to fix the coefficients of 1 and x in the polynomial, so we start with the correct position and velocity, and then solve for the coefficients of x^2 and x^3
- Another option would be to optimize the time of the trajectory instead of holding it constant. This is more difficult (it stops being a nice QP problem and starts to become more convoluted: you hold p0, v0, p1, v1 constant based on specifications and minimize t given the same constraint with M as a function of t, which is decidedly non-quadratic). It seems like it addresses the underlying problem a bit better, though.

I really see two problems that we're trying to solve here: generating paths that aren't "sharp" (i.e. don't create tight corners that are tough for robots to go around) and turning those paths into something that the robot can follow given [p0, v0, p1, v1] without slipping. This seems to try to solve them both at the same time, which seems to have quite a few drawbacks.

Given a path (a series of x, y, theta points), assigning a specific point in time to each point so that the robot moves down the path as quickly as possible without violating wheel-space constraints is pretty easy, but this doesn't help the path-planning problem (one option here is to use cubic hermite splines, basically what Joe talked about, but using some s in the interval [0, 1] instead of t - in this case, you care about the shape of the path only, not the rate). I have an implementation of this that I'm going to push as soon as I clean it up a bit.

This paper solves the problem using the assumptions we had earlier (world-space acceleration constraints instead of wheel-space), but it does optimize both parts of the problem (path and trajectory planning), optimizing for the path with the minimum time. The approach might be worth taking a look at to see if we can borrow anything from it (although as I understand it any attempt to do time-optimal control bounding the wheel accelerations only is going to result in bang-bang).

ashaw596 commented 5 years ago

This a useful paper we were looking at. http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf. It might be helpful too look at. Since, we don't actually need an optimal solution. I might think that taking a look at optimization might, be pretty effective. Solving Path planning, Path optimization, and Trajectory planning separately is probably generally easier. Unless you go full extended RRT.

JNeiger commented 5 years ago

One way to approach this would be to fix the coefficients of 1 ...

I wonder if you can also setup the acceleration curve in a favorable way too to maximize initial and final acceleration. Especially if you split the path into a few more sections and select portions where it's non-feasible. It may be abusing the setup a little too much though. I'll have to think through it a little more.

When you mention the "sharp" paths, is it more a problem with the trajectory generation through two pairs of points or the trajectory generation algorithm itself with specific classes of inputs?

Given a path (a series of x, y, theta points),..

This seems like an interesting idea. I'm not sure I follow how the amplitude scales change between the scaled and final paths.

@ashaw596 , that's the one we use right now right?

ashaw596 commented 5 years ago

Sorta. We took the velocity profile from it. But it covers alot of other stuff actually we never used.

kylestach commented 5 years ago

@ashaw596 that paper seems to take a pretty similar approach to what I was proposing, although I actually haven't seen that particular one before. I'll definitely take a look.

@JNeiger what I meant by "Given a path...assigning a specific point in time to each point" is probably better described as generating a velocity profile for the path (that's what the paper @ashaw596 linked to calls it).

Also, by "sharp" paths, I was just referring to an overall goal of path planning (to reduce sharp corners). In this planning scheme, it'll happen when your planning is independent in two different axes and your path ends up with a large second derivative in one direction over a period where you have a small first derivative on the other. One approach I've seen (this only works when path and trajectory planning are decoupled) is to change the magnitudes of the initial and final "velocities" (they aren't actually velocities in this case, more like control points) to minimize curvature squared, or some similar heuristic. I'm just throwing that out, though - not entirely sure how well it would work.

I wonder if you can also setup the acceleration curve in a favorable way...

Could you explain what you mean by that? The feasibility issue with that method is related to time, so I'm not sure splitting it into multiple segments is going to help (if anything, that just places more time constraints on things).

Also, @ashaw596, when you say "Extended RRT" - do you just mean RRT through state space instead of through kinematic space, or some other variation on RRT? (Do you have a paper link? Edit: this one?)

ashaw596 commented 5 years ago

Yeah basically, that. @justbuchanan tried it before I think and it was too slow I think. We looked at this paper http://msl.cs.uiuc.edu/~lavalle/papers/LavKuf01b.pdf The paths looked really cool though if I remember. Hmm. The curvature method is basically what we have currently implemented. I would look more into the wheel dynamics than the actual path dynamics. We're overly cautious about corners. If you're interested in a seperated velocity profile planning method, I'd recommend just hacking in the wheel limits into the current system to test first. I think another interesting thing to try might be solving a bezier system, and then using some stochastic optimization to nudge it within the wheel constraints while maximizing speed. Not sure how feasible it would be, but it would be cool. Nonlinear constrained optimization

kylestach commented 5 years ago

@JNeiger I updated my branch to only optimize the coefficients of the quadratic and cubic terms, which fixes the issue with it starting at the wrong initial position/velocity.

RoboJackets / robocup-software