CN109283490B - Hybrid least square method-based Taylor series expansion UWB positioning method - Google Patents

Hybrid least square method-based Taylor series expansion UWB positioning method Download PDF

Info

Publication number
CN109283490B
CN109283490B CN201811355237.6A CN201811355237A CN109283490B CN 109283490 B CN109283490 B CN 109283490B CN 201811355237 A CN201811355237 A CN 201811355237A CN 109283490 B CN109283490 B CN 109283490B
Authority
CN
China
Prior art keywords
uwb
positioning
series expansion
matrix
hybrid
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
Application number
CN201811355237.6A
Other languages
Chinese (zh)
Other versions
CN109283490A (en
Inventor
徐晓苏
赵北辰
喻增威
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Southeast University filed Critical Southeast University
Priority to CN201811355237.6A priority Critical patent/CN109283490B/en
Publication of CN109283490A publication Critical patent/CN109283490A/en
Application granted granted Critical
Publication of CN109283490B publication Critical patent/CN109283490B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/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)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention provides a hybrid least square method-based UWB positioning method based on Taylor series expansion, which comprises the steps of firstly, directly processing the time from sending a pulse signal by a UWB tag to receiving the pulse signal by a base station by using the hybrid least square method to obtain a positioning initial value; then, the Taylor series expansion algorithm is adopted to carry out reprocessing on the obtained positioning initial value, so that the positioning initial value is more accurate; and finally, connecting the obtained discrete positioning points into a track through a motion model by using extended Kalman filtering to achieve the optimization effect of the positioning precision. The positioning method provided by the invention has the advantages of high positioning precision, good anti-interference effect and effectively improved positioning efficiency.

Description

UWB positioning method based on Taylor series expansion of hybrid least square method
Technical Field
The invention belongs to the technical field of navigation positioning, relates to a UWB positioning method, and particularly relates to a hybrid least square method-based UWB positioning method of TOA expanded in Taylor series.
Background
In modern warehouse factories, due to the extremely large production scale, a large number of mobile robots are used for completing various works and become the key of production, transportation and other links, so that very high requirements are put forward on the accuracy of indoor positioning.
The indoor positioning technology mainly comprises the following steps: laser positioning, wiFi, zigBee, RFID, bluetooth, inertial navigation technology, UWB positioning technology and the like. These indoor positioning technologies have different advantages and disadvantages. In the UWB positioning technology, the bandwidth of the UWB signal is very large, a good time resolution can be obtained, and the UWB signal is widely applied to short-distance measurement, but the positioning algorithm is not accurate enough, and cannot meet the current high-precision requirement.
Disclosure of Invention
In order to solve the problems, the invention provides a hybrid least square method-based Taylor series expansion UWB positioning algorithm of TOA, which combines the rapidness of the hybrid least square method positioning based on TOA, the accuracy of the Taylor series expansion algorithm and the optimization of extended Kalman filtering.
In order to achieve the purpose, the invention provides the following technical scheme:
the UWB positioning method based on Taylor series expansion of the hybrid least square method comprises the following steps:
step (1), a network structure based on a UWB base station is established, and a label is fixedly placed on a trolley;
step (2), the distance from the UWB tag to the base station is obtained by reading the time from the UWB tag sending the pulse signal to the base station receiving the pulse signal;
step (3), resolving the data obtained in the step (2) by a hybrid least square method to obtain a positioning initial value of the UWB tag;
step (4), taking the initial positioning value of the UWB tag obtained in the step (3) as an initial value of a Taylor series expansion algorithm, and substituting the initial positioning value into the Taylor series expansion algorithm to obtain a second positioning result of the UWB tag;
and (5) performing extended Kalman filtering on the second positioning result of the UWB tag to obtain a final positioning result of the UWB tag.
Further, the step (1) comprises the following steps:
the network structure comprises N UWB base stations and UWB tags, and the operation of the trolley is contained in and around a solid geometry formed by the UWB base stations; establishing a space rectangular coordinate system, and recording the coordinate of the UWB base station as Ni＝(xi,yi,zi) (i =1,2,. Cndot., N); the UWB tag is placed on the trolley and fixedly connected with the trolley, the UWB tag coordinates are regarded as trolley coordinates and are marked as (x, y, z), clock synchronization is carried out among the base stations, and the clock of the tag is not synchronized with the base stations.
Further, the number of the UWB base stations is greater than or equal to 4.
Further, the step (2) includes the following processes:
recording the time t from the time when the USB interface of the UWB transmission signal reads and obtains the pulse signal sent by the label to the time when the ith base station receives the pulse signali(i =1, 2.. Times.n), multiplying the speed of light to obtain the distance Rdi＝c*(ti- Δ t); wherein, Δ t is the difference between the base station clock and the tag clock,
Figure BDA0001865918990000021
denotes the distance from the i-th base station to the UWB tag (i.e., car), c =2.997 × 108m·s-1Is the propagation velocity of the electromagnetic wave.
Further, the step (3) includes the following processes:
from RdiThe two expression modes are combined to obtain the following equation set:
Figure BDA0001865918990000022
wherein x, y, z and delta t are unknown quantities to be solved; processing it, subtracting the square of the first row equation after the square of the equations except the first row, and eliminating (c Δ t)2,x2+y2+z2To obtain
Figure BDA0001865918990000023
After finishing, is simplified into
Figure BDA0001865918990000024
Written in matrix form as
Figure BDA0001865918990000031
Is recorded as AX = B;
since there is no deviation in the first three columns of the a matrix and there is a deviation in the fourth column, the calculation result is obtained by using the hybrid least square method.
Further, a mixed least square method is processed by a QR decomposition method, and the method comprises the following steps:
performing QR decomposition on the matrix A, and decomposing the matrix A of m x n into the product of the matrix Q of m x n and the matrix R of n x n, wherein the matrix Q of m x n is an orthogonal matrix, namely satisfying QTQ = I, R is the upper triangular matrix; conversion of AX = B to QRX = B, taking into account QTQ = I, further conversion to RX = QTB; then, the constant part of the equation group is directly solved by the least square method, the non-constant part is solved by the whole least square method, and [ x y z delta t ] is solved]TIn which [ x y z ] is]TNamely the initial positioning value.
Further, the step (4) comprises the following processes:
by
Figure BDA0001865918990000032
The expression for TDOA is obtained as follows:
Figure BDA0001865918990000033
further processing with Taylor series expansion algorithm, and setting coordinate N of base stationi＝(xi,yi,zi) The functional relationship existing between (i =1, 2.. Said., N) and the UWB tag coordinates, i.e. the trolley coordinates (x, y, z), is f (x, y, z, x)i,yi,zi) Let the measured value of the function be
Figure BDA0001865918990000034
True value is m, so the error is
Figure BDA0001865918990000035
The initial positioning value (x) obtained in the step (3)0,y0,z0) And x = x0+Δx，y＝y0+Δy，z＝z0+ Δ z, then f (x, y, z, x)i,yi,zi) At the initial value of positioning (x)0,y0,z0) Is subjected to Taylor-series expansion, the result is
Figure BDA0001865918990000036
Omitting high-order terms and simplifying into
Figure BDA0001865918990000037
To pair
Figure BDA0001865918990000041
At the initial value of positioning (x)0,y0,z0) Carrying out Taylor series expansion, and omitting components of the second order and above to obtain psi = h-G delta;
wherein
Figure BDA0001865918990000042
Ri(i =1, 2.., N) is the initial value of the location (x)0,y0,z0) Distance to each base station coordinate;
solving the least square solution of psi = h-G delta to obtain the weighted least square solution of the equation as follows:
Figure BDA0001865918990000043
where Q is the covariance matrix of the TDOA measurements;
after solving the weighted least squares solution of the equation, let x0＝x0+Δx，y0＝y0+Δy，z＝z0+ Δ z, to obtain the initial positioning value (x) of the next iteration0,y0,z0) Then, performing the next iteration;
setting the iteration termination condition as | Deltax + Deltay + Deltaz | < Epsilon, where Epsilon is the set threshold, and calculating x once again0＝x0+Δx，y0＝y0+Δy，z＝z0+ Δ z yields (x)0,y0,z0) And the final estimation result is the final estimation result of the Taylor series expansion algorithm.
Further, the step (5) comprises the following processes:
get tkThe state vector at the moment is Xk＝[xk yk zk vxk vyk vzk]TWherein x isk、yk、zkIs tkPosition coordinates of time of day, vxk、vyk、vzkAre respectively tkThe velocity components of the moment along the directions of x, y and z coordinate axes;
the motion equation of the UWB tag, namely the trolley, is taken as follows: xk+1＝ΦXk+Wk；
Wherein
Figure BDA0001865918990000044
TsIs a sampling interval, and WkCovariance matrix of
Figure BDA0001865918990000051
Figure BDA0001865918990000052
Is the system noise covariance;
the observation equation of the UWB tag, i.e. the car, is taken as follows: zk＝h(Xk)+Vk(ii) a Wherein ZkIs an observation vector, VkIs the observation noise. VkOf the covariance matrix
Figure BDA0001865918990000053
Figure BDA0001865918990000054
Is the observed error of TOA;
h(Xk) Namely to represent
Figure BDA0001865918990000055
The method is a nonlinear equation, and the approximate linear representation of the equation can be obtained by truncation after Taylor expansion; let HkIs h (X)k) At tkJacobian matrices of time of day, i.e.
Figure BDA0001865918990000056
Wherein
Figure BDA0001865918990000057
Figure BDA0001865918990000058
At this moment watchWriting of equation of measurement Zk＝HkXk+VkAnd performing extended Kalman filtering to obtain a UWB tag after filtering, namely a positioning coordinate result of the trolley.
Further, the specific iteration step of the extended kalman filter is as follows:
step (1): the columns write the initial state vector and the initial covariance matrix.
Step (2): computing one-step predictions
Figure BDA0001865918990000059
And (3): calculating a one-step prediction mean square error Pk+1|k＝ΦPkΦT+Q
And (4): calculating tkH of timek
And (5): calculating a gain matrix
Figure BDA00018659189900000510
And (6): computing state estimates
Figure BDA00018659189900000511
And (7): calculating an estimation error covariance Pk+1＝(I-Kk+1Hk)Pk+1|k
Repeating the step (2) to the step (7), namely completing the iteration of the extended Kalman filtering; after the Kalman filtering is extended, the filtered UWB label, namely the positioning coordinate result of the trolley, can be obtained from each state vector.
Compared with the prior art, the invention has the following advantages and beneficial effects:
1. firstly, directly processing the time from the pulse signal sent by the UWB tag to the pulse signal received by the base station by using a hybrid least square method to obtain a positioning initial value; then, the Taylor series expansion algorithm is adopted to carry out reprocessing on the obtained positioning initial value, so that the positioning initial value is more accurate; and finally, connecting the obtained discrete positioning points into a track through a motion model by using extended Kalman filtering to achieve the optimization effect of the positioning precision. The positioning method provided by the invention has a good anti-interference effect, and effectively improves the positioning efficiency.
2. The method solves the problem that the error of the regression matrix is not considered when the initial positioning value is calculated, the calculation is not complex, the initial positioning value has higher precision, and the positioning precision is further improved by expanding Kalman filtering.
3. Because the taylor series expansion algorithm has extremely high requirements on the initial value, the taylor series expansion algorithm is less prone to trapping in a local optimal error value.
4. The movement process is brought into the positioning calculation, so that the defects that the original movement information is ignored because the movement process is positioned only at a certain moment are overcome.
Drawings
FIG. 1 is a flow chart of a hybrid least square method-based UWB positioning method based on Taylor series expansion.
FIG. 2 is a flow chart of extended Kalman filtering.
Detailed Description
The technical solutions provided by the present invention will be described in detail with reference to specific examples, which should be understood that the following specific embodiments are only illustrative and not limiting the scope of the present invention.
The flow of the UWB positioning method based on Taylor series expansion of the hybrid least square method is shown in figure 1, and the UWB positioning method comprises the following steps:
step (1), a network structure based on a UWB base station is established, and a label is fixedly placed on a trolley;
the network structure comprises N UWB base stations (more than or equal to 4) and UWB tags. And the operation of the cart should be contained within and around the solid geometry formed by the UWB base station. Establishing a space rectangular coordinate system, and recording the coordinate of the base station as Ni＝(xi,yi,zi) (i =1,2.., N). The UWB tag is placed on the trolley and is fixedly connected with the trolley. The UWB tag coordinates are considered as trolley coordinates, noted as (x, y, z). And clock synchronization is carried out among the base stations, and the clock of the label is not synchronized with the base stations.
Step (2), the distance from the UWB tag to the base station is obtained by reading the time from the pulse signal sent by the UWB tag to the pulse signal received by the base station;
in this step, at any time of operation, the UWB base station may receive the pulse signal transmitted from the UWB tag, and the time t from when the USB interface of the UWB transmission signal reads the pulse signal transmitted from the tag to when the ith base station receives the pulse signal is recordedi(i =1, 2.. Ang., N), multiplying the speed of light yields the distance. Definition of
Figure BDA0001865918990000071
Represents the distance from the ith base station to the UWB tag (i.e., the car), and RdiPropagation velocity c =2.997 × 10 of passing electromagnetic waves8m·s-1And tiIs calculated to obtain, i.e. Rdi＝c*(ti- Δ t), where Δ t is the difference between the base station clock and the tag clock, and is a constant.
And (3) resolving the data obtained in the step (2) by a hybrid least square method to obtain an initial positioning value of the UWB tag, and specifically comprising the following processes:
from RdiThe two expression modes of (2) can be combined to obtain the following equation set:
Figure BDA0001865918990000072
wherein x, y, z, Δ t are unknown quantities to be determined. Processing it, subtracting the square of the first row equation after the square of the equations except the first row, and eliminating (c Δ t)2,x2+y2+z2To obtain
Figure BDA0001865918990000073
Can be simplified into after finishing
Figure BDA0001865918990000074
Written in matrix form as
Figure BDA0001865918990000075
Is noted AX = B.
Analysis of the a matrix reveals that the first three columns all use base station coordinate values, which are known, determined quantities, while the fourth column uses measurements with measurement errors. The traditional least square method considers that the regression matrix has no deviation, so the method is not suitable for being used for solving the matrix. Considering that there is no deviation in the first three columns and there is a deviation in the fourth column, a mixed least square method is used to obtain the calculation result. The mixed least squares problem is usually handled by QR decomposition.
The QR decomposition of a real matrix a is the decomposition of an m x n matrix a into the product of an m x n matrix Q and an n x n matrix R. The matrix Q of m x n here is an orthogonal (unitary) matrix, i.e. Q is satisfiedTQ = I, R is the upper triangular matrix.
After QR decomposition of matrix a, AX = B is converted to QRX = B, considering QTQ = I, and can be further converted into RX = QTB. At this time, the coefficient matrix of the equation set is an upper triangular matrix R, namely the coefficient matrix is divided into a constant part and a non-constant part, and the variable separation of the equation set is realized. At the moment, the constant part of the equation set is directly solved by the least square method, and the non-constant part is solved by the integral least square method. In the algorithm, the matrix A only has four columns, and the columns with errors only have one column, so that the separated non-constant part only has one dimension, and the method can be directly solved without carrying out an integral least square method. In summary, after QR decomposition is performed on the matrix A, the [ x y z Δ t ] can be solved]TOf [ c ], where [ x y z ] is]TNamely the initial value of the positioning.
And (4) taking the initial positioning value of the UWB tag obtained in the step (3) as an initial value of a Taylor series expansion algorithm, substituting the initial value into the Taylor series expansion algorithm to obtain a second positioning result of the UWB tag, and specifically comprising the following processes:
by
Figure BDA0001865918990000081
The expression for TDOA is obtained as follows:
Figure BDA0001865918990000082
because the equation contains an open-square computation, the TDOA expression is a non-linear equation that can be further processed using a Taylor series expansion algorithm. The taylor series expansion algorithm is a recursive algorithm based on the initial positioning value of the label, and uses a recursive method to converge the solved value from the initial positioning value to the estimated position.
Let coordinate N of the base stationi＝(xi,yi,zi) The functional relationship existing between (i =1, 2.. Said., N) and the UWB tag coordinates, i.e. the trolley coordinates (x, y, z), is f (x, y, z, x)i,yi,zi). The measured value of the function is
Figure BDA0001865918990000083
True value is m, so the error is
Figure BDA0001865918990000084
Setting the initial positioning value (x) obtained in the step (3)0,y0,z0) And x = x0+Δx，y＝y0+Δy，z＝z0+ Δ z, then f (x, y, z, x)i,yi,zi) At the initial value of positioning (x)0,y0,z0) Is subjected to Taylor-series expansion, the result is
Figure BDA0001865918990000085
Omitting high-order terms and simplifying into
Figure BDA0001865918990000091
To pair
Figure BDA0001865918990000092
At the initial value of positioning (x)0,y0,z0) By performing Taylor-series expansion and omitting components of the second order and above, psi = h-G δ can be obtained.
Wherein
Figure BDA0001865918990000093
Ri(i =1, 2.., N) is the initial value of the location (x)0,y0,z0) Distance to each base station coordinate.
The least squares solution is found for ψ = h-G δ, and the weighted least squares solution for this equation can be found as:
Figure BDA0001865918990000094
where Q is the covariance matrix of the TDOA measurements.
After solving the weighted least squares solution of the equation, let x0＝x0+Δx，y0＝y0+Δy，z＝z0+ Δ z, to obtain the initial positioning value (x) of the next iteration0,y0,z0) The next iteration can then be performed.
The iteration termination condition is set to be | delta x | + | delta y | + | delta z | < epsilon, and epsilon is a set threshold value. At this point x is calculated again0＝x0+Δx，y0＝y0+Δy，z＝z0+ Δ z yields (x)0,y0,z0) And the final estimation result is the final estimation result of the Taylor series expansion algorithm.
And (5) performing extended Kalman filtering on the second positioning result of the UWB tag to obtain a final positioning result of the UWB tag, and specifically comprising the following processes:
get tkThe state vector at the moment is Xk＝[xk yk zk vxk vyk vzk]TWherein x isk、yk、zkIs tkPosition coordinates of time of day, vxk、vyk、vzkAre respectively tkVelocity components of the time along the x, y, z coordinate axes.
The motion equation of the UWB tag, namely the trolley, is taken as follows: xk+1＝ΦXk+Wk。
Wherein
Figure BDA0001865918990000101
TsIs a sampling interval, and WkCovariance matrix of
Figure BDA0001865918990000102
Figure BDA0001865918990000103
Is the system noise covariance.
The observation equation of the UWB tag, i.e. the car, is taken as follows: zk＝h(Xk)+Vk. Wherein ZkIs an observation vector, VkIs the observation noise. VkCovariance matrix of
Figure BDA0001865918990000104
Figure BDA0001865918990000105
Is the observed error of the TOA.
h(Xk) Namely, represent
Figure BDA0001865918990000106
Is a nonlinear equation, and can obtain an approximate linear representation thereof by truncation after Taylor expansion. Let HkIs h (X)k) At tkJacobian matrices of time of day, i.e.
Figure BDA0001865918990000107
Wherein
Figure BDA0001865918990000108
Figure BDA0001865918990000109
At this time, the observation equation is written as Zk＝HkXk+Vk。
As shown in fig. 2, the specific iteration steps of the extended kalman filter are:
step (1): the columns write the initial state vector and the initial covariance matrix.
Step (2): computing one-step predictions
Figure BDA00018659189900001010
And (3): calculating a one-step prediction mean square error Pk+1|k＝ΦPkΦT+Q
And (4): calculating tkH of timek
And (5): calculating a gain matrix
Figure BDA0001865918990000111
And (6): computing state estimates
Figure BDA0001865918990000112
And (7): calculating an estimation error covariance Pk+1＝(I-Kk+1Hk)Pk+1|k
And (5) repeating the step (2) to the step (7) to complete the iteration of the extended Kalman filtering. After the Kalman filtering is extended, the UWB label after filtering, namely the positioning coordinate result of the trolley, can be obtained from each state vector.
The technical means disclosed in the invention scheme are not limited to the technical means disclosed in the above embodiments, but also include the technical scheme formed by any combination of the above technical features. It should be noted that modifications and adaptations can be made by those skilled in the art without departing from the principles of the present invention and are intended to be within the scope of the present invention.

Claims (8)

1. The UWB positioning method based on Taylor series expansion of the hybrid least square method is characterized by comprising the following steps:
step (1), a network structure based on a UWB base station is established, and a label is fixedly placed on a trolley;
step (2), the distance from the UWB tag to the base station is obtained by reading the time from the pulse signal sent by the UWB tag to the pulse signal received by the base station;
step (3), resolving the data obtained in the step (2) by a hybrid least square method to obtain a positioning initial value of the UWB tag;
step (4), taking the initial positioning value of the UWB tag obtained in the step (3) as an initial value of a Taylor series expansion algorithm, and substituting the initial positioning value into the Taylor series expansion algorithm to obtain a second positioning result of the UWB tag;
the step (4) comprises the following processes:
by
Figure FDA0003834724310000011
The expression for TDOA is derived as follows:
Figure FDA0003834724310000012
in the above formula, i =2,3.,;
further processing by Taylor series expansion algorithm, and setting the coordinate N of the base stationi＝(xi,yi,zi) I =1, 2.. The functional relationship existing between N and the UWB tag coordinates, i.e. the trolley coordinates (x, y, z), is f (x, y, z, x)i,yi,zi) Let the measured value of the function be
Figure FDA0003834724310000013
True value is m, so the error is
Figure FDA0003834724310000014
Setting the initial positioning value (x) obtained in the step (3)0,y0,z0) And x = x0+Δx，y＝y0+Δy，z＝z0+ Δ z, then f (x, y, z, x)i,yi,zi) At the initial value of positioning (x)0,y0,z0) Is subjected to Taylor-series expansion, the result is
Figure FDA0003834724310000015
Zeta is more than 0 and less than 1, high-order terms are omitted, and the process is simplified into
Figure FDA0003834724310000016
To pair
Figure FDA0003834724310000017
At the initial value of positioning (x)0,y0,z0) Carrying out Taylor series expansion, i =2, 3.., N, and omitting components of the second order and above, so as to obtain psi = h-G delta;
wherein
Figure FDA0003834724310000021
RiIs the initial value (x) of the location0,y0,z0) Distance to each base station coordinate, i =1, 2.., N;
the least squares solution is solved for ψ = h-G δ, resulting in a weighted least squares solution of the equation:
Figure FDA0003834724310000022
where Q is the covariance matrix of the TDOA measurements;
after solving the weighted least squares solution of the equation, let x0＝x0+Δx，y0＝y0+Δy，z＝z0+ Δ z, to obtain the initial positioning value (x) of the next iteration0,y0,z0) Then, the next iteration is carried out;
the iteration termination condition is set as | delta x | + | delta y | + | delta z | < epsilon, epsilon is the set threshold value, and then x is calculated once more0＝x0+Δx，y0＝y0+Δy，z＝z0+ Δ z yields (x)0,y0,z0) The final estimation result is the final estimation result of the Taylor series expansion algorithm;
and (5) performing extended Kalman filtering on the second positioning result of the UWB tag to obtain a final positioning result of the UWB tag.
2. The hybrid least squares based taylor series expansion UWB positioning method according to claim 1, characterized in that said step (1) comprises the following procedure:
the network structure comprises N UWB base stations and UWB tags, and the operation of the trolley is contained in and around a solid geometry formed by the UWB base stations; establishing a space rectangular coordinate system, and recording the coordinate of the UWB base station as Ni＝(xi,yi,zi) I =1,2, ·, N; the UWB tag is placed on the trolley and fixedly connected with the trolley, the UWB tag coordinates are regarded as trolley coordinates and are marked as (x, y, z), clock synchronization is carried out among the base stations, and the clock of the tag is not synchronized with the base stations.
3. The hybrid least squares based taylor series expansion UWB positioning method of claim 2 wherein the number of UWB base stations is greater than or equal to 4.
4. The hybrid least squares based taylor series expansion UWB positioning method according to claim 1, wherein the step (2) comprises the following procedure:
recording the time t from the time when the USB interface of the UWB transmission signal reads and obtains the pulse signal sent by the label to the time when the ith base station receives the pulse signaliI =1, 2.. N, multiplied by the speed of light to obtain the distance Rdi＝c*(ti- Δ t); wherein, Δ t is the difference between the base station clock and the tag clock,
Figure FDA0003834724310000031
denotes the distance of the i-th base station to the UWB tag, c =2.997 x 108m·s-1Is the propagation velocity of the electromagnetic wave.
5. The hybrid least squares based taylor series expansion UWB positioning method according to claim 1, wherein the step (3) comprises the following procedure:
from RdiThe two expression modes are combined to obtain the following equation set:
Figure FDA0003834724310000032
wherein x, y, z and delta t are unknown quantities to be solved; processing it, subtracting the square of the first row equation after the square of the equations except the first row, and eliminating (c Δ t)2,x2+y2+z2To obtain
Figure FDA0003834724310000033
After finishing, the process is simplified into
Figure FDA0003834724310000034
Written in matrix form as
Figure FDA0003834724310000041
Is recorded as AX = B;
since there is no deviation in the first three columns of the a matrix and there is a deviation in the fourth column, the calculation result is obtained by using the hybrid least square method.
6. The UWB positioning method based on Taylor series expansion of the hybrid least square method according to claim 5, wherein the hybrid least square method adopts QR decomposition method, which includes the following steps:
performing QR decomposition on the matrix A, and decomposing the matrix A of m x n into the product of the matrix Q of m x n and the matrix R of n x n, wherein the matrix Q of m x n is an orthogonal matrix, namely satisfying QTQ = I, R is an upper triangular matrix; conversion of AX = B to QRX = B, taking into account QTQ = I, further conversion into RX = QTB; then, the constant part of the equation group is directly solved by the least square method, the non-constant part is solved by the whole least square method, and [ x y z delta t ] is solved]TIn which [ x y z ] is]TNamely the initial value of the positioning.
7. The hybrid least squares based taylor series expansion UWB positioning method according to claim 1, characterized in that said step (5) comprises the following procedure:
get tkThe state vector at the moment is Xk＝[xk yk zk vxk vyk vzk]TWherein x isk、yk、zkIs tkPosition coordinates of time of day, vxk、vyk、vzkAre respectively tkThe velocity components of the moment along the directions of x, y and z coordinate axes;
the motion equation of the UWB tag, namely the trolley, is taken as follows: xk+1＝ΦXk+Wk；
Wherein
Figure FDA0003834724310000042
TsIs a sampling interval, and WkCovariance matrix of
Figure FDA0003834724310000043
Figure FDA0003834724310000044
Is the system noise covariance;
the observation equation of the UWB tag, i.e. the car, is taken as follows: zk＝h(Xk)+Vk(ii) a Wherein ZkIs an observation vector, VkIs the observation noise; vkCovariance matrix of
Figure FDA0003834724310000051
Figure FDA0003834724310000052
Is the observed error of TOA;
h(Xk) Namely to represent
Figure FDA0003834724310000053
Is a nonlinear equation, and can obtain an approximate linear representation thereof by truncation after Taylor expansion; let HkIs h (X)k) At tkJacobian matrices of time of day, i.e.
Figure FDA0003834724310000054
Wherein
Figure FDA0003834724310000055
Figure FDA0003834724310000056
At this time, the observation equation is written as Zk＝HkXk+VkAnd performing extended Kalman filtering to obtain a filtered UWB tag, namely a positioning coordinate result of the trolley.
8. The hybrid least squares based Taylor series expansion UWB positioning method of claim 7 wherein the specific iteration step of the extended Kalman filtering is:
step (1): column-writing an initial state vector and an initial covariance matrix;
step (2): computing one-step predictions
Figure FDA0003834724310000057
And (3): calculating a one-step prediction mean square error Pk+1|k＝ΦPkΦT+Q；
And (4): calculating tkH of timek；
And (5): calculating a gain matrix
Figure FDA0003834724310000058
And (6): computing state estimates
Figure FDA0003834724310000059
And (7): calculating an estimation error covariance Pk+1＝(I-Kk+1Hk)Pk+1|k；
Repeating the step (2) to the step (7), namely completing the iteration of the extended Kalman filtering; after the Kalman filtering is extended, the UWB label after filtering, namely the positioning coordinate result of the trolley, can be obtained from each state vector.
CN201811355237.6A 2018-11-14 2018-11-14 Hybrid least square method-based Taylor series expansion UWB positioning method Active CN109283490B (en)

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 CN109283490A (en) 2019-01-29
CN109283490B true 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)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
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
CN112472432B (en) * 2020-11-25 2023-06-30 武汉理工大学 Walking stick-wheelchair automatic following system and method
CN114383631B (en) * 2021-12-10 2024-06-21 中国兵器工业集团第二一四研究所苏州研发中心 Real-time calibration method based on least square, taylor expansion and comprehensive residual combination
CN114594421B (en) * 2022-02-15 2022-11-18 湖北大学 Moving target point position calculation method based on least square method and Kalman filter
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
CN116699618B (en) * 2023-08-08 2023-10-03 中交第一航务工程局有限公司 Underwater sonar positioning method

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2904415A4 (en) * 2012-10-04 2016-06-08 Univ Ramot Method and system for estimating position
KR101511442B1 (en) * 2013-10-28 2015-04-13 서울과학기술대학교 산학협력단 ＬＥＤ-ＩＤ/ＲＦ communication smart device using camera and the method of LBS using the same
CN105824003B (en) * 2014-12-16 2019-02-22 国家电网公司 A kind of indoor moving object localization method based on smooth trajectory
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
CN107907857B (en) * 2017-10-25 2021-08-17 深圳大学 UWB-based real-time positioning method and positioning device

Also Published As

Publication number Publication date
CN109283490A (en) 2019-01-29

Similar Documents

Publication Publication Date Title
CN109283490B (en) Hybrid least square method-based Taylor series expansion UWB positioning method
CN107817469B (en) Indoor positioning method based on ultra-wideband ranging in non-line-of-sight environment
CN109548141B (en) Indoor environment base station coordinate position calibration method based on Kalman filtering algorithm
CN103925925B (en) A kind of real-time high-precision position calculation method for multipoint location system
CN108762309B (en) Human body target following method based on hypothesis Kalman filtering
CN112423265B (en) CSI-based dual-cycle neural network shipborne environment indoor positioning method
CN101221238B (en) Dynamic deviation estimation method based on gauss average value mobile registration
CN104199022B (en) Target modal estimation based near-space hypersonic velocity target tracking method
CN111862165B (en) Target tracking method for updating Kalman filter based on deep reinforcement learning
CN103152695A (en) Time division-synchronization code division multiple access (TD-SCDMA) system-based method for accurately positioning underground personnel
CN110187337B (en) LS and NEU-ECEF space-time registration-based high maneuvering target tracking method and system
CN112887899B (en) Positioning system and positioning method based on single base station soft position information
CN111157943B (en) TOA-based sensor position error suppression method in asynchronous network
CN102981160B (en) Method and device for ascertaining aerial target track
CN111352066B (en) Particle filter-based positioning method and device, computer equipment and storage medium
Zhao et al. Closed-form two-way TOA localization and synchronization for user devices with motion and clock drift
CN109188353B (en) Single-station passive positioning method based on Doppler frequency difference and compressed sensing
CN107860388B (en) Multi-robot collaborative navigation positioning algorithm based on hybrid topological structure
Lin et al. Drift-free visual slam for mobile robot localization by integrating uwb technology
CN114339595B (en) Ultra-wide band dynamic inversion positioning method based on multi-model prediction
CN115865157A (en) Method for intelligent reflecting surface to assist multi-user communication prediction beam forming
CN104931932A (en) Improved debiased coordinate transform Kalman filtering method
CN113030945A (en) Phased array radar target tracking method based on linear sequential filtering
CN112929818A (en) Indoor positioning method based on Kalman filtering and pan-kriging interpolation
CN113359837B (en) Method for tracking moving source by using single unmanned aerial vehicle

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