Robotics. Chapter 25-b. Chapter 25-b 1

Transcription

1 Robotics Chapter 25-b Chapter 25-b 1

2 Particle Filtering Particle filtering uses a population of particles (each particle is a state estimate) to localize a robot s position. This is called Monte Carlo Localization (MCL) 1. Start with a population of N samples taken from prior distribution at time 0: P(X 0 ). 2. The algorithm or user chooses an action a t, and then moves each particle based on the a t. 3. Propagate each particle sample forward by sampling P: x t+1 = sample(p(x t+1 x t,a t )) 3. Weight each sample by the likelihood that it assigns to the new evidence, P(e t+1 x t+1 ) 4. Resample the population, duplicating some samples. Mutate duplicated samples, or let later sampling spread them out. Chapter 25-b 2

3 Monte Carlo Localization Example Robot position At time t0, the robot does not know where it is, so it distributes particles throughout the environment. Chapter 25-b 3

4 Monte Carlo Localization Example Robot position After a few iterations, the robot has produced a bimodal distribution of its possible location. Chapter 25-b 4

5 Monte Carlo Localization Example Robot position After some more interaction, the robot is able to localize its position quite well. Chapter 25-b 5

6 Monte Carlo Localization Questions (Q) How many particles are needed? (Q) How to estimate the actual robot position from the particles? (Q) How do we balance exploration and exploitation? (Q) How accurate do our probabilities really need to be? (Q) What is needed to implement Monte Carlo localization for the assignment? Chapter 25-b 6

7 Localization and Mapping Problem Variants Localization: given map and observed landmarks, update pose distribution The robot is given the map. Mapping: given pose and observed landmarks, update map distribution The robot is trying to make the map, but knows where it is. SLAM (Simultaneous Localization and Mapping): The robot must both make the map and determine where it is! Chapter 25-b 7

8 SLAM Most SLAM methods use an extended Kalman Filter (EKF). - Robot pose and landmark positions are tracked. - Landmarks added as the robot explores the environment. The space is represented as a single, high-dimensional Gaussian: - Contains convariances between pose and landmark variables: Σ t = Σ XX Σ XM Σ T XM Σ MM Σ XX = robot pose covariance (just localization) Σ MM = covariance between landmark positions Σ XM = covariance between landmarks and robot pose. (Q) Draw a picture and indicate covariance by drawing ellipses. Chapter 25-b 8

9 Motion Planning Plan in configuration space defined by the robot s DOFs conf-2 conf-3 conf-1 conf-2 conf-1 conf-3 elb shou Solution is a trajectory in free C-space (avoiding occupied space) (Q) Sketch a robot setup and configuration space for it. (Q) Why do we want to plan in configuration space rather than world space? Chapter 25-b 9

10 Motion Planning Definitions Point-to-point motion planning - simply move robot or its effector from one point to another Conformant motion - Move while in contact with physical obstacle (screw in light bulb) Configuration Space - the space defined by robot s DOF - location, orientation, joint angles, etc. Path Planning - Finding a path from one point of configuration space to another. (Forward) Kinematics - Transform configuration space to workspace. Inverse Kinematics - Find a configuration to place effector in specific workspace coords. Chapter 25-b 10

11 Configuration space planning Basic problem: d states! Convert to finite state space. Cell decomposition: divide up space into simple cells, each of which can be traversed easily (e.g., convex) Skeletonization: identify finite number of easily connected points/lines that form a graph such that any two points are connected by a path on the graph Chapter 25-b 11

12 Cell decomposition example goal start start goal Because of discretization, there may be no path in pure freespace cells - Recursive decomposition of mixed (free+obstacle) cells - Exact polygonal decomposition Keeping away from obstacles obstacles: - Repelling potential field. Chapter 25-b 12

13 Skeletonization: Voronoi diagram Voronoi diagram: locus of points equidistant from obstacles Problem: doesn t scale well to higher dimensions! (Q) Does cell decomposition scale better? Chapter 25-b 13

14 Robot Controllers Issues with following C space plans - Robots cannot follow arbitrary paths in configuration space! - Most robots exert forces rather than directly changing position. Controllers can be used to keep the robots on track - Must take into account both kinematic and dynamic state - Must work within the constraints of the robot s effectors Chapter 25-b 14

15 P Controller P Controllers provide a correction force proportional to robot error (how far it is off the path): a t = K P (y(t) x t ) a t = force applied x t = robot state at time t y(t) = the desired point on the path at time t. K P = gain parameter (how strong the force is) Unfortunately, this is essentially a spring equation! Thus, it oscillates about the true path. Reducing K P slows the oscillation, but doesn t remove it. P Controllers may be VERY unstable! We want STABLE controllers! Chapter 25-b 15

16 PD Controller PD Controller - Modify P Controller by a Derivative term: a t = K P (y(t) x t )+K D (y(t) x t ) t (y(t) x t )/ t = derivative of error over time. K D = derivative gain term (Q) What happens if the robot is moving towards the goal (y(t))? (Q) What happens if the robot is moving away from the goal? Chapter 25-b 16

17 PID Controller PID Controller - Add a new Integration term that pushes harder the longer we have been off the path: a t = K P (y(t) x t )+K I (y(t) xt )dt+k D (y(t) x t ) t (y(t) x t )dt = the integral of the error over time K I = gain for the integration term PID Controllers often used in industry. Chapter 25-b 17

18 PD controller for inverted pendulum (Q) How do we create a PD controller for an inverted pendulum? Chapter 25-b 18