Mecanum Wheel Odometry Team 1768. Mecanum Wheels ...
-
Upload
raymond-campbell -
Category
Documents
-
view
251 -
download
13
Transcript of 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