阅读说明：本技术 基于两盏led灯及角度传感器的定位算法及定位系统 (Positioning algorithm and positioning system based on two LED lamps and angle sensor ) 是由 安富通 关伟鹏 伍文飞 于 2020-07-20 设计创作，主要内容包括：本发明公开一种基于两盏LED灯及角度传感器的定位算法及定位系统,通过拍摄两盏LED灯得到图片,对捕获的所述两盏LED灯进行解码,获取所述两盏LED灯的位置信息,并计算所述两盏LED灯的几何参数信息,根据两盏LED灯、摄像头、两盏LED灯的成像椭圆三者之间的几何关系,结合两盏LED灯的位置信息以及两盏LED灯的几何参数信息,并根据两盏LED灯的成像平面与水平面之间的夹角,求得摄像头中心的Z坐标,根据求得的摄像头中点的Z坐标,计算得到摄像头中点的X坐标与Y坐标。本发明基于双灯进行定位,接收端在水平或倾斜状态均可实现三维定位,定位精确,丰富了现有的定位算法,拓展了应用场景,有广阔的市场价值。(The invention discloses a positioning algorithm and a positioning system based on two LED lamps and an angle sensor, wherein pictures are obtained by shooting the two LED lamps, the two captured LED lamps are decoded to obtain position information of the two LED lamps, geometric parameter information of the two LED lamps is calculated, the Z coordinate of the center of a camera is obtained according to the geometric relationship among the two LED lamps, the camera and imaging ellipses of the two LED lamps, the position information of the two LED lamps and the geometric parameter information of the two LED lamps are combined, the included angle between the imaging plane of the two LED lamps and the horizontal plane is obtained, and the X coordinate and the Y coordinate of the midpoint of the camera are obtained by calculation according to the obtained Z coordinate of the midpoint of the camera. The invention carries out positioning based on double lamps, the receiving end can realize three-dimensional positioning in a horizontal or inclined state, the positioning is accurate, the existing positioning algorithm is enriched, the application scene is expanded, and the market value is wide.)

1. A positioning algorithm based on two LED lamps and an angle sensor is characterized by comprising the following steps:

s1: shooting two LED lamps to obtain pictures, decoding the two captured LED lamps to obtain position information of the two LED lamps, and calculating geometric parameter information of the two LED lamps;

s2: according to the geometric relationship among the two LED lamps, the camera and the imaging ellipses of the two LED lamps, combining the position information of the two LED lamps and the geometric parameter information of the two LED lamps in the step S1, and according to the included angle between the imaging planes of the two LED lamps and the horizontal plane, obtaining the Z coordinate of the center of the camera;

s3: and according to the geometric relationship among the two LED lamps, the camera and the imaging ellipses of the two LED lamps, combining the Z coordinate of the midpoint of the camera obtained in the step S2, and calculating to obtain the X coordinate and the Y coordinate of the midpoint of the camera.

2. The two LED lamp and angle sensor based positioning algorithm of claim 1, wherein in step S1, the camera takes two LED lamps to obtain a picture, decodes the captured two LED lamps, and obtains the position information of the two LED lamps, including:

s1.1: at the emitting end of the LED lamp, the frequency and the phase difference of input PWM are controlled by utilizing a PWM modulation mode to obtain LED lamps with different IDs, wherein the specific forms of the IDs are alternate black and white stripes, the IDs comprise the thickness of the stripes and the number of the stripes, and the IDs of different LED lamps show different characteristics;

s1.2: shooting two LED lamps to obtain pictures;

s1.3: processing the shot picture through an image processing technology, and decoding to obtain the area, the white stripe number, the black-white stripe duty ratio characteristics of the two LED lamps and the geometric position information of the two LED lamps in the shot picture respectively;

s1.4: and matching the acquired characteristic information of the two LED lamps with the IDs of the LED lamps in a pre-established local database to obtain specific position coordinates (x1, y1,0) and (x2, y2,0) of the two photographed LED lamps.

3. The two LED lamp and angle sensor based positioning algorithm of claim 2, wherein in step S1, the calculating the geometric parameter information of the two LED lamps comprises:

calculating the geometric parameter information of the two LED lamps by an image processing technology;

the geometric parameter information includes: the center position coordinates (i1, j1), (i2, j2) of the imaging ellipses of the two LED lamps, the distances d1 and d2 from the centers of the imaging ellipses of the two LED lamps to the center of an imaging plane, the lengths a1 and a2 of the semi-major axes of the imaging ellipses of the two LED lamps and the radius r of the LED lamps.

4. The two-LED-lamp-and-angle-sensor-based positioning algorithm of claim 3, wherein the step S2 comprises:

s2.1: obtaining an inclination angle t of the camera by using an angle sensor, wherein the inclination angle t is an included angle between an imaging plane and a horizontal plane;

s2.2: calculating the vertical distance h between the center of a lens of the camera and the horizontal plane where the long axis of the imaging ellipse of each LED lamp is located;

s2.3: and obtaining the Z coordinate of the midpoint of the camera according to the fact that the ratio of the vertical distance from the center of the lens to the plane where the LED and the like are located to the vertical distance h from the center of the lens to the horizontal plane where the long axis of the imaging ellipse is located is equal to the ratio of the radius of the LED lamp to the semi-long axis of the imaging ellipse.

5. The two LED lamp and angle sensor based positioning algorithm of claim 4, wherein step S2.2 comprises:

s2.2.1: when the imaging ellipse of the LED lamp is located on the high side of the tilted imaging plane,

h＝(f-d·tant(t))·cost；

when the imaging ellipse of the LED lamp is located at the low side of the tilted imaging plane,

h＝(d·tant(t)+f)·cost；

wherein f is the focal length of the lens, and d is the distance from the center of the imaging ellipse of the LED lamp to the center of the imaging plane;

s2.2.2: since it is not known whether the imaging ellipses of the two LED lamps are located on the high side or the low side of the imaging plane, four h are calculated, h11, h12, h21, and h22, respectively.

6. The two LED lamp and angle sensor based positioning algorithm of claim 5, wherein step 2.3 comprises:

s2.3.1: according to the fact that the ratio of the vertical distance between the center of the lens and the plane where the LED and the like are located to the vertical distance h between the center of the lens and the horizontal plane where the long axis of the imaging ellipse is located is equal to the ratio of the radius of the LED lamp and the semi-long axis of the imaging ellipse, the following four formulas are obtained by combining four h calculated in step S2.2.2:

obtaining four Z coordinate values Z1, Z2, Z3 and Z4 of the center of the camera according to the four formulas;

s2.3.2: and performing difference on four Z coordinate values Z1, Z2, Z3 and Z4 at the center of the camera in pairs, taking an absolute value, averaging two Z coordinates with the minimum absolute value of the difference, and taking the average value as the Z coordinate at the center of the camera.

7. The two LED lamp and angle sensor based positioning algorithm of claim 1, wherein step S3 comprises:

s3.1: according to the geometric relationship, the ratio of the radius of the LED lamp to the semi-major axis of the imaging ellipse is equal to the ratio of the distance from the center of the lens to the center of the LED lamp to the distance from the center of the lens to the center of the imaging ellipse, and two formulas are obtained:

s3.2: the two formulas are combined and substituted into the Z coordinate value obtained in step S2 to obtain the X and Y coordinate values of the camera center.

8. The two LED lamp and angle sensor based positioning algorithm of claim 7, wherein step 3.2 comprises:

s3.2.1: equations 1 and 2 correspond to two circles with centers of (X1, Y1) and (X2, Y2), two intersecting points of the two circles are usually marked as (X1, Y1), (X2, Y2), and coordinates of (X1, Y1), (X2, Y2) are obtained by combining equations 1 and 2, wherein the coordinate of only one intersecting point is the X, Y coordinate of the camera;

s3.2.2: the vector of the center coordinates (X2, Y2) pointing to (X1, Y1) of the two LED lamps is defined as vector 1, two intersection points (X1, Y1), (X2, Y2) are subjected to rotation conversion processing, an included angle between the vector 1 and an X axis is represented as an angle 0, the opposite number of the angle 0 is taken as an angle t, and the coordinates of (X11, Y11), (X22, Y22) are obtained by a rotation angle formula which is as follows:

wherein (X11, Y11) is new coordinates after (X1, Y1) is subjected to rotation transformation, and (X22, Y22) is new coordinates after (X2, Y2) is subjected to rotation transformation;

if Y11 is greater than Y22, (X1, Y1) is located on the left side of vector 1, (X2, Y2) is located on the right side of vector 1; if Y11 is less than Y22, (X1, Y1) is located on the right side of vector 1, (X2, Y2) is located on the left side of vector 1;

s3.2.3: converting an angle 0 into an included angle between a vector 1 and the positive direction of a Y axis, recording the included angle as an angle 1 in a range of 0-360 degrees, measuring an AZIMUTH angle of a receiving end by an angle sensor, namely an included angle between the receiving end and the positive north direction, recording the included angle as an angle 2 in a range of 0-360 degrees, recording an included angle between the positive direction of the Y axis of a specified coordinate axis and the positive north direction of geography as an angle 3 in a range of 0-360 degrees, performing operation processing on the angle 1 and the angle 3 to obtain an included angle between the vector 1 and the positive north direction, recording the included angle as an angle 4, and acquiring an ROLL angle of a camera by the angle sensor in a range;

s3.2.4: in case of a ROLL angle > 0: if the absolute value of the difference value between the angle 4 and the angle 2 is between 90 degrees and 270 degrees, selecting a coordinate point on the left side from the final positioning coordinate of the camera, and otherwise, selecting the coordinate on the right side as the final positioning coordinate of the camera;

in case of a ROLL angle < 0: if the absolute value of the difference value between the angle 4 and the angle 2 is between 90 degrees and 270 degrees, the coordinate point on the right side is selected as the final positioning coordinate of the camera, and otherwise, the coordinate on the left side is selected as the final positioning coordinate of the camera.

9. A positioning system comprising a receiving end, a processor and a computer program stored in and executable on the processor, the processor implementing the positioning algorithm based on two LED lamps and an angle sensor according to any one of claims 1 to 8 when executing the computer program;

the receiving end comprises a camera and an angle sensor, and the camera is used for shooting two LED lamps to obtain pictures;

the angle sensor is used for sensing the inclination angle of the camera.

Technical Field

The invention relates to the technical field of visible light communication, in particular to a positioning algorithm and a positioning system based on two LED lamps and an angle sensor.

Background

The visible light communication technology utilizes modulated high-frequency flickering LED lamps which cannot be distinguished by naked eyes, and can be realized by modifying the existing illuminating lamps. Through the continuous improvement to traditional technique for transmission speed constantly accelerates, and construction cost constantly reduces, receives interference degree littleer and more, and the security is high moreover, and the application scene is more and more extensive.

The visible light communication positioning technology is a good embodiment of the technical characteristics. Nowadays, CMOS image sensors are embedded in various types of electronic devices, especially smart phone terminals, and are used as light receiving devices in visible light imaging communication to quickly capture information and complete wireless transmission of the information. And then, the technology of positioning by utilizing the visible light by utilizing the space geometric relationship is generated.

However, the LED lamp is used for positioning, the effect is poor under the single lamp technology, and the three-dimensional space positioning cannot be realized at present. The LED lamps with more requirements for the three-lamp positioning technology have the problems of large calculated amount, difficulty in capturing complete images and the like. However, all the existing double-lamp positioning technologies only realize positioning of the receiving end in a horizontal state, so that the application scenarios are limited and cannot be popularized.

Therefore, it is necessary to explore a new two-lamp positioning technology, especially to realize three-dimensional positioning in a general state, i.e. a receiving end is in an arbitrary inclined state.

Disclosure of Invention

The invention aims to overcome the defects of the prior art and provides a positioning algorithm and a positioning system based on two LED lamps and an angle sensor, and the accurate positioning of a receiving end in an inclined state is realized based on two lamps.

The technical scheme of the invention is as follows: a positioning algorithm based on two LED lamps and an angle sensor is provided, which comprises the following steps:

s1: shooting two LED lamps to obtain pictures, decoding the two captured LED lamps to obtain position information of the two LED lamps, and calculating geometric parameter information of the two LED lamps;

s2: according to the geometric relationship among the two LED lamps, the camera and the imaging ellipses of the two LED lamps, combining the position information of the two LED lamps and the geometric parameter information of the two LED lamps in the step S1, and according to the included angle between the imaging planes of the two LED lamps and the horizontal plane, obtaining the Z coordinate of the center of the camera;

s3: and according to the geometric relationship among the two LED lamps, the camera and the imaging ellipses of the two LED lamps, combining the Z coordinate of the midpoint of the camera obtained in the step S2, and calculating to obtain the X coordinate and the Y coordinate of the midpoint of the camera.

Further, in step S1, the capturing two LED lamps by the camera to obtain pictures, decoding the captured two LED lamps, and obtaining position information of the two LED lamps, including:

s1.1: at the emitting end of the LED lamp, the frequency and the phase difference of input PWM are controlled by utilizing a PWM modulation mode to obtain LED lamps with different IDs, wherein the specific forms of the IDs are alternate black and white stripes, the IDs comprise the thickness of the stripes and the number of the stripes, and the IDs of different LED lamps show different characteristics;

s1.2: shooting two LED lamps to obtain pictures;

s1.3: processing the shot picture through an image processing technology, and decoding to obtain the area, the white stripe number, the black-white stripe duty ratio characteristics of the two LED lamps and the geometric position information of the two LED lamps in the shot picture respectively;

s1.4: and matching the acquired characteristic information of the two LED lamps with the IDs of the LED lamps in a pre-established local database to obtain specific position coordinates (x1, y1,0) and (x2, y2,0) of the two photographed LED lamps.

Further, in step S1, the calculating geometric parameter information of the two LED lamps includes:

calculating the geometric parameter information of the two LED lamps by an image processing technology;

the geometric parameter information includes: the center position coordinates (i1, j1), (i2, j2) of the imaging ellipses of the two LED lamps, the distances d1 and d2 from the centers of the imaging ellipses of the two LED lamps to the center of an imaging plane, the lengths a1 and a2 of the semi-major axes of the imaging ellipses of the two LED lamps and the radius r of the LED lamps.

Further, step S2 includes:

s2.1: obtaining an inclination angle t of the camera by using an angle sensor, wherein the inclination angle t is an included angle between an imaging plane and a horizontal plane;

s2.2: calculating the vertical distance h between the center of a lens of the camera and the horizontal plane where the long axis of the imaging ellipse of each LED lamp is located;

s2.3: and obtaining the Z coordinate of the midpoint of the camera according to the fact that the ratio of the vertical distance from the center of the lens to the plane where the LED and the like are located to the vertical distance h from the center of the lens to the horizontal plane where the long axis of the imaging ellipse is located is equal to the ratio of the radius of the LED lamp to the semi-long axis of the imaging ellipse.

Further, step S2.2 comprises:

s2.2.1: when the imaging ellipse of the LED lamp is located on the high side of the tilted imaging plane,

h＝(f-d·tant(t))·cost；

when the imaging ellipse of the LED lamp is located at the low side of the tilted imaging plane,

h＝(d·tant(t)+f)·cost；

wherein f is the focal length of the lens, and d is the distance from the center of the imaging ellipse of the LED lamp to the center of the imaging plane;

s2.2.2: since it is not known whether the imaging ellipses of the two LED lamps are located on the high side or the low side of the imaging plane, four h are calculated, h11, h12, h21, and h22, respectively.

Further, step 2.3 comprises:

s2.3.1: according to the fact that the ratio of the vertical distance between the center of the lens and the plane where the LED and the like are located to the vertical distance h between the center of the lens and the horizontal plane where the long axis of the imaging ellipse is located is equal to the ratio of the radius of the LED lamp and the semi-long axis of the imaging ellipse, the following four formulas are obtained by combining four h calculated in step S2.2.2:

obtaining four Z coordinate values Z1, Z2, Z3 and Z4 of the center of the camera according to the four formulas;

s2.3.2: and performing difference on four Z coordinate values Z1, Z2, Z3 and Z4 at the center of the camera in pairs, taking an absolute value, averaging two Z coordinates with the minimum absolute value of the difference, and taking the average value as the Z coordinate at the center of the camera.

Further, step S3 includes:

s3.1: according to the geometric relationship, the ratio of the radius of the LED lamp to the semi-major axis of the imaging ellipse is equal to the ratio of the distance from the center of the lens to the center of the LED lamp to the distance from the center of the lens to the center of the imaging ellipse, and two formulas are obtained:

s3.2: the two formulas are combined and substituted into the Z coordinate value obtained in step S2 to obtain the X and Y coordinate values of the camera center.

Further, step 3.2 comprises:

s3.2.1: equations 1 and 2 correspond to two circles with centers of (X1, Y1) and (X2, Y2), two intersecting points of the two circles are usually marked as (X1, Y1), (X2, Y2), and coordinates of (X1, Y1), (X2, Y2) are obtained by combining equations 1 and 2, wherein the coordinate of only one intersecting point is the X, Y coordinate of the camera;

s3.2.2: the vector of the center coordinates (X2, Y2) pointing to (X1, Y1) of the two LED lamps is defined as vector 1, two intersection points (X1, Y1), (X2, Y2) are subjected to rotation conversion processing, an included angle between the vector 1 and an X axis is represented as an angle 0, the opposite number of the angle 0 is taken as an angle t, and the coordinates of (X11, Y11), (X22, Y22) are obtained by a rotation angle formula which is as follows:

wherein (X11, Y11) is new coordinates after (X1, Y1) is subjected to rotation transformation, and (X22, Y22) is new coordinates after (X2, Y2) is subjected to rotation transformation;

if Y11 is greater than Y22, (X1, Y1) is located on the left side of vector 1, (X2, Y2) is located on the right side of vector 1; if Y11 is less than Y22, (X1, Y1) is located on the right side of vector 1, (X2, Y2) is located on the left side of vector 1;

s3.2.3: converting an angle 0 into an included angle between a vector 1 and the positive direction of a Y axis, recording the included angle as an angle 1 in a range of 0-360 degrees, measuring an AZIMUTH angle of a receiving end by an angle sensor, namely an included angle between the receiving end and the positive north direction, recording the included angle as an angle 2 in a range of 0-360 degrees, recording an included angle between the positive direction of the Y axis of a specified coordinate axis and the positive north direction of geography as an angle 3 in a range of 0-360 degrees, performing operation processing on the angle 1 and the angle 3 to obtain an included angle between the vector 1 and the positive north direction, recording the included angle as an angle 4, and acquiring an ROLL angle of a camera by the angle sensor in a range;

s3.2.4: in case of a ROLL angle > 0: if the absolute value of the difference value between the angle 4 and the angle 2 is between 90 degrees and 270 degrees, selecting a coordinate point on the left side from the final positioning coordinate of the camera, and otherwise, selecting the coordinate on the right side as the final positioning coordinate of the camera;

in case of a ROLL angle < 0: if the absolute value of the difference value between the angle 4 and the angle 2 is between 90 degrees and 270 degrees, the coordinate point on the right side is selected as the final positioning coordinate of the camera, and otherwise, the coordinate on the left side is selected as the final positioning coordinate of the camera.

The invention also provides a positioning system, which comprises a receiving end, a processor and a computer program which is stored in the processor and can run on the processor, wherein the processor realizes the positioning algorithm and the positioning system based on the two LED lamps and the angle sensor when executing the computer program;

the receiving end comprises a camera and an angle sensor, and the camera is used for shooting two LED lamps to obtain pictures;

the angle sensor is used for sensing the inclination angle of the camera.

By adopting the scheme, the invention carries out positioning based on double lamps, the receiving end can realize three-dimensional positioning in a horizontal or inclined state, the positioning is accurate, the existing positioning algorithm is enriched, the application scene is expanded, and the invention has wide market value.

Drawings

FIG. 1 is a schematic diagram of the present invention.

Fig. 2 is a schematic diagram of imaging when the imaging ellipse of the LED lamp is located on the high side of the imaging plane.

Fig. 3 is a schematic diagram of imaging when the imaging ellipse of the LED lamp is located on the low side of the imaging plane.

Fig. 4 is a similarity relationship diagram.

Fig. 5 is a diagram of an actual application scenario.

Fig. 6 is a schematic diagram of equation 1 and equation 2 in parallel.

Fig. 7 is a schematic diagram of the image after the rotation conversion processing.

FIG. 8 is a schematic view of simultaneous geodetic coordinate systems.

FIG. 9 is a schematic flow chart of the present invention.

Detailed Description

The invention is described in detail below with reference to the figures and the specific embodiments.

Referring to fig. 1, 5 and 9, the present invention provides a positioning algorithm based on two LED lamps and an angle sensor, comprising the following steps:

s1: shooting two LED lamps to obtain pictures, decoding the two captured LED lamps to obtain position information of the two LED lamps, and calculating geometric parameter information of the two LED lamps.

S2: and according to the geometric relationship among the two LED lamps, the camera and the imaging ellipses of the two LED lamps, combining the position information of the two LED lamps and the geometric parameter information of the two LED lamps in the step S1, and according to the included angle between the imaging planes of the two LED lamps and the horizontal plane, obtaining the Z coordinate of the center of the camera.

S3: and according to the geometric relationship among the two LED lamps, the camera and the imaging ellipses of the two LED lamps, combining the Z coordinate of the midpoint of the camera obtained in the step S2, and calculating to obtain the X coordinate and the Y coordinate of the midpoint of the camera.

In step S1, the capturing two LED lamps by the camera to obtain pictures, decoding the captured two LED lamps, and obtaining position information of the two LED lamps, including:

s1.1: and at the transmitting end of the LED lamp, the frequency and the phase difference of input PWM are controlled by utilizing a PWM modulation mode to obtain the LED lamps with different IDs, the specific forms of the IDs are black and white stripe intervals, the characteristics of the IDs comprise the thickness of the stripes and the number of the stripes, and the characteristics displayed by the IDs of different LED lamps are different. The indoor LED lamp is modulated through a visible light communication coding technology, so that the LED lamp emits high-frequency light which cannot be identified by naked eyes, the lighting use is not influenced, different modulation frequencies correspond to different LED-IDs, the dual purposes of lighting and communication are achieved, and the position of the indoor LED lamp and the corresponding ID are recorded.

S1.2: the two LED lamps are shot to obtain pictures, and a CMOS camera of the mobile phone can be used for shooting.

S1.3: and processing the shot picture through an image processing technology, and decoding to obtain the area, the white stripe number, the black-white stripe duty ratio characteristics of the two LED lamps and the geometric position information of the two LED lamps in the shot picture respectively.

S1.4: and matching the acquired characteristic information of the two LED lamps with the IDs of the LED lamps in a pre-established local database to obtain specific position coordinates (x1, y1,0) and (x2, y2,0) of the two photographed LED lamps.

In step S1, the calculating the geometric parameter information of the two LED lamps includes:

and calculating the geometrical parameter information of the two LED lamps by an image processing technology.

The geometric parameter information includes: the center position coordinates (i1, j1), (i2, j2) of the imaging ellipses of the two LED lamps, the distances d1 and d2 from the centers of the imaging ellipses of the two LED lamps to the center of an imaging plane, the lengths a1 and a2 of the semi-major axes of the imaging ellipses of the two LED lamps and the radius r of the LED lamps. And each indoor LED lamp is the same specification LED lamp, namely the radius of each LED lamp is equal.

Step S2 includes:

s2.1: and obtaining an inclination angle t of the camera by using the angle sensor, wherein the inclination angle t is an included angle between the imaging plane and the horizontal plane.

S2.2: calculating the vertical distance h between the center of the lens of the camera and the horizontal plane where the long axis of the imaging ellipse of each LED lamp is located, and specifically comprising the following steps:

s2.2.1: since the imaging plane is an inclined plane, the imaging ellipse of the LED lamp may be located on the high side of the imaging plane or the low side of the imaging plane, which is not known. Referring to fig. 2, when the imaging ellipse of the LED lamp is located at the high side of the inclined imaging plane,

h＝(f-d·tant(t))·cost；

referring to fig. 3, when the imaging ellipse of the LED lamp is located at the lower side of the inclined imaging plane,

h＝(d·tant(t)+f)·cost；

wherein f is the focal length of the lens, and d is the distance from the center of the imaging ellipse of the LED lamp to the center of the imaging plane.

S2.2.2: since it is unknown whether the imaging ellipses of the two LED lamps are located on the high side or the low side of the imaging plane, each LED lamp can calculate two h, and the two lamps can calculate four h, namely h11, h12, h21 and h 22.

S2.3: and obtaining the Z coordinate of the midpoint of the camera according to the fact that the ratio of the vertical distance from the center of the lens to the plane where the LED and the like are located to the vertical distance h from the center of the lens to the horizontal plane where the long axis of the imaging ellipse is located is equal to the ratio of the radius of the LED lamp to the semi-long axis of the imaging ellipse. The method specifically comprises the following steps:

s2.3.1: according to the fact that the ratio of the vertical distance between the center of the lens and the plane where the LED and the like are located to the vertical distance h between the center of the lens and the horizontal plane where the long axis of the imaging ellipse is located is equal to the ratio of the radius of the LED lamp and the semi-long axis of the imaging ellipse, the following four formulas are obtained by combining four h calculated in step S2.2.2:

and obtaining four Z coordinate values Z1, Z2, Z3 and Z4 of the camera center according to the four formulas.

S2.3.2: and performing difference on four Z coordinate values Z1, Z2, Z3 and Z4 at the center of the camera in pairs, taking an absolute value, averaging the two Z coordinates with the minimum absolute value of the difference, and taking the average as the Z coordinate at the center of the camera.

Step S3 includes:

s3.1: referring to fig. 4, according to the geometric relationship, the ratio of the radius of the LED lamp to the semi-major axis of the imaging ellipse is equal to the ratio of the distance from the center of the lens to the center of the LED lamp to the distance from the center of the lens to the center of the imaging ellipse, thereby constructing two pairs of similarity relationships:

s3.2: the above two sets of equations are combined, and the Z coordinate value obtained in step S2 is substituted to obtain X and Y coordinate values of the center of the camera, thereby obtaining three-dimensional coordinates of the camera.

Specifically, step 3.2 comprises:

s3.2.1: referring to fig. 6, equation 1 and equation 2 correspond to two circles with (X1, Y1) and (X2, Y2) as the centers, and there are two intersection points of the two circles, which are respectively denoted as (X1, Y1), (X2, and Y2), and it is necessary to determine which intersection point is the final positioning coordinate of the camera. Combining formula 1 with formula 2, the coordinates of (X1, Y1), (X2, Y2) are obtained, wherein the coordinate of only one intersection point is X, Y coordinates of the camera.

S3.2.2: the vector of the center coordinates (X2, Y2) pointing to (X1, Y1) of the two LED lamps is defined as vector 1, two intersection points (X1, Y1), (X2, Y2) are subjected to rotation conversion processing, an included angle between the vector 1 and an X axis is represented as an angle 0, the opposite number of the angle 0 is taken as an angle t, and the coordinates of (X11, Y11), (X22, Y22) are obtained by a rotation angle formula which is as follows:

wherein (X11, Y11) is the new coordinate after (X1, Y1) being processed by rotation transformation, and (X22, Y22) is the new coordinate after (X2, Y2) being processed by rotation transformation.

Referring to fig. 7, after the rotation transformation process, two points (X11, Y11) and (X22, Y22) are symmetric about the X axis after the rotation transformation, so if Y11 is greater than Y22, (X11, Y11) is located on the left side of the positive direction of the X axis, (X22, Y22) is located on the right side of the positive direction of the X axis, that is, (X1, Y1) is located on the left side of vector 1, and (X2, Y2) is located on the right side of vector 1; if Y11 is smaller than Y22, (X11, Y11) is located to the right of the positive X-axis, (X22, Y22) is located to the left of the positive X-axis, that is, (X1, Y1) is located to the right of vector 1, (X2, Y2) is located to the left of vector 1.

S3.2.3: referring to fig. 8, the angle 0 is converted into an angle between the vector 1 and the positive direction of the Y axis, which is denoted as angle 1, and ranges from 0 to 360 degrees (based on the north direction, the rotation to the east direction is the positive direction). And then an AZIMUTH angle of the receiving end is measured by the angle sensor, namely the included angle between the receiving end and the due north direction is recorded as an angle 2, and the range is 0-360 degrees (the due north direction is taken as a reference, and the rotation to the east is taken as a positive direction). An included angle between the positive Y-axis direction of the specified coordinate axis and the geographical north direction is marked as an angle 3, and the range is 0-360 degrees (the north direction is taken as a reference, and the rotation to the east is taken as the positive direction). And carrying out operation processing on the angle 1 and the angle 3 to obtain an included angle between the vector 1 and the due north direction, recording the included angle as an angle 4, and acquiring an ROLL angle of the camera by using an angle sensor, wherein the included angle ranges from-90 degrees to 90 degrees.

S3.2.4: in case of a ROLL angle > 0: if the absolute value of the difference value between the angle 4 and the angle 2 is between 90 degrees and 270 degrees, selecting a coordinate point on the left side from the final positioning coordinate of the camera, and otherwise, selecting the coordinate on the right side as the final positioning coordinate of the camera;

in case of a ROLL angle < 0: if the absolute value of the difference value between the angle 4 and the angle 2 is between 90 degrees and 270 degrees, the coordinate point on the right side is selected as the final positioning coordinate of the camera, and otherwise, the coordinate on the left side is selected as the final positioning coordinate of the camera.

The invention also provides a positioning system, which comprises a receiving end, a processor and a computer program which is stored in the processor and can run on the processor, wherein the processor realizes the positioning algorithm and the positioning system based on the two LED lamps and the angle sensor when executing the computer program.

The receiving end comprises a camera and an angle sensor, and the camera is used for shooting two LED lamps to obtain pictures.

The angle sensor is used for sensing the inclination angle of the camera.

In conclusion, the invention carries out positioning based on double lamps, the receiving end can realize three-dimensional positioning in a horizontal or inclined state, the positioning is accurate, the existing positioning algorithm is enriched, the application scene is expanded, and the invention has wide market value.

The present invention is not limited to the above preferred embodiments, and any modifications, equivalent substitutions and improvements made within the spirit and principle of the present invention should be included in the protection scope of the present invention.