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

Abstract

This invention discloses a positioning method and system based on inertial navigation and UWB wireless sensor networks. The method includes: acquiring distance values ​​measured by sensors and actual distance values ​​to construct a sensor perception model; optimizing the sensor perception model based on multiple sensors in the target space, using coverage rate as the objective function, and solving the optimization problem using an improved particle swarm optimization algorithm to obtain a deployment scheme for multiple sensors in the target space; acquiring UWB positioning data of the target and inertial navigation positioning data of the target through inertial measurement; and fusing the UWB positioning data and inertial navigation positioning data using error Kalman filtering to obtain a fused positioning result. This invention utilizes an improved particle swarm optimization algorithm to obtain a UWB sensor deployment scheme and uses error Kalman filtering to fuse the position data of the UWB positioning network and the position data calculated by the inertial navigation system, achieving high-precision and high-stability positioning in enclosed environments.

Description

Technical Field

[0001] This invention relates to the field of indoor positioning technology, and in particular to an indoor positioning method and system based on inertial navigation and UWB sensor networks. Background Technology

[0002] Positioning technology is used to obtain the location information of targets (people, equipment, or other objects) in an environment, and different situations 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. Because satellite signals can propagate unimpeded in open outdoor spaces, these positioning systems can provide high-quality positioning services outdoors. However, in indoor environments, due to obstruction by walls and obstacles, satellite signals will be interfered with, making accurate positioning impossible. Therefore, GPS, BDS, etc., are not suitable for positioning in enclosed environments. Furthermore, compared to outdoor positioning, enclosed spaces are more prone to multipath effects and delays due to signal obstruction or reflection from walls, people, and equipment, thus increasing the difficulty of positioning. Other wireless signal interference sources indoors may also affect the signal propagation of positioning sensors. With the continuous expansion of mobile robots in daily life and production, the demand for indoor positioning technology is gradually increasing.

[0003] In recent years, with the popularization of wireless positioning technology, Ultra Wide Band (UWB) positioning schemes have received widespread attention and are used to solve the problem of inaccurate positioning in indoor environments due to interference from walls and obstacles. 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 capability, security, and transmission rate, and is relatively inexpensive. UWB was initially used primarily in military fields such as radar search and secure wireless communication, but in recent years it has demonstrated its superiority in indoor positioning. However, in actual positioning, although the hardware of UWB devices can improve positioning accuracy to a certain extent, obstacles in the indoor environment can still reduce the system's positioning accuracy. Furthermore, when using UWB as the sole positioning method, the positioning results still exhibit some fluctuation and low stability due to environmental factors (such as unknown dynamic obstacles), and its performance cannot meet the needs of actual indoor positioning. In other words, the reliability and stability of indoor positioning using UWB as a single positioning scheme are still not high, and it cannot cope with complex and ever-changing real-world environments.

[0004] A well-designed sensor deployment scheme can reduce the deployment cost of UWB positioning networks and, to some extent, address the problem of wireless signal obstruction by indoor obstacles, thereby improving the service quality and positioning accuracy of sensor networks. Wireless Sensor Network (WSN) deployment optimization methods can be categorized into three types: algorithms based on virtual forces, algorithms based on computational geometry, and algorithms based on intelligent optimization. Since the quality of solutions to sensor deployment optimization problems is closely related to the performance of the optimization algorithm and the optimization problem model, designing a reasonable problem model and improving the optimization algorithm are key challenges in improving the quality of positioning systems.

[0005] Furthermore, existing technologies have proposed various schemes to improve the stability of positioning networks to a certain extent through fusion positioning. For example, a compact combination positioning method based on UWB and inertial measurement units (IMU) is proposed. First, a least-squares support vector machine model is trained using the actual and measured distances between the robot and each UWB base station. Then, this model is used to correct the measured distances of each UWB base station during the robot's movement to reduce the impact of non-line-of-sight errors on the final positioning accuracy. Finally, Kalman filtering based on error states is used to combine the corrected distance measurements with the distance information calculated by the inertial navigation system. This combined positioning system improves the positioning accuracy of robots in mining environments. An improved particle filtering method is also proposed and applied to the fusion positioning of UWB and inertial navigation systems. This method first introduces the minimum variance estimation theory of the adaptive optimal weighted fusion algorithm. The particle distribution weights are adjusted, and a threshold is set to limit the observation variance to avoid divergence. Finally, the optimal weighting factor for each sensor is obtained using the root mean square error after particle filtering, effectively improving the positioning accuracy of vehicle navigation. A fusion positioning method based on unscented Kalman filtering for underground coal mining machine positioning is proposed. Using UWB system data as observations, an inertial navigation and UWB fusion positioning model is established, and the results are smoothed using the VB-UKF adaptive filtering algorithm, achieving real-time compensation for inertial navigation measurement errors. Addressing the issue of reduced UWB positioning accuracy in non-line-of-sight environments, a fusion positioning method based on extended Kalman filtering is proposed. Taking advantage of the fact that inertial measurement units are less affected by obstacles in non-line-of-sight environments, it is combined with UWB to overcome the limitations of single positioning technologies. Therefore, to avoid the poor reliability and stability of single positioning schemes, how to achieve fusion positioning is another key and challenging issue in improving indoor positioning accuracy. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention provides an indoor positioning method and system based on inertial navigation and UWB sensor networks. This method is suitable for positioning in enclosed spaces. It utilizes an improved particle swarm optimization algorithm to obtain a reasonable UWB sensor deployment scheme. By pre-deploying these sensors, the impact of known static obstacles on the system's positioning accuracy is reduced, further improving the accuracy and stability of the positioning network. Then, error Kalman filtering is used to fuse the position data from the UWB positioning network and the position data calculated by the inertial navigation system, solving the problem of insufficient positioning accuracy and stability of single sensors and achieving high-precision, high-stability positioning in enclosed environments.

[0007] In one aspect, this disclosure provides an indoor positioning method based on inertial navigation and UWB sensor networks.

[0008] An indoor positioning method based on inertial navigation and UWB sensor networks includes:

[0009] The sensor obtains the distance value measured by the sensor and the actual distance value, determines the sensor's sensing radius and reliability parameters, and then constructs the sensor sensing model of the sensor.

[0010] Based on a sensor perception model of multiple sensors in the target space, with coverage as the optimization objective, an improved particle swarm optimization algorithm is used to optimize and solve the problem, thereby obtaining a deployment scheme for multiple sensors in the target space.

[0011] According to the deployment plan, multiple sensors are deployed in the target space to acquire UWB positioning data of the target under test, and inertial navigation positioning data of the target under test is acquired through inertial measurement. Error Kalman filtering is used to fuse the UWB positioning data and inertial navigation positioning data to obtain the fused positioning result of the target under test.

[0012] Secondly, this disclosure provides an indoor positioning system based on inertial navigation and UWB sensor networks.

[0013] An indoor positioning system based on inertial navigation and UWB sensor networks includes:

[0014] The sensor perception model construction module is used to obtain the distance values ​​measured by the sensor and the actual distance values, determine the sensor's sensing radius and reliability parameters, and then construct the sensor perception model of the sensor.

[0015] The sensor deployment scheme solution module is used to optimize the sensor perception model of multiple sensors in the target space with coverage as the optimization objective. It uses an improved particle swarm optimization algorithm to solve the optimization problem and obtain the deployment scheme of multiple sensors in the target space.

[0016] The fusion positioning result acquisition module is used to deploy multiple sensors in the target space according to the deployment plan, acquire UWB positioning data of the target under test, acquire inertial navigation positioning data of the target under test through inertial measurement, and fuse the UWB positioning data and inertial navigation positioning data using error Kalman filtering to obtain the fusion positioning result of the target under test.

[0017] Thirdly, this disclosure also provides an electronic device, including a memory and a processor, and computer instructions stored in the memory and running on the processor, wherein the computer instructions, when executed by the processor, perform the steps of the method described in the first aspect.

[0018] Fourthly, this disclosure also provides a computer-readable storage medium for storing computer instructions, which, when executed by a processor, perform the steps of the method described in the first aspect.

[0019] The above one or more technical solutions have the following beneficial effects:

[0020] 1. This invention provides an indoor positioning method and system based on inertial navigation and UWB sensor networks, which is suitable for positioning in enclosed spaces. It optimizes the deployment of wireless sensor networks to reduce the impact of known static obstacles on wireless signal obstruction and thus on the positioning accuracy of the system, improves the service quality and positioning accuracy of the sensor network, and reduces the deployment cost of the positioning sensor network.

[0021] 2. This invention utilizes error Kalman filtering to fuse position data from a UWB positioning network and position data calculated by an inertial navigation system, thereby solving the problem of insufficient positioning accuracy and stability of a single sensor and achieving high-precision and high-stability positioning in enclosed environments. Attached Figure Description

[0022] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0023] Figure 1 This is a schematic diagram of the statistical sensing model in an embodiment of the present invention;

[0024] Figure 2 This is a schematic diagram illustrating the relationship between the measurement accuracy of the UWB sensor and the distance in an embodiment of the present invention;

[0025] Figure 3 This is a flowchart illustrating the optimization solution using the improved particle swarm optimization algorithm IPSO-VF in an embodiment of the present invention.

[0026] Figure 4 This is a flowchart illustrating the fusion positioning process in an embodiment of the present invention;

[0027] Figure 5 This is a coverage effect diagram of the initial random deployment of UWB sensors in an embodiment of the present invention;

[0028] Figure 6 This is a schematic diagram showing the final coverage results of four different algorithms under different sensor densities in an embodiment of the present invention;

[0029] Figure 7 This is a diagram showing the coverage effect after deploying UWB sensors using four different algorithms in an embodiment of the present invention.

[0030] Figure 8 This is a schematic diagram of the experimental trajectory for fusion positioning in an embodiment of the present invention. Detailed Implementation

[0031] It should be noted that the following detailed descriptions are exemplary and intended to provide further illustration of the invention. Unless otherwise specified, 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 pertains.

[0032] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of exemplary embodiments according to the invention. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0033] Example 1

[0034] This embodiment provides an indoor positioning method based on inertial navigation and UWB sensor networks. An improved particle swarm optimization algorithm is used to obtain a reasonable UWB wireless sensor deployment scheme. By pre-optimizing the deployment of the wireless sensor network, the impact of known static obstacles on wireless signal obstruction and thus on the system's positioning accuracy is reduced, improving the sensor network service quality and positioning accuracy, and lowering the deployment cost of the positioning sensor network. Error Kalman filtering is used to fuse the position data from the UWB positioning network and the position data calculated by the inertial navigation system, addressing the problem of insufficient positioning accuracy and stability of single sensors, and achieving high-precision, high-stability positioning in enclosed environments (indoors).

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

[0036] Step S1: Obtain the distance value measured by the sensor and the actual distance value, determine the sensor's sensing radius and reliability parameters, and then construct the sensor sensing model of the sensor.

[0037] Step S2: Based on the sensor perception models of multiple sensors in the target space, with the coverage rate as the optimization objective, use the improved particle swarm optimization algorithm for optimization and solution to obtain the deployment plan of multiple sensors in the target space;

[0038] 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, and obtain the inertial navigation positioning data of the target to be measured through inertial measurement. Use the error Kalman filter to fuse the UWB positioning data and the inertial navigation positioning data to obtain the fused positioning result of the target to be measured.

[0039] The implementation of the above indoor positioning method based on inertial navigation and UWB sensor network in this embodiment is further introduced through the following content.

[0040] In step S1, first, considering that in the real environment, the perception ability of sensors is affected by other nodes, and there is a non-linear relationship between the accuracy of data collection and the distance. Therefore, this embodiment uses a more practical statistical perception model as the sensor perception model of the sensors, as Figure 1 shown. Among them, R is the perception radius of the sensor, and r is the measurement reliability parameter (r < R). In this model, the probability calculation formula for the target point T to be detected by the sensor N is:

[0041]

[0042] β1 = r - R + d N,T (2)

[0043] β2 = r + R - d N,T (3)

[0044] where d N,T is the Euclidean distance between the sensor N and the target point T, R is the perception radius of the sensor, r is the measurement reliability parameter, and α1, α2, δ1, and δ2 are measurement parameters.

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

[0046]

[0047] where dis m and dis r are the distance values measured by the UWB sensor and the true distance value (unit: centimeter) respectively.

[0048] Specifically, the sensing radius and reliability parameters of the sensor are calculated and determined through the following method. Preset a positioning base station and a positioning tag, adjust the distance between the two, measure and record the actual measured distance, and the test range is 0m to 7.2m. In addition, to avoid the contingency of the results, multiple (20 rounds in this embodiment) independent tests are carried out in total, and the average value of the experimental results is as Figure 2 shown in Table 1 below.

[0049] Table 1 Average Measurement Accuracy of UWB Sensors

[0050]

[0051] According to Table 1 above, set the sensor sensing radius R to 2.5 and the reliability parameter r to 0.5. According to the definition of the statistical sensing model, when the distance d between the measured point and the base station sensor satisfies d < R - r, the measured point 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 < d < R + r, the accuracy of the measured point sensed by the base station sensor decreases with the increase of the distance, corresponding to the uncertain sensing area in the model; when d > R + r, the accuracy of the measured point sensed by the base station sensor is close to 0, corresponding to the non-sensing area in the model.

[0052] Through the above scheme, the sensing radius and reliability parameters of the sensor are determined, and then the sensor sensing model of the sensor is constructed.

[0053] In step S2, in order to enable the improved particle swarm optimization algorithm to more efficiently solve the deployment scheme of the optimal sensor positioning network and thus improve the positioning accuracy of the sensor network for the target environment, the coverage rate is used as the optimization goal in this embodiment. Assume that the target monitoring area E is a square area of L×W, and n sensor nodes are randomly distributed in E, denoted as N = {N1, N2,..., N n}, and N i is the position coordinate of the i-th sensor node, denoted as N i = (x1, y1). For the convenience of subsequent calculations, the target monitoring area E is divided into l×w = e measured target pixel points, denoted as T = {T1, T2,..., 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 measured target pixel points covered by the sensor network to the total number of measured target pixel points, and the calculation formula is as follows:

[0054]

[0055] Among them, P cov (N, T i ) is the wireless sensor network N for the i-th measured target point T iThe joint detection probability is calculated as shown in formulas (6) and (7):

[0056]

[0057]

[0058] Where p(N,T) j For all sensor nodes N pairs T within region E j The joint detection probability, p(N) i ,T j ) represents sensor node N i For the detection target point T j The detection probability is calculated based on the sensor perception model determined in step S1 above, and the calculation method is shown in formulas (1)-(3); p th As the probability threshold, when p(N,T) j If the value is greater than the threshold, then the target pixel T is determined to be the target pixel point. j It can be effectively detected by a wireless sensor network N.

[0059] Subsequently, an improved particle swarm optimization algorithm is used to optimize the solution and obtain a deployment scheme for multiple sensors in the target space. In this embodiment, the improved particle swarm optimization algorithm is denoted as IPSO-VF, and the flowchart of IPSO-VF is as follows. Figure 3 As shown.

[0060] Step S2.1: Initialize relevant parameters, i.e., set the initial parameters of the optimization algorithm, such as the number of pixels l and w in the target monitoring area, the number of sensors n, the number of iterations K, and the iteration threshold k. th Population size M, particle dimension 2n, search space range [P] min ,P max ], the range of particle velocity [V min V max ]

[0061] and the maximum and minimum inertia weights ω min and ω max The maximum value of the learning factor c max .

[0062] Step S2.2: Initialize the position and velocity of all particles using the following formula:

[0063] P i,j =P min +(P max -P min Rand (8)

[0064] V i,j =V min+(V max -V min Rand (9)

[0065] In the formula, P i,j and V i,j Let be the position component and velocity component of the i-th particle in the j-th dimension, respectively, and Rand be a random number that follows a standard uniform distribution of U(0,1).

[0066] Step S2.3: Calculate the fitness value of all individuals, update the individual historical best value BP and the global best value BG, dynamically update the strategy according to the S-shaped inertia weight and recalculate the inertia weight, as shown in the following formula:

[0067]

[0068] Where o is the adjustment factor, k is the current iteration number, and K is the maximum iteration number. This strategy allows the inertia weight ω to maintain a large value in the early stage of iteration, giving the algorithm a high global search capability. Subsequently, it decreases rapidly in the middle stage, and finally maintains a low value in the later stage of iteration, giving the algorithm a large local search capability, thus better balancing the algorithm's global and local search capabilities.

[0069] Step S2.4: Introduce a random learning term to update the particle velocity, using the following formula:

[0070]

[0071] Among them, V i,j (k) represents the j-th dimension velocity component of the i-th particle in the k-th generation, ω is the inertial weight, c1, c2, and c3 are the individual learning factor, social learning factor, and random learning factor, respectively; u1, u2, and u3 are random numbers uniformly distributed in [0,1]; BP rand,j The j-th component represents the individual historical best value of the random particle randi.

[0072] This embodiment introduces a random learning term based on the original velocity update formula of the particle swarm optimization algorithm: during velocity update, the algorithm randomly selects a particle randi from the population, and then uses the individual historical best value of randi as the third learning object to guide the search of the next generation of the population, preventing the algorithm from getting trapped in local optima too early.

[0073] Step S2.5: Based on the number of iterations and the iteration threshold, make a judgment and adaptively adjust and update the learning factor. If the number of iterations k < the iteration threshold k... th The update formulas for learning factors c1, c2, and c3 are as follows:

[0074]

[0075] If k≥kth The update formulas for learning factors c1, c2, and c3 are as follows:

[0076]

[0077] Where BG is the global historical best value of the population, and BP is the global historical best value of the population. i and BP randi represents the individual historical best value of the i-th particle and the random particle randi, respectively.

[0078] As shown in equations (12) and (13), the random learning guidance will be activated in the early stage and adjusted to a dormant state in the later stage. This allows the algorithm to fully explore the entire search area in the early stage of the search, avoiding premature entry into local solutions dominated by individual historical optima and global historical optima. In the later stage of the search, it enables the algorithm to quickly converge to the global optimum. In addition, the learning factor will be adaptively adjusted according to the objective function value of the learning object. That is, individuals with better fitness have higher leadership in the search, ensuring that individuals move to more promising areas while increasing population diversity.

[0079] Furthermore, when the number of iterations exceeds the iteration threshold k th The learning objects are limited to the global historical optimum and the individual historical optimum, with the global historical optimum being the currently found optimal position and serving as a reference for all particles. Therefore, the quality of the global historical optimum significantly impacts the accuracy of the final solution. Appropriate perturbation of the global historical optimum in the later stages of the search can more efficiently explore potential regions. In this embodiment, the global historical optimum is preserved after Lévy flight and Cauchy mutation, respectively, using the following formulas:

[0080]

[0081] BG(k)={X|max g(X),X∈{BG(k),New1,New2}} (15)

[0082] Where λ is the step size of the Lévy flight, x0 is the location parameter of the Cauchy mutation, γ is the scale parameter of the Cauchy mutation, and BG i Let i represent the i-th dimension component of BG.

[0083] Step S2.6: Update the positions of each particle in the next generation particle swarm, using the following formula:

[0084] P i,j (k)=P i,j (k-1)+V i,j (k) (16)

[0085] Step S2.7: Calculate the virtual repulsive force between sensor nodes. The calculation formula is as follows:

[0086]

[0087] Where, μ r It is the repulsion coefficient, d th It is the distance threshold, d ij It is sensor node N i With N j Euclidean distance, θ ij Represents N i Pointing to N j The angle between the vector and the positive x-axis.

[0088] The positions of each particle in the particle swarm guided by the virtual repulsion force are updated based on the virtual repulsion force between sensor nodes. Wherein, node N... i The resultant force F of the repulsive forces of all sensor nodes within region E i As shown in equation (18), N i The step size of the movement in the horizontal and vertical coordinate directions due to the influence of the virtual repulsive force is shown in equation (19):

[0089]

[0090]

[0091] Where, Δx i and Δy i Representing N respectively i The step size F in the x-axis and y-axis directions. ix and F iy The resultant force F i In the x-axis and y-axis components, step is the maximum movement step size.

[0092] The above method introduces the concept of virtual repulsion from the virtual force algorithm to avoid overly dense distribution of sensor nodes in Wireless Sensor Networks (WSNs). Here, sensors are abstracted as charged particles; when the distance between nodes is less than a threshold, a repulsive force is generated, causing the sensor nodes to tend to expand outwards to avoid concentrated distribution.

[0093] Step S2.8: Determine whether the maximum number of iterations has been reached. If it is, the iteration stops and the sensor deployment scheme optimization is completed; otherwise, return to step S2.3 and perform the next iteration calculation.

[0094] Using the above scheme, the global optimum value of the population and the fitness value of individuals are output, and the deployment scheme of multiple sensors in the target space is obtained.

[0095] In step S3, multiple sensors are deployed in the target space according to the deployment scheme obtained in step S2. Sensor deployment optimization can effectively improve the disadvantage of increased system positioning error caused by static obstacles blocking signals in the UWB system. However, when using UWB as the only positioning method, the positioning results still have certain fluctuations due to environmental factors (unknown dynamic obstacles, etc.), and the working performance cannot meet the actual indoor positioning needs. In order to reduce positioning error 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 by the inertial measurement unit. The inertial measurement unit (IMU) is a device that measures the three-axis attitude angles (or angular rates) and acceleration of an object. The inertial measurement data of the target to be measured is obtained through the IMU, including acceleration and angular velocity. Then, the position, velocity and attitude estimation of the target to be measured are obtained through inertial navigation calculation. The state prediction of the target to be measured is performed using these data. The process involves using data acquired by the inertial measurement unit (IMU) to calculate the nominal state and predicted error state of the target. UWB position measurement information (i.e., UWB positioning data) is then used as observations to correct the predicted error state. Finally, the nominal state is combined with the actual state of the target to obtain the final fused positioning result. The fused positioning process is as follows: Figure 4 As shown.

[0096] Step S3.1: Acquire the UWB positioning data of the target under test, and acquire the inertial measurement data (including the acceleration and angular velocity of the target under test) of the target under test through the inertial measurement unit. Based on the inertial measurement data, calculate the inertial navigation positioning data of the target under test through inertial navigation, including estimated values ​​of position, velocity, attitude, etc. Further, use the inertial navigation positioning data of the target under test as the nominal state of the target under test.

[0097] Step S3.2: Based on the inertial navigation positioning data of the target under test, predict the error state to obtain the predicted error state. Specifically, the error state is the state variable of the fused positioning system during actual operation, denoted as... This represents the error state. Where δp T It is the position error, δv T For velocity error, δθ T For attitude error, This is due to gyroscope drift bias. For accelerometer drift bias (gyroscope offset bias and accelerometer offset bias are inherent biases of the inertial measurement unit / device during measurement), the differential equation of the system error state is:

[0098]

[0099] Among them, Cb n Let μ be the coordinate system transformation matrix. ω and μ a n is the time constant. b and n ω These represent the noise from acceleration measurement and the noise from angular velocity measurement, respectively. ω and γ a [f×] represents white noise interference with a mean of 0, defined as follows:

[0100]

[0101] To facilitate subsequent analysis and calculation, the state equation (20) of the error state is transformed into the following form using Kalman filtering:

[0102]

[0103] Among them, F k Let G be the state transition matrix at time k. k Let n be the noise gain matrix at time k. k Let K be the noise matrix at time k, and its definitions are as follows:

[0104]

[0105]

[0106] n k =[n ω ,n a ,γ ω ,γ a ] T (25)

[0107] The error state prediction equation based on IMU data for two consecutive time intervals Δt in a discrete system is:

[0108]

[0109] To simplify the calculation formula, the above error state prediction equation is transformed into equation (27), and its covariance prediction method is shown in equation (28):

[0110] δX k|k-1 =A k|k-1 δX k-1|k-1 +G k n k (27)

[0111]

[0112] Among them, P k-1|k-1 Let Q be the posterior estimated covariance at time k-1, and let n be the variance.k covariance, A k The error state transition matrix of the inertial measurement unit at time k is estimated by the following formula:

[0113] A k|k-1 ≈I 15×15 +F k Δt (29)

[0114] Step S3.3: Based on the acquired UWB positioning data of the target, update the prediction error state using Kalman filtering. The formulas for updating both sides of the error state and calculating the covariance matrix are as follows:

[0115]

[0116] P k|k =(IK k H k )P k|k-1 (31)

[0117] Where, δX k|k It is the error state updated at time k, δX k|k-1 It is the prior estimate of the error state at time k. and p k UWB These are the position coordinates of the target measured by the inertial navigation system and the UWB system at time k, respectively. k It is the transition matrix from state variables to quantity measurements, defined as H. k =[I 3×3 0 3×3 0 3×3 0 3×3 0 3×3 ], K k The Kalman gain at time k is calculated using the following formula:

[0118]

[0119] Step S3.4: In the Error-State Kalman Filter (ESKF) model, the actual motion state of the target is equal to the sum of the nominal state and the error state. Therefore, based on the updated predicted error state and nominal state, state merging is performed to obtain the fused localization result of the target, that is, the true state X of the target. k|k The calculation formula is as follows:

[0120] X k|k =X k|k-1 +δX k|k (33)

[0121] Among them, Xk|k-1 Let δX be the nominal state at time k. k|k This represents the prediction error state updated by ESKF.

[0122] Step S3.5: Output the true state X of the target under test. k|k That is, the target position of the target to be measured is obtained by the fusion positioning system.

[0123] To further verify the superiority of the above-described scheme in this embodiment, the following example is used for verification. First, 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. To avoid random errors, all results are the average of 30 independent experiments, and the relevant experimental parameters are shown in Table 2 below. To avoid the influence of different random initialization schemes on the results of the comparative experiments, all algorithms have the same initial solution. Figure 5 This shows the coverage effect of the initial network deployment, with an initial coverage rate of 67.16%. The values ​​in the right-hand bar represent the detection probability; a higher color temperature indicates a higher detection probability from the sensor network in that area, meaning higher positioning accuracy. Furthermore, this experiment deployed different numbers of sensors within a 100×100 area to test the algorithm's versatility. The results are as follows... Figure 6 As shown in the figure, the coverage of all algorithms gradually increases to 1 with the increase of the number of sensor nodes, and the proposed IPSO-VF achieves complete coverage of the entire detection area first 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.

[0124] Table 2 Experimental parameter settings

[0125]

[0126] Figure 7 The image shows the final coverage effect of the sensor network after optimization using four different algorithms when the number of sensors is 25. Figure 5 It can be seen that the sensor distribution in the randomly initially deployed sensor network is uneven, with a coverage rate of only 67.16%, and there are a large number of coverage vulnerabilities. Figure 7The sensor network optimized by the proposed algorithms IPSO-VF and VFA in this embodiment demonstrates good coverage of the target detection area, with more uniform sensor distribution and fewer overlapping coverage areas. The rightmost bar indicates that the sensor network deployed with IPSO-VFA has a minimum detection probability of around 0.65 for the target detection area, VFA is around 0.5, and PSO and HGWOP are 0. This means that compared to other comparative algorithms, IPSO-VF achieves higher precision coverage of the entire target area. Furthermore, the proposed IPSO-VF algorithm achieves the best coverage performance, with a coverage rate of 99.6%, a 32.44% improvement over the initial coverage rate, and 11.36%, 4.24%, and 5.36% higher than PSO, VFA, and HGWOP, respectively.

[0127] The experiments above demonstrate that, compared to the comparative algorithms, the IPSO-VF algorithm proposed in this embodiment achieves a superior sensor deployment scheme, and its optimization performance is significantly improved compared to the original PSO. Furthermore, thanks to the introduction of the virtual force guidance strategy, IPSO-VF can utilize the virtual repulsion forces between sensors to optimize sensor distribution, reduce redundant sensor coverage, and achieve efficient sensor utilization.

[0128] Furthermore, to verify the positioning accuracy and stability of the fusion positioning system based on the error-state Kalman filter and the inertial navigation, an Ackerman robot was used as the motion platform to conduct a fusion positioning experiment, and the IPSO-VF was used to obtain the UWB sensor deployment scheme. The platform used an STM32F405 as the lower-level control board and a Raspberry Pi as the upper-level 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.

[0129] To verify the accuracy improvement effect of the fusion positioning method, the theoretical trajectory of the motion platform, the trajectory calculated by pure UWB, and the trajectory calculated by the fusion positioning method were plotted respectively. The results are as follows: Figure 8 As shown. Among them. Figure 8 Figure (a) shows the overall situation of the three trajectories. Figure 8 Image (b) shows local details of the trajectory. Figure 8 (a) It can be seen that the overall trajectory measured by single UWB positioning and fusion positioning both converge to a circular trajectory, but the overlap between the IMU+UWB fusion trajectory and the real trajectory is greater and the difference is smaller, that is, the fusion positioning result is more consistent 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 show frequent and violent fluctuations and have a large difference from the actual trajectory, while the fused positioning trajectory is smoother and closer to the actual motion trajectory, indicating that the ESKF-based UWB positioning network and inertial navigation fusion positioning method have higher positioning stability and accuracy than the single positioning system.

[0130] Example 2

[0131] This embodiment provides an indoor positioning system based on inertial navigation and UWB sensor networks, including:

[0132] The sensor perception model construction module is used to obtain the distance values ​​measured by the sensor and the actual distance values, determine the sensor's sensing radius and reliability parameters, and then construct the sensor perception model of the sensor.

[0133] The sensor deployment scheme solution module is used to optimize the sensor perception model of multiple sensors in the target space with coverage as the optimization objective. It uses an improved particle swarm optimization algorithm to solve the optimization problem and obtain the deployment scheme of multiple sensors in the target space.

[0134] The fusion positioning result acquisition module is used to deploy multiple sensors in the target space according to the deployment plan, acquire UWB positioning data of the target under test, acquire inertial navigation positioning data of the target under test through inertial measurement, and fuse the UWB positioning data and inertial navigation positioning data using error Kalman filtering to obtain the fusion positioning result of the target under test.

[0135] Example 3

[0136] This embodiment provides an electronic device, including a memory and a processor, as well as computer instructions stored in the memory and running on the processor. When the processor executes the computer instructions, it completes the steps in the indoor positioning method based on inertial navigation and UWB sensor networks as described above.

[0137] Example 4

[0138] This embodiment also provides a computer-readable storage medium for storing computer instructions, which, when executed by a processor, complete the steps in the indoor positioning method based on inertial navigation and UWB sensor networks as described above.

[0139] The steps and methods involved in Embodiments 2 to 4 above correspond to those in Embodiment 1. For specific implementation details, please refer to the relevant description section of Embodiment 1. The term "computer-readable storage medium" should be understood as a single medium or multiple media including one or more instruction sets; it should also be understood as including any medium capable of storing, encoding, or carrying an instruction set for execution by a processor and enabling the processor to perform any of the methods in this invention.

[0140] Those skilled in the art will understand that the modules or steps of the present invention described above can be implemented using general-purpose computer devices. Optionally, they can be implemented using computer-executable program code, thereby allowing them to be stored in a storage device for execution by a computer device, or they can be fabricated as separate integrated circuit modules, or multiple modules or steps can be fabricated as a single integrated circuit module. The present invention is not limited to any particular combination of hardware and software.

[0141] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

[0142] While the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. An indoor positioning method based on inertial navigation and UWB sensor networks, characterized in that, including: Obtain the distance value and the true distance value measured by the sensor, determine the sensing radius R and the reliability parameter r of the sensor, and then construct the sensor sensing model of the sensor; where, when the distance d between the measurement point and the sensor satisfies d < R - r, it corresponds to the absolute sensing area of the model; when R - r < d < R + r, it corresponds to the uncertain sensing area in the model; when d > R + r, it corresponds to the non-sensing area in the model; Based on the sensor sensing models of multiple sensors in the target space, with the coverage rate as the optimization objective, use the improved particle swarm optimization algorithm for optimization and solution to obtain the deployment plan of multiple sensors in the target space, including: Initialize the relevant parameters and the positions and velocities of all particles; Calculate the fitness values ​​of all individuals and update the historical best values ​​of each individual. BP and the global optimum of the population BG ,according to S The dynamic update strategy for inertia weights and the recalculation of inertia weights are as follows: in, o It is an adjustment factor. k It is the current iteration number. K It is the maximum number of iterations, ω max and ω min These are the maximum and minimum inertia weights; Introducing a random learning term to update particle velocity, a particle is randomly selected from the population. randi , to particles randi The individual's historical best value is used as a new learning object to guide the search of the next generation of the population, and the formula is: in, V i,j ( k ) is the first k The generation i The first particle j 3D velocity components, P i, j ( k- 1) is the first k- 1st generation i The first particle j 3D positional components, It is inertial weight. c 1. c 2 and c The three factors are individual learning factor, social learning factor, and random learning factor. u 1. u 2 and u 3 is a random number uniformly distributed in [0,1]. BP randi,j Represents random particles randi The first of the individual historical best values j Dimensional components; Judge according to the iteration number and the iteration threshold, and adaptively adjust and update the learning factors; Based on the updated particle velocities, update the positions of each particle in the next generation of particle swarm; Calculate the virtual repulsive force between sensor nodes, and update the positions of each particle in the particle swarm guided by the virtual repulsive force; Judge whether the maximum iteration number is reached. If satisfied, the iteration stops, output the global optimal value of the population and the individual fitness value, and obtain the deployment plan of multiple sensors in the target space; otherwise, perform iterative calculation repeatedly; Deploy multiple sensors in the target space according to the deployment plan, obtain the UWB positioning data of the measurement target, and obtain the inertial navigation positioning data of the measurement target through inertial measurement. Use the error Kalman filter to fuse the UWB positioning data and the inertial navigation positioning data to obtain the fused positioning result of the measurement target.

2. The indoor positioning method based on inertial navigation and UWB sensor networks as described in claim 1, characterized in that, The coverage rate is the ratio of the number of pixels of the measurement target covered by the sensor network to the total number of pixels of the measurement target.

3. The indoor positioning method based on inertial navigation and UWB sensor networks as described in claim 1, characterized in that, The obtaining the inertial navigation positioning data of the measurement target through inertial measurement includes: Obtain the inertial measurement data of the measurement target through an inertial measurement unit; the inertial measurement data includes acceleration and angular velocity; Obtain the inertial navigation positioning data of the measurement target through inertial navigation calculation according to the inertial measurement data; the inertial navigation positioning data includes position, velocity, and attitude estimation values.

4. The indoor positioning method based on inertial navigation and UWB sensor networks as described in claim 1, characterized in that, The using the error Kalman filter to fuse the UWB positioning data and the inertial navigation positioning data to obtain the fused positioning result of the measurement target includes: Take the inertial navigation positioning data of the measurement target as the nominal state of the measurement target, and based on the inertial navigation positioning data of the measurement target, predict the error state to obtain the predicted error state; According to the obtained UWB positioning data of the measurement target, use the Kalman filter to update the predicted error state; Based on the updated predicted error state and the nominal state, perform state merging to obtain the fused positioning result of the measurement target.

5. An indoor positioning system based on inertial navigation and UWB sensor networks, characterized in that, including: The sensor perception model construction module is used to obtain the distance value and the true distance value measured by the sensor, determine the perception radius R and the reliability parameter r of the sensor, and then construct the sensor perception model of the sensor; where, when the distance d between the measurement point and the base station sensor satisfies d < R - r, it corresponds to the absolute perception area of the model; when R - r < d < R + r, it corresponds to the uncertain perception area in the model; when d > R + r, it corresponds to the non-perception area in the model. The sensor deployment scheme solving module is used to optimize and solve based on the sensor perception models of multiple sensors in the target space, with the coverage rate as the optimization objective function, and use the improved particle swarm optimization algorithm to obtain the deployment scheme of multiple sensors in the target space, including: Initialize the relevant parameters and the positions and velocities of all particles; Calculate the fitness values ​​of all individuals and update the historical best values ​​of each individual. BP and the global optimum of the population BG ,according to S The dynamic update strategy for inertia weights and the recalculation of inertia weights are as follows: in, o It is an adjustment factor. k It is the current iteration number. K It is the maximum number of iterations, ω max and ω min These are the maximum and minimum inertia weights; Introducing a random learning term to update particle velocity, a particle is randomly selected from the population. randi , to particles randi The individual's historical best value is used as a new learning object to guide the search of the next generation of the population, and the formula is: in, V i,j ( k ) is the first k The generation i The first particle j 3D velocity components, P i, j ( k- 1) is the first k- 1st generation i The first particle j 3D positional components, It is inertial weight. c 1. c 2 and c The three factors are individual learning factor, social learning factor, and random learning factor. u 1. u 2 and u 3 is a random number uniformly distributed in [0,1]. BP randi,j Represents random particles randi The first of the individual historical best values j Dimensional components; Judge according to the iteration number and the iteration threshold, and adaptively adjust and update the learning factors; Update the positions of each particle in the next-generation particle swarm based on the updated particle velocities; Calculate the virtual repulsive force between sensor nodes, and update the positions of each particle in the particle swarm guided by the virtual repulsive force; Judge whether the maximum iteration number is reached. If so, stop the iteration, output the global optimal value of the population and the individual fitness value, and obtain the deployment scheme of multiple sensors in the target space; otherwise, perform iterative calculation repeatedly; The fusion positioning result acquisition module is used to deploy multiple sensors in the target space according to the deployment scheme, obtain the UWB positioning data of the measurement target, and obtain the inertial navigation positioning data of the measurement target through inertial measurement, and fuse the UWB positioning data and the inertial navigation positioning data by using error Kalman filtering to obtain the fusion positioning result of the measurement target.

6. The indoor positioning system based on inertial navigation and UWB sensor networks as described in claim 5, characterized in that, The coverage rate is the ratio of the number of pixels of the measurement target covered by the sensor network to the total number of pixels of the measurement target.

7. An electronic device, characterized in that, It includes a memory, a processor, and computer instructions stored on the memory and running on the processor. When the computer instructions are run by the processor, the steps of an indoor positioning method based on inertial navigation and UWB sensor network as described in any one of claims 1-4 are completed.

8. A computer-readable storage medium, characterized in that, For storing computer instructions, when the computer instructions are executed by the processor, the steps of an indoor positioning method based on inertial navigation and UWB sensor network as described in any one of claims 1-4 are completed.

Images