基于角速度的两轮机器人里程计计算及自定位编程问询

Got it, let's walk through exactly how to build out your odometry system using the angular velocity code you already have. This is standard for differential-drive robots (which I assume you're working with) and relies entirely on internal wheel speed data—no external cameras or sensors needed.

First, Let's Fill in the Missing Pieces #

Your current code tracks the robot's heading with angular velocity, but we need to tie that to linear motion to calculate position. Here's the core logic, broken down step by step:

1. Define Fixed Robot Parameters

First, you need two accurate physical measurements (small errors here will compound over time):

• wheelRadius: Radius of each drive wheel (e.g., 0.05 meters)
• wheelBase: Distance between the centers of the two drive wheels (e.g., 0.2 meters)

2. Track Time Intervals (Critical!)

Your line Angle = Angle + omega is missing a key component: time. Angular velocity (omega) is in radians per second, so you need to multiply by the time elapsed since the last calculation (deltaTime) to get the actual change in heading (dTheta). This deltaTime should be the fixed interval of your control loop (e.g., 0.1 seconds for a 10Hz loop).

3. Calculate Linear Velocity

The robot's forward linear velocity (v) is the average of the two wheels' linear velocities:

v = (rightSpeed + leftSpeed) * wheelRadius / 2

• rightSpeed/leftSpeed: Angular velocity of each wheel (from encoders, in radians per second)
• Multiply by wheelRadius to convert wheel angular speed to linear speed, then average to get the robot's forward speed.

4. Calculate Position Change

We can use a single accurate formula for both straight-line motion and turning (avoids approximation errors during turns):

For Turning (omega ≠ 0):

When the robot turns, it follows a circular path. The radius of this path is R = v / omega. The position change over deltaTime uses trigonometry for circular motion:

dx = R * (sin(currentAngle) - sin(currentAngle - dTheta))
dy = -R * (cos(currentAngle) - cos(currentAngle - dTheta))

For Straight Motion (omega ≈ 0):

To avoid division by zero when omega is very small, fall back to a linear approximation:

dx = v * deltaTime * cos(currentAngle)
dy = v * deltaTime * sin(currentAngle)

5. Update Position and Heading

Add the calculated dx/dy to your current coordinates, and update the heading with dTheta.

Full Code Example #

Here's how this all comes together in practice (using Python as an example):

import math

# Initialization
initX = 0.0
initY = 0.0
currentAngle = 0.0  # Start facing along the X-axis (0 radians)
wheelRadius = 0.05  # Replace with your wheel's actual radius
wheelBase = 0.2     # Replace with your actual wheel base distance
deltaTime = 0.1     # Control loop interval (e.g., 100ms)

currentX = initX
currentY = initY

# Main control loop (run this repeatedly)
while True:
    # Get real-time wheel speeds from encoders (in radians per second)
    rightSpeed = get_right_wheel_speed()  # Replace with your sensor function
    leftSpeed = get_left_wheel_speed()    # Replace with your sensor function

    # Calculate angular velocity and heading change
    omega = (rightSpeed - leftSpeed) * (wheelRadius / wheelBase)
    dTheta = omega * deltaTime
    currentAngle += dTheta

    # Calculate forward linear velocity
    v = (rightSpeed + leftSpeed) * wheelRadius / 2

    # Calculate position change
    if abs(omega) < 1e-6:
        # Approximate straight-line motion
        dx = v * deltaTime * math.cos(currentAngle)
        dy = v * deltaTime * math.sin(currentAngle)
    else:
        # Accurate circular motion calculation
        R = v / omega
        dx = R * (math.sin(currentAngle) - math.sin(currentAngle - dTheta))
        dy = -R * (math.cos(currentAngle) - math.cos(currentAngle - dTheta))

    # Update current position
    currentX += dx
    currentY += dy

    # Optional: Print or use currentX/currentY for navigation/obstacle avoidance
    print(f"Current Position: ({currentX:.2f}, {currentY:.2f}), Heading: {currentAngle:.2f} rad")

Key Notes to Avoid Errors #

• Unit Consistency: Keep all units the same (e.g., meters for distance, seconds for time, radians for angles). Mixing units will break your calculations.
• Encoder Accuracy: Odometry relies entirely on wheel encoder data—calibrate your encoders correctly, as slippage or encoder drift will cause position errors over time.
• Loop Timing: Ensure deltaTime is as consistent as possible. Use a timer in your control loop to measure actual elapsed time instead of hardcoding if your loop might vary.

内容的提问来源于stack exchange，提问作者CompSci