Differential Swerve Advice Needed From Controls Experts

5205 has been developing a Differential Swerve design for the past 2 years.

Mechanically, it’s a very mature design. The issue has been programming and finding the correct algorithms to control the differential swerve. After 2 years of development, I feel like we grasp the math and PID programming. The challenge stems from the differential swerve concept itself. Because both motors control drive and steering, there is no perfect way to independently gear the motors. Traditional swerve allow for separate gear ratios and that allows for good steering response and good speed. Our traction swerve does not. We thought we had mitigate that issue by spacing the two drive wheels further apart but width constraints did not allow us to fully take advantage of that strategy.

The result is that at low speeds, the robot performs relatively well. At high speeds, about 8ft/s, drive pods tend to dive inward or outward and the system becomes fairly unstable. I believe the low gear ratio for steering, about 10:1, coupled with the lag of the RIO, is the cause of this instability. What makes it worse potentially is the low encoder CPR on the NEO’s we are using. Trying to control in closed loop velocity control mode using the controls built into Spark Max results in either too much overshoot and oscillation or not enough stiffness. D gains seem to not be helpful because of the lag and low gear ratio. So there is no good way to damp out the oscillation. (Max velocity commands are on the order of 500 to 800 RPM)

We have placed it into smart motion mode. That results in much improved steerability. We had thought this was going to be our solution. But still when moving at higher speeds, the swerve pods still want to dive in and out.

Perhaps our algorithm is wrong. We are essentially commanding a position by reading current position and adding to that with two components. First component is for steering. That is based on absolute encoder reading, Target Angle - Current Angle multiplied by a gain, essentially P, in PID. The gain is just the gear ratio * the wheel spacing/wheel diameter and a little bit to account for wheel slip. The second component is the hypotenuse of the X and Y axis of the joystick * another gain. We find that the higher the gain, the faster the max speed is. You add those together to get the combined command position.

Here is a video showing just the steering command. It is spritely and responsive with little overshoot. And the stiffness of the system is fairly adequate.

(You’ll notice a jitter when it gets to target. I think that is another issue to solve. )

I don’t have any video of our test bot in motion. I can provide that if someone wants to see it. I think the diving is caused by the fact that controlling position via this combined signal doesn’t guarantee the pod will turn and go straight at the same time. Honestly, I’m not really sure why this doesn’t work.

My question is, is there a smarter way to do this? Is there another type of algorithm that would provide much better control.

This post seems to suggest that position control is possible:

We are in possession of 2 falcon 500’s and we will be installing it on one of the pods to see if that improves control-ability. We are also going to attempt to measure physical differences in each pod to create a offset table for each wheel to attempt to counteract slight variations in wheel OD. I don’t have a lot of faith in either avenues.

Looking for any thoughts, advice or help.

Lastly, we do have a backup plan in case this drive system does not succeed, just for all of you thinking right now that I’m crazy not to have a operational drive design at week 5.

Appreciate all who have read to the end of this thread. See you in competition.

4 Likes

So, my 2c is that a system this complicated, with dependent/linked dynamics, really can’t be adequately controlled by PID, which I think is what you’re experiencing — your separate PID loops can fight each other, because each is trying to minimize error without knowledge of what the others are doing. On top of that, your system dynamics are extremely complicated in the first place — your wheel moments of inertia are effectively different, depending on the behavior of the other motor (if both are driving in the same direction, you’re accelerating the robot, and if both are driving in opposite directions, you’re accelerating the pod). Also, your wheels might have variable traction, which’ll kill any sort of controllability.

You should look to the field of modern control theory to deal with this problem — my money is that PID won’t cut it. @calcmogul literally wrote the book on using it in FRC. If you’re able to model the system as a set of differential equations in state-space form, you can use what’s called an LQR (Linear-Quadratic Regulator) to control the system — you can think of it as a really, really advanced PID controller, one that’s able to control multiple states of the system at once. In addition, you can use a Kalman filter to both improve your sensor readings by filtering your encoders and “faking” a higher update rate, and to do things like estimate the traction each wheel has against the ground, which is information that can be fed into the LQR to better control the system.

This is some really, really advanced stuff — probably a solid offseason or two worth of work to get this sort of control system up and running, but if you ask me, it’s the best way to get everything to top performance, reliability, and robustness. I’d be happy to lend some more assistance if this is the direction you decide to go.

Also, a two-wheel traction diffy like this is pretty fundamentally flawed — you’re very much at the mercy of your wheel traction; your control loss at high velocities might be a result of differences in traction. There’s ways to compensate for this (Kalman filter or load cells for estimating per-wheel traction, for instance), but that being said I’d recommend doing a single-wheel diffy, with a gear differential. Bit trickier on the design side, but eliminates a point of failure.

As for things you can do right now, your Rio almost certainly isn’t having any real lag, and if it is, that’s a code issue, which should be eliminated. I’ve run 200Hz control loops on the Rio, no problem (even in Java/Kotlin). You’re probably getting some latency reading encoders over the CAN bus — you should run separate encoders to the Rio and see if that helps. Plenty of DIO ports, thankfully (10 on the DIO header, 16 on MXP, which is enough to read a quadrature encoder from each motor as well as the wheel pod angles).

All that being said, you might just want to slap some conventional swerve pods on, and continue diffy development in the offseason.

10 Likes

To build on this, I think what you’re looking for is a MIMO (multiple input, multiple output) controller, where the motor speeds are inputs and the module wheel and rotational speeds of the module. The implementation, however, would be quite complicated and probably not achievable in the next 2 weeks unless you really know what you’re doing. There’s some stuff on it here: wiki

I think 971 may have done one in 2017, but I could be wrong about that.

1 Like

The controls issue is aggravated by differences in traction at speed pulling the pod heading around…

Adding a pneumatic or servo-driven friction brake would preserve low robot speed agility and simultaneously add the high robot speed damping…

1 Like

So you are doing position control of each motor independently (via smart motion, so you get acceleration and velocity limiting), but adjusting the position setpoint based on pod steering error and joystick throttle? I am not surprised that this doesn’t work across a wide range of speeds, because there is no explicit synchronization between the two motors.

The yaw rate of your pod is proportional to the speed differential between the two motors. So you need to close the loop such that yaw errors command a speed differential. The most direct way to do this is to use voltage or velocity control on each motor, and adjust the voltage / velocity setpoints by the output of a yaw controller, ex:

setpoint_L = desired_speed + PID(yaw_error)
setpoint_R = desired_speed - PID(yaw_error)
(Then optionally normalize such that neither side exceeds the max setpoint but the ratio between the two is preserved)

PID may just be P in practice. Note that Talons / Falcons can actually do this all in hardware by using the auxiliary closed loop feature.

This architecture has worked well for many teams for getting differential drive trains to drive in straight lines or controlled turns. That’s all your swerve pod is - a tiny little differential drive train.

It may well be that the dynamics in your case may be more challenging than PID alone can control, but I wouldn’t draw that conclusion yet.

6 Likes

Thanks for everyone’s input.

@Jared_Russell, We were a little hopeful that commanding displacement was enough to sync the 2 sides. By setting a simple P gain, we were able to modulate the resulting speed. But, we came to the same conclusion tonight after trying a few additional tests that even though velocity could be changed by commanding larger displacements, it did not allow the two sides to be correctly sync’d.

What we know is that displacement control works really well in just pure steering control. Closed Loop Velocity control worked fairly well in driving straight.

Somehow we need to find a way to combine the two modes. The trapezoidal velocity profiling of the smart motion displacement control is what allows it to perform better than just a normal PID on velocity. I believe that trapezoidal profile could be mimicked on the RIO using state space modeling as @solomondg is suggesting. Setting an acceleration rate, max V, and reading current displacement, velocity and acceleration, I should be able to create the same trapezoidal velocity profile on the RIO and combine it with a velocity command for forward motion and send that to the Spark Max under closed loop velocity mode.

My only concern is that under Smart Motion Mode the Spark Max was creating the velocity at 1 khz rate. Under the RIO, I’m afraid the 200hz rate will be too slow for it to respond quick enough to the state changes.

Does any of this sound like a worthwhile path to explore?

Trying to learn and teach State Space control and LQR in the middle of a season is not going to happen. So while that might be the optimal path, right now I’m looking for a band-aid that won’t likely come off in the middle of a competition.

Again, appreciate all the inputs.

The other theory we are working from is that at high speed the steering command is getting lost in the general fluctuations of the forward command. We have tried scaling the steering command with the forward command. But that just leads to oscillation. I believe a standard PID isn’t capable of handling blind scaling. @solomondg is correct that a standard PID won’t provide a tight enough control.

Any ideas on how to combat that and does anyone thing the motion profiling idea via RIO will assist in that?

It has been a few years since I’ve worked on a swerve system and we had to do it with jaguars. Our system was completely unstable on a test bed. I would try to drive it and see how it really performs before trying to characterize a convoluted system and solve differential equations. Also testing at full weight on similar carpets changed our tuning significantly.

The video above is on a testbot with full weight. It is as actual as can be.

We’ve made two traction differential swerve drive chassis and abandoned the idea because the difference in traction seemed like it would be impossible to overcome.

We switched to a “traditional” differential swerve and it is much more stable. We plan to compete with it this year, but there is a quick way for us to convert to a traditional swerve.

My control recommendation is as follows: Run the Sparkmaxes in velocity mode. The default PID values are a pretty good place to start. Do trapezoidal ramping in the Rio. Don’t accelerate so fast you might lose traction. Limit yourself to an achievable top speed. Use your heading error to slow down one wheel and speed up the other. Just use a simple P loop. Scale the heading P with wheel velocity so the faster you are going, the steering loop has less power. Think about how far you want the wheels to rotate in one rio loop. Limit the wheel speed difference with this knowledge.

If you want to go down a real rabbit hole, use two Falcons and a Cancoder to move the loops off the rio entirely.

Unfortunately, I think you will struggle with a traction design.

Ryan Shoff
4143

What was the behavior that lead you to think that traction or lack thereof was the issue.

Was it steering control at high speed like what we are seeing?

We have found that smooth transition in speed, controlling acceleration closely, results in better control. But our interpretation of this was as acceleration rates increase it was harder to maintain general velocity control.

We are finding that maintaining traction isn’t really what is causing our issue. We have moved to a 3 pod design to ensure good contact with the ground. But we do not see improvements in steering.

I think that making the leap from not synchronizing your motors to declaring that you need full state control and LQR to solve this problem is conflating two things. The reason that independent position control works for turning the module in place is that synchronization error doesn’t have a big effect when you aren’t also translating. Step 1 is closing the loop on the right process variable and output (yaw error -> differential speed). Step 2 is figuring out where your control loop is deficient. It sounds like others have had modest success using PID to control differential speed with some setpoint smoothing measures and gain scheduling to avoid saturation and prevent instability at higher speeds.

All that aside, I would not use this type of drivetrain in FRC

4 Likes

Its been a few years, but I remember three things that I didn’t like with the design. First was one module always had less traction than the other 3 and behaved different. It sounds like you designed around that be going with 3 modules and maybe some suspension. Another one was it could work ok on carpet but hardly at all on a tiled surface. And finally you can’t just put it on blocks and test it. Those are why switched to a different design. I don’t mean to discourage you. You went farther with it than we did, so you very well may get it to work.

If you do, try some 6 wire sliprings on those modules.

So you have two open-loop methods that both work for narrow use cases. Calling them “open loop” because it’s not clear you’re synchronizing the speed differential delta-V, even when calling the velocity command. You need a closed-loop method to directly control the delta-V like Jared suggested in his first post.

Later in your post, I think you’re starting to get there - but it’s still not clear to me whether your synchronization method is actually drawing a direct control loop from yaw error to the differential in velocity.

If I needed a Build Season Bandaid, I would add a physical brake on the swerve bearing, then use your two working methods to get a robot that turns pods and goes in discrete steps.

You’re going to have issues maintaining heading at speed no matter what.

Traction

As soon as you break traction on one wheel (hit a piece of tape, get up on the edge of a field element (ie trench footplate, switch footplates, switch floor protector), start pushing another robot, have weight dynamically shifting onto outside pod wheels through a sweeping turn), your heading will whip around and the algorithm will need to cut power at that drive pod.

That could get mitigated with advanced* module suspension, mechanically damping the bearing, a servo/pneumatic-actuated brake, or all of them.

It looks like you have linear “contact suspension” right now which would work for a single point of contact, but it’s not clear whether the inside wheel will get held down through a sweeping turn with the current setup.

Dynamic load purple needs to get distributed to inside wheel green

Targeting a field performance closer to an old point-and-go crab drive rather than a modern swerve might help you bound the problem better and get the robot on the field.

For a tank drive it’s pretty easy to assume both sides of the robot have traction - it is hard to assume that all 8 contact patches on this robot have good traction, which introduces a potentially very large non-deterministic error term in the physical plant. The effect of the delta of velocity controlling the heading of the pod therefore is also not deterministic in this design.

I’d like to try to reset the baseline. I think I’ve given the impression that after 2 years of development we still have a fancy paperweight. I think my use of “bandaid” was a poor choice of words.

This is video of it running at the max speed we feel will prevent instability. This uses a closed loop velocity speed command pid in the spark max. Math running on the RIO based on @ether’s whitepapers calc pod angle and magnitude of speed command. We then send pod target angle into a PID loop as the set point and use an absolute encoder of each pod as the PV. The encoder on this bot is a CTRE mag encoder. We read absolute heading via PWM duty cycle first and then use quad signal to calc instantaneous change in angle. That angle PID command is then added to the speed command from above to create an overall speed command and sent to the Spark Max closed loop velocity PID. Lastly we also realized that being a two wheeled pod the inner and outer wheel should be traveling at slight different speeds. So we do have math that differentiates inner and outer wheel speed.

One more thing because tuning in the air is not possible we have a flywheel dyno setup that allows us to trial tune the spark max pid loop.

Totally understand that by all measures this design will always be a sub optimal swerve. Our hope is to create a hybrid swerve tank drive that has good mobility and can hold its own in a heavy def. Or play heavy d on a standard swerve. Video below shows pushing power.

@RyanShoff we did try a slipring design earlier on and found it to be a potential failure point. We haven’t given up on that idea totally but for now we’ve shelved that.

I do appreciate all of the words of warning. We do have a plan B and C waiting in the wings. Without bag this year we are pushing out schedule and being a bit less cautious than we would normally be. Cross my fingers that it works out. Believe it or not this is not the craziest thing we have ever tried. I give you exhibit A:

and exhibit B:

1 Like

Quick update -

Original post was to ask for advice about algorithms that can improve differential traction swerve steering performance. This was back in February.

It turns out that our original hardware was based on NEO’s with limited encoder resolution. We finally got a chance to change the NEO’s out and use Falcon 500’s instead. With the increase resolution, and fairly straight forward PID tuning, the pods are incredibly responsive. There is still instability at extremely high speeds but we have yet to implement normalizing and ramping algorithms so we are hoping that will provide good control at very high speeds. Clearly more work needs to be done but complicated state space controls seems unnecessary.

Here is a video showing the pod steering responsiveness.