is my robot skidding?

background

In my free time, I mentor FIRST Robotics, where part of my job is teaching advanced controls theory. It turns out it's moderately hard to get the average highschooler to have fun doing math, so I decided a more visual self-paced format to absorb information would (hopefully) prove fruitful. This is the first installment in a series of incredibly niche concepts, but the problem solving and the concepts behind them are universally applicable.

on to the lecture,,,

how to tell when ur skidding

Based on Team 1690 Orbit's Software Session

the problem with trusting your wheels

Ordinarily, we fuse readings from various sensors on the robot to calculate a representation of the robot's position in space. This is usually a combination of a locally-derived position based on the robot's motor readings, and an objectively-derived position based on vision. A significant factor to the locally calculated (dead-reckoned) odometry is the wheel's encoder values. By seeing how much the wheels have turned, and multiplying that by pi and the wheel's diameter, we can calculate the linear displacement contributed at any moment, and sum over these to attain a position. This is mostly fine, with a caveat.

Wheel encoders measure velocity by counting how fast the wheel spins. But if a wheel is not maintaining constant and perfect traction with the floor, the encoder reports more or less motion than is actually happening. Simple enough, but how do we determine when this is the case?

redundancy

A swerve drive has 4 independent modules, each reporting:

speed — how fast the wheel is driving along the floor (m/s)
angle — which direction the wheel is pointing (radians)

Robot motion only has 3 degrees of freedom:

$v_x$ — forward/backward velocity
$v_y$ — left/right velocity
$\omega$ — rotational velocity

We have 8 numbers describing something that only needs 3. This system is overdetermined.

the rigid body constraint

A swerve robot is a rigid body. The distance between any two points on it stays fixed, no stretching, no deformation.

When a rigid body moves, any point's velocity is the sum of two components:

$\vec{v}_{\text{point}} = \vec{v}_{\text{translation}} + \vec{v}_{\text{rotation}}$

The translational component is the velocity of the reference point we measure module positions from, which for us is the robot's center

Translation is identical for every point on a rigid body. If the robot's center moves at $(v_x, v_y)$ , then every point on the robot — including all four wheel modules — shares that same translational velocity.

This is the foundation of the entire algorithm.

Pure translation: all four modules have identical velocity vectors.

rotation

Rotation is different. When a rigid body spins around its center, points farther from the center move faster. A point at position $\vec{r} = (x, y)$ relative to the center has rotational velocity:

$\vec{v}_{\text{rot}} = \omega \times \vec{r}$

In 2D, this cross product simplifies beautifully. If we define $\omega$ as a scalar (positive = counterclockwise), then:

$\vec{v}_{\text{rot}} = \omega \cdot (-y, x)$

Why $(-y, x)$ ? This is the vector perpendicular to $\vec{r}$ , pointing in the direction of rotation. You can verify: if a point is at $(1, 0)$ — directly to the right of center — and we rotate counterclockwise, it should move upward. Indeed: $(-0, 1) = (0, 1)$ .

The magnitude follows directly:

$|\vec{v}_{\text{rot}}| = |\omega| \cdot \sqrt{x^2 + y^2} = |\omega| \cdot r$

Points farther from center move faster. This is why the rotational component differs for each module based on its position.

Pure rotation: each module's velocity is perpendicular to its radius, with magnitude proportional to distance from center.

superposition

Practical motion on a swerve combines both. Each module's measured velocity is:

$\vec{v}_{\text{measured}}^{(i)} = \vec{v}_{\text{trans}} + \vec{v}_{\text{rot}}^{(i)}$

For module $i$ at position $(x_i, y_i)$ :

$\vec{v}_{\text{measured}}^{(i)} = (v_x, v_y) + \omega \cdot (-y_i, x_i)$

Expanding into components:

$v_{\text{measured},x}^{(i)} = v_x - \omega \cdot y_i$ $v_{\text{measured},y}^{(i)} = v_y + \omega \cdot x_i$

The measured velocities differ between modules, but the underlying translation is the same for all of them. The differences come entirely from the rotational component.

Combined motion: measured velocities (blue) differ, but translational components (orange) are identical.

the inverse problem

Now we can frame the detection algorithm. We have 4 modules reporting measured velocities. We want to:

Estimate the robot's motion $(v_x, v_y, \omega)$
Extract each module's implied translational component
Check if they agree

forward kinematics

Each module gives us two equations (x and y components). With 4 modules, that's 8 equations for 3 unknowns. In matrix form:

$\begin{bmatrix} v_{1x} \\ v_{1y} \\ v_{2x} \\ v_{2y} \\ v_{3x} \\ v_{3y} \\ v_{4x} \\ v_{4y} \end{bmatrix} = \begin{bmatrix} 1 & 0 & -y_1 \\ 0 & 1 & x_1 \\ 1 & 0 & -y_2 \\ 0 & 1 & x_2 \\ 1 & 0 & -y_3 \\ 0 & 1 & x_3 \\ 1 & 0 & -y_4 \\ 0 & 1 & x_4 \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ \omega \end{bmatrix}$

Or more compactly: $\vec{m} = A \vec{s}$ , where $\vec{m}$ is measurements, $A$ is the kinematics matrix, and $\vec{s} = (v_x, v_y, \omega)$ is the chassis state.

This is overdetermined — 8 equations, 3 unknowns. We solve it with least squares regression:

$\vec{s} = (A^T A)^{-1} A^T \vec{m}$

This finds the chassis state that best explains all 4 module measurements, minimizing squared error. We have to use least squares instead of solving the system directly because an overdetermined system only has an exact solution when the measurements are perfectly consistent, and thanks to sensor noise and real-world factors, they basically never are. So we find the closest thing instead.

java

ChassisSpeeds speeds = kinematics.toChassisSpeeds(moduleStates);
double omega = speeds.omegaRadiansPerSecond;

inverse kinematics

Now we compute what each module's velocity would be if the robot was only rotating (no translation):

$\vec{v}_{\text{rot}}^{(i)} = \omega \cdot (-y_i, x_i)$

java

SwerveModuleState[] rotationalOnly = kinematics.toSwerveModuleStates(
    new ChassisSpeeds(0, 0, omega)
);

isolating translation

If $\vec{v}_{\text{measured}} = \vec{v}_{\text{trans}} + \vec{v}_{\text{rot}}$ , then $\vec{v}_{\text{trans}} = \vec{v}_{\text{measured}} - \vec{v}_{\text{rot}}$ .

For each module, subtract the rotational component to get the implied translational velocity:

$\vec{v}_{\text{trans}}^{(i)} = \vec{v}_{\text{measured}}^{(i)} - \vec{v}_{\text{rot}}^{(i)}$

java

Translation2d measured = new Translation2d(measuredState.speedMetersPerSecond, measuredState.angle);
Translation2d rotational = new Translation2d(rotationalState.speedMetersPerSecond, rotationalState.angle);
Translation2d translational = measured.minus(rotational);

Decomposition: after subtracting rotation (purple) from measured (blue), all four translational components (orange) should match.

the consistency check

Now we can finally do something useful. In a perfect world with no skidding:

$|\vec{v}_{\text{trans}}^{(1)}| = |\vec{v}_{\text{trans}}^{(2)}| = |\vec{v}_{\text{trans}}^{(3)}| = |\vec{v}_{\text{trans}}^{(4)}|$

Why? Because translation is the same for all points on a rigid body. After removing rotation, what's left must be identical. If the extracted translations disagree, the measurements are inconsistent.

We quantify disagreement with the skid ratio:

$\text{skidRatio} = \frac{\max_i |\vec{v}_{\text{trans}}^{(i)}|}{\min_i |\vec{v}_{\text{trans}}^{(i)}|}$

Skid Ratio	Meaning
1.0	All 4 points are moving uniformly
> 1	something funny is happening

what skidding looks like

There are two kinds of wheel slip, and they produce opposite encoder errors:

Wheel spin — the wheel loses traction and spins freely. The encoder reports more velocity than the robot is actually achieving. Less common in FRC, but happens on worn carpet or when accelerating aggressively.

Wheel drag — the wheel locks up or drags across the surface. The robot is still moving, but the wheel isn't rotating as fast as it should. The encoder reports less velocity than the robot's actual motion. This is the classic scenario where you suddenly brake, and your wheels indeterminately skid across the carpet while momentum carries you forward.

Both cases cause the robot to yaw toward the affected wheel. Say the front-right (FR) wheel is the one misbehaving: with wheel drag, FR locks up and creates more resistance, so the robot pivots around that corner — like slamming one brake on a car. With wheel spin, FR provides less thrust, so the left side overpowers it.

The detection algorithm catches both due to only caring about magnitude of disagreement rather than direction

Wheel drag: FR locks up and reports low velocity (short red vector) while the robot curves toward the dragging wheel

edge cases

division by zero

When the robot is stationary, all translational magnitudes approach zero. The ratio becomes $\frac{0}{0}$ , which is undefined.

java

if (minTranslational < 1e-4) {
    return 1.0;  // Can't skid if you're not moving
}

pure rotation

When the robot spins in place with no translation, all extracted translations should be zero. In practice, sensor noise gives small random values. The ratio might be $\frac{0.01}{0.005} = 2.0$ , which looks like skidding.

But notice: the magnitudes are tiny. We can either threshold on absolute magnitude (ignore small translations) or accept that the ratio is unstable when translation is near-zero. In practice, pure rotation without any translation is rare during actual driving.

why this works

this solution is actually quite elegant; four wheels reporting to three degrees of freedom means we have more information than strictly necessary. This allows us to do internal consistency checking, and is actually a very common theme across engineering! it's why a three-legged stool never wobbles but a four-legged table can: three feet define a plane, and the fourth foot either lands on that plane or it doesn't. the redundant leg is a built-in consistency check, same as our fourth wheel.

sandbox

if you just wanna play around with the simulation made for this article

references

Team 1690 Orbit Software Session — Original explanation
WPILib Swerve Kinematics
254 Cheesy Poofs