mecanum wheel odometry team 1768. mecanum wheels ...

Mecanum Wheel Odometry

Team 1768

Mecanum Wheels

http://cdn.intechopen.com/pdfs/465/InTech-Omnidirectional_mobile_robot_design_and_implementation.pdf

Kinematics

http://cdn.intechopen.com/pdfs/465/InTech-Omnidirectional_mobile_robot_design_and_implementation.pdf

Robot’s Coordinate System Changes

Y

X

YX

Y

X

Y

X

Coordinate System Rotation

http://en.wikibooks.org/wiki/Robotics_Kinematics_and_Dynamics/Description_of_Position_and_Orientation

Rotation Matrix

http://www.google.com/imgres?imgurl=http://upload.wikimedia.org/math/2/8/5/2851c9dc2031127e6dacfb84b96446d8.png&imgrefurl=http://en.wikipedia.org/wiki/Rotation_matrix&h=285&w=232&sz=5&tbnid=PYsJd3YnjdzOBM:&tbnh=90&tbnw=73&zoom=1&usg=__IZuCb9VKTnQwLPoCL0Z

Field Centric Same Thing

• // if "theta" is measured CLOCKWISE from the zero reference:

• forward_field = forward*cos(theta) + right*sin(theta); • right_field = -forward*sin(theta) + right*cos(theta); • // if "theta" is measured COUNTER-CLOCKWISE from

the zero reference: • forward_field = forward*cos(theta) - right*sin(theta);

right_field = forward*sin(theta) + right*cos(theta); • http://www.chiefdelphi.com/media/papers/2390

Changing Coordinate System

• Sample often• Compute the delta from last reading• Resolve to field coordinates• Sum the changes

Robot Position and Rotation// Compute the change in position of each wheel in inchesdeltaFrontRight = (encoderFrontRight - lastEncoderFrontRight) * ticks_to_inches;deltaBackRight = (encoderBackRight - lastEncoderBackRight) * ticks_to_inches;deltaFrontLeft = (encoderFrontLeft - lastEncoderFrontLeft) * ticks_to_inches;deltaBackLeft = (encoderBackLeft - lastEncoderBackLeft) * ticks_to_inches;

// Save the previous encoder values to compute deltalastEncoderFrontRight = encoderFrontRight;lastEncoderBackRight = encoderBackRight;lastEncoderFrontLeft = encoderFrontLeft;lastEncoderBackLeft = encoderBackLeft;

// Convert wheel position change to robot position changedelta_y_r = ( deltaFrontRight + deltaFrontLeft + deltaBackLeft + deltaBackRight)/4.0;delta_x_r = (-deltaFrontRight + deltaFrontLeft - deltaBackLeft + deltaBackRight)/4.0;delta_theta_r = ( deltaFrontRight - deltaFrontLeft - deltaBackLeft + deltaBackRight)/(4.0*lr);

// Convert from robot reference frame to field reference framedelta_x = (delta_x_r * cos(-theta) - delta_y_r * sin(-theta));delta_y = (delta_x_r * sin(-theta) + delta_y_r * cos(-theta));delta_theta = delta_theta_r;

// Sum delta changes to get current position and rotationx += delta_x;y += delta_y;theta += delta_theta;

Using Position info

Y

X

Θ

d

Given: known start point & known destinationRobot can get to goal

Goal can be determined by known field location or distance and angle information from camera or other sensors

Can use PID to get there quick

Proportional part of PID

• errx = (destx – currentx)

• erry = (desty – currenty)

• errΘ = (destΘ – currentΘ)

• drivex = kpx * errx

• Cap drivex at some max value• Loop until errors are less than epsilon

err_y = (dest_y - y);err_x = (dest_x - x);err_a = (dest_a - theta);if ((abs(err_y) < eps) && (abs(err_x) < eps) && (abs(err_a) < eps_a))

drive = 0

if (drive) { if (abs(Kp_y * err_y) > maxForward) { forward_f = (err_y > 0) ? maxForward : -maxForward; } else { forward_f = Kp_y * err_y; }

if (abs(Kp_x * err_x) > maxSide) { right_f = (err_y > 0) ? maxSide : -maxSide; } else { right_f = Kp_x * err_x; }

if (abs(Kp_a * err_a) > maxRotate) { rotate = (err_a > 0) ? maxRotate : -maxRotate; } else { rotate = Kp_a * err_a; }

// Convert to robot coordinates right = right_f * cos(theta) - forward_f * sin(theta); forward = right_f * sin(theta) + forward_f * cos(theta);}

// forward, right, & rotate drive the motors

Mecanum

• Lots of Mecanum info: http://www.chiefdelphi.com/media/papers/2390

• Good Mecanum Paper:• http://www.chiefdelphi.com/media/papers/1

836