2d trilateration

问题

I am writing some code to participate in an AI challenge. The main objective for the AI challenge is to take a simulated robot and navigate it through a maze to a destination zone. The secondary objective which is optional is to find a recharger placed in the maze at an unknown location. This is all done in a 2D grid.

My program can call a method to get a distance measurement from the recharger. So using trilateration I should be able to locate the recharger by calling this method, recording my ai's current position and the distance the recharger is away from that point 3 times over.

I found this example of trilateration on wikipedia http://en.wikipedia.org/wiki/Trilateration but this applies to a 3d space. I'm only dealing with a 2D space. Also I don't understand how to use the formula shown in Wikipedia, searching the web for a working example with numbers plugged in and boiling down to the final coordinates is scarce with Google searches.

I'm not a math major; I am just an enthusiast exploring AI problems.

An explanation and step by step example of how to calculate the problem is what I need as mathematics are not my strong point. Below is some sample data:

• Point 1: x=39, y=28, distance=8
• Point 2: x=13, y=39, distance=11
• Point 3: x=16, y=40, distance=8

Any example using my sample data would be greatly appreciated. The programming to this will be very straight forward once I can wrap my head around the mathematics.

回答1:

As the Wikipedia trilateriation article describes, you compute (x,y) coordinates by successively calculating: e_x, i, e_y, d, j, x, y. You have to be familiar with vector notation, so, for example, e_x = (P2 - P1) / ‖P2 - P1‖ means:

• e_x,x = (P2_x - P1_x) / sqrt((P2_x - P1_x)² + (P2_y - P1_y)²)
• e_x,y = (P2_y - P1_y) / sqrt((P2_x - P1_x)² + (P2_y - P1_y)²)

Your data is:

• P1 = (39, 28); r₁ = 8
• P2 = (13, 39); r₂ = 11
• P3 = (16, 40); r₃ = 8

The calculation steps are:

1. e_x = (P2 - P1) / ‖P2 - P1‖
2. i = e_x(P3 - P1)
3. e_y = (P3 - P1 - i · e_x) / ‖P3 - P1 - i · e_x‖
4. d = ‖P2 - P1‖
5. j = e_y(P3 - P1)
6. x = (r₁² - r₂² + d²) / 2d
7. y = (r₁² - r₃² + i² + j²) / 2j - ix / j

来源：https://stackoverflow.com/questions/9747227/2d-trilateration