CN101598540B - Three-dimensional positioning method and three-dimensional positioning system - Google Patents

Abstract

The invention discloses a three-dimensional positioning method and a three-dimensional positioning system, the three-dimensional positioning method comprises the following steps: acquiring a three-dimensional accelerated speed of a positioning object relative to a first coordinates and an included angle between a coordinate axis of the first coordinates and the direction of the gravity accelerated speed, the first coordinates is a dynamic coordinates with the centre of the positioning object as an original point, the original point of the first coordinates and the direction of the coordinate axis thereof are changed following the movement of the positioning object; acquiring the original speed of the positioning object; determining the displacement of the positioning object on z axis and a horizontal coordinates which is reversely parallel to the direction of the gravity accelerated speed according to the three-dimensional accelerated speed of the positioning object relative to the first coordinates, the included angle between the coordinate axis of the first coordinates and the direction of the gravity accelerated speed, and the original speed of the positioning object. The three-dimensional positioning method and the three-dimensional positioning system of the invention have low dependency on the environment and high reliability.

Description

Three-dimensional positioning method and three-dimensional positioning system

Technical Field

The invention relates to a three-dimensional positioning method and a three-dimensional positioning system.

Background

At present, the three-dimensional positioning technology is developed rapidly, and a plurality of modes are available for measuring three-dimensional motion parameters, wherein the modes mainly comprise an electromagnetic mode and an optical mode.

The electromagnetic mode mainly depends on generating a space magnetic field, and analyzes the motion characteristic by collecting the electrical parameter change generated when the positioning object moves in the space magnetic field. The disadvantage is that the magnetic field is easily disturbed and therefore has high environmental requirements.

The optical mode mainly collects and analyzes the motion characteristic parameters through an optical image processing technology, the three-dimensional positioning method of the optical mode can capture the real-time motion of the positioned object, but the workload of the post-processing (including the identification and tracking of the motion characteristics of the positioned object, the calculation of space coordinates and the like) is large, the cost of optical equipment is high, the positioning error is easy to occur due to interference, and the requirement on the environment is high.

In addition, a positioning method based on GPS and a positioning method based on microwave are available, but the error of the three-dimensional positioning method is large, and the system equipment for realizing the method is complex and has higher cost.

Disclosure of Invention

In order to solve the problems of large dependence on environment and low reliability of a three-dimensional positioning method in the prior art, the invention provides the three-dimensional positioning method which is small in dependence on the environment and high in reliability.

The invention provides a three-dimensional positioning method, which comprises the following steps:

the method comprises the steps that three-dimensional acceleration of a positioning object relative to a first coordinate system is obtained through measurement of an acceleration sensor, an included angle between a coordinate axis of the first coordinate system and the direction of the gravity acceleration is obtained through measurement of an inclination angle sensor, the first coordinate system is a dynamic coordinate system taking the center of the positioning object as an origin, and the origin of the first coordinate system and the direction of the coordinate axis of the first coordinate system change along with the movement of the positioning object;

calculating the initial speed of the positioning object from rest to a certain moment, and acquiring the initial speed of the positioning object;

determining the displacement of the positioning object in a horizontal coordinate system with the z axis in reverse parallel with the gravity acceleration direction according to the three-dimensional acceleration of the positioning object relative to the first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, and the initial speed of the positioning object;

judging whether the speed, the displacement and the change situation of the coordinate calculation value of the positioning object in the horizontal coordinate system meet the corresponding kinematics rule or not; and when the probability that the calculated value does not accord with the corresponding motion law reaches 25%, stopping calculating the currently measured data, and re-measuring the initial speed of the positioned object from the standstill, and re-acquiring the initial speed of the positioned object relative to the three-dimensional acceleration of the first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, so as to determine the displacement of the positioned object in the horizontal coordinate system.

Preferably, in the three-dimensional positioning method, the method for determining the displacement of the positioning object in the horizontal coordinate system includes:

and according to the three-dimensional acceleration of the positioning object relative to a first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, obtaining the three-dimensional acceleration of the positioning object relative to a horizontal coordinate system, and according to the three-dimensional acceleration of the positioning object relative to the horizontal coordinate system and the initial speed of the positioning object, determining the displacement of the positioning object in the horizontal coordinate system.

Preferably, in the three-dimensional positioning method, the step of obtaining the three-dimensional acceleration of the positioning object relative to a first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravitational acceleration direction includes the following steps:

sampling the three-dimensional acceleration of the positioning object relative to a first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction at intervals of sampling time;

and the method for determining the displacement of the positioning object in the horizontal coordinate system comprises the following steps:

and determining the displacement of the positioning object in the horizontal coordinate system after the sampling time interval is finished according to the three-dimensional acceleration of the positioning object relative to the first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, and the initial speed of the positioning object, which are obtained by sampling at the beginning of each sampling time interval.

Preferably, in the three-dimensional positioning method, the method for obtaining the initial speed of the positioning object includes:

and calculating the speed of the positioning object after the nth sampling time interval is ended according to the three-dimensional acceleration of the positioning object relative to the first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, which is obtained by sampling at the beginning of the nth sampling time interval, and the initial speed of the positioning object, which is used as the initial speed of the positioning object at the beginning of the (n + 1) th sampling time interval.

Preferably, in the three-dimensional positioning method, the step of obtaining the three-dimensional acceleration of the positioning object relative to a first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction includes a step of performing error balance processing on the sampled data.

Preferably, in the three-dimensional positioning method, the method for determining the displacement of the positioning object in the horizontal coordinate system includes:

according to the three-dimensional acceleration of the positioning object relative to a first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction and the initial speed of the positioning object, which are obtained by sampling at the beginning of each sampling time interval, the displacement variation of the positioning object in the horizontal coordinate system after the sampling time interval is finished is determined, and according to the displacement variation of the object in the horizontal coordinate system, the displacement of the positioning object in the horizontal coordinate system after the sampling time interval is finished is determined.

Preferably, in the three-dimensional positioning method, the step of determining the displacement of the positioned object in the horizontal coordinate system according to the displacement variation of the object in the horizontal coordinate system specifically includes:

and judging whether the calculated value of the displacement meets a corresponding kinematic rule or not according to the change situation of the calculated value of the displacement of the positioning object in the horizontal coordinate system, stopping calculating the current sampling data when the probability that the calculated value does not accord with the corresponding kinematic rule reaches 25%, re-sampling the object from a standstill, relative to the three-dimensional acceleration of the first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, and re-acquiring the initial speed of the positioning object to determine the displacement of the positioning object in the horizontal coordinate system.

Compared with the prior art, the three-dimensional positioning method obtains the three-dimensional acceleration of the positioning object in the first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, and then determines the displacement of the positioning object in the horizontal coordinate system according to the three-dimensional acceleration of the positioning object in the first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction and the initial speed of the positioning object. The acquisition of the acceleration of the object and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction is not easily influenced by the environment, so the three-dimensional positioning method has small dependence on the environment and high reliability.

Meanwhile, sampling is carried out on the three-dimensional acceleration of the positioning object relative to a first coordinate system and an included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction at intervals of sampling time, then the displacement of the positioning object in a horizontal coordinate system is determined according to data obtained by sampling, and the accuracy of the three-dimensional positioning method can be flexibly set by setting the size of the sampling time interval.

In addition, error balance processing is carried out on the sampled data, and the calculated value of the motion track of the object is detected according to the kinematics rule, so that the calculated result can better accord with the rule of the actual motion of the object, and the positioning is more accurate.

In order to solve the problems of large dependence on environment and low reliability of a three-dimensional positioning system in the prior art, the invention provides the three-dimensional positioning system with small dependence on the environment and high reliability.

The present invention provides a three-dimensional positioning system, comprising: the device comprises an acceleration sensing module, an inclination angle sensing module, an initial speed acquisition module and a data processing module.

The acceleration sensing module is connected with an acceleration sensor and used for measuring and acquiring the three-dimensional acceleration of a positioning object relative to a first coordinate system through the acceleration sensor, the first coordinate system is a dynamic coordinate system taking the center of the positioning object as an origin, and the directions of the origin and a coordinate axis of the first coordinate system are changed along with the movement of the positioning object;

the inclination angle sensing module is connected with an inclination angle sensor and used for measuring and acquiring an included angle between a coordinate axis of the first coordinate system and the gravity acceleration direction through the inclination angle sensor;

the initial speed acquisition module is used for calculating the initial speed of the positioning object from rest to a certain moment and acquiring the initial speed of the positioning object;

the data processing module is connected with the acceleration sensing module, the inclination angle sensing module and the initial speed acquisition module, and is used for determining the displacement of the positioning object in a horizontal coordinate system with the z axis in reverse parallel with the gravity acceleration direction according to the three-dimensional acceleration of the positioning object relative to a first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, and the initial speed of the positioning object;

the detection module judges whether the speed, the displacement and the change condition of the coordinate calculation value of the positioning object in the horizontal coordinate system meet the corresponding kinematics rule or not according to the change condition of the speed, the displacement and the coordinate calculation value of the positioning object in the horizontal coordinate system; when the probability that the calculated value does not accord with the corresponding motion law reaches 25%, the data processing module stops calculating the current data, the acceleration sensing module and the inclination angle sensing module acquire the three-dimensional acceleration of the positioned object from the rest state again and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, the initial speed acquiring module acquires the initial speed of the positioned object again, and the data processing module determines the displacement of the positioned object in the horizontal coordinate system again.

Preferably, in the three-dimensional positioning system, the acceleration sensing module and the inclination sensing module sample the three-dimensional acceleration of the positioned object relative to the first coordinate system and an included angle between a coordinate axis of the first coordinate system and the gravity acceleration direction at intervals of a sampling time, and send the sampled data to the data processing module;

and the data processing module determines the displacement of the positioning object in the horizontal coordinate system after the sampling time interval is finished according to the three-dimensional acceleration of the positioning object relative to the first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, and the initial speed of the positioning object, which are obtained by sampling at the beginning of each sampling time interval.

Preferably, in the three-dimensional positioning system, the data processing module further includes:

and the detection module receives the calculated value of the displacement of the positioning object in the horizontal coordinate system and judges whether the calculated value meets the corresponding kinematics rule or not according to the change condition of the calculated value of the displacement of the positioning object in the horizontal coordinate system. When the probability that the calculated value does not accord with the corresponding kinematics law reaches 25%, the data processing module stops calculating the current sampling data, the sampling module re-samples the object from a standstill and relative to the three-dimensional acceleration of the first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, the initial speed acquisition module re-acquires the initial speed of the positioned object, and the data processing module re-determines the displacement of the positioned object in the horizontal coordinate system.

Compared with the prior art, the three-dimensional positioning system provided by the invention utilizes the acceleration sensing module and the inclination angle sensing module to acquire the three-dimensional acceleration of the positioning object in the first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, then utilizes the initial speed acquisition module to acquire the initial speed of the positioning object, and the data processing module determines the displacement of the positioning object in the horizontal coordinate system. The acceleration sensing module and the inclination angle sensing module are not easily influenced by the environment for obtaining the acceleration and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, so that the three-dimensional positioning system is less dependent on the environment and higher in reliability.

Meanwhile, the sampling module samples the three-dimensional acceleration of the positioning object relative to the first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction at intervals of one sampling time interval, the data processing module determines the displacement of the positioning object in the horizontal coordinate system according to data obtained by sampling each time, and the accuracy of the three-dimensional positioning method can be flexibly set by setting the size of the sampling time interval.

In addition, by setting the detection module, the object calculation result can better accord with the rule of the actual object motion, and the positioning is more accurate.

Drawings

FIG. 1 is a flow chart of a three-dimensional positioning method of the present invention.

Fig. 2 is a block diagram of the structure of the first embodiment of the three-dimensional positioning system of the present invention.

Fig. 3 is a block diagram of the structure of a second embodiment of the three-dimensional positioning system of the present invention.

Detailed Description

Please refer to fig. 1, which is a flowchart illustrating a three-dimensional positioning method according to the present invention.

The three-dimensional positioning method comprises the following steps:

s102, acquiring a three-dimensional acceleration of a positioning object relative to a first coordinate system and an included angle between a coordinate axis of the first coordinate system and the direction of the gravity acceleration, wherein the first coordinate system is a dynamic coordinate system taking the center of the positioning object as an origin, and the origin of the first coordinate system and the direction of the coordinate axis of the first coordinate system change along with the movement of the positioning object;

s104, acquiring the initial speed of the positioning object;

s106, determining the displacement of the positioning object in a horizontal coordinate system according to the three-dimensional acceleration of the positioning object relative to the first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, and the initial speed of the positioning object.

In this embodiment, in step S102, the three-dimensional acceleration of the positioning object in the first coordinate system may be measured by an acceleration sensor, and the coordinate axes and the gravitational acceleration direction of the first coordinate system may be measured by an inclination sensor. In addition, other technical means commonly used in the art may also be used to obtain the three-dimensional acceleration of the positioning object in the first coordinate system, and the coordinate axes and the gravitational acceleration direction of the first coordinate system, which are not listed here.

And when the acceleration sensor is used for measuring the acceleration of the positioning object in the first coordinate system, the acceleration sensor is fixed on the positioning object. The first coordinate system is a dynamic coordinate system with the center of the positioning object as an origin, that is, the directions of the origin of coordinates and the coordinate axes of the first coordinate system change with the movement of the positioning object, and when the positioning object is inclined by a certain angle relative to the horizontal coordinate system, the coordinate axes of the first coordinate system are also inclined relative to the horizontal coordinate system.

Assuming that the measured three-dimensional acceleration of the positioned object in the first coordinate system is: a is_x，a_y，a_z. Wherein, a_xIs the acceleration component of the positioned object on the x-axis of the first coordinate system, a_yIs the acceleration component of the positioned object in the y-axis of the first coordinate system, a_zIs the acceleration component of the positioned object in the z-axis of the first coordinate system.

Meanwhile, the included angle between each coordinate axis of the first coordinate system and the gravity acceleration direction is measured by using an inclination angle sensor:

wherein,is the included angle between the x coordinate axis of the first coordinate system and the gravity acceleration direction,is the included angle between the y coordinate axis of the first coordinate system and the gravity acceleration direction,

and the included angle between the z coordinate axis of the first coordinate system and the gravity acceleration direction is shown.

The horizontal coordinate system, i.e., the x-axis and the y-axis, is in the horizontal plane, and the z-axis is anti-parallel to the gravitational acceleration direction. According to the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, the acceleration of the positioning object in the first coordinate system, which is measured by the acceleration sensor, can be converted into the acceleration of the positioning object in the horizontal coordinate system.

Furthermore, when the acceleration of the positioning object in the first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction are obtained, the acceleration of the positioning object in the first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction can be sampled once at intervals, and the time between two sampling actions is called as a sampling time interval. When the sampling time interval is sufficiently short, the motion of the positioned object within one sampling time interval can be regarded as a uniform acceleration linear motion.

Further, error judgment and balance optimization can be performed on the sampled data of the three-dimensional acceleration of the positioning object relative to a first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction.

When data is measured, measurement errors are generated, and the measurement errors are mainly related to the performance and the use setting of the measuring equipment; when sampling data, there is also a sampling error, which is mainly related to the performance and usage settings of the sampling device itself. The measured or sampled data is subjected to error judgment and optimization, so that the influence caused by the measurement error and the sampling error can be reduced, the measured data is close to the real object motion track as much as possible, and the positioning accuracy is improved.

In step S104, the initial velocity of the positioning object may be obtained by directly measuring the actual velocity of the positioning object, or may be obtained by measuring the acceleration, and the velocity of the object from a standstill to a certain time is calculated as the initial velocity of the time.

As an embodiment of the present invention, there is provided a method for calculating an initial velocity of an object from rest to a certain time by measuring an acceleration, comprising:

when the positioning object is static, the initial speed is zero; when the positioning object moves to a certain moment, the initial speed of the positioning object is V₀. Assuming that the object to be located starts from rest and reaches the time of said certain momentThe interval includes n time intervals dt, and the time of each time interval dt is short enough, the motion of the positioning object in each time interval dt can be regarded as uniform acceleration linear motion, and the formula is as follows:

V_n＝V_n-1+a_n-1dt，V_n-1＝V_n-2+a_n-2dt，......V₀＝0。

wherein, V_nIs the speed of said located object at the end of the nth time interval, a_nIs the speed of said positioned object at the beginning of the nth time interval, dt being the time interval.

The V is_nI.e. the initial velocity of the positioned object at the beginning of the (n + 1) th time interval.

Further, the calculation result of the initial velocity of the positioned object may be stored in a memory, and may be easily read directly from the memory at the time of data processing. For example, the speed V of the object is located at the end of the nth time interval_nStored in memory, then V is calculated_n+1Then V can be read from the storage_nAnd obtaining the following according to a formula:

V_n+1＝V_n+a_ndt

wherein, V_nIs the speed of said located object at the end of the nth time interval, a_nIs the initial velocity of said positioned object at the beginning of the nth time interval, dt being the time interval.

In step S106, determining a displacement of the positioning object in a horizontal coordinate system according to the three-dimensional acceleration of the positioning object relative to the first coordinate system, an included angle between a coordinate axis of the first coordinate system and a gravitational acceleration direction, and the initial velocity of the positioning object.

Firstly, calculating the three-dimensional acceleration of the positioning object in a horizontal coordinate system according to the three-dimensional acceleration of the positioning object relative to the first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction; then, the displacement of the positioning object in the horizontal coordinate system is calculated by using the three-dimensional acceleration of the positioning object in the horizontal coordinate system and the initial velocity of the positioning object.

Calculating the three-dimensional acceleration of the positioning object in the horizontal coordinate system according to the following formula:

wherein,

and

three-dimensional acceleration of the positioned object in the horizontal coordinate system.

For the acceleration component of the positioned object in the x-axis of the horizontal coordinate system,

for the acceleration component of the positioned object in the y-axis of the horizontal coordinate system,

is the acceleration component of the positioned object in the z-axis of the horizontal coordinate system.

In particular, for the case where the acceleration is sampled once every other sampling time interval,

andis the sampled value of the three-dimensional acceleration of the positioned object in the horizontal coordinate system at the beginning of a sampling time interval.

According to the three-dimensional acceleration and the initial speed of the positioning object in the horizontal coordinate system, calculating the displacement variation of the positioning object in the horizontal coordinate system by using the following formula:

$\mathrm{\Δ}{\stackrel{\→}{S}}_{\mathrm{dt}}=\stackrel{\→}{V_{n-1}}\mathrm{dt}+\frac{1}{2}{\stackrel{\→}{a}}_n{\left(\mathrm{dt}\right)}^2---\left(1.4\right)$

wherein,

for the change of the displacement of the positioning object in the horizontal coordinate system from the start to the end of the nth time interval, V_n-1For the start of the nth time interval t, the initial velocity of the object in the horizontal coordinate system is located,

the acceleration of the object in the horizontal coordinate system is located for the start of the nth time interval. Also, the time interval dt is sufficiently short that the motion of the object within the time interval dt is considered to be a uniformly accelerated linear motion.

The quantities represented by the symbols of the above formulas may represent components generated on a certain coordinate axis of the horizontal coordinate system, except for the time interval dt.

At the end of the nth time interval t, the total displacement of the positioning object in the horizontal coordinate system is:

$\stackrel{\→}{S}=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{\left(\mathrm{\Δ}{\stackrel{\→}{S}}_{\mathrm{dt}}\right)}_i$

further, for simplifying the calculation, the displacement of the positioning object in the horizontal coordinate system at the end of the n-1 th time interval can be stored, and then, the coordinate components of the positioning object on each coordinate axis of the horizontal coordinate system at the end of the nth time interval can be represented by the following formula:

$f(\stackrel{\→}{X},T)=X_{T-1}+\mathrm{\Δ}{\stackrel{\→}{S}}_{X_{\mathrm{DT}}}---\left(1.8\right)$

$f(\stackrel{\→}{Y},T)=Y_{T-1}+\mathrm{\Δ}{\stackrel{\→}{S}}_{Y_{\mathrm{DT}}}---\left(1.9\right)$

$f(\stackrel{\→}{Z},T)=Z_{T-1}+\mathrm{\Δ}{\stackrel{\→}{S}}_{Z_{\mathrm{DT}}}---\left(1.10\right)$

wherein,

for locating the displacement, X, of the object on each coordinate axis of said horizontal coordinate system at the end of the Tth time interval_T-1，Y_T-1，Z_T-1For the end of the (n-1) th time interval, the displacement of the object on each coordinate axis of the horizontal coordinate system is located,

and the displacement variation quantity of the positioning object on each coordinate axis of the horizontal coordinate system in the (n + 1) th time interval.

Further, in order to simplify the operation, n consecutive time intervals may be divided into a discrete time interval, and the total displacement of the positioning object in one discrete time interval may be calculated first in the calculation, as shown in formula 1.6.

$\mathrm{\Δ}{\stackrel{\→}{S}}_{n_{\mathrm{DT}}}=\mathrm{\Δ}{\stackrel{\→}{S}}_{{\mathrm{dt}}_1}+\mathrm{\Δ}{\stackrel{\→}{S}}_{{\mathrm{dt}}_2}+...+\mathrm{\Δ}{\stackrel{\→}{S}}_{{\mathrm{dt}}_n}---\left(1.6\right)$

Wherein,

is the displacement variation of the positioning object in a discrete time interval;

is the displacement variation of the positioning object in the nth time interval.

And calculating the total displacement of the coordinates of the positioning object after a discrete time interval according to a formula 1.8-1.10.

Meanwhile, a step of detecting the calculated values of the speed, the displacement, the coordinates and the like of the positioning object in the horizontal coordinate system can be performed.

And judging whether the positioning object meets the corresponding kinematics rule or not according to the change conditions of the speed, the displacement, the coordinate and other calculated values of the positioning object in the horizontal coordinate system. And when the probability that the calculated value does not accord with the corresponding motion law reaches 25%, stopping calculating the currently measured data, and re-measuring the initial speed of the object to be positioned from the standstill, and re-acquiring the initial speed of the object to be positioned relative to the three-dimensional acceleration of the first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, so as to determine the displacement of the object to be positioned in the horizontal coordinate system.

By the steps of judging the calculation result, the object calculation result can better accord with the rule of the actual object motion, and the positioning is more accurate.

Further, after the step S106 is finished, the positioning information (speed, displacement variation, etc.) of the positioning object may be transmitted to another application processing end through the wireless transmission device for post-processing. The post-processing comprises: motion tracking, calculation of spatial coordinates and modeling of data representation, etc. When the positioning information of the positioning object is transmitted by the wireless transmitting device, the positioning information is transmitted according to the common data transmission rules, such as data communication anti-collision, communication error code retransmission mechanism and the like.

Through wireless transmission, the positioning information of the three-dimensional positioning method can be wirelessly transmitted to other processing systems for further processing or application, so that the three-dimensional positioning method has a wider application range.

Compared with the prior art, the three-dimensional positioning method obtains the three-dimensional acceleration of the positioning object in the first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, and then determines the displacement of the positioning object in the horizontal coordinate system according to the three-dimensional acceleration of the positioning object in the first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction and the initial speed of the positioning object. The measurement of the acceleration of the object and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction is not easily influenced by the environment, so the three-dimensional positioning method has small dependence on the environment and high reliability.

Please refer to fig. 2, which is a block diagram illustrating a three-dimensional positioning system according to a first embodiment of the present invention. The three-dimensional positioning system comprises an acceleration sensing module, an inclination angle sensing module, an initial speed acquisition module and a data processing module.

The acceleration sensing module is used for acquiring three-dimensional acceleration of a positioning object relative to a first coordinate system, the first coordinate system is a dynamic coordinate system taking the center of the positioning object as an origin, and the direction of the origin and the direction of a coordinate axis of the first coordinate system change along with the movement of the positioning object.

The inclination angle sensing module is used for acquiring an included angle between a coordinate axis of the first coordinate system and the gravity acceleration direction.

The initial speed acquisition module is used for acquiring the initial speed of the positioning object;

the data processing module is connected with the acceleration sensing module, the inclination angle sensing module and the initial speed acquisition module, and is used for determining the displacement of the positioning object in a horizontal coordinate system with the z axis in reverse parallel with the gravity acceleration direction according to the three-dimensional acceleration of the positioning object relative to a first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, and the initial speed of the positioning object.

In this embodiment, the acceleration sensing module is connected to an acceleration sensor (not shown) for measuring the acceleration of the positioning object in the first coordinate system. The acceleration sensor is fixedly arranged on the positioning object and measures the three-dimensional acceleration of the positioning object relative to the first coordinate system. The first coordinate system is a dynamic coordinate system with the center of the acceleration sensor as an origin, that is, the directions of the origin of coordinates and the coordinate axes of the first coordinate system change with the movement of the positioning object, and when the positioning object is inclined by a certain angle relative to the horizontal coordinate system, the coordinate axes of the first coordinate system are also inclined relative to the horizontal coordinate system.

The tilt angle sensing module is connected with a tilt angle sensor (not shown), and since the direction of the gravitational acceleration is antiparallel to the direction of the z-axis of the horizontal coordinate system, the tilt angle sensor can measure the included angle between the coordinate axis of the first coordinate system and the direction of the gravitational acceleration, and convert the three-dimensional acceleration of the object relative to the first coordinate system into the three-dimensional acceleration of the object relative to the horizontal coordinate system.

Furthermore, the acceleration sensing module and the inclination sensing module may sample the acceleration of the positioning object in the first coordinate system and an included angle between a coordinate axis of the first coordinate system and the direction of the gravitational acceleration at intervals, and transmit the sampled data to the data processing module. The time between two sampling actions is called the sampling time interval, and when the sampling time interval is short enough, the motion of the positioning object in each sampling time interval can be regarded as uniform acceleration linear motion.

The three-dimensional acceleration of the positioning object relative to a first coordinate system and an included angle between a coordinate axis of the first coordinate system and the gravity acceleration direction are sampled once every other sampling time interval, the data processing module determines the displacement of the positioning object in a horizontal coordinate system according to data obtained by each sampling, and the accuracy of the three-dimensional positioning method can be flexibly set by setting the size of the sampling time interval.

The initial speed acquisition module can acquire the initial speed of the positioning object by means of direct measurement or acquisition of a calculated value from the data processing module. The initial speed acquisition module includes a memory. The memory is used for storing the initial speed of the positioning object, and the initial speed can be obtained by the data processing module through calculation. At the next operation, the data processing module can directly read the initial speed of the positioning object from the storage.

The storage in the initial information obtaining module may further store the initial displacement of the positioning object, and the data processing module may determine the displacement of the positioning object according to the initial displacement and the displacement variation of the positioning object. By storing the previous calculation results in the memory, the calculation steps can be simplified, and the positioning can be more rapid.

The data processing module is connected with the acceleration sensing module, the inclination angle sensing module and the initial speed acquisition module, and converts the three-dimensional acceleration of the object relative to the first coordinate system into the three-dimensional acceleration of the object relative to the horizontal coordinate system through calculation processing. And calculating the displacement variation of the object according to the three-dimensional acceleration of the object relative to a horizontal coordinate system and the initial speed of the positioned object.

The data processing module comprises a detection module. The detection module is used for detecting calculated values of the speed, the displacement, the coordinates and the like of the positioning object in the horizontal coordinate system.

And the detection module judges whether the change conditions of the calculated values of the speed, the displacement, the coordinates and the like of the positioning object in the horizontal coordinate system meet the corresponding kinematics rule or not. When the probability that the calculated value does not accord with the corresponding motion law reaches 25%, the data processing module stops calculating the current data, the acceleration sensing module and the inclination angle sensing module acquire the three-dimensional acceleration of the positioned object from the beginning of rest and relative to a first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction again, the initial speed acquiring module acquires the initial speed of the positioned object again, and the data processing module determines the displacement of the positioned object in a horizontal coordinate system again.

The detection module judges the motion trail of the positioning object obtained by calculation according to the kinematics rule, so that the calculation result can better accord with the rule of actual object motion, and the positioning is more accurate.

Compared with the prior art, the three-dimensional positioning system provided by the invention utilizes the acceleration sensing module and the inclination angle sensing module to acquire the three-dimensional acceleration of the positioning object in the first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, then utilizes the initial speed acquisition module to acquire the initial speed of the positioning object, and the data processing module determines the displacement of the positioning object in the horizontal coordinate system. The acceleration sensing module and the inclination angle sensing module are not easily influenced by the environment for obtaining the acceleration and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, so that the three-dimensional positioning system is less dependent on the environment and higher in reliability.

Please refer to fig. 3, which is a block diagram illustrating a structure of a three-dimensional positioning system according to a second embodiment of the present invention. The structure of the three-dimensional positioning system of the second embodiment is basically the same as that of the three-dimensional positioning system of the first embodiment, and the main differences are that: the three-dimensional positioning system further comprises a wireless transmitting device.

The wireless transmitting device is connected with the data processing module, and is used for transmitting the positioning information (speed, displacement variation and the like) of the positioned object to an application processing end outside the positioning system through a wireless signal for post-processing, wherein the post-processing comprises: motion tracking, calculation of spatial coordinates and modeling of data representation, etc. When the positioning information of the positioning object is transmitted by the wireless transmitting device, the transmission is carried out according to the common data transmission rules, such as data communication anti-collision, communication error code retransmission mechanism and the like.

By using the wireless transmission device, the positioning information of the three-dimensional positioning method can be transmitted to other processing systems for further processing or application, so that the application range of the three-dimensional positioning method is wider.

The above-described embodiments of the present invention do not limit the scope of the present invention. Any modification, equivalent replacement, and improvement made within the spirit and principle of the present invention should be included in the scope of the claims of the present invention.

Claims (8)

1. A three-dimensional positioning method, characterized by comprising the steps of:

the method comprises the steps that three-dimensional acceleration of a positioning object relative to a first coordinate system is obtained through measurement of an acceleration sensor, an included angle between a coordinate axis of the first coordinate system and the direction of the gravity acceleration is obtained through measurement of an inclination angle sensor, the first coordinate system is a dynamic coordinate system taking the center of the positioning object as an origin, and the origin of the first coordinate system and the direction of the coordinate axis of the first coordinate system change along with the movement of the positioning object;

calculating the initial speed of the positioning object from rest to a certain moment, and acquiring the initial speed of the positioning object;

determining the displacement of the positioning object in a horizontal coordinate system with the z axis in reverse parallel with the gravity acceleration direction according to the three-dimensional acceleration of the positioning object relative to the first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, and the initial speed of the positioning object;

judging whether the speed, the displacement and the change situation of the coordinate calculation value of the positioning object in the horizontal coordinate system meet the corresponding kinematics rule or not; and when the probability that the calculated value does not accord with the corresponding motion law reaches 25%, stopping calculating the currently measured data, and re-measuring the initial speed of the positioned object from the standstill, and re-acquiring the initial speed of the positioned object relative to the three-dimensional acceleration of the first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, so as to determine the displacement of the positioned object in the horizontal coordinate system.

2. The three-dimensional positioning method according to claim 1, wherein the method of determining the displacement of the positioned object in the horizontal coordinate system is:

and according to the three-dimensional acceleration of the positioning object relative to a first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, obtaining the three-dimensional acceleration of the positioning object relative to a horizontal coordinate system, and according to the three-dimensional acceleration of the positioning object relative to the horizontal coordinate system and the initial speed of the positioning object, determining the displacement of the positioning object in the horizontal coordinate system.

3. The three-dimensional positioning method according to claim 1 or 2, wherein the step of obtaining the three-dimensional acceleration of the positioned object with respect to a first coordinate system and the angle between the coordinate axis of the first coordinate system and the direction of gravitational acceleration comprises the steps of:

sampling the three-dimensional acceleration of the positioning object relative to a first coordinate system and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction at intervals of sampling time;

and the method for determining the displacement of the positioning object in the horizontal coordinate system comprises the following steps:

and determining the displacement of the positioning object in the horizontal coordinate system after the sampling time interval is finished according to the three-dimensional acceleration of the positioning object relative to the first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, and the initial speed of the positioning object, which are obtained by sampling at the beginning of each sampling time interval.

4. The three-dimensional positioning method according to claim 3, wherein the method of acquiring the initial velocity of the positioned object is:

and calculating the speed of the positioning object after the nth sampling time interval is ended according to the three-dimensional acceleration of the positioning object relative to the first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, which is obtained by sampling at the beginning of the nth sampling time interval, and the initial speed of the positioning object, which is used as the initial speed of the positioning object at the beginning of the (n + 1) th sampling time interval.

5. A three-dimensional localization method according to claim 3, wherein the step of obtaining the three-dimensional acceleration of the localized object with respect to a first coordinate system and the angle between the coordinate axis of the first coordinate system and the gravitational acceleration direction comprises the step of performing error-balancing processing on the sampled data.

6. The three-dimensional positioning method according to claim 3, wherein the method of determining the displacement of the positioned object in the horizontal coordinate system is:

according to the three-dimensional acceleration of the positioning object relative to a first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction and the initial speed of the positioning object, which are obtained by sampling at the beginning of each sampling time interval, the displacement variation of the positioning object in the horizontal coordinate system after the sampling time interval is finished is determined, and according to the displacement variation of the object in the horizontal coordinate system, the displacement of the positioning object in the horizontal coordinate system after the sampling time interval is finished is determined.

7. A three-dimensional positioning system, comprising:

the acceleration sensing module is connected with an acceleration sensor and used for measuring and acquiring the three-dimensional acceleration of a positioning object relative to a first coordinate system through the acceleration sensor, the first coordinate system is a dynamic coordinate system taking the center of the positioning object as an origin, and the directions of the origin and a coordinate axis of the first coordinate system are changed along with the movement of the positioning object;

the inclination angle sensing module is connected with an inclination angle sensor and used for measuring and acquiring an included angle between a coordinate axis of the first coordinate system and the gravity acceleration direction through the inclination angle sensor;

the initial speed acquisition module is used for calculating the initial speed of the positioning object from rest to a certain moment and acquiring the initial speed of the positioning object;

the data processing module is connected with the acceleration sensing module, the inclination angle sensing module and the initial speed acquisition module and is used for determining the displacement of the positioning object in a horizontal coordinate system with the z axis in reverse parallel with the gravity acceleration direction according to the three-dimensional acceleration of the positioning object relative to a first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction and the initial speed of the positioning object;

the detection module judges whether the speed, the displacement and the change condition of the coordinate calculation value of the positioning object in the horizontal coordinate system meet the corresponding kinematics rule or not according to the change condition of the speed, the displacement and the coordinate calculation value of the positioning object in the horizontal coordinate system; when the probability that the calculated value does not accord with the corresponding motion law reaches 25%, the data processing module stops calculating the current data, the acceleration sensing module and the inclination angle sensing module acquire the three-dimensional acceleration of the positioned object from the rest state again and the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, the initial speed acquiring module acquires the initial speed of the positioned object again, and the data processing module determines the displacement of the positioned object in the horizontal coordinate system again.

8. The three-dimensional positioning system of claim 7, wherein:

the acceleration sensing module and the inclination angle sensing module sample the three-dimensional acceleration of the positioning object relative to a first coordinate system and an included angle between a coordinate axis of the first coordinate system and the gravity acceleration direction at intervals of sampling time, and send sampling data to the data processing module;

and the data processing module determines the displacement of the positioning object in the horizontal coordinate system after the sampling time interval is finished according to the three-dimensional acceleration of the positioning object relative to the first coordinate system, the included angle between the coordinate axis of the first coordinate system and the gravity acceleration direction, and the initial speed of the positioning object, which are obtained by sampling at the beginning of each sampling time interval.

Images