Find the robot pose from three beacon measurement

Question

At every timestep my robot gets sensor measurements from a scanner that finds three beacons with known poses $B_1 = (B_{1x}, B_{1y})^T, B_2 = (B_{2x}^T, B_{2y}) , B_3 = (B_{3x}, B_{3y})^T$ these measurements include the distance and angle to the beacon, the measuremnt for $B_1$ would be $m_{1t} = (d_{it}, \omega_{1t})^T$. and equivalently for the other beacons.

From these measurements i want to calculate the robots pose containing its position and orientaion $x_t = (p_{xt}, p_{yt}, \Theta_{xt})^T$.

Calculating the position can be done by trilateration, but I can't seem to find a way to get the orientation of the robot from these measurements. Is there possible a model to calculate both in a single calculation? If not a solution for finding the orientation of the robot would be great.

Answer 1

If you have already computed the position of the robot, a single(!) angle towards a beacon is sufficient to get its orientation.

The computation could look roughly like

// b1 is position of beacon1, r is computed position of robot alpha = atan2(b1.x-r.x, b1.y-r.y)

So if the robot is looking towards east, the angle-measurement of B1 should alpha. So roughly alpha-b1.w is your robot's orientation.

Answer 2

You cannot directly find the direction (heading) of your robot. You only can derive the triangulated x, y positions.

However, if you have some constraints in motion dynamics (for example if your robot only moves in its body-x direction when steering is zero) you may have a time-derivative of the position and find the velocity vector.

And if you filter the velocity and position data to an estimation algorithm (I.e. Kalman filter) you may get an accurate position and velocity in time. Naturally, it does not give a solution when the robot is stationary.

Answer 3

I'm not sure where you're having the trouble with this. As you mention:

Step 1 - Find the position of the robot by trilateration with the three beacon distances.

Step 2 - Find the heading of a straight line between the location you are at and any of the three beacons

Step 3 - Find your heading by subtracting your local heading from the result of Step 2

Step 4 - Put bounds on your heading (0-360 degrees, for example).

So, for instance, say you have beacons at (x,y) positions of (1,0), (1,1), and (0,1). You trilaterate your own location to be (0,0), because the distance reading from each beacon was 1.

Great.

Now, you have to define what "heading" means to you. Is this an "engineering" heading, where 0 is the +x-axis and the angle goes positive counter-clockwise? Or is this a navigation heading where 0 is "up" (North) and the angle goes positive clockwise?

Whatever the case, let's chose to look at the beacon located at (1,0). There is a heading from your location to that beacon. If you're using navigation coordinates, it's 90 degrees (East).

Now, what is the angle of that beacon as it appears to your robot?

If you are measuring it with an angle of zero, then that means that you are pointed directly at it, which means that your heading is the same as the heading between your point and that beacon, which means that your (navigation) heading is also 90 degrees.

Say you measure it as being at -90 degrees relative to your robot (again, on a navigation setup). If the beacon is -90 degrees relative to you, then you are +90 degrees relative to the beacon. This means that your heading is whatever the heading is to the beacon, plus 90 degrees. That is, your robot's heading is 180 degrees (South).

You should be able to do this with any of the beacons and get the same answer.