Patent Application
20230280442
The present invention is related to the technical field of positioning, and more particularly to a 3D wireless optical positioning method and system.
Positioning demand is present everywhere in life. Traditional outdoor positioning techniques, such as Beidou and GPS, have meter-level positioning accuracy outdoors, but their signals will be severely attenuated when they pass through walls and other obstructions, so Beidou and GPS are not suitable for indoor positioning that requires high accuracy. In recent years, as an alternative to GPS, many new indoor positioning techniques, such as ultrasonic, Bluetooth, WiFi, visible light positioning and so on, have been continuously developed. Because of its advantages of energy saving, high accuracy, fast positioning speed, strong anti-interference and low cost, visible light positioning stands out among many indoor positioning techniques. However, due to the limitation of its positioning principle, there is still great room for improvement in 3D positioning and orientation, which limits the large-scale use of visible light positioning to a certain extent.
The common visible light positioning solutions have the following problems: in use of the scenario analysis method, the characteristic parameters of the environment where the LED light source is located need to be measured first. If the space of the area to be measured is large, the parameter measurement process requires a great amount of work. Secondly, when the environment changes, the parameters need to be re-determined, and the portability of scenario analysis method is poor.
In the traditional geometric measurement method, at least three LED light sources are needed in order to realize trilateration or triangulation, which limits the use of positioning solutions in scenarios with insufficient number of LEDs. In addition, with this method, it is difficult to determine the orientation of the receiving terminal because there is only one photoelectric detector at the receiving terminal.
In the image sensor method, because it deals with the change in projection of the light source in the image sensor, the 3D spatial information becomes the 2D plane information, and the image sensor method cannot directly calculate the height of the receiving terminal, but can only realize 2D positioning.
Therefore, current indoor visible light positioning methods are mainly intended for indoor 2D positioning, which makes it difficult to achieve accurate indoor 3D positioning, and also makes it difficult to determine the actual orientation of the receiving terminal.
It is an object of the present invention to provide a 3D wireless optical positioning method and system that enables 3D positioning and orientation of a terminal while providing illumination and enables accurate indoor 3D positioning and accurate determination of the actual orientation of the terminal even in the case of a reduced number of optical transmitters.
To address the technical problem mentioned above, the present invention provides a 3D wireless optical positioning method including the steps of:
As a further improvement of the present invention, the corresponding photodetector and LED lamp are in time synchronization in calculating the distances d1-d4.
As a further improvement of the present invention, the following equation set can be obtained for the distances d1-d4 in the XYZ coordinate system:
d
1
2=({circumflex over (x)}r1−xt1)2+(ŷr1−yt1)2+({circumflex over (z)}r1−zt1)2 (1)
d
2
2=({circumflex over (x)}r1−xt2)2+(ŷr1−yt2)2+({circumflex over (z)}r1−zt2)2 (2)
d
3
2=({circumflex over (x)}r2−xt1)2+(ŷr2−yt1)2+({circumflex over (z)}r2−zt1)2 (3)
d
4
2=({circumflex over (x)}r2−xt2)2+(ŷr2−yt2)2+({circumflex over (z)}r2−zt2)2 (4)
Meanwhile, the following supplementary equations can be obtained as the first photodetector and the second photodetector have a distance determined as l therebetween and are situated in the same receiving plane:
l
2=({circumflex over (x)}r2−{circumflex over (x)}r1)2+(ŷr2−ŷr1)2+({circumflex over (z)}r2−{circumflex over (z)}r1)2 (5)
{circumflex over (z)}
r2
={circumflex over (z)}
r1 (6)
Given the known d1, d2, d3 and d4 and the formulas (5) and (6) and in combination with the fact that the receiver is situated below the first LED lamp and the second LED lamp and the range where the receiver is to be positioned is on any side of the plane consisting of the first LED lamp, the second LED lamp and the origin, the actual position of the receiver (({circumflex over (x)}r1+{circumflex over (x)}r2)/2, (ŷr1+ŷr2)/2, ({circumflex over (z)}r1+{circumflex over (z)}r2)/2) and the orientation angle of the receiver can be obtained through solution of:
As a further improvement of the present invention, the process of solving the actual position of the receiver (({circumflex over (x)}r1+{circumflex over (x)}r2)/2, (ŷr1+ŷr2)/2, ({circumflex over (z)}r1+{circumflex over (z)}r2)/2) and the orientation angle of the receiver specifically includes the following steps:
K
i(ai,bi,ci)=(xt1+(xt2−xt1)wi/L, yt1+(yt2−yt1)wi/L, zt1+(zt2−zt1)wi/L), i=1,2 . . . (7)
R
1=√{square root over (d12−w12)}
R
2=√{square root over (d32−w22)} (8)
where wi represents the distance between the first LED lamp and the plane Pi (i=1, 2) where the two spheres intersect each other and L represents the distance between the first LED and the second LED lamp;
Φ1−Φ2=±arccos M (9)
β(R2 sin Φ2−R1 sin Φ1)=−γS (10)
where M=(R12+R22+S2−l2)/(2R1R2), the distance between the two circle centres K1 and K2 is expressed as S=√{square root over (a2+b2+c2)}, β=−√{square root over ((a2+b2))}/S, γ=c/S, a=a2−a1, b=b2−b1, c=c2−c1;
({circumflex over (x)}r1,ŷr1,{circumflex over (z)}r1)′=(ex,ey,ez)(R1 cos Φ1,R1 sin Φ1,0)′+(a1,b1,c1)′ (11)
({circumflex over (x)}r2,ŷr2,{circumflex over (z)}r2)′=(ex,ey,ez)(R2 cos Φ2,R2 sin Φ2,S)′+(a2,b2,c2)′ (12)
where
ex=(b/√{square root over (a2+b2)}, −a/√{square root over (a2+b2)}, 0)′, ey=[ac/(S√{square root over ((a2+b2))}), bc/(S√{square root over ((a2+b2))}), −√{square root over ((a2+b2)}/S]′, ez=(a/S, b/S, c/S)′ are the orthogonal basis of the first coordinate transformation.
As a further improvement of the present invention, in the step S3, four sets of solution are obtained by solving Φ1 and Φ2, and accordingly, four sets of coordinates of the first photodetector and the second photodetector in the XYZ coordinate system are obtained through two inverse coordinate transformations, whereas the actual position includes only one set, and as the four sets of solution are spatially symmetrical with respect to the line interconnecting the first LED lamp and the second LED lamp, the real solution can be obtained through determination based on the following conditions, including specifically the following steps:
where sign is the sign function.
As a further improvement of the present invention, the number of the LED lamps is defined depending on the region where they are to be positioned.
A 3D wireless optical positioning systems includes: LED lamps including a first LED lamp and a second LED lamp with a coordinate of (xt1, yt1, zt1) and (xt2, yr2, zt2) respectively, arranged on the ceiling to transmit optical information and provide illumination;
As a further improvement of the present invention, the first LED lamp and the second LED lamp and the first photodetector and the second photodetector have synchronized operation time.
As a further improvement of the present invention, the following set of equations for the distances d1 to d4 in the XYZ coordinate system is obtained based on the geometrical relationship between the LED lamps and the receiver in the XYZ coordinate system:
d
1
2=({circumflex over (x)}r1−xt1)2+(ŷr1−yt1)2+({circumflex over (z)}r1−zt1)2 (1)
d
2
2=({circumflex over (x)}r1−xt2)2+(ŷr1−yt2)2+({circumflex over (z)}r1−zt2)2 (2)
d
3
2=({circumflex over (x)}r2−xt1)2+(ŷr2−yt1)2+({circumflex over (z)}r2−zt1)2 (3)
d
4
2=({circumflex over (x)}r2−xt2)2+(ŷr2−yt2)2+({circumflex over (z)}r2−zt2)2 (4)
Meanwhile, the following supplementary equations can be obtained as the first photodetector and the second photodetector have a distance determined as l therebetween and are situated in the same receiving plane:
l
2=({circumflex over (x)}r2−{circumflex over (x)}r1)2+(ŷr2−ŷr1)2+({circumflex over (z)}r2−{circumflex over (z)}r1)2 (5)
{circumflex over (z)}
r2
={circumflex over (z)}
r1 (6)
Given the known d1, d2, d3 and d4 and the formulas (5) and (6) and in combination with the fact that the receiver is situated below the first LED lamp and the second LED lamp and the range where the receiver is to be positioned is on any side of the plane consisting of the first LED lamp, the second LED lamp and the origin, the actual position of the receiver (({circumflex over (x)}r1+{circumflex over (x)}r2)/2, (ŷr1+ŷr2)/2, ({circumflex over (z)}r1+{circumflex over (z)}r2)/2) and the orientation angle of the receiver can be obtained through solution of:
As a further improvement of the present invention, the first LED lamp, the second LED lamp, the first photodetector and the second photodetector are provided with a time synchronization device.
The present invention has the following beneficial effects. In the present invention, a small number of optical transmitters is utilized and LED lamps are used as the light source, so that the deployment is simpler, the portability is better and the limitation is reduced. The present positioning method can be applied to various indoor scenarios to achieve accurate indoor 3D positioning without the need for additional devices, such as image sensors, and by use of the pair of photodetectors, the actual orientation of the terminal can be accurately determined. For the scenario of a row of lamps indoors, the method of the present invention is particularly applicable by positioning on one side of the lamps. That is, the present method has good extensibility and good portability, and can be stably used in various indoor scenarios.
The invention will be further explained with reference to the following drawings and particular embodiments, so that those skilled in the art can better understand and implement the present invention. However, the listed embodiments should not be taken as limitation of the present invention.
Referring to
Specifically, it is proposed that the system consists of two elements as a transmitter and a receiver. The transmitter includes two LED lamps installed on the ceiling, LED1 and LED2, with a coordinate of (xt1, yt1, zt1) and (xt2, yt2, zt2) respectively, that can provide illumination. The plane where the receiver is situated is the receiving plane. A pair of (two) photodetectors is installed on the receiver. The two photodetectors are PD1 and PD2 respectively, with a coordinate of ({circumflex over (x)}r1, ŷr1, {circumflex over (z)}r1) and ({circumflex over (x)}r2, ŷr2, {circumflex over (z)}r2) respectively, configured to receive the optical information transmitted from the light source. The distance between PD1 and PD2 is l, and PD1 and PD2 both face upwards. The middle point between PD1 and PD2 defines the actual position of the receiving terminal to be predicted. The direction from PD1 to PD2 defines the orientation of the receiving terminal. The included angle between the line interconnecting PD1 and PD2 and the positive half of the X axis is defined as the orientation angle η.
Positioning: through the TOA (Time of Arrival) principle, the time required for the optical signal to be transmitted from LED1 and LED2 to and received by PD1 and PD2 respectively is measured, and the propagation time is multiplied by the speed of light to calculate the distance d1 between LED1 and PD1, the distance d2 between LED2 and PD1, the distance d3 between LED1 and PD2 and the distance d4 between LED2 and PD2. The following set of equations is obtained:
d
1
2=({circumflex over (x)}r1−xt1)2+(ŷr1−yt1)2+({circumflex over (z)}r1−zt1)2 (1)
d
2
2=({circumflex over (x)}r1−xt2)2+(ŷr1−yt2)2+({circumflex over (z)}r1−zt2)2 (2)
d
3
2=({circumflex over (x)}r2−xt1)2+(ŷr2−yt1)2+({circumflex over (z)}r2−zt1)2 (3)
d
4
2=({circumflex over (x)}r2−xt2)2+(ŷr2−yt2)2+({circumflex over (z)}r2−zt2)2 (4)
Meanwhile, the following supplementary equations can be obtained as PD1 and PD2 have a distance determined as l therebetween and are situated in the same receiving plane:
l
2=({circumflex over (x)}r2−{circumflex over (x)}r1)2+(ŷr2−ŷr1)2+({circumflex over (z)}r2−{circumflex over (z)}r1)2 (5)
{circumflex over (z)}
r2
={circumflex over (z)}
r1 (6)
The equation set including formulas (1)-(6) is solved by the following steps:
First step: as shown in
K
i(ai,bi,ci)=(xt1+(xt2−xt1)wi/L, yt1+(yt2−yt1)wi/L, zt1+(zt2−zt1)wi/L), i=1,2 . . . (7)
R
1=√{square root over (d12−w12)}
R
2=√{square root over (d32−w22)} (8)
where wi represents the distance between LED1 and the plane Pi (i=1, 2) where the two spheres intersect each other and L represents the distance between LED1 and LED2.
Second step: as shown in
Φ1−Φ2=±arcos M (9)
β(R2 sin Φ2−R1 sin Φ1)=−γS (10)
where M=(R12+R22+S2−l2)/(2R1R2), the distance between the two circle centres K1 and K2 is expressed as S=√{square root over (a2+b2+c2)}, β=−√{square root over ((a2+b2))}/S, γ=c/S, a=a2−a1, b=b2−b1, c=c2−c1.
Third step: Φ1 and Φ2 are calculated according to the set of equations (9) and (10), then two inverse coordinate transformations are performed to recover the coordinates of PD1 and PD2 in the XYZ coordinate system through the formulas (11) and (12).
({circumflex over (x)}r1,ŷr1,{circumflex over (z)}r1)′=(ex,ey,ez)(R1 cos Φ1,R1 sin Φ1,0)′+(a1,b1,c1)′ (11)
({circumflex over (x)}r2,ŷr2,{circumflex over (z)}r2)′=(ex,ey,ez)(R2 cos Φ2,R2 sin Φ2,S)′+(a2,b2,c2)′ (12)
where
ex=(b/√{square root over (a2+b2)}, −a/√{square root over (a2+b2)}, 0)′, ey=[ac/(S√{square root over ((a2+b2))}), bc/(S√{square root over ((a2+b2))}), −√{square root over ((a2+b2)}/S]′, ez=(a/S, b/S, c/S)′ are the orthogonal basis of the first coordinate transformation.
Fourth step: in the step S3, four sets of solution are obtained by solving Φ1 and Φ2, and accordingly, four sets of coordinates of PD1 and PD2 in the XYZ coordinate system are obtained through two inverse coordinate transformations, whereas the actual position includes only one set. As the four sets of solution are spatially symmetrical with respect to the line interconnecting LED1 and LED2, the real solution can be obtained through determination based on the following conditions, including specifically the following steps:
where sign is the sign function.
It is noted that, in the present invention, the range where the terminal is to be positioned is on any side of the plane consisting of LED1, LED2 and the origin.
According to the present invention, accurate 3D positioning and orientation of the terminal can be achieved. Only two LED lamps are used at the transmitting terminal to enable 3D positioning and orientation while providing illumination. A pair of (2 in total) photodetectors is installed on the terminal to receive signals. These two photodetectors are positioned at the same level in the same receiving plane and have a constant and known distance therebetween. According to the present invention, through the TOA (Time of Arrival) principle, the time required for the optical signal to be transmitted from LED1 and LED2 to and received by PD1 and PD2 respectively is measured and the propagation time is multiplied by the speed of light to calculate the distance d1 between LED1 and PD1, the distance d2 between LED2 and PD1, the distance d3 between LED1 and PD2 and the distance d4 between LED2 and PD2. Positioning is achieved by the calculation steps in the formulas (1) to (13) described above based on the geometrical relationship between the transmitter and the receiver.
The present invention further provides a 3D wireless optical positioning system, including:
LED lamps including a first LED lamp and a second LED lamp with a coordinate of (xt1, yt1, zt1) and (xt2, yt2, zt2) respectively, arranged on the ceiling to transmit optical information and provide illumination;
To evaluate the performance of the proposed 3D wireless optical positioning method and system, a specific indoor space scenario of a size of 3 m×5 m×3 m (length×width×height) is considered for positioning. The LED lamps are deployed on the ceiling. LED1 has a coordinate of (0, 1.5, 3) and LED2 has a coordinate of (0, 3.5, 3). The distance between the transmitting terminal and the receiving terminal is estimated by using the TOA method. Assuming that a random error Δd is present in distance estimation for d1, d2, d3 and d4 measured through TOA, and the individual random errors are independent and subject to normal distribution. The mobile terminal is positioned on the receiving plane and has a orientation angle (orientation) that is randomly distributed. Tests are conducted at various positions at an interval of 0.5 m, as shown by the cross line in
The embodiments described above are only preferred embodiments listed for fully explaining the present invention, and the scope of protection of the present invention is not limited thereto. Equivalent substitutions or changes made by those skilled in the art on the basis of the present invention shall fall within the scope of protection of the present invention. The scope of protection of the present invention is defined by the claims.
| Number | Date | Country | Kind |
|---|---|---|---|
| 202111016290.5 | Aug 2021 | CN | national |
This application is the National Stage Application of PCT/CN2021/116639, filed on Sep. 6, 2021, which claims priority to Patent Application No. 202111016290.5, filed on Aug. 31, 2021, which is incorporated by reference for all purposes as if fully set forth herein.
| Filing Document | Filing Date | Country | Kind |
|---|---|---|---|
| PCT/CN2021/116639 | 9/6/2021 | WO |
