Field Relative SwerveDrive Drift (Even With Simulated Perfect Modules)

Technical Control System
, ,
1

So our team has not used a Swerve Drive in any of our seasons before, but we were experimenting with it using the simulation tools within WPILib. We noticed that when doing field relative controls, if you turned while moving, you would slightly drift in the direction you were turning. We thought that this was because the PID loops were not perfect (which does contribute to some drifting), but even when we simulate perfect modules, there is still a bit of drift while moving and turning.

We soon realized that this is a bi-product of controlling the robot in discrete time steps. As you can see from this desmos we made, the swerve drive will drift slightly in the direction you’re turning due to the robot traveling in a curved path. We also found the exact amount you can rotate your translation vector by to account for this which is 0.5 * w * dt where w is your angular velocity and dt is the time between each robot update. But even this wasn’t perfect, because when your swerve drive desaturates the motor velocities, it changes your angular velocity, so you need to adjust w based on how much your wheels are being desaturated. Accounting for all of this, we were able to get our swerve drive to move in a straight line while turning in the simulation.

However more issues arose when we simulated our PID loop. Because the modules were not perfectly following their trajectories, the robot would drift while moving and turning. We are using NEO’s on our swerve drive as we do not have access to falcons, which makes the PID loops a little bit worse for our modules. Is there any way to account for the drift caused by imperfect swerve modules? or do teams generally just ignore the drift that happens during a match.

5 Likes
2

Are you guys running custom swerve modules or COTS ones? As I know SDS has base code with PIDs for their modules, which it seems that a lot of teams used, so I don’t know if they are doing anything special.

2 Likes
3

We are using COTS ones, but we are using NEO’s and we are also using an absolute encoder. It still drifts when moving and turning though, due to provided PID loop not being great and there just being some fundamental math issues that cause drift.

4

I haven’t really worked with swerve so this might not work at all, but couldn’t you try to use the gyro to measure any drift that is occurring and then compensate by nudging the robot in opposite direction it is drifting.

5

You could try running a PID loop using the gyro angle as the input and the output being a target robot rotation that gets sent to the swerve modules. This would serve as an “active” method of compensating for the drift of the robot angle. This is the approach I took for the same issue on our robot.

1 Like
6

We don’t have issues with the robot angle drifting, but just that, when we are turning and moving, the robot does not move in the direction that we are telling it to.

1 Like
7

Oh I misunderstood. Are you using the onboard PID controllers, because those are not limited by the robot code’s running frequency.

2 Likes
8

We are planning on using these, but there we will have to see if the drift issue persists even when using those.

9

Have you tuned your PID controllers? if you haven’t that could be another explanation for the improper angles.

10

if you are using sds ones I’m 99% sure they have code for both NEOs and absolute encoders built in so I would consider looking at it if you want a fast solution

2 Likes
11

Have you verified this behavior on a robot or is this all in the simulator?

12

We expected that we would need to compensate for this in our field coordinates calculation. After we were happy with the body frame tuning, we adjusted the coordinate transform with a little lead compensation:

void Drivetrain::ToFieldCoordinates(double *xdot, double *ydot)
{

double vNorth, vEast, theta;
vNorth = *xdot;
vEast = *ydot;
// field coordinates conversion
theta = GetGyroReading() * (M_PI / 180.0);
theta = theta + GetGyroVel() * (M_PI / 180.0) * 0.15;
*xdot = cos(theta) * vNorth + sin(theta) * vEast;
*ydot = -sin(theta) * vNorth + cos(theta) * vEast;

}

Note the 0.15*gyro velocity compensation to theta. That’s 150 milliseconds of effective delay (between when we measure the heading and when it gets to the motors).

We tuned it with the highly scientific process of spinning the robot and driving it backwards and forwards. When properly adjusted the robot would travel in the same direction spinning or not. For what it’s worth, this is about the value we would expect (not that it matters).

13

This post came out later with a solution that basically solves this. I don’t know if you’ve seen it yet, but I’ve tried it out and it seems to work.

2 Likes
14

TLDR: you want the Twist2D of the motion to land on the wanted pose at the next timestep

15

for those interested, we did something similar to @sspoldi , but ended up dialing even more delay, 250ms!

    double gyroRate = m_gyro.getRate() * 0.25;
    Rotation2d correctedRotation = getPose().getRotation().minus(new Rotation2d(gyroRate));
    desiredChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
            kMaxSpeedMetersPerSecond * xSpeed,
            kMaxSpeedMetersPerSecond * ySpeed,
            kMaxAngularSpeedRadiansPerSecond * rot,
            correctedRotation);
16

this is cool. I think the primary driver for the delay is the swerve steering bandwidth. Our steering loop closure is very high gain, 150 ms is good for our implementation, but 250 ms is not unreasonable (note that we reduced the internal velocity output filtering in the falcons to get faster velocity data response time. I think we’re using 50 ms for the difference value and 32 ms for the average(vs the baseline 100 ms and 64 ms respectively).

Steve S.

1 Like
17

update: we turned off the falcon velocity smoothing entirely, and that let us turn down the delay to 150 ms.

1 Like