Navigation methods and systems

Transcription

1 Navigation methods and systems Navigare necesse est Content: Navigation of mobile robots a short overview Maps Motion Planning SLAM (Simultaneous Localization and Mapping)

2 Navigation of mobile robots a short overview

3 Basic concepts Navigation in a space means Knowing the position or pose (position +orientation) of a moving object Planning a passable route from position (or pose) A to position (or pose) B Positions are defined on a map, which is a simplified depiction of the space highlighting relations between components (objects, regions) of that space.

4 Maps There are many different type of maps. In principle, they can be divided into two categories: metric and topological. Metric maps describe details of the environment in a metric co-ordinate system. Position (pose) is determined by its co-ordinates. In topological maps features of the environment are described and position is determined logically relative to the features rather than geometrically. In practice, a map can include both metric and topological properties. Modern electronic maps are often layered presentations including both features.

5 Maps

6

7 Map made for a forest area using 3D laser scanner and a SLAM method

8 Occupancy grid A 2D representation Each cell is assumed independent Probability of a cell being occupied is being estimated Bayes

9 Occupancy grid Based on sensor model and known pose, determine the evidence Update for each cell:

10 Motion planning Inevitable part of mobility! Motion planning is classically divided into two parts: Path Planning = finding an optimal collision free route to the final destination when obstacles on a map are known Obstacle avoidance = high frequency layer that follows the plan, but reacts to dynamic obstacles.

11 Configuration Space A configuration describes the pose of the robot, and the configuration space C is the set of all possible configurations. Many classical methods uses configuration space (Cspace). In C-space the object (robot) is described as a point, which makes motion planning simple in principle. However, transferring from the original physical space to configuration space needs mapping of objects between these spaces and is not necessarily an easy process.

12 Motion planning is sometime called solving piano moving problem

13 Path planning Route graph methods (A*, Dijkstra, ) Voronoi Cell decomposition Potential field (or similar) methods Sampling (probabilistic) Methods (Probabilistic roadmap planner)

14 A* Algorithm Uses a best-first search and finds the leastcost path from a given initial node to goal node Distance + cost heuristic function the path-cost function, which is the cost from the starting node to the current node Shortest distance to goal heuristic function

15 A* Algorithm Current Node, Goal Node Open set, closed set Node values: Distance traveled, Came from, Score Roughly: Take the best node from open set as current For each neighbor nodes, not in closed set: Compute score If in open set and score smaller, replace If not in open set, add to open set

16 Example

17 Case example Map as surface model Find safe path to target

18

19 Rapidly-Exploring-Random tree planner Not all robots can be considered holonomic Plans in control space Machine specific kinematic model Machine specific collision detection

20 Collision Avoidance Local planning Based on excising global plan and sensor readings, generate a local plan (or controls) that will safely guide the robot to goal

21 Collision avoidance methods Potential Field methods Vector field histogram Elastic Band Dynamic window approach Nearness Diagram

22 Dynamic Window Approach Constrained optimization in control space Assumes circular trajectories (v,w), forward speed and angular speed Optimizes the controls that lead towards goal, while avoiding collision in small time window

23 Admissible speeds A given speed is admissible if the robot is able to stop before colliding

24 Reachable speeds The set of speeds that can be reached within the time window

25 DWA search Subset of (v,w) to search are the admissible speeds (that the robot can drive), which are also reachable Cost function to maximize (Fox & Al):

26 Example

27 SLAM Simultaneous Localization and Mapping

28

29 SLAM Problem A Map is needed to localize a robot A Pose estimate is needed to build a map Uncertainty in pose and in the measurements Errors in map and pose estimates are correlated How to match observations and landmarks Data association

30 Naive SLAM Estimate your pose with respect to the previous map (Localization) Add measurement to the map based on pose estimate (Mapping) Locally consistent how about globally?

31 Our implementation Occupancy Grid as map Branch and Bound global localization algorithm as maximum likelihood estimator Sequential (update global map with single measurement) Batch (update global map with local batch) Videos Mapping with Avant J2B2 exploration

32 Loop closing The localization errors are induced to the map Returning to previously visited place causes inconsistency Gutmann&Konolige added a loop detection and maintained links between estimates to correct

33 FAST-SLAM with grids Rao-Blackwellized Mapping Particle filter where Each particle represent a possible trajectory of the robot maintains its own map and updates it upon mapping with known poses Each particle survives with a probability proportional to the likelihood of the observations relative to its own map Problems big maps

34

35

36 EKF

37 EKF cycle State Prediction Measurement prediction Observations Data-Association Update Integration of new landmarks Interested? Please refer to Tim Baileys thesis pages (and if still interested, read the whole book)

38 Kalman filter Kalman filter is a recursive algorithm, which produces on-line minimum variance estimate for the state vector of a dynamical system when data is collected from measurements. It can be applied to large variety of estimation problems in systems science. In the following filter itself and its usage is explained. The mathematical proof is not presented. It is not very straightforward, but can be found from literature. There exists a Matlab toolbox that supports its usage well.

39 Kalman filter The starting point is linear dynamics, which is given in a state form x(k+1) = A(k)x(k) + B(k)u(k) + w(k) y(k) = C(k)x(k) + v(k), dynamics measurement where w and v are assumed sequences of non-correlated stochastic variables of which we know covariance matrix functions E(w(k)w(k)T) = Q(k) E(v(k)v(k)T) = R(k). In addition it is supposed, that the mean values satisfy E(w(k)) = E(v(k)) = 0. The sequences x(0), x(1)... ja y(0), y(1)... are so-called Markov-sequences due to the next-step form of the state equation. It can be shown that if w ja v have Gaussian distributions both x and y have also Gaussisia distributions. Furthermore, the covariances and means of the x and y sequences can be calculated relatively easily. Kalman filter solves the following estimation problem: From the known inputs and measurements u(0), u(1),..., y(0), y(1),... we have to form minimumvariance estimate x(k k) so that it can be obtained recursively from the known previous estimate x(k-1) and new input-measurement pair u(k), y(k). In minimum variace estimation the following minimum is searched E [ x k k E x k T x k k E x k ] min.

40 The Kalman filter gives the solution for this problem, i.e. the estimate x k k, which can be obtained in recursive easily calculated form. The notation x k k means the estimate at time k when all data up to the time k are available. Correspondingly the notation x k k 1 means the estimate at time k when data up to the time k-1 is available. The basic form of Kalman filter is x k k = x k k 1 +K k [ y k C k x k k 1 ], where K k is so-called gaim matrix of the filter. In Kalman filter this matrix is determined in the optimal way as described above (not proved here; proof can be found in litterature) when the system model is known. It can be seen that the estimate includes two parts, so-called predicting part x k k 1 and correcting part K k [ y k C k x k k 1 ], which in turn is obtained by multiplying the estimation error of the last measurement by the gain. The Kalman filter includes the equations for the predicting part and for the gain matrix, both of which are obtained in a recursive form. The equations give also a covariance matrix, which indicates the accuracy of the estimate. The equations are x k k = x k k 1 +K k [ y k C k x k k 1 ] taking into account the new measurement x k k 1 =A k x k 1 k 1 +B k u k 1 T T predicting T K k =P k k 1 C k [ C k P k k 1 C k +R k ] T 1 gain T 1 P k k =P k k 1 P k k 1 C k [ C k P k k 1 C k +R k ] C k P k k 1 T P k k 1 =A k 1 P k 1 k 1 A k 1 +Q k P k k = covariance of the estimate x k k P k k 1 = covariance of the estimate x k k 1 y k k =C k x k k covariance equations, where measurement

41 Extended Kalman filter Kalman filter gives the optimal solution for the linear case. In many applications, however, the model is non-linear. The minimum variance estimation problem cannot be solved in a general case. However, the filter, called Extended Kalman filter (EKF), which is made in the following ad-hock way by generalizing the linear filter, works well in many cases in practice. x(k+l) = f(x(k), u(k)) + w(k) y(k) = g(x(k), u(k)) + v(k) model Predicting is made by using the equation x k k 1 =f x k 1 k 1,u k 1 and the new measurement is taking into account by the equation x k k = x k k 1 +K k y k g x k k 1,u k, where the gain matrix is calculated from the linearized (differential) form of the model.

42 Use of Kalman filter Kalman filter or its extended form EKF is one of the most frequently used algorithm in estimation and signal processing. In navigation problems it can be used to - estimate the pose under incomplete measurement information - fuse different type of navigation data Following example illustrate its use in a real example, where locations of a distributed underwater robot float system is estimated (SWARM-system).