Autonomous Navigation Workshop

1
Autonomous Navigation Workshop
January 14th, 2006 Hauppage High School SPBLI -
FIRST

• Simona Doboli
• Assistant Professor
• Computer Science Department
• Hosftra University
• Email Simona.Doboli_at_hofstra.edu

Mark McLeod Programming Coach Hauppauge Team
358 Northrop Grumman Corp. Mark.McLeod_at_ngc.com
2
Agenda

• General Points
• Dead Reckoning
• Sensor-Based Navigation
• Demonstrations
• Lego Rotation sensors / Light Sensor
• Vex Ultrasonic sensor
• FRC Encoders / Gyroscope
• CMUCam2
• Conclusions
• Playtime

3
General Points

• Balance your drivetrain mechanically or through
  software, e.g.,
• define BALANCE 16
• pwm02 - BALANCE (pwm02 127) / 127
• Technique
• State Machine
• Function Driven
• Script Driven
• Multiple fallback implementations for when
  sensors or appendages break
• Pay close attention to LIMITS when designing for
  sensors

4
AutonomousDead Reckoning

• Driving by timer without sensors
• Simplest form of autonomous mode and a backup for
  sensor failure
• Good for short duration movements such as driving
  to an approximate spot on the field
• Unable to correct itself if disrupted
• Significant changes to robot mechanical system or
  battery power can disrupt operations and require
  program to be modified

5
Autonomous Navigation
6
Autonomous Navigation
Motors
Sensors
Feedback
7
Sensors FIRST 2006

• Yaw Rate Gyro (ADXRS150)
• Measures angular rate (150o/sec) along the Z
  axis.
• Supply voltage 5V.
• Output analog.
• Applications Stability control, guidance.

8
Sensors FIRST 2006

• What can you do with the gyro sensor in
  autonomous mode?
• Rotate robot left or right while the gyro reading
  is less than Xo.
• If robot rotates too fast (the rate of change of
  the gyro sensor) ? reduce motor speed (good in
  any mode).

9
Sensors FIRST 2006

• Dual Axis Accelerometer (ADXL 311)
• Measures dynamic and static acceleration on both
  X and Y axis.
• Applications tilt or motion sensor.
• Supply voltage 5 V.
• Output analog. Bandwidth 3KHZ.
• Sampling frequency gt 6 KHZ.

10
Sensors FIRST 2006

• What can you do with the accelerometer?
• Am I standing straight?
• Measure pitch and roll in degrees.
• Pitch asin(Ax/1g) Roll asin(Ay/1g).
• Orient an arm.
• Collision detection.

11
Sensors FIRST 2006

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

a
Speed
a
Distance
12
Sensors FIRST 2006

• Gear Tooth Sensors (2)
• ATS651 speed and direction sensor.
• Generates a pulse when a gear tooth is detected.
• Speed of the gear pulse rate.
• Direction of the gear pulse width.
• Forward (from pin 4 to pin 1) 45 us width.
• Reverse (from pin 1 to pin 4) 90 us width.
• Supply voltage 28 V.

13
Sensors FIRST 2006

• What can you do with the gear tooth sensors?
• Control the speed of the wheel.
• Adjust for relative speed difference between
  wheels.
• AUTONOMOUS MODE
• Move d feet distance at an angle of xo.

14
Sensors FIRST 2006

• CMUCam2 Camera
• RC default camera code-Kevin Watson
• Labview / camera driven servos

15
Sensors FIRST 2006

• What can you do with the camera?
• Track colors, e.g., illuminated target
• Locate the high scoring goal
• Orient the robot or a turret to the high goal
• Heading angle or distance
• Drive to the high goal (PID)
• Fire Control

16
Other Sensors Proximity sensors

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

17
Proximity Sensors - Sonars

• Emit a sound and measure the time of flight ?
  distance.
• Ranges 1 30 feet, with a field of view of 30o.

18
Proximity Sensors IR Sensors

• Emit modulated infrared (IR) energy and measure
  amount of (IR) returned.
• Range inches to several feet.
• Led IR sensors have ranges of 3-5 inches.

19
LEGO sensors Touch Sensor

• If pressure is applied to it, an electrical
  signal is generated.
• What can you do with it?
• I already bumped into something. I better get
  back, or move around it.

20
Light Sensor

• Red LED emits light and a phototransistor
  measures the incoming light.
• What can you do with a light sensor?
• Recognize objects of certain colors.
• Follow a line.
• Problems Ambient light and battery level affect
  sensor readings.

21
Rotation Sensor

• Measures the rotation angle relative to a
  reference position.
• LEGO rotation sensor Measures increments of
  22.5o.
• What can you do with rotation sensors?
• Same as the gear tooth sensor.

22
Case Study A LEGO robot
Light Sensor
Rotation Sensors
DC Motors
23
Rotation Sensors

• 1 rot. Tire Gear ? 3 rot. Sensor Gear
• X degrees Tire Gear ? 3X degrees Sensor

24
Rotation Sensors

• Sensor reading multiple of 22.5o.
• Calibrate distance
• Initialize rotation sensors to 0.
• Move robot.
• Until tire wheel moves one full rotation.
• Stop motors.
• Measure distance.
• Simpler ? measure wheel diameter.

25
Move Algorithm

• // moves fwd (dist gt0) or rev (dist lt 0) dist cm
• void move(int dist)
• int degreesFwd 3 abs(dist)360/DIAMETER_F
  ULL_SPEED
• int n degreesFwd 10/225 // desired
  increment of rot. sensor
• if (dist gt 0)
• OnFwd(LEFTRIGHT)
• else
• OnRev(LEFTRIGHT)
• int last_rot ROT_LEFT
• while (abs(ROT_LEFT - last_rot) lt n)
• Off(RIGHTLEFT)

26
Issues with Move

• Problems with distance calibration
• Should account for motors inertia at different
  speeds (stopping distance).

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

27
Rotation

• Calibrate rotation angle.
• Rotate in both directions a set of angles on the
  rotation sensor, measure robot angles.
• Fit a line then calculate tangent.
• You should account for the speed of the motor too
  (stopping angle).

28
Rotation Algorithm

• // degrees may be only positive and greater than
  or equal 4
• // postcondition - both motors are on forward
• void rotateLeft(int degrees)
• int n (degrees 4)/3 // increments on
  rotation sensor
• OnRev(LEFT)
• OnFwd(RIGHT)
• int last_rot ROT_LEFT
• while(abs(ROT_LEFT - last_rot) lt n)
• OnFwd(RIGHTLEFT)

29
Follow a black line

• Follow a black line (stay on the edge).
• Error(n) Edge LIGHT(n)
• Positive robot on black Negative on white.
• Action
• Rotate left deltaAngle degrees if Error gt 0
• Rotate right deltaAngle degrees if Error lt 0
• P Controller
• deltaAngle(n) Kp Error(n)

30
P Algorithm

• error edge LIGHT // read light sensor
• while (true)
• deltaAngle Kperror
• if (abs(deltaAngle) gt maxDeltaAngle)
• deltaAngle sign(deltaAngle) maxDeltaAngle
• if (deltaAngle gt 4) // insensitive area
  
• rotateLeft(deltaAngle)
• else if (deltaAngle lt -4)
• rotateRight(deltaAngle)
• error edge - LIGHT // read light sensor

31
PD Controller

• Error(n) Edge LIGHT(n)
• LastError Error(n-1)
• DeltaError(n) Error(n) Error(n-1)
• PD Controller
• deltaAngle(n) Kp Error(n)
• Kd DeltaError(n)

32
PD Algorithm

• deltaAngle(n) Kp Error(n)
• Kd DeltaError(n)
• Effect of DeltaError
• DeltaAngle is proportional with the rate of
  change in error.
• Derivative component amplifies noise. (limit its
  value).
• Try negative values of Kd.
• Better than P controller?
• Faster control Reacts faster to abrupt changes
  in error.

33
PD Algorithm

• while(true)
• deltaAngle Kperror
• der Kd(error-lastError)
• if (abs(der) gt maxDerivative)
• der sign(der)maxDerivative
• deltaAngle - der
• if (abs(deltaAngle) gt maxDeltaAngle)
• deltaAngle sign(deltaAngle)
• maxDeltaAngle

if (deltaAngle gt 4) rotateLeft(deltaAngle) e
lse if (deltaAngle lt -4) rotateRight(deltaAng
le) lastError error error edge - LIGHT

34
PID Controller

• deltaAngle Kp Error(n)
• Ki Error(i)
• Kd(Error(n) Error(n-1))
• Integral component ? Corrective action
  proportional to the amount of accumulated error
  (faster control).
• Limit each P, I, D term and the cumulative error.

35
PID Algorithm

• sumError 0
• while(true)
• deltaAngle Kperror
• der Kd(error-lastError)
• if (abs(der) gt maxDerivative)
• der sign(der) maxDerivative
• deltaAngle - derivative
• sumError error
• if (abs(sumError) gt maxSumError)
• sumError sign(sumError)
• maxSumError
• deltaAngle KisumError

if (abs(deltaAngle) gt maxDeltaAngle)
deltaAngle sign(deltaAngle)
maxDeltaAngle if (deltaAngle gt 4)
rotateLeft(deltaAngle) else if (deltaAngle lt
-4) rotateRight(deltaAngle) lastError
error error edge - LIGHT
36
Case Study Vex
On-Board Controls
IR Rangefinders
Ultrasonic Rangefinder
Line Followers
37
Keep Your Distance

• Maintains a constant distance from an obstacle
  via ultrasonic IR rangefinders
• User sets distance via potentiometer
• P power to the motors proportional to the error
  in distance
• Obstacle must be perpendicular to sensor to
  reflect echo

Polaroid 6500
38
Closed-Loop Feedback Ultrasonic Algorithm (P)
Initialize
Filter echo results
Timer / interrupt process
Echo Interrupt
Compare requested distance to echo
Send Trigger Pulse
Right Distance ?
Listen time echo
Yes
No
Motor Stop
Motor distance error KP
In this example KP5 distance error1
to 25
39
How The Sensor WorksTimer / Interrupt Process

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 Rangefinder
SRF05 (1-150) 25
40
Closed-Loop FeedbackIR Algorithm (PI)
Initialize
V 1/(R .42) to linearize input
Get IR Sensed Distance
Compare to requested distance
Right Distance ?
Yes
No
Motor Stop
Motor distance error KP
Sharp GP2Y0A02YK (6-60) 16.50 GP2D120
(.5-30) 12.50
41
Case Study FRC
Arm Angle Potentiometer
Arm Telescoping Potentiometer
Gyroscope
Encoders
42
Closed-Loop Feedback Gyro-Based Turn (PI)
Initialize
Timer
Are we there yet ?
Yes
No
Get Gyro Value
GyroSumGyroRaw/Sample Rate
Motor Stop
P(GyroSum-target) KP
GyroRawGyro - Neutral
ICumError KI
Done
CumError GyroSum-target)
In this example KP6/10 KI3
Motor P I
43
CMUCam2Labview Interface

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

44
CMUCam2Kevin Watson Camera Code

• Just does the camera tracking
• Load initial calibration data
• Tracking.h Settings
• PAN/TILT Gains
• Reversing Servos
• Camera/tracking menus through Hyperterminal
• Camera settings stored permanently on the RC
• For PID use PAN_SERVO TILT_SERVO
• Confidence use pan_error and tilt_error

45
Conclusions

• Autonomous mode
• Fixed sequence of actions.
• Advantage simple and fast but calibrate it to
  actual conditions.
• More flexible solutions, but maybe too slow for
  10 sec.
• Use camera to search for objects of certain
  color.
• Use infrared sensors to avoid obstacles or move
  along the walls.

46
Conclusions

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

47
Conclusions

• Situations where you can use P,PD,PID
• Drive straight (correct small errors in the
  relative speed of the two motors using the gyro
  sensor too).
• Turn to a precise heading.
• Drive an exact distance.
• Follow a wall.
• Home on a beacon or a retroreflective target
• Follow an object of a certain color (using the
  camera).

48
Sensor Suppliers

• Acroname.com (easiest to window shop)
• Digikey.com
• Newarkinone.com
• Mouser.com
• Bannerengineering.com
• Senscomp.com
• Alliedelec.com

49

• Presentation slides at
• www.cs.hofstra.edu/sdoboli
• or
• Team358.org
• Questions/Help please email us.
• Simona.Doboli_at_hofstra.edu
• Mark.McLeod_at_ngc.com