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

3-D positioning method and 3 D positioning system

Technical field

The present invention relates to a kind of 3-D positioning method and a kind of 3 D positioning system.

Background technology

At present, the development of three-dimensional localization techniques is very fast, and the measurement of three-dimensional motion parameter is also possessed various ways, wherein mainly contains electromagnetic mode and optical mode.

Electromagnetic mode mainly relies on the generation space magnetic field, changes to analyze kinetic characteristic by gather the electrical quantity that occurs when positioning object moves in space magnetic field.Its shortcoming is that magnetic field is disturbed easily, therefore having relatively high expectations for environment.

Optical mode mainly gathers and analyzes the kinetic characteristic parameter by the optical imagery treatment technology, the 3-D positioning method of optical mode can catch the real time kinematics of positioning object, but the workload of its aftertreatment (comprising the calculating of the identification of positioning object motion feature, tracking, volume coordinate etc.) is larger, the optical device cost is also higher, and be disturbed easily and locate mistake, for having relatively high expectations of environment.

Also have in addition based on the localization method of GPS with based on the localization method of microwave, but the error of this type of 3-D positioning method is large, and the system equipment of implementation method is complicated, cost is higher.

Summary of the invention

Large to environmental factor dependence for solving in the prior art 3-D positioning method, the problem that reliability is lower, the invention provides a kind of less to environmental factor dependence, the 3-D positioning method that reliability is higher.

The invention provides a kind of 3-D positioning method, it comprises step:

Obtain positioning object with respect to the three-dimensional acceleration of one first coordinate system by the acceleration transducer measurement, obtain the angle of coordinate axis and the acceleration of gravity direction of described the first coordinate system by the obliquity sensor measurement, described the first coordinate is to take at the center of described positioning object as the dynamic coordinate system of initial point, and the initial point of described the first coordinate system and the direction of its coordinate axis change with the motion of described positioning object;

Calculate described positioning object from static, to the initial velocity in a certain moment, obtain the initial velocity of described positioning object;

According to three-dimensional acceleration and the coordinate axis of described first coordinate system and the angle of acceleration of gravity direction of described positioning object with respect to described the first coordinate system, and the initial velocity of described positioning object, determine the displacement of described positioning object in z axle and the antiparallel horizontal coordinates of acceleration of gravity direction;

According to the situation of change of speed, displacement and the coordinate Calculation value of this positioning object in this horizontal coordinates, judge whether it satisfies corresponding Kinematics Law; When the probability that is not inconsistent when described calculated value and the corresponding characteristics of motion reaches 25%, stop the calculating to the data of current measurement, remeasure object from static, three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction with respect to the first coordinate system, again obtain the initial velocity of described positioning object, determine the displacement of described positioning object in horizontal coordinates.

Preferably, in the described 3-D positioning method, determine that the method for the displacement of positioning object in horizontal coordinates is:

According to three-dimensional acceleration and the coordinate axis of described first coordinate system and the angle of acceleration of gravity direction of described positioning object with respect to the first coordinate system, obtain described positioning object with respect to the three-dimensional acceleration of horizontal coordinates, with respect to the three-dimensional acceleration of horizontal coordinates and the initial velocity of described positioning object, determine the displacement of described positioning object in horizontal coordinates according to described positioning object.

Preferably, in the described 3-D positioning method, obtain positioning object and may further comprise the steps with respect to the step of the angle of the coordinate axis of the three-dimensional acceleration of one first coordinate system and described the first coordinate system and acceleration of gravity direction:

To described positioning object with respect to the angle of the coordinate axis of the three-dimensional acceleration of the first coordinate system and described the first coordinate system and acceleration of gravity direction every a sampling time interval once sampling;

And, determine that the method for the displacement of described positioning object in horizontal coordinates is:

The described positioning object that sampling obtains when beginning according to each sampling time interval is with respect to three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction of the first coordinate system, and the initial velocity of described positioning object, determine the displacement in horizontal coordinates after described sampling time interval finishes of described positioning object.

Preferably, in the described 3-D positioning method, the method for obtaining the initial velocity of positioning object is:

The described positioning object that sampling obtains when beginning according to n sampling time interval is with respect to three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction of the first coordinate system, and the initial velocity of described positioning object, the speed of described positioning object after calculating described n sampling time interval and finishing, and the initial velocity of described positioning object when beginning as n+1 sampling time interval.

Preferably, in the described 3-D positioning method, obtain positioning object and comprise that with respect to the step of the angle of the coordinate axis of the three-dimensional acceleration of one first coordinate system and described the first coordinate system and acceleration of gravity direction the data that a pair of sampling obtains carry out the step that error balance is processed.

Preferably, in the described 3-D positioning method, determine that the method for the displacement of positioning object in horizontal coordinates is:

The described positioning object that sampling obtains when beginning according to each sampling time interval is with respect to three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction of the first coordinate system, and the initial velocity of described positioning object, determine described positioning object displacement variable in horizontal coordinates after described sampling time interval finishes, according to the displacement variable of described object in horizontal coordinates, determine after described sampling time interval finishes the displacement of described positioning object in horizontal coordinates.

Preferably, in the described 3-D positioning method, according to the displacement variable of described object in horizontal coordinates, determine that the step of the displacement of described positioning object in horizontal coordinates specifically comprises:

Situation of change according to this positioning object calculated value of displacement in this horizontal coordinates, whether the calculated value of judging described displacement satisfies corresponding Kinematics Law, when the probability that is not inconsistent when calculated value and corresponding Kinematics Law reaches 25%, stop the calculating to current sampled data, the resampling object is from static, three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction with respect to the first coordinate system, and again obtain the initial velocity of described positioning object, determine the displacement of described positioning object in horizontal coordinates.

Compared with prior art, 3-D positioning method of the present invention obtains the three-dimensional acceleration of positioning object in the first coordinate, and the angle of the coordinate axis of described the first coordinate system and acceleration of gravity direction, again according to three-dimensional acceleration, the coordinate axis of described first coordinate system and the angle of acceleration of gravity direction of described positioning object in the first coordinate system, and the initial velocity of described positioning object, determine the displacement of described positioning object in horizontal coordinates.Owing to obtaining of the angle of the coordinate axis of object acceleration and described the first coordinate system and acceleration of gravity direction is difficult for affected by environment, so 3-D positioning method of the present invention is less to environmental factor dependence, reliability is higher.

Simultaneously, to described positioning object with respect to the angle of the coordinate axis of the three-dimensional acceleration of the first coordinate system and described the first coordinate system and acceleration of gravity direction every a sampling time interval once sampling, then the data that obtain according to sampling, determine the displacement of described positioning object in horizontal coordinates, can by setting the size of this sampling time interval, set neatly the degree of accuracy of this 3-D positioning method.

In addition, the data that sampling is obtained are carried out error balance and are processed, and according to Kinematics Law, and the calculated value of movement locus of object is detected, and can make the rule of the more realistic object of which movement of result of calculation, make the location more accurate.

Large to environmental factor dependence for solving in the prior art 3 D positioning system, the problem that reliability is lower, the invention provides a kind of less to environmental factor dependence, the 3 D positioning system that reliability is higher.

The invention provides a kind of 3 D positioning system, it comprises: an acceleration sensing module, an inclination angle sensing module, an initial velocity acquisition module and a data processing module.

Described acceleration sensing module connects an acceleration transducer, and be used for obtaining positioning object with respect to the three-dimensional acceleration of the first coordinate system by described acceleration transducer measurement, described the first coordinate is to take at the center of described positioning object as the dynamic coordinate system of initial point, and the direction of described the first coordinate origin and coordinate axis changes with the motion of described positioning object;

Described inclination angle sensing module connects an obliquity sensor, is used for obtaining by described obliquity sensor measurement the angle of coordinate axis and the acceleration of gravity direction of described the first coordinate system;

Described initial velocity acquisition module is used for calculating described positioning object from static, to the initial velocity in a certain moment, obtains the initial velocity of described positioning object;

Described data processing module connects described acceleration sensing module, described inclination angle sensing module and described initial velocity acquisition module, be used for according to three-dimensional acceleration and the coordinate axis of described first coordinate system and the angle of acceleration of gravity direction of described positioning object with respect to the first coordinate system, and the initial velocity of described positioning object, determine the displacement of described positioning object in z axle and the antiparallel horizontal coordinates of acceleration of gravity direction;

And a detection module, described detection module judge according to the situation of change of speed, displacement and the coordinate Calculation value of this positioning object in this horizontal coordinates whether it satisfies corresponding Kinematics Law; When the probability that is not inconsistent when described calculated value and the corresponding characteristics of motion reaches 25%, make data processing module stop calculating to current data, make described acceleration sensing module and described inclination angle sensing module again obtain positioning object from static, three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction with respect to the first coordinate system, and making described initial velocity acquisition module again obtain the initial velocity of described positioning object, this data processing module redefines the displacement of described positioning object in horizontal coordinates.

Preferably, in the described 3 D positioning system, this acceleration sensing module and this inclination angle sensing module to described positioning object with respect to the angle of the coordinate axis of the three-dimensional acceleration of the first coordinate system and described the first coordinate system and acceleration of gravity direction every a sampling time interval once sampling, and sampling data transmitting delivered to this data processing module;

The described positioning object that sampling obtained when described data processing module began according to each sampling time interval is with respect to three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction of the first coordinate system, and the initial velocity of described positioning object, determine the displacement in horizontal coordinates after described sampling time interval finishes of described positioning object.

Preferably, in the described 3 D positioning system, described data processing module further comprises:

One detection module, described detection module receives the calculated value of the displacement of this positioning object in this horizontal coordinates, and according to the situation of change of the calculated value of the displacement of this positioning object in this horizontal coordinates, whether it satisfies corresponding Kinematics Law to judge described calculated value.When the probability that is not inconsistent when calculated value and corresponding Kinematics Law reaches 25%, make data processing module stop calculating to current sampled data, make this sampling module resampling object from static, three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction with respect to the first coordinate system, and making described initial velocity acquisition module again obtain the initial velocity of described positioning object, this data processing module redefines the displacement of described positioning object in horizontal coordinates.

Compared with prior art, 3 D positioning system of the present invention utilizes described acceleration sensing module and described inclination angle sensing module, obtain the three-dimensional acceleration of described positioning object in the first coordinate system, and the angle of the coordinate axis of described the first coordinate system and acceleration of gravity direction, recycle the initial velocity that described initial velocity acquisition module obtains described positioning object, described data processing module is determined the displacement of described positioning object in horizontal coordinates.Because described acceleration sensing module and described inclination angle sensing module are difficult for affected by environment to obtaining of the angle of the coordinate axis of acceleration and described the first coordinate system and acceleration of gravity direction, so 3 D positioning system of the present invention is less to environmental factor dependence, reliability is higher.

Simultaneously, described sampling module to described positioning object with respect to the angle of the coordinate axis of the three-dimensional acceleration of the first coordinate system and described the first coordinate system and acceleration of gravity direction every a sampling time interval once sampling, the data that described data processing module obtains according to each sampling, determine the displacement of described positioning object in horizontal coordinates, can by setting the size of this sampling time interval, set neatly the degree of accuracy of this 3-D positioning method.

In addition, by setting described detection module, can make the rule of the more realistic object of which movement of object result of calculation, make the location more accurate.

Description of drawings

Fig. 1 is the process flow diagram of 3-D positioning method of the present invention.

Fig. 2 is the structure block diagram of 3 D positioning system the first embodiment of the present invention.

Fig. 3 is the structure block diagram of 3 D positioning system the second embodiment of the present invention.

Embodiment

See also Fig. 1, it is the process flow diagram of 3-D positioning method of the present invention.

The step of 3-D positioning method of the present invention comprises:

S102 obtains positioning object with respect to three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction of one first coordinate system, described the first coordinate is to take at the center of described positioning object as the dynamic coordinate system of initial point, and the initial point of described the first coordinate system and the direction of its coordinate axis change with the motion of described positioning object;

S104 obtains the initial velocity of described positioning object;

S106 is according to three-dimensional acceleration and the coordinate axis of described first coordinate system and the angle of acceleration of gravity direction of described positioning object with respect to described the first coordinate system, and the initial velocity of described positioning object, determine the displacement of described positioning object in horizontal coordinates.

In the present embodiment, to this step S102, can utilize acceleration transducer to measure described positioning object at the three-dimensional acceleration of described the first coordinate system, simultaneously, utilize obliquity sensor to measure coordinate axis and the acceleration of gravity direction of described the first coordinate system.In addition, can also remove to obtain described positioning object in coordinate axis and the acceleration of gravity direction of three-dimensional acceleration and described first coordinate system of described the first coordinate system with technological means commonly used in other this areas, not enumerate one by one at this.

Utilize acceleration transducer to measure described positioning object when the acceleration of described the first coordinate system, described acceleration transducer is fixed on this positioning object.Described the first coordinate is to take at the center of described positioning object as the dynamic coordinate system of initial point, namely, the true origin of this first coordinate system and the direction of coordinate axis change with the motion of described positioning object, when described positioning object during with respect to horizontal coordinates inclination certain angle, the coordinate axis of this first coordinate system also tilts with respect to horizontal coordinates.

Suppose that the three-dimensional acceleration of described positioning object in described the first coordinate system that measures is: a _x, a _y, a _zWherein, a _xBe the component of acceleration of this positioning object on the x of this first coordinate system axle, a _yBe the component of acceleration of this positioning object on the y of this first coordinate system axle, a _zBe the component of acceleration of this positioning object on the z of this first coordinate system axle.

Simultaneously, utilize obliquity sensor to measure each coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction is:

Wherein, Be the angle of x coordinate axis and the acceleration of gravity direction of described the first coordinate system, Be the angle of y coordinate axis and the acceleration of gravity direction of described the first coordinate system,

Angle for z coordinate axis and the acceleration of gravity direction of described the first coordinate system.

Described horizontal coordinates be x axle and y axle in surface level, and z axle and the antiparallel coordinate system of acceleration of gravity direction.According to the angle of coordinate axis and the acceleration of gravity direction of described the first coordinate system, the positioning object that described acceleration transducer can be measured changes into the acceleration of described positioning object in horizontal coordinates at the acceleration of described the first coordinate system.

Further, when obtaining the angle of positioning object in the coordinate axis of the acceleration of described the first coordinate system and described the first coordinate system and acceleration of gravity direction, can be to positioning object in the angle of the coordinate axis of the acceleration of described the first coordinate system and described the first coordinate system and acceleration of gravity direction once sampling at set intervals, the time between the double sampling action is called sampling time interval.When sampling time interval enough in short-term, the motion of positioning object in a sampling time interval can be regarded uniformly accelrated rectilinear motion as.

Further, can be first the described positioning object that samples be carried out error judgment and balance optimizing with respect to the data of the angle of the coordinate axis of the three-dimensional acceleration of one first coordinate system and described the first coordinate system and acceleration of gravity direction.

When data are measured, have measuring error, it mainly arranges relevant with performance and the use of measuring equipment itself; When data are sampled, also have sampling error, it mainly arranges relevant with performance and the use of sample devices itself.The data of measuring or sample are carried out error judgment and optimization, can reduce the impact that is brought by measuring error and sampling error, make measurement data as far as possible near real movement locus of object, improve the accuracy of location.

In step S104, the initial velocity of described positioning object can obtain by the direct measurement to the actual speed of described positioning object, also can by the measurement to acceleration, calculate object from static, to the speed in a certain moment, as the initial velocity in the described moment.

As a kind of embodiment of the present invention, provide a kind of by the measurement to acceleration, calculate object from static, arrive the method for the initial velocity in a certain moment:

When positioning object was static, its initial velocity was zero; When positioning object moved to a certain moment, the initial velocity of positioning object was V ₀Suppose that positioning object is from static, comprise n time interval dt within the time in the described a certain moment, and the time of each time interval dt is enough short, described positioning object is in each time interval dt, the motion of doing can be regarded uniformly accelrated rectilinear motion as, passes through formula:

V _n＝V _n-1+a _n-1dt，V _n-1＝V _n-2+a _n-2dt，......V ₀＝0。

Wherein, V _nWhen finishing in n time interval, the speed of described positioning object, a _nWhen beginning in n the time interval, the speed of described positioning object, dt is the time interval.

Described V _nBe n+1 the time interval when beginning, the initial velocity of this positioning object.

Further, the result of calculation to the initial velocity of described positioning object can be stored in the storer, can from storer, directly read easily when data are processed carrying out.When for example n the time interval being finished, the speed V of positioning object _nBe stored in the storer, then calculating V _N+1The time, just can from reservoir, read V _n, and obtain according to formula:

V _n+1＝V _n+a _ndt

Wherein, V _nWhen finishing in n time interval, the speed of described positioning object, a _nBe when beginning in n the time interval, the initial velocity of described positioning object, dt are the time interval.

In step S106, according to three-dimensional acceleration and the coordinate axis of described first coordinate system and the angle of acceleration of gravity direction of described positioning object with respect to described the first coordinate system, and the initial velocity of described positioning object, determine the displacement of described positioning object in horizontal coordinates.

At first, according to three-dimensional acceleration and the coordinate axis of described first coordinate system and the angle of acceleration of gravity direction of described positioning object with respect to described the first coordinate system, calculate the three-dimensional acceleration of described positioning object in horizontal coordinates; Then, utilize the three-dimensional acceleration of described positioning object in horizontal coordinates and the initial velocity of positioning object, the displacement of compute location object in horizontal coordinates.

Specifically according to the three-dimensional acceleration of following formula compute location object in described horizontal coordinates:

Wherein,

With

Be the three-dimensional acceleration of this positioning object in described horizontal coordinates.

Be the component of acceleration of this positioning object on the x of horizontal coordinates axle,

Be the component of acceleration of this positioning object on the y of horizontal coordinates axle,

Be the component of acceleration of this positioning object on the z of horizontal coordinates axle.

Especially, for every the situation of a sampling time interval to the acceleration once sampling,

With Be described positioning object when a sampling time interval begins, the sampled value of the three-dimensional acceleration in this horizontal coordinates.

According to three-dimensional acceleration and the initial velocity of described positioning object in horizontal coordinates, utilize the displacement variable of following formula compute location object in described horizontal coordinates:

$\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 since n the time interval to end, described positioning object is at the displacement variable of horizontal coordinates, V _N-1Be when n the time interval, t began, positioning object in horizontal coordinates initial velocity,

Be when beginning in n the time interval, the acceleration of positioning object in horizontal coordinates.And described time interval dt is enough short, makes the motion of object in the time interval dt can regard uniformly accelrated rectilinear motion as.

The amount of above each symbolic representation of formula can be illustrated in the component that produces on a certain coordinate axis of described horizontal coordinates except time interval dt.

When n time interval t finished, the displacement total amount of positioning object in horizontal coordinates was:

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

Further, in order to simplify calculating, can store n-1 time interval when finishing, the displacement of described positioning object in horizontal coordinates, then, the coordinate components of positioning object on each coordinate axis of horizontal coordinates can be represented by the formula when finishing n time interval:

$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,

Be when finishing in T time interval, the displacement of positioning object on each coordinate axis of described horizontal coordinates, X _T-1, Y _T-1, Z _T-1Be when finishing in n-1 time interval, the displacement of positioning object on each coordinate axis of described horizontal coordinates,

Be in n+1 the time interval, the displacement variable of positioning object on each coordinate axis of described horizontal coordinates.

Further, for simplified operation, n continuous time interval can be divided into a discrete time interval, can be first with in the discrete time interval during calculating, the displacement calculation of total of described positioning object out, 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,

Be in the discrete time interval, the displacement variable of described positioning object;

Be in n the time interval, the displacement variable of described positioning object.

After calculating a discrete time interval according to formula 1.8-1.10 again, the displacement total amount of the coordinate of positioning object.

Simultaneously, can carry out the step that the calculated values such as speed, displacement and coordinate of a pair of this positioning object in this horizontal coordinates detects.

According to the situation of change of the calculated values such as speed, displacement and coordinate of this positioning object in this horizontal coordinates, judge whether it satisfies corresponding Kinematics Law.When the probability that is not inconsistent when calculated value and the corresponding characteristics of motion reaches 25%, stop the calculating to the data of current measurement, remeasure object from static, three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction with respect to the first coordinate system, again obtain the initial velocity of described positioning object, determine the displacement of described positioning object in horizontal coordinates.

By the above-mentioned step that result of calculation is judged, can make the rule of the more realistic object of which movement of object result of calculation, make the location more accurate.

Further, after this step S106 finishes, the locating information (speed, displacement and displacement variable etc.) of described positioning object can be sent to other application end for process by wireless base station apparatus and carry out aftertreatment.Described aftertreatment comprises: motion tracking, the calculating of volume coordinate and the modeling of data representation etc.When the locating information of this positioning object is transmitted by described wireless base station apparatus, transmit according to common data transmission rule, such as data communication anti-collision and the error code retransmission mechanism etc. of communicating by letter.

By wireless transmission, the locating information of this 3-D positioning method can be wirelessly transmitted in other the disposal system, do to process further or use, make the usable range of this 3-D positioning method more extensive.

Compared with prior art, 3-D positioning method of the present invention obtains the three-dimensional acceleration of described positioning object in the first coordinate, and the angle of the coordinate axis of described the first coordinate system and acceleration of gravity direction, again according to three-dimensional acceleration, the coordinate axis of described first coordinate system and the angle of acceleration of gravity direction of described positioning object in the first coordinate system, and the initial velocity of described positioning object, determine the displacement of described positioning object in horizontal coordinates.Owing to the measurement of the angle of the coordinate axis of object acceleration and described the first coordinate system and acceleration of gravity direction is difficult for affected by environment, so 3-D positioning method of the present invention is less to environmental factor dependence, reliability is higher.

See also Fig. 2, it is the structure block diagram of 3 D positioning system the first embodiment of the present invention.This 3 D positioning system comprises an acceleration sensing module, an inclination angle sensing module, an initial velocity acquisition module and a data processing module.

This acceleration sensing module, be used for obtaining positioning object with respect to the three-dimensional acceleration of the first coordinate system, described the first coordinate is to take at the center of described positioning object as the dynamic coordinate system of initial point, and the initial point of described the first coordinate system and the direction of coordinate axis change with the motion of described positioning object.

This inclination angle sensing module is for the angle of coordinate axis and the acceleration of gravity direction of obtaining described the first coordinate system.

This initial velocity acquisition module, it is used for obtaining the initial velocity of described positioning object;

This data processing module, it connects described acceleration sensing module, described inclination angle sensing module and described initial velocity acquisition module, be used for according to three-dimensional acceleration and the coordinate axis of described first coordinate system and the angle of acceleration of gravity direction of described positioning object with respect to the first coordinate system, and the initial velocity of described positioning object, determine the displacement of described positioning object in z axle and the antiparallel horizontal coordinates of acceleration of gravity direction.

In the present embodiment, this acceleration sensing module connects an acceleration transducer (not shown), and described acceleration transducer is used for the measurement and positioning object at the acceleration of described the first coordinate system.Described acceleration transducer is fixedly installed on this positioning object, and the measurement and positioning object is with respect to the three-dimensional acceleration of the first coordinate system.Described the first coordinate is to take at the center of described acceleration transducer as the dynamic coordinate system of initial point, namely, the true origin of this first coordinate system and the direction of coordinate axis change with the motion of described positioning object, when described positioning object during with respect to horizontal coordinates inclination certain angle, the coordinate axis of this first coordinate system also tilts with respect to horizontal coordinates.

Described inclination angle sensing module connects an obliquity sensor (not shown), because the direction of the direction of described acceleration of gravity and the z axle of horizontal coordinates is antiparallel, so can measure by obliquity sensor the angle of coordinate axis and the acceleration of gravity direction of described the first coordinate system, with the three-dimensional acceleration of described object with respect to the first coordinate system, change into described object with respect to the three-dimensional acceleration of horizontal coordinates.

Further, described acceleration sensing module and inclination angle sensing module can be to acceleration and the coordinate axis of described first coordinate system and the angle of acceleration of gravity direction of described positioning object at described the first coordinate system, once sampling at set intervals, and with the data transmission of sampling to this data processing module.Time between the double sampling action is called sampling time interval, when sampling time interval enough in short-term, the motion of positioning object in each sampling time interval can be regarded uniformly accelrated rectilinear motion as.

To described positioning object with respect to the angle of the coordinate axis of the three-dimensional acceleration of the first coordinate system and described the first coordinate system and acceleration of gravity direction every a sampling time interval once sampling, the data that described data processing module obtains according to each sampling, determine the displacement of described positioning object in horizontal coordinates, can by setting the size of this sampling time interval, set neatly the degree of accuracy of this 3-D positioning method.

This initial velocity acquisition module can or obtain the initial velocity that the modes such as calculated value are obtained positioning object by direct measurement from data processing module.This initial velocity acquisition module comprises a reservoir.Described reservoir is used for storing the initial velocity of described positioning object, and this initial velocity can be obtained through computing by this data processing module.When next time computing, this data processing module can directly read the initial velocity of described positioning object from this reservoir.

Reservoir in this initial information acquisition module can further store the initial displacement of described positioning object, and described data processing module can be determined according to the displacement variable of described initial displacement and positioning object the displacement of described positioning object.Store result of calculation before by described reservoir, can simplify calculation procedure, make the location rapider.

This data processing module connects described acceleration sensing module, described inclination angle sensing module and described initial velocity acquisition module, this data processing module is by computing, with the three-dimensional acceleration of described object with respect to the first coordinate system, change into described object with respect to the three-dimensional acceleration of horizontal coordinates.With respect to the three-dimensional acceleration of horizontal coordinates and the initial velocity of described positioning object, calculate the displacement variable of object according to described object.

Described data processing module comprises a detection module.Described detection module is used for the described positioning object that calculates is detected at the calculated value of speed, displacement and the coordinate etc. of this horizontal coordinates.

Described detection module judges according to the situation of change of the calculated values such as speed, displacement and coordinate of this positioning object in this horizontal coordinates whether it satisfies corresponding Kinematics Law.When the probability that is not inconsistent when calculated value and the corresponding characteristics of motion reaches 25%, make data processing module stop calculating to current data, make described acceleration sensing module and described inclination angle sensing module again obtain positioning object from static, three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction with respect to the first coordinate system, and making described initial velocity acquisition module again obtain the initial velocity of described positioning object, this data processing module redefines the displacement of described positioning object in horizontal coordinates.

Described detection module is according to Kinematics Law, and the movement locus of the described positioning object that calculates is judged, can make the rule of the more realistic object of which movement of result of calculation, makes the location more accurate.

Compared with prior art, 3 D positioning system of the present invention utilizes described acceleration sensing module and described inclination angle sensing module, obtain the three-dimensional acceleration of described positioning object in the first coordinate system, and the angle of the coordinate axis of described the first coordinate system and acceleration of gravity direction, recycle the initial velocity that described initial velocity acquisition module obtains described positioning object, described data processing module is determined the displacement of described positioning object in horizontal coordinates.Because described acceleration sensing module and described inclination angle sensing module are difficult for affected by environment to obtaining of the angle of the coordinate axis of acceleration and described the first coordinate system and acceleration of gravity direction, so 3 D positioning system of the present invention is less to environmental factor dependence, reliability is higher.

See also Fig. 3, it is the block diagram of the second embodiment structure of 3 D positioning system of the present invention.The structure of the structure of the 3 D positioning system of described the second embodiment and this first embodiment 3 D positioning system is basic identical, and its key distinction is: described 3 D positioning system further comprises a wireless base station apparatus.

Described wireless base station apparatus connects described data processing module, it is used for the locating information (speed, displacement, displacement variable etc.) of this positioning object is carried out aftertreatment by the application end for process that transmission of wireless signals to is positioned at outside the described positioning system, described aftertreatment comprises: motion tracking, the calculating of volume coordinate and the modeling of data representation etc.When the locating information of this positioning object is passed through described wireless base station apparatus transmission, transmit according to common data transmission rule, such as data communication anti-collision and the error code retransmission mechanism etc. of communicating by letter.

By using described wireless base station apparatus, the locating information of this 3-D positioning method can be sent in other the disposal system, do to process further or use, make the usable range of this 3-D positioning method more extensive.

Above-described embodiment of the present invention does not consist of the restriction to protection domain of the present invention.Any modification of doing within the spirit and principles in the present invention, be equal to and replace and improvement etc., all should be included within the claim protection domain of the present invention.

Claims (8)

1. 3-D positioning method is characterized in that comprising step:

Obtain positioning object with respect to the three-dimensional acceleration of one first coordinate system by the acceleration transducer measurement, obtain the angle of coordinate axis and the acceleration of gravity direction of described the first coordinate system by the obliquity sensor measurement, described the first coordinate is to take at the center of described positioning object as the dynamic coordinate system of initial point, and the initial point of described the first coordinate system and the direction of its coordinate axis change with the motion of described positioning object;

Calculate described positioning object from static, to the initial velocity in a certain moment, obtain the initial velocity of described positioning object;

According to three-dimensional acceleration and the coordinate axis of described first coordinate system and the angle of acceleration of gravity direction of described positioning object with respect to described the first coordinate system, and the initial velocity of described positioning object, determine the displacement of described positioning object in z axle and the antiparallel horizontal coordinates of acceleration of gravity direction;

According to the situation of change of speed, displacement and the coordinate Calculation value of this positioning object in this horizontal coordinates, judge whether it satisfies corresponding Kinematics Law; When the probability that is not inconsistent when described calculated value and the corresponding characteristics of motion reaches 25%, stop the calculating to the data of current measurement, remeasure object from static, three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction with respect to the first coordinate system, again obtain the initial velocity of described positioning object, determine the displacement of described positioning object in horizontal coordinates.

2. 3-D positioning method as claimed in claim 1 is characterized in that, the method for the displacement of described definite positioning object in horizontal coordinates is:

According to three-dimensional acceleration and the coordinate axis of described first coordinate system and the angle of acceleration of gravity direction of described positioning object with respect to the first coordinate system, obtain described positioning object with respect to the three-dimensional acceleration of horizontal coordinates, with respect to the three-dimensional acceleration of horizontal coordinates and the initial velocity of described positioning object, determine the displacement of described positioning object in horizontal coordinates according to described positioning object.

3. 3-D positioning method as claimed in claim 1 or 2 is characterized in that, the described positioning object that obtains may further comprise the steps with respect to the step of the angle of the coordinate axis of the three-dimensional acceleration of one first coordinate system and described the first coordinate system and acceleration of gravity direction:

To described positioning object with respect to the angle of the coordinate axis of the three-dimensional acceleration of the first coordinate system and described the first coordinate system and acceleration of gravity direction every a sampling time interval once sampling;

And, determine that the method for the displacement of described positioning object in horizontal coordinates is:

The described positioning object that sampling obtains when beginning according to each sampling time interval is with respect to three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction of the first coordinate system, and the initial velocity of described positioning object, determine the displacement in horizontal coordinates after described sampling time interval finishes of described positioning object.

4. 3-D positioning method as claimed in claim 3 is characterized in that, the described method of obtaining the initial velocity of positioning object is:

The described positioning object that sampling obtains when beginning according to n sampling time interval is with respect to three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction of the first coordinate system, and the initial velocity of described positioning object, the speed of described positioning object after calculating described n sampling time interval and finishing, and the initial velocity of described positioning object when beginning as n+1 sampling time interval.

5. 3-D positioning method as claimed in claim 3, it is characterized in that the described positioning object that obtains comprises that with respect to the step of the angle of the coordinate axis of the three-dimensional acceleration of one first coordinate system and described the first coordinate system and acceleration of gravity direction the data that a pair of sampling obtains carry out the step that error balance is processed.

6. 3-D positioning method as claimed in claim 3 is characterized in that, the method for the displacement of described definite positioning object in horizontal coordinates is:

The described positioning object that sampling obtains when beginning according to each sampling time interval is with respect to three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction of the first coordinate system, and the initial velocity of described positioning object, determine described positioning object displacement variable in horizontal coordinates after described sampling time interval finishes, according to the displacement variable of described object in horizontal coordinates, determine after described sampling time interval finishes the displacement of described positioning object in horizontal coordinates.

7. a 3 D positioning system is characterized in that, this 3 D positioning system comprises:

One acceleration sensing module, it connects an acceleration transducer, and be used for obtaining positioning object with respect to the three-dimensional acceleration of the first coordinate system by described acceleration transducer measurement, described the first coordinate is to take at the center of described positioning object as the dynamic coordinate system of initial point, and the direction of described the first coordinate origin and coordinate axis changes with the motion of described positioning object;

One inclination angle sensing module, it connects an obliquity sensor, is used for obtaining by described obliquity sensor measurement the angle of coordinate axis and the acceleration of gravity direction of described the first coordinate system;

One initial velocity acquisition module, it is used for calculating described positioning object from static, to the initial velocity in a certain moment, obtains the initial velocity of described positioning object;

One data processing module, it connects described acceleration sensing module, described inclination angle sensing module and described initial velocity acquisition module, be used for according to three-dimensional acceleration and the coordinate axis of described first coordinate system and the angle of acceleration of gravity direction of described positioning object with respect to the first coordinate system, and the initial velocity of described positioning object, determine the displacement of described positioning object in z axle and the antiparallel horizontal coordinates of acceleration of gravity direction;

And a detection module, described detection module judge according to the situation of change of speed, displacement and the coordinate Calculation value of this positioning object in this horizontal coordinates whether it satisfies corresponding Kinematics Law; When the probability that is not inconsistent when described calculated value and the corresponding characteristics of motion reaches 25%, make data processing module stop calculating to current data, make described acceleration sensing module and described inclination angle sensing module again obtain positioning object from static, three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction with respect to the first coordinate system, and making described initial velocity acquisition module again obtain the initial velocity of described positioning object, this data processing module redefines the displacement of described positioning object in horizontal coordinates.

8. 3 D positioning system as claimed in claim 7 is characterized in that:

This acceleration sensing module and this inclination angle sensing module to described positioning object with respect to the angle of the coordinate axis of the three-dimensional acceleration of the first coordinate system and described the first coordinate system and acceleration of gravity direction every a sampling time interval once sampling, and sampling data transmitting delivered to this data processing module;

The described positioning object that sampling obtained when described data processing module began according to each sampling time interval is with respect to three-dimensional acceleration and the coordinate axis of described the first coordinate system and the angle of acceleration of gravity direction of the first coordinate system, and the initial velocity of described positioning object, determine the displacement in horizontal coordinates after described sampling time interval finishes of described positioning object.

Images