Autonomous Navigation Autonomous Navigation Workshop Workshop

Simona DoboliSimona DoboliAssistant ProfessorAssistant Professor

Computer Science DepartmentComputer Science DepartmentHosftra UniversityHosftra University

Email: Email: Simona.Doboli@hofstra.eduSimona.Doboli@hofstra.edu

January 14th, 2006Hauppage High School

SPBLI - FIRST

Mark McLeodMark McLeodProgramming CoachProgramming Coach

Hauppauge Team 358Hauppauge Team 358Northrop Grumman Corp.Northrop Grumman Corp.Mark.McLeod@ngc.comMark.McLeod@ngc.com

AgendaAgenda

General PointsGeneral Points Dead ReckoningDead Reckoning Sensor-Based NavigationSensor-Based Navigation DemonstrationsDemonstrations

– Lego – Rotation sensors / Light SensorLego – Rotation sensors / Light Sensor– Vex – Ultrasonic sensorVex – Ultrasonic sensor– FRC – Encoders / GyroscopeFRC – Encoders / Gyroscope– CMUCam2CMUCam2

ConclusionsConclusions PlaytimePlaytime

General PointsGeneral Points

Balance your drivetrain mechanically or Balance your drivetrain mechanically or through software, e.g.,through software, e.g., #define BALANCE 16#define BALANCE 16 pwm02 -= BALANCE * (pwm02 – 127) / 127;pwm02 -= BALANCE * (pwm02 – 127) / 127;

TechniqueTechnique– State MachineState Machine– Function DrivenFunction Driven– Script DrivenScript Driven

Multiple fallback implementations for Multiple fallback implementations for when sensors or appendages breakwhen sensors or appendages break

Pay close attention to LIMITS when Pay close attention to LIMITS when designing for sensorsdesigning for sensors

AutonomousAutonomousDead ReckoningDead Reckoning Driving by timer without sensorsDriving by timer without sensors Simplest form of autonomous mode Simplest form of autonomous mode

and a backup for sensor failureand a backup for sensor failure Good for short duration movements Good for short duration movements

such as driving to an approximate spot such as driving to an approximate spot on the fieldon the field

Unable to correct itself if disrupted Unable to correct itself if disrupted Significant changes to robot Significant changes to robot

mechanical system or battery power mechanical system or battery power can disrupt operations and require can disrupt operations and require program to be modifiedprogram to be modified

Autonomous Autonomous NavigationNavigation

Autonomous Autonomous NavigationNavigation

Motors

Sensors

Feedback

Sensors FIRST 2006Sensors FIRST 2006

Yaw Rate Gyro (ADXRS150)Yaw Rate Gyro (ADXRS150)– Measures angular rate (150Measures angular rate (150oo/sec) along /sec) along

the Z axis.the Z axis.– Supply voltage 5V.Supply voltage 5V.– Output – analog.Output – analog.– Applications: Stability control, Applications: Stability control,

guidance.guidance.

Sensors FIRST 2006Sensors FIRST 2006

What can you do with the gyro sensor What can you do with the gyro sensor in autonomous mode?in autonomous mode?

– Rotate robot left or right while the gyro Rotate robot left or right while the gyro reading is less than Xreading is less than Xoo..

– If robot rotates too fast (the rate of If robot rotates too fast (the rate of change of the gyro sensor) change of the gyro sensor) reduce reduce motor speed (good in any mode).motor speed (good in any mode).

– Drive straight.Drive straight.

Sensors FIRST 2006Sensors FIRST 2006

Dual Axis Accelerometer (ADXL Dual Axis Accelerometer (ADXL 311)311)– Measures dynamic and static Measures dynamic and static

acceleration on both X and Y axis.acceleration on both X and Y axis.– Applications: tilt or motion Applications: tilt or motion

sensor.sensor.– Supply voltage 5 V.Supply voltage 5 V.– Output – analog. Bandwidth 3KHZ. Output – analog. Bandwidth 3KHZ. – Sampling frequency > 6 KHZ.Sampling frequency > 6 KHZ.

Sensors FIRST 2006Sensors FIRST 2006

What can you do with the accelerometer?What can you do with the accelerometer?– Am I standing straight? Am I standing straight?

Measure pitch and roll in degrees.Measure pitch and roll in degrees. Pitch = asin(Ax/1g); Roll asin(Ay/1g).Pitch = asin(Ax/1g); Roll asin(Ay/1g).

– Orient an arm.Orient an arm.– Collision detection.Collision detection.

Sensors FIRST 2006Sensors FIRST 2006

What can you do with the accelerometer?What can you do with the accelerometer?– Move robot d feet with a desired speed. Move robot d feet with a desired speed. – Need to accelerate the first part and Need to accelerate the first part and

decelerate the last part.decelerate the last part.

Distance

Speed a a

Sensors FIRST 2006Sensors FIRST 2006

Gear Tooth Sensors (2)Gear Tooth Sensors (2)– ATS651 speed and direction sensor.ATS651 speed and direction sensor.– Generates a pulse when a gear tooth Generates a pulse when a gear tooth

is detected.is detected.– Speed of the gear: pulse rate.Speed of the gear: pulse rate.– Direction of the gear: pulse width.Direction of the gear: pulse width.

Forward (from pin 4 to pin 1): 45 us width.Forward (from pin 4 to pin 1): 45 us width. Reverse (from pin 1 to pin 4): 90 us width.Reverse (from pin 1 to pin 4): 90 us width.

– Supply voltage: 5 V.Supply voltage: 5 V.

Sensors FIRST 2006Sensors FIRST 2006

What can you do with the gear What can you do with the gear tooth sensors?tooth sensors?– Control the speed of the wheel.Control the speed of the wheel.– Adjust for relative speed difference Adjust for relative speed difference

between wheels.between wheels.– AUTONOMOUS MODE:AUTONOMOUS MODE:

Move d feet distance at an angle of xMove d feet distance at an angle of xoo..

Sensors FIRST 2006Sensors FIRST 2006

CMUCam2 CameraCMUCam2 Camera– RC default camera code-Kevin WatsonRC default camera code-Kevin Watson– Labview / camera driven servosLabview / camera driven servos

Sensors FIRST 2006Sensors FIRST 2006

What can you do with the camera?What can you do with the camera?– Track colors, e.g., illuminated targetTrack colors, e.g., illuminated target– Locate the high scoring goalLocate the high scoring goal– Orient the robot or a turret to the high Orient the robot or a turret to the high

goalgoal Heading & angle or distanceHeading & angle or distance

– Drive to the high goal (PID)Drive to the high goal (PID)– Fire ControlFire Control

Other Sensors – Other Sensors – Proximity sensorsProximity sensors

Is something close to me that I will hit soon? Is something close to me that I will hit soon?

Proximity Sensors - Proximity Sensors - SonarsSonars

Emit a sound and measure the time Emit a sound and measure the time of flight of flight distance. distance.

Ranges: 1 – 30 feet, with a field of Ranges: 1 – 30 feet, with a field of view of 30view of 30oo..

Proximity Sensors – IR Proximity Sensors – IR SensorsSensors

Emit modulated infrared (IR) energy and Emit modulated infrared (IR) energy and measure amount of (IR) returned.measure amount of (IR) returned.

Range: inches to several feet.Range: inches to several feet. Led IR sensors have ranges of 3-5 inches. Led IR sensors have ranges of 3-5 inches.

LEGO sensors – Touch LEGO sensors – Touch SensorSensor

If pressure is applied to it, an If pressure is applied to it, an electrical signal is generated.electrical signal is generated.

What can you do with it? What can you do with it? – I already bumped into something. I better I already bumped into something. I better

get back, or move around it.get back, or move around it.

Light SensorLight Sensor

Red LED emits light and a Red LED emits light and a phototransistor measures the phototransistor measures the incoming light. incoming light.

What can you do with a light sensor?What can you do with a light sensor?– Recognize objects of certain colors.Recognize objects of certain colors.– Follow a line.Follow a line.

Problems: Ambient light and battery Problems: Ambient light and battery level affect sensor readings. level affect sensor readings.

Rotation SensorRotation Sensor

Measures the rotation angle Measures the rotation angle relative to a reference position. relative to a reference position.

LEGO rotation sensor: Measures LEGO rotation sensor: Measures increments of 22.5increments of 22.5oo..

What can you do with rotation What can you do with rotation sensors?sensors?– Same as the gear tooth sensor.Same as the gear tooth sensor.

Case Study: A LEGO Case Study: A LEGO robotrobot

Rotation Sensors

Light Sensor

DC Motors

Rotation SensorsRotation Sensors

Tire Gear (24)

Motor Gear (16)

Transmission Gear (40)Sensor

Gear (8)

1 rot. Tire Gear 1 rot. Tire Gear 3 rot. Sensor Gear 3 rot. Sensor Gear X degrees Tire Gear X degrees Tire Gear 3*X degrees Sensor 3*X degrees Sensor

Rotation SensorsRotation Sensors

Sensor reading – multiple of 22.5Sensor reading – multiple of 22.5oo.. Calibrate distance: Calibrate distance:

– Initialize rotation sensors to 0.Initialize rotation sensors to 0.– Move robot.Move robot.– Until tire wheel moves one full rotation. Until tire wheel moves one full rotation. – Stop motors. Stop motors. – Measure distance. Measure distance.

Simpler Simpler measure wheel diameter. measure wheel diameter.

Move AlgorithmMove Algorithm

// moves fwd (dist >0) or rev (dist < 0) dist cm// moves fwd (dist >0) or rev (dist < 0) dist cmvoid move(int dist)void move(int dist){{ int degreesFwd = 3 * abs(dist)*360/DIAMETER_FULL_SPEED; int degreesFwd = 3 * abs(dist)*360/DIAMETER_FULL_SPEED; int n = degreesFwd *10/225; // desired increment of rot. int n = degreesFwd *10/225; // desired increment of rot.

sensorsensor if (dist > 0)if (dist > 0) OnFwd(LEFT+RIGHT);OnFwd(LEFT+RIGHT); else else OnRev(LEFT+RIGHT);OnRev(LEFT+RIGHT); int last_rot = ROT_LEFT;int last_rot = ROT_LEFT; while (abs(ROT_LEFT - last_rot) < n);while (abs(ROT_LEFT - last_rot) < n); Off(RIGHT+LEFT);Off(RIGHT+LEFT);}}

Issues with MoveIssues with Move

Problems with distance calibration:Problems with distance calibration:– Should account for motors’ inertia at Should account for motors’ inertia at

different speeds (stopping distance).different speeds (stopping distance).

N teeth tire wheel

N – p = full power, p decrease power slowly

distance

Or, measure distance of one wheel Or, measure distance of one wheel rotation at different speeds.rotation at different speeds.

Rotation Rotation

Calibrate rotation angle.Calibrate rotation angle.– Rotate in both directions a set of Rotate in both directions a set of

angles on the rotation sensor, angles on the rotation sensor, measure robot angles.measure robot angles.

– Fit a line then calculate tangent.Fit a line then calculate tangent.– You should account for the speed of You should account for the speed of

the motor too (stopping angle).the motor too (stopping angle).

Rotation AlgorithmRotation Algorithm

// degrees may be only positive and greater than or equal // degrees may be only positive and greater than or equal 4 4

// postcondition - both motors are on forward// postcondition - both motors are on forward

void rotateLeft(int degrees)void rotateLeft(int degrees){{ int n = (degrees *4)/3; // increments on rotation sensorint n = (degrees *4)/3; // increments on rotation sensor OnRev(LEFT);OnRev(LEFT); OnFwd(RIGHT);OnFwd(RIGHT);

int last_rot = ROT_LEFT;int last_rot = ROT_LEFT; while(abs(ROT_LEFT - last_rot) < n);while(abs(ROT_LEFT - last_rot) < n);

OnFwd(RIGHT+LEFT);OnFwd(RIGHT+LEFT);}}

Follow a black Follow a black lineline

Follow a black line (stay on the edge).Follow a black line (stay on the edge). Error(n) = Edge – LIGHT(n);Error(n) = Edge – LIGHT(n);

– Positive – robot on black; Negative – on white.Positive – robot on black; Negative – on white.

Action: Action: – Rotate left deltaAngle degrees if Error > 0Rotate left deltaAngle degrees if Error > 0– Rotate right deltaAngle degrees if Error < 0Rotate right deltaAngle degrees if Error < 0

P Controller: P Controller: deltaAngle(n) = Kp * Error(n)deltaAngle(n) = Kp * Error(n)

P P Algorithm Algorithm error = edge – LIGHT; // read light sensorerror = edge – LIGHT; // read light sensorwhile (true)while (true) {{

deltaAngle = Kp*error; deltaAngle = Kp*error;

if (abs(deltaAngle) > maxDeltaAngle) if (abs(deltaAngle) > maxDeltaAngle) deltaAngle = sign(deltaAngle) * deltaAngle = sign(deltaAngle) *

maxDeltaAngle;maxDeltaAngle;

if (deltaAngle >= 4) // insensitive area if (deltaAngle >= 4) // insensitive area rotateLeft(deltaAngle);rotateLeft(deltaAngle); else if (deltaAngle <= -4)else if (deltaAngle <= -4) rotateRight(deltaAngle);rotateRight(deltaAngle);

error = edge - LIGHT; // read light sensorerror = edge - LIGHT; // read light sensor }}

PD PD ControllerController

Error(n) = Edge – LIGHT(n);Error(n) = Edge – LIGHT(n); LastError = Error(n-1)LastError = Error(n-1) DeltaError(n) = Error(n) – Error(n-1);DeltaError(n) = Error(n) – Error(n-1); PD ControllerPD Controller

deltaAngle(n) = Kp * Error(n) + deltaAngle(n) = Kp * Error(n) +

Kd * DeltaError(n)Kd * DeltaError(n)

PD AlgorithmPD Algorithm

deltaAngle(n) = Kp * Error(n) + deltaAngle(n) = Kp * Error(n) + Kd * DeltaError(n)Kd * DeltaError(n)

Effect of DeltaError:Effect of DeltaError: – DeltaAngle is proportional with the rate of DeltaAngle is proportional with the rate of

change in error.change in error.– Derivative component amplifies noise. Derivative component amplifies noise.

(limit its value).(limit its value).– Try negative values of Kd. Try negative values of Kd.

Better than P controller?Better than P controller?– Faster control: Reacts faster to abrupt changes in Faster control: Reacts faster to abrupt changes in

error. error.

PD AlgorithmPD Algorithm

while(true){while(true){ deltaAngle = Kp*error;deltaAngle = Kp*error; der = Kd*(error-lastError);der = Kd*(error-lastError);

if (abs(der) > maxDerivative)if (abs(der) > maxDerivative) der= sign(der)*maxDerivative;der= sign(der)*maxDerivative;

deltaAngle -= der; deltaAngle -= der;

if (abs(deltaAngle) > if (abs(deltaAngle) > maxDeltaAngle)maxDeltaAngle)

deltaAngle = sign(deltaAngle)* deltaAngle = sign(deltaAngle)* maxDeltaAngle;maxDeltaAngle;

if (deltaAngle >= 4)if (deltaAngle >= 4) rotateLeft(deltaAngle);rotateLeft(deltaAngle);else if (deltaAngle <= -4)else if (deltaAngle <= -4) rotateRight(deltaAngle);rotateRight(deltaAngle);

lastError = error;lastError = error; error = edge - LIGHT;error = edge - LIGHT; }}

PID ControllerPID Controller

deltaAngle = Kp *Error(n) +deltaAngle = Kp *Error(n) +

Ki* Error(i) +Ki* Error(i) +

Kd*(Error(n) – Error(n-1))Kd*(Error(n) – Error(n-1)) Integral component Integral component Corrective action Corrective action

proportional to the amount of proportional to the amount of accumulated error (faster control).accumulated error (faster control).

Limit each P, I, D term and the cumulative Limit each P, I, D term and the cumulative error.error.

i

n

0

PID AlgorithmPID Algorithm

sumError = 0;sumError = 0;while(true){ while(true){ deltaAngle = Kp*error;deltaAngle = Kp*error; der = Kd*(error-lastError);der = Kd*(error-lastError); if (abs(der) > maxDerivative)if (abs(der) > maxDerivative) der = sign(der)* maxDerivative;der = sign(der)* maxDerivative;

deltaAngle -= derivative;deltaAngle -= derivative;

sumError += error;sumError += error; if (abs(sumError) > if (abs(sumError) >

maxSumError)maxSumError) sumError = sign(sumError)*sumError = sign(sumError)* maxSumError;maxSumError; deltaAngle += Ki*sumError;deltaAngle += Ki*sumError;

if (abs(deltaAngle) > maxDeltaAngle) deltaAngle = sign(deltaAngle)*

maxDeltaAngle;

if (deltaAngle >= 4)if (deltaAngle >= 4) rotateLeft(deltaAngle);rotateLeft(deltaAngle);else if (deltaAngle <= -4)else if (deltaAngle <= -4) rotateRight(deltaAngle);rotateRight(deltaAngle);

lastError = error;lastError = error; error = edge - LIGHT;error = edge - LIGHT;

}

Case Study: VexCase Study: Vex

On-BoardControls

UltrasonicRangefinder

IR Rangefinders

LineFollowers

Keep Your Distance Keep Your Distance

Maintains a constant distance from an Maintains a constant distance from an obstacle via ultrasonic & IR obstacle via ultrasonic & IR rangefindersrangefinders

User sets distance via potentiometerUser sets distance via potentiometer P – power to the motors proportional P – power to the motors proportional

to the error in distanceto the error in distance Obstacle must be perpendicular to Obstacle must be perpendicular to

sensor to reflect echosensor to reflect echo

Polaroid 6500

Closed-Loop Feedback Closed-Loop Feedback Ultrasonic Algorithm Ultrasonic Algorithm (P)(P)

Initialize

Send Trigger Pulse

Listen & time echo

Filter echo results Timer / interrupt process

EchoInterruptCompare requested distance to echo

Right Distance?

Yes No

Motor Stop Motor= distance error* KP

* In this example KP=5 distance error=1 to 25

How The Sensor WorksHow The Sensor WorksTimer / Interrupt Timer / Interrupt ProcessProcess

1. Program requests a sonar pulse

2. Pulse is set out

3. Program is told pulse is sent

4. Program is told when echo returns

5. Calculate the time it took

6. Wait before requesting another

For Devantech SRF05 RangefinderSRF05 (1-150”) $25

Closed-Loop FeedbackClosed-Loop FeedbackIR Algorithm (PI)IR Algorithm (PI)

Initialize

Get IR Sensed Distance

Compare to requested distance

Right Distance?

Yes No

Motor Stop Motor= distance error* KP

V = 1/(R + .42)to linearize input

Sharp GP2Y0A02YK (6-60”) $16.50 GP2D120 (.5-30”) $12.50

Case Study: FRCCase Study: FRC

Encoders Gyroscope

Arm TelescopingPotentiometer

Arm AnglePotentiometer

Closed-Loop Feedback Closed-Loop Feedback Gyro-Based Turn (PI)Gyro-Based Turn (PI)

Initialize

Get Gyro Value

GyroRaw+=Gyro - Neutral

Timer

Are we there yet?

Yes No

Motor Stop

Motor= P + I

P=(GyroSum-target)* KP

I=CumError* KIDone

CumError +=GyroSum-target)

GyroSum=GyroRaw/Sample Rate

* In this example KP=6/10 KI=3

CMUCam2CMUCam2Labview InterfaceLabview Interface

• Servo Orientation• Camera Focus• Load Configuration• Tracking• Min/Max Servo Positions

CMUCam2CMUCam2Kevin Watson Camera Kevin Watson Camera CodeCode Just does the camera trackingJust does the camera tracking Load initial calibration dataLoad initial calibration data Tracking.h SettingsTracking.h Settings

– PAN/TILT GainsPAN/TILT Gains– Reversing ServosReversing Servos

Camera/tracking menus through HyperterminalCamera/tracking menus through Hyperterminal Camera settings stored permanently on the RCCamera settings stored permanently on the RC For PID use PAN_SERVO & TILT_SERVOFor PID use PAN_SERVO & TILT_SERVO Confidence use pan_error and tilt_errorConfidence use pan_error and tilt_error

ConclusionsConclusions

Autonomous mode: Autonomous mode: – Fixed sequence of actions.Fixed sequence of actions.

Advantage: simple and fast; but Advantage: simple and fast; but calibrate it to actual conditions.calibrate it to actual conditions.

– More flexible solutions, but maybe More flexible solutions, but maybe too slow for 10 sec.too slow for 10 sec. Use camera to search for objects of Use camera to search for objects of

certain color. certain color. Use infrared sensors to avoid obstacles Use infrared sensors to avoid obstacles

or move along the walls.or move along the walls.

ConclusionsConclusions

P, PD, PID controllers.P, PD, PID controllers.– Try P first, if happy stick with it.Try P first, if happy stick with it.– For faster reaction try PD. For faster reaction try PD. – If error is too great, try PI.If error is too great, try PI.– For both fast reaction and error, try For both fast reaction and error, try

PIDPID

ConclusionsConclusions

Situations where you can use P,PD,PID: Situations where you can use P,PD,PID: – Drive straight (correct small errors in the Drive straight (correct small errors in the

relative speed of the two motors using the relative speed of the two motors using the gyro sensor too).gyro sensor too).

– Turn to a precise heading.Turn to a precise heading.– Drive an exact distance.Drive an exact distance.– Follow a wall.Follow a wall.– Home on a beacon or a retroreflective targetHome on a beacon or a retroreflective target– Follow an object of a certain color (using the Follow an object of a certain color (using the

camera).camera).

Sensor SuppliersSensor Suppliers

Acroname.com (easiest to window Acroname.com (easiest to window shop)shop)

Digikey.comDigikey.com Newarkinone.comNewarkinone.com Mouser.comMouser.com Bannerengineering.comBannerengineering.com Senscomp.comSenscomp.com Alliedelec.comAlliedelec.com

Presentation slides at: Presentation slides at:

www.cs.hofstra.edu/~sdoboliwww.cs.hofstra.edu/~sdoboli

oror

Team358.orgTeam358.org

Questions/Help please email us. Questions/Help please email us.

Simona.Doboli@hofstra.eduSimona.Doboli@hofstra.edu

Mark.McLeod@ngc.comMark.McLeod@ngc.com