CS 1567 Intermediate Programming and System Design Using a Mobile Robot Aibo Lab3 Localization and Path Planning

Transcription

1 CS 1567 Intermediate Programming and System Design Using a Mobile Robot Aibo Lab3 Localization and Path Planning In this lab we will create an artificial landscape in the Aibo pen. The landscape has two types of features, walls and pink post-its. The landscape is mapped on a 6x6 grid of 50cm square cells. All walls are aligned with cell boundaries and all post-its are positioned in the center of the corresponding cell. Your Aibo will start with a map of the pen such as the one below that shows the location of all features. The location will also include the coordinates of a goal where Aibo s bone is buried. (Not really). Your Aibo will start at an unknown location in the pen. Your task has two parts. First, explore the landscape until you unambiguously determine your location. Second, plan and execute a walk through the landscape from this position to the goal location. As always, your grade will include a competition for the speed and accuracy with which your Aibo finishes the task. (0,0) (0,5) (5,0) (5,5) Getting Started Movement through the pen The best strategy for completing is lab is to take on the movement components first. In the lab your Aibo will need to move with much greater precision than last time and be able to follow specific paths. The best place to start will be to implement two types of primitive movements, turning within a cell and moving from one cell to another.

2 Step 1 Implement the following two methods Turn (enum {LEFT,RIGHT,AROUND} direction.) This method should initiate a turning walk-in-place. The method takes and enumerated argument with three values, LEFT means to turn 90 degrees to the left, RIGHT means 90 degress to the right, AROUND means to turn 180 degrees. You can control your turn by a combination of timing the walk and using the IR sensors to align yourself with the walls Move(int NumofCells) This method should initiate a forward walk for distance equal to NumofCells*CellSize. There are several ways to keep track of how far you have moved. You can use a timer event to get approximate distances but you should also select reference points before and after the move, a front and/or side wall if for example to adjust your final position. Once you have implemented and thoroughly tested these movements the next step will be to string a sequence of movements together such that the Aibo follows a path. In this formulation a path is defined as a set of move and turn operations. For example, if the Aibo is at grid coordinate (2,2), facing in the positive x direction and needs to move to (4,4), a possible path (ignoring obstacles) would be Move(2), Turn(RIGHT), Move(2), Turn(LEFT) Obviously, there will usually be many paths between two grid locations. We will deal with how to discover and choose between them later. First you need to implement the code to follow a known path. Step 2 Implement a Path following behavior Decide on an encoding for each step and create a list subclass that holds the path definition. You will need a method to start down the path that will remove the first step from the list and initiate that movement. After that, you will need a state, entered on completion of the previous movement that you remove the next step and initiate it. This repeats until the path list is exhausted. Planning a Path Ever wonder how MapQuest is so efficient at looking at road maps and plotting driving directions for you. We are going to do a mini-version of that algorithm here. It s a depthfirst search with iterative deepening to keep the search time manageable. Consider a path from cell (1,2) to cell (3,4) in the example map. To compute this path, we build a tree of

3 the possible paths between these nodes such as the one shown in figure 2. As then name implies we use a depth first algorithm to build the tree but we will limit the depth that we build during each iteration. At each node, you will build a subtree rooted in node to the north, east, south, and west respectively unless one of the following is true. a) There is a wall obstructing your path. b) You have visited the grid location already c) You have reach the max depth for this iteration If b) is true, this means that the node is already in the tree, but it may not be on best path to reach that node. You resolve this conflict by associating with each node a cost of the path to reach that node from the starting point. In your case, your cost is the number of steps in the path and this corresponds to the depth of the three at the node location. So if you have already visited a node, you will move the node to the new path only if it is at a deeper level in the tree. Step 3 Plan and Execute a Path between known locations Write a behavior that reads in the map of the pen and computes the path between a goal location and a known starting grid cell location in the pen. Localization This brings us to the other half of this lab. Since the path planner above assumes a known starting location, we cannot reach the goal until we determination where we are. In robotics this is called localization. The basic approach to our localization problem is too look around and identify a specific feature and then compares that with the map of the features for the pen. Most of the time a single feature will give us ambiguous results. We will learn how to deal with that later. First, we need to get familiar with the feature set and how to recognize them. We have organized the pen in a fairly regular way so the set of possible feature configurations is relatively small. Basically, there are walls and post-it notes, where the walls are aligned (if present) with the borders of each cell and the post-its (if present) are placed in the center. You can assume that there are enclosing walls around the entire pen. This means that there are a fixed number of configurations of walls and post-its that you can see. These are enumerated in figure 3. In addition to the individual cell types, each may appear in any of several orientations on the map. These are indistinguishable until you have global information about the orientation of the dog. Step 4 What do I See? Write a behavior that scans the immediate vicinity for walls and post-it notes, based on this observation your behavior should classify the current grid cell location as one of the ten possible cell type listed below.

4 Walls None one two two three Post-it No Post-it Once you have code written that can determine the cell type, the localization process can begin. Begin by thinking of a set of states that represent all of the possible positions and orientations of the dog in the pen. Initially, if the size of the pen is NxM there are n*m*4 possible states, one for each grid location and 4 orientations in each. Your algorithm show proceed as follows Step 5 Localization algorithm 1-transform the map into an initial state set where each element encodes what the dog would see for each grid location and each orientation at that grid location 2-Iterate as follows until the size of the state set is reduced to one 1. Scan the current node and determine the type of node configuration that the Aibo perceives 2. For each element in the state set, determined if the perceived configuration is consistent with the position and orientation encoded for that element, if not delete the element. 3. Determine the next action based on the current state set. The action should be one of the basic actions defined above 4. Do the action 5. Replace the state-set with the set that the Aibo could be in after the action 6. Check if you are at the goal.

5 Most of this algorithm is doing state-set tracking: determine what possible states the robot might be in, based on its current perceptual input, and what states might it be in based on the action it just took? Here is some further discussion of the functions called in this algorithm sketch. Localization algorithm: Step 2 Given a set of states that the robot might be in, which of them are consistent with the robot s current percept? The sees function filters the current state-set C by removing states that are incompatible with the current percept. The resulting state-set R will be a subset (but not necessarily a proper subset!) of C. For each state in the state-set, you will need to compare the current percept with the expected percept for that state. For example, in the environment below, when you start (complete uncertainty), all the possible states are in your state set. Suppose that your robot immediately sees, as a result of examining its area, three walls around itself. Then filter away all possible states except for one particular goal state; you re done in this case! Localization algorithm: Step 3 Given a set of states that the robot might be in, what is the proper action to take? It s easy to answer this question when the set of states contains precisely one state (a singleton set), but what if there are many states in the set? This is not an easy question to answer, so we're giving you a set of hints below (see Appendix A). We will test your state-set tracking automata by placing the robot in several different initial starting positions and orientations and running your program. Localization algorithm: Step 5 Given an action and a set of states that the robot might be in, what is the result of applying the action to the state-set? This is a transformation function. For each state in the original state-set, you will need to compute the resulting state based on the specified action. Remember that if you invoke Move(1)

6 while you re facing a wall, your robot will not go anywhere (assuming you ve got Move implemented right). Thus, it is possible for multiple states to map onto one, so you will need to make sure that you don t duplicate states in your new state-set. Appendix A: On Determining Your Next Action First, let s consider the problem of representing possible decisions. We could create a big lookup table. The first column would list each possible state set and the corresponding entry in the second column would list the correct action to perform. The two-node world For example, given a simple two-node world shown above, the table would be: state-set action (((0,0) u) ((0,0) r) ((0,0) d) ((0,0) l) ((1,0) u) ((1,0) r) ((1,0) d) ((1,0) l)) <action> (((0,0) u) ((0,0) r) ((0,0) d) ((0,0) l) ((1,0) u) ((1,0) r) ((1,0) d)) <action> (((0,0) u) ((0,0) r) ((0,0) d) ((0,0) l) ((1,0) u) ((1,0) r) ((1,0) l)) <action> (((0,0) u) ((0,0) r) ((0,0) d) ((0,0) l) ((1,0) u) ((1,0) r)) <action>... etc., etc., etc., and eventually: (((0,0) r)) (Move 1) (((0,0) d)) (Turn LEFT) (((0,0) u)) (Turn RIGHT) (((0,0) l)) (Turn AROUND) Unfortunately, for any reasonably large problem, this table can be huge. Even for the two-node world, the complete table would have 248 rows. For the environment of 6.3, the total number of rows would be x This is bad. But don t give up! We can shrink the size of the table considerably by only including state-sets that can be reached from the global state-set that contains all possible states. For our two-node world, there are only four possible (distinguishable) state-sets after the first call to sees. In other words, once you filter based on examining your area, you will know that you re in one of the following four state sets: 1. (((0,0) r) ((0,1),l))) 2. (((0,0),u) ((0,1),d))) 3. (((0,0),l) ((0,1),r))) 4. (((0,0),d) ((0,1),u)))

7 Here s another example. In the five-node world pictured below, there are only 12 possible statesets after the first call to sees: 1. (((0,0) r) ((0,1) r) ((2,0) l)) 2. (((0,0) u) ((0,1) u) ((2,0) d)) 3. (((0,0) d) ((0,1) d) ((2,0) u)) 4. (((0,0) l) ((0,1) l) ((2,0) r)) 5. (((1,0) u)) 6. (((1,0) r)) 7. (((1,0) d)) 8. (((1,0) l)) 9. (((1,1) u)) 10. (((1,1) r)) 11. (((1,1) d)) 12. (((1,1) l)) The five-node world Now lets think about how to choose the correct action for a particular state-set. If the state-set contains only one state, the robot knows its location exactly. The choice of actions is simply the one that moves (or turns) the robot toward the goal node. If there are multiple states in the stateset, then the idea is to choose an action that will allow the robot to eliminate some of the states the next time it calls sees. Eventually, all states but one will be eliminated. Now, we could ask you to write an optimal get-action function, but we aren t that sadistic. Just write a decent one. Once again: your get-action does not need to be optimal for this assignment. Appendix B: A Theoretical Aside for the Incurably Curious There is a side-effect of only including reachable state-sets in the table of actions: what happens if the robot somehow believes that its state-set is one of those not in the table? This could happen if we were to provide your robot with extra information. For instance, if we specify that the robot starts out facing north in the five-node world, the initial state set is not in the simplified table of reachable state-sets. Another way this can happen is if your area examination function errors, resulting in an incorrect state set. Handling these situations in the general case is hard. To be optimal for all possible state-sets, you would have to think through the right actions for each and every such state set. However, note that you can simply choose a table entry that corresponds to a superset of the actual state set. The action prescribed by this entry will certainly work, since it works on a superset (i.e. a more uncertain case).

8 Of course, if the impossible state set was the result of an error examining the area, then you ve got a problem on your hands. In this case, the robot would have to actually modify the state-set, growing it to encompass the real-world state. In effect, when faced with such impossibility, the robot forgets some of the information that it has previously acquired. Another solution would be to just completely start over, setting the state-set to all possible states again. This is especially useful if the state-set becomes empty after a call to sees!