CN116448106A - Method and device for positioning long and narrow environment based on UWB/SINS combined system - Google Patents

Method and device for positioning long and narrow environment based on UWB/SINS combined system Download PDF

Info

Publication number
CN116448106A
CN116448106A CN202310615693.4A CN202310615693A CN116448106A CN 116448106 A CN116448106 A CN 116448106A CN 202310615693 A CN202310615693 A CN 202310615693A CN 116448106 A CN116448106 A CN 116448106A
Authority
CN
China
Prior art keywords
uwb
positioning
sins
base station
ranging
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202310615693.4A
Other languages
Chinese (zh)
Other versions
CN116448106B (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.)
China Railway Siyuan Survey and Design Group Co Ltd
Original Assignee
China Railway Siyuan Survey and Design Group Co Ltd
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 China Railway Siyuan Survey and Design Group Co Ltd filed Critical China Railway Siyuan Survey and Design Group Co Ltd
Priority to CN202310615693.4A priority Critical patent/CN116448106B/en
Publication of CN116448106A publication Critical patent/CN116448106A/en
Application granted granted Critical
Publication of CN116448106B publication Critical patent/CN116448106B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention provides a method and a device for positioning an elongated environment based on a UWB/SINS combined system, belonging to the technical field of wireless positioning, wherein the method comprises the following steps: a coordinate initialization stage and a dynamic positioning stage; wherein, the coordinate initialization stage: based on ultra wideband UWB technology, the particle swarm algorithm is utilized to obtain the static coordinates of the UWB/SINS combined system by taking the minimum ranging error as the target; dynamic positioning stage: based on the static coordinates acquired by UWB, a robust unscented Kalman filtering fusion algorithm is adopted by combining the SINS of the strapdown inertial navigation system, and a UWB ranging noise matrix is rebuilt, so that fusion positioning of the UWB/SINS combined system in a long and narrow environment is realized. The method does not need Jacobian matrix calculation in the traditional method no matter in the static initialization and dynamic fusion process, avoids the problem of large positioning of short axis errors of long and narrow environments caused by linearization, and can improve the robustness of UWB/SINS positioning under NLOS to a certain extent.

Description

Method and device for positioning long and narrow environment based on UWB/SINS combined system
Technical Field
The invention relates to the technical field of wireless positioning, in particular to a method and a device for positioning an elongated environment based on a UWB/SINS combined system.
Background
The continuous and reliable high-precision positioning has important significance in tunnel construction operation, personnel management, disaster emergency, management and control scheduling. Ultra-wideband (UWB) can reach centimeter level in theory, has higher time resolution, stronger multipath resistance and certain penetration capability, is considered as one of the indoor positioning technologies with great potential, and has been successfully applied to more tunnel positioning at present. However, the existing indoor positioning method based on the traditional UWB in the tunnel still has the problem of large short axis positioning error, and especially the tunnel with the long and narrow geometric structure is more serious. In the long and narrow tunnel, if the long axis direction of the tunnel is the x axis and the short axis is the y axis, because UWB anchor points are generally deployed on two sides in the tunnel and approximate two straight lines, the difference of the coordinates of all anchor points on the y axis is small, the special distribution geometric structure of the anchor points is unfavorable for UWB label positioning, and for plane positioning, the y axis error of the traditional analytic method is large. In addition, non line of sight errors (NLOS) are also a common problem that results in poor UWB positioning robustness because UWB signal occlusion or reflection phenomena are unavoidable within tunnels. Thus, achieving continuous, robust, high-precision UWB positioning within a complex, elongated environment, represented by tunnels, presents a significant challenge.
The UWB tag positioning is to calculate the position of the tag through a plurality of acquired anchor point ranging, and the coordinate of the target point is essentially solved by utilizing a positioning equation set. The most direct is based on a least square mode, and the core is to linearize a nonlinear observation equation into a linear equation set and realize an optimal solution according to an error minimum principle. The algorithm is finally used for solving an error equation or an iterative increment equation, linearization operation (jacobian matrix) is needed, when the spatial distribution of UWB base stations is relatively good, the jacobian matrix has good mathematical characteristics, when the jacobian matrix tends to have a pathological problem in a long and narrow environment, and therefore a non-negligible model error is brought, and after the model error is coupled with a ranging error, the positioning error is finally further increased.
Disclosure of Invention
The invention provides a method and a device for positioning an elongated environment based on a UWB/SINS combined system, which are used for solving the problem that the prior art possibly has a disease state by using a jacobian matrix in the elongated environment.
In a first aspect, the present invention provides a method for positioning an elongated environment based on a UWB/SINS combined system, comprising: a coordinate initialization stage and a dynamic positioning stage; wherein, the coordinate initialization stage: based on ultra wideband UWB technology, the particle swarm algorithm is utilized to obtain the static coordinates of the UWB/SINS combined system by taking the minimum ranging error as the target; dynamic positioning stage: based on the static coordinates acquired by UWB, a robust unscented Kalman filtering fusion algorithm is adopted by combining the SINS of the strapdown inertial navigation system, and a UWB ranging noise matrix is rebuilt, so that fusion positioning of the UWB/SINS combined system in a long and narrow environment is realized.
According to the long and narrow environment positioning method based on the UWB/SINS combined system, the fitness function of the particle swarm algorithm is specifically as follows:
wherein F (p) i ) Representing PSO fitness value corresponding to particle i, m representing the number of UWB base stations, p j Coordinates corresponding to base station of UWB, p i For the position of the ith particle, t represents the number of static observation epochs of UWB,representing the actual ranging between the UWB tag and the base station j at time t, w ij The weight coefficient representing particle i to base station j.
According to the long and narrow environment positioning method based on the UWB/SINS combined system provided by the invention, the weight coefficient w ij Is determined from the ranging of particle i to base station j and the vertical distance of particle i to base station j.
According to the long and narrow environment positioning method based on the UWB/SINS combined system provided by the invention, the weight coefficient w is determined according to the distance measurement from the particle i to the base station j and the vertical distance from the particle i to the base station j ij The specific mode of (a) is as follows:
wherein w= Σw ij ，D ij For the vertical distance d of particle i from base station j ij Indicating the ranging of particle i to base station j.
According to the method for positioning the long and narrow environment based on the UWB/SINS combined system, which is provided by the invention, in the coordinate initialization stage, the method further comprises the following steps:
in the case that the number of base stations of UWB is determined to be more than 3, arbitrarily selecting three base stations from a set containing all base stations to construct a plurality of base station subsets;
executing the particle swarm algorithm on each base station subset to obtain a corresponding fitness value;
determining an optimal base station subset from all the base station subsets according to the fitness value corresponding to each base station subset;
and initializing static coordinates by using the base stations corresponding to the optimal base station subset.
According to the long and narrow environment positioning method based on the UWB/SINS combined system, which is provided by the invention, the robust unscented Kalman filtering fusion algorithm is as follows:
under the condition that the observed value is abnormal according to the innovation residual error, calculating the self-adaptive adjustment coefficient of the UWB ranging noise matrix by adopting a preset equivalent weight function;
reconstructing a UWB ranging noise matrix by utilizing the self-adaptive adjustment coefficient;
the information residual error is determined as follows:
y＝ρ+R；
where y=ρ+r represents the UWB observation equation, ρ is the virtual observation, R is the UWB ranging noise matrix, δz represents the innovation residual,representing observed quantity, namely UWB ranging, wherein R is UWB ranging noise matrix; let SINS predicted position->Then for the jth base station position (x j ,y j ) Virtual distance measurement->
The self-adaptive adjustment coefficient is determined as follows:
wherein, gamma represents the adaptive adjustment coefficient,representing the absolute value, k, of the normalized innovation residual 0 Is a preset constant.
According to the method for positioning the long and narrow environment based on the UWB/SINS combined system, which is provided by the invention, the long and narrow environment is a tunnel environment.
In a second aspect, the present invention also provides an elongated environment positioning device based on a UWB/SINS combined system, comprising: the system comprises a coordinate initialization module and a dynamic positioning module; the coordinate initialization module is used for acquiring the static coordinates of the UWB/SINS combined system by using a particle swarm algorithm based on the ultra wideband UWB technology by taking the minimum ranging error as a target; the dynamic positioning module is used for reconstructing a UWB ranging noise matrix by combining a strapdown inertial navigation system SINS and adopting a robust unscented Kalman filtering fusion algorithm on the basis of the static coordinates acquired by UWB so as to realize fusion positioning of the UWB/SINS combined system in a long and narrow environment.
In a third aspect, the present invention provides an electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the steps of the elongate environment positioning method based on a UWB/SINS combined system as described in any of the above when the program is executed.
In a fourth aspect, the present invention also provides a non-transitory computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements the steps of an elongate environment positioning method based on a UWB/SINS combined system as described in any of the above.
The invention is based on the fact that UWB can improve the precision through a static multi-epoch smoothing mode, and firstly, the self-adaptive PSO (particle swarm optimization) search algorithm of static multi-epoch ranging is adopted to realize the position initialization of the UWB/SINS combined system. In the dynamic process, the influence of NLOS errors is considered, and the invention designs a robust UKF (lossless Kalman filtering) fusion positioning algorithm based on post-verification innovation residual errors, namely a UWB ranging noise matrix is rebuilt according to the filtered UKF innovation residual errors, so that the influence of NLOS errors is effectively weakened.
Drawings
In order to more clearly illustrate the invention or the technical solutions of the prior art, the following description will briefly explain the drawings used in the embodiments or the description of the prior art, and it is obvious that the drawings in the following description are some embodiments of the invention, and other drawings can be obtained according to the drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of an elongated environment positioning method based on a UWB/SINS combined system provided by the invention;
FIG. 2 is a schematic illustration of the UWB positioning principle of an elongated environment provided by the present invention;
fig. 3 is a schematic structural diagram of an electronic device provided by the present invention.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the present invention more apparent, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings, and it is apparent that the described embodiments are some embodiments of the present invention, not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
It should be noted that in the description of embodiments of the present invention, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element.
The following describes an elongated environment positioning method and apparatus based on a UWB/SINS combined system according to an embodiment of the present invention with reference to fig. 1-3.
Fig. 1 is a flow chart of an elongated environment positioning method based on a UWB/SINS combined system according to the present invention, as shown in fig. 1, including but not limited to the following steps:
step 101 (coordinate initialization stage): and taking the minimum ranging error as a target, and acquiring the static coordinates of the UWB/SINS combined system by utilizing a particle swarm algorithm based on the ultra-wideband UWB technology.
Step 102 (dynamic positioning phase): based on the static coordinates acquired by UWB, a robust unscented Kalman filtering fusion algorithm is adopted by combining the SINS of the strapdown inertial navigation system, and a UWB ranging noise matrix is rebuilt, so that fusion positioning of the UWB/SINS combined system in a long and narrow environment is realized. Optionally, the elongate environment is a tunnel environment.
The technical scheme of the invention is described below with reference to specific embodiments.
FIG. 2 is a schematic view of UWB positioning principle of the long and narrow environment provided by the invention, as shown in FIG. 2, the positioning essence based on distance measurement is a system of solving nonlinear equations, taking trilateral intersection 2-D positioning as an example, assuming UWB three base station coordinates are (x 1 ，y 1 )、(x 2 ，y 2 )、(x 3 ，y 3 ) The position of the label to be solved is (x, y), and the three-edge ideal ranging is d 1 、d 2 、d 3 The following equation is satisfied:
the above formula (1 a) is different from (1 b), (1 c), respectively, taking into account y 1 ≈y 2 The tag position (x, y) can be obtained:
constant of the above type Considering that the actual UWB ranging contains noise, assume +.>The noisy tag position +.>And (x, y) corresponding to the ideal ranging, a noisy tag positioning error (sigma) can be obtained x ，σ y )：
Constant of the above type In an elongated environment, for the short axis +.>Can be regarded as sigma y Due to y 3 And y 1 The phase difference is small, the noise coefficient becomes very large, and the short axis positioning error sigma is directly caused y Amplified.
The UWB positioning generally has redundant observation, and the traditional analytic method is a least square or other nonlinear iterative optimization method, and all the traditional analytic methods need to solve a linearized error equation or an iterative increment equation, and the specific form is that
Hx＝b (4)
Above, x is the state to be solvedB is a constant vector, H is a jacobian matrix corresponding to Deltax, and for the situation that m base station signals are observed, the base station coordinates are p 1 ，…，p m For the ith base station, p i ＝{x i ，y i Label coordinate approximation p 0 ＝{x 0 ，y 0 The H matrix is typically of the form:
the upper part of the device is provided with a plurality of grooves, I.I 2 Representing a binary norm, deltaK 1 ＝x i -x 0 ，Δy i ＝y i -y 0 . Due to Deltay in long and narrow environment i Generally, the noise H matrix is relatively small, the disease state of the noise H matrix is easy, and solving the formula (3) inevitably generates solving errors. According to the equation set error analysis theory in the numerical analysis, the solved state model error delta x can be obtained, and meets the following conditions
Above, i·i represents the norm, δb may be considered as the UWB ranging error. cond (H) = |h -1 The H is the condition number of the matrix H and can be according to H T And calculating the maximum and minimum eigenvalues of the H matrix. The H matrix tends to be ill-conditioned in a long and narrow environment, and the condition number cond (H) tends to be large. The condition number of the coefficient matrix of the equation set may describe the effect of solution errors of the equation set due to constant vector errors, with greater condition numbers causing greater uncertainty in the solution.
The procedure of UWB static coordinate initialization based on Particle Swarm Optimization (PSO) algorithm is described below.
The PSO basic core idea is to use individual in the group to share information so that the motion of the whole group generates an evolution process from disorder to order in a problem solving space, thereby obtaining the optimal solution of the problem. For an N-dimensional space, taking the population scale as N, let x i ＝(x i1 x i2 … x iN )，v i ＝(v i1 v i2 … v iN ) The position vector and the velocity vector used for representing the ith particle of the PSO algorithm are of dimension N. pBest for the current optimal position of the ith particle i To indicate that the current optimal position of the population is denoted as gBest, and the standard particle swarm algorithm can be expressed by the following formula:
the above equation, i= {1,2, …, n } represents the number of particles, k represents the number of iterations, ω represents the inertial weight, d represents the dimension, d= {1,2} for UWB positioning of the present invention. The self-cognition part and the social cognition part of the particles are composed of learning factors c 1 、c 2 And (5) determining. To preserve the diversity of the population, [0,1 ] was introduced]Random number r between 1 、r 2 . From equation (6), it can be seen that the particle velocity update is mainly composed of the previous inertial velocity of the particle, the particle self-sensing portion pBest i And social perception gBest. Particle passing through continual updating pBest i And gBest to adjust the speed until a certain iteration convergence condition is met, so as to realize the searching process of the optimal target.
The standard particle swarm algorithm is fast in convergence speed, so that the diversity of the population can be rapidly reduced, the population is extremely easy to fall into premature convergence, and an improved PSO searching algorithm is generally adopted in practical application. The invention adopts an adaptive calculation inertial weight omega based on linear change.
Above, k max Representing the maximum number of iterations. Along with the increase of the iteration times, the inertia weight is continuously reduced, so that the particle swarm algorithm has stronger global convergence capacity in the initial stageHas stronger local convergence capacity in the later stage.
For different search tasks, most important is to build fitness functions to calculate pBest i Aiming at the optimal 2-D position searching task of the UWB static tag in the long and narrow environment, a weighted PSO fitness function based on particle positions is established by considering the influence of ranging errors of different base stations on y-axis positioning errors.
The distance measurement from any particle (x, y) to a certain Base Station (BS) is d, which satisfies the following conditionAssume that different stations measure distance and other precision sigma d 。
Deriving short axis positioning error sigma from error propagation law y Relationship with range error:
above, sigma x The long axis positioning error is generally small. d/y is the ranging magnification factor, further finding the short axis positioning error sigma y And distance D of the particle to the short axis.
Thus, the weight coefficient w is determined from the ranging of particle i to base station j and the vertical distance of particle i to base station j ij The method comprises the steps of carrying out a first treatment on the surface of the The method comprises the following steps:
above, w= Σw ij ，D ij Is the vertical distance of particle i from base station j. Establishing a fitness function of a particle swarm algorithm:
above, F (p) i ) Representing PSO fitness value corresponding to particle i, m representing UWB base station number, p j For the corresponding coordinates, p i For the position of the ith particle, t represents the number of static observation epochs of UWB,the actual ranging between the UWB tag and the base station j at time t is shown.
Equation (11) reflects the random particle position p during the period T i The particle with the smallest fitness function value in each iteration can be judged as pBest i The particle with the smallest historical fitness function value represents gBest. In practical application, p can be determined by only 2-3 base stations i The optimal search location, considering that some base station ranging may have a large system difference or NLOS error, does not necessarily utilize all base station ranging information.
Optionally, in the case that it is determined that the number of base stations of UWB is greater than 3, arbitrarily selecting three base stations from a set including all base stations to construct a plurality of base station subsets; executing the particle swarm algorithm on each base station subset, and executing PSO search to obtain a corresponding fitness value; determining an optimal base station subset from all the base station subsets according to the fitness value corresponding to each base station subset; and initializing static coordinates by using the base stations corresponding to the optimal base station subset.
The invention utilizes the optimal subset of base stations to determine p i The optimal search position has a certain robustness.
The following describes the dynamic positioning phase based on the robust unscented Kalman filter fusion algorithm (RUKF).
The core of fusion positioning of the UWB/SINS combined system is filter state estimation, the invention designs a 2-D positioning model based on UWB/SINS tight combination, and the invention adopts a robust UKF filter algorithm based on post-test residual error self-adaption adjustment UWB ranging noise to realize UWB/SINS fusion in consideration of the characteristics of long and narrow environment and the influence of NLOS errors.
Assume that the nonlinear state equation and the observation equation of the system are respectively
x k ＝f(x k-1 )+w k (12a)
y k ＝h(x k )+v k (12b)
Above, wherein x k And y k The state vector and the observation vector are respectively at the moment k, f () is a state transition equation, h () is an observation equation, and w k V is the system noise vector k Is observed as noise vector, is Gaussian white noise and meetsThe process noise and the measurement noise matrix of the system are respectively.
For UWB/SINS positioning, the predicted state of the system is obtained by using the IMU (inertial measurement unit) mechanical arrangement of the SINS, and the variance-covariance matrix of the state can be deduced according to an error equation. The error state δx in the close combination is:
the upper part of the device is provided with a plurality of grooves,projection of two-dimensional position, two-dimensional speed and misalignment angle error of carrier system (b system) relative to navigation system (n system) on n system, delta b g ，δb a And the gyro and the acceleration are respectively zero offset errors.
The state equation in equation (12 a) needs to be linearized, and a phi angle error model is usually adopted, and the differential equation of the error state δx is as follows
The upper part of the device is provided with a plurality of grooves,is the rotation angular velocity of the earth, f b Acceleration of b series, ++>For the b-series versus n-series attitude matrix, δg n For the earth gravity term, w is the random walk process noise for each state error, and the corresponding spectral density parameter can be given by an Allan variance calibration or experience. The linearized state equation is expressed as:
the above formula, F is a state matrix, obtainable according to (14)
The upper part of the device is provided with a plurality of grooves,F 3 is->First two rows of>
The IMU updating time interval is tau, when tau is smaller, the discretized state matrix is phi, and the process is thatThe noise matrix is Q, the state error δx k，k-1 Variance-covariance P k，k-1 The recurrence formula is delta x k|k-1 ＝Φ k|k-1 δx k-1 (17a)
Φ k|k-1 ＝I+τ·F k-1 (17c)
Q k-1 ＝τ·q k-1 (17d)
Above, δx k|k-1 and The error state and variance matrix are predicted for the IMU. q is a variance intensity matrix corresponding to white noise w, and the dimension unit is consistent with the unit of power spectral density.
The IMU can obtain the recursive position of the UWB tag after mechanical arrangement, and a virtual observation value rho can be formed between the IMU and the UWB base station and the tag, so that a UWB observation equation and an innovation error equation can be obtained as follows:
y＝ρ+R(18a)
the upper part of the device is provided with a plurality of grooves,for observational quantity, i.e. UWB ranging. R is UWB ranging noise matrix, IMU (SINS) predicted positionThen for the jth base station position (x j ，y j ) Virtual distance measurement->
EKF (kalman filter) generally requires deriving a jacobian from the state according to equation (18), and the jacobian is prone to model errors in localization due to morbidity in long and narrow environments. UKF (lossless Kalman Filter) does not require linearization of the observation error equation, and UKF generally approximates the random distribution characteristics of the state vector by 2n+1 Sigma points and corresponding weights. The cost function approaching to the output performance index is minimized while the distribution characteristics of the input variables are ensured.
IMU mechanical arrangement acquisition state prediction value x k|k-1 Corresponding variance-covariance matrix P k|k-1 Symmetric sampling strategy based on Cholesky decomposition was used to generate 2n+1 Sigma points χ k 。
Above, n is x k|k-1 Dimension, D of k The diagonal elements of the Cholesky decomposition for n x n are the lower triangular matrix of positive numbers. λ=α 2 (n+beta) -n, namely scale factor, beta is a constant, and is set to be 0 or 3-n, alpha is more than or equal to 0.0001 and less than or equal to 1, and the weight of each Sigma point is as follows:
above, W n And W is c The weights of the mean and variance, respectively.
UWB measurement update according to (18 a)
y k|k-1 ＝h(χ k )(21a)
Above, whereinTo weight the predicted observations, P yy Is the corresponding covariance matrix.
And (4) filtering and updating according to UWB actual ranging:
above, y k I.e. UWB ranging observations, K k As a gain matrix, δx k For estimated error state, x k|k-1 The filtering state x can be updated through error closed loop k ，P k Is a corresponding state covariance matrix (variance-covariance (VC) matrix).
The UWB ranging noise matrices in general formulas (18 a) and (21 c) are given according to a priori accuracy, and when NLOS error formulas exist, the UWB ranging noise matrices cannot be well identified, and large errors in positioning can be caused without processing. By utilizing the characteristic of high recurrence precision in a short time of inertial navigation, the abnormal observation value is detected according to the formula (18 b) innovation residual error delta z, the UWB redundant observation is considered to be generally less, and the self-adaptive adjustment coefficient gamma of the UWB ranging noise matrix is calculated by adopting an equivalent weight function (preset equivalent weight function) which is not reset.
On the upper partRepresenting normalized innovation residual, k 0 For a preset constant, the present invention is given as 3. When NLOS error exists in the observed value, noise of corresponding ranging can be amplified through the above formula, and negative influence of the NLOS error can be restrained to a certain extent.
The upper part of the device is provided with a plurality of grooves,to reconstruct the new UWB ranging noise matrix, the gain matrix is calculated by re-substituting the above formula (22 b) to realize x k P k Robust solution.
In summary, the method for positioning the long and narrow environment based on the UWB/SINS combined system provided by the invention has the following technical effects:
(1) In the coordinate initialization stage, aiming at the defects that the short axis positioning precision of the traditional positioning method is not high and the traditional positioning method is easy to be interfered by the environment in a long and narrow environment, a particle position-based self-adaptive PSO adaptability model is provided, and when the UWB base station has redundancy, the optimal base station subset can be self-adaptively determined according to the global optimal adaptability value, so that the PSO static positioning precision is further improved;
(2) In the dynamic positioning stage, UWB/SINS is integrated, the nonlinear filter UKF is adopted to effectively reduce the model error of the traditional linear filter in the long and narrow environment, and the robust model based on the innovation residual can effectively inhibit the NLOS influence of UWB, so that the dynamic positioning precision is further improved.
(3) The jacobian matrix calculation in the traditional method is not needed in the static initialization and dynamic fusion process stages, the problem of large positioning of short axis errors of long and narrow environments caused by linearization is avoided, and the robustness of UWB/SINS positioning under NLOS can be improved to a certain extent.
The invention also provides a long and narrow environment positioning device based on the UWB/SINS combined system, which comprises: and the coordinate initializing module and the dynamic positioning module.
The coordinate initialization module is used for acquiring the static coordinates of the UWB/SINS combined system by using a particle swarm algorithm based on the ultra wideband UWB technology by taking the minimum ranging error as a target;
the dynamic positioning module is used for reconstructing a UWB ranging noise matrix by combining a strapdown inertial navigation system SINS and adopting a robust unscented Kalman filtering fusion algorithm on the basis of the static coordinates acquired by UWB so as to realize fusion positioning of the UWB/SINS combined system in a long and narrow environment.
It should be noted that, when the positioning device for an elongated environment based on a UWB/SINS combined system according to the embodiment of the present invention specifically operates, the positioning method for an elongated environment based on a UWB/SINS combined system described in any one of the above embodiments may be executed, which is not described in detail in this embodiment.
Fig. 3 is a schematic structural diagram of an electronic device provided by the present invention, and as shown in fig. 3, the electronic device may include: processor 310, communication interface (communications interface) 320, memory 330 and communication bus 340, wherein processor 310, communication interface 320, memory 330 accomplish communication with each other through communication bus 340. Processor 310 may invoke logic instructions in memory 330 to perform an elongate environment positioning method based on a UWB/SINS combined system, the method comprising: a coordinate initialization stage and a dynamic positioning stage; wherein, the coordinate initialization stage: based on ultra wideband UWB technology, the particle swarm algorithm is utilized to obtain the static coordinates of the UWB/SINS combined system by taking the minimum ranging error as the target; dynamic positioning stage: based on the static coordinates acquired by UWB, a robust unscented Kalman filtering fusion algorithm is adopted by combining the SINS of the strapdown inertial navigation system, and a UWB ranging noise matrix is rebuilt, so that fusion positioning of the UWB/SINS combined system in a long and narrow environment is realized.
Further, the logic instructions in the memory 330 described above may be implemented in the form of software functional units and may be stored in a computer-readable storage medium when sold or used as a stand-alone product. Based on this understanding, the technical solution of the present invention may be embodied essentially or in a part contributing to the prior art or in a part of the technical solution, in the form of a software product stored in a storage medium, comprising several instructions for causing a computer device (which may be a personal computer, a server, a network device, etc.) to perform all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk, or an optical disk, or other various media capable of storing program codes.
In another aspect, the present invention also provides a computer program product comprising a computer program stored on a non-transitory computer readable storage medium, the computer program comprising program instructions which, when executed by a computer, are capable of performing the method for positioning an elongate environment based on a UWB/SINS combined system provided by the above embodiments, the method comprising: a coordinate initialization stage and a dynamic positioning stage; wherein, the coordinate initialization stage: based on ultra wideband UWB technology, the particle swarm algorithm is utilized to obtain the static coordinates of the UWB/SINS combined system by taking the minimum ranging error as the target; dynamic positioning stage: based on the static coordinates acquired by UWB, a robust unscented Kalman filtering fusion algorithm is adopted by combining the SINS of the strapdown inertial navigation system, and a UWB ranging noise matrix is rebuilt, so that fusion positioning of the UWB/SINS combined system in a long and narrow environment is realized.
In yet another aspect, the present invention further provides a non-transitory computer readable storage medium having stored thereon a computer program which, when executed by a processor, is implemented to perform the method for positioning an elongate environment based on a UWB/SINS combined system provided in the above embodiments, the method comprising: a coordinate initialization stage and a dynamic positioning stage; wherein, the coordinate initialization stage: based on ultra wideband UWB technology, the particle swarm algorithm is utilized to obtain the static coordinates of the UWB/SINS combined system by taking the minimum ranging error as the target; dynamic positioning stage: based on the static coordinates acquired by UWB, a robust unscented Kalman filtering fusion algorithm is adopted by combining the SINS of the strapdown inertial navigation system, and a UWB ranging noise matrix is rebuilt, so that fusion positioning of the UWB/SINS combined system in a long and narrow environment is realized.
The apparatus embodiments described above are merely illustrative, wherein the elements illustrated as separate elements may or may not be physically separate, and the elements shown as elements may or may not be physical elements, may be located in one place, or may be distributed over a plurality of network elements. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment. Those of ordinary skill in the art will understand and implement the present invention without undue burden.
From the above description of the embodiments, it will be apparent to those skilled in the art that the embodiments may be implemented by means of software plus necessary general hardware platforms, or of course may be implemented by means of hardware. Based on this understanding, the foregoing technical solution may be embodied essentially or in a part contributing to the prior art in the form of a software product, which may be stored in a computer readable storage medium, such as ROM/RAM, a magnetic disk, an optical disk, etc., including several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the method described in the respective embodiments or some parts of the embodiments.
Finally, it should be noted that: the above embodiments are only for illustrating the technical solution of the present invention, and are not limiting; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims (10)

1. An elongated environment positioning method based on a UWB/SINS combined system, which is characterized by comprising the following steps: a coordinate initialization stage and a dynamic positioning stage;
wherein, the coordinate initialization stage:
based on ultra wideband UWB technology, the particle swarm algorithm is utilized to obtain the static coordinates of the UWB/SINS combined system by taking the minimum ranging error as the target;
dynamic positioning stage:
based on the static coordinates acquired by UWB, a robust unscented Kalman filtering fusion algorithm is adopted by combining the SINS of the strapdown inertial navigation system, and a UWB ranging noise matrix is rebuilt, so that fusion positioning of the UWB/SINS combined system in a long and narrow environment is realized.
2. The method for positioning an elongated environment based on a UWB/SINS combined system according to claim 1, wherein the fitness function of the particle swarm algorithm is specifically:
wherein F (p) i ) Representing PSO fitness value corresponding to particle i, m representing the number of UWB base stations, p j Coordinates corresponding to base station of UWB, p i For the position of the ith particle, t represents the number of static observation epochs of UWB,indicating time t between UWB tag and base station jIs the actual ranging, w ij The weight coefficient representing particle i to base station j.
3. An elongate environment positioning method based on UWB/SINS combined system according to claim 2, characterized in that the weight coefficient w ij Is determined from the ranging of particle i to base station j and the vertical distance of particle i to base station j.
4. A method for locating an elongate environment based on a combined UWB/SINS system according to claim 3 wherein the weight coefficient w is determined based on the ranging of particle i to base station j and the vertical distance of particle i to base station j ij The specific mode of (a) is as follows:
wherein w= Σw ij ，D ij For the vertical distance d of particle i from base station j ij Indicating the ranging of particle i to base station j.
5. The method for positioning an elongate environment based on a UWB/SINS combined system according to claim 3, further comprising, at the coordinate initialization stage:
in the case that the number of base stations of UWB is determined to be more than 3, arbitrarily selecting three base stations from a set containing all base stations to construct a plurality of base station subsets;
executing the particle swarm algorithm on each base station subset to obtain a corresponding fitness value;
determining an optimal base station subset from all the base station subsets according to the fitness value corresponding to each base station subset;
and initializing static coordinates by using the base stations corresponding to the optimal base station subset.
6. The method for positioning an elongated environment based on a UWB/SINS combined system according to claim 3, wherein the robust unscented kalman filter fusion algorithm comprises:
under the condition that the observed value is abnormal according to the innovation residual error, calculating the self-adaptive adjustment coefficient of the UWB ranging noise matrix by adopting a preset equivalent weight function;
reconstructing a UWB ranging noise matrix by utilizing the self-adaptive adjustment coefficient;
the information residual error is determined as follows:
y＝ρ+R；
where y=ρ+r represents the UWB observation equation, ρ is the virtual observation, R is the UWB ranging noise matrix, δz represents the innovation residual,representing observed quantity, namely UWB ranging, wherein R is UWB ranging noise matrix; let SINS predicted position->Then for the jth base station position (x j ,y j ) Virtual distance measurement->
The self-adaptive adjustment coefficient is determined as follows:
wherein, gamma represents the adaptive adjustment coefficient,representing the absolute value, k, of the normalized innovation residual 0 Is a preset constant.
7. The method for locating an elongate environment based on a combined UWB/SINS system of claim 1 wherein the elongate environment is a tunnel environment.
8. An elongated environmental positioning device based on a UWB/SINS combination system, comprising: the system comprises a coordinate initialization module and a dynamic positioning module;
the coordinate initialization module is used for acquiring the static coordinates of the UWB/SINS combined system by using a particle swarm algorithm based on the ultra wideband UWB technology by taking the minimum ranging error as a target;
the dynamic positioning module is used for reconstructing a UWB ranging noise matrix by combining a strapdown inertial navigation system SINS and adopting a robust unscented Kalman filtering fusion algorithm on the basis of the static coordinates acquired by UWB so as to realize fusion positioning of the UWB/SINS combined system in a long and narrow environment.
9. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor implements the steps of the elongate environment positioning method based on the UWB/SINS combined system according to any of claims 1 to 7 when executing the computer program.
10. A non-transitory computer readable storage medium having stored thereon a computer program, wherein the computer program when executed by a processor implements the steps of the elongate environment positioning method based on a UWB/SINS combined system according to any of claims 1 to 7.
CN202310615693.4A 2023-05-24 2023-05-24 Method and device for positioning long and narrow environment based on UWB/SINS combined system Active CN116448106B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310615693.4A CN116448106B (en) 2023-05-24 2023-05-24 Method and device for positioning long and narrow environment based on UWB/SINS combined system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310615693.4A CN116448106B (en) 2023-05-24 2023-05-24 Method and device for positioning long and narrow environment based on UWB/SINS combined system

Publications (2)

Publication Number Publication Date
CN116448106A true CN116448106A (en) 2023-07-18
CN116448106B CN116448106B (en) 2024-05-03

Family

ID=87128725

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310615693.4A Active CN116448106B (en) 2023-05-24 2023-05-24 Method and device for positioning long and narrow environment based on UWB/SINS combined system

Country Status (1)

Country Link
CN (1) CN116448106B (en)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111896008A (en) * 2020-08-20 2020-11-06 哈尔滨工程大学 Improved robust unscented Kalman filtering integrated navigation method
CN112073909A (en) * 2020-08-20 2020-12-11 哈尔滨工程大学 UWB (ultra wide band)/MEMS (micro electro mechanical systems) combination based UWB base station position error compensation method
CN113074739A (en) * 2021-04-09 2021-07-06 重庆邮电大学 UWB/INS fusion positioning method based on dynamic robust volume Kalman
CN114166221A (en) * 2021-12-08 2022-03-11 中国矿业大学 Auxiliary transportation robot positioning method and system in dynamic complex mine environment
CN115469267A (en) * 2022-09-07 2022-12-13 辽宁工程技术大学 UWB positioning method based on particle swarm optimization support vector machine and autonomous integrity monitoring
CN115767412A (en) * 2022-10-13 2023-03-07 厦门大学 Indoor positioning method integrating ultra-wideband and inertial measurement unit

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111896008A (en) * 2020-08-20 2020-11-06 哈尔滨工程大学 Improved robust unscented Kalman filtering integrated navigation method
CN112073909A (en) * 2020-08-20 2020-12-11 哈尔滨工程大学 UWB (ultra wide band)/MEMS (micro electro mechanical systems) combination based UWB base station position error compensation method
CN113074739A (en) * 2021-04-09 2021-07-06 重庆邮电大学 UWB/INS fusion positioning method based on dynamic robust volume Kalman
CN114166221A (en) * 2021-12-08 2022-03-11 中国矿业大学 Auxiliary transportation robot positioning method and system in dynamic complex mine environment
CN115469267A (en) * 2022-09-07 2022-12-13 辽宁工程技术大学 UWB positioning method based on particle swarm optimization support vector machine and autonomous integrity monitoring
CN115767412A (en) * 2022-10-13 2023-03-07 厦门大学 Indoor positioning method integrating ultra-wideband and inertial measurement unit

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
HAI YANG, TAO LUO, WEI LI, LI LI, YUE RAO, AND CHENGMING LUO: "A Stable SINS/UWB Integrated Positioning Method of Shearer Based on the Multi-Model Intelligent Switching Algorithm", IEEE ACCESS, vol. 7, 31 December 2019 (2019-12-31), pages 29128 - 29138, XP011715620, DOI: 10.1109/ACCESS.2019.2898212 *
YUAN XUA, CHOON KI AHNB, YURIY S. SHMALIYC, XIYUAN CHEND, YUEYANG LI: "Adaptive robust INS/UWB-integrated human tracking using UFIR filter bank", MEASUREMENT, vol. 123, 31 December 2018 (2018-12-31), pages 1 - 7, XP085387292, DOI: 10.1016/j.measurement.2018.03.043 *
刘迅成: "针对室内狭长环境的低成本UWB/SINS组合定位系统", 中国优秀硕士学位论文全文数据库 信息科技辑, no. 03, 15 March 2020 (2020-03-15), pages 136 - 736 *
刘韬;徐爱功;隋心: "超宽带与惯性导航组合的室内导航定位", 测绘科学, vol. 41, no. 12, 20 December 2016 (2016-12-20), pages 162 - 166 *

Also Published As

Publication number Publication date
CN116448106B (en) 2024-05-03

Similar Documents

Publication Publication Date Title
Hajiyev et al. Robust adaptive unscented Kalman filter for attitude estimation of pico satellites
RU2701194C2 (en) Method of estimating navigation state in conditions of limited possibility of observation
CN109597864B (en) Method and system for real-time positioning and map construction of ellipsoid boundary Kalman filtering
CN110514203B (en) Underwater integrated navigation method based on ISR-UKF
Wang et al. Augmented cubature Kalman filter for nonlinear RTK/MIMU integrated navigation with non-additive noise
Sabet et al. Optimal design of the own ship maneuver in the bearing-only target motion analysis problem using a heuristically supervised extended Kalman filter
CN112161632B (en) Satellite formation initial positioning method based on relative position vector measurement
RU2454631C1 (en) Method for autonomous navigation and orientation of spacecraft based on virtual measurements of zenith distances of stars
Gao et al. Rapid alignment method based on local observability analysis for strapdown inertial navigation system
Goppert et al. Invariant Kalman filter application to optical flow based visual odometry for UAVs
Gao et al. Adaptively random weighted cubature Kalman filter for nonlinear systems
CN115265532A (en) Auxiliary filtering method for marine integrated navigation
Cucci et al. On raw inertial measurements in dynamic networks
CN116448106B (en) Method and device for positioning long and narrow environment based on UWB/SINS combined system
Zhang et al. Error prediction for SINS/GPS after GPS outage based on hybrid KF‐UKF
Ryan Information-theoretic tracking control based on particle filter estimate
Zong et al. Randomly weighted CKF for multisensor integrated systems
Jia et al. A Quaternion‐Based Robust Adaptive Spherical Simplex Unscented Particle Filter for MINS/VNS/GNS Integrated Navigation System
Hajiyev et al. Integration of algebraic method and EKF for attitude determination of small information satellites
Yun et al. On-the-fly ambiguity resolution method for pseudolite/INS integration based on double-difference square observations
Liu et al. Absolute navigation and positioning of mars rover using gravity-aided odometry
Gong et al. Unscented particle smoother and its application to transfer alignment of airborne distributed POS
Chen et al. Spacecraft autonomous GPS navigation based on polytopic linear differential inclusion
US20230026000A1 (en) Particle filtering method and navigation system using measurement correlation
Marcus et al. Variable Resolution Quadtree Mapping for Planetary Landing Using Planar Elements

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