6D localization with 6 lasers

Question

I have to know where a multi-rotor is, in a rectangular room, via 6 lasers, 2 on each axis.

The problem is like this:

Inputs :

• Room : square => 10 meters by 10 meters
• 6 positions of the lasers : Fixed on the frame
• 6 orientations of the lasers : Fixed on the frame
• The 6 measurements of the lasers
• The quaternion from the IMU of my flight controller (PixHawk).
• The origin is centered on the gravity center of the multi-rotor and defined as if the walls are perpendicular to each axes (the normal of the wall in X is (-1,0,0))

Output :

• Position in 3D (X,Y,Z)
• Angular position (quaternion)

Since I got the angular position of the multi-rotor, I rotated the laser positions and orientations via the quaternion, then extrapolate via the 6 measurements and I got the 3 walls. (orientations of the walls are trivial, then only one point is enough to determine its position.

Badly, I noticed that the yaw (rotation about z) measurement from the PixHawk is unreliable. Then I should measure the yaw from the lasers, but I do not success to do it. Event if the 2D problem is easy, I am lost in 3D.

Does someone know if it [Algorithm to know XYZ position and quaternion from 6 measurments] exists somewhere ? Or what is the right way to go on this problem ?

The question : How could I get the yaw from 2 measurements from 2 lasers which I know the original position, orientation and the pitch and roll.

NOTE : Green pointers are the origin position, Red pointers are the "final" position, but could be rotated around the red circle (due to yaw).

Answer 1

Solution : Is there another solution without prerotating vectors ?

I finally got a solution, and here it is.

Python, ROS geometry library, numpy

My actual code/maths in short :

1) Rotate the position & orientation of lasers by roll & pitch. The axes='sxyz' means : Static axis, apply roll, pitch, yaw.

quaternion_matrix creates a 4x4 transformation matrix from the quaternion.

laser = (1,1,1,0) # laser position
orientation = (1,0,0,0) # laser orientation

roll, pitch, _ = list(euler_from_quaternion(q, axes='sxyz'))
q = quaternion_from_euler(roll, pitch, 0, axes="sxyz")
laser = numpy.dot(quaternion_matrix(q), laser)
orientation = numpy.dot(quaternion_matrix(q), orientation)

2) Algebric solution : Rotation around Z in function of yaw

laser       = [-sin(a)*laser[1] + cos(t)*laser[0], 
                cos(t)*laser[1] + sin(t)*laser[0],
                laser[2]]

orientation = [-sin(a)*orientation[1] + cos(t)*orientation[0], 
                cos(t)*orientation[1] + sin(t)*orientation[0],
                orientation[2]]

3) Algebric solution : Extrapolation from the measurments in function of yaw

Important notice : Since the rotation do not scale vectors, the denominator of the K factor is a constant. Then, we can simplify it by precompute length of the orientation vector.

M = 100 # distance
K = sqrt(M^2 / (orientation[0]^2 + orientation[01]^2 + orientation[1]^2))
PointOnWall = [ K * orientation[0] + laser[0],
                K * orientation[1] + laser[1],
                K * orientation[2] + laser[2]]

4) Algebric solution : From this, on two laser, get walls.

The two "PointOnWall" equations should gives enough data to get the yaw. Knowing this is a (-1,0,0) normale, I can find 2 planes from the two points :

5) Algebric solution : Measure the YAW.

One plane in the other (Via XMaxima), we got :

def getYaw(position1, orientation1, measure1, position2, orientation2, measure2):
    length1 = length(orientation1)
    length2 = length(orientation2)
    k1 = measure1/length1
    k2 = measure2/length2
    numerator   = -k2*orientation2[0] + k1*orientation1[0] + position1[0] - position2[0]
    denominator = -k2*orientation2[1] + k1*orientation1[1] + position1[1] - position2[1]
    return atan(numerator/denominator)

As expected, roll & pitch DO NOT interfere, since the positions and orientations are prerotated.