CN109031199B - Three-dimensional positioning method, system and device - Google Patents
Three-dimensional positioning method, system and device Download PDFInfo
- Publication number
- CN109031199B CN109031199B CN201810647908.XA CN201810647908A CN109031199B CN 109031199 B CN109031199 B CN 109031199B CN 201810647908 A CN201810647908 A CN 201810647908A CN 109031199 B CN109031199 B CN 109031199B
- Authority
- CN
- China
- Prior art keywords
- signal
- laser plane
- photoelectric sensor
- rotation angle
- plane signal
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/16—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using electromagnetic waves other than radio waves
Landscapes
- Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
The invention discloses a three-dimensional positioning method, a three-dimensional positioning system and a three-dimensional positioning device. The method comprises the following steps: determining a first rotation angle of the first laser plane signal, wherein the first laser plane signal rotates around a first rotation axis, and the first rotation angle is an angle of the first laser plane signal relative to a plane determined by the first rotation axis and a second rotation axis when the photoelectric sensor detects the first laser plane signal; determining a second rotation angle of the second laser plane signal, wherein the second laser plane signal rotates around a second rotation axis, and the second rotation angle is an angle of the second laser plane signal relative to a plane determined by the first rotation axis and the second rotation axis when the photoelectric sensor detects the second laser plane signal; determining the distance from the signal emitting device to the photoelectric sensor; and determining the three-dimensional coordinates of the photoelectric sensor according to the first rotation angle, the second rotation angle and the distance. The method not only improves the accuracy of the positioning calculation, but also reduces the complexity of the calculation.
Description
Technical Field
The invention relates to a three-dimensional positioning method, and also relates to a three-dimensional positioning system for realizing the positioning method, belonging to the technical field of space positioning.
Background
In recent years, the related technologies and industries of location services are developing indoors, and particularly in the VR (virtual reality) and AR (augmented reality) fields, location technologies have become the basis of VR and AR interactions. Existing VR and AR field positioning techniques include: infrared optical positioning, laser positioning, ultrasonic positioning and visible light positioning. In order to increase the use experience of the user, the VR content interaction, such as archery, drawing and the like, needs to be performed in cooperation with a device such as a handle or a glove.
The positioning technology needs a tracking system capable of tracking the movement tracks of handles, gloves and the like, and the existing tracking system comprises an inside-out mode and usually utilizes a camera to track; the outside-in method generally uses a laser or a camera for tracking.
Disclosure of Invention
In view of the deficiencies of the prior art, the primary technical problem to be solved by the present invention is to provide a three-dimensional positioning method.
Another technical problem to be solved by the present invention is to provide a three-dimensional positioning system and a device thereof for implementing the three-dimensional positioning method.
In order to achieve the purpose, the invention adopts the following technical scheme:
according to a first aspect of the embodiments of the present invention, there is provided a three-dimensional positioning method, including the steps of:
determining a first rotation angle of a first laser plane signal, wherein the first laser plane signal rotates around a first rotation axis, and the first rotation angle is an angle of the first laser plane signal relative to a plane determined by the first rotation axis and a second rotation axis when the photoelectric sensor detects the first laser plane signal;
determining a second rotation angle of a second laser plane signal, wherein the second laser plane signal rotates around a second rotation axis, and the second rotation angle is an angle of the second laser plane signal relative to a plane determined by the first rotation axis and the second rotation axis when the photoelectric sensor detects the second laser plane signal;
determining a distance from a signal emitting device to the photosensor;
and determining the three-dimensional coordinates of the photoelectric sensor according to the first rotation angle, the second rotation angle and the distance.
Preferably, the determining the first rotation angle of the first laser plane signal includes the following steps:
determining a first transmission time of the first laser plane signal;
determining a first receiving time and a first duration time at which the photoelectric sensor detects the first laser plane signal; the first receiving moment is the moment when the photoelectric sensor starts to detect a first laser plane signal; the first duration time is the time when the photoelectric sensor detects the first laser plane signal at last;
the first rotation angle is determined as a function of the first transmission time, the first reception time and the first duration.
Preferably, the first rotation angle is determined by the following formula:
wherein a is a first rotation angle; t is t2Is a first duration; t is t1Is a first reception time; t is t0Is a first transmission time; omega1Is the angular velocity of rotation of the first laser plane signal.
Preferably, the determining the second rotation angle of the second laser plane signal includes the following steps:
determining a second emission time of the second laser plane signal;
determining a second receiving time and a second duration time at which the photoelectric sensor detects the second laser plane signal; the second receiving moment is the moment when the photoelectric sensor starts to detect a second laser plane signal; the second duration time is the time when the photoelectric sensor detects a second laser plane signal at last;
determining the second rotation angle from the second transmission time, the second reception time and the second duration.
Preferably, the second angle of rotation is determined by the following formula:
wherein b is a second rotation angle; t is t4Is a second duration; t is t3Is a second reception time; t is t02Is a second transmission time; omega2Is the angular velocity of rotation of the second laser plane signal.
Preferably, the determining the distance from the signal emitting device to the photoelectric sensor comprises the following steps:
determining a first transmission time of the first laser plane signal;
determining a first receiving time and a first duration time at which the photoelectric sensor detects the first laser plane signal; the first receiving moment is the moment when the photoelectric sensor starts to detect a first laser plane signal; the first duration time is the time when the photoelectric sensor detects the first laser plane signal at last;
determining a first receiving rotation angle according to the first transmitting time and the first receiving time;
determining a first continuous rotation angle according to the first transmitting time and the first continuous time;
combining attitude data of the photoelectric sensor according to the first receiving rotation angle, the first continuous rotation angle and the diameter of the photoelectric sensor; and determining the distance from the signal emitting device to the photoelectric sensor.
Preferably, the distance from the signal emitting device to the photoelectric sensor is determined by the following formula:
wherein Ts is a point on the photoelectric sensor where a first laser plane signal starts to be detected; b is the center of the photoelectric sensor; o is the position of the signal transmitting device; OB is the distance from the signal emitting device to the photoelectric sensor; t issB is the radius of the photoelectric sensor; OTsThe distance between the signal transmitting device and a point on the photoelectric sensor where the first laser plane signal starts to be detected is the distance; angle OTsB ═ 90 ° + the first reception rotation angle + the attitude data of the photosensor.
Preferably, when the three-dimensional measurement coordinate system is a cartesian coordinate system, the first rotating shaft is taken as a Y axis, and the second rotating shaft is taken as an X axis, the Z axis is determined by a right-hand rule, and the three-dimensional coordinate of the photoelectric sensor is obtained by solving according to the following formula:
XB 2+YB 2+ZB 2＝OB2；
YB×tanb＝XB×tana＝ZB；
wherein (X)B，YB，ZB) The OB is the distance from the signal emitting device to the photoelectric sensor, a is a first rotation angle, and b is a second rotation angle.
According to a second aspect of embodiments of the present invention, there is provided a three-dimensional positioning system, comprising a processor and a memory; the memory having stored thereon a computer program operable on the processor, the computer program when executed by the processor implementing the steps of:
determining a first rotation angle of a first laser plane signal, wherein the first laser plane signal rotates around a first rotation axis, and the first rotation angle is an angle of the first laser plane signal relative to a plane determined by the first rotation axis and a second rotation axis when the photoelectric sensor detects the first laser plane signal;
determining a second rotation angle of a second laser plane signal, wherein the second laser plane signal rotates around a second rotation axis, and the second rotation angle is an angle of the second laser plane signal relative to a plane determined by the first rotation axis and the second rotation axis when the photoelectric sensor detects the second laser plane signal;
determining a distance from a signal emitting device to the photosensor;
and determining the three-dimensional coordinates of the photoelectric sensor according to the first rotation angle, the second rotation angle and the distance.
According to a third aspect of the embodiments of the present invention, there is provided a three-dimensional positioning apparatus, including the three-dimensional positioning system, further including a signal transmitter, a signal receiver;
the signal transmitter is used for transmitting a first laser plane signal around a first rotating shaft and transmitting a second laser plane signal around a second rotating shaft;
the signal receiver comprises at least one photoelectric sensor for receiving a first laser plane signal and a second laser plane signal.
The three-dimensional positioning method provided by the invention determines a first rotation angle of a photoelectric sensor in a signal receiving device according to a first transmitting time when a signal transmitting device transmits a first laser plane signal, a first receiving time when the photoelectric sensor in the signal receiving device detects the laser plane signal and a first duration time; determining a second rotation angle of the photoelectric sensor in the signal receiving device according to a second transmitting time when the signal transmitting device transmits the second laser plane signal, and a second receiving time and a second duration time when the photoelectric sensor in the signal receiving device detects the second laser plane signal; and determining the three-dimensional coordinates of the photoelectric sensor in the signal receiving device according to the first rotation angle, the second rotation angle and the distance between the photoelectric sensor and the signal transmitting device. According to the method, when the three-dimensional coordinates of the photoelectric sensor in the signal receiving device are calculated, the first rotating angle, the second rotating angle and the distance between the photoelectric sensor in the signal receiving device and the signal transmitting device are obtained according to the received laser plane signals, so that the accuracy of positioning calculation is improved, the complexity of calculation is reduced, and the calculation efficiency is improved.
Drawings
FIG. 1 is a schematic diagram of an effective configuration of a photoelectric sensor according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a laser transmitter for transmitting laser planes about an X-axis and a Y-axis according to an embodiment of the present invention;
FIG. 3 is a flow chart of a three-dimensional positioning method provided by the present invention;
FIG. 4 is a schematic diagram illustrating a first principle of calculating three-dimensional coordinates of a photosensor in a signal receiving device according to an embodiment of the present invention;
FIG. 5 is a schematic diagram illustrating a second principle of calculating three-dimensional coordinates of a photosensor in the signal receiving device according to an embodiment of the present invention;
fig. 6 is a schematic structural diagram of a three-dimensional positioning system provided in the present invention.
Detailed Description
The invention will be further described in detail with reference to the drawings and specific examples. It should be understood that the following illustrative examples are only for the purpose of illustrating and explaining the present application and are not intended to limit the present application.
The three-dimensional positioning device provided by the invention comprises a signal transmitting device, a signal receiving device and a three-dimensional positioning system; the signal transmitting device is a laser transmitter and is used for transmitting a first laser plane signal around a first rotating shaft and transmitting a second laser plane signal around a second rotating shaft. The signal receiving device carries out positioning calculation by receiving laser plane signals transmitted by the laser transmitter. The signal receiver includes at least one photosensor for receiving the first laser plane signal and the second laser plane signal. The laser sweeps across the photoelectric sensor for a certain duration, the time is related to the distance between the signal receiving device and the signal transmitting device and the angle relative to the signal transmitting device, the three-dimensional positioning system can obtain the included angle between the signal receiving device and the signal transmitting device through calibrated data, the distance relative to the signal transmitting device can be judged according to the length of the time for the photoelectric sensor to receive the signal, and the synchronous time for the photoelectric sensor to receive the signal is the angle relative to the signal transmitting device. The angle and distance constitute positioning information (polar coordinates).
Specifically, a photosensor is provided on the signal receiving device, and the laser light sweeps across the photosensor for a duration that is related to the distance of the photosensor from the signal emitting device and the angle relative to the signal emitting device. In the embodiment provided by the present invention, the photosensor is set to be circular, and if not, the effective shape of the photosensor can be changed to be circular by covering the light shielding sheet, and the like, as shown in fig. 1, the diameter of the photosensor is d. Alternatively, when the photosensor has a square shape or the like, the side length dimension is set to approximately the diameter d.
As shown in fig. 2, the laser transmitter transmits laser plane signals around X and Y axes perpendicular to each other. Two motors are arranged in the signal transmitting device, and the motors rotate around the X axis and the Y axis respectively to drive a laser transmitter arranged on the motors to transmit laser plane signals to the space in a rotating way around the X axis and the Y axis. As shown in fig. 3, the three-dimensional positioning method provided by the present invention includes the following steps: firstly, determining a first rotation angle of a first laser plane signal, wherein the first laser plane signal rotates around a first rotation axis, and the first rotation angle is an angle of the first laser plane signal relative to a plane determined by the first rotation axis and a second rotation axis when the photoelectric sensor detects the first laser plane signal; secondly, determining a second rotation angle of a second laser plane signal, wherein the second laser plane signal rotates around a second rotation axis, and the second rotation angle is an angle of the second laser plane signal relative to a plane determined by the first rotation axis and the second rotation axis when the photoelectric sensor detects the second laser plane signal; then, determining the distance from the signal transmitting device to the photoelectric sensor; and finally, determining the three-dimensional coordinates of the photoelectric sensor according to the first rotation angle, the second rotation angle and the distance from the signal transmitting device to the photoelectric sensor. This process is described in detail below.
The method comprises the steps of S1, determining a first rotation angle of a first laser plane signal, wherein the first laser plane signal rotates around a first rotation axis, and the first rotation angle is an angle of the first laser plane signal relative to a plane determined by the first rotation axis and a second rotation axis when a photoelectric sensor detects the first laser plane signal.
In the embodiment provided by the present invention, a three-dimensional measurement coordinate system is taken as a cartesian coordinate system, an X axis is a first rotation axis, a Y axis is a second rotation axis, and a signal transmitting device is taken as an origin of the three-dimensional measurement coordinate system for illustration. Two motors are arranged in the signal transmitting device, and the motors rotate around the X axis and the Y axis respectively to drive a laser transmitter arranged on the motors to transmit laser plane signals to the space in a rotating way around the X axis and the Y axis. The laser transmitter Oy of the signal transmitting device rotates around the Y axis to transmit a first laser plane signal at a rotation speed of, for example, ω1(ii) a The laser transmitter Ox of the signal transmitter rotates around the X axis to transmit a second laser plane signal at a rotation speed of, for example, ω2. Determined by X-axis and Y-axisThe determined plane is an XOY plane.
By synchronizing the reference time of the signal transmitting device and the signal receiving device, the clock synchronization of the signal transmitting device and the signal receiving device is ensured, so that the subsequent calculation is based on the same reference, and the accuracy of the three-dimensional coordinate calculation is improved. The signal transmitting device may transmit a time synchronization signal to the signal receiving device, and the signal receiving device may calculate the reference time from the time when the time synchronization signal is received. Then, a first transmission timing at which the signal transmission device transmits the first laser plane signal while rotating around the first rotation axis is determined.
Determining a first rotation angle of the first laser plane signal, specifically comprising the following steps:
s11, determining a first emitting time of the first laser plane signal; namely, a first emitting time t of the signal emitting device for emitting the first laser plane signal in rotation around the first rotation axis is determined0。
First emission time t of signal emission device for emitting first laser plane signal0Specifically, when a trigger point on a motor rotating around a Y axis in the signal transmitting device rotates to form a corresponding angle with the XOY plane, a trigger signal is obtained in a Hall sensor, an optical trigger and the like, and therefore a first laser plane signal starts to be transmitted along the XOY plane. It can also be shown that when the signal transmitting device continuously transmits the first laser plane signal around the Y axis, when the first laser plane signal is converted to the XOY plane, the trigger signal is obtained by means of a hall sensor, an optical trigger, etc., and the first transmitting time is obtained by the time of the trigger signal.
S12, determining a first receiving time and a first duration time when the photoelectric sensor detects the first laser plane signal; the first receiving moment is the moment when the photoelectric sensor starts to detect the first laser plane signal; the first duration is the time when the photoelectric sensor detects the first laser plane signal last.
As shown in FIG. 4, the Y-axis laser light is received from a first time instant (t)1Time) reaches a point Ts, the photoelectric sensor is activated, and the photoelectric sensor in the signal receiving device detects the first photoelectric sensorLaser plane signals. That is, the first receiving timing is a timing at which the photosensor in the signal receiving device starts detecting the first laser plane signal within one signal emission period.
As the first laser plane signal rotates around the Y axis, the photoelectric sensor continuously receives the first laser plane signal in a signal emission period until a first continuous time (t)2Time) reaches Te point, after which the photo sensor no longer receives the first laser plane signal. The first duration is the time when the photoelectric sensor in the signal receiving device finally detects the first laser plane signal in one signal transmitting period. The concrete can be represented as follows: after receiving the optical signal, the photoelectric sensor converts the optical signal into a pulse electric signal t1The time is the rising edge time of the pulse electric signal t2The moment is the falling edge moment of the pulse electric signal.
S13, determining a first rotation angle according to the first transmitting time, the first receiving time and the first duration, specifically including the following steps:
s131, determining a first receiving rotation angle according to the first transmitting time and the first receiving time.
According to a first transmission time t0And a first receiving time t1And Y-axis motor angular velocity ω1Determining a first receiving rotation angle & lt AyOTs(ii) a Wherein A isyThe first receiving rotation angle is an angle of the first laser plane signal relative to the XOY plane when the photoelectric sensor in the signal receiving device starts to detect the first laser plane signal in one signal transmitting period. Specifically, from the time when the signal transmitting device transmits the first laser plane signal to the time when the photoelectric sensor in the signal receiving device detects the first continuous laser plane signal, the angle swept by the first laser plane signal is calculated as follows:
∠AyOTs＝(t1-t0)ω1；
and S132, determining a first continuous rotation angle according to the first transmitting time and the first continuous time.
According to a first transmission time t0And a first duration t2And Y-axis motor angular velocity ω1Determining first continuous rotation angle & lt AyOTe(ii) a Wherein Te is a point on the photoelectric sensor in the signal receiving device at which the first laser plane signal is detected last, and the first continuous rotation angle is an angle of the first laser plane signal with respect to the XOY plane when the photoelectric sensor in the signal receiving device detects the first laser plane signal last in one signal transmission period.
∠AyOTe＝(t2-t0)ω1。
S133, a first rotation angle of the signal receiving device is determined according to the first reception rotation angle and the first continuous rotation angle.
The first rotation angle is a Y-axis rotation angle measured by a Y-axis motor, and is an angle of an XOY plane determined by the first laser plane signal relative to a Y axis of the first rotation axis and an X axis of the second rotation axis when the photoelectric sensor detects the first laser plane signal;
i.e. the first angle of rotation is determined by the following formula:
wherein a is a first rotation angle; t is t2Is a first duration; t is t1Is a first reception time; t is t0Is a first transmission time; omega1Is the angular velocity of rotation of the first laser plane signal.
And S2, determining a second rotation angle of the second laser plane signal, wherein the second laser plane signal rotates around a second rotation axis, and the second rotation angle is the angle of the second laser plane signal relative to the plane determined by the first rotation axis and the second rotation axis when the photoelectric sensor detects the second laser plane signal.
Wherein, the determining the second rotation angle of the second laser plane signal comprises the following steps:
s21, determining a second emission time t of the second laser plane signal02。
The laser transmitter Ox of the signal transmitter rotates around the X axis to transmit a second laser plane signal at a rotation speed of, for example, ω2. Determining a second emission time t at which the signal emitting device emits the second laser plane signal in rotation about the second axis of rotation02。
In order that the photoelectric sensor can receive the first laser plane signal and the second laser plane signal in a time-sharing manner, the first laser plane signal and the second laser plane signal are emitted in a time-sharing manner at a time interval within one signal period, preferably, the first emission time t0And a second transmission time t02Differing by a time of half a signal period. In the first half of the signal period, the first laser plane signal is emitted and continuously sweeps across the spatial range, and in the second half of the signal period, the second laser plane signal is emitted and continuously sweeps across the spatial range.
S22, determining a second receiving time and a second duration time when the photoelectric sensor detects the second laser plane signal; the second receiving moment is the moment when the photoelectric sensor starts to detect a second laser plane signal; the second duration is the time when the photoelectric sensor detects the second laser plane signal last.
Specifically, a second receiving time and a second duration time when a photoelectric sensor in the signal receiving device detects a second laser plane signal are determined; the second receiving moment is the moment when the photoelectric sensor in the signal receiving device starts to detect the second laser plane signal in a signal emission period; the second duration time is the time when the photoelectric sensor in the signal receiving device in one signal transmitting period detects a second laser plane signal finally;
let X-axis laser receive time (t) from second3Time of day) continuesTo Ts2The photoelectric sensor is activated when the point is on, and the photoelectric sensor in the signal receiving device detects the second laser plane signal. That is, the second receiving timing is a timing at which the photosensor in the signal receiving device starts detecting the second laser plane signal within one signal emission period.
The photoelectric sensor continuously receives the second laser plane signal in a signal emission period until a second continuous time (t)4Time) reaches Te2The point after which the second laser plane signal is no longer received by the photosensor. The second duration is the time when the photoelectric sensor in the signal receiving device finally detects the second laser plane signal in one signal transmitting period.
S23, determining a second rotation angle according to the second transmitting time, the second receiving time and the second duration, specifically including the following steps:
s231, determining a second receiving rotation angle according to the second transmitting time and the second receiving time;
according to the second transmission time t02And a second receiving time t3And X-axis motor angular velocity ω2Determining second receiving rotation angle & lt AxOTs2；
Wherein A isxIs any point on the Y axis, and O is the coordinate origin where the base station signal transmitting device is located.
∠AxOTs2＝(t3-t02)ω2。
S232, determining a second continuous rotation angle according to the second emission time and the second continuous time;
according to the second transmission time t02And a second time duration t4And X-axis motor angular velocity ω2Determining second continuous rotation angle & lt AxOTe2；
∠AxOTe2＝(t4-t0)ω2。
S233, a second rotation angle of the signal receiving device is determined according to the second reception rotation angle and the second continuous rotation angle.
The second rotation angle is an X-axis rotation angle measured by an X-axis motor, and is an angle of an XOY plane determined by the second laser plane signal relative to the Y axis of the first rotation axis and the X axis of the second rotation axis when the photoelectric sensor detects the second laser plane signal;
further, the air conditioner is provided with a fan,
wherein b is a second rotation angle; t is t4Is a second duration; t is t3Is a second reception time; t is t02Is a second transmission time; omega2Is the angular velocity of rotation of the second laser plane signal.
S3, determining the distance from the signal emitting device to the photoelectric sensor, comprising the following steps:
s31, determining a first emitting time of the first laser plane signal; specifically, refer to step S11.
S32, determining a first receiving time and a first duration time when the photoelectric sensor detects the first laser plane signal; the first receiving moment is the moment when the photoelectric sensor starts to detect the first laser plane signal; the first duration time is the time when the photoelectric sensor detects the first laser plane signal at last; specifically, refer to step S12.
S33, determining a first receiving rotation angle according to the first transmitting time and the first receiving time;
determining a first receiving rotation angle according to the first transmitting time and the first receiving time; and determining a first continuous rotation angle according to the first transmitting time and the first continuous time.
According to a first transmission time t0And a first receiving time t1And Y-axis motor angular velocity ω1Determining a first receiving rotation angle & lt AyOTs(ii) a Wherein A isyIs any point on the X axis, and O is the origin of coordinates of the signal transmitting deviceTs is a point on the photo sensor in the signal receiving device where the first laser plane signal starts to be detected, and the first receiving rotation angle is an angle of the first laser plane signal relative to the XOY plane when the photo sensor in the signal receiving device starts to detect the continuously excited first laser plane signal in one signal transmitting period. Specifically, from the time when the signal transmitting device transmits the first laser plane signal to the time when the photoelectric sensor in the signal receiving device detects the first continuous laser plane signal, the angle swept by the first laser plane signal is calculated as follows:
∠AyOTs＝(t1-t0)ω1；
s34, determining a first continuous rotation angle according to the first transmitting time and the first continuous time;
according to a first transmission time t0And a first duration t2And Y-axis motor angular velocity ω1Determining first continuous rotation angle & lt AyOTe(ii) a Wherein Te is a point on the photoelectric sensor in the signal receiving device at which the first laser plane signal is detected last, and the first continuous rotation angle is an angle of the first laser plane signal with respect to the XOY plane when the photoelectric sensor in the signal receiving device detects the first laser plane signal last in one signal transmission period.
∠AyOTe＝(t2-t0)ω1。
S35, combining the attitude data of the photoelectric sensor according to the first receiving rotation angle, the first continuous rotation angle and the diameter of the photoelectric sensor; the distance of the signal emitting device from the photoelectric sensor is determined.
Combining the attitude data of the signal receiving device according to the first receiving rotation angle, the first continuous rotation angle and the diameter of the photoelectric sensor; and determining the distance OB between the center of the signal receiving device and the signal transmitting device, wherein B is the center of the photoelectric sensor in the signal receiving device, and O is the point where the signal transmitting device is located. The photoelectric sensor in the signal receiving device is opened in a signal transmitting periodA triangle T consisting of a point Ts for detecting the first laser plane signal, a point Te for detecting the first laser plane signal by the photoelectric sensor in the signal receiving device and an original point O where the signal sending device is positionedsTeO, B are TsTeThe midpoint of (a).
At the triangle TsTeIn O, TsTeD, where d is the diameter of the photosensor in the signal receiving device, TeTsO＝90+∠OTsC+ ∠TeTsD＝90+∠AyOTsB ++ p; wherein A isyIs any point on the X axis, and the angle P is the attitude pitch angle.
According to the sine theorem
The length of the OTs is obtained.
Knowing the length and BT of OTssLength of (2)
And OTsB, obtaining the length of OB according to the cosine theorem:
namely, the distance from the signal emitting device to the photoelectric sensor is determined by the following formula:
wherein, TsA point on the photoelectric sensor where a first laser plane signal starts to be detected; b is the center of the photoelectric sensor; o is the position of the signal transmitting device; OB is the distance from the signal emitting device to the photoelectric sensor; t issB is the radius of the photoelectric sensor; OTsAs signal emitting devicesThe distance to the point on the photoelectric sensor where the first laser plane signal starts to be detected; angle OTsB ═ 90 ° + the first reception rotation angle + the attitude data of the photosensor.
And S4, determining the three-dimensional coordinates of the photoelectric sensor according to the first rotation angle, the second rotation angle and the distance.
When the three-dimensional measurement coordinate system is a cartesian coordinate system, as shown in fig. 5, the first rotation axis is a Y axis, and the second rotation axis is an X axis, the right-hand rule determines the Z axis, and the three-dimensional coordinates of the photoelectric sensor in the three-dimensional measurement coordinate system can be obtained by solving according to the following formula:
XB 2+YB 2+ZB 2＝OB2
YB×tanb＝XB×tana＝ZB
wherein (X)B，YB，ZB) Representing the three-dimensional coordinates of the photoelectric sensor in the signal receiving device in a three-dimensional coordinate system, OB being the distance from the signal transmitting device to the photoelectric sensor, a being the first rotation angle, b being the second rotation angle.
In summary, the three-dimensional positioning method based on the photoelectric sensor provided by the present invention determines the first emitting time at which the signal emitting device emits the first laser plane signal; the signal receiving device detects a first receiving time and a first duration time of the laser plane signal; then, determining a first receiving rotation angle according to the first transmitting time and the first receiving time; determining a first continuous rotation angle according to the first transmitting time and the first continuous time; combining attitude data of the signal receiving device according to the first receiving rotation angle, the first continuous rotation angle and a first distance of the first laser plane signal sweeping through the signal receiving device; the distance between the center of the signal receiving means and the signal transmitting means is determined. Then, determining a first rotation angle of the signal receiving device according to the first transmitting time, the first receiving time and the first duration time; determining a second rotation angle of the signal receiving device according to a second transmitting time when the signal transmitting device transmits the second laser plane signal, and a second receiving time and a second duration when the signal receiving device detects the second laser plane signal; and determining the three-dimensional coordinates of the photoelectric sensor in the signal receiving device according to the first rotating angle, the second rotating angle and the distance between the center of the signal receiving device and the signal transmitting device. When the three-dimensional coordinates of the signal receiving device are calculated, the first rotating angle, the second rotating angle and the distance between the center of the signal receiving device and the signal transmitting device are obtained according to the received laser plane signals. The accuracy of positioning calculation is improved, the complexity of calculation is reduced, and the calculation efficiency is improved.
It should be noted that the distance between the center of the signal receiving device and the signal transmitting device is determined by the corresponding time and angle of the first laser plane signal in this embodiment, and in other embodiments that can be implemented, the distance may also be determined by the corresponding time and angle of the second laser plane signal in this embodiment. In this embodiment, the first laser plane signal is a laser plane signal rotating around the Y axis, and the second laser plane signal is a laser plane signal rotating around the X axis. So long as the manner in which the modifications can be made is within the spirit of the invention, they are within the scope of the invention.
The invention also provides a three-dimensional positioning system. As shown in fig. 6, the system includes a processor 62 and a memory 61 storing instructions executable by the processor 62;
the processor 62 may be a general-purpose processor, such AS a Central Processing Unit (CPU), a Digital Signal Processor (DSP), an application specific integrated circuit (AS IC), or one or more integrated circuits configured to implement embodiments of the present invention, among others.
The memory 61 is configured to store a program code, and transmit the program code to a CPU or a single chip Microcomputer (MCU). Memory 61 may include volatile memory, such as Random Access Memory (RAM); the memory 61 may also include non-volatile memory, such as read-only memory, flash memory, a hard or solid state disk, an MCU, etc.; the memory 61 may also comprise a combination of memories of the kind described above.
Specifically, the three-dimensional positioning system provided by the embodiment of the present invention includes a processor 62 and a memory 61; the memory 61 has stored thereon a computer program operable on the processor 62, which when executed by the processor 62, performs the steps of:
determining a first rotation angle of the first laser plane signal, wherein the first laser plane signal rotates around a first rotation axis, and the first rotation angle is an angle of the first laser plane signal relative to a plane determined by the first rotation axis and a second rotation axis when the photoelectric sensor detects the first laser plane signal;
determining a second rotation angle of the second laser plane signal, wherein the second laser plane signal rotates around a second rotation axis, and the second rotation angle is an angle of the second laser plane signal relative to a plane determined by the first rotation axis and the second rotation axis when the photoelectric sensor detects the second laser plane signal;
determining the distance from the signal emitting device to the photoelectric sensor;
and determining the three-dimensional coordinates of the photoelectric sensor according to the first rotation angle, the second rotation angle and the distance.
Wherein, when determining the first rotation angle of the first laser plane signal, the computer program is executed by the processor 62 to carry out the following steps;
determining a first transmitting time of a first laser plane signal;
determining a first receiving time and a first duration time when the photoelectric sensor detects the first laser plane signal; the first receiving moment is the moment when the photoelectric sensor starts to detect the first laser plane signal; the first duration time is the time when the photoelectric sensor detects the first laser plane signal at last;
the first rotation angle is determined as a function of the first transmission time, the first reception time and the first duration.
Wherein, when determining the first rotation angle from the first transmission instant, the first reception instant and the first duration, the computer program is executed by the processor 62 to carry out the following steps;
determining a first receiving rotation angle according to the first transmitting time and the first receiving time;
determining a first continuous rotation angle according to the first transmitting time and the first continuous time;
a first angle of rotation of the signal receiving device is determined as a function of the first reception angle of rotation and the first continuous angle of rotation.
Wherein the computer program realizes the following steps when executed by the processor 62;
the first rotation angle is determined by the following formula:
wherein a is a first rotation angle; t is t2Is a first duration; t is t1Is a first reception time; t is t0Is a first transmission time; omega1Is the angular velocity of rotation of the first laser plane signal.
Wherein, when determining the second rotation angle of the second laser plane signal, the computer program is executed by the processor 62 to carry out the following steps;
determining a second emission time of the second laser plane signal;
determining a second receiving time and a second duration time when the photoelectric sensor detects a second laser plane signal; the second receiving moment is the moment when the photoelectric sensor starts to detect a second laser plane signal; the second duration time is the time when the photoelectric sensor detects the second laser plane signal finally;
a second rotation angle is determined as a function of the second transmission time, the second reception time and the second duration.
Wherein the computer program is executed by the processor 62 to carry out the following steps when determining the second rotation angle of the signal receiving means on the basis of the second transmission instant, the second reception instant and the second duration;
determining a second receiving rotation angle according to the second transmitting time and the second receiving time;
determining a second continuous rotation angle according to the second transmitting time and the second continuous time;
a second angle of rotation of the signal receiving device is determined as a function of the second reception angle of rotation and the second continuous angle of rotation.
Wherein the computer program realizes the following steps when executed by the processor 62;
the second rotation angle is determined by the following formula:
wherein b is a second rotation angle; t is t4Is a second duration; t is t3Is a second reception time; t is t02Is a second transmission time; omega2Is the angular velocity of rotation of the second laser plane signal.
Wherein, when determining the distance of the signal emitting device to the photosensor, the computer program is executed by the processor 62 to carry out the following steps;
determining a first transmitting time of a first laser plane signal;
determining a first receiving time and a first duration time when the photoelectric sensor detects the first laser plane signal; the first receiving moment is the moment when the photoelectric sensor starts to detect the first laser plane signal; the first duration time is the time when the photoelectric sensor detects the first laser plane signal at last;
determining a first receiving rotation angle according to the first transmitting time and the first receiving time;
determining a first continuous rotation angle according to the first transmitting time and the first continuous time;
combining attitude data of the photoelectric sensor according to the first receiving rotation angle, the first continuous rotation angle and the diameter of the photoelectric sensor; the distance of the signal emitting device from the photoelectric sensor is determined.
Wherein the computer program realizes the following steps when executed by the processor 62;
determining the distance from the signal transmitting device to the photoelectric sensor by the following formula:
wherein, TsA point on the photoelectric sensor where a first laser plane signal starts to be detected; b is the center of the photoelectric sensor; o is the position where the signal sending device is located; OB is the distance from the signal emitting device to the photoelectric sensor; t issB is the radius of the photoelectric sensor; OTsThe distance between a signal transmitting device and a point on the photoelectric sensor where a first laser plane signal starts to be detected is the distance; angle OTsB ═ 90 ° + the first reception rotation angle + the attitude data of the photosensor.
Wherein the computer program realizes the following steps when executed by the processor 62;
when the three-dimensional measurement coordinate system is a Cartesian coordinate system, the first rotating shaft is used as a Y axis, the second rotating shaft is used as an X axis, the Z axis is determined by the right-hand rule, and the three-dimensional coordinate of the photoelectric sensor is obtained by solving according to the following formula:
XB 2+YB 2+ZB 2＝OB2
YB×tanb＝XB×tana＝ZB
wherein (X)B，YB，ZB) Representing the three-dimensional coordinates of the photoelectric sensor in the signal receiving device in a three-dimensional coordinate system, OB being the distance between the signal emitting device and the photoelectric sensor, a being the first rotation angle, b being the second rotation angle.
Wherein the computer program realizes the following steps when executed by the processor 62;
synchronizing reference times of the signal transmitting device and the signal receiving device before transmitting the laser plane signal.
The embodiment of the invention also provides a computer readable storage medium. The computer-readable storage medium herein stores one or more programs. Among other things, computer-readable storage media may include volatile memory, such as random access memory; the memory may also include non-volatile memory, such as read-only memory, flash memory, hard or solid state disk, MCU, etc.; the memory may also comprise a combination of memories of the kind described above. When the one or more programs in the computer-readable storage medium are executed by one or more processors, part of or all of the steps for implementing the three-dimensional positioning method in the above-described method embodiments are implemented.
The three-dimensional positioning method, system and device provided by the invention are explained in detail above. Any obvious modifications to the invention, which would occur to those skilled in the art, without departing from the true spirit of the invention, would constitute a violation of the patent rights of the invention and would carry a corresponding legal responsibility.
Claims (10)
1. A three-dimensional positioning method is characterized by comprising the following steps:
determining a first rotation angle of a first laser plane signal, wherein the first laser plane signal rotates around a first rotation axis, and the first rotation angle is an angle of the first laser plane signal relative to a plane determined by the first rotation axis and a second rotation axis when the photoelectric sensor detects the first laser plane signal;
determining a second rotation angle of a second laser plane signal, wherein the second laser plane signal rotates around a second rotation axis, and the second rotation angle is an angle of the second laser plane signal relative to a plane determined by the first rotation axis and the second rotation axis when the photoelectric sensor detects the second laser plane signal;
determining the distance from a signal transmitting device to the photoelectric sensor according to the first receiving rotation angle, the first continuous rotation angle and the diameter of the photoelectric sensor in combination with the attitude data of the photoelectric sensor; the first laser plane signal and the second laser plane signal are transmitted in time within one signal period, and the phase difference is half of the signal period; the first receiving rotation angle is an angle of the first laser plane signal relative to an XOY plane when the photoelectric sensor starts to detect the first laser plane signal in a signal transmitting period; the first continuous rotation angle is an angle of the first laser plane signal relative to an XOY plane when the photoelectric sensor detects the first laser plane signal finally in a signal emission period;
and determining the three-dimensional coordinates of the photoelectric sensor according to the first rotation angle, the second rotation angle and the distance.
2. The three-dimensional positioning method of claim 1, wherein determining the first rotation angle of the first laser plane signal comprises:
determining a first transmission time of the first laser plane signal;
determining a first receiving time and a first duration time at which the photoelectric sensor detects the first laser plane signal; the first receiving moment is the moment when the photoelectric sensor starts to detect a first laser plane signal; the first duration time is the time when the photoelectric sensor detects the first laser plane signal at last;
and determining the first rotation angle according to the first transmitting time, the first receiving time and the first duration time.
3. The three-dimensional positioning method according to claim 2, wherein the first rotation angle is determined by the following formula:
wherein a is a first rotation angle; t is t2Is a first duration; t is t1Is a first reception time; t is t0Is a first transmission time;ω1is the angular velocity of rotation of the first laser plane signal.
4. The three-dimensional positioning method of claim 1, wherein determining the second rotation angle of the second laser plane signal comprises:
determining a second emission time of the second laser plane signal;
determining a second receiving time and a second duration time at which the photoelectric sensor detects the second laser plane signal; the second receiving moment is the moment when the photoelectric sensor starts to detect a second laser plane signal; the second duration time is the time when the photoelectric sensor detects a second laser plane signal at last;
and determining the second rotation angle according to the second transmitting time, the second receiving time and the second duration time.
5. The three-dimensional positioning method according to claim 4, wherein the second rotation angle is determined by the following formula:
wherein b is a second rotation angle; t is t4Is a second duration; t is t3Is a second reception time; t is t02Is a second transmission time; omega2Is the angular velocity of rotation of the second laser plane signal.
6. The three-dimensional positioning method according to claim 1, characterized in that:
determining a first transmission time of the first laser plane signal;
determining a first receiving time and a first duration time at which the photoelectric sensor detects the first laser plane signal; the first receiving moment is the moment when the photoelectric sensor starts to detect a first laser plane signal; the first duration time is the time when the photoelectric sensor detects the first laser plane signal at last;
determining the first receiving rotation angle according to the first transmitting time and the first receiving time;
and determining the first continuous rotation angle according to the first transmitting time and the first continuous time.
7. The three-dimensional positioning method according to claim 1, wherein the distance from the signal emitting device to the photosensor is determined by the following formula:
wherein Ts is a point on the photoelectric sensor where a first laser plane signal starts to be detected; b is the center of the photoelectric sensor; o is the position of the signal transmitting device; OB is the distance from the signal emitting device to the photoelectric sensor; t issB is the radius of the photoelectric sensor; OTsThe distance between the signal transmitting device and a point on the photoelectric sensor where the first laser plane signal starts to be detected is the distance; angle OTsB ═ 90 ° + the first reception rotation angle + the attitude data of the photosensor.
8. The three-dimensional positioning method according to claim 1, characterized in that:
when the three-dimensional measurement coordinate system is a Cartesian coordinate system, the first rotating shaft is used as a Y axis, the second rotating shaft is used as an X axis, the Z axis is determined by the right-hand rule, and the three-dimensional coordinate of the photoelectric sensor is obtained by solving according to the following formula:
XB 2+YB 2+ZB 2＝OB2；
YB×tanb＝XB×tana＝ZB；
wherein (X)B，YB，ZB) Is a three-dimensional coordinate of the photoelectric sensor, OB is a signal emitting deviceAnd the distances to the photoelectric sensor, a is a first rotation angle, and b is a second rotation angle.
9. A three-dimensional positioning system comprising a processor and a memory; the memory having stored thereon a computer program operable on the processor, the computer program when executed by the processor implementing the steps of:
determining a first rotation angle of a first laser plane signal, wherein the first laser plane signal rotates around a first rotation axis, and the first rotation angle is an angle of the first laser plane signal relative to a plane determined by the first rotation axis and a second rotation axis when the photoelectric sensor detects the first laser plane signal;
determining a second rotation angle of a second laser plane signal, wherein the second laser plane signal rotates around a second rotation axis, and the second rotation angle is an angle of the second laser plane signal relative to a plane determined by the first rotation axis and the second rotation axis when the photoelectric sensor detects the second laser plane signal;
determining the distance from a signal transmitting device to the photoelectric sensor according to the first receiving rotation angle, the first continuous rotation angle and the diameter of the photoelectric sensor in combination with the attitude data of the photoelectric sensor; the first laser plane signal and the second laser plane signal are transmitted in time within one signal period, and the phase difference is half of the signal period; the first receiving rotation angle is an angle of the first laser plane signal relative to an XOY plane when the photoelectric sensor starts to detect the first laser plane signal in a signal transmitting period; the first continuous rotation angle is an angle of the first laser plane signal relative to an XOY plane when the photoelectric sensor detects the first laser plane signal finally in a signal emission period;
and determining the three-dimensional coordinates of the photoelectric sensor according to the first rotation angle, the second rotation angle and the distance.
10. A three-dimensional positioning apparatus comprising the three-dimensional positioning system of claim 9, further comprising a signal transmitter, a signal receiver;
the signal transmitter is used for transmitting a first laser plane signal around a first rotating shaft and transmitting a second laser plane signal around a second rotating shaft;
the signal receiver comprises at least one photoelectric sensor for receiving a first laser plane signal and a second laser plane signal.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810647908.XA CN109031199B (en) | 2018-06-22 | 2018-06-22 | Three-dimensional positioning method, system and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810647908.XA CN109031199B (en) | 2018-06-22 | 2018-06-22 | Three-dimensional positioning method, system and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109031199A CN109031199A (en) | 2018-12-18 |
| CN109031199B true CN109031199B (en) | 2021-04-16 |
Family
ID=64610225
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810647908.XA Active CN109031199B (en) | 2018-06-22 | 2018-06-22 | Three-dimensional positioning method, system and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109031199B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110221247A (en) * | 2019-06-04 | 2019-09-10 | 国家电网有限公司 | A kind of power equipment 3-D positioning method and device |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US3898445A (en) * | 1972-11-30 | 1975-08-05 | Univ Australian | Digitizing device |
| CN102384717B (en) * | 2011-08-17 | 2013-03-13 | 天津大学 | Quick orientating method of work space measuring and positioning system by standard rod |
| CN105607034A (en) * | 2015-12-23 | 2016-05-25 | 北京凌宇智控科技有限公司 | Three-dimensional space detection system, positioning method and system |
| CN106526538B (en) * | 2016-11-25 | 2024-02-20 | 北京凌宇智控科技有限公司 | Positioning base station, positioning system and positioning method |
| CN108061879B (en) * | 2017-11-30 | 2020-10-09 | 歌尔科技有限公司 | Space positioning method and device, electronic equipment and system |
-
2018
- 2018-06-22 CN CN201810647908.XA patent/CN109031199B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN109031199A (en) | 2018-12-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR102133105B1 (en) | 3D spatial detection system, positioning method and system | |
| JP2020533721A5 (en) | ||
| US6176837B1 (en) | Motion tracking system | |
| US9208565B2 (en) | Method and apparatus for estimating three-dimensional position and orientation through sensor fusion | |
| WO2017206571A1 (en) | Target positioning method using laser scanning and laser receiving apparatus | |
| CN110672097B (en) | Indoor positioning and tracking method, device and system based on laser radar | |
| WO1992009904A1 (en) | Absolute position tracker | |
| CN109073740A (en) | Ranging and object positioning system and its application method | |
| CN104613946A (en) | Three-dimensional measuring method and surveying system | |
| CN112348886B (en) | Visual positioning method, terminal and server | |
| CN110262667B (en) | Virtual reality equipment and positioning method | |
| CN106802412A (en) | A kind of closely movable positioning system and its method based on laser and wireless technology | |
| CN106291567A (en) | A kind of laser radar, laser radar system and data processing method | |
| CN108664197A (en) | The localization method and device of interaction point in interactive projection system | |
| CN109031199B (en) | Three-dimensional positioning method, system and device | |
| CN106878944B (en) | Method for calibrating coordinate system of positioning base station and positioning calibration device | |
| WO2020113391A1 (en) | Heading determining method and device, storage medium, and moving platform | |
| CN210952856U (en) | Indoor positioning and tracking device and system based on laser radar | |
| CN104569958A (en) | Target positioning method and system based on ultrasonic wave and inertial navigation combination | |
| CN109031205A (en) | Robotic positioning device, method and robot | |
| CN109828255B (en) | Scanning laser radar device and detection method for detecting and angle synchronizing | |
| CN110376550A (en) | A kind of three dimension location method and system based on position compensation | |
| US20210201011A1 (en) | Data processing method for multi-sensor fusion, positioning apparatus and virtual reality device | |
| CN115248446A (en) | Three-dimensional ultrasonic imaging method and system based on laser radar | |
| US10261185B2 (en) | System and method for remotely measuring distances between two points |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |