Odometry

Odometry is a method used by robots to track their position and orientation relative to a starting point by using data from sensors such as encoders or tracking wheels. This technique is essential for autonomous navigation, as it allows a robot to estimate its location accurately in real time, without relying on external reference points such as GPS.

In Illini VEX Robotics, we implemented an odometry system that tracks the robot’s position and orientation using wheel encoders and differential drive kinematics. This odometry system is crucial for tasks like path following, object placement, and precise navigation on the field.

Asynchronous Operation

Our odometry system runs asynchronously to continuously update the robot’s position in the background while the robot performs other tasks. This allows the robot to track its position without interrupting the main control loop. By running the odometry updates in parallel, we ensure that the robot can react to changes in its environment and maintain accurate localization, even during high-speed maneuvers.

C++ Implementation

Our system uses a differential drive model, where the robot’s position is updated based on the encoder values of the left and right wheels. By calculating the distance each wheel has traveled and the change in orientation, we estimate the robot’s movement along a straight line or an arc.

Key Concepts

1. Differential Drive Kinematics:
   • The robot uses two wheels that rotate independently to move forward, backward, and rotate.
   • The movement of each wheel is tracked using encoders, and these values are used to calculate the robot’s displacement and change in orientation.
2. Encoder Data:
   • Encoders measure the rotation of the wheels, allowing the system to estimate how far each wheel has traveled.
   • By converting encoder ticks into physical distances, we can update the robot’s position and heading.
3. Position and Orientation:
   • The robot’s position is represented as (x, y) coordinates, and its orientation (heading) is represented by an angle (θ).
   • The odometry system continuously updates these values based on encoder data.

Basic C++ Implementation

#include "../../include/common/odometry.h"

// Initializes the odometry system with given encoder distances
Odometry::Odometry(double wheelBase, double wheelRadius, double encoderResolution) {
    this->wheelBase = wheelBase;  // Distance between the left and right wheels
    this->wheelRadius = wheelRadius;  // Radius of the wheels
    this->encoderResolution = encoderResolution;  // Number of encoder ticks per revolution
    this->x = 0;  // Initial x position
    this->y = 0;  // Initial y position
    this->theta = 0;  // Initial heading (orientation)
}

// Asynchronous update function to track position based on encoder data
void Odometry::update(double leftEncoderTicks, double rightEncoderTicks) {
    // Calculate the distance each wheel has traveled
    double leftDistance = (leftEncoderTicks / encoderResolution) * (2 * M_PI * wheelRadius);
    double rightDistance = (rightEncoderTicks / encoderResolution) * (2 * M_PI * wheelRadius);
    
    // Calculate the robot's change in position and orientation
    double deltaDistance = (leftDistance + rightDistance) / 2;  // Average distance moved
    double deltaTheta = (rightDistance - leftDistance) / wheelBase;  // Change in heading

    // Update the robot's position and orientation
    x += deltaDistance * cos(theta);
    y += deltaDistance * sin(theta);
    theta += deltaTheta;

    // Normalize the angle to be within -π to π
    if (theta > M_PI) {
        theta -= 2 * M_PI;
    } else if (theta < -M_PI) {
        theta += 2 * M_PI;
    }
}

// Returns the current position and orientation (x, y, theta)
void Odometry::getPosition(double &xOut, double &yOut, double &thetaOut) {
    xOut = x;
    yOut = y;
    thetaOut = theta;
}

void Odometry::init() {
    if (trackingTask == nullptr) {
        trackingTask = new pros::Task {[=] {
            while (true) {
                update();
                pros::delay(10);
            }
        }};
    }
}

Explanation of the Code

1. Initialization (Odometry::Odometry):
   • The constructor initializes the odometry system with the robot’s wheelbase (distance between the left and right wheels), wheel radius, and encoder resolution (the number of ticks per wheel revolution).
   • The initial robot position and heading (x, y, θ) are set to zero.
2. Asynchronous Update Function (Odometry::update):
   • The update() function calculates the distance traveled by each wheel based on the encoder ticks, then computes the average distance traveled (deltaDistance) and the change in orientation (deltaTheta).
   • The robot’s position (x, y) and orientation (θ) are updated based on the movement and rotation calculated from the encoder data.
   • The angle θ is normalized to keep it within the range of -π to π.
3. Get Position Function (Odometry::getPosition):
   • This function returns the current position and orientation (x, y, θ) of the robot, allowing other parts of the system to access this data for navigation or path-following purposes.

Applications for Illini VEX Robotics

In Illini VEX Robotics, odometry is a crucial part of the autonomous system. It allows the robot to:

• Navigate the Field: The robot’s position is tracked in real-time, enabling it to follow a predefined path, reach specific locations, and complete autonomous tasks with high precision.
• Path Following: Odometry helps the robot maintain an accurate trajectory during complex movements, such as navigating through tight spaces or performing complex maneuvers.
• Object Manipulation: Accurate localization allows the robot to position itself precisely to pick up or place game objects in specific areas of the field.

Advantages of Using Odometry

• Independent of External References: Odometry allows the robot to operate independently of external sensors like GPS or cameras, making it reliable for indoor environments like a VEX competition field.
• Real-Time Localization: The robot’s position is continuously updated, providing real-time feedback that is essential for adjusting movements and ensuring precision.
• Cost-Effective: Wheel encoders and basic sensors are relatively inexpensive, making odometry a cost-effective solution for VEX robots.

Limitations

• Drift Over Time: Over long distances, small errors in the encoder readings can accumulate, causing the robot to drift from its actual position.
• Wheel Slippage: If the wheels slip or lose traction (e.g., during high-speed turns or on slippery surfaces), the odometry system may provide inaccurate position estimates.
• Lack of Global Reference: Odometry provides only relative localization. Without external references, it cannot give an absolute position on the field.

Enhancements

• Sensor Fusion: Combining odometry with other sensors like IMUs (Inertial Measurement Units), gyros, or vision systems can significantly improve the accuracy of the robot’s position estimate.
• Correction Algorithms: Periodically correcting the position using known landmarks, GPS, or vision-based feedback can help reduce drift and improve long-term accuracy.

Conclusion

Odometry is an essential technique for real-time robot localization, enabling autonomous robots to track their position and orientation on the field. In Illini VEX Robotics, we use odometry to enhance the robot’s ability to navigate autonomously, follow precise paths, and perform tasks like object manipulation. By integrating wheel encoder data with differential drive kinematics, we can continuously update the robot’s position and ensure accurate execution of autonomous routines.

By running the odometry system asynchronously, we can continuously update the robot’s position without interrupting other tasks, ensuring high performance and responsiveness.

For more information on odometry, check out the Purdue SIGBots Wiki on Odometry.