Odometry implementation is innacurate

[Aditya1104]

It’s my second year doing VRC, and this year I decided to extend my knowledge in programming my implementing a odometry system. Trying to challenge myself, I tried to create it from scratch by myself, and it works! I find X and Y positions relative to the starting point by using the gyro’s heading + the distance covered by the drivetrain encoders. Although this works like a charm, as soon as you go anywhere over 30% speed, the numbers start going haywire. I thought that maybe putting a 10ms delay between each loop doesn’t update it fast enough, but removing that loop timer doesn’t change anything. Any kind of solutions I can implement? Will I have to add a tracking wheel?

Here’s my code, it’s very far from done but the odometry code is located in “Odom.cpp”

GitHub - adityacodes5/Redesign

[5062F_Adam]

Yes, it would be much more precise to use a horizontal tracking wheel than to just use the gyro sensor for position.

[NO1]

So if I had to guess, your wheels might be slipping when you drive over 30% speed, so instead of moving the robot, the wheels spin in place and this throws off your odometery because your algorithm thinks the robot is moving when it is not. Generally you can use a tracking wheels to account for this.

[technik3k]

In addition to wheel slippage, mentioned by @NO1, another source of error could be erroneous sensor readings that may occasionally return out of range values.

For example, if your rotation() function returns sequence: “121, 123, 125, 0, 128, 130, …” then 0 is, clearly, out of range and should be filtered out even before it gets to the code that accumulates instantaneous sensor values to compute integrated robot position.

You may want to print a debug stream of input sensor values to see if there is anything jumping out.

[anon4126930]

I want to help with anyone trying to build their own odometry. First of all, my example code simulation below is based upon the odometry unit center:

The following illustration shows the center of the odometry unit. The odom unit center is the virtual intercept of the perpendicular faces of the odometry trackers. The measurements is from the center of the odom unit to the designated tracker distances.

The forward tracker is always offset right/left from the center of the odometry unit center, while the sideways tracker is always forward/backwards from the center of the odometry unit.

Based upon the following illustration above, with 2.5189 inch wheels, the forward tracker being offset to the right, and sideways tracker being offset back of the odometry unit:

python

# Configuration

encoder_ticks_per_revolution = 360

wheel_radius = inches_to_meters(2.5189) # meters (can be wrapped from inches via inches_to_meters)

odom = WheelOdom()

odom.set_physical_distances(inches_to_meters(1.5),inches_to_meters(-4.468)) # (forward tracker position [+right, meters], sideways tracker position [+forward, meters])

It’s important to note that this example does not include a transformation to the center of the robot. Either you must program a transformation from this point to the center of the robot, or you should just place the odometry wheels such that the odom unit center is center of the robot. Note that traction wheels will shift the center of the robot towards its position

From this we will set the starting position of the odometry unit center to 0, 0 meters with a radius of 0 radians:

odom.set_position(0, 0, 0) # Reset to 0,0 meters and a yaw of 0 radians

Then, I will run a simulation where I simulate encoder ticks of the odometry wheels, alongside the yaw of the robot, and translate it to meters:

# List of updates: (forward_tracker_pos, sideways_tracker_pos, new_orientation_rad)
updates = [
    (encoder_ticks_to_meters(750, encoder_ticks_per_revolution, wheel_radius), encoder_ticks_to_meters(0, encoder_ticks_per_revolution, wheel_radius), np.pi/4), # forward tracker sensor reading of 1, yaw to pi/4
    (encoder_ticks_to_meters(1500, encoder_ticks_per_revolution, wheel_radius), encoder_ticks_to_meters(0, encoder_ticks_per_revolution, wheel_radius), 0), # forward sensor reading of 2, yaw of 0
    (encoder_ticks_to_meters(750, encoder_ticks_per_revolution, wheel_radius), encoder_ticks_to_meters(0, encoder_ticks_per_revolution, wheel_radius), 0) # forward sensor reading of 1, yaw of pi/4 <- notice the robot goes in reverse
]

This is the following result:

As you see, this is arc-based odometry. Its tracking is a lot more accurate than linear-based odometry because it respects the motion of the robot.

You can mess around with the odometry class and you can translate it to C++ if you’d like via the link below. Note that a yaw of 0 radians is facing the +X axis:

Google Colab

[sazrocks]

@anon4126930

You can mess around with the odometry class and you can translate it to C++ if you’d like via the link below.

Where is plot_trajectory defined? I get a NameError when I run your code. It also can’t find /content/plotter.py.

[sazrocks]

@anon4126930

As you see, this is arc-based odometry. Its tracking is a lot more accurate than linear-based odometry because it respects the motion of the robot.

I’d be curious to see a proof of this. Given an infinitely small time step, both should be equally accurate. Given a finite timestep, the arc odometric assumes all robot movements since the last timestep have been perfect arcs and linear assumes perfectly linear movement. At the small timesteps used in VEX odometry (10ms), I’m not sure one of these approaches is significantly more valid than the other.

[anon4126930]

It was initially in another folder. I think it likely was only I can see. I have relocated the plotter and it should work now.

[sazrocks]

@anon4126930

Looking at this animation, it seems curious to me that the sideways tracking wheel reads a constant 0 despite the robot yawing during its movements. Can you explain why this is the case?

[anon4126930]

@sazrocks

I’d be curious to see a proof of this. Given an infinitely small time step, both should be equally accurate. Given a finite timestep, the arc odometric assumes all robot movements since the last timestep have been perfect arcs and linear assumes perfectly linear movement. At the small timesteps used in VEX odometry (10ms), I’m not sure one of these approaches is significantly more valid than the other.

Intro to Odometry in FTC | The Clueless #11212

I think you may have a point about them both being identical in precision in an infinitely small time step. However, I feel like 10 milliseconds is a lot more significant than you would think.

If we assume 600 RPM on 2.75" wheels:

The robot is traveling at a speed of 2.19 meters per second. If we were to multiply this by 0.01 seconds (for 10 milliseconds) and then convert back to inches:

If we assume the near limit of a VEX Robot in competition, we would assume 0.86 inches of travel between each 10 millisecond step, assuming the robot travels in a straight line. I believe even if the RPM were half the amount, I believe the arc-based odometry performs a lot better than linear-based odometry. I am unsure if okapilib uses arc-based odometry, even after looking through the okapilib source code (I am hoping I am wrong about okapi not having arc-based odometry). If it is correct that okapilib uses linear-based odometry, then hard-coding your own arc-based odometry would perform better and deviate less than okapilib.

@sazrocks

Looking at this animation, it seems curious to me that the sideways tracking wheel reads a constant 0 despite the robot yawing during its movements. Can you explain why this is the case?

(I am assuming the 0,0 of the odom unit center is the 0,0 of the robot locally)
Realistically, if the robot were to spin on a dime, the sideways tracker should spin with the robot at a rate based upon it’s offset from the center. However, given the simulation the sideways tracker is not moving which explains the yawning:

updates = [
    (encoder_ticks_to_meters(750, encoder_ticks_per_revolution, wheel_radius), encoder_ticks_to_meters(0, encoder_ticks_per_revolution, wheel_radius), np.pi/4), # forward tracker sensor reading of 1, yaw to pi/4
    (encoder_ticks_to_meters(1500, encoder_ticks_per_revolution, wheel_radius), encoder_ticks_to_meters(0, encoder_ticks_per_revolution, wheel_radius), 0), # forward sensor reading of 2, yaw of 0
    (encoder_ticks_to_meters(750, encoder_ticks_per_revolution, wheel_radius), encoder_ticks_to_meters(0, encoder_ticks_per_revolution, wheel_radius), 0) # forward sensor reading of 1, yaw of pi/4 <- notice the robot goes in reverse
]

If I were to change the forward tracker and sideways tracker position to be 0 and 0 (I guess if your tracker is a mouse with a balled tracker for local x and y?):

odom.set_physical_distances(inches_to_meters(0),inches_to_meters(0)) # (forward tracker position [+right, meters], sideways tracker position [+forward, meters])

You can see the robot no longer yawns

Yet again, the yawning is intended behavior because the sideways tracker will move when the robot is turning. If you apply the same algorithm on a robot, the yawning would not appear because it correctly applies the deltas accordingly to the sideways tracker.

[2775Josh]

@anon4126930

The following illustration shows the center of the odometry unit. The odom unit center is the virtual intercept of the perpendicular faces of the odometry trackers. The measurements is from the center of the odom unit to the designated tracker distances.

This is misleading. If you’ve read the Pilons doc you know that this point is basically meaningless. Instead you can just pick your tracking center first, measure to each tracker, input those measurements into the code, and call it a day.

[anon4126930]

@2775Josh

This is misleading. If you’ve read the Pilons doc you know that this point is basically meaningless. Instead you can just pick your tracking center first, measure to each tracker, input those measurements into the code, and call it a day.

Honestly, I did read the Pilon’s document but I was unable to fully understand it. One of the common problems Is trying to accurately provide a degree of explicitness so that it is easier to digest. I have created an example image below, trying to understand what you’re telling me. The measurements are made up but are simply meant to exist to provide context. Would this diagram be correct, such that if inputted into the JAR-Template or any other template it would work no problem?

[2775Josh]

Yes, that’s exactly right.