Tuning a Swerve Drive Yaw PID Control Loop

I have a very specific question about tuning a PID loop to control the Yaw (angle) of a swerve drive base. A loop such as this allows the user to input a field relative angle, and have the robot autonomously rotate to the desired heading. This was one of the features we implemented in order to accurately align with the vision targets on the field using vision processing. We successfully used this feature in 2019, only instead of using PID, we used hard set values that were proportional to the distance left to rotate. This worked ok, but we are hoping to use a PID loop this season to maintain the heading of the robot more accurately.

My question stems from a behavior specific to swerve drives. When tuning the Yaw control loop (PID or otherwise), when the drive base is in a state of rest, it runs to the desired heading just fine. But once you drive it around the field, the ratio of the input rotation command to the actual speed of rotation of the drive base goes down. This isn’t a problem per say, it’s just a fact of how swerve works. You can think of the X, Y, and Z (rotate) commands as “sharing” the maximum movement capacity of the robot. You maximize your rotation speed by sitting in one spot, but once you start to move, the same rotation input will yield a slower rotation speed. If I didn’t explain that clearly enough, here are some graphical swerve simulations I created that demonstrate this principle quite nicely.

So, my question is, to those who have also implemented a Yaw control loop similar to the one I’ve described (or to anyone who may have an answer), how do you deal with the discrepancy between the behavior of the robot when it is stationary and when it is moving? In our testing, whenever we tuned the Yaw loop properly with the robot in a stationary state, driving the robot around resulted in either oscillating back and forth over the desired heading, or not attaining the desired heading at all (at least until the robot comes to a rest). And due to the face that the ratio of X to Y to Z movement of a swerve drive is infinitely variable, there isn’t a one and done solution that I can think of to account for this discrepancy (i.e. tuning the loop while the robot is moving forward would result in unpredictable behavior when moving diagonally backwards).

As I said before, we were able to deal with this problem last season, but we had to move quite slowly to get accurate results, and we want to find a more robust solution.

Thoughts, comments, and criticism are welcome.

I assume you are using the gyro heading as the variable of your PID control loop.

The problem that you are having is that the gain of the system changes with different robot speeds. At stationary, you have the full speed of the drive wheels to turn the robot. But when you are translating, the rotational speed is reduced.

One way to deal with a system with variable gain like this is to simply over-damp it. This, of course, will slow down the response of the system, but it will be stable for all gains.

It seems like you tried this already and it did not work?

Have you plotted the response of the system versus the desired value versus time? Can you see anything in the plot that could indicate why the system is not reaching the final value?

We have not tried to control the yaw with a PID loop like you are attempting. We have used the yaw variable in a pathfinder routine with good success. Think of rotation angle as a dimension and then generate a “path” that gets you from the current rotation angle to the desired angle.

1 Like

We’ve experienced the problem you’re describing with our swerve. It is a hard one to solve well. In Steamworks, after acquiring a gear on the opposite end of the field, we would automatically rotate the robot to face a selected face of the airship while translating back towards our side of the field. This worked fine because it took several seconds to drive that distance, and in that time, the suboptimal PID worked itself out.

Conversely, this past year, there was a much shorter drive between the loading station and the places to score the game pieces. Because of the difficulty in getting the PID tuned well for all driving scenarios, sometimes the robot wouldn’t have settled on the perfect orientation by the time we got to the cargo ship or rocket. So sometimes we had to pause for a second or two to let itself line up.

The suggested solution of overdamping is a practical, if not ideal, solution, and it is what we’ve done in the past.

You could limit the maximum translation speed command to 80% or so of your max speed, to allow for faster rotation. Alternatively, you can prioritize rotation and cap your translation speed to allow for full-speed rotation. That way you slow down when you do a combo move, but you have consistent rotation speed.

1 Like

The problem isn’t the maximum rotation speed being too slow while moving, the problem is that in order for a control loop to accurately maintain any given angle under a variety of conditions (i.e. moving, not moving, everything in between), it must be very accurate. And as in all things, when you gain accuracy, you typically lose speed. So in order for the Yaw control loop to be accurate, some speed must be sacrificed. This is the heart of the problem.

Is that what you’ve observed? The “heart” of the problem I’m talking about is that mathematically adding two vectors with magnitudes less than 1 can result in a vector with magnitude greater than 1. You’re describing a different but valid problem where the control loops don’t function correctly under dynamic conditions. Increasing gear ratio on the modules can help combat this.

Okay, I misunderstood what you were saying.

This is the issue I thought you were addressing, and is what I’m currently trying to solve. Although speeding up each of the pods wouldn’t completely remove the issue of making the control loop stable in varying states of motion, I agree that gearing the pods for speed would help to increase the maximum rotation speed of the control loop, and consequently would help minimize the time it takes it to arrive at any given angle.

Actually, my thinking is that increasing gear ratio will slow down the rotation of each pod, making it easier to control. If you’re talking about those situations wherea pod needs to flip around 180* during a combined rotation/translation move, that would be easier with a faster rotating module at the cost of making the module PID harder to find.

I don’t think changing the rotation speed of the modules is the issue here.

For one thing, during normal operation, there is a PID loop running on the module rotation to keep them pointing in the directions they are commanded to point. If you change the speed of the modules, you could make those PID loops unstable.

For another thing, as the robot is rotating and translating, the modules are not needing to change their steering angle very quickly. I don’t think the issue here is that the modules are not getting to their commanded positions. Rather, I think that the commanded positions are just changing too slowly because of the nature of the control loop that is setting those steering angles. But this is something that would be easy to verify. If you plot the commanded angles of each of the modules versus the actual angles, you would be able to verify that the modules are responding correctly.

I suspect that the issue really lies in the control loop that is setting the 4 steering angles of the modules and trying to achieve a robot rotation (yaw) angle. I suspect that if you plotted that versus time, you would see that the commanded angle is just slow at achieving the desired angle. You may see that the measured yaw angle is not as stable as you would like it to be due to the dynamic conditions. There may be other issues that are revealed by plotting this.

You could, in theory, have the PID settings change dynamically as the available steering speed changes. This would be tricky to tune, but if you could find stable, but not over damped settings for each translation speed (and associated available rotation speed), then you should be able to optimize the response. But I would not venture down this path until you have analyzed the current behavior of the robot and looked for other, simpler issues that need to be resolved.

This was my thinking as well.

I was referring to the speed of the driven wheel, not the rotation of the pod. Additionally, this situation isn’t a problem for us as our pods never turn more than 90 degrees (if they have to turn 180 degrees they simply turn 90 degrees and reverse their direction of travel).

I’ve considered this before, but I agree that it is a rabbit hole best not jumped down. I think that over damping the PID loop may be the solution we’re looking for (even if it isn’t the solution to this problem).

Next time I have access to our robot I’ll try implementing some of the suggestions mentioned in this thread. I’ll report my results for those interested.