CN114363807B - Indoor three-dimensional positioning method and computer readable storage medium - Google Patents
Indoor three-dimensional positioning method and computer readable storage medium Download PDFInfo
- Publication number
- CN114363807B CN114363807B CN202111661701.6A CN202111661701A CN114363807B CN 114363807 B CN114363807 B CN 114363807B CN 202111661701 A CN202111661701 A CN 202111661701A CN 114363807 B CN114363807 B CN 114363807B
- Authority
- CN
- China
- Prior art keywords
- value
- error
- measured object
- initial
- estimation
- 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
- 238000000034 method Methods 0.000 title claims abstract description 67
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 57
- 238000001914 filtration Methods 0.000 claims description 25
- 238000005259 measurement Methods 0.000 claims description 19
- 239000011159 matrix material Substances 0.000 claims description 17
- 230000001133 acceleration Effects 0.000 claims description 16
- 238000004590 computer program Methods 0.000 claims description 7
- 230000008569 process Effects 0.000 claims description 4
- 238000012360 testing method Methods 0.000 claims description 4
- 230000001419 dependent effect Effects 0.000 claims description 3
- 230000036961 partial effect Effects 0.000 claims description 3
- 230000004807 localization Effects 0.000 claims 3
- 238000005516 engineering process Methods 0.000 description 17
- 230000006870 function Effects 0.000 description 15
- 238000004891 communication Methods 0.000 description 10
- 230000001360 synchronised effect Effects 0.000 description 9
- 238000004364 calculation method Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 8
- 230000008901 benefit Effects 0.000 description 6
- 238000012545 processing Methods 0.000 description 6
- 230000003068 static effect Effects 0.000 description 6
- 238000012937 correction Methods 0.000 description 5
- 230000005291 magnetic effect Effects 0.000 description 5
- 238000011161 development Methods 0.000 description 3
- 238000012544 monitoring process Methods 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000008030 elimination Effects 0.000 description 2
- 238000003379 elimination reaction Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000000474 nursing effect Effects 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000005294 ferromagnetic effect Effects 0.000 description 1
- 230000000670 limiting effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 238000001228 spectrum Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000003313 weakening effect Effects 0.000 description 1
Landscapes
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
The application provides an indoor three-dimensional positioning method and a computer readable storage medium, wherein the method comprises the following steps: acquiring first distance values from a plurality of base stations based on ultra-wideband to a measured object, and correcting errors of the first distances to obtain second distance values; obtaining initial estimation of the position coordinate of the measured object by adopting a Chan algorithm according to the second distance value, taking the initial estimation as an initial value of Taylor algorithm iteration and obtaining the position coordinate estimation of the measured object; and estimating according to the position coordinates to obtain a positioning track curve of the measured object. The method uses the position estimation result of the Chan algorithm as the initial positioning value of the Taylor algorithm, effectively avoids the condition that the Taylor algorithm is not converged in iteration, and ensures that the positioning result is more accurate.
Description
Technical Field
The present application relates to the field of indoor three-dimensional positioning technology, and in particular, to an indoor three-dimensional positioning method and a computer readable storage medium.
Background
With the rapid development of technology, smart phones and mobile internet are ubiquitous in people's life, and social networks, mobile searches, object recognition and the like are all kept away from location-aware services. Technologies for outdoor accurate positioning and position navigation are mature, people are used to acquire position navigation outdoors by using GPS no matter driving on roads or walking on streets, and position services based on GPS and maps play a role and become one of the most used applications of mobile terminals. The GPS technology is used for outdoor positioning with high precision, but due to physical limitations, such as weakening satellite signals of buildings, the signal errors received by an indoor GPS receiver are too large, and positioning is inaccurate, so that reverse car searching in a parking lot and finding of a specific storefront become abnormal difficulties. Indoor positioning refers to an indoor position positioning system formed by utilizing a plurality of technologies such as wireless communication technology, base station positioning, inertial navigation and the like, and realizes positioning and real-time monitoring of personnel, objects and the like in an indoor environment. Operators such as Google, hundred degrees, etc. for base station positioning use Wi-Fi for indoor positioning. Indoor positioning technology plays an increasingly important role in fire protection, large malls, nursing home industries, security monitoring, personal services and the like, for example: the user can use the mobile device in conjunction with the prompts on the device screen to find the desired merchandise in the store; the medical equipment, medical staff and special patients in the hospital are positioned and tracked in real time; positioning firefighters and personnel waiting for rescue in a fire scene; the staff in the nursing home knows the position of the old in real time, so that the old is prevented from getting lost or accidents and the like; personnel monitoring and intelligent management, such as functions of electronic fence, one-key roll call and the like, are performed in judicial occasions such as prisons or less management stations. If hospitals, commercial buildings, schools and the like are taken into consideration, the location service based on indoor location can generate more commercial value than outdoor location, and has better development prospect.
Ultra Wideband (UWB) technology is an emerging wireless communication technology that uses nanosecond non-sinusoidal narrow pulses to transmit very low power signals over a very wide spectrum, thereby enabling carrierless communication. In 2002, the U.S. federal communications commission (Federal Communications Commission, FCC) has considered the rapid development of UWB technology, and after having made a discussion about whether UWB technology causes interference and electromagnetic compatibility problems with current narrowband wireless communications systems, has opened the 3.1GHz-10.6GHz band for UWB technology for communication purposes, which marks the opening of UWB technology in the field of civil wireless communications. The UWB system has the advantages of high transmission rate, high multipath resolution, low cost, good confidentiality and the like, has great advantages in the application of positioning tracking and navigation of stationary or moving objects and people in the room, utilizes the UWB technology to position the room, has higher positioning precision in an ideal environment, has extremely low UWB signal transmitting power, does not cause interference to other communication systems, and can be widely applied to various military and civil occasions due to a plurality of advantages.
In short, the indoor positioning technology based on UWB is continuously developed and innovated, and is an important research technology in the future new generation indoor positioning field. UWB positioning is generally combined with devices such as gyroscopes and sensors to assist in positioning, however, there is a problem of insufficient accuracy in the absence of such devices. The traditional UWB positioning method has the advantages of a Chan method and a Taylor method, and the Chan method has the advantage of small calculation amount, but the Chan method does not perform well when the number of base stations is small; while Taylor algorithm has a certain advantage in accuracy, it needs a relatively accurate initial value to achieve convergence of the iterative calculation result, and it cannot be judged in advance for the case of non-convergence.
The foregoing background is only for the purpose of facilitating an understanding of the principles and concepts of the application and is not necessarily in the prior art to the present application and is not intended to be used as an admission that such background is not entitled to antedate such novelty and creativity by virtue of prior application or that it is already disclosed at the date of filing of this application.
Disclosure of Invention
The application provides an indoor three-dimensional positioning method and a computer readable storage medium for solving the existing problems.
In order to solve the problems, the technical scheme adopted by the application is as follows:
an indoor three-dimensional positioning method comprises the following steps: s1: acquiring first distance values from a plurality of base stations based on ultra-wideband to a measured object, and correcting errors of the first distances to obtain second distance values; s2: obtaining initial estimation of the position coordinate of the measured object by adopting a Chan algorithm according to the second distance value, taking the initial estimation as an initial value of Taylor algorithm iteration and obtaining the position coordinate estimation of the measured object; s3: and estimating according to the position coordinates to obtain a positioning track curve of the measured object.
Preferably, correcting the error of the first distance value to obtain the second distance valueThe method comprises the following steps: s11: obtaining the true value d from a plurality of base stations to the detected object 1 ,d 2 ,…d n And corresponding average value measured from the base station to the measured objectObtain distance measurement error->Wherein n is a positive integer; s12: obtaining a dependent variable as the distance measurement error e by using a least square method i And the independent variable is the true value d i I.e. the error fitting function:
wherein w= (W 0 ,w 1 …) are coefficients of a polynomial;
s13: correcting the first distance value d according to the error fitting function to obtain the second distance value
Preferably, the error fitting function employs an empirical error fitting formula:
preferably, the bringing the second distance value into the Chan algorithm obtains the initial estimation of the position coordinates of the measured object, specifically including:
s21: converting the initial time-of-flight-based nonlinear quadratic equation into a system of linear equations: psi=h-G a Z a Wherein ψ represents the error, Z a Is an unknown vector, G a Is a coefficient of the degree of freedom, is a second distance value, k i Is the sum of squares of the coordinates of the base stations;
s22: initial solution using weighted least squares solution algorithm wherein
S23: constraint for adding distance and error using the resulting initial solution
wherein ,R0 Is an additional distance parameter e 1 ,e 2 ,e 3 ,e 4 Respectively x, y, z, R 0 Error with the true position of the measured object; constructing a system of equations ψ '=h' -G a 'Z p, wherein ,Zp Is the square of the real position coordinates of the measured object, psi' represents the error, G a 'and H' are coefficients;
s24: solving the positioning result by using a weighted least square method:
selecting a better solution according to the prior information, wherein the better solution is the initial estimation of the position coordinates of the measured object
Preferably, the initial estimation is used as an initial value of a Taylor algorithm, and covariance matrix iterative operation of error is constructed by using a residual value weighting function to obtain the position coordinate estimation of the measured object, which specifically comprises the following steps:
s25: setting the second distance valueIn the initial estimation +.>The taylor expansion is applied and the term more than twice is ignored:
s26: measurement error ψ=h using partial derivatives t -G t Delta, error is obtained by weighted least squares:
δ＝(G t T Q -1 G t ) -1 G t T Q -1 h t ；
wherein δ= (δ) x ,δ y ,δ z )；
S27: judging residual errorWith a given threshold value, if the value is smaller than the threshold value, then the obtained valueEstimating (x, y, z) position coordinates for said object under test; no->S25 and S26 are repeated.
Preferably, the second distance value is obtained by an angle sensor during distance measurementElevation angle information theta i Deviation angle information +.>Each base station measuring result error is different, and a weight is set for the base station:
calculating to obtain coordinate differences between the measured object and n base stations:
the initial estimates of n measured object position coordinates can be calculated:
using a weighted centroid method to obtain a position coordinate estimate of the tag:
preferably, a Kalman filtering algorithm is adopted to carry out filtering processing on the position coordinate estimation, and a system equation and an observation equation are as follows:
wherein W, V are process noise and system noise, respectively, Z k ＝(x k ,y k ,z k ) The bit of the kth object under test being the inputEstimating a set coordinate, namely an observed value; x is X k Is the value of the last iteration output by the Kalman filtering algorithm, A, B, u k H is a coefficient, respectively.
Preferably, the filtering processing of the position coordinate estimation by using a Kalman filtering algorithm comprises the following steps:
s31: calculating a velocity vector and an acceleration vector by using position coordinate estimation of the measured object with two continuous inputs:
s32: the prior estimate, the prior covariance and the Kalman gain are calculated respectively:
s33: and (3) obtaining posterior estimation:
and updating the covariance matrix:
and repeating steps S31 to S33.
Preferably, the filtering processing is performed on the position coordinate estimation by adopting a constant Kalman filtering algorithm, and the method comprises the following steps: a gyroscope and an acceleration sensor are adopted to obtain a rotation matrix R and acceleration information a of the measured object t ；
The system equation for invariant kalman filtering is:
wherein ,is the measurement noise, g is the gravitational acceleration, p t Is the value output by the last iteration or the first iteration, and the position coordinate estimation of the measured object is carried into the value, w t Is a parameter, v t Is the speed;
the state variables are written as:
covariance matrix update:
wherein ,Φt ＝∫F t dt，I 3 Is a three-dimensional identity matrix>Representing group accompaniment, Q is a coefficient;
kalman gain update:wherein H is a parameter;
status is moreThe new technology is as follows: wherein zt Is a coefficient.
The application also provides a computer readable storage medium storing a computer program which when executed by a processor implements the steps of any of the methods described above.
The beneficial effects of the application are as follows: the indoor three-dimensional positioning method and the computer readable storage medium are provided, and position calculation is carried out by using a mode of combining a Chan-Taylor algorithm, namely, a position estimation result of the Chan algorithm is used as an initial positioning value of the Taylor algorithm, so that the situation that the Taylor algorithm is not converged in iteration is effectively avoided, and the positioning result is more accurate.
Further, under the condition that the number of the base stations is large, the method and the device use the calculation result of the Chan algorithm as the initial value calculation final result of the Taylor algorithm, and can obtain more accurate results.
Still further, the application calculates the position coordinates by adopting the ranging error elimination, the position correction and the base station joint positioning method based on the ultra-wideband multi-base station high-precision co-positioning algorithm, and can realize the UWB indoor three-dimensional positioning within 10cm of the position error.
Drawings
Fig. 1 is a schematic diagram of an indoor three-dimensional positioning method according to an embodiment of the application.
Fig. 2 is a schematic diagram of a method for correcting an error of a first distance value to obtain a second distance value according to an embodiment of the present application.
FIG. 3 is a schematic diagram of time-of-flight based positioning in an embodiment of the application.
Fig. 4 is a schematic diagram of a method for obtaining an initial estimate of position coordinates in an embodiment of the present application.
Fig. 5 is a schematic diagram of a method for obtaining a position coordinate estimate according to an embodiment of the present application.
Fig. 6 is a schematic diagram of a method for performing a filtering process on a position coordinate estimation by using a kalman filtering algorithm according to an embodiment of the present application.
Fig. 7 (a) -7 (b) are schematic diagrams of positioning in an embodiment of the present application.
FIG. 8 is a schematic diagram of positioning and tracking according to an embodiment of the present application.
Detailed Description
In order to make the technical problems, technical schemes and beneficial effects to be solved by the embodiments of the present application more clear, the present application is further described in detail below with reference to the accompanying drawings and the embodiments. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the application.
It will be understood that when an element is referred to as being "mounted" or "disposed" on another element, it can be directly on the other element or be indirectly on the other element. When an element is referred to as being "connected to" another element, it can be directly connected to the other element or be indirectly connected to the other element. In addition, the connection may be for both the fixing action and the circuit communication action.
It is to be understood that the terms "length," "width," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," "outer," and the like are merely for convenience in describing embodiments of the application and to simplify the description, and do not denote or imply that the devices or elements referred to must have a particular orientation, be constructed and operated in a particular orientation, and thus are not to be construed as limiting the application.
Furthermore, the terms "first," "second," and the like, are used for descriptive purposes only and are not to be construed as indicating or implying a relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defining "a first" or "a second" may explicitly or implicitly include one or more such feature. In the description of the embodiments of the present application, the meaning of "plurality" is two or more, unless explicitly defined otherwise.
As shown in fig. 1, the present application provides an indoor three-dimensional positioning method, which includes the following steps:
s1: acquiring first distance values from a plurality of base stations based on ultra-wideband to a measured object, and correcting errors of the first distances to obtain second distance values;
s2: obtaining initial estimation of the position coordinate of the measured object by adopting a Chan algorithm according to the second distance value, taking the initial estimation as an initial value of Taylor algorithm iteration and obtaining the position coordinate estimation of the measured object;
s3: and estimating according to the position coordinates to obtain a positioning track curve of the measured object.
The number of the base stations is more than 4, and under the condition that the number of the base stations is more, the method uses the calculation result of the Chan algorithm as the initial value calculation final result of the Taylor algorithm, so that a more accurate result can be obtained.
Furthermore, the ultra-wideband-based multi-base-station high-precision co-location algorithm of the application calculates the position coordinates by adopting a ranging error elimination method, a position correction method and a base station joint location method, and can realize UWB indoor three-dimensional location within 10cm of the position error.
In one embodiment of the application, at least 4 UWB base stations are deployed at the site, the locations of the base stations being fixedly known (or the locations being variable but necessarily trackable), and the object being measured being defined as a tag. Firstly, using UWB ranging to obtain n measured values d between base station and tag 1 ,d 2 ,…d n (or less than n due to poor network, but not less than 4). The n measurements are relatively coarse and the result should be corrected. The application next provides a measured value correction method based on an error fitting function.
A 30m straight line (or a straight line as long as possible) is pulled in the field, a UWB base station is placed at the origin, and the tag is measured at intervals of 50cm (or 20cm, the smaller the interval, the more accurate the end result). The one measurement is called: the tag and base station are placed at the same elevation and all distance information received by the base station within 30s is recorded (or longer time the longer the end result is more accurate). Will be tested i timesAll distance information in the quantity is averaged to obtain an average valueReferred to as UWB measurement average. Then measuring the true value of the distance between the tag and the base station at the moment by using a tape measure (or other devices capable of accurately measuring the distance, such as an infrared distance meter, and the like), and recording as d i . For the obtained measured value, the error between the tag and the base station is calculated according to the true value of the distance between the tag and the base station and the UWB measured average value, and the error fitting function is obtained by utilizing the relation between the least square fitting and the fitting. And obtaining a corresponding error value under the condition of the measured value according to the error fitting function by the measured value, and subtracting the error value from the measured value to obtain a corrected final ranging value.
As shown in fig. 2, correcting the error of the first distance value to obtain the second distance value includes the following steps:
s11: obtaining the true value d from a plurality of base stations to the detected object 1 ,d 2 ,…d n And corresponding average value measured from the base station to the measured objectObtain distance measurement error->Wherein n is a positive integer;
s12: obtaining a dependent variable as the distance measurement error e by using a least square method i And the independent variable is the true value d i I.e. the error fitting function:
wherein w= (W 0 ,w 1 …) are coefficients of a polynomial;
s13: correcting the first distance value d according to the error fitting function to obtain the second distance value
The application utilizes a least square fitting error polynomial, the positioning error fitting is continuous, and no break point exists; and the positioning data is corrected in this way, so that the error is smaller.
In another embodiment of the application, the error law of the UWB positioning chip obeys the piecewise function after a plurality of experiments on different sites: the error increases linearly over a range and then tends to stabilize. If the workload is to be saved, the error fitting function in S12 may directly use an empirical error fitting formula:
after obtaining correction measured values from a measured object to a plurality of base stations, the method brings the correction measured values into a Chan algorithm according to the characteristics of UWB signals, calculates initial estimation of the position coordinates of the measured object, uses the initial value of a Taylor algorithm as an initial value, constructs a covariance matrix of errors by using a residual value weighting function, and obtains the position estimation of the tag through iterative operation.
As shown in fig. 3, a time-of-flight based positioning schematic is shown in an embodiment of the application.
The Chan algorithm performs best in a line-of-sight environment when the range error follows a zero-mean Gaussian distribution. When the number of the base stations is more than or equal to 4, the positioning accuracy can approach the lower boundary of the caramerro in an ideal environment. The Chan algorithm is applied to indoor three-dimensional positioning, has a good effect, is small in calculated amount, and is suitable for actual engineering.
As shown in fig. 4, the step of bringing the second distance value into the Chan algorithm to obtain the initial estimate of the position coordinates of the measured object specifically includes:
s21: converting the initial time-of-flight-based nonlinear quadratic equation into a system of linear equations: psi=h-G a Z a Wherein ψ represents the error, Z a Is an unknown vector, G a Is a coefficient of the degree of freedom, is a second distance value, k i Is the sum of squares of the coordinates of the base stations;
s22: initial solution using weighted least squares solution algorithm wherein
S23: constraint for adding distance and error using the resulting initial solution
wherein ,R0 Is an additional distance parameter e 1 ,e 2 ,e 3 ,e 4 Respectively x, y, z, R 0 Error with the true position of the measured object; constructing a system of equations ψ '=h' -G a 'Z p, wherein ,Zp Is the square of the real position coordinates of the measured object, psi' represents the error, G a 'and H' are coefficients;
s24: solving the positioning result by using a weighted least square method:
selecting a better solution according to the prior information, wherein the better solution is the initial estimation of the position coordinates of the measured object
Through the steps, an initial estimate of the position coordinates of the object to be measured can be obtained. However, the initial estimation does not converge to an optimal value, so the application adjusts the covariance matrix next and iteratively optimizes the estimated coordinates again by using the Taylor algorithm; the Taylor algorithm needs an initial estimated position close to the actual position to ensure the algorithm convergence, and cannot be judged in advance for the non-convergence condition.
As shown in fig. 5, the initial estimation is used as an initial value of a Taylor algorithm, and a covariance matrix of an error is constructed by using a residual weighting function to obtain the position coordinate estimation of the measured object through iterative operation, which specifically includes:
s25: setting the second distance valueIn the initial estimation +.>The taylor expansion is applied and the term more than twice is ignored:
s26: measurement error ψ=h using partial derivatives t -G t Delta, error is obtained by weighted least squares:
δ＝(G t T Q -1 G t ) -1 G t T Q -1 h t ；
wherein δ= (δ) x ,δ y ,δ z )；
S27: judging residual errorIn relation to a given threshold value, if less than the threshold value,then get itEstimating (x, y, z) position coordinates for said object under test; no->S25 and S26 are repeated.
In one embodiment of the present application, if the number of iterations is too high, the iterations may be terminated directly, and the current result is used as the position estimate for the tag, with the number of iterations generally not exceeding 5.
The method and the device use the Chan-Taylor algorithm to combine to carry out position calculation, namely, the position estimation result of the Chan algorithm is used as the initial positioning value of the Taylor algorithm, so that the situation that the Taylor algorithm is not converged in iteration is effectively avoided, and the positioning result is more accurate.
In another embodiment of the present application, the second distance value is obtained by assisting an angle sensor in distance measurementElevation angle information theta i Deviation angle information +.>Each base station measuring result error is different, and a weight is set for the base station:
calculating to obtain coordinate differences between the measured object and n base stations:
the initial estimates of n measured object position coordinates can be calculated:
using a weighted centroid method to obtain a position coordinate estimate of the tag:
the position coordinate estimation of the measured object obtained as described above is a discrete point, and has a problem that it is discontinuous and an error may be excessively large due to individual measurement errors. If the gyroscope or other sensors are directly connected in sequence to form a final positioning track without additional information, the curve is not smooth enough and is not close to the real track. It is necessary to continuously smooth these discrete points.
In one embodiment of the present application, a kalman filter algorithm is used to perform filtering processing on the position coordinate estimation, and a system equation and an observation equation are:
wherein W, V are process noise and system noise, respectively, Z k ＝(x k ,y k ,z k ) The position coordinate estimation of the kth measured object is input and is called an observed value; x is X k Is the value of the last iteration output by the Kalman filtering algorithm, A, B, u k H is a coefficient, respectively.
As shown in fig. 6, the filtering processing of the position coordinate estimation by using the kalman filtering algorithm includes the following steps:
s31: calculating a velocity vector and an acceleration vector by using position coordinate estimation of the measured object with two continuous inputs:
s32: the prior estimate, the prior covariance and the Kalman gain are calculated respectively:
s33: and (3) obtaining posterior estimation:
and updating the covariance matrix:
and repeating steps S31 to S33.
Through the steps, when the refresh speed of the positioning points is high and only the coordinate position information exists, the motion between the adjacent points can be regarded as uniform linear motion, and the speed and the acceleration vector can be calculated according to the uniform linear motion. And adding the high-dimensional vector containing the position information, the speed and the acceleration information into a Kalman filtering system equation, and finally obtaining a smooth positioning curve.
According to the application, kalman filtering based on adjacent position information is provided, a brand new system equation is applied, and under the condition of only the position information of the tag, the speed and the acceleration are obtained, so that the filtering data are more complete, and the obtained curve is smoother.
In another embodiment of the present application, in the presence of a gyroscope and an acceleration sensor, the position coordinate estimation is filtered by adopting a constant kalman filtering algorithm, and the method includes the following steps:
a gyroscope and an acceleration sensor are adopted to obtain a rotation matrix R and acceleration information a of the measured object t ；
The system equation for invariant kalman filtering is:
wherein ,is the measurement noise, g is the gravitational acceleration, p t Is the value output by the last iteration or the first iteration, and the position coordinate estimation of the measured object is carried into the value, w t Is a parameter, v t Is the speed;
covariance matrix update:
wherein ,Φt ＝∫F t dt，I 3 Is a three-dimensional identity matrix>Representing group accompaniment, Q is a coefficient;
the state variables are written as:
kalman gain update:wherein H is a parameter;
the state update is: wherein zt Is a coefficient.
In a specific embodiment of the present application, five UWB base stations and 1 UWB static tag are deployed within a venue without adding other sensors, collecting ranging data, and locating with and without the present application, respectively.
As shown in fig. 7 (a) -7 (b), in the two figures, "represents the position of the base station UWB base station, and" "point set represents the positioning result, and in fig. 7 (a) -represents the result obtained in the case of static application of the algorithm of the present application, it is seen that the positioning result is elongated, and the average positioning error reaches 27cm; FIG. 7 (b) represents the results obtained using the algorithm of the present application, with more precise results and a result error of only 6cm, with a 78% improvement in accuracy.
In the above scenario, the object to be measured is moved in a certain track in the field, the ranging data is collected, and the positioning and track tracking are performed without using the present application, using the conventional filtering means and using the present application, respectively.
As shown in fig. 8, "" represents that the set of points obtained without the present application, the average error reached 41.65cm; the more dithered broken line trace represents a 30.9% improvement in trace accuracy using conventional filtering means, whereas the smoother trace represents a trace obtained after use of the present application, reducing the dynamic average error to 15.08cm, i.e., 63.8%.
The embodiment of the application also provides a control device, which comprises a processor and a storage medium for storing a computer program; wherein the processor is adapted to perform at least the method as described above when executing said computer program.
The embodiments of the present application also provide a storage medium storing a computer program which, when executed, performs at least the method as described above.
The embodiments of the present application also provide a processor executing a computer program, at least performing the method as described above.
The storage medium may be implemented by any type of volatile or non-volatile storage device, or combination thereof. Wherein the nonvolatile Memory may be Read Only Memory (ROM), programmable Read Only Memory (PROM, programmable Read-Only Memory), erasable programmable Read Only Memory (EPROM, erasable Programmable Read-Only Memory), electrically erasable programmable Read Only Memory (EEPROM, electrically Erasable Programmable Read-Only Memory), magnetic random access Memory (FRAM, ferromagnetic Random Access Memory), flash Memory (Flash Memory), magnetic surface Memory, optical disk, or compact disk Read Only Memory (CD-ROM, compact Disc Read-Only Memory); the magnetic surface memory may be a disk memory or a tape memory. The volatile memory may be random access memory (RAM, random Access Memory), which acts as external cache memory. By way of example, and not limitation, many forms of RAM are available, such as static random access memory (SRAM, static Random Access Memory), synchronous static random access memory (SSRAM, synchronous Static Random Access Memory), dynamic random access memory (DRAM, dynamic Random Access Memory), synchronous dynamic random access memory (SDRAM, synchronous Dynamic Random Access Memory), double data rate synchronous dynamic random access memory (ddr SDRAM, double Data Rate Synchronous Dynamic Random Access Memory), enhanced synchronous dynamic random access memory (ESDRAMEnhanced Synchronous Dynamic Random Access Memory), synchronous link dynamic random access memory (SLDRAM, sync Link Dynamic Random Access Memory), direct memory bus random access memory (DRRAM, direct Rambus Random Access Memory). The storage media described in embodiments of the present application are intended to comprise, without being limited to, these and any other suitable types of memory.
In the several embodiments provided by the present application, it should be understood that the disclosed systems and methods may be implemented in other ways. The above described device embodiments are only illustrative, e.g. the division of the units is only one logical function division, and there may be other divisions in practice, such as: multiple units or components may be combined or may be integrated into another system, or some features may be omitted, or not performed. In addition, the various components shown or discussed may be coupled or directly coupled or communicatively coupled to each other via some interface, whether indirectly coupled or communicatively coupled to devices or units, whether electrically, mechanically, or otherwise.
The units described as separate units may or may not be physically separate, and units displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units; some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.
In addition, each functional unit in each embodiment of the present application may be integrated in one processing unit, or each unit may be separately used as one unit, or two or more units may be integrated in one unit; the integrated units may be implemented in hardware or in hardware plus software functional units.
Those of ordinary skill in the art will appreciate that: all or part of the steps for implementing the above method embodiments may be implemented by hardware associated with program instructions, where the foregoing program may be stored in a computer readable storage medium, and when executed, the program performs steps including the above method embodiments; and the aforementioned storage medium includes: a mobile storage device, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk or an optical disk, or the like, which can store program codes.
Alternatively, the above-described integrated units of the present application may be stored in a computer-readable storage medium if implemented in the form of software functional modules and sold or used as separate products. Based on such understanding, the technical solutions of the embodiments of the present application may be embodied in essence or a part contributing to the prior art in the form of a software product stored in a storage medium, including several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the methods described in the embodiments of the present application. And the aforementioned storage medium includes: a removable storage device, ROM, RAM, magnetic or optical disk, or other medium capable of storing program code.
The methods disclosed in the method embodiments provided by the application can be arbitrarily combined under the condition of no conflict to obtain a new method embodiment.
The features disclosed in the several product embodiments provided by the application can be combined arbitrarily under the condition of no conflict to obtain new product embodiments.
The features disclosed in the embodiments of the method or the apparatus provided by the application can be arbitrarily combined without conflict to obtain new embodiments of the method or the apparatus.
The foregoing is a further detailed description of the application in connection with the preferred embodiments, and it is not intended that the application be limited to the specific embodiments described. It will be apparent to those skilled in the art that several equivalent substitutions and obvious modifications can be made without departing from the spirit of the application, and the same should be considered to be within the scope of the application.
Claims (8)
1. An indoor three-dimensional positioning method is characterized by comprising the following steps:
s1: acquiring first distance values from a plurality of base stations based on ultra-wideband to a measured object, and correcting errors of the first distances to obtain second distance values;
s2: obtaining initial estimation of the position coordinate of the measured object by adopting a Chan algorithm according to the second distance value, taking the initial estimation as an initial value of Taylor algorithm iteration and obtaining the position coordinate estimation of the measured object;
s3: obtaining a positioning track curve of the measured object according to the position coordinate estimation;
in step S2, substituting the second distance value into a Chan algorithm to obtain the initial estimate of the position coordinate of the measured object, specifically includes:
s21: converting the initial time-of-flight-based nonlinear quadratic equation into a system of linear equations: psi=h-G a Z a Wherein ψ represents the error, Z a Is an unknown vector, G a Is a coefficient of the degree of freedom, is a second distance value, k i Is the sum of squares of the coordinates of the base stations;
s22: initial solution using weighted least squares solution algorithm wherein />
S23: constraint for adding distance and error using the resulting initial solution
wherein ,R0 Is an additional distance parameter e 1 ,e 2 ,e 3 ,e 4 Respectively x, y, z, R 0 Error with the true position of the measured object; constructing a system of equations ψ '=h' -G a 'Z p, wherein ,Zp Is the square of the real position coordinates of the measured object, psi' represents the error, G a 'and H' are coefficients;
s24: solving the positioning result by using a weighted least square method:
selecting a better solution according to the prior information, wherein the better solution is the initial estimation of the position coordinates of the measured object
The second distance value is obtained by an angle sensor during distance measurementElevation angle information theta i Deviation angle information +.>Each base station measuring result error is different, and a weight is set for the base station:
calculating to obtain coordinate differences between the measured object and n base stations:
the initial estimates of n measured object position coordinates can be calculated:
using a weighted centroid method to obtain a position coordinate estimate of the tag:
2. the indoor three-dimensional positioning method of claim 1, wherein correcting the error of the first distance value to obtain the second distance value comprises the steps of:
s11: obtaining the true value d from a plurality of base stations to the detected object 1 ,d 2 ,…d n And corresponding average value measured from the base station to the measured objectObtain distance measurement error->Wherein n is a positive integer;
s12: obtaining a dependent variable as the distance measurement error e by using a least square method i And the independent variable is the true value d i Is a fitting polynomial of the range error e i I.e. error fitting function f (d i )：
Wherein w= (W 0 ,w 1 …) are coefficients of a polynomial;
s13: correcting the first distance value d according to the error fitting function to obtain the second distance value
3. The indoor three-dimensional localization method of claim 2, wherein the error fitting function employs an empirical error fitting equation:
4. the indoor three-dimensional localization method of claim 1, wherein the initial estimation is used as an initial value of a Taylor algorithm and a covariance matrix of an error is constructed by using a residual weighting function to obtain the position coordinate estimation of the measured object through iterative operation, and the method specifically comprises:
s25: setting the second distance valueIn the initial estimation +.>The taylor expansion is applied and the term more than twice is ignored:
s26: measurement error ψ=h using partial derivatives t -G t Delta, error is obtained by weighted least squares:
wherein δ= (δ) x ,δ y ,δ z )；
S27: judging residual errorWith a given threshold value, if the value is smaller than the threshold value, then the obtained valueEstimating (x, y, z) position coordinates for said object under test; no->S25 and S26 are repeated.
5. The indoor three-dimensional positioning method according to claim 1, wherein the position coordinate estimation is filtered by a kalman filter algorithm, and a system equation and an observation equation are as follows:
wherein W, V are process noise and system noise, respectively, Z k ＝(x k ,y k ,z k ) The position coordinate estimation of the kth measured object is input and is called an observed value; x is X k Is the value of the last iteration output by the Kalman filtering algorithm, A, B, u k H is a coefficient, respectively.
6. The indoor three-dimensional localization method of claim 5, wherein filtering the position coordinate estimate using a kalman filter algorithm comprises the steps of:
s31: calculating a velocity vector and an acceleration vector by using position coordinate estimation of the measured object with two continuous inputs:
s32: the prior estimate, the prior covariance and the Kalman gain are calculated respectively:
s33: and (3) obtaining posterior estimation:
and updating the covariance matrix:
and repeating steps S31 to S33.
7. The indoor three-dimensional positioning method of claim 5, wherein the filtering the position coordinate estimate using a constant kalman filter algorithm comprises the steps of:
a gyroscope and an acceleration sensor are adopted to obtain a rotation matrix R of the measured object t And acceleration information a t ；
The system equation for invariant kalman filtering is:
wherein ,is the measurement noise, g is the gravitational acceleration, p t Is the value output by the last iteration or the first iteration, and the position coordinate estimation of the measured object is carried into the value, w t Is a parameter, v t Is the speed;
the state variables are written as:
covariance matrix update:
wherein ,Φt ＝∫F t dt，I 3 Is a three-dimensional identity matrix>Representing group accompaniment, Q t Is a coefficient;
kalman gain update:wherein H is a parameter;
the state update is: wherein zt Is a coefficient.
8. A computer readable storage medium storing a computer program, characterized in that the computer program when executed by a processor implements the steps of the method according to any of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111661701.6A CN114363807B (en) | 2021-12-31 | 2021-12-31 | Indoor three-dimensional positioning method and computer readable storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111661701.6A CN114363807B (en) | 2021-12-31 | 2021-12-31 | Indoor three-dimensional positioning method and computer readable storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114363807A CN114363807A (en) | 2022-04-15 |
| CN114363807B true CN114363807B (en) | 2023-10-27 |
Family
ID=81104821
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111661701.6A Active CN114363807B (en) | 2021-12-31 | 2021-12-31 | Indoor three-dimensional positioning method and computer readable storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114363807B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117194854B (en) * | 2023-11-01 | 2024-02-27 | 辽宁天衡智通防务科技有限公司 | Three-dimensional positioning method and device based on improved Chan algorithm |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104080165A (en) * | 2014-06-05 | 2014-10-01 | 杭州电子科技大学 | Indoor wireless sensor network positioning method based on TDOA |
| CN109186609A (en) * | 2018-10-09 | 2019-01-11 | 南京航空航天大学 | UWB localization method based on KF algorithm, Chan algorithm and Taylor algorithm |
| CN109975793A (en) * | 2019-04-25 | 2019-07-05 | 苏州元联传感技术有限公司 | Calibration method of laser two-dimensional distance measurement sensor |
| CN112748397A (en) * | 2020-12-22 | 2021-05-04 | 重庆邮电大学 | UWB positioning method based on self-adaptive BP neural network under non-line-of-sight condition |
| CN113567925A (en) * | 2021-06-22 | 2021-10-29 | 山东师范大学 | Ultra-wideband technology-based accurate positioning method, system and device |
-
2021
- 2021-12-31 CN CN202111661701.6A patent/CN114363807B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104080165A (en) * | 2014-06-05 | 2014-10-01 | 杭州电子科技大学 | Indoor wireless sensor network positioning method based on TDOA |
| CN109186609A (en) * | 2018-10-09 | 2019-01-11 | 南京航空航天大学 | UWB localization method based on KF algorithm, Chan algorithm and Taylor algorithm |
| CN109975793A (en) * | 2019-04-25 | 2019-07-05 | 苏州元联传感技术有限公司 | Calibration method of laser two-dimensional distance measurement sensor |
| CN112748397A (en) * | 2020-12-22 | 2021-05-04 | 重庆邮电大学 | UWB positioning method based on self-adaptive BP neural network under non-line-of-sight condition |
| CN113567925A (en) * | 2021-06-22 | 2021-10-29 | 山东师范大学 | Ultra-wideband technology-based accurate positioning method, system and device |
Non-Patent Citations (3)
| Title |
|---|
| 叶庆雨 ; 张润梅 ; 吴跃波 ; .一种基于超宽带技术的改进定位算法.荆楚理工学院学报.2020,(第02期),全文. * |
| 王鑫.基于UWB的室内目标三维跟踪定位技术研究.中国优秀硕士学位论文全文数据库 (信息科技辑).2021,正文第15-17、35-46页，图1-3. * |
| 蒋文美 ; 王玫 ; .一种基于TOA的UWB直接-Taylor复合定位算法.桂林电子科技大学学报.2006,(第01期),全文. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114363807A (en) | 2022-04-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Tian et al. | A low-cost INS and UWB fusion pedestrian tracking system | |
| US9113310B2 (en) | Systems and methods for simultaneously and automatically creating databases of wifi signal information | |
| Poulose et al. | Localization error analysis of indoor positioning system based on UWB measurements | |
| EP4056949A2 (en) | Systems and methods for calibrating unstable sensors | |
| Tian et al. | A resetting approach for INS and UWB sensor fusion using particle filter for pedestrian tracking | |
| Liu et al. | Fusing similarity-based sequence and dead reckoning for indoor positioning without training | |
| Kim et al. | Distance estimation with weighted least squares for mobile beacon-based localization in wireless sensor networks | |
| Yu et al. | NLOS identification and mitigation for mobile tracking | |
| CN111722180B (en) | Kalman filtering-based indoor pedestrian positioning method, device and system | |
| Mouhammad et al. | BLE indoor localization based on improved RSSI and trilateration | |
| Schmid et al. | An experimental evaluation of position estimation methods for person localization in wireless sensor networks | |
| EP2952925A1 (en) | Method, device and system for indoor localization and tracking using ultra-wideband radio signals | |
| CN114363807B (en) | Indoor three-dimensional positioning method and computer readable storage medium | |
| US10356556B2 (en) | Method for locating mobile devices in a common frame of reference | |
| CN109506653A (en) | Based on the indoor positioning pedestrian's method for improving particle filter under NLOS environment | |
| Lategahn et al. | Tdoa and rss based extended kalman filter for indoor person localization | |
| CN112068073A (en) | UWB positioning error correction method, UWB positioning system, device, and storage medium | |
| Huilla et al. | Indoor localization with Wi-Fi fine timing measurements through range filtering and fingerprinting methods | |
| Khan et al. | Experimental testbed evaluation of cell level indoor localization algorithm using Wi-Fi and LoRa protocols | |
| Farnham | Indoor localisation of iot devices by dynamic radio environment mapping | |
| CN113884096A (en) | Indoor navigation system and method | |
| Jose et al. | Taylor series method in TDOA approach for indoor positioning system. | |
| Liu et al. | Exploit Kalman filter to improve fingerprint-based indoor localization | |
| Chóliz et al. | Evaluation of algorithms for UWB indoor tracking | |
| CN115866517A (en) | Indoor positioning system based on empirical mode decomposition and spatial autoregressive neural network |
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 |