Robot Building Lab: Localization DEMO  Show us your working differential drive robot  Start it running a fixed pattern that includes –Forward –Backward –Left –Right –Straight –Curved  Lab 6: Localization, virtual path following, and proportional control

Robot Building Lab: Localization Lab 6 Localization, Virtual Path Following, and Proportional Control es/robotlab/labs/lab06.pdf es/robotlab/labs/lab06.pdf

Robot Building Lab: Localization Today  Odometry overview  Encoder software  Kinematics, Inverse Kinematics: Odometry calculations  Closed-loop control using encoders  Proportional derivative control

Robot Building Lab: Localization Dead Reckoning (deductive reckoning)  Integration of incremental motion over time  Given known start position/orientation (pose)  Given relationship between motor commands and robot displacement (linear and rotational)  Compute current robot pose with simple geometric equations  Provides good short-term relative position accuracy  Accumulation of errors in long-term – wheel slippage, bumps, etc.,

Robot Building Lab: Localization Problems with Dead Reckoning  Calibration, uncertain robot geometry  Friction, wheel slippage  Errors grow without bound unless periodic absolute position corrections from other sensors used

Robot Building Lab: Localization Odometry Errors  Systematic Errors Asymmetries, uncertainties in robot dimensions Kinematic imperfections* –Can be measured and adjusted for – vehicle-specific calibration  Non-systematic Errors Bumps, uneven floors, slippage –More difficult to handle but not as dominant as systematic errors on smooth indoor surfaces  Frequent absolute position adjustments needed in any case  *kinematics: given starting point and motor commands, specify ending point – what would inverse kinematics be?

Robot Building Lab: Localization Systematic Errors  Two primary sources: –Unequal wheel diameters (Ed) – lead to curved trajectory –Uncertainty about wheel base (Eb) – lead to errors in turn angle  Errors may also be affected by load distribution  Vehicle-specific calibration needed – for error correction in software  Careful experiments needed to measure errors accurately  Periodic absolute pose updates cannot be avoided but the frequency can be reduced

Robot Building Lab: Localization Reducing Odometry Error with Absolute Measurements  Uncertainty Ellipses  Change shape based on other sensor information  Artificial/natural landmarks  Active beacons  Model matching – compare sensor-induced features to features of known map – geometric or topological

Robot Building Lab: Localization Today  Odometry overview  Encoder software  Kinematics, Inverse Kinematics: Odometry calculations  Closed-loop control using encoders  Proportional derivative control

Robot Building Lab: Localization Library Drivers to do the Counting Machine language routine loaded into IC’s underlying layer of direct 68HC11 code, with user interface - IC binary (ICB) files installed in interrupt structure of 68HC11 Monitors shaft encoder values and calculates encoder steps and velocity needs quickly and at regular intervals HB’s software libraries include set of routines for supporting shaft encoders for both position- counting and velocity measurement. For each analog input on HB, a pair of shaft encoder routines is provided. Once loaded into IC, the encoder routines are automatically active; no additional commands are needed to turn them on. – Each encoder0_counts variable (running total of transitions on encoder sensor) will automatically increment every time it senses a transition on its corresponding encoder sensor – The encoder0_velocity value (velocity measurement) is continuously updated (copyright Prentice Hall 2001)

Robot Building Lab: Localization Programming Encoders /* Normal encoders, on digital ports 7 and 8. Must load encoders.lis to use this, more info in the HB manual. */ void main(void) { enable_encoder(0); /* Turn on encoder on port 7 */ motor(0, 20); while (read_encoder(0) < 130) ; reset_encoder(0); motor(0, -20); while (read_encoder(0) < 130) ; ao(); } /* Using encoders on analog ports 0 through 5 Must load the relevant file, sendr0.icb in this case. Consult the readme in the libs directory for info. */ void main(void) { motor(0, 20); while (encoder0_counts < 130) ; encoder0_counts = 0; motor(0, -20); while (encoder0_counts < 130) ; ao(); /* Note that these analog functions also provide velocity information */ }

Robot Building Lab: Localization Today  Odometry overview  Encoder software  Kinematics, Inverse Kinematics: Odometry calculations  Closed-loop control using encoders  Proportional derivative control

Robot Building Lab: Localization Kinematics  Kinematics: –Given: Starting point Motor commands –Compute Ending point  Inverse kinematics –Go from starting and ending points to motor commands  Problems: –Systematic and non-systematic errors make computations only approximations of actual results  Solution: –Calculate sequences of small movements and adjust for observed movements

Robot Building Lab: Localization Figuring Out Kinematics of Your Robot  Assume encoders mounted on drive motors  Let –Cm = encoder count to linear displacement conversion factor –Dn = wheel diameter –Ce = encoder pulses per revolution –N = gear ratio  Cm =  Dn / N Ce  Incremental travel distance for left wheel = Cm NL (NL = encoder counts on left wheel)  Incremental travel distance for right wheel = Cm NR (NR = encoder counts on right wheel)  That’s all we need for determining horizontal displacement and rotation from encoder counts

Robot Building Lab: Localization Differential Drive Odometry/Kinematics  DL = distance traveled by left wheel  DR = distance traveled by right wheel Distance traveled by center point of robot is then D = (DR+DL)/2 Change in orientation Dtheta is then (DR – DL)/base New orientation is then theta’=theta + Dtheta If old robot position is x,y new robot position is now x’ = x + D cos theta’ y’ = y + D sin theta’

Robot Building Lab: Localization Inverse Kinematics Key question: Given a desired position or velocity, what can we do to achieve it? V R (t) V L (t) starting position final position x y (Courtesy of Dodds)

Robot Building Lab: Localization Inverse Kinematics Key question: Given a desired position or velocity, what can we do to achieve it? V R (t) V L (t) starting position final position x y (Courtesy of Dodds)

Robot Building Lab: Localization Inverse Kinematics V R (t) V L (t) starting position final position x y Finding some solution is not hard, but finding the “best” solution is... quickest time most energy efficient smoothest velocity profiles V L (t) t Key question: Given a desired position or velocity, what can we do to achieve it? (Courtesy of Dodds)

Robot Building Lab: Localization Inverse Kinematics Key question: Given a desired position or velocity, what can we do to achieve it? V R (t) V L (t) starting position final position x y Need to solve these equations: for V L (t) and V R (t). x = ∫ V(t) cos(  (t)) dt y = ∫ V(t) sin(  (t)) dt  = ∫  (t) dt  = ( V R - V L ) / 2d V  =  R = ( V R + V L ) / 2 There are lots of solutions... (Courtesy of Dodds)

Robot Building Lab: Localization Inverse Kinematics Usual approach: decompose the problem and control only a few DOF at a time V R (t) V L (t) starting position final position x y Differential Drive (Courtesy of Dodds)

Robot Building Lab: Localization Inverse Kinematics Usual approach: decompose the problem and control only a few DOF at a time V R (t) V L (t) starting position final position x y Differential Drive (1) turn so that the wheels are parallel to the line between the original and final position of the robot origin. -V L (t) = V R (t) = V max (Courtesy of Dodds)

Robot Building Lab: Localization Inverse Kinematics Usual approach: decompose the problem and control only a few DOF at a time V R (t) V L (t) starting position final position x y Differential Drive (1) turn so that the wheels are parallel to the line between the original and final position of the robot origin. (2) drive straight until the robot’s origin coincides with the destination -V L (t) = V R (t) = V max V L (t) = V R (t) = V max (Courtesy of Dodds)

Robot Building Lab: Localization Inverse Kinematics Usual approach: decompose the problem and control only a few DOF at a time V R (t) V L (t) starting position final position x y Differential Drive (1) turn so that the wheels are parallel to the line between the original and final position of the robot origin. (2) drive straight until the robot’s origin coincides with the destination (3) rotate again in order to achieve the desired final orientation -V L (t) = V R (t) = V max V L (t) = V R (t) = V max -V L (t) = V R (t) = V max V L (t) t V R (t) (Courtesy of Dodds)

Robot Building Lab: Localization Inverse Kinematics  If you start at x=0, y=0, theta=0  And you want to drive straight to x=x’, y=y’, theta=0  What is your target NR and NL? SOLVE NOW

Robot Building Lab: Localization Today  Odometry overview  Encoder software  Kinematics, Inverse Kinematics: Odometry calculations  Closed-loop control using encoders  Proportional derivative control

Robot Building Lab: Localization Review: Control Paradigms  Open-loop control –execute actions without feedback – imprecise actuation (friction, gear slippage, battery discharge) and dynamic environments cause problems  Closed-loop control –use sensor feedback to monitor and modify actions

Robot Building Lab: Localization Review: Closed-loop Control  Drive parallel to wall  Feedback from proximity sensors (e.g. bump, IR, sonar)  Feedback loop, continuous monitoring and correction of motors -- adjusting distance to wall to maintain goal distance (Courtesy of Bennet)

Robot Building Lab: Localization Using Inverse Kinematics and Closed-Loop Control for Virtual Path Following  Given: –Initial x,y,theta –Target x’,y’,theta or x,y,theta’ or x’,y’,theta’  Compute (using simple inverse kinematics) –Target right encoder counts –Target left encoder counts  Loop (while targets not reached) –Compute current ratio and set state = Below_target_ratio or above_target_ratio or at_target_ratio –Adjust motor velocities to stay in at_target_ratio state

Robot Building Lab: Localization Review: Simple Processing Using Single Threshold Robot ground sensor can be in one of two states: State A: Over line State B: Over floor Compare sensor reading with setpoint value If less than this threshold set variable to indicate robot is in State A Otherwise, set variable to indicate State B What to use as setpoint threshold? midpoint between floor value and line value E.g. 10 when aimed at the floor, and 50 when aimed at the line  choose 30 as setpoint threshold (copyright Prentice Hall 2001)

Robot Building Lab: Localization Review: Two Thresholds for Hysteresis Problem with single threshold – variances in sensor readings Bump on floor may spike the readings Shiny spots on line may reflect as well as the floor, dropping the sensor readings up into the range of the floor Solution: two setpoints can be used – Imposes hysteresis on the interpretation of sensor values, i.e., prior state of system (on/off line) affects system’s movement into a new state Line Following performance run : Setpoint =20 int LINE_SETPOINT= 35; int FLOOR_SETPOINT= 10; void waituntil_on_the_line() { while (line_sensor() < LINE_SETPOINT); } void waituntil_off_the_line() { while (line_sensor() > FLOOR_SETPOINT); } (copyright Prentice Hall 2001)

Robot Building Lab: Localization Review: Separate Sensor State Processing from Control Functions might each make use of other sensors and functions – need to decide how to implement each (Courtesy of Bennet)

Robot Building Lab: Localization Today  Odometry overview  Encoder software  Kinematics, Inverse Kinematics: Odometry calculations  Closed-loop control using encoders  Proportional derivative control

Robot Building Lab: Localization Does it matter how we adjust motor velocities?  Given: –Initial x,y,theta –Target x’,y’,theta or x,y,theta’ or x’,y’,theta’  Compute (using simple inverse kinematics) –Target right encoder counts –Target left encoder counts  Loop (while targets not reached) –Compute current ratio and set state = Below_target_ratio or above_target_ratio or at_target_ratio –Adjust motor velocities to stay in at_target_ratio state

Robot Building Lab: Localization Proportional-Derivative Control Control algorithm generates a stronger response the farther away the system is from the goal state — response of control algorithm is proportional to amount of error Proportional Error Controller: Set the encoder counter to 0, and then write an infinite loop to repeatedly set the motor speed to the difference between a desired position and actual position: {encoder10_counts= 0; while (1) {motor(0, encoder10_counts);}} When the program starts to run, the difference between the desired position (setpoint =100) and the actual position (0) is 100, so the motor turns on full speed, driving the wheel toward the desired position. As it starts going, the error becomes progressively smaller. When it’s halfway, at position 50, the error is only 50, so at that point the motor goes at 50% of full power. When it arrives at the intended position of 100, the error is zero, and the motor is off. Proportional Gain (ratio between error and power): Instead of a one-to-one ratio between error counts and motor power percentage, modify the controller so it multiplies the error value by 5: {encoder10_counts= 0; while (1) {motor(0, 5 * (100 -encoder10_counts));}} Response should feel much “snappier.” The wheel should reach the setpoint position faster, and it should resist being turned away from it much more aggressively. Issues: Overshoot, oscillations, and offset (copyright Prentice Hall 2001)

Robot Building Lab: Localization Gain and Overshoot, Oscillations, and Offset Pgain = 10 Pgain = 20 Data taken every 0.2 seconds of real time. System is driven from 100 counts to 0 counts Pgain=10: Full-power command as wheel heads toward setpoint. –Offset Error: System did not stabilize at the goal. From around the 1.2 second mark onward, the position was fixed at a count of 1. This generated a power command of 10%, which was too small to activate the motor. Pgain=20: should ameliorate the offset problem, since the same static error will result in a higher power command – Offset error is solved – Another problem: the beginnings of an oscillation. Now the system overshoots three times—twice beyond the setpoint and once before it. (copyright Prentice Hall 2001)

Robot Building Lab: Localization Proportional-Derivative Control Pgain = 30 Pgain = 50 Pgain=30: Oscillation problem is more pronounced; there are a total of five oscillatory swings Pgain=50: Oscillation behavior has taken over – System moves to within a slight distance from the setpoint, but cannot stabilize at the setpoint – Even a small error generates a power command that moves the system across the setpoint, resulting in a power command in the opposite direction – While the position error is small on the graph, the power command swings are quite evident. (copyright Prentice Hall 2001)

Robot Building Lab: Localization Momentum Problem: Simply cranking up the pgain does not get the system to perform better –When pgain is high, then even a slight deviation from the setpoint causes a large power command Solution: Correct for the momentum of the system as it moves toward the setpoint – Momentum is mass times velocity; therefore, momentum is directly proportional to velocity – Correct for the velocity when the system nears its setpoint by subtracting an amount from the power equation based on the velocity of the system void collect_data() { int i, power, counts, velocity; for (i= 0; i< SAMPLES;) { counts= encoder10_counts; velocity= encoder10_velocity; power= pgain * (0 - counts) - dgain * velocity; motor(0, power); data[i++]= counts; data[i++]= velocity; data[i++]= power; } Power command is now combination between proportion of error and velocity of system (copyright Prentice Hall 2001)

Robot Building Lab: Localization Pgain and Dgain Pgain=4, Dgain=1: Overshoot is minimized, and there is no oscillatory behavior at all. Pgain=10, Dgain=5: unstable; dgain is too large – Position graph: controller “puts on the brakes” too hard and the system stops moving before the destination setpoint (between the 0.8 and 1.0 second mark) – When the velocity hits zero, the proportional gain kicks in again and the system corrects PD Control is used extensively in industrial process control – Combination of varying the power input when the system is far away from the setpoint, and correcting for the momentum of the system as it approaches the setpoint is quite effective pgain=4, dgain=1 pgain=10, dgain=5 (copyright Prentice Hall 2001)

Robot Building Lab: Localization Lab 6 Localization, Virtual Path Following, Proportional Control es/robotlab/labs/lab06.pdf es/robotlab/labs/lab06.pdf

Robot Building Lab: Localization Lab 6: Localization, Virtual Path Following, Proportional Control  In-class lab overview: –Empirically derive kinematics of robot. Program robot to move specified motor speeds, times: straight, turn-in-place, curved movement (left, right, and backward left, right) – you select a representative group Record encoder counts and distances and display counts on LCD Show LCD counts to TA –Implement kinematic equations for your robot and compare distances predicted by kinematics to actual distances recorded in previous step. Save results for this week’s write-up. –Write program that takes a set of target encoder counts and uses their ratio with a wall-following-like closed-loop to track ratio while trying to achieve target. (I.e. states above_target_ratio, below_target_ratio, at_target_ratio) –Implement inverse kinematic equations to be able to take a distance and angle as input and compute the encoder counts needed to achieve that target point; implement using closed- loop program above. Show results to TA

Robot Building Lab: Localization Next Week  Take-home part of Lab –Add proportional control to adjust velocity during trajectory tracking program (play with loop frequency as well) –Demo: move in subscribed patterns (e.g. square, zig-zag) using inverse kinematics, closed-loop encoder monitoring, and proportional control  Read on-line documents; answer questions for Week 6 on Plan  Help Session Monday  Lab 7: Understanding sonar, obstacle avoidance while tracking trajectory

Robot Building Lab: Localization Lab 6 Programs Global variables for: Encoder state Function to determine encoder state Function to move toward left Function to move toward right Function for forward kinematics Function for inverse kinematics Function to loop until target (x,y,theta) is reached Function to input sequence of (x,y,theta) targets Functions for movement with proportional derivative control void main(void) { start process to keep track of encoder states for each (x,y,theta) in list start function to loop until (x,y, theta) is reached }

Robot Building Lab: Localization References  ab/labs/lab06.pdf ab/labs/lab06.pdf  ab/readings/hbmanual.pdf ab/readings/hbmanual.pdf  gs gs

Robot Building Lab: Localization Ports  inputs for 7 analog sensors and 9 digital sensors  We are using 2/4 motor ports, 7/7 analog sensor ports and 6/9 digital sensor ports next week  Light sensors – 3 analog ports (eg 0,1,2)  IR sensors – 2 analog ports (eg 3,4)  Wheel encoders – 2 analog ports 5,6  Sonar – 1 digital port 7  Turret servo – digital port 9  Bumpers – 4 digital ports (eg 10,11,12,13)  Motors – 2 DC motor ports