CN109283490A - The UWB localization method of Taylor series expansion based on mixing least square method - Google Patents
The UWB localization method of Taylor series expansion based on mixing least square method Download PDFInfo
- Publication number
- CN109283490A CN109283490A CN201811355237.6A CN201811355237A CN109283490A CN 109283490 A CN109283490 A CN 109283490A CN 201811355237 A CN201811355237 A CN 201811355237A CN 109283490 A CN109283490 A CN 109283490A
- Authority
- CN
- China
- Prior art keywords
- uwb
- base station
- taylor series
- series expansion
- matrix
- 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.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/06—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
Landscapes
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
The present invention provides a kind of UWB localization methods of Taylor series expansion based on mixing least square method, and first using mixing least square method, directly processing sends out the time that pulse signal receives to base station pulse signal by UWB label, obtain positioning initial value；Then obtained positioning initial value is handled using Taylor series expansion algorithm again, is allowed to more accurate；The anchor point of obtained various discrete is finally linked to be track by motion model with Extended Kalman filter, reaches the effect of optimization to positioning accuracy.Localization method positioning accuracy proposed by the present invention is high, has good anti-jamming effectiveness, and effectively increase location efficiency.
Description
Technical field
The invention belongs to technical field of navigation and positioning, are related to UWB localization method, in particular to a kind of based on mixing minimum two
The UWB localization method of the TOA of the Taylor series expansion of multiplication.
Background technique
In the warehouse factory of modernization, due to its great production scale, a large amount of mobile robot is used to complete
Various work become the key of the links such as production, transport, therefore propose very high want to the accuracy of indoor positioning
It asks.
Indoor positioning technologies mainly have: laser positioning, WiFi, ZigBee, RFID, bluetooth, inertial navigation technology, UWB are fixed
Position technology etc..These indoor positioning technologies are each has something to recommend him, each have their own advantage and disadvantage.In UWB location technology, the band of UWB signal
It is wide greatly, good temporal resolution can be obtained, is widely used in short-range measurement, but its location algorithm is not smart enough
Really, it is unable to satisfy high-precision requirement instantly.
Summary of the invention
To solve the above problems, the invention proposes a kind of TOA of Taylor series expansion based on mixing least square method
UWB location algorithm, closed based on TOA mixing least square method positioning it is quick, Taylor series expansion algorithm accurate and
The optimization of Extended Kalman filter.
In order to achieve the above object, the invention provides the following technical scheme:
The UWB localization method of Taylor series expansion based on mixing least square method, includes the following steps:
Step (1) establishes the network structure based on the base station UWB, and the fixed placement label on trolley；
Step (2) sends out the time that pulse signal receives to base station pulse signal by reading UWB label, obtains UWB
Distance of the label to base station；
Step (3) resolves data obtained in step (2), obtains UWB label by mixing least square method
Positioning initial value；
Step (4), using the positioning initial value of UWB label obtained in step (3) as the initial value of Taylor series expansion algorithm,
Taylor series expansion algorithm is substituted into, second of positioning result of UWB label is obtained；
Second of positioning result of UWB label is extended Kalman filtering, obtains final UWB label by step (5)
Positioning result.
Further, the step (1) comprises the following processes:
Network structure includes that N number of base station UWB, UWB label, and the operation of trolley are vertical included in being formed by by the base station UWB
Within and around body solid；Rectangular coordinate system in space is established, and remembers that the coordinate of the base station UWB is Ni=(xi,yi,zi) (i=1,
2,...,N)；UWB label is placed on trolley, is fixedly connected with a trolley, and UWB tag coordinate is considered as trolley coordinate, is denoted as (x, y, z),
It is synchronous that clock is wherein carried out between each base station, the clock of label not with base station synchronization.
Further, the number of the base station UWB is greater than or equal to 4.
Further, the step (2) comprises the following processes:
Remember that the USB interface from UWB transmission signal reads to obtain label transmission pulse signal to i-th of base station reception pulse letter
Number time ti(i=1,2 ..., N) obtains the i.e. R of distance multiplied by the light velocitydi=c* (ti-Δt)；Wherein, Δ t is base station clock
With the difference of tag clock,Indicate i-th of base station to UWB label (i.e. trolley)
Distance, c=2.997*108m·s-1For electromagnetic wave propagation speed.
Further, the step (3) comprises the following processes:
By RdiTwo kinds of expression way simultaneous to obtain equation group as follows:
Wherein x, y, z, Δ t are unknown amount to be asked；It is handled, will be subtracted after the equation square in addition to the first row
Square of a line equation eliminates (c* Δ t)2,x2+y2+z2, obtain
Abbreviation is after arrangement
Being write as matrix form is
It is denoted as AX=B；
Since deviation is not present in A matrix first three columns, and there are deviations for the 4th column, therefore mixing least square method is used to be counted
Calculate result.
Further, mixing least square method is handled using QR decomposition method, comprised the following processes:
QR decomposition is carried out to matrix A, the matrix A of m*n is decomposed into the product of the matrix R of the matrix Q and n*n of m*n, herein
M*n matrix Q be orthogonal matrix, that is, meet QTQ=I, R are upper triangular matrix；QRX=B is converted by AX=B, it is contemplated that
QTQ=I is further converted to RX=QTB；Least square method solution, non-constant portion are directly carried out to the constant component of equation group again
Divide and carry out the solution of Least Square method, solves [x y z Δ t]TOne group of solution, it is [x y z] thereinTAs position initial value.
Further, the step (4) comprises the following processes:
ByThe expression formula for obtaining TDOA is as follows:
It is further processed with Taylor series expansion algorithm, if the coordinate N of base stationi=(xi,yi,zi) (i=1,2 ...,
N) existing functional relation is f (x, y, z, x between UWB tag coordinate, that is, trolley coordinate (x, y, z)i,yi,zi), if function
Measured value isTrue value is m, therefore error isPositioning initial value (the x obtained by step (3)0,y0,z0), and x=
x0+ Δ x, y=y0+ Δ y, z=z0+ Δ z, then f (x, y, z, xi,yi,zi) in positioning initial value (x0,y0,z0) at do Taylor series
Expansion, result are
High-order term is omitted, abbreviation is
It is right
In positioning initial value (x0,y0,z0) Taylor series expansion is carried out, and the component of second order and second order or more is omitted, obtain ψ=h-G δ；
Wherein
Ri(i=1,2 ..., N) it is positioning initial value (x0,y0,z0) arrive the distance between each base station coordinates；
Least square solution is asked to ψ=h-G δ, obtains the weighted least-square solution of the equation are as follows:
Wherein Q is the covariance matrix of TDOA measured value；
After finding out the weighted least-square solution of the equation, x is enabled0=x0+ Δ x, y0=y0+ Δ y, z=z0+ Δ z, obtains next
Positioning initial value (the x of a iteration0,y0,z0), then carry out iteration next time；
Stopping criterion for iteration is set as | Δ x+ Δ y+ Δ z | < ε, ε are the threshold value set, calculate an x again at this time0=
x0+ Δ x, y0=y0+ Δ y, z=z0+ Δ z obtains (x0,y0,z0), the as final estimated result of Taylor series expansion algorithm.
Further, the step (5) comprises the following processes:
Take tkThe state vector at moment is Xk=[xk yk zk vxk vyk vzk]T, wherein xk、yk、zkIt is tkThe position at moment
Coordinate, vxk、vyk、vzkIt is t respectivelykThe velocity component along x, y, z change in coordinate axis direction at moment；
Take the UWB label i.e. equation of motion of trolley as follows: Xk+1=Φ Xk+Wk；
WhereinTsFor the sampling interval, and WkCovariance matrix
It is system noise covariance；
Take the UWB label i.e. observational equation of trolley as follows: Zk=h (Xk)+Vk；Wherein ZkIt is observation vector, VkIt is that observation is made an uproar
Sound.VkCovariance matrix It is the observation error of TOA；
h(Xk) indicateIt is a nonlinear equation, can passes through
Truncation indicates after its Taylor expansion to obtain its approximate linearization；If HkIt is h (Xk) in tkThe Jacobian matrix at moment, i.e.,Wherein
Observational equation writes Z at this timek=HkXk+Vk, it is extended Kalman filtering, the UWB label after being filtered is i.e.
The positioning coordinate result of trolley.
Further, the specific iterative step of the Extended Kalman filter are as follows:
Step is 1.: column write initial state vector and initial covariance matrix.
Step is 2.: calculating one-step prediction
Step is 3.: calculating one-step prediction mean square deviation Pk+1|k=Φ PkΦT+Q
Step is 4.: calculating tkThe H at momentk
Step is 5.: calculating gain matrix
Step is 6.: calculating state estimation
Step is 7.: calculating evaluated error covariance Pk+1=(I-Kk+1Hk)Pk+1|k
Repeat step 2.-step 7., that is, complete Extended Kalman filter iteration；After Extended Kalman filter, i.e.,
The positioning coordinate result of the i.e. trolley of UWB label after being filtered from each state vector.
Compared with prior art, the invention has the advantages that and the utility model has the advantages that
1. the present invention is first using mixing least square method, directly processing sends out pulse signal to base station by UWB label
The time for receiving pulse signal obtains positioning initial value；Then obtained positioning initial value is carried out using Taylor series expansion algorithm
It handles again, is allowed to more accurate；The anchor point of obtained various discrete is finally passed through into movement mould with Extended Kalman filter
Type is linked to be track, reaches the effect of optimization to positioning accuracy.Localization method proposed by the present invention has good anti-jamming effectiveness, has
Effect improves location efficiency.
2. the method for the present invention solves the problems, such as not considering the error of regression matrix when calculating and positioning initial value, calculate not multiple
It is miscellaneous, initial value precision with higher is positioned, while positioning accuracy is further increased by Extended Kalman filter.
3. being less prone to after Taylor series expansion algorithm since Taylor series expansion algorithm is high to initial value requirement
Fall into the error values such as local optimum.
4. motion process is also included in location Calculation, solves and originally rely only on a certain moment positioning, motion information is neglected
Slightly the drawbacks of.
Detailed description of the invention
Fig. 1 is the UWB localization method flow chart of the Taylor series expansion provided by the invention based on mixing least square method.
Fig. 2 is the flow chart of Extended Kalman filter.
Specific embodiment
Technical solution provided by the invention is described in detail below with reference to specific embodiment, it should be understood that following specific
Embodiment is only illustrative of the invention and is not intended to limit the scope of the invention.
The UWB localization method of Taylor series expansion proposed by the present invention based on mixing least square method, process such as Fig. 1
It is shown, comprising the following steps:
Step (1) establishes the network structure based on the base station UWB, and the fixed placement label on trolley；
Wherein, network structure includes N number of base station UWB (being more than or equal to 4), UWB label.And the operation of trolley should be included in
It is formed by within and around solid geometry body by the base station UWB.Rectangular coordinate system in space is established, and remembers that the coordinate of base station is Ni=
(xi,yi,zi) (i=1,2 ..., N).UWB label is placed on trolley, is fixedly connected with a trolley.UWB tag coordinate is considered as trolley seat
Mark, is denoted as (x, y, z).It is synchronous that clock is wherein carried out between each base station, the clock of label not with base station synchronization.
Step (2) sends out the time that pulse signal receives to base station pulse signal by reading UWB label, obtains UWB
Distance of the label to base station；
In this step, in any time of operation, the base station UWB can receive the pulse letter sent out by UWB label
Number, remember that the USB interface from UWB transmission signal reads to obtain label and sends pulse signal to i-th base station return pulse signal
Time ti(i=1,2 ..., N), obtains distance multiplied by the light velocity.DefinitionIt indicates
Distance of i-th of base station to UWB label (i.e. trolley), and RdiElectromagnetic wave propagation speed c=2.997*10 can be passed through8m·s-1
And tiIt is calculated, i.e. Rdi=c* (tiΔ t), wherein Δ t is the difference of base station clock and tag clock, is constant.
Step (3) resolves data obtained in step (2), obtains UWB label by mixing least square method
Positioning initial value, specifically include following process:
By RdiTwo kinds of expression ways can simultaneous to obtain equation group as follows:
Wherein x, y, z, Δ t are unknown amount to be asked.It is handled, will be subtracted after the equation square in addition to the first row
Square of a line equation eliminates (c* Δ t)2,x2+y2+z2, obtain
After arrangement can abbreviation be
Being write as matrix form is
It is denoted as AX=B.
A matrix is analyzed, what discovery first three columns used is base station coordinates value, it is measured for known determination, and the 4th
Column use measurement result, have measurement error.And traditional least square method thinks that deviation is not present in regression matrix, therefore not
It is suitble to the use when resolving this matrix.In view of deviation is not present in first three columns, and there are deviations for the 4th column, therefore use mixing minimum
Square law obtains calculated result.QR decomposition method is generallyd use to handle mixing Least Square Solution.
The QR decomposition method of real number matrix A is the matrix A of m*n to be decomposed into the product of the matrix R of matrix Q and n*n of m*n.
The matrix Q of m*n herein is orthogonal (tenth of the twelve Earthly Branches) matrix, that is, meets QTQ=I, R are upper triangular matrix.
After carrying out QR decomposition to matrix A, AX=B is converted into QRX=B, it is contemplated that QTQ=I can be further converted to RX
=QTB.The coefficient matrix of equation group is upper triangular matrix R at this time, i.e., coefficient matrix has been divided into constant component and very
Number part, realizes the separating variables to equation group.Least square method is directly carried out to the constant component of equation group again at this time to ask
Solution, non-constant component, which carries out the solution of Least Square method, can be completed.Since matrix A only has four column, and band in this algorithm
There are the column of error there was only a column, thus the non-constant component isolated only have it is one-dimensional, do not need carry out Least Square method, and
It is direct solution.To sum up so, after by carrying out QR decomposition to matrix A, [x y z Δ t] can be solvedTOne group
Solution, it is [x y z] thereinTAs position initial value.
Step (4), using the positioning initial value of UWB label obtained in step (3) as the initial value of Taylor series expansion algorithm,
Taylor series expansion algorithm is substituted into, second of positioning result of UWB label is obtained, specifically includes following process:
ByThe expression formula for obtaining TDOA is as follows:
Because containing square root in equation, TDOA expression formula is nonlinear equation, can use Taylor series expansion
Algorithm is further processed.Taylor series expansion algorithm be it is a kind of by label positioning initial value based on recursive algorithm, it makes
The value solved is converged to estimated location from positioning initial value with recursive method.
If the coordinate N of base stationi=(xi,yi,zi) (i=1,2 ..., N) and UWB tag coordinate, that is, trolley coordinate (x, y, z)
Between existing functional relation be f (x, y, z, xi,yi,zi).If the measured value of function isTrue value is m, therefore error isIf the positioning initial value (x obtained by step (3)0,y0,z0), and x=x0+ Δ x, y=y0+ Δ y, z=z0+ Δ z,
Then f (x, y, z, xi,yi,zi) in positioning initial value (x0,y0,z0) at do Taylor series expansion, result is
Omit high-order term, can abbreviation be
It is right
In positioning initial value (x0,y0,z0) Taylor series expansion is carried out, and the component of second order and second order or more is omitted, ψ=h-G δ can be obtained.
Wherein
Ri(i=1,2 ..., N) it is positioning initial value (x0,y0,z0) arrive the distance between each base station coordinates.
Least square solution is asked to ψ=h-G δ, the weighted least-square solution of the equation can be obtained are as follows:
Wherein Q is the covariance matrix of TDOA measured value.
After finding out the weighted least-square solution of the equation, x is enabled0=x0+ Δ x, y0=y0+ Δ y, z=z0+ Δ z, obtains next
Positioning initial value (the x of a iteration0,y0,z0), it then can carry out iteration next time.
Stopping criterion for iteration is set as | Δ x |+| Δ y |+| Δ z | < ε, ε are the threshold value set.It calculates again at this time primary
x0=x0+ Δ x, y0=y0+ Δ y, z=z0+ Δ z obtains (x0,y0,z0), the as final estimation of Taylor series expansion algorithm is tied
Fruit.
Second of positioning result of UWB label is extended Kalman filtering, obtains final UWB label by step (5)
Positioning result, specifically include following process:
Take tkThe state vector at moment is Xk=[xk yk zk vxk vyk vzk]T, wherein xk、yk、zkIt is tkThe position at moment
Coordinate, vxk、vyk、vzkIt is t respectivelykThe velocity component along x, y, z change in coordinate axis direction at moment.
Take the UWB label i.e. equation of motion of trolley as follows: Xk+1=Φ Xk+Wk。
WhereinTsFor the sampling interval, and WkCovariance matrix
It is system noise covariance.
Take the UWB label i.e. observational equation of trolley as follows: Zk=h (Xk)+Vk.Wherein ZkIt is observation vector, VkIt is that observation is made an uproar
Sound.VkCovariance matrix It is the observation error of TOA.
h(Xk) indicateIt is a nonlinear equation, can passes through
Truncation indicates after its Taylor expansion to obtain its approximate linearization.If HkIt is h (Xk) in tkThe Jacobian matrix at moment, i.e.,Wherein
Observational equation writes Z at this timek=HkXk+Vk。
As shown in Fig. 2, the specific iterative step of Extended Kalman filter are as follows:
Step is 1.: column write initial state vector and initial covariance matrix.
Step is 2.: calculating one-step prediction
Step is 3.: calculating one-step prediction mean square deviation Pk+1|k=Φ PkΦT+Q
Step is 4.: calculating tkThe H at momentk
Step is 5.: calculating gain matrix
Step is 6.: calculating state estimation
Step is 7.: calculating evaluated error covariance Pk+1=(I-Kk+1Hk)Pk+1|k
Repeat step 2.-step 7., the iteration of Extended Kalman filter can be completed.After Extended Kalman filter,
The positioning coordinate result of the i.e. trolley of UWB label after being filtered from each state vector.
The technical means disclosed in the embodiments of the present invention is not limited only to technological means disclosed in above embodiment, further includes
Technical solution consisting of any combination of the above technical features.It should be pointed out that for those skilled in the art
For, various improvements and modifications may be made without departing from the principle of the present invention, these improvements and modifications are also considered as
Protection scope of the present invention.
Claims (9)
1. the UWB localization method of the Taylor series expansion based on mixing least square method, which comprises the steps of:
Step (1) establishes the network structure based on the base station UWB, and the fixed placement label on trolley；
Step (2) sends out the time that pulse signal receives to base station pulse signal by reading UWB label, obtains UWB label
To the distance of base station；
Step (3) resolves data obtained in step (2), obtains determining for UWB label by mixing least square method
Position initial value；
Step (4) is substituted into using the positioning initial value of UWB label obtained in step (3) as the initial value of Taylor series expansion algorithm
Taylor series expansion algorithm obtains second of positioning result of UWB label；
Second of positioning result of UWB label is extended Kalman filtering, obtains determining for final UWB label by step (5)
Position result.
2. the UWB localization method of the Taylor series expansion according to claim 1 based on mixing least square method, feature
It is, the step (1) comprises the following processes:
Network structure includes that N number of base station UWB, UWB label, and the operation of trolley are three-dimensional several included in being formed by by the base station UWB
Within and around what body；Rectangular coordinate system in space is established, and remembers that the coordinate of the base station UWB is Ni=(xi,yi,zi) (i=1,
2,...,N)；UWB label is placed on trolley, is fixedly connected with a trolley, and UWB tag coordinate is considered as trolley coordinate, is denoted as (x, y, z),
It is synchronous that clock is wherein carried out between each base station, the clock of label not with base station synchronization.
3. the UWB localization method of the Taylor series expansion according to claim 2 based on mixing least square method, feature
It is, the number of the base station UWB is greater than or equal to 4.
4. the UWB localization method of the Taylor series expansion according to claim 1 based on mixing least square method, feature
It is, the step (2) comprises the following processes:
Remember that the USB interface from UWB transmission signal reads to obtain label and sends pulse signal to i-th base station return pulse signal
Time ti(i=1,2 ..., N) obtains the i.e. R of distance multiplied by the light velocitydi=c* (ti-Δt)；Wherein, Δ t is base station clock and mark
The difference of clock is signed,Indicate i-th of base station to UWB label (i.e. trolley) away from
From c=2.997*108m·s-1For electromagnetic wave propagation speed.
5. the UWB localization method of the Taylor series expansion according to claim 1 based on mixing least square method, feature
It is, the step (3) comprises the following processes:
By RdiTwo kinds of expression way simultaneous to obtain equation group as follows:
Wherein x, y, z, Δ t are unknown amount to be asked；It is handled, the first row will be subtracted after the equation square in addition to the first row
Square of equation eliminates (c* Δ t)2,x2+y2+z2, obtain
Abbreviation is after arrangement
Being write as matrix form is
It is denoted as AX=B；
Since deviation is not present in A matrix first three columns, and there are deviations for the 4th column, therefore use mixing least square method to obtain and calculate knot
Fruit.
6. the UWB localization method of the Taylor series expansion according to claim 5 based on mixing least square method, feature
It is, the mixing least square method is handled using QR decomposition method, specifically include following process:
QR decomposition is carried out to matrix A, the matrix A of m*n is decomposed into the product of the matrix R of the matrix Q and n*n of m*n, m* herein
The matrix Q of n is orthogonal matrix, that is, meets QTQ=I, R are upper triangular matrix；QRX=B is converted by AX=B, it is contemplated that QTQ=
I is further converted to RX=QTB；Least square method solution directly carried out to the constant component of equation group again, non-constant component into
Row Least Square method solves, and solves [x y z Δ t]TOne group of solution, it is [x y z] thereinTAs position initial value.
7. the UWB localization method of the Taylor series expansion according to claim 1 based on mixing least square method, feature
It is, the step (4) comprises the following processes:
ByThe expression formula for obtaining TDOA is as follows:
It is further processed with Taylor series expansion algorithm, if the coordinate N of base stationi=(xi,yi,zi) (i=1,2 ..., N) with
Existing functional relation is f (x, y, z, x between UWB tag coordinate, that is, trolley coordinate (x, y, z)i,yi,zi), if the measurement of function
Value isTrue value is m, therefore error isIf the positioning initial value (x obtained by step (3)0,y0,z0), and x=x0+
Δ x, y=y0+ Δ y, z=z0+ Δ z, then f (x, y, z, xi,yi,zi) in positioning initial value (x0,y0,z0) at do Taylor series exhibition
It opens, result is
High-order term is omitted, abbreviation is
It is right
In positioning initial value (x0,y0,z0) Taylor series expansion is carried out, and the component of second order and second order or more is omitted, obtain ψ=h-G δ；
Wherein
Ri(i=1,2 ..., N) it is positioning initial value (x0,y0,z0) arrive the distance between each base station coordinates；
Least square solution is asked to ψ=h-G δ, obtains the weighted least-square solution of the equation are as follows:
Wherein Q is the covariance matrix of TDOA measured value；
After finding out the weighted least-square solution of the equation, x is enabled0=x0+ Δ x, y0=y0+ Δ y, z=z0+ Δ z obtains next change
Positioning initial value (the x in generation0,y0,z0), then carry out iteration next time；
Stopping criterion for iteration is set as | Δ x |+| Δ y |+| Δ z | < ε, ε are the threshold value set, calculate an x again at this time0=
x0+ Δ x, y0=y0+ Δ y, z=z0+ Δ z obtains (x0,y0,z0), the as final estimated result of Taylor series expansion algorithm.
8. the UWB localization method of the Taylor series expansion according to claim 1 based on mixing least square method, feature
It is, the step (5) comprises the following processes:
Take tkThe state vector at moment is Xk=[xk yk zk vxk vyk vzk]T, wherein xk、yk、zkIt is tkThe position coordinates at moment,
vxk、vyk、vzkIt is t respectivelykThe velocity component along x, y, z change in coordinate axis direction at moment；
Take the UWB label i.e. equation of motion of trolley as follows: Xk+1=Φ Xk+Wk；
WhereinTsFor the sampling interval, and WkCovariance matrix
It is system noise covariance；
Take the UWB label i.e. observational equation of trolley as follows: Zk=h (Xk)+Vk；Wherein ZkIt is observation vector, VkIt is observation noise；Vk
Covariance matrix It is the observation error of TOA；
h(Xk) indicateIt is a nonlinear equation, it can be passed through
Truncation indicates after Taylor expansion to obtain its approximate linearization；If HkIt is h (Xk) in tkThe Jacobian matrix at moment, i.e.,Wherein
Observational equation writes Z at this timek=HkXk+Vk, it is extended Kalman filtering, the UWB label i.e. trolley after being filtered
Positioning coordinate result.
9. the UWB localization method of the Taylor series expansion according to claim 8 based on mixing least square method, feature
It is, the specific iterative step of the Extended Kalman filter are as follows:
Step is 1.: column write initial state vector and initial covariance matrix
Step is 2.: calculating one-step prediction
Step is 3.: calculating one-step prediction mean square deviation Pk+1|k=Φ PkΦT+Q
Step is 4.: calculating tkThe H at momentk
Step is 5.: calculating gain matrix
Step is 6.: calculating state estimation
Step is 7.: calculating evaluated error covariance Pk+1=(I-Kk+1Hk)Pk+1|k
Repeat step 2.-step 7., that is, complete Extended Kalman filter iteration；After Extended Kalman filter, Ji Kecong
The positioning coordinate result of the i.e. trolley of UWB label after being filtered in each state vector.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811355237.6A CN109283490B (en) | 2018-11-14 | 2018-11-14 | Hybrid least square method-based Taylor series expansion UWB positioning method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811355237.6A CN109283490B (en) | 2018-11-14 | 2018-11-14 | Hybrid least square method-based Taylor series expansion UWB positioning method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109283490A true CN109283490A (en) | 2019-01-29 |
| CN109283490B CN109283490B (en) | 2022-11-01 |
Family
ID=65176325
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811355237.6A Active CN109283490B (en) | 2018-11-14 | 2018-11-14 | Hybrid least square method-based Taylor series expansion UWB positioning method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109283490B (en) |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109884586A (en) * | 2019-03-07 | 2019-06-14 | 广东工业大学 | Unmanned plane localization method, device, system and storage medium based on ultra-wide band |
| CN110045331A (en) * | 2019-03-08 | 2019-07-23 | 浙江亚特电器有限公司 | A kind of UWB location algorithm of real time diagnostic data failure |
| CN110213719A (en) * | 2019-05-29 | 2019-09-06 | 电子科技大学中山学院 | Indoor positioning method based on UWB |
| CN110333481A (en) * | 2019-07-10 | 2019-10-15 | 杭州电子科技大学 | A kind of joint positioning method based on distance than round algorithm and Taylor Series Expansion Method |
| CN111948602A (en) * | 2020-08-17 | 2020-11-17 | 南京工程学院 | Two-dimensional UWB indoor positioning method based on improved Taylor series |
| CN112472432A (en) * | 2020-11-25 | 2021-03-12 | 武汉理工大学 | Automatic walking stick-wheelchair following system and method |
| CN114383631A (en) * | 2021-12-10 | 2022-04-22 | 中国兵器工业集团第二一四研究所苏州研发中心 | Real-time calibration method based on least square, Taylor expansion and comprehensive residual combination |
| CN114594421A (en) * | 2022-02-15 | 2022-06-07 | 湖北大学 | Moving target point position calculation method based on least square method and Kalman filter |
| CN115022800A (en) * | 2022-05-27 | 2022-09-06 | 国网江苏省电力有限公司电力科学研究院 | Self-adaptive positioning method and system for indoor personnel of transformer substation |
| CN116699618A (en) * | 2023-08-08 | 2023-09-05 | 中交第一航务工程局有限公司 | Underwater sonar positioning method |
| CN117761615A (en) * | 2023-07-06 | 2024-03-26 | 江苏金麟技术检测鉴定集团有限公司 | NLOS construction site oriented constructor positioning and tracking algorithm |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20150282112A1 (en) * | 2012-10-04 | 2015-10-01 | Ramot at Tel-Aviv University Ltd. Acorporation | Method and system for estimating position |
| CN105824003A (en) * | 2014-12-16 | 2016-08-03 | 国家电网公司 | Indoor moving target positioning method based on trajectory smoothing |
| US20160248505A1 (en) * | 2013-10-28 | 2016-08-25 | Seoul National University Of Technology Center For Industry Collaboration | Smart device performing led-id/rf communication through a camera, and system and method for providing location-based services using the same |
| CN106908759A (en) * | 2017-01-23 | 2017-06-30 | 南京航空航天大学 | A kind of indoor pedestrian navigation method based on UWB technology |
| CN107515384A (en) * | 2017-09-07 | 2017-12-26 | 南京理工大学 | The positioning of Indoor Robot based on UWB and multisensor and environmental modeling method |
| CN107907857A (en) * | 2017-10-25 | 2018-04-13 | 深圳大学 | A kind of real-time location method and positioner based on UWB |
-
2018
- 2018-11-14 CN CN201811355237.6A patent/CN109283490B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20150282112A1 (en) * | 2012-10-04 | 2015-10-01 | Ramot at Tel-Aviv University Ltd. Acorporation | Method and system for estimating position |
| US20160248505A1 (en) * | 2013-10-28 | 2016-08-25 | Seoul National University Of Technology Center For Industry Collaboration | Smart device performing led-id/rf communication through a camera, and system and method for providing location-based services using the same |
| CN105824003A (en) * | 2014-12-16 | 2016-08-03 | 国家电网公司 | Indoor moving target positioning method based on trajectory smoothing |
| CN106908759A (en) * | 2017-01-23 | 2017-06-30 | 南京航空航天大学 | A kind of indoor pedestrian navigation method based on UWB technology |
| CN107515384A (en) * | 2017-09-07 | 2017-12-26 | 南京理工大学 | The positioning of Indoor Robot based on UWB and multisensor and environmental modeling method |
| CN107907857A (en) * | 2017-10-25 | 2018-04-13 | 深圳大学 | A kind of real-time location method and positioner based on UWB |
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109884586A (en) * | 2019-03-07 | 2019-06-14 | 广东工业大学 | Unmanned plane localization method, device, system and storage medium based on ultra-wide band |
| CN110045331A (en) * | 2019-03-08 | 2019-07-23 | 浙江亚特电器有限公司 | A kind of UWB location algorithm of real time diagnostic data failure |
| CN110213719A (en) * | 2019-05-29 | 2019-09-06 | 电子科技大学中山学院 | Indoor positioning method based on UWB |
| CN110333481A (en) * | 2019-07-10 | 2019-10-15 | 杭州电子科技大学 | A kind of joint positioning method based on distance than round algorithm and Taylor Series Expansion Method |
| CN111948602A (en) * | 2020-08-17 | 2020-11-17 | 南京工程学院 | Two-dimensional UWB indoor positioning method based on improved Taylor series |
| CN112472432A (en) * | 2020-11-25 | 2021-03-12 | 武汉理工大学 | Automatic walking stick-wheelchair following system and method |
| CN114383631A (en) * | 2021-12-10 | 2022-04-22 | 中国兵器工业集团第二一四研究所苏州研发中心 | Real-time calibration method based on least square, Taylor expansion and comprehensive residual combination |
| CN114594421A (en) * | 2022-02-15 | 2022-06-07 | 湖北大学 | Moving target point position calculation method based on least square method and Kalman filter |
| CN114594421B (en) * | 2022-02-15 | 2022-11-18 | 湖北大学 | Moving target point position calculation method based on least square method and Kalman filter |
| CN115022800A (en) * | 2022-05-27 | 2022-09-06 | 国网江苏省电力有限公司电力科学研究院 | Self-adaptive positioning method and system for indoor personnel of transformer substation |
| CN115022800B (en) * | 2022-05-27 | 2024-02-20 | 国网江苏省电力有限公司电力科学研究院 | Self-adaptive positioning method and system for indoor personnel of transformer substation |
| CN117761615A (en) * | 2023-07-06 | 2024-03-26 | 江苏金麟技术检测鉴定集团有限公司 | NLOS construction site oriented constructor positioning and tracking algorithm |
| CN116699618A (en) * | 2023-08-08 | 2023-09-05 | 中交第一航务工程局有限公司 | Underwater sonar positioning method |
| CN116699618B (en) * | 2023-08-08 | 2023-10-03 | 中交第一航务工程局有限公司 | Underwater sonar positioning method |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109283490B (en) | 2022-11-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109283490A (en) | The UWB localization method of Taylor series expansion based on mixing least square method | |
| CN107526073B (en) | Motion multi-station passive time difference and frequency difference combined positioning method | |
| CN103471595B (en) | A kind of iteration expansion RTS mean filter method towards the navigation of INS/WSN indoor mobile robot tight integration | |
| Kim et al. | UWB positioning system based on LSTM classification with mitigated NLOS effects | |
| CN107832575A (en) | Band feedback maneuvering target Asynchronous Track Fusion based on pseudo-measurement | |
| CN108120438B (en) | Indoor target rapid tracking method based on IMU and RFID information fusion | |
| CN105549005A (en) | Dynamic target direction of arrive tracking method based on mesh dividing | |
| CN104035083A (en) | Radar target tracking method based on measurement conversion | |
| CN107270894A (en) | GNSS/SINS deep integrated navigation systems based on Dimensionality Reduction | |
| CN104090261A (en) | Positioning method adopting distance modeling in TDOA positioning system | |
| CN111157943A (en) | TOA-based sensor position error suppression method in asynchronous network | |
| CN105046046A (en) | Ensemble Kalman filter localization method | |
| CN116430310A (en) | UWB technology-based high-precision unmanned ship positioning system, method and storage medium | |
| CN111460074A (en) | Trajectory collision analysis method and device and electronic equipment | |
| Yang et al. | Cooperative indoor localization system based uwb and random forest algorithm in complicated underground nlos scenario | |
| Di Franco et al. | Solving ambiguities in MDS relative localization | |
| Stahlke et al. | Velocity-based channel charting with spatial distribution map matching | |
| CN104849697B (en) | It is a kind of based on the α β filtering methods for going inclined Coordinate Conversion | |
| CN109164419B (en) | Multi-platform disorder measurement processing method based on interactive multi-model | |
| Chen et al. | IMM tracking of a 3D maneuvering target with passive TDOA system | |
| CN114339595B (en) | Ultra-wide band dynamic inversion positioning method based on multi-model prediction | |
| CN103529425A (en) | Method for rapidly tracking indoor target | |
| CN109084752B (en) | Geomagnetic navigation positioning method based on full connectivity constraint | |
| Zhang et al. | State estimation with a heading constraint | |
| CN110401915A (en) | SEKF is the same as the Moving objects location method combined apart from reconstruct under the conditions of a kind of NLOS |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |