CN106123901B - A kind of localization method and device - Google Patents
A kind of localization method and device Download PDFInfo
- Publication number
- CN106123901B CN106123901B CN201610573619.0A CN201610573619A CN106123901B CN 106123901 B CN106123901 B CN 106123901B CN 201610573619 A CN201610573619 A CN 201610573619A CN 106123901 B CN106123901 B CN 106123901B
- Authority
- CN
- China
- Prior art keywords
- target object
- linear
- linear velocity
- linear acceleration
- sensor
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Abstract
The present invention relates to field of computer technology, more particularly to a kind of localization method and device, it include: the linear velocity and linear acceleration of the target object that the sensor on acquisition target object is calculated, and using the linear velocity of the target object and linear acceleration as the control function of extended Kalman filter；The first position posture for the target object that photographic device is calculated is obtained, and using the first position posture of the target object as the measurand of extended Kalman filter；According to the control function of determining extended Kalman filter and the measurand of extended Kalman filter, the second position posture of target object is obtained.The method that the embodiment of the present invention proposes a kind of position and attitude of completely new calculating target object, due to the recursive nature of extended Kalman filter itself, in the position and attitude for determining target object, twice adjacent calculation is the result is that be closely related, so as to obtaining more accurate position and attitude.
Description
Technical field
The present invention relates to field of computer technology more particularly to a kind of localization methods and device.
Background technique
Virtual reality is pursued as the nearly following most very powerful and exceedingly arrogant display technology by scientific and technological circle, and virtual reality space positions skill
Art has become the key link, while being also technological difficulties.
Currently in order to realizing accurate positioning to target object, mainly have optical positioning method, Space-Based Radar scan method,
Laser Scanning etc..These methods can realize to a certain extent in space target object it is accurate, but these
All there is registration deficiency in method, cannot achieve the accurate positioning to target object.
In conclusion there is positioning accuracy deficiency in the target object localization method of the prior art.
Summary of the invention
The present invention provides a kind of localization method and device, existing in the prior art determines target object to solve
The technical problem of positioning accuracy deficiency when position.
On the one hand, the embodiment of the present invention provides a kind of localization method, comprising:
Obtain the linear velocity and linear acceleration of target object that the sensor on target object is calculated, and by the mesh
Mark the control function of the linear velocity and linear acceleration of object as extended Kalman filter；
Obtain the first position posture of the target object that photographic device is calculated, and by the of the target object
Measurand of one position and attitude as extended Kalman filter；
According to the measurement of the control function of the determining extended Kalman filter and the extended Kalman filter
Variable obtains the second position posture of the target object.
Optionally, when the photographic device can take the target object, the linear velocity of the target object and
Linear acceleration is obtained according to following manner:
Obtain the first linear velocity and the first linear acceleration of the target object that the photographic device is calculated；
According to the line for the target object that first linear velocity and the first linear acceleration respectively get the sensor
Speed and linear acceleration are updated, and by the linear velocity and linear acceleration of the updated target object, as the biography
The linear velocity and linear acceleration for the target object that sensor is calculated；
When the photographic device can not take the target object, the linear velocity and line of the target object accelerate
Degree is calculated according to following manner:
The linear velocity and linear acceleration for the target object that the sensor is got are calculated as the sensor
The linear velocity and linear acceleration of obtained target object.
Optionally, when the photographic device can take the target object, the first position of the target object
Posture is obtained according to following manner:
The target object is shot by the photographic device, and utilizes optical positioning method, obtains the mesh
Mark the first position posture of object；
When the photographic device can not take the target object, the first position posture of the target object is
It is obtained according to following manner:
The position and attitude for the target object that described photographic device the last time is got, as the target object
First position posture.
Optionally, the mesh that the sensor is got respectively according to first linear velocity and the first linear acceleration
The linear velocity and linear acceleration for marking object are updated, comprising:
The linear velocity for the target object that the sensor is got is added with linear differential magnitude, obtains updated institute
The linear velocity of target object is stated, the linear differential magnitude is the history line that the target object is got according to the sensor
The first linear velocity of history that speed, the linear velocity of the target object and the photographic device are got, the target object
What the first linear velocity obtained；
The linear acceleration for the target object that the sensor is got is added with linear acceleration quantity value, after obtaining update
The target object linear acceleration, the linear acceleration quantity value is the target pair got according to the sensor
The history First Line that the history linear acceleration of elephant, the linear acceleration of the target object and the photographic device are got accelerates
It spends, the first linear acceleration of the target object obtains.
Optionally, the parameter of the extended Kalman filter meets following constraint condition:
Gain matrix
Gain matrix
Gain matrix
Procedure activation noise covariance matrix
Observation noise covariance matrix
Wherein, a, b, c, d, e are preset constant.
On the other hand, the embodiment of the present invention provides a kind of positioning device, comprising:
First acquisition unit, the linear velocity and line of the target object that the sensor for obtaining on target object is calculated
Acceleration, and using the linear velocity of the target object and linear acceleration as the control function of extended Kalman filter；
Second acquisition unit, for obtaining the first position posture for the target object that photographic device is calculated, and
Using the first position posture of the target object as the measurand of extended Kalman filter；
Positioning unit, for the control function and the spreading kalman according to the determining extended Kalman filter
The measurand of filter obtains the second position posture of the target object.
Optionally, described device further includes the first computing unit, specifically for that can take institute when the photographic device
When stating target object, the linear velocity and linear acceleration of the target object are calculated according to following manner:
Obtain the first linear velocity and the first linear acceleration of the target object that the photographic device is calculated；
According to the line for the target object that first linear velocity and the first linear acceleration respectively get the sensor
Speed and linear acceleration are updated, and by the linear velocity and linear acceleration of the updated target object, as the biography
The linear velocity and linear acceleration for the target object that sensor is calculated；
First computing unit is also used to when the photographic device can not take the target object, according to
The linear velocity and linear acceleration of the target object is calculated in following manner:
The linear velocity and linear acceleration for the target object that the sensor is got are calculated as the sensor
The linear velocity and linear acceleration of obtained target object.
Optionally, described device further includes the second computing unit, specifically for that can take institute when the photographic device
When stating target object, the first position posture of the target object is calculated according to following manner:
The target object is shot by the photographic device, and utilizes optical positioning method, obtains the mesh
Mark the first position posture of object；
Second computing unit is also used to when the photographic device can not take the target object, according to
The first position posture of the target object is calculated in following manner:
The position and attitude for the target object that described photographic device the last time is got, as the target object
First position posture.
Optionally, first computing unit, is specifically used for:
The linear velocity for the target object that the sensor is got is added with linear differential magnitude, obtains updated institute
The linear velocity of target object is stated, the linear differential magnitude is the history line that the target object is got according to the sensor
The first linear velocity of history that speed, the linear velocity of the target object and the photographic device are got, the target object
What the first linear velocity obtained；
The linear acceleration for the target object that the sensor is got is added with linear acceleration quantity value, after obtaining update
The target object linear acceleration, the linear acceleration quantity value is the target pair got according to the sensor
The history First Line that the history linear acceleration of elephant, the linear acceleration of the target object and the photographic device are got accelerates
It spends, the first linear acceleration of the target object obtains.Optionally, the parameter of the extended Kalman filter meets following
Constraint condition:
Gain matrix
Gain matrix
Gain matrix
Procedure activation noise covariance matrix
Observation noise covariance matrix
Wherein, a, b, c, d, e are preset constant.
The embodiment of the present invention, the linear velocity and line for obtaining the target object that the sensor on target object is calculated accelerate
Degree, and using the linear velocity of the target object and linear acceleration as the control function of extended Kalman filter；Obtain camera shooting
The first position posture for the target object that device is calculated, and using the first position posture of the target object as expansion
Open up the measurand of Kalman filter；According to the control function and the expansion card of the determining extended Kalman filter
The measurand of Thalmann filter obtains the second position posture of the target object.The embodiment of the present invention proposes a kind of completely new
Calculating target object position and attitude method, that is, combine optical positioning method, sensor positioning method and spreading kalman
Filter, due to the recursive nature of extended Kalman filter itself, is determining target to determine the position and attitude of target object
When the position and attitude of object, twice adjacent calculation is the result is that be closely related, and the movement of target object in practical applications
It is to have successional, therefore the position and attitude of two neighboring position is also to be closely related, to be filtered using spreading kalman
The available accurate position and attitude of wave device.
Detailed description of the invention
To describe the technical solutions in the embodiments of the present invention more clearly, make required in being described below to embodiment
Attached drawing is briefly introduced, it should be apparent that, drawings in the following description are only some embodiments of the invention, for this
For the those of ordinary skill in field, without any creative labor, it can also be obtained according to these attached drawings
His attached drawing.
Fig. 1 is a kind of localization method flow chart provided in an embodiment of the present invention；
Fig. 2 is localization method flow diagram provided in an embodiment of the present invention；
Fig. 3 is positioning device schematic diagram provided in an embodiment of the present invention；
Fig. 4 is a kind of localization method detail flowchart provided in an embodiment of the present invention；
Fig. 5 is a kind of positioning device schematic diagram provided in an embodiment of the present invention.
Specific embodiment
To make the objectives, technical solutions, and advantages of the present invention clearer, below in conjunction with attached drawing to the present invention make into
It is described in detail to one step, it is clear that described embodiments are only a part of the embodiments of the present invention, rather than whole implementation
Example.Based on the embodiments of the present invention, obtained by those of ordinary skill in the art without making creative efforts
All other embodiment, shall fall within the protection scope of the present invention.
The embodiment of the present invention is described in further detail with reference to the accompanying drawings of the specification.
As shown in Figure 1, a kind of localization method method provided in an embodiment of the present invention, comprising:
Step 101, the linear velocity and linear acceleration for obtaining the target object that the sensor on target object is calculated, and
Using the linear velocity of the target object and linear acceleration as the control function of extended Kalman filter；
Step 102 obtains the first position posture of the target object that photographic device is calculated, and by the target
Measurand of the first position posture of object as extended Kalman filter；
Step 103, the control function according to the determining extended Kalman filter and the Extended Kalman filter
The measurand of device obtains the second position posture of the target object.
In the embodiment of the present invention, target object refers to the object for needing to carry out position and attitude positioning, what position and attitude referred to
It is the spatial coordinate location of target object, such as needs to position VR (Virtual Reality, virtual reality) helmet,
Then target object is the VR helmet.
When positioning to target object, it is generally obtained the spatial positional information of the target object, spatial position
Information is made of 9 magnitudes, is respectively: linear velocity, linear acceleration and the position and attitude of X-direction, the linear velocity of Y direction,
Linear acceleration and position and attitude, linear velocity, linear acceleration and the position and attitude of Z-direction.
In the embodiment of the present invention, what final needs obtained is position and attitude (the i.e. position of X-direction of a target object
Posture, the position and attitude of Y direction and the position and attitude of Z-direction).
With reference to Fig. 2, it is localization method flow diagram provided in an embodiment of the present invention, combines two kinds of target objects
Localization method is calculated the position and attitude of final goal object.
First method is that the spatial positional information of target object is calculated by optical positioning method.How to pass through
Optical positioning method obtains the spatial positional information of target object, belongs to the prior art, does not do and excessively repeats herein.Optical alignment
Method substantially process can refer to Fig. 3, and wherein Fig. 3 is positioning device schematic diagram provided in an embodiment of the present invention, wherein target pair
As being equipped at least one LED on (in figure by taking target object is the VR helmet as an example), (Light Emitting Diode shines two
Pole pipe) indicator light, LED light can issue detection light, then be shot by photographic device to target object, pass through calculating
The spatial positional information of LED light in the image taken, to obtain the spatial positional information of target object.
Second method is that the spatial positional information of target object is calculated by sensor positioning method.How to lead to
It crosses sensor positioning method and obtains the spatial positional information of target object, also belong to the prior art, do not do and excessively repeat herein.It passes
Sensor localization method substantially process can refer to Fig. 3, wherein and sensor refers to the sensor inside target object,
In Fig. 3 with sensor be 9 axis IMU (Inertial measurement unit, Inertial Measurement Unit) for be illustrated.It is logical
Cross the 3D gyroscope in 9 axis IMU, 3D accelerometer, 3D earth magnetism is calculated the spatial positional information of target object.
The method of the above two spatial positional information that target object is calculated has the advantages that respective:
The advantages of the first optical positioning method are as follows: the position and attitude for the target object being calculated is more accurate.
The advantages of second of sensor positioning method are as follows: calculating speed is very fast.
The following problems exist respectively for the method for the above two spatial positional information that target object is calculated:
The first optical positioning method the problem is that: due to shooting angle or the original of target object displacement
Cause, photographic device can not take target object sometimes, therefore will cause when photographic device shoots fall short object, with regard to nothing
The spatial positional information of target object is calculated in method, and then is unable to get the position and attitude of target object.
Second of sensor positioning method the problem is that: due to sensor positioning method itself, calculate knot
Fruit is accurate not as good as the result that optical positioning method calculates.
Therefore, the embodiment of the present invention combines the advantage of both localization methods respectively, proposes the new target object of one kind
Localization method.
Specifically, in a step 101, obtain the linear velocity of target object that the sensor on target object is calculated and
Linear acceleration, and using the linear velocity of the target object and linear acceleration as the control function of extended Kalman filter.
In above-mentioned steps 102, the first position posture for the target object that photographic device is calculated is obtained, and will
Measurand of the first position posture of the target object as extended Kalman filter.
In above-mentioned steps 103, according to the control function and the expansion card of the determining extended Kalman filter
The measurand of Thalmann filter obtains the second position posture of the target object.
Extended Kalman filter is simply introduced first below.
Extended Kalman filter is mainly made of following two renewal process:
Time update equation:
State renewal equation:
In above-mentioned 5 formula,(- represent priori, ^ represent estimation) it is the state feelings before known kth step
The prior state estimation that kth walks under condition；For known measurand zkWhen kth step posteriority state estimation.
uk-1For control function,For the covariance of prior estimate error, PkFor the covariance of Posterior estimator error, zkFor
Measurand, KkIt is called remaining gain or mixing factor.
A, B, H are gain matrix, and Q is procedure activation noise covariance matrix, and R is observation noise covariance, and A,
B, H, Q, R can be preset.
In above-mentioned 5 formula, wherein formula (1), (2), (3), (5) are intermediate quantities, and what formula (4) acquired is most to terminate
Fruit, specifically:
In formula (1), A and B are known quantities,The spatial positional information for indicating last moment, is the column of a 9*1
Vector, 9 amounts wherein included are respectively Xx, Vx, Ax, Yy, Vy, Ay, Zz, Vz, Az, and wherein Xx is that the X-axis of position and attitude is sat
Mark, Vx are X-axis linear velocity, and Ax is X-axis linear acceleration, and Yy is the Y axis coordinate of position and attitude, and Vy is Y-axis linear velocity, and Ay is Y-axis
Linear acceleration, Zz are the Z axis coordinate of position and attitude, and Vz is z axis speed, and Az is z axis acceleration；uk-1It is a control letter
Number is the column vector of a 9*1 in the embodiment of the present invention, and uk-1It 101 gets through the above steps, i.e., it will sensing
The linear velocity and linear acceleration for the target object that device is calculated are as control function uk-1, due to uk-1Be a 9*1 column to
Amount, therefore, in practical application, uk-1=(0, Vx, Ax, 0, Vy, Ay, 0, Vz, Az), therefore the k moment can be calculated
In formula (2), A, Q are preset known quantity, Pk-1It is the covariance of the Posterior estimator error of last moment,
Therefore the k moment can be calculated
In formula (3),It can be calculated according to formula (2), H and R are known quantities, therefore can be calculated
Kk。
In formula (4),It can be calculated by formula (1), KkIt can be calculated by formula (3), H is
The amount of knowing, and measurand zkIt 102 acquires through the above steps, i.e. the target object that is calculated of photographic device
First position posture, and using the first position posture of the target object as the measurand of extended Kalman filter
zk, and zkIt is the vector of a 3*1, i.e. zk=(Xx, Yy, Zz),The as spatial positional information that is calculated of k moment, and
And it willIn second position posture as the target object of position and attitude Xx, Yy, Zz, i.e., willIn position and attitude
Xx, Yy, Zz are as finally to the positioning result of target object.
In formula (5), since I and H is known quantity, KkIt can be calculated by formula (3),It can be according to public affairs
Formula (2) is calculated, therefore P can be calculatedk, can be used for update iteration next time.
Therefore, by extended Kalman filter, by the position and attitude of the target object obtained by optical positioning method
As the measurand of extended Kalman filter, by the acceleration and line of the target object obtained by sensor positioning method
Control function of the acceleration as extended Kalman filter can be with then by the recursive iteration of extended Kalman filter
Target object is obtained in the position and attitude at each moment.
Due to the recursive nature of Kalman filter itself, so that the position and attitude that the two neighboring moment calculates is that have tightly
Close connection relationship, this is more conform with the actual conditions in practical application, in practice it has proved that, use side provided in an embodiment of the present invention
Method, finally obtained position and attitude be it is point-device, than be used alone optical positioning method calculate target object position
The position and attitude that sensor positioning method calculates target object is used alone in posture, will be accurate.
When in practical applications, due to using optical positioning method, photographic device is that can take mesh under normal circumstances
Object is marked, but in some special cases, photographic device can not take target object, such as due to photographic device is certainly
The reason of body shooting angle, or due to target object movement, may cause photographic device in certain situations is nothing
Method takes target object.
Below according to the photographic device in optical positioning method whether target object can be taken, divided in two kinds of situation
It does not mentionlet alone bright.
Situation one, photographic device can take target object
1, the acquisition modes of the measurand of extended Kalman filter
When the photographic device can take the target object, the first position posture of the target object is root
It obtains according to following manner: the target object being shot by the photographic device, and utilize optical positioning method, obtain
To the first position posture of the target object.
I.e. when photographic device can take the target object, current goal is directly obtained by optical positioning method
The first position posture of object, and using the first position posture as the measurand of extended Kalman filter.
2, the acquisition of the control function of extended Kalman filter
When the photographic device can take the target object, the linear velocity and linear acceleration of the target object
It is to be obtained according to following manner: obtains the first linear velocity and first for the target object that the photographic device is calculated
Linear acceleration；According to the line for the target object that first linear velocity and the first linear acceleration respectively get the sensor
Speed and linear acceleration are updated, and by the linear velocity and linear acceleration of the updated target object, as the biography
The linear velocity and linear acceleration for the target object that sensor is calculated.
I.e. when photographic device can take the target object, spatial positional information that photographic device will be calculated
In the first linear velocity and the first linear acceleration be sent to sensor, the linear speed for the target object that then sensor is calculated
Degree and linear acceleration are updated, and by by the linear velocity and linear acceleration of the updated target object, as the biography
The linear velocity and linear acceleration for the target object that sensor is calculated, are sent to extended Kalman filter, as extension
The control function of Kalman filter.
Optionally, the mesh that the sensor is got respectively according to first linear velocity and the first linear acceleration
The linear velocity and linear acceleration for marking object are updated, comprising:
The linear velocity for the target object that the sensor is got is added with linear differential magnitude, obtains updated institute
The linear velocity of target object is stated, the linear differential magnitude is the history line that the target object is got according to the sensor
The first linear velocity of history that speed, the linear velocity of the target object and the photographic device are got, the target object
What the first linear velocity obtained；
The linear acceleration for the target object that the sensor is got is added with linear acceleration quantity value, after obtaining update
The target object linear acceleration, the linear acceleration quantity value is the target pair got according to the sensor
The history First Line that the history linear acceleration of elephant, the linear acceleration of the target object and the photographic device are got accelerates
It spends, the first linear acceleration of the target object obtains.
For example, the linear velocity of the target object currently obtained by sensor positioning method is A1, and linear acceleration is
B1, the first linear velocity of the target object obtained by optical positioning method are A2, and the first linear acceleration is B2.
History linear velocity, the linear velocity of the target object of the target object then got according to the sensor
A1, and the first linear velocity A2 of the first linear velocity of history, the target object got according to the photographic device, obtain
Linear differential magnitude, such as each linear differential magnitude can be averaged, obtain final linear differential magnitude.Then
The linear velocity A1 for the target object that presently described sensor is got is added with linear differential magnitude, update can be obtained
The linear velocity of the target object afterwards.
The history linear acceleration of the target object got according to the sensor, the line of the target object accelerate
Spend B1, and the first linear acceleration of history, the first linear acceleration of the target object that get according to the photographic device
B2 obtains linear acceleration quantity value, such as each linear acceleration quantity value can be averaged, and obtains final line and accelerates
Spend quantity value.Then the linear acceleration B1 and linear acceleration quantity value of the target object presently described sensor got
It is added, the linear acceleration of the updated target object can be obtained.
Situation two, photographic device can not take target object
1, the acquisition modes of the measurand of extended Kalman filter
When the photographic device can not take the target object, the first position posture of the target object is
It is obtained according to following manner: the position and attitude for the target object that described photographic device the last time is got, as
The first position posture of the target object, then using the first position posture of the target object as Extended Kalman filter
The measurand of device.
2, the acquisition of the control function of extended Kalman filter
When the photographic device can not take the target object, the linear velocity and line of the target object accelerate
Degree is calculated according to following manner: the linear velocity and line for the target object that the sensor is got accelerate
Degree, as the linear velocity and linear acceleration of the target object that the sensor is calculated, then by the line of the target object
The control function of speed and linear acceleration as extended Kalman filter.
In conclusion in embodiments of the present invention, regardless of whether the photographic device in optical positioning method can take
Target object, can finally obtain the position and attitude of target object, to improve application and Generalization Ability, expand application
Range.
In addition, in practical applications, optionally, the parameter of the extended Kalman filter meets following constraint condition:
Gain matrix
Gain matrix
Gain matrix
Procedure activation noise covariance matrix
Observation noise covariance matrix
Wherein, a, b, c, d, e are preset constant.
Wherein, the value of a, b, c, d, e do not do any restriction, can set according to actual needs, a kind of optional realization
Mode are as follows: it be 1, c value be 1, d value is 1*e that a value, which is 1, b value,-5, e value is 0.3.
The embodiment of the present invention, the linear velocity and line for obtaining the target object that the sensor on target object is calculated accelerate
Degree, and using the linear velocity of the target object and linear acceleration as the control function of extended Kalman filter；Obtain camera shooting
The first position posture for the target object that device is calculated, and using the first position posture of the target object as expansion
Open up the measurand of Kalman filter；According to the control function and the expansion card of the determining extended Kalman filter
The measurand of Thalmann filter obtains the second position posture of the target object.The embodiment of the present invention proposes a kind of completely new
Calculating target object position and attitude method, that is, combine optical positioning method, sensor positioning method and spreading kalman
Filter, due to the recursive nature of extended Kalman filter itself, is determining target to determine the position and attitude of target object
When the position and attitude of object, twice adjacent calculation is the result is that be closely related, and the movement of target object in practical applications
It is to have successional, therefore the position and attitude of two neighboring position is also to be closely related, to be filtered using spreading kalman
The available accurate position and attitude of wave device.
A kind of localization method provided in an embodiment of the present invention is described in detail below, as shown in Figure 4, comprising:
Step 401 judges whether photographic device can take target object, if so, step 402 is gone to, if it is not, then
Go to step 405；
Step 402, the first linear velocity for obtaining the target object that the photographic device is calculated and First Line add
Speed；
Step 403, the target that the sensor is got respectively according to first linear velocity and the first linear acceleration
The linear velocity and linear acceleration of object are updated, and by the linear velocity and linear acceleration of the updated target object, are made
For the linear velocity and linear acceleration of the target object that sensor is calculated, and the linear velocity of the target object and line are accelerated
Spend the control function as extended Kalman filter；
Step 404 shoots the target object by the photographic device, and utilizes optical positioning method, obtains
To the first position posture of target object, and using the first position posture of target object as extended Kalman filter
Measurand；
The linear velocity and linear acceleration of step 405, the target object for getting the sensor, as the biography
The linear velocity and linear acceleration for the target object that sensor is calculated, and the linear velocity of the target object and linear acceleration are made
For the control function of extended Kalman filter；
The position and attitude of step 406, the target object for getting described photographic device the last time, as described
The first position posture of target object, and using the first position posture of the target object as the survey of extended Kalman filter
Quantitative change amount；
Step 407, the control function according to the determining extended Kalman filter and the Extended Kalman filter
The measurand of device obtains the second position posture of the target object.
The embodiment of the present invention, the linear velocity and line for obtaining the target object that the sensor on target object is calculated accelerate
Degree, and using the linear velocity of the target object and linear acceleration as the control function of extended Kalman filter；Obtain camera shooting
The first position posture for the target object that device is calculated, and using the first position posture of the target object as expansion
Open up the measurand of Kalman filter；According to the control function and the expansion card of the determining extended Kalman filter
The measurand of Thalmann filter obtains the second position posture of the target object.The embodiment of the present invention proposes a kind of completely new
Calculating target object position and attitude method, that is, combine optical positioning method, sensor positioning method and spreading kalman
Filter, due to the recursive nature of extended Kalman filter itself, is determining target to determine the position and attitude of target object
When the position and attitude of object, twice adjacent calculation is the result is that be closely related, and the movement of target object in practical applications
It is to have successional, therefore the position and attitude of two neighboring position is also to be closely related, to be filtered using spreading kalman
The available accurate position and attitude of wave device.
Based on the same technical idea, the embodiment of the present invention also provides a kind of positioning device, as shown in Figure 5, comprising:
On the other hand, the embodiment of the present invention provides a kind of positioning device, comprising:
First acquisition unit 501, the linear velocity for the target object that the sensor for obtaining on target object is calculated
And linear acceleration, and using the linear velocity of the target object and linear acceleration as the control function of extended Kalman filter；
Second acquisition unit 502, for obtaining the first position posture for the target object that photographic device is calculated,
And using the first position posture of the target object as the measurand of extended Kalman filter；
Positioning unit 503, for the control function and the expansion card according to the determining extended Kalman filter
The measurand of Thalmann filter obtains the second position posture of the target object.
Optionally, described device further includes the first computing unit 504, specifically for that can take when the photographic device
When the target object, the linear velocity and linear acceleration of the target object are calculated according to following manner:
Obtain the first linear velocity and the first linear acceleration of the target object that the photographic device is calculated；
According to the line for the target object that first linear velocity and the first linear acceleration respectively get the sensor
Speed and linear acceleration are updated, and by the linear velocity and linear acceleration of the updated target object, as the biography
The linear velocity and linear acceleration for the target object that sensor is calculated；
First computing unit 504 is also used to when the photographic device can not take the target object, root
The linear velocity and linear acceleration of the target object are calculated according to following manner:
The linear velocity and linear acceleration for the target object that the sensor is got are calculated as the sensor
The linear velocity and linear acceleration of obtained target object.
Optionally, described device further includes the second computing unit 505, specifically for that can take when the photographic device
When the target object, the first position posture of the target object is calculated according to following manner:
The target object is shot by the photographic device, and utilizes optical positioning method, obtains the mesh
Mark the first position posture of object；
Second computing unit 505 is also used to when the photographic device can not take the target object, root
The first position posture of the target object is calculated according to following manner:
The position and attitude for the target object that described photographic device the last time is got, as the target object
First position posture.
Optionally, first computing unit 504, is specifically used for:
The linear velocity for the target object that the sensor is got is added with linear differential magnitude, obtains updated institute
The linear velocity of target object is stated, the linear differential magnitude is the history line that the target object is got according to the sensor
The first linear velocity of history that speed, the linear velocity of the target object and the photographic device are got, the target object
What the first linear velocity obtained；
The linear acceleration for the target object that the sensor is got is added with linear acceleration quantity value, after obtaining update
The target object linear acceleration, the linear acceleration quantity value is the target pair got according to the sensor
The history First Line that the history linear acceleration of elephant, the linear acceleration of the target object and the photographic device are got accelerates
It spends, the first linear acceleration of the target object obtains.
Optionally, the parameter of the extended Kalman filter meets following constraint condition:
Gain matrix
Gain matrix
Gain matrix
Procedure activation noise covariance matrix
Observation noise covariance matrix
Wherein, a, b, c, d, e are preset constant.
The embodiment of the present invention, the linear velocity and line for obtaining the target object that the sensor on target object is calculated accelerate
Degree, and using the linear velocity of the target object and linear acceleration as the control function of extended Kalman filter；Obtain camera shooting
The first position posture for the target object that device is calculated, and using the first position posture of the target object as expansion
Open up the measurand of Kalman filter；According to the control function and the expansion card of the determining extended Kalman filter
The measurand of Thalmann filter obtains the second position posture of the target object.The embodiment of the present invention proposes a kind of completely new
Calculating target object position and attitude device, that is, combine optical positioning method, sensor positioning method and spreading kalman
Filter, due to the recursive nature of extended Kalman filter itself, is determining target to determine the position and attitude of target object
When the position and attitude of object, twice adjacent calculation is the result is that be closely related, and the movement of target object in practical applications
It is to have successional, therefore the position and attitude of two neighboring position is also to be closely related, to be filtered using spreading kalman
The available accurate position and attitude of wave device.
The present invention be referring to according to the method for the embodiment of the present invention, the process of equipment (system) and computer program product
Figure and/or block diagram describe.It should be understood that every one stream in flowchart and/or the block diagram can be realized by computer program instructions
The combination of process and/or box in journey and/or box and flowchart and/or the block diagram.It can provide these computer programs
Instruct the processor of general purpose computer, special purpose computer, Embedded Processor or other programmable data processing devices to produce
A raw machine, so that being generated by the instruction that computer or the processor of other programmable data processing devices execute for real
The device for the function of being specified in present one or more flows of the flowchart and/or one or more blocks of the block diagram.
These computer program instructions, which may also be stored in, is able to guide computer or other programmable data processing devices with spy
Determine in the computer-readable memory that mode works, so that it includes referring to that instruction stored in the computer readable memory, which generates,
Enable the manufacture of device, the command device realize in one box of one or more flows of the flowchart and/or block diagram or
The function of being specified in multiple boxes.
These computer program instructions also can be loaded onto a computer or other programmable data processing device, so that counting
Series of operation steps are executed on calculation machine or other programmable devices to generate computer implemented processing, thus in computer or
The instruction executed on other programmable devices is provided for realizing in one or more flows of the flowchart and/or block diagram one
The step of function of being specified in a box or multiple boxes.
Although preferred embodiments of the present invention have been described, it is created once a person skilled in the art knows basic
Property concept, then additional changes and modifications can be made to these embodiments.So it includes excellent that the following claims are intended to be interpreted as
It selects embodiment and falls into all change and modification of the scope of the invention.
Obviously, various changes and modifications can be made to the invention without departing from essence of the invention by those skilled in the art
Mind and range.In this way, if these modifications and changes of the present invention belongs to the range of the claims in the present invention and its equivalent technologies
Within, then the present invention is also intended to include these modifications and variations.
Claims (8)
1. a kind of localization method characterized by comprising
Obtain the linear velocity and linear acceleration of target object that the sensor on target object is calculated, and by the target pair
Control function of the linear velocity and linear acceleration of elephant as extended Kalman filter；When photographic device can take the mesh
When marking object, the linear velocity and linear acceleration of the target object are obtained according to following manner: obtaining the photographic device
The first linear velocity and the first linear acceleration for the target object being calculated；Added according to first linear velocity and First Line
The linear velocity and linear acceleration for the target object that speed respectively gets the sensor are updated, and by updated institute
The linear velocity and linear acceleration for stating target object, linear velocity and line as the target object that the sensor is calculated
Acceleration；Specifically, including: to be added the linear velocity for the target object that the sensor is got with linear differential magnitude, obtain
To the linear velocity of the updated target object, the linear differential magnitude is to get the target according to the sensor
The first linear velocity of history that the history linear velocity of object, the linear velocity of the target object and the photographic device are got, institute
State what the first linear velocity of target object obtained；The linear acceleration and linear acceleration for the target object that the sensor is got
Quantity value is added, and obtains the linear acceleration of the updated target object, the linear acceleration quantity value is according to the biography
The history linear acceleration for the target object that sensor is got, the linear acceleration of the target object and the photographic device obtain
The first linear acceleration of history, the first linear acceleration of the target object got obtain；
The first position posture for the target object that photographic device is calculated is obtained, and by first of the target object
Set measurand of the posture as extended Kalman filter；
According to the control function of the determining extended Kalman filter and the measurand of the extended Kalman filter,
Obtain the second position posture of the target object.
2. the method as described in claim 1, which is characterized in that the method also includes:
When the photographic device can not take the target object, the linear velocity and linear acceleration of the target object are
It is calculated according to following manner:
The linear velocity and linear acceleration for the target object that the sensor is got, are calculated as the sensor
Target object linear velocity and linear acceleration.
3. the method as described in claim 1, which is characterized in that when the photographic device can take the target object
When, the first position posture of the target object is obtained according to following manner:
The target object is shot by the photographic device, and utilizes optical positioning method, obtains the target pair
The first position posture of elephant；
When the photographic device can not take the target object, the first position posture of the target object is basis
What following manner obtained:
The position and attitude for the target object that described photographic device the last time is got, as the target object
One position and attitude.
4. the method as described in claim 1, which is characterized in that the parameter of the extended Kalman filter meets following constraint
Condition:
Gain matrix
Gain matrix
Gain matrix
Procedure activation noise covariance matrix
Observation noise covariance matrix
Wherein, a, b, c, d, e are preset constant.
5. a kind of positioning device characterized by comprising
First acquisition unit, the linear velocity and line for the target object that the sensor for obtaining on target object is calculated accelerate
Degree, and using the linear velocity of the target object and linear acceleration as the control function of extended Kalman filter；
First computing unit, specifically for being calculated according to following manner when photographic device can take the target object
It obtains the linear velocity and linear acceleration of the target object: obtaining the of the target object that the photographic device is calculated
One linear velocity and the first linear acceleration；The sensor is got respectively according to first linear velocity and the first linear acceleration
Target object linear velocity and linear acceleration be updated, and the linear velocity of the updated target object and line are accelerated
Degree, linear velocity and linear acceleration as the target object that the sensor is calculated；First computing unit, tool
Body is used for:
The linear velocity for the target object that the sensor is got is added with linear differential magnitude, obtains the updated mesh
The linear velocity of object is marked, the linear differential magnitude is the history linear speed that the target object is got according to the sensor
The of the first linear velocity of history that degree, the linear velocity of the target object and the photographic device are got, the target object
What one linear velocity obtained；The linear acceleration for the target object that the sensor is got is added with linear acceleration quantity value, is obtained
To the linear acceleration of the updated target object, the linear acceleration quantity value is the institute got according to the sensor
State the history first that the history linear acceleration of target object, the linear acceleration of the target object and the photographic device are got
Linear acceleration, the first linear acceleration of the target object obtain；
Second acquisition unit, for obtaining the first position posture for the target object that photographic device is calculated, and by institute
State measurand of the first position posture of target object as extended Kalman filter；
Positioning unit, for according to the determining extended Kalman filter control function and the Extended Kalman filter
The measurand of device obtains the second position posture of the target object.
6. device as claimed in claim 5, which is characterized in that
First computing unit is also used to when the photographic device can not take the target object, according to following
The linear velocity and linear acceleration of the target object is calculated in mode:
The linear velocity and linear acceleration for the target object that the sensor is got, are calculated as the sensor
Target object linear velocity and linear acceleration.
7. device as claimed in claim 5, which is characterized in that described device further includes the second computing unit, specifically for working as
When the photographic device can take the target object, first of the target object is calculated according to following manner
Set posture:
The target object is shot by the photographic device, and utilizes optical positioning method, obtains the target pair
The first position posture of elephant；
Second computing unit is also used to when the photographic device can not take the target object, according to following
The first position posture of the target object is calculated in mode:
The position and attitude for the target object that described photographic device the last time is got, as the target object
One position and attitude.
8. device as claimed in claim 5, which is characterized in that the parameter of the extended Kalman filter meets following constraint
Condition:
Wherein, a, b, c, d, e are preset constant.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610573619.0A CN106123901B (en) | 2016-07-20 | 2016-07-20 | A kind of localization method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610573619.0A CN106123901B (en) | 2016-07-20 | 2016-07-20 | A kind of localization method and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106123901A CN106123901A (en) | 2016-11-16 |
| CN106123901B true CN106123901B (en) | 2019-08-06 |
Family
ID=57289166
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610573619.0A Active CN106123901B (en) | 2016-07-20 | 2016-07-20 | A kind of localization method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106123901B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107030699B (en) * | 2017-05-18 | 2020-03-10 | 广州视源电子科技股份有限公司 | Pose error correction method and device, robot and storage medium |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102927980A (en) * | 2012-10-12 | 2013-02-13 | 深圳市宇恒互动科技开发有限公司 | Indoor positioning system and indoor positioning method based on three-dimensional multipoint wireless and micro-inertia navigation |
| CN103279186A (en) * | 2013-05-07 | 2013-09-04 | 兰州交通大学 | Multiple-target motion capturing system integrating optical localization and inertia sensing |
| CN104764452A (en) * | 2015-04-23 | 2015-07-08 | 北京理工大学 | Hybrid position-posture tracking method based on inertia and optical tracking systems |
| WO2015103621A1 (en) * | 2014-01-06 | 2015-07-09 | Oculus Vr, Llc | Calibration of virtual reality systems |
| CN104834917A (en) * | 2015-05-20 | 2015-08-12 | 北京诺亦腾科技有限公司 | Mixed motion capturing system and mixed motion capturing method |
| CN105551059A (en) * | 2015-12-08 | 2016-05-04 | 国网山西省电力公司技能培训中心 | Power transformation simulation human body motion capturing method based on optical and inertial body feeling data fusion |
-
2016
- 2016-07-20 CN CN201610573619.0A patent/CN106123901B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102927980A (en) * | 2012-10-12 | 2013-02-13 | 深圳市宇恒互动科技开发有限公司 | Indoor positioning system and indoor positioning method based on three-dimensional multipoint wireless and micro-inertia navigation |
| CN103279186A (en) * | 2013-05-07 | 2013-09-04 | 兰州交通大学 | Multiple-target motion capturing system integrating optical localization and inertia sensing |
| WO2015103621A1 (en) * | 2014-01-06 | 2015-07-09 | Oculus Vr, Llc | Calibration of virtual reality systems |
| CN104764452A (en) * | 2015-04-23 | 2015-07-08 | 北京理工大学 | Hybrid position-posture tracking method based on inertia and optical tracking systems |
| CN104834917A (en) * | 2015-05-20 | 2015-08-12 | 北京诺亦腾科技有限公司 | Mixed motion capturing system and mixed motion capturing method |
| CN105551059A (en) * | 2015-12-08 | 2016-05-04 | 国网山西省电力公司技能培训中心 | Power transformation simulation human body motion capturing method based on optical and inertial body feeling data fusion |
Non-Patent Citations (1)
| Title |
|---|
| 基于惯性与光学技术的头位测量方法设计;王勇军等;《压电与声光》;20141231;第36卷(第6期);第1002-1004页 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106123901A (en) | 2016-11-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110118554A (en) | SLAM method, apparatus, storage medium and device based on visual inertia | |
| Weiss et al. | Real-time metric state estimation for modular vision-inertial systems | |
| US10306206B2 (en) | 3-D motion estimation and online temporal calibration for camera-IMU systems | |
| CN108846857A (en) | The measurement method and visual odometry of visual odometry | |
| CN109540126A (en) | A kind of inertia visual combination air navigation aid based on optical flow method | |
| US9378559B2 (en) | System and method for motion estimation | |
| ES2706123T3 (en) | Estimation of the positions of a device and of at least one objective in an environment | |
| CN108235735A (en) | Positioning method and device, electronic equipment and computer program product | |
| CN108364319A (en) | Scale determines method, apparatus, storage medium and equipment | |
| JP2019078560A (en) | Gyro sensor offset correcting device, offset correction program, and pedestrian autonomous navigation device | |
| CN104280022A (en) | Digital helmet display device tracking system of visual-aided inertial measuring unit | |
| Yousuf et al. | Sensor fusion of INS, odometer and GPS for robot localization | |
| CN110345936A (en) | The track data processing method and its processing system of telecontrol equipment | |
| EP3227634B1 (en) | Method and system for estimating relative angle between headings | |
| CN110503684A (en) | Camera position and orientation estimation method and device | |
| CN106123901B (en) | A kind of localization method and device | |
| KR101059748B1 (en) | Feature point placement method and helmet position estimation method in head tracker using feature point pattern | |
| KR20150058882A (en) | Apparatus and method for motion capture using inertial sensor and optical sensor | |
| Chen et al. | A robust optical/inertial data fusion system for motion tracking of the robot manipulator | |
| JP6653151B2 (en) | Heading direction estimation system | |
| Sun et al. | Research on combination positioning based on natural features and gyroscopes for AR on mobile phones | |
| US20240135703A1 (en) | System and method for estimating the pose of a localizing apparatus using reflective landmarks and other features | |
| Grießbach et al. | Vision aided inertial navigation | |
| Maxudov et al. | Effect of camera-IMU displacement calibration error on tracking performance | |
| TWI681168B (en) | Landmark re-parametrization method and device thereof for slam system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |