CN116772835A - Indoor positioning method and system based on inertial navigation and UWB sensor network - Google Patents

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

Indoor positioning method and system based on inertial navigation and UWB sensor network

Technical field

The present invention relates to the technical field of indoor positioning, and in particular to an indoor positioning method and system based on inertial navigation and UWB sensor network.

Background technique

Positioning technology is used to obtain location information of targets (people, equipment or other objects) in the environment, and different occasions have different requirements for positioning technology. Systems such as the Global Positioning System (GPS) and the Beidou Navigation Satellite System (BDS) are currently the mainstream global positioning systems. Since satellite signals can be transmitted without obstacles in open outdoor spaces, this type of positioning The system can provide high-quality positioning services outdoors, but in indoor environments due to the obstruction of walls and obstacles, satellite signals will be interfered with and cannot be accurately positioned. Therefore, GPS, BDS, etc. are not suitable for positioning in closed environments. In addition, compared with outdoor positioning, the obstruction or reflection of signals by walls, people and equipment in a confined space is more likely to cause multipath effects and delay problems, thereby increasing the difficulty of positioning, and other indoor wireless signal interference sources may also affect Signal propagation of positioning sensors. With the continuous expansion of mobile robots in daily life and production, the demand for indoor environment positioning technology is gradually increasing.

In recent years, with the popularization of wireless positioning technology, Ultra Wide Band (UWB) positioning solutions have received widespread attention and are used to solve the problem of inability to perform precise positioning due to interference with satellite signals due to obstruction by walls and obstacles in indoor environments. question. UWB technology is a wireless communication technology that uses sub-nanosecond pulses for data transmission. It is less affected by multipath effects, has high anti-interference capabilities, security, transmission rate and low cost. In the early days, UWB was mainly used in military fields such as radar search and wireless security communications. In recent years, it has demonstrated its superiority in the field of indoor positioning. However, in the actual positioning process, although the hardware conditions of the UWB device can improve the positioning accuracy to a certain extent, the obstruction of the signal by obstacles in the indoor environment will still reduce the positioning accuracy of the system. In addition, when using UWB as the only positioning method, due to the influence of environmental factors (unknown dynamic obstacles, etc.), the positioning results still have a certain degree of volatility and low stability, and its working performance cannot meet the needs of actual indoor positioning. . That is, the reliability and stability of indoor positioning using a single positioning solution, the UWB positioning solution, are still not high and cannot cope with complex and changeable real-world environments.

A reasonable sensor deployment solution can reduce the deployment cost of the UWB positioning network, and at the same time solve the problem of indoor obstacles blocking wireless signals to a certain extent, thereby improving the sensor network service quality and positioning accuracy. 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 solution to the sensor deployment optimization problem is 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 is one of the keys and difficulties in improving the quality of the positioning system.

In addition, the existing technology has proposed a variety of solutions to improve the stability of the positioning network to a certain extent through fused positioning. For example, a tight combination positioning method based on UWB and inertial measurement units is proposed. First, the actual distance between the robot and each UWB base station is used. Train a least squares support vector machine model with the measured distance, and then use this model to correct the measured distance values of UWB base stations during the robot's movement to reduce the impact of non-line-of-sight errors on the final positioning accuracy. Finally, the Kalman filter based on the error state is used The corrected ranging value is combined with the distance information calculated by the inertial navigation system. The combined positioning system improves the positioning accuracy of the robot in the mine environment. An improved particle filter method is proposed and applied to the fusion of UWB and inertial navigation systems. Positioning, this method first introduces the minimum variance estimation theory of the adaptive optimal weighted fusion algorithm to adjust the particle distribution weight, then sets a threshold to limit the observation variance to avoid the observation variance divergence, and finally uses the root mean square error after particle filtering The optimal weighting factor of each sensor is obtained to effectively improve the positioning accuracy of vehicle navigation; a fusion positioning method based on unscented Kalman filter is proposed for underground shearer positioning, using the data of the UWB system as an observation quantity, and establishing an inertial The navigation and UWB fusion positioning model is used, and the VB-UKF adaptive filtering algorithm is used to smooth the results to achieve real-time compensation of inertial navigation measurement errors. To address the problem of reduced UWB positioning accuracy in non-line-of-sight environments, an extended Kalman-based The filtered fusion positioning method utilizes the characteristics of the inertial measurement unit that is less affected by obstacles in the non-line-of-sight environment, and combines it with UWB to overcome the limitations of a single positioning technology. Therefore, in order to avoid the problems of poor reliability and stability of a single positioning solution, how to achieve fusion positioning is another key and one of the difficult problems to improve indoor positioning accuracy.

Contents of the invention

In order to solve the above-mentioned shortcomings of the existing technology, the present invention provides an indoor positioning method and system based on inertial navigation and UWB sensor network, which is suitable for positioning in a confined space and uses an improved particle swarm optimization algorithm to obtain a reasonable UWB sensor deployment plan. , reduce the impact of known static obstacles on the system positioning accuracy through early deployment, and further improve the accuracy and stability of the positioning network, and then use error Kalman filtering to fuse the position data of the UWB positioning network and the position data solved by the inertial navigation system , solve the problem of insufficient positioning accuracy and stability of a single sensor, and achieve high-precision and high-stability closed environment positioning.

In a first aspect, the present disclosure provides an indoor positioning method based on inertial navigation and UWB sensor network.

An indoor positioning method based on inertial navigation and UWB sensor network, including:

Obtain the distance value and real distance value measured by the sensor, determine the sensing radius and reliability parameters of the sensor, and then construct the sensor sensing model of the sensor;

Based on the sensor sensing model of multiple sensors in the target space, with coverage as the optimization goal, the improved particle swarm optimization algorithm is used for optimization and solution, and the deployment plan of multiple sensors in the target space is obtained;

Deploy multiple sensors in the target space according to the deployment plan, obtain the UWB positioning data of the target to be measured, obtain the inertial navigation positioning data of the target to be measured through inertial measurement, and use error Kalman filtering to perform the UWB positioning data and inertial navigation positioning data. Fusion to obtain the fusion positioning result of the target to be measured.

In a second aspect, the present disclosure provides an indoor positioning system based on inertial navigation and UWB sensor network.

An indoor positioning system based on inertial navigation and UWB sensor network, including:

The sensor perception model building module is used to obtain the distance value and the real distance value measured by the sensor, determine the sensing radius and reliability parameters of the sensor, and then construct the sensor perception model of the sensor;

The sensor deployment solution module is used to solve the sensor perception model based on multiple sensors in the target space. It uses the coverage rate as the optimization goal and uses the improved particle swarm optimization algorithm to optimize and solve to obtain the deployment plan of multiple sensors in the target space;

The fusion positioning result acquisition module is used to deploy multiple sensors in the target space according to the deployment plan, obtain the UWB positioning data of the target to be measured, and obtain the inertial navigation positioning data of the target to be measured through inertial measurement, and use error Kalman filtering to perform UWB The positioning data and inertial navigation positioning data are fused to obtain the fusion positioning result of the target to be measured.

In a third aspect, the present disclosure also provides an electronic device, including a memory, a processor, and computer instructions stored in the memory and executed on the processor. When the computer instructions are executed by the processor, the computer instructions in the first aspect are completed. Method steps.

In a fourth aspect, the present disclosure also provides a computer-readable storage medium for storing computer instructions. When the computer instructions are executed by a processor, the steps of the method described in the first aspect are completed.

One or more of the above technical solutions have the following beneficial effects:

1. The present invention provides an indoor positioning method and system based on inertial navigation and UWB sensor network, which is suitable for positioning in confined spaces and optimizes the deployment of wireless sensor networks to reduce the obstruction of wireless signals by known static obstacles and thereby position the system. The impact of accuracy can improve the service quality and positioning accuracy of sensor networks, and reduce the deployment cost of positioning sensor networks.

2. The present invention uses error Kalman filtering to fuse the position data of the UWB positioning network and the position data solved by the inertial navigation system to solve the problem of insufficient positioning accuracy and stability of a single sensor and achieve high-precision and high-stability closed environment positioning.

Description of drawings

The description and drawings that constitute a part of the present invention are used to provide a further understanding of the present invention. The illustrative embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention.

Figure 1 is a schematic diagram of a statistical perception model in an embodiment of the present invention;

Figure 2 is a schematic diagram of the relationship between UWB sensor measurement accuracy and distance in the embodiment of the present invention;

Figure 3 is a flow chart for optimization and solution using the improved particle swarm algorithm IPSO-VF in the embodiment of the present invention;

Figure 4 is a flow chart of fusion positioning in an embodiment of the present invention;

Figure 5 is a coverage effect diagram of the initial random deployment of UWB sensors in the embodiment of the present invention;

Figure 6 is a schematic diagram of the final coverage results of four different algorithms under different sensor densities in the embodiment of the present invention;

Figure 7 is a coverage effect diagram after the UWB sensor is deployed and optimized by four different algorithms in the embodiment of the present invention;

Figure 8 is a schematic diagram of the experimental trajectory of fusion positioning in the embodiment of the present invention.

Detailed ways

It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the present 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 should be noted that the terms used herein are for the purpose of describing specific embodiments only, and are not intended to limit the exemplary embodiments according to the present invention. As used herein, the singular forms are also intended to include the plural forms unless the context clearly indicates otherwise. Furthermore, it will be understood that when the terms "comprises" and/or "includes" are used in this specification, they indicate There are features, steps, operations, means, components and/or combinations thereof.

Embodiment 1

This embodiment provides an indoor positioning method based on inertial navigation and UWB sensor network, using an improved particle swarm optimization algorithm to obtain a reasonable UWB wireless sensor deployment plan, and optimizing the deployment of wireless sensor networks in advance to reduce known static obstacles. The impact of wireless signal obstruction on system positioning accuracy improves sensor network service quality and positioning accuracy, and reduces the deployment cost of positioning sensor networks; the error Kalman filter is used to calculate the position data of UWB positioning network and inertial navigation system The location data is fused to solve the problem of insufficient positioning accuracy and stability of a single sensor and achieve high-precision and high-stability positioning in a closed environment (indoor).

The indoor positioning method based on inertial navigation and UWB sensor network proposed in this embodiment includes the following steps:

Step S1: Obtain the distance value and the real distance value measured by the sensor, determine the sensing radius and reliability parameters of the sensor, and then construct a sensor sensing model of the sensor;

Step S2: Based on the sensor sensing model of multiple sensors in the target space, use the coverage rate as the optimization goal, use the improved particle swarm optimization algorithm to optimize and solve, and obtain the deployment plan of multiple sensors in the target space;

Step S3: Deploy multiple sensors in the target space according to the deployment plan, obtain the UWB positioning data of the target to be measured, obtain the inertial navigation positioning data of the target to be measured through inertial measurement, and use error Kalman filtering to compare the UWB positioning data and inertial navigation data. The positioning data is fused to obtain the fusion positioning result of the target to be measured.

The following content further introduces the implementation of the indoor positioning method based on inertial navigation and UWB sensor network in this embodiment.

In step S1, first of all, considering that in the real environment, the sensor's sensing ability will be interfered by other nodes, and the accuracy of data collection and distance show a non-linear relationship, therefore this embodiment uses a statistical sensing model that is closer to reality as the sensor. The sensor perception model is shown in Figure 1. Among them, R is the sensing radius of the sensor, and r is the measurement reliability parameter (r<R). In this model, the probability calculation formula of the point T to be measured being detected by the sensor N is:

β ₁ =r-R+d _N,T (2)

β ₂ =r+Rd _N,T (3)

Among them, d _N,T is the Euclidean distance between sensor N and the point to be measured T, R is the sensing radius of the sensor, r is the measurement reliability parameter, α ₁ , α ₂ , δ ₁ and δ ₂ are the measurement parameters.

Secondly, since the optimization effect of sensor network deployment is closely related to the actual sensor sensing model, in order to make the sensing model closer to reality, this embodiment uses the measurement accuracy formula to determine the sensor sensing radius R and reliability parameter r. The formula is as follows:

Among them, dis _m and dis _r are the distance value and the real distance value measured by the UWB sensor respectively (unit: centimeters).

Specifically, the sensing radius and reliability parameters of the sensor are calculated and determined in the following manner. Preset a positioning base station and a positioning tag, adjust the distance between the two, measure and record the actual measurement distance, the test range is 0m ~ 7.2m. In addition, in order to avoid the randomness of the results, a total of multiple independent tests (20 rounds were used in this embodiment) were conducted. The average value of the experimental results is shown in Figure 2 and Table 1 below.

Table 1 UWB sensor average measurement accuracy

According to Table 1 above, the sensor sensing radius R is set to 2.5 and the reliability parameter r is set to 0.5. According to the definition of the statistical sensing model, when the distance d between the point to be measured and the base station sensor satisfies d<R-r, the point to be measured can be sensed by the base station sensor with an accuracy close to 100%, corresponding to the absolute sensing area of the model; when R-r< When d<R+r, the accuracy with which the point to be measured is perceived by the base station sensor decreases as the distance increases, corresponding to the uncertain sensing area in the model; when d>R+r, the accuracy with which the point to be measured is perceived by the base station sensor The degree is close to 0, corresponding to the imperceptible area in the model.

Through the above solution, the sensing radius and 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 more efficiently solve the optimal sensor positioning network deployment plan and thereby improve the sensor network's positioning accuracy for the target environment, this embodiment uses coverage as the optimization target. Assume that the target monitoring area E is a square area of L×W, and n sensor nodes are randomly distributed in E, recorded as N = {N ₁ , N ₂ ,..., N _n }, N _i is the position coordinate of the i-th sensor node , recorded as N _i =(x1,y ₁ ). In order to facilitate subsequent calculations, the target monitoring area E is divided into l×w=e target pixel points to be measured, recorded as T={T ₁ , T ₂ ,..., T _e }. The coverage rate reflects the coverage effect of the sensor network on the target monitoring area, and is defined as the ratio of the number of target pixels to be measured covered by the sensor network to the total number of target pixels to be measured. The calculation formula is as follows:

Among them, P _cov (N,T _i ) is the joint detection probability of the i-th target point T _i to be measured by the wireless sensor network N. The calculation method is as shown in formulas (6) and (7):

Among them, p(N,T _j ) is the joint detection probability of all sensor nodes N in the E area to T _j , p(N _i , T _j ) is the detection probability of sensor node N _i to the detection target point T _j . This probability It is calculated based on the sensor sensing model determined in step S1 above. The calculation method is as shown in formulas (1)-(3); p _th is the probability threshold. When p (N, T _j ) is greater than the threshold, the target to be measured is determined. Pixel point T _j can be effectively detected by the wireless sensor network N.

Afterwards, the improved particle swarm algorithm is used for optimization and solution, and the deployment plan of multiple sensors in the target space is obtained. In this embodiment, the improved particle swarm optimization algorithm is recorded as IPSO-VF, and the flow chart of IPSO-VF is shown in Figure 3.

Step S2.1. Initialize relevant parameters, that is, set the initial parameters of the optimization algorithm, such as pixel points l and w in the target monitoring area, number of sensors n, number of iterations K, iteration threshold k _th , population size M, particle dimension 2n, search The range of space [P _min ,P _max ], the range of particle speed [V _min ,V _max ]

As well as the maximum and minimum inertia weights ω _min and ω _max , the maximum learning factor c _max .

Step S2.2. Initialize the positions and velocities of all particles. The 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)

In the formula, P _i,j and V _i,j are the position component and velocity component of the j-th dimension of the i-th particle respectively, and Rand is a random number obeying the U(0,1) standard uniform distribution.

Step S2.3: Calculate the fitness values of all individuals, update the individual historical optimal value BP and the population global optimal value BG, dynamically update the strategy according to the S-shaped inertia weight and recalculate the inertia weight. The formula is as follows:

Among them, o is the adjustment factor, k is the current iteration number, and K is the maximum iteration number. This strategy can keep the inertia weight ω at a large value in the early iteration, allowing the algorithm to obtain a higher global search capability, then decrease rapidly in the mid-term, and finally maintain a low value in the late iteration, allowing the algorithm to obtain a greater local search capability. Better balance the global and local search capabilities of the algorithm.

Step S2.4. Introduce random learning terms and update the particle speed. The formula is:

Among them, V _i,j (k) is the j-th dimensional velocity component of the i-th particle in the k-th generation, ω is the inertia weight, c ₁ , c ₂ and c ₃ are the individual learning factor, social learning factor and random learning factor respectively. ; u ₁ , u ₂ and u ₃ are random numbers uniformly distributed in [0,1]; BP _rand,j represents the j-th dimensional component of the individual historical optimal value of random particle randi.

This embodiment introduces a random learning term based on the original speed update formula of the particle swarm algorithm: when updating the speed, the algorithm will randomly select a particle randi from the population, and then use the individual historical optimal value of randi as the third learning object. Guide the search for the next generation of the population to prevent the algorithm from falling into the local optimal solution prematurely.

Step S2.5: Make a judgment based on the number of iterations and the iteration threshold, and adaptively adjust and update the learning factors. If the number of iterations k < iteration threshold k _th , then the update formulas of learning factors c ₁ , c ₂ and c ₃ are as follows:

If k ≥ k _th , the update formulas of learning factors c ₁ , c ₂ and c ₃ are as follows:

Among them, BG is the global historical optimal value of the population, BP _i and BP _randi represent the individual historical optimal values of the i-th particle and random particle randi respectively.

It can be seen from equations (12) and (13) that random learning guidance will be activated in the early stage and adjusted to a dormant state in the later stage, which can enable the algorithm to fully explore the entire search area by the population in the early stage of search and avoid prematurely falling into the individual history. In the local solution dominated by the optimal value and the global historical optimal value; in the later stage of the search, the algorithm quickly converges to the global optimal value. In addition, the learning factor will be adaptively adjusted according to the objective function value of the learning object, that is, individuals with good fitness will have higher leadership in search, ensuring that individuals move to areas with greater potential on the basis of increasing population diversity.

In addition, when the number of iterations exceeds the iteration threshold k _th , the learning object only has the global historical optimal value and the individual historical optimal value, and the global historical optimal value is the optimal position currently searched and is the reference object for all particles. Therefore, the quality of the global historical optimal value has a great impact on the accuracy of the final solution. Appropriate perturbation of the global historical optimal value in the later stage of the search can explore potential areas more efficiently. This embodiment uses Levy flight and Cauchy mutation for the global historical optimal value respectively and then retains the better value. The formula is as follows:

BG(k)＝{X|max g(X),X∈{BG(k),New ₁ ,New ₂ }} (15)

Among them, λ is the step size of Levy flight, x ₀ is the position parameter of Cauchy variation, γ is the scale parameter of Cauchy variation, and BG ⁱ represents the i-th dimensional component of BG.

Step S2.6, update the position of each particle in the next generation particle swarm, the formula is as follows:

P _i,j (k)=P _i,j (k-1)+V _i,j (k) (16)

Step S2.7: Calculate the virtual repulsion between sensor nodes. The calculation formula is as follows:

Among them, μ _r is the repulsion coefficient, d _th is the distance threshold, d _ij is the Euclidean distance between sensor nodes N _i and N _j , and θ _ij represents the angle between the vector N _i pointing to N _j and the positive direction of the x-axis.

According to the virtual repulsion between sensor nodes, the position of each particle in the particle swarm guided by the virtual repulsion is updated. Among them, the resultant force F _i of node N _i subjected to the repulsion of all sensor nodes in area E is shown in Equation (18), and the step length of N _i moving in the horizontal and vertical coordinate directions due to the influence of the virtual repulsion is shown in Equation (19):

Among them, Δx _i and Δy _i represent the step length of Ni _'s movement in the x-axis and y-axis directions respectively, F _ix and F _iy refer to the components of the resultant force F _i in the x-axis and y-axis directions, and step is the maximum moving step length.

The above method introduces the concept of virtual repulsion in the virtual force algorithm to avoid too dense distribution of sensor nodes in Wireless Sensor Networks (WSN). Among them, the sensor is abstracted as a charged particle. When the distance between nodes is less than a threshold, a repulsive force is generated, and the distance between sensor nodes will tend to expand outward to avoid centralized distribution.

Step S2.8: Determine whether the maximum number of iterations has been reached. If satisfied, the iterations stop and the sensor deployment plan optimization is completed; otherwise, return to step S2.3 to perform the next iteration calculation.

Through the above scheme, the global optimal value of the population and the individual fitness value are output, and the deployment scheme of multiple sensors in the target space is obtained.

In step S3, multiple sensors are deployed in the target space according to the deployment plan obtained in step S2. Sensor deployment optimization can effectively improve the UWB system's shortcomings of increased system positioning errors due to static obstacles blocking signals. However, when UWB is used as the only positioning method for positioning, due to the influence of environmental factors (unknown dynamic obstacles, etc.), the positioning results still have a certain degree of volatility, and the working performance cannot meet the needs of actual indoor positioning. In order to reduce positioning errors and increase the positioning stability of the system, this embodiment also uses error Kalman filtering to fuse UWB positioning data and inertial navigation positioning data. Among them, the inertial navigation positioning data is the data obtained through the inertial measurement unit. The inertial measurement unit (IMU) is a device that measures the three-axis attitude angle (or angular rate) and acceleration of the object. The inertia of the target to be measured is obtained through the IMU. The measured data, including acceleration and angular velocity, are then solved through inertial navigation to obtain the position, speed and attitude estimates of the target to be measured, and these data are used to predict the state of the target to be measured. That is, the data obtained by the inertial measurement unit are used to calculate the nominal state and prediction error state of the target to be measured, the UWB position measurement information (i.e., UWB positioning data) is used as the observation value to correct the prediction error state, and the nominal state is combined to calculate the target to be measured The real state is obtained to obtain the final fusion positioning result. The process of this fusion positioning is shown in Figure 4.

Step S3.1. Obtain the UWB positioning data of the target to be measured, and obtain the inertial measurement data of the target to be measured (including the acceleration and angular velocity of the target to be measured) through the inertial measurement unit. Based on the inertial measurement data, the inertial navigation solution is used to obtain the target to be measured. Inertial navigation positioning data of the target, including estimated values such as position, speed, attitude, etc. Further, the inertial navigation positioning data of the target to be measured is used as the nominal state of the target to be measured.

Step S3.2: Predict the error state based on the inertial navigation positioning data of the target to be measured, and obtain the predicted error state. Specifically, the error state is the state quantity of the actual operation of the fusion positioning system, recorded as is an error state. Among them, δp ^T is the position error, δv ^T is the velocity error, δθ ^T is the attitude error,/> is the gyroscope drift deviation,/> is the accelerometer drift deviation (gyro offset deviation and accelerometer offset deviation are the deviations of the inertial measurement unit/device itself during measurement), then the differential equation of the system error state is:

Among them, C _b ⁿ is the coordinate system transformation matrix, μ _ω and μ _a are time constants, n _b and n _ω represent acceleration measurement noise and angular velocity measurement noise respectively, γ _ω and γ _a represent white noise interference with a mean value of 0, [ f×] is defined as follows:

In order to facilitate subsequent analysis and calculation, Kalman filtering is used to transform the state equation (20) of the error state into the following form:

Among them, F _k is the state transition matrix at time k, G _k is the noise gain matrix at time k, and n _k is the noise matrix at time k. Their definitions are:

n _k =[n _ω ,n _a ,γ _ω ,γ _a ] ^T (25)

In two continuous time intervals Δt of the discrete system, the error state prediction equation based on IMU data is:

In order to simplify the calculation formula, the above error state prediction equation is transformed into Equation (27), and its covariance prediction method is as shown in Equation (28):

δX _k|k-1 ＝A _k|k-1 δX _k-1|k-1 +G _k n _k (27)

Among them, P _k-1|k-1 is the posterior estimated covariance at time k-1, Q is the covariance of n _k , and A _k is the estimate of the error state transition matrix of the inertial measurement unit at time k. The calculation formula is as follows:

A _k|k-1 ≈I _15×15 +F _k Δt (29)

Step S3.3: Use Kalman filter to update the prediction error state according to the obtained UWB positioning data of the target to be measured. The error state update and covariance matrix calculation formulas on both sides are as follows:

P _k|k = (IK _k H _k )P _k|k-1 (31)

Among them, δX _k|k is the updated error state at time k, δX _k|k-1 is the a priori estimate of the error state at time k, and p _k ^UWB are the target position coordinates to be measured measured by the inertial navigation system and UWB system at time k respectively, H _k is the transfer matrix from state quantity to quantity measurement, defined as H _k = [I _3×3 0 _3×3 0 _{3 ×3} 0 _3×3 0 _3×3 ], K _k is the Kalman gain at time k, and the calculation formula is as follows:

Step S3.4. In the Error-State Kalman Filter (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. Therefore, based on the updated prediction error state and nominal state , perform state merging to obtain the fusion positioning result of the target to be measured, that is, the true value state X _k|k of the target to be measured is obtained. The calculation formula is as follows:

X _k|k ＝X _k|k-1 +δX _k|k (33)

Among them, X _k|k-1 is the nominal state at time k, and δX _k|k is the prediction error state updated by ESKF.

Step S3.5: Output the true value state X _k|k of the target to be measured, that is, output the target position of the target to be measured measured by the fusion positioning system.

In order to further verify the superiority of the above solution in this embodiment, the following examples are used to verify. First, in order to verify the performance of the improved particle swarm optimization algorithm, the algorithm proposed in this embodiment is compared with the original PSO, VFA and HGWOP. In order to avoid accidental errors, all result data are the average of 30 independent experiments. The relevant parameters of the experiment are shown in Table 2 below. In order to avoid different random initialization schemes affecting the results of the comparative experiments, all algorithms have the same initial solution. Figure 5 shows the coverage effect of the initial deployment network. The initial coverage rate is 67.16%. The value of the bar on the right is the detection probability. , the higher the color temperature, the greater the detection probability of the sensor network in the area, that is, the higher the positioning accuracy. In addition, this experiment deployed different numbers of sensors in a 100×100 area to test the versatility of the algorithm. The results are shown in Figure 6. The results show that the coverage of all algorithms gradually increases to 1 as the number of sensor nodes increases, and the proposed IPSO-VF is the first to achieve complete coverage of the entire detection area when n=35. Therefore, in the optimization of sensor deployment at different densities, IPSO-VF can utilize sensors more efficiently and achieve coverage of the target area.

Table 2 Experimental parameter settings

Figure 7 shows the final coverage effect of the sensor network after optimization and solution using four algorithms when the number of sensors is 25. As can be seen from Figure 5, the sensors in the randomly initially deployed sensor network are unevenly distributed, with a coverage rate of only 67.16% and a large number of coverage holes. Figure 7 shows the sensor network optimized by the algorithms IPSO-VF and VFA proposed in this embodiment. It has better coverage effect on the target detection area, the sensors are more evenly distributed and there are fewer repeated coverage areas. The bar on the far right shows that the sensor network deployed by IPSO-VFA has the lowest detection probability of the target detection area at around 0.65, VFA at around 0.5, PSO and HGWOP at 0, which means that compared to other comparison algorithms IPSO -VF achieves higher-precision coverage of the entire target area. At the same time, the proposed IPSO-VF algorithm achieved the optimal coverage effect, with a coverage rate of 99.6%. The final coverage rate was 32.44% higher than the initial coverage rate, which was 11.36%, 4.24% and 11.36% higher than PSO, VFA and HGWOP respectively. 5.36%.

The above experiments show that compared with the comparative algorithm, the IPSO-VF algorithm proposed in this embodiment can obtain a better sensor deployment solution, and the algorithm optimization performance is significantly improved compared with the original PSO. In addition, thanks to the introduction of the virtual force guidance strategy, IPSO-VF can use the virtual repulsion between sensors to optimize the distribution of sensors, reduce repeated coverage of sensors, and achieve efficient use of sensors.

In addition, in order to verify the positioning accuracy and stability of the UWB positioning network and inertial navigation fusion positioning system based on the error state Kalman filter, the Ackerman robot was used as a motion platform to carry out fusion positioning experiments, and IPSO-VF was used to obtain UWB sensor deployment plan. The platform uses STM32F405 as the host computer control board and Raspberry Pi as the host computer processor. During the experiment, the robot platform moved along a circular trajectory with a center of (4m, 4.7m) and a radius of 1.57m.

In order to test the positioning accuracy improvement effect of the fusion positioning method, the theoretical trajectory of the motion platform, the pure UWB solution trajectory and the solution trajectory of the fusion positioning method were drawn respectively. The results are shown in Figure 8. Figure 8 (a) shows the overall situation of the three trajectories, and Figure 8 (b) shows the local details of the trajectories. It can be seen from Figure 8(a) that the overall trajectory measured by single UWB positioning and fusion positioning converges to a circular trajectory, but the IMU+UWB fusion trajectory and the real trajectory have more overlapping sections and a smaller gap, that is, the fusion positioning result It is more in line with the actual trajectory of the robot platform; Figure 8(b) shows the local details of the trajectory, in which the trajectory points measured by the single UWB method fluctuate frequently and violently, and are significantly different from the real trajectory, while the fused positioning trajectory is smoother and It is closer to the actual motion trajectory, indicating that the UWB positioning network and inertial navigation fusion positioning method based on ESKF has higher positioning stability and accuracy than a single positioning system.

Embodiment 2

This embodiment provides an indoor positioning system based on inertial navigation and UWB sensor network, including:

The sensor perception model building module is used to obtain the distance value and the real distance value measured by the sensor, determine the sensing radius and reliability parameters of the sensor, and then construct the sensor perception model of the sensor;

The sensor deployment solution module is used to solve the sensor perception model based on multiple sensors in the target space. It uses the coverage rate as the optimization goal and uses the improved particle swarm optimization algorithm to optimize and solve to obtain the deployment plan of multiple sensors in the target space;

The fusion positioning result acquisition module is used to deploy multiple sensors in the target space according to the deployment plan, obtain the UWB positioning data of the target to be measured, and obtain the inertial navigation positioning data of the target to be measured through inertial measurement, and use error Kalman filtering to perform UWB The positioning data and inertial navigation positioning data are fused to obtain the fusion positioning result of the target to be measured.

Embodiment 3

This embodiment provides an electronic device, including a memory, a processor, and computer instructions stored in the memory and run on the processor. When the computer instructions are run by the processor, the above-mentioned inertial navigation and UWB-based navigation are completed. Steps in indoor localization methods for sensor networks.

Embodiment 4

This embodiment also provides a computer-readable storage medium for storing computer instructions. When the computer instructions are executed by a processor, the above-mentioned steps in the indoor positioning method based on inertial navigation and UWB sensor networks are completed.

Each step involved in the above Embodiments 2 to 4 corresponds to the method Embodiment 1. For specific implementation details, please refer to the relevant description of Embodiment 1. The term "computer-readable storage medium" shall be understood to include a single medium or multiple media that includes one or more sets of instructions; and shall also be understood to include any medium capable of storing, encoding, or carrying instructions for use by a processor. The executed instruction set causes the processor to perform any method in the present invention.

Those skilled in the art should understand that each module or each step of the present invention described above can be implemented by a general-purpose computer device. Alternatively, they can be implemented by program codes executable by the computing device, so that they can be stored in a storage device. The device is executed by a computing device, or they are respectively made into individual integrated circuit modules, or multiple modules or steps among them are made into a single integrated circuit module. The invention is not limited to any specific combination of hardware and software.

The above descriptions are only preferred embodiments of the present invention and are not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and changes. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention shall be included in the protection scope of the present invention.

Although the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, they do not limit the scope of the present invention. Those skilled in the art should understand that based on the technical solutions of the present invention, those skilled in the art do not need to perform creative work. Various modifications or variations that can be made are still within the protection scope 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.