Find location using only distance and bearing?

Triangulation works by checking your angle to three KNOWN targets.

"I know the that's the Lighthouse of Alexandria, it's located here (X,Y) on a map, and it's to my right at 90 degrees." Repeat 2 more times for different targets and angles.

Trilateration works by checking your distance from three KNOWN targets.

"I know the that's the Lighthouse of Alexandria, it's located here (X,Y) on a map, and I'm 100 meters away from that." Re开发者_运维问答peat 2 more times for different targets and ranges.

But both of those methods rely on knowing WHAT you're looking at.

Say you're in a forest and you can't differentiate between trees, but you know where key trees are. These trees have been hand picked as "landmarks."

You have a robot moving through that forest slowly.

Do you know of any ways to determine location based solely off of angle and range, exploiting geometry between landmarks? Note, you will see other trees as well, so you won't know which trees are key trees. Ignore the fact that a target may be occluded. Our pre-algorithm takes care of that.

1) If this exists, what's it called? I can't find anything.

2) What do you think the odds are of having two identical location 'hits?' I imagine it's fairly rare.

3) If there are two identical location 'hits,' how can I determine my exact location after I move the robot next. (I assume the chances of having 2 occurrences of EXACT angles in a row, after I reposition the robot, would be statistically impossible, barring a forest growing in rows like corn). Would I just calculate the position again and hope for the best? Or would I somehow incorporate my previous position estimate into my next guess?

If this exists, I'd like to read about it, and if not, develop it as a side project. I just don't have time to reinvent the wheel right now, nor have the time to implement this from scratch. So if it doesn't exist, I'll have to figure out another way to localize the robot since that's not the aim of this research, if it does, lets hope it's semi-easy.

Great question.

1. The name of the problem you're investigating is localization, and it, together with mapping, are two of the most important and challenging problems in robotics at the moment. Put simply, localization is the problem of "given some sensor observations how do I know where I am?"

2. Landmark identification is one of the hidden 'tricks' that underpin so much of the practice of robotics. If it isn't possible to uniquely identify a landmark, you can end up with a high proportion of misinformation, particularly given that real sensors are stochastic (ie/ there will be some uncertainty associate with the result). Your choice of an appropriate localisation method, will almost certainly depend on how well you can uniquely identify a landmark, or associate patterns of landmarks with a map.

3. The simplest method of self-localization in many cases is Monte Carlo localization. One common way to implement this is by using particle filters. The advantage of this is that they cope well when you don't have great models of motion, sensor capability and need something robust that can deal with unexpected effects (like moving obstacles or landmark obscuration). A particle represents one possible state of the vehicle. Initially particles are uniformly distributed, as the vehicle moves and add more sensor observations are incorporated. Particle states are updated to move away from unlikely states - in the example given, particles would move away from areas where the range / bearings don't match what should be visible from the current position estimate. Given sufficient time and observations particles tend to clump together into areas where there is a high probability of the vehicle being located. Look up the work of Sebastian Thrun, particularly the book "probabilistic robotics".

What you're looking for is Monte Carlo localization (also known as a particle filter). Here's a good resource on the subject.

Or nearly anything from the probabilistic robotics crowd, Dellaert, Thrun, Burgard or Fox. If you're feeling ambitious, you could try to go for a full SLAM solution - a bunch of libraries are posted here.

Or if you're really really ambitious, you could implement from first principles using Factor Graphs.

I assume you want to start by turning on the robot inside the forest. I further assume that the robot can calculate the position of every tree using angle and distance.

Then you can identify the landmarks by iterating through the trees and calculating the distance to all its neighbours. In Matlab you can use pdist to get a list of all (unique) pairwise distances.

Then you can iterate through the trees to identify landmarks. For every tree, compare the distances to all its neighbours to the known distances between landmarks. Whenever you find a candidate landmark, you check its possible landmark neighbours for the correct distance signature. Since you say that you always should be able to see five landmarks at any given time, you will be trying to match 20 distances, so I'd say that the chance of false positives is not too high. If the candidate landmark and its candidate fellow landmarks do not match the complete relative distance pattern, you go check the next tree.

Once you have found all the landmarks, you simply triangulate.

Note that depending on how accurately you can measure angles and distances, you need to be able to see more landmark trees at any given time. My guess is that you need to space landmarks with sufficiently density that you can see at least three at a time if you have high measurement accuracy.

I guess you need only distance to two landmarks and the order of seeing them (i.e. from left to right you see point A and B)

• (1) "Robotic mapping" and "perceptual aliasing".
• (2) Two identical hits are inevitable. Since the robot can only distinguish between a finite number X of distinguishable tree configurations, even if the configurations are completely random, there is almost certainly at least one location that looks "the same" as some other location even if you encounter far fewer than X/2 different trees. Those are called "birthday paradox collisions". You may be lucky that the particular location you are at is in fact actually unique, but I wouldn't bet my robot on it.

So you:

• (a) have a map of a large area with some, but not all trees on it.
• (b) a robot somewhere in the actual forest that, without looking at the map, has looked at the nearby trees and generated an internal map of a all the trees in a tiny area and its relative position to them
• (c) To the robot, every tree looks the same as every other tree.
• You want to find: Where is the robot on the large map?

If only each actual tree had a unique name written on it that the robot could read, and then (some of) those trees and their names were on the map, this would be trivial.

One approach is to attach a (not necessarily unique) "signature" to each tree that describes its position relative to nearby trees.

Then, as you travel along, the robot drives up to a tree and finds a "signature" for that tree, and you find all the trees on the map that "match" that signature. If only one unique tree on the map matches, then the tree the robot is looking might be that tree on the map (yay, you know where the robot is) -- put down a weighty but tentative dot on the map at the robot's relative position to the matching tree -- the tree the robot is next to is certainly not any of the other trees on the map. If several of the trees on the map match -- they all have the same non-unique signature -- then you could put some less-weighty tentative dots on the map at the robots position relative to each one of them. Alas, even if find one or more matches, it is still possible that the tree the robot is looking at is not on the map at all, and the signature of that tree is coincidentally the same as one or more trees on the map, and so the robot could be anywhere on the map. If none of the trees on the map matches, then the tree the robot is looking at is definitely not on the map. (Perhaps later on, once the robot knows exactly where it is, it should start adding these trees to the map?)

As you drive down the path, you push the dots in your estimated direction and speed of travel.

Then as you inspect other trees, possibly after driving down the path a little further, you eventually have lots of dots on the map, and hopefully one heavy, highly overlapping cluster at the actual position, and hopefully each other dot is an easily-ignored isolated coincidences.

The simplest signature is a list of distances from a particular tree to nearby trees. A particular tree on the map is "matched" to a particular tree in the forest when, for each and every nearby tree on the map, there is a corresponding nearby tree in the forest at "the same" distance, as far as you can tell with your known distance and angular errors.

(By "nearby", I mean "close enough that the robot should be able to definitely confirm that the tree is actually there", although it's probably simpler to approximate this with something like "My robot can see all trees out to a range of R, so I'm only going to bother even trying to match trees that are within a circle of R*1/3 from my robot, and my list of distances only include trees that are within a circle of R*2/3 from the particular tree I'm trying to match").

If you know your north-south orientation even very roughly, you can create signatures that are "more unique", i.e., have fewer spurious matches on the map and (hopefully) in the real forest. A "match" for the tree the robot is next to occurs when, for each nearby tree on the map, there is a corresponding tree in the forest at "the same" distance and direction, as far as you can tell with your known distance and angular errors. Say you see that tree "Fred" on the map has another tree 10 meters in the N to W quadrant from it, but the robot is next to a tree that definitely doesn't have any trees at that distance in the N to W quadrant, but it has a tree 10 meters away to the South. In that case, then (using a more complex signature) you can definitely tell the robot is not next to Fred, even though the simple signature would give a (false) match.

Another approach: The "digital paper" solves a similar problem ... Can you plant a few trees in a pattern that is specifically designed to be easily recognized?