Localizing two points using self position and angle between this points

Question

my question is about localizing points in 2D space.

I know exact position (x,y,alpha) of my robot in some room. In room there are two points of unknow location.

Robot can find angle between he and two points in room. Also robot can move on room, and find more angles between this two points and his.

How to solve this problem, when I want to discover the position of this two points in room?

I can move robot where it's needed, but what should I do to discover this positions??

Answer 1

I don't have the time at the moment to post a full answer with every step shown, but consider the following diagram:

Your robot starts at origin 1, $O_1$, and moves some distance $d$ to get to the second origin, $O_2$. Each origin has its own $<x,y>$ coordinate.

You can measure the angle to point 1, $a_1$, at the first origin. When you re-measure a new angle, $a_2$, at the second origin, you can find the second interior angle of the triangle $O_1$ $\mbox{pt}_1$ $O_2$ as $\pi-a_2$. Now, with two angles and a side, you can use the law of sines to calculate the distance from $O_1$ to $\mbox{pt}_1$, $r_{a_1}$, and the distance from $O_2$ to $\mbox{pt}_1$, $r_{a_2}$.

You can imagine this as the intersection of two circles now:

And, indeed, you can solve the problem with the formula for the intersection of two circles.

There are two intersections shown in the image above, and the circle intersection formula will also give two answers, but only one will correspond with the angles you found.

Please give this method a try. If you find you have more questions, please start a new question and link this one in the post.

Answer 2

I assume that (x,y,alpha) represents the pose of the robot with (x,y) as the origin of the robot frame with respect to the reference frame in the corner (image shown below). And alpha is the angle of the vector connecting the origin of our reference frame to the origin of robot frame (you could also call it rotation around Z axis pointing out of the screen).

For example in this diagram, the alpha angle is zero:

So if you now look closely to the figure, you find what I think is the solution.

The angle $\alpha$ shown in the figure is the angle between the robot and the unknown point in the reference XY plane. If you go from position 1 to position 2 where the angle $\alpha$ stays constant or ($180+\alpha$) and go to position 3 & 4 where you know a second angle, you can calculate the position of the point.

The point lies on the cross of the two lines connecting 1 to 2 and 3 to 4. But you have to be sure you cross the unknown point.

In the meantime while writing this, I have a second solution:

• You move in the +X direction.
• If angle is increasing, once you are at 90 $\deg$, the robot has the same $y$ value as the point.
• If it's constant you have the $y$ already.

• If it decreases you are going in the wrong direction and have to go -X direction.

• Then you repeat the same for the +Y motion.