CN116772835A - Indoor positioning method and system based on inertial navigation and UWB sensor network - Google Patents
Indoor positioning method and system based on inertial navigation and UWB sensor network Download PDFInfo
- Publication number
- CN116772835A CN116772835A CN202310736682.1A CN202310736682A CN116772835A CN 116772835 A CN116772835 A CN 116772835A CN 202310736682 A CN202310736682 A CN 202310736682A CN 116772835 A CN116772835 A CN 116772835A
- Authority
- CN
- China
- Prior art keywords
- target
- positioning
- inertial navigation
- sensor
- uwb
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 44
- 239000002245 particle Substances 0.000 claims abstract description 45
- 238000005259 measurement Methods 0.000 claims abstract description 40
- 238000005457 optimization Methods 0.000 claims abstract description 39
- 238000001914 filtration Methods 0.000 claims abstract description 22
- 230000008447 perception Effects 0.000 claims abstract description 20
- 230000006870 function Effects 0.000 claims abstract description 3
- 230000004927 fusion Effects 0.000 claims description 31
- 230000013016 learning Effects 0.000 claims description 15
- 230000001133 acceleration Effects 0.000 claims description 5
- 238000003860 storage Methods 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 description 14
- 230000000694 effects Effects 0.000 description 12
- 238000001514 detection method Methods 0.000 description 7
- 230000033001 locomotion Effects 0.000 description 7
- 239000011159 matrix material Substances 0.000 description 7
- 238000009826 distribution Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 238000002474 experimental method Methods 0.000 description 4
- 238000012544 monitoring process Methods 0.000 description 4
- 230000003068 static effect Effects 0.000 description 4
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000007704 transition Effects 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 240000007651 Rubus glaucus Species 0.000 description 1
- 235000011034 Rubus glaucus Nutrition 0.000 description 1
- 235000009122 Rubus idaeus Nutrition 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 239000003245 coal Substances 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012843 least square support vector machine Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000005065 mining Methods 0.000 description 1
- 230000000717 retained effect Effects 0.000 description 1
- 230000009326 social learning Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0257—Hybrid positioning
- G01S5/0258—Hybrid positioning by combining or switching between measurements derived from different systems
- G01S5/02585—Hybrid positioning by combining or switching between measurements derived from different systems at least one of the measurements being a non-radio measurement
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/004—Artificial life, i.e. computing arrangements simulating life
- G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Biophysics (AREA)
- Evolutionary Computation (AREA)
- Artificial Intelligence (AREA)
- Biomedical Technology (AREA)
- Health & Medical Sciences (AREA)
- Computational Linguistics (AREA)
- Data Mining & Analysis (AREA)
- Life Sciences & Earth Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Molecular Biology (AREA)
- Computing Systems (AREA)
- General Engineering & Computer Science (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The invention discloses a positioning method and a system based on inertial navigation and UWB wireless sensor network, wherein the method comprises the following steps: obtaining a distance value and a real distance value measured by a sensor, and constructing to obtain a sensor perception model; based on a sensor perception model of a plurality of sensors in a target space, taking coverage rate as an optimization objective function, and carrying out optimization solution by utilizing an improved particle swarm optimization algorithm to obtain a deployment scheme of the plurality of sensors in the target space; UWB positioning data of the target to be detected are obtained, inertial navigation positioning data of the target to be detected are obtained through inertial measurement, and error Kalman filtering is utilized to fuse the UWB positioning data and the inertial navigation positioning data, so that a fused positioning result is obtained. According to the invention, an improved particle swarm optimization algorithm is utilized to obtain a UWB sensor deployment scheme, and the error Kalman filtering is utilized to fuse the position data of the UWB positioning network and the position data calculated by the inertial navigation system, so that the high-precision and high-stability closed environment positioning is realized.
Description
Technical Field
The invention relates to the technical field of indoor positioning, in particular to an indoor positioning method and system based on inertial navigation and UWB sensor network.
Background
Positioning techniques are used to obtain location information of objects (persons, equipment or other objects) in an environment, and there are different demands on the positioning techniques in different situations. The global positioning system (Global Positioning System, GPS) and the beidou satellite navigation system (Beidou Navigation Satellite System, BDS) are currently mainstream global positioning systems, and because satellite signals can be transmitted in open outdoor space without obstacles, the positioning systems can provide high-quality positioning service outdoors, but in indoor environments, satellite signals are interfered and cannot be precisely positioned due to the shielding of walls and obstacles, so that the GPS, BDS and the like are not suitable for positioning in closed environments. In addition, compared with outdoor positioning, multipath effect and delay are more easily caused due to shielding or reflection of signals by walls, people, equipment and the like in the closed space, so that positioning difficulty is increased, and signal propagation of a positioning sensor can be influenced by other indoor wireless signal interference sources. With the continuous expansion of mobile robots in living production, the current demands for indoor environment positioning technology are gradually increasing.
In recent years, with the popularization of wireless positioning technology, an Ultra Wide Band (UWB) positioning scheme is widely paid attention to, and is used for solving the problem that satellite signals are interfered and cannot be accurately positioned under indoor environments due to shielding of walls and obstacles. The UWB technology is a wireless communication technology, utilizes subnanosecond pulses to carry out data transmission, has lower influence degree due to multipath effect, has higher anti-interference capability, safety and transmission rate, and has lower cost. UWB is mainly used in the military fields such as radar searching and wireless safety communication in early stage, and the advantages of UWB are shown in the indoor positioning field in recent years. However, in the actual positioning process, although the hardware condition of the UWB device itself can improve the positioning accuracy to a certain extent, the signal shielding by the obstacle in the indoor environment still reduces the positioning accuracy of the system. In addition, when UWB is used as a unique positioning method for positioning, the positioning result still has certain volatility and lower stability due to the influence of environmental factors (unknown dynamic obstacles and the like), and the working performance of the positioning result cannot meet the requirements of actual indoor positioning. That is, the reliability and stability of indoor positioning by a single positioning scheme, i.e., UWB positioning scheme, are still not high, and cannot cope with complex and varied real environments.
The deployment cost of the UWB positioning network can be reduced through a reasonable sensor deployment scheme, meanwhile, the problem that indoor obstacles block wireless signals is solved to a certain extent, and further the service quality and the positioning accuracy of the sensor network are improved. The wireless sensor network (Wireless Sensor Network, WSN) deployment optimization methods can be divided into three categories: virtual force-based algorithms, computational geometry-based algorithms, and intelligent optimization-based algorithms. Since the advantages and disadvantages of the sensor deployment optimization problem solution are closely related to the performance of the optimization algorithm and the optimization problem model, how to design a reasonable problem model and improve the optimization algorithm are one of the keys and difficulties in improving the quality of the positioning system.
In addition, the prior art proposes various schemes for improving the stability of a positioning network to a certain extent through fusion positioning, such as a tightly combined positioning method based on UWB and an inertial measurement unit, wherein firstly, a least square support vector machine model is trained by utilizing the actual distance and the measured distance of each base station of a robot and UWB, then, the actual distance measurement value of each base station of the UWB in the movement process of the robot is corrected through the model so as to reduce the influence of non-line-of-sight errors on the final positioning precision, and finally, the corrected distance measurement value is combined with the distance information solved by an inertial navigation system through Kalman filtering based on an error state, and the combined positioning system improves the positioning precision of the robot in a mine environment; the method comprises the steps of firstly introducing a minimum variance estimation theory of a self-adaptive optimal weighted fusion algorithm to adjust particle distribution weights, then setting a threshold value for limiting an observed variance to avoid divergence of the observed variance, and finally obtaining an optimal weighting factor of each sensor by using root mean square error after particle filtering, thereby effectively improving the positioning accuracy of vehicle navigation; the fusion positioning method based on unscented Kalman filtering for positioning the underground coal mining machine is provided, the data of a UWB system is used as observables, an inertial navigation and UWB fusion positioning model is established, and the result is smoothed by using a VB-UKF adaptive filtering algorithm, so that the real-time compensation of inertial navigation measurement errors is realized; aiming at the problem of reduced UWB positioning accuracy in a non-line-of-sight environment, an extended Kalman filtering-based fusion positioning method is provided, and the inertial measurement unit is combined with UWB to overcome the limitations of a single positioning technology and the like by utilizing the characteristic that the inertial measurement unit is less influenced by obstacles in the non-line-of-sight environment. Therefore, to avoid the problem of poor reliability and stability of a single positioning scheme, how to implement fusion positioning is one of the other key and difficult problems of improving indoor positioning accuracy.
Disclosure of Invention
In order to solve the defects of the prior art, the invention provides an indoor positioning method and system based on inertial navigation and UWB sensor network, which are suitable for positioning in a closed space, a reasonable UWB sensor deployment scheme is obtained by utilizing an improved particle swarm optimization algorithm, the influence of a known static obstacle on the positioning precision of the system is reduced by deploying in advance, the precision and stability of the positioning network are further improved, and then the position data of the UWB positioning network and the position data calculated by the inertial navigation system are fused by utilizing error Kalman filtering, so that the problem of insufficient positioning precision and stability of a single sensor is solved, and the closed environment positioning with high precision and high stability is realized.
In a first aspect, the present disclosure provides an indoor positioning method based on inertial navigation and UWB sensor networks.
An indoor positioning method based on inertial navigation and UWB sensor network, comprising:
obtaining a distance value and a real distance value measured by a sensor, determining a sensing radius and reliability parameters of the sensor, and further constructing a sensor sensing model of the sensor;
based on a sensor perception model of a plurality of sensors in a target space, taking coverage rate as an optimization target, and carrying out optimization solution by utilizing an improved particle swarm optimization algorithm to obtain a deployment scheme of the plurality of sensors in the target space;
according to the deployment scheme, a plurality of sensors are deployed in a target space, UWB positioning data of a target to be detected are obtained, inertial navigation positioning data of the target to be detected are obtained through inertial measurement, and error Kalman filtering is utilized to fuse the UWB positioning data and the inertial navigation positioning data, so that a fusion positioning result of the target to be detected is obtained.
In a second aspect, the present disclosure provides an indoor positioning system based on inertial navigation and UWB sensor networks.
An inertial navigation and UWB sensor network based indoor positioning system comprising:
the sensor perception model construction module is used for acquiring a distance value and a real distance value which are measured by the sensor, determining a perception radius and reliability parameters of the sensor, and further constructing a sensor perception model of the sensor;
the sensor deployment scheme solving module is used for optimizing and solving by using an improved particle swarm optimization algorithm based on sensor perception models of a plurality of sensors in a target space and taking coverage rate as an optimization target to obtain a deployment scheme of the plurality of sensors in the target space;
the fusion positioning result acquisition module is used for deploying a plurality of sensors in a target space according to a deployment scheme, acquiring UWB positioning data of a target to be detected, acquiring inertial navigation positioning data of the target to be detected through inertial measurement, and fusing the UWB positioning data and the inertial navigation positioning data by utilizing error Kalman filtering to obtain a fusion positioning result of the target to be detected.
In a third aspect, the present disclosure also provides an electronic device comprising a memory and a processor, and computer instructions stored on the memory and running on the processor, which when executed by the processor, perform the steps of the method of the first aspect.
In a fourth aspect, the present disclosure also provides a computer readable storage medium storing computer instructions which, when executed by a processor, perform the steps of the method of the first aspect.
The one or more of the above technical solutions have the following beneficial effects:
1. the invention provides an indoor positioning method and system based on an inertial navigation and UWB sensor network, which are suitable for positioning in a closed space, optimize and deploy a wireless sensor network to reduce the influence of known static obstacles on wireless signal shielding and further on system positioning accuracy, improve the service quality and positioning accuracy of the sensor network and reduce the deployment cost of the positioning sensor network.
2. According to the invention, the problem of insufficient positioning precision and stability of a single sensor is solved by utilizing the position data of the UWB positioning network and the position data calculated by the inertial navigation system fused by error Kalman filtering, and the closed environment positioning with high precision and high stability is realized.
Drawings
The accompanying drawings, which are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate embodiments of the invention and together with the description serve to explain the invention.
FIG. 1 is a schematic diagram of a statistical perceptual model in an embodiment of the present invention;
FIG. 2 is a schematic diagram showing the relationship between the measurement accuracy and the distance of a UWB sensor according to the embodiment of the invention;
FIG. 3 is a flow chart of an optimization solution using the improved particle swarm algorithm IPSO-VF in accordance with one embodiment of the present invention;
FIG. 4 is a flow chart of a fusion positioning in an embodiment of the invention;
FIG. 5 is a graph of coverage effects of initial random deployment of UWB sensors in an embodiment of the invention;
FIG. 6 is a graph showing the final coverage results for four different algorithms at different sensor densities in an embodiment of the invention;
FIG. 7 is a graph of coverage effects of an optimized UWB sensor deployment via four different algorithms in an embodiment of the invention;
FIG. 8 is a schematic diagram of an experimental trajectory for fusion positioning in an embodiment of the present invention.
Detailed Description
It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the invention. Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs.
It is noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of exemplary embodiments according to the present invention. As used herein, the singular is also intended to include the plural unless the context clearly indicates otherwise, and furthermore, it is to be understood that the terms "comprises" and/or "comprising" when used in this specification are taken to specify the presence of stated features, steps, operations, devices, components, and/or combinations thereof.
Example 1
The embodiment provides an indoor positioning method based on an inertial navigation and UWB sensor network, which utilizes an improved particle swarm optimization algorithm to obtain a reasonable UWB wireless sensor deployment scheme, reduces the influence of known static obstacles on wireless signal shielding and further on system positioning accuracy by optimizing and deploying the wireless sensor network in advance, improves the service quality and positioning accuracy of the sensor network, and reduces the deployment cost of the positioning sensor network; the position data of the UWB positioning network and the position data calculated by the inertial navigation system are fused by utilizing error Kalman filtering, so that the problem of insufficient positioning precision and stability of a single sensor is solved, and the positioning of a high-precision and high-stability closed environment (indoor) is realized.
The indoor positioning method based on the inertial navigation and UWB sensor network provided by the embodiment comprises the following steps:
step S1, obtaining a distance value and a real distance value which are measured by a sensor, determining a sensing radius and reliability parameters of the sensor, and further constructing a sensor sensing model of the sensor;
s2, based on a sensor perception model of a plurality of sensors in a target space, taking coverage rate as an optimization target, and carrying out optimization solution by utilizing an improved particle swarm optimization algorithm to obtain a deployment scheme of the plurality of sensors in the target space;
and S3, deploying a plurality of sensors in a target space according to a deployment scheme, acquiring UWB positioning data of the target to be detected, acquiring inertial navigation positioning data of the target to be detected through inertial measurement, and fusing the UWB positioning data and the inertial navigation positioning data by utilizing error Kalman filtering to obtain a fused positioning result of the target to be detected.
The implementation of the indoor positioning method based on the inertial navigation and UWB sensor network according to the embodiment is further described through the following matters.
In step S1, first, considering that in a real environment, the sensing capability of a sensor is interfered by other nodes, and the accuracy and the distance of data acquisition show a nonlinear relationship, the embodiment uses a statistical sensing model closer to the actual as a sensor sensing model of the sensor, as shown in fig. 1. Where R is the perceived radius of the sensor and R is the measurement reliability parameter (R < R). In the model, a probability calculation formula of the point to be detected T detected by the sensor N is as follows:
β 1 ＝r-R+d N,T (2)
β 2 ＝r+R-d N,T (3)
wherein d N,T Is the Euclidean distance between the sensor N and the point to be measured T, R is the sensing radius of the sensor, and R is the measurement reliabilityParameters, alpha 1 、α 2 、δ 1 And delta 2 Is a measurement parameter.
Secondly, because the deployment optimizing effect of the sensor network is closely related to the actual sensor sensing model, in order to make the sensing model more practical, the embodiment determines the sensor sensing radius R and the reliability parameter R by using a measurement accuracy formula, and the formula is as follows:
wherein dis m And dis r Distance values and true distance values (in centimeters) measured by the UWB sensor, respectively.
Specifically, the perceived radius and reliability parameters of the sensor are calculated and determined in the following manner. Presetting a positioning base station and a positioning label, adjusting the distance between the positioning base station and the positioning label, measuring and recording the actual measurement distance, wherein the test range is 0-7.2 m. In addition, to avoid accidental results, a total of multiple independent tests (20 rounds of this example) were performed, and the average of the experimental results is shown in fig. 2 and table 1 below.
Table 1 average measurement accuracy of UWB sensor
According to table 1 above, the sensor perceived radius R is set to 2.5 and the reliability parameter R is set to 0.5. According to the definition of the statistical perception model, when the distance d between the to-be-measured point and the base station sensor meets d < R-R, the to-be-measured point can be perceived by the base station sensor with an accuracy rate close to 100%, and the absolute perception area of the model is corresponding; when R-R is less than d is less than R+r, the accuracy of the sensing of the point to be detected by the base station sensor decreases along with the increase of the distance, and the sensing area is uncertain in the corresponding model; when d > R+r, the accuracy of the sensing of the point to be measured by the base station sensor is close to 0, corresponding to an imperceptible area in the model.
Through the scheme, the sensing radius and the reliability parameters of the sensor are determined, and then the sensor sensing model of the sensor is constructed.
In step S2, in order to enable the improved particle swarm optimization algorithm to solve the deployment scheme of the optimal sensor positioning network more efficiently, so as to improve the positioning accuracy of the sensor network to the target environment, the embodiment uses coverage rate as an optimization target. Assuming that the target monitoring area E is a square area of lxw, N sensor nodes are randomly distributed in the E, denoted as n= { N 1 ,N 2 ,…,N n }，N i The position coordinate of the ith sensor node is marked as N i ＝(x1,y 1 ). For facilitating subsequent calculation, the target monitoring area E is divided into l×w=e target pixel points to be measured, denoted as t= { T 1 ,T 2 ,…,T e }. The coverage rate reflects the coverage effect of the sensor network on the target monitoring area, is defined as the ratio of the number of the target pixel points to be detected covered by the sensor network to the total number of the target pixel points to be detected, and has the following calculation formula:
wherein P is cov (N,T i ) Is the wireless sensor network N to the ith target point T to be measured i The calculation method is shown in formulas (6) and (7):
wherein p (N, T) j ) For all sensor nodes N to T in E region j P (N) i ,T j ) For the sensor node N i For the detection target point T j The probability is calculated based on the sensor perception model determined in the step S1, and the calculation method is as shown in formulas (1) - (3)Showing; p is p th Is a probability threshold, when p (N, T j ) When the threshold value is larger than the threshold value, the target pixel point T to be detected is judged j Can be effectively detected by the wireless sensor network N.
And then, carrying out optimization solution by utilizing an improved particle swarm algorithm to obtain a deployment scheme of a plurality of sensors in a target space. In this embodiment, the modified particle swarm optimization algorithm is denoted as IPSO-VF, whose flow chart is shown in FIG. 3.
Step S2.1, initializing related parameters, namely setting initial parameters of an optimization algorithm, such as pixel points l and w of a target monitoring area, the number of sensors n, the iteration times K and the iteration threshold K th Population size M, particle dimension 2n, search space range [ P min ,P max ]Range of particle velocity [ V min ,V max ]
Maximum and minimum inertial weights ω min And omega max Maximum value of learning factor c max 。
Step S2.2, initializing the positions and the speeds of all particles, wherein an initialization formula is as follows:
P i,j ＝P min +(P max -P min )*Rand (8)
V i,j ＝V min +(V max -V min )*Rand (9)
wherein P is i,j And V i,j Rand is a random number uniformly distributed in compliance with the U (0, 1) standard, which is the position component and the velocity component of the j-th dimension of the i-th particle, respectively.
Step S2.3, calculating fitness values of all individuals, updating an individual history optimal value BP and a population global optimal value BG, dynamically updating a strategy according to S-shaped inertia weights, and re-calculating the inertia weights, wherein the formula is as follows:
where o is the adjustment factor, K is the current iteration number, and K is the maximum iteration number. The strategy can enable the inertia weight omega to maintain a larger value in the early iteration stage, enable the algorithm to obtain higher global searching capability, then rapidly decline in the middle stage, finally maintain a lower value in the later iteration stage, enable the algorithm to obtain larger local searching capability, and better balance the global searching capability and the local searching capability of the algorithm.
Step S2.4, introducing a random learning term, and updating the particle speed, wherein the formula is as follows:
wherein V is i,j (k) The j-th-dimensional velocity component, ω, being the inertial weight, c, for the kth-generation ith particle 1 、c 2 And c 3 Individual learning factors, social learning factors and random learning factors, respectively; u (u) 1 、u 2 And u 3 Is uniformly distributed in [0,1 ]]Random numbers of (a); BP (BP) rand,j The j-th dimensional component representing the individual historical optimum of random particles randi.
The embodiment introduces a random learning term based on an original speed update formula of the particle swarm algorithm: when the speed is updated, the algorithm randomly selects a particle randi from the population, and then the individual history optimal value of the randi is used as a third learning object to guide the search of the next generation of the population, so that the algorithm is prevented from being sunk into a local optimal solution too early.
And S2.5, judging according to the iteration times and the iteration threshold value, and adaptively adjusting and updating the learning factor. If the iteration number is k<Iteration threshold k th Then learn factor c 1 、c 2 And c 3 The updated formula of (c) is as follows:
if k is greater than or equal to k th Then learn factor c 1 、c 2 And c 3 The updated formula of (c) is as follows:
wherein BG is a global historical optimal value of the population, BP i And BP randi Representing the individual historical optimum of the ith particle and random particle randi, respectively.
As can be seen from formulas (12) and (13), the random learning guidance is activated in the early stage and is adjusted to be in the dormant state in the later stage, so that the algorithm can fully explore the whole searching area by the population in the early stage of searching, and the situation that the population falls into a local solution dominated by the historical optimal value and the global historical optimal value of an individual is avoided; at the later stage of the search, the algorithm is quickly converged to a global optimum. In addition, the learning factors are adaptively adjusted according to the objective function values of the learning objects, namely, individuals with good fitness have higher leadership to search, and on the basis of increasing population diversity, the individuals are ensured to move to more potential areas.
In addition, when the number of iterations exceeds the iteration threshold k th The learning object only remains a global historical optimum and an individual historical optimum, and the global historical optimum is the best position currently searched and is the reference object for all particles. Therefore, the quality of the global history optimal value has a great influence on the precision of the final solution, and the global history optimal value is appropriately disturbed in the later period of searching, so that the potential area can be more efficiently explored. In this embodiment, the optimal global history value is retained by the following formulas after the Lewy flight and the Cauchy variation are adopted respectively:
BG(k)＝{X|max g(X),X∈{BG(k),New 1 ,New 2 }} (15)
where λ is the step size of the Lewy flight, x 0 Is the position parameter of the cauchy variation, gamma is the scale parameter of the cauchy variation, BG i Representing the ith dimension component of BG.
Step S2.6, updating the positions of the particles in the next generation of particle swarm, wherein the formula is as follows:
P i,j (k)＝P i,j (k-1)+V i,j (k) (16)
s2.7, calculating virtual repulsive force among the sensor nodes, wherein a calculation formula is as follows:
wherein mu r Is the repulsive force coefficient, d th Is a distance threshold, d ij Is the sensor node N i And N j Euclidean distance, θ ij Represents N i Pointing to N j Included angle of vector of (c) and positive direction of x-axis.
And updating the positions of the particles in the particle swarm guided by the virtual repulsive force according to the virtual repulsive force among the sensor nodes. Wherein node N i Resultant force F subjected to repulsive force of all sensor nodes in area E i N is as shown in formula (18) i The step length of the movement in the abscissa direction due to the virtual repulsive force is shown as formula (19):
wherein Deltax is i And Deltay i Respectively represent N i Step length moving in x-axis and y-axis directions, F ix And F iy Finger force F i Step is the maximum movement step in the x-axis, y-axis components.
The method introduces the concept of virtual repulsive force in the virtual force algorithm to avoid too dense distribution of sensor nodes of the wireless sensor network (Wireless Sensor Networks, WSN). Wherein the sensor is abstracted into charged particles, repulsive force is generated when the distance between the nodes is smaller than a threshold value, and the distance between the sensor nodes tends to expand outwards to avoid concentrated distribution.
S2.8, judging whether the maximum iteration times are reached, if so, stopping iteration, and completing optimizing the sensor deployment scheme; otherwise, returning to the step S2.3, and performing the next iterative calculation.
Through the scheme, the population global optimal value and the individual fitness value are output, and the deployment scheme of a plurality of sensors in the target space is obtained.
In step S3, a plurality of sensors are deployed in the target space according to the deployment scheme obtained in step S2, so that the defect that the positioning error of the UWB system is increased due to the shielding signal of the static obstacle can be effectively improved through sensor deployment optimization. However, when UWB is used as a unique positioning method for positioning, the positioning result still has a certain fluctuation due to the influence of environmental factors (unknown dynamic obstacles, etc.), and the working performance cannot meet the requirement of actual indoor positioning. In order to reduce the positioning error and increase the positioning stability of the system, the embodiment also utilizes error Kalman filtering to fuse UWB positioning data and inertial navigation positioning data. The inertial navigation positioning data are data obtained through an inertial measurement unit, the inertial measurement unit (Inertial Measurement Unit, IMU) is a device for measuring three-axis attitude angles (or angular rates) and accelerations of an object, inertial measurement data of a target to be measured are obtained through the IMU, the inertial measurement data comprise accelerations and angular velocities, position, velocity and attitude estimation of the target to be measured are obtained through inertial navigation solution, and state prediction of the target to be measured is carried out by utilizing the data. The nominal state and the prediction error state of the target to be measured are obtained through calculation by utilizing the data obtained by the inertial measurement unit, the prediction error state is corrected by taking the position measurement information (namely UWB positioning data) of UWB as an observation value, and the real state of the target to be measured is calculated by combining the nominal state, so that a final fusion positioning result is obtained. The flow of this fusion positioning is shown in fig. 4.
Step S3.1, UWB positioning data of the target to be measured are obtained, inertial measurement data (comprising acceleration and angular velocity of the target to be measured) of the target to be measured are obtained through an inertial measurement unit, inertial navigation positioning data of the target to be measured are obtained through inertial navigation solution according to the inertial measurement data, and estimated values of position, velocity, gesture and the like are obtained. Furthermore, inertial navigation positioning data of the target to be detected is used as a nominal state of the target to be detected.
And S3.2, predicting the error state based on inertial navigation positioning data of the target to be detected to obtain a prediction error state. Specifically, the error state is the state quantity of the actual operation of the fusion positioning system, and is recordedIs an error state. Wherein δp T Is the position error δv T As speed error, delta theta T For posture error->For drift bias of gyroscope->For accelerometer drift bias (gyroscope bias and accelerometer bias are bias existing by the inertial measurement unit/device during measurement), the differential equation of the system error state is:
wherein C is b n Is a coordinate system conversion matrix, mu ω Sum mu a Is a time constant, n b And n ω Represents acceleration measurement noise and angular velocity measurement noise, gamma ω And gamma a Representing white noise interference with mean value of 0, [ f× ]]The definition is as follows:
to facilitate subsequent analytical calculations, the state equation (20) of the error state is converted to the following form using Kalman filtering:
wherein F is k For the state transition matrix at time k, G k For the noise gain matrix at time k, n k The k-time noise matrix is defined as:
n k ＝[n ω ,n a ,γ ω ,γ a ] T (25)
in two continuous time intervals deltat of the discrete system, an error state prediction equation based on IMU data is:
in order to simplify the calculation formula, the above error state prediction equation is converted into a formula (27), and the covariance prediction method is shown in a formula (28):
δX k|k-1 ＝A k|k-1 δX k-1|k-1 +G k n k (27)
wherein P is k-1|k-1 Estimating covariance for a posterior at time k-1, Q being n k Covariance of A) k For the estimation of the error state transition matrix of the inertia measurement unit at the moment k, the calculation formula is as follows:
A k|k-1 ≈I 15×15 +F k Δt (29)
and S3.3, updating a prediction error state by using Kalman filtering according to the acquired UWB positioning data of the target to be detected. The error state two-side update and covariance matrix calculation formula is as follows:
P k|k ＝(I-K k H k )P k|k-1 (31)
wherein δX is k|k Is the error state after the k moment is updated, delta X k|k-1 Is an a priori estimate of the error state at time k,and p k UWB The position coordinates of the target to be measured are measured by an inertial navigation system at k moment and an UWB system, H k Is a state quantity-to-quantity transition matrix, defined as H k ＝[I 3×3 0 3×3 0 3×3 0 3×3 0 3×3 ]，K k For the Kalman gain at time k, the calculation formula is as follows:
in step S3.4, in the Error Kalman filtering (ESKF) model, the actual motion state of the target to be measured is equal to the sum of the nominal state and the Error state, so that based on the updated predicted Error state and the nominal state, the state combination is performed to obtain the fusion positioning result of the target to be measured, namely the true state X of the target to be measured is obtained k|k The calculation formula is as follows:
X k|k ＝X k|k-1 +δX k|k (33)
wherein X is k|k-1 Is the nominal state at time k, δX k|k Is the prediction error state updated by ESKF.
Step S3.5, outputting the true value state X of the target to be tested k|k And outputting the target position of the target to be detected, which is measured by the fusion positioning system.
To further verify the superiority of the solution described above for this embodiment, verification is performed by the following example. First, to verify the performance of the improved particle swarm optimization algorithm, the algorithm proposed in this example was compared to the original PSO, VFA, and HGWOP. To avoid occasional errors, all results data were averaged over 30 independent experiments, with the experimental parameters shown in table 2 below. In order to avoid that the effect of the comparison experiment is affected by different random initialization schemes, all algorithms have the same initial solution, fig. 5 is a coverage effect of the initial deployment network, the initial coverage rate is 67.16%, the numerical value of the bar on the right side is the detection probability, and the higher the color temperature is, the greater the detection probability of the sensor network to the region is, namely the higher the positioning accuracy is. Further, the experiment was performed with a different number of sensors in a 100×100 area to check the versatility of the algorithm, and the result is shown in fig. 6. The result shows that the coverage rate of all algorithms gradually increases to 1 along with the increase of the number of the sensor nodes, and the proposed IPSO-VF achieves complete coverage of the whole detection area at first when n=35. Thus, IPSO-VF can more efficiently utilize sensors and achieve coverage of a target area in sensor deployment optimization of different densities.
Table 2 experimental parameter settings
Fig. 7 shows a final coverage effect diagram of the sensor network after optimization solution by four algorithms when the number of sensors is 25. As can be seen from fig. 5, the sensors in the sensor network deployed at random are unevenly distributed, the coverage rate is only 67.16%, and a large number of coverage holes exist; fig. 7 shows that the sensor network optimized by the algorithms IPSO-VF and VFA according to this embodiment has a better coverage effect on the target detection area, and the sensor distribution is more uniform and the repeated coverage area is less. The rightmost bar indicates that the lowest detection probability point of the sensor network deployed by the IPSO-VFA to the target detection area is about 0.65, the VFA is about 0.5, and PSO and HGWOP are 0, which means that the coverage of the whole target area is realized with higher precision compared with other comparison algorithms IPSO-VF. Meanwhile, the IPSO-VF algorithm achieves the optimal coverage effect, the coverage rate is 99.6%, the final coverage rate is improved by 32.44% compared with the initial coverage rate, and the final coverage rate is 11.36%, 4.24% and 5.36% higher than PSO, VFA and HGWOP respectively.
Compared with a comparison algorithm, the IPSO-VF algorithm provided by the embodiment can obtain a better sensor deployment scheme, and the optimization performance of the algorithm is obviously improved compared with that of the original PSO. In addition, due to the introduction of the virtual force guiding strategy, the IPSO-VF can optimize the distribution of the sensors by utilizing the virtual repulsive force among the sensors, reduce the repeated coverage of the sensors and realize the efficient utilization of the sensors.
In addition, in order to verify the positioning accuracy and stability of the UWB positioning network and the inertial navigation fusion positioning system based on the error state Kalman filter, an Ackerman robot is used as a motion platform to carry out fusion positioning experiments, and the IPSO-VF is utilized to obtain a UWB sensor deployment scheme. The platform uses STM32F405 as a lower computer control board and raspberry pie as an upper computer processor. In the experimental process, the robot platform moves along a circular track with the circle center of (4 m,4.7 m) and the radius of 1.57 m.
In order to examine the positioning accuracy improving effect of the fusion positioning method, a theoretical track of the motion platform, a pure UWB (ultra-wideband) resolving track and a fusion positioning method resolving track are respectively drawn, and the result is shown in fig. 8. Wherein (a) in fig. 8 shows the overall situation of three tracks and (b) in fig. 8 shows the local details of the tracks. As can be seen from fig. 8 (a), the overall tracks measured by the single UWB positioning and the fusion positioning are converged to a circular track, but the overlapping sections of the imu+uwb fusion track and the real track are more and the difference is smaller, i.e. the fusion positioning result more accords with the actual track of the robot platform; fig. 8 (b) shows a local detail of the track, wherein the track points measured by the single UWB method have frequent and severe fluctuation and have a larger gap from the real track, and the fused positioning track is more gentle and more similar to the real motion track, which indicates that the ESKF-based UWB positioning network and the inertial navigation fused positioning method have higher positioning stability and accuracy compared with the single positioning system.
Example two
The embodiment provides an indoor positioning system based on inertial navigation and UWB sensor network, comprising:
the sensor perception model construction module is used for acquiring a distance value and a real distance value which are measured by the sensor, determining a perception radius and reliability parameters of the sensor, and further constructing a sensor perception model of the sensor;
the sensor deployment scheme solving module is used for optimizing and solving by using an improved particle swarm optimization algorithm based on sensor perception models of a plurality of sensors in a target space and taking coverage rate as an optimization target to obtain a deployment scheme of the plurality of sensors in the target space;
the fusion positioning result acquisition module is used for deploying a plurality of sensors in a target space according to a deployment scheme, acquiring UWB positioning data of a target to be detected, acquiring inertial navigation positioning data of the target to be detected through inertial measurement, and fusing the UWB positioning data and the inertial navigation positioning data by utilizing error Kalman filtering to obtain a fusion positioning result of the target to be detected.
Example III
The present embodiment provides an electronic device comprising a memory and a processor and computer instructions stored on the memory and running on the processor, which when executed by the processor, perform the steps in an indoor positioning method based on inertial navigation and UWB sensor networks as described above.
Example IV
The present embodiment also provides a computer readable storage medium storing computer instructions that, when executed by a processor, perform the steps in an inertial navigation and UWB sensor network based indoor positioning method as described above.
The steps involved in the second to fourth embodiments correspond to the first embodiment of the method, and the detailed description of the second embodiment refers to the relevant description of the first embodiment. The term "computer-readable storage medium" should be taken to include a single medium or multiple media including one or more sets of instructions; it should also be understood to include any medium capable of storing, encoding or carrying a set of instructions for execution by a processor and that cause the processor to perform any one of the methods of the present invention.
It will be appreciated by those skilled in the art that the modules or steps of the invention described above may be implemented by general-purpose computer means, alternatively they may be implemented by program code executable by computing means, whereby they may be stored in storage means for execution by computing means, or they may be made into individual integrated circuit modules separately, or a plurality of modules or steps in them may be made into a single integrated circuit module. The present invention is not limited to any specific combination of hardware and software.
The above description is only of the preferred embodiments of the present invention and is not intended to limit the present invention, but various modifications and variations can be made to the present invention by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
While the foregoing description of the embodiments of the present invention has been presented in conjunction with the drawings, it should be understood that it is not intended to limit the scope of the invention, but rather, it is intended to cover all modifications or variations within the scope of the invention as defined by the claims of the present invention.
Claims (10)
1. An indoor positioning method based on inertial navigation and UWB sensor network is characterized by comprising the following steps:
obtaining a distance value and a real distance value measured by a sensor, determining a sensing radius and reliability parameters of the sensor, and further constructing a sensor sensing model of the sensor;
based on a sensor perception model of a plurality of sensors in a target space, taking coverage rate as an optimization target, and carrying out optimization solution by utilizing an improved particle swarm optimization algorithm to obtain a deployment scheme of the plurality of sensors in the target space;
according to the deployment scheme, a plurality of sensors are deployed in a target space, UWB positioning data of a target to be detected are obtained, inertial navigation positioning data of the target to be detected are obtained through inertial measurement, and error Kalman filtering is utilized to fuse the UWB positioning data and the inertial navigation positioning data, so that a fusion positioning result of the target to be detected is obtained.
2. The indoor positioning method based on inertial navigation and UWB sensor network according to claim 1, wherein the coverage rate is the ratio of the number of target pixel points to be detected covered by the sensor network to the total number of target pixel points to be detected.
3. The indoor positioning method based on inertial navigation and UWB sensor network of claim 1, wherein the optimizing solution by using the improved particle swarm optimization algorithm, to obtain a deployment scheme of a plurality of sensors in a target space, comprises:
initializing relevant parameters and the positions and speeds of all particles;
calculating fitness values of all individuals, updating historical optimal values BP of the individuals and global optimal values BG of the population, dynamically updating strategies according to S-shaped inertia weights, and recalculating the inertia weights;
introducing a random learning item, and updating the particle speed;
judging according to the iteration times and the iteration threshold value, and adaptively adjusting and updating the learning factor;
updating the position of each particle in the next generation particle swarm based on the updated particle velocity;
calculating virtual repulsive force among the sensor nodes, and updating the positions of particles in the particle swarm guided by the virtual repulsive force;
judging whether the maximum iteration times are reached, if so, stopping iteration, and outputting a population global optimal value and an individual fitness value to obtain a deployment scheme of a plurality of sensors in a target space; otherwise, repeating iterative computation.
4. An inertial navigation and UWB sensor network based indoor positioning method according to claim 3, wherein the introducing random learning term, updating the particle velocity, comprises:
and randomly selecting a particle randi from the population, and guiding the search of the next generation of the population by taking the individual history optimal value of the particle randi as a newly added learning object.
5. The method for indoor positioning based on inertial navigation and UWB sensor network according to claim 1, wherein the acquiring inertial navigation positioning data of the object to be measured by inertial measurement comprises:
acquiring inertial measurement data of a target to be measured through an inertial measurement unit; the inertial measurement data includes acceleration and angular velocity;
according to the inertial measurement data, inertial navigation positioning data of the target to be measured are obtained through inertial navigation solution; the inertial navigation positioning data comprises position, speed and attitude estimation values.
6. The indoor positioning method based on inertial navigation and UWB sensor network according to claim 1, wherein the fusion of UWB positioning data and inertial navigation positioning data by error Kalman filtering to obtain a fusion positioning result of a target to be detected comprises:
taking inertial navigation positioning data of the target to be detected as a nominal state of the target to be detected, and predicting an error state based on the inertial navigation positioning data of the target to be detected to obtain a prediction error state;
updating a prediction error state by using Kalman filtering according to the acquired UWB positioning data of the target to be detected;
and carrying out state combination based on the updated prediction error state and nominal state to obtain a fusion positioning result of the target to be detected.
7. An indoor positioning system based on inertial navigation and UWB sensor network, characterized by comprising:
the sensor perception model construction module is used for acquiring a distance value and a real distance value which are measured by the sensor, determining a perception radius and reliability parameters of the sensor, and further constructing a sensor perception model of the sensor;
the sensor deployment scheme solving module is used for optimizing and solving by utilizing an improved particle swarm optimization algorithm based on sensor perception models of a plurality of sensors in a target space and taking coverage rate as an optimization objective function to obtain a deployment scheme of the plurality of sensors in the target space;
the fusion positioning result acquisition module is used for deploying a plurality of sensors in a target space according to a deployment scheme, acquiring UWB positioning data of a target to be detected, acquiring inertial navigation positioning data of the target to be detected through inertial measurement, and fusing the UWB positioning data and the inertial navigation positioning data by utilizing error Kalman filtering to obtain a fusion positioning result of the target to be detected.
8. The inertial navigation and UWB sensor network based indoor positioning system of claim 7 wherein the coverage is a ratio of the number of target pixels to be measured to the total number of target pixels to be measured covered by the sensor network.
9. An electronic device comprising a memory and a processor and computer instructions stored on the memory and running on the processor, which when executed by the processor, perform the steps of a method of inertial navigation and UWB sensor network based indoor positioning as defined in any of claims 1-6.
10. A computer readable storage medium storing computer instructions which, when executed by a processor, perform the steps of a method of inertial navigation and UWB sensor network based indoor positioning as defined in any of claims 1-6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310736682.1A CN116772835A (en) | 2023-06-20 | 2023-06-20 | Indoor positioning method and system based on inertial navigation and UWB sensor network |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310736682.1A CN116772835A (en) | 2023-06-20 | 2023-06-20 | Indoor positioning method and system based on inertial navigation and UWB sensor network |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116772835A true CN116772835A (en) | 2023-09-19 |
Family
ID=88009391
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310736682.1A Pending CN116772835A (en) | 2023-06-20 | 2023-06-20 | Indoor positioning method and system based on inertial navigation and UWB sensor network |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116772835A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118347507A (en) * | 2024-06-18 | 2024-07-16 | 中国科学院西安光学精密机械研究所 | Method for sensing near space target for aviation detection |
-
2023
- 2023-06-20 CN CN202310736682.1A patent/CN116772835A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118347507A (en) * | 2024-06-18 | 2024-07-16 | 中国科学院西安光学精密机械研究所 | Method for sensing near space target for aviation detection |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110118549B (en) | Multi-source information fusion positioning method and device | |
| CN106714110B (en) | Wi-Fi position fingerprint map automatic construction method and system | |
| US9594150B2 (en) | Determining device locations using movement, signal strength | |
| CN109597864B (en) | Method and system for real-time positioning and map construction of ellipsoid boundary Kalman filtering | |
| CN110118560B (en) | Indoor positioning method based on LSTM and multi-sensor fusion | |
| Zhang et al. | An efficient UAV localization technique based on particle swarm optimization | |
| Yan et al. | Extreme learning machine for accurate indoor localization using RSSI fingerprints in multifloor environments | |
| CN112729301A (en) | Indoor positioning method based on multi-source data fusion | |
| CN116772835A (en) | Indoor positioning method and system based on inertial navigation and UWB sensor network | |
| CN116678421B (en) | Multisource fusion positioning method and system based on multi-module BLE transmitting device | |
| Dou et al. | A bisection reinforcement learning approach to 3-D indoor localization | |
| CN108759846B (en) | Method for establishing self-adaptive extended Kalman filtering noise model | |
| Wang et al. | Deep neural network‐based Wi‐Fi/pedestrian dead reckoning indoor positioning system using adaptive robust factor graph model | |
| Bai et al. | Enhancing localization of mobile robots in distributed sensor environments for reliable proximity service applications | |
| KR20190122423A (en) | Method and system for indoor positioning based on machine learning | |
| CN117369507B (en) | Unmanned aerial vehicle dynamic path planning method of self-adaptive particle swarm algorithm | |
| Guo et al. | Research on indoor wireless positioning precision optimization based on UWB | |
| CN112130110A (en) | Unmanned aerial vehicle passive positioning track generation method and device | |
| CN116341344A (en) | Industrial moving target hybrid optimization wireless positioning method under uncertain measurement | |
| CN113891270B (en) | Electronic device and method for improving smoothness and accuracy of positioning | |
| Wang et al. | Research on SLAM road sign observation based on particle filter | |
| Gong et al. | Application of improved robust adaptive algorithm in UWB/MEMS positioning system | |
| CN117367431B (en) | MEMS and UWB (micro-electromechanical systems) tightly combined positioning method and system with unknown measurement bias | |
| CN118913262A (en) | UWB/IMU fusion positioning method based on environment judgment | |
| Cheng et al. | An INS/UWB joint indoor positioning algorithm based on hypothesis testing and yaw angle |
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 |