robotics and autonomous systems - cgi.csc.liv.ac.uktrp/comp329_files/comp329_lecture13.pdf ·...
TRANSCRIPT
![Page 1: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/1.jpg)
Robotics and Autonomous SystemsLecture 13: Navigation in LeJOS
Terry Payne
Department of Computer ScienceUniversity of Liverpool
1 / 1
![Page 2: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/2.jpg)
Today
• From navigation to motion control.
2 / 1
![Page 3: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/3.jpg)
Navigation and path execution
• We started this course with three questions:
?
• Where am I ?• Where am I going ?• How do I get there ?
• Digging into the detail of how you do the last two.
3 / 1
![Page 4: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/4.jpg)
Navigation and path execution
• Last time convered how to go from:• Map• Start point• End point
to a sequence of waypoints that the robot has to traverse to get fromthe start to the goal.
• Turns out LeJOS has quite a lot of support for this.
• LeJOS can also help with the business of following the waypoints.
4 / 1
![Page 5: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/5.jpg)
Path following
• The planning methods we covered last time returned a sequence ofwaypoints.
• Sequence of robot poses:pxs , ys , θsq
px1, y1, θ1q
...pxg, yg, θgq
• Robot then needs to know how to move between the points.
5 / 1
![Page 6: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/6.jpg)
Path following
• Plan doesn’t restrict movement between waypoints.
6 / 1
![Page 7: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/7.jpg)
Path following
α
β
γ
• Simplest route is to turn to face the next point, then drive straight.
7 / 1
![Page 8: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/8.jpg)
Path following
• In this case we need to:• Rotate α• Drive• Rotate β• Drive• Rotate γ
• Not trivial to figure out the rotations required.
• Distances are pretty easy using the Euclidian distance formula fromthe previous lecture.
8 / 1
![Page 9: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/9.jpg)
Navigator
• LeJOS supports this through the Navigator class.
9 / 1
![Page 10: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/10.jpg)
LeJOS Navigator
• Provides all the functionality you need to have the robot follow asequence of waypoints.
• Controls the robot using our old friend:• DifferentialPilot
with all the good and bad things that entails.• Tracks where the robot is currently using:
• PoseProvider
• Default is OdometryPoseProvider
• But you can use MCLPoseProvider also
10 / 1
![Page 11: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/11.jpg)
LeJOS Navigator
• The pilot allows it to control the robot
. . . by sending it rotations and translations.
• The pose provider allows it to keep track of where it is.
. . . so it can compute what movements are necessary.
11 / 1
![Page 12: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/12.jpg)
LeJOS Navigator
• Navigator can be fed a sequence of waypoints.
• Or it can be fed a path.
12 / 1
![Page 13: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/13.jpg)
Waypoint
• LeJOS has an object to represent waypoints.• A waypoint can be a point:
• Point object• x and y coordinate.
or it can be a pose:• Pose object• x and y coordinate, plus a heading.
• A waypoint that is a point has heading 0.
13 / 1
![Page 14: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/14.jpg)
Waypoint
• API for waypoint includes the method:boolean checkValidity(Pose p)
which checks if the pose is close enough to the waypoint to count ashaving reached it.
• So, you can extend the class and override the function to create yourown waypoint that is reached approximately.
14 / 1
![Page 15: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/15.jpg)
Path
• Path is an ordered sequence of Waypoints.
• Implemented as a Java ArrayList of Waypoints
• But you don’t need to manipulate it.
15 / 1
![Page 16: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/16.jpg)
Navigator example
• Key functions are the following:
• Navigator(MoveController p)
Constructor, takes a pilot object as an argument.The navigator then uses this to drive the robot.
• void addWayPoint(float x, float y)
void addWayPoint(float x, float y,
float heading)
Adds a waypoint to the path.
• void followPath()
Drive to each waypoint in the path, in turn.
16 / 1
![Page 17: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/17.jpg)
Navigator example
• That’s all you need to use the Navigator
17 / 1
![Page 18: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/18.jpg)
Navigator example
• My code (below) uses a couple of additional functions, mainly forshow.
• boolean pathCompleted()
Reports if the robot has completed the current path.
• Waypoint getWaypoint()
Returns the current waypoint, the one the robot is heading to.
18 / 1
![Page 19: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/19.jpg)
Navigator example
public class PathFollower{
private DifferentialPilot pilot
= new DifferentialPilot(3.25, 19.8, Motor.C, Motor.B);
private Navigator navigator = new Navigator(pilot);
private Waypoint next;
public void newWaypoint(int x, int y){
navigator.addWaypoint(x, y);
}
public void navigate(){
while(!navigator.pathCompleted()){
navigator.followPath();
next = navigator.getWaypoint();
LCD.drawString("Moving to...", 0, 0);
LCD.drawString("(" + (int)next.getX() +
"," + (int)next.getY() + ")", 0, 1);
}
}
}
19 / 1
![Page 20: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/20.jpg)
Navigator example
• Note the use of private members for the PathFollower class.
• These, naturally, need public access functions.
• IMHO, this is good Java (and OO) style.
20 / 1
![Page 21: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/21.jpg)
Navigator example
• The navigator runs its own thread, so the while loop is really just fordriving the LCD display.
• Should work perfectly well as:public void navigate(){
navigator.followPath();
while(!navigator.pathCompleted()){
next = navigator.getWaypoint();
LCD.drawString("Moving to...", 0, 0);
LCD.drawString("(" + (int)next.getX() +
"," + (int)next.getY() + ")", 0, 1);
}
(though I haven’t tested this version).
21 / 1
![Page 22: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/22.jpg)
Navigator example
• The main() is in another class, which creates a Pathfinder object,and sets up the waypoints.
public class RunNavigator{
public static void main(String[] args)
throws Exception{
PathFollower pFollow = new PathFollower();
pFollow.newWaypoint(40, 0);
pFollow.newWaypoint(40, 40);
pFollow.newWaypoint(0, 40);
pFollow.newWaypoint(0, 0);
pFollow.navigate();
}
}
22 / 1
![Page 23: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/23.jpg)
Navigator example
• This is another version of the “drive in a square” program.
23 / 1
![Page 24: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/24.jpg)
Navigator example
• Both these classes are available from the module website.
24 / 1
![Page 25: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/25.jpg)
Path finding
• What we have so far is a program that will work when it already knowswhat the waypoints are.
• Not much use if you don’t don’t them.
• But LeJOS can help with this too.
25 / 1
![Page 26: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/26.jpg)
LineMap
• To do any pathfinding, we need a map, and LeJOS gives us theLineMap class.
• We set up a LineMap like this:Line[] lines = new Line[3];
lines[0] = new Line(50, 100, 100, 100);
lines[1] = new Line(100, 100, 100, 50);
lines[2] = new Line(50, 100, 100, 50);
Rectangle bounds = new Rectangle(-50, -50, 200, 200);
LineMap map = new LineMap(lines, bounds);
26 / 1
![Page 27: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/27.jpg)
LineMap
• A LineMap is an array of lines and a bounding box.• A bounding box is just a rectangle, specified by the:
• bottom left(smallest x and y coordinates)
• top right(largest x and y coordinates)
• Each line is defined by two pairs of coordinates that mark the endpoints of the line.
27 / 1
![Page 28: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/28.jpg)
LineMap
(200, 50)
(200, 100)
(200, 200)(50, 200)
(100, −50)(−50, −50)
28 / 1
![Page 29: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/29.jpg)
PathFinder
• Then all we have to do is:
Pose start = new Pose(0, 0, 270);
Waypoint goal = new Waypoint(125, 150);
ShortestPathFinder finder
= new ShortestPathFinder(map);
finder.lengthenLines(5);
PathFollower pFollow = new PathFollower();
pFollow.newPose(start);
Path path = finder.findRoute(start, goal);
pFollow.newPath(path);
pFollow.navigate();
29 / 1
![Page 30: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/30.jpg)
PathFinder
• Set up start and end points.For some reason they are different types of object.
• Create a path finder object and give it the map.• ShortestPathFinder• DijkstraPathFinder
Not clear what the difference is between these.
• Create clearance for the robotBasic path finder only works for infinitely small robots/points.
• Create an instance of our PathFollower class.
• Call the PathFinder on the start and goal points, and get a path back.
• Pass the path to the PathFollower and let it use its navigator tofollow it.
30 / 1
![Page 31: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/31.jpg)
PathFinder
(200, 50)
(200, 100)
(200, 200)(50, 200)
(100, −50)(−50, −50)
S
G
31 / 1
![Page 32: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/32.jpg)
Other forms of navigation
• Having seen how LeJOS can implement path finding in a map, let’slook at a couple of other forms of navigation that we might implementon the NXT.
• Simpler in some ways.
• Not necessarily as good at getting to the goal.
32 / 1
![Page 33: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/33.jpg)
Potential field
• Robot is treated as a point under the influence of an artificial potentialfield.
• The goal attracts it and obstacles repel it.
33 / 1
![Page 34: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/34.jpg)
Potential field
• If you know your location and the goal, can compute the “force” on therobot.
• Navigator will make direct the robot towards the goal point.• Just make it the next waypoint.
• Obstacle avoidance provides the “repulsion” from obstacles.• Steer a path that is a combination of repulsion from obstacle and
heading to goal.• This will require taking control back from the Navigator.
• ‘Don’t need a map.
34 / 1
![Page 35: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/35.jpg)
Potential field
35 / 1
![Page 36: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/36.jpg)
Bug algorithms
• The bug algorithms assume localization but no map.
• Here we see the first such algorithm, bug 1, working.
36 / 1
![Page 37: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/37.jpg)
Bug algorithms
• When you meet an obstacle you follow around the edge.
• Leave the obstacle at the point closest to the goal.
• Circle the obstacle to be sure that you know where this point is.
37 / 1
![Page 38: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/38.jpg)
Bug algorithms
• Here’s the second bug algorithm in action.
• Improves on the performance of bug 1
38 / 1
![Page 39: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/39.jpg)
Bug algorithms
• Follow the obstacle always on the left or right side.
• Leave the obstacle if you cross the direct (line of sight) connectionbetween start and goal.
39 / 1
![Page 40: Robotics and Autonomous Systems - cgi.csc.liv.ac.uktrp/COMP329_files/COMP329_Lecture13.pdf · Robotics and Autonomous Systems Lecture 13: Navigation in LeJOS Terry Payne Department](https://reader034.vdocument.in/reader034/viewer/2022050402/5f80140e0555aa1f0e4bc278/html5/thumbnails/40.jpg)
Summary
• Today we looked more at navigation.• Primarily we looked at the support LeJOS provides for navigation:
• Path following• Path finding
which make it possible to carry out the kind of navigation discussed inLecture 12.
• We also looked at a couple of additional navigation techniques.
40 / 1