An INS / UWB indoor pedestrian integrated navigation method based on factor graph

Abstract

The present invention discloses an INS / UWB indoor pedestrian combined navigation method based on factor graph, and the steps are as follows: 1. Establishing the state equation of the INS / UWB indoor pedestrian combined navigation system; 2. Establishing the measurement equation; 3. Establishing the factor graph model; 4. Calculating the probability density function of the two-dimensional position variable and the speed variable of the pedestrian to be located after the time update in the first message transmission process in the factor graph according to 3; 5. Calculating the probability density function of the two-dimensional position variable after the measurement update of the pedestrian to be located in the first message transmission process in the factor graph according to 3; 6. Calculating the probability density function of the two-dimensional position variable and the speed variable after the measurement update of the pedestrian to be located in the second message iteration process in the factor graph according to 4 and 5; 7. Calculating the probability density function of the two-dimensional position variable and the speed variable of the pedestrian to be located in the factor graph according to 4 and 6. The present invention effectively solves the indoor pedestrian positioning and speed measurement problems under the condition that the number of UWB ranging information received by the pedestrian to be located in complex indoor application scenarios is uncertain.

Description

Technical Field

[0001] The invention belongs to the field of indoor pedestrian navigation, and in particular relates to an INS / UWB indoor pedestrian combined navigation method based on a factor graph. Background Art

[0002] As the Global Navigation Satellite System (GNSS) related technologies become increasingly mature, users' needs for location-based services in outdoor scenarios can be basically met. However, in indoor environments, it is difficult to obtain relatively accurate satellite navigation data, so GNSS cannot play its positioning function indoors.

[0003] In recent years, inertial navigation systems (INS) based on micro-electro-mechanical systems (MEMS) technology have played an increasingly important role in indoor navigation applications. However, considering that the error of low-precision inertial navigation systems alone accumulates rapidly over time, combining them with ultra-wideband (UWB) to construct an INS / UWB combined navigation system is a commonly used indoor navigation and positioning technology. However, in complex indoor application scenarios, due to environmental constraints, such as walls, objects and other obstacles, the number of effective base stations perceived by pedestrians to be located may be less than 3. In this case, the pedestrians to be located will not be able to obtain accurate UWB positioning information, but can only obtain ranging information between the UWB base station. In some extreme environments, the pedestrians to be located cannot even communicate with any base station, but can only obtain ranging information between certain pedestrians. In the above situation, how to use these uncertain UWB ranging information for effective data fusion to suppress the accumulation of inertial solution errors is the key to improving the indoor positioning performance of the INS / UWB integrated navigation system.

[0004] In published articles, such as the article "UWB Enhanced Integrated Navigation Method under GNSS Constrained Conditions" published by Jiang Wei of Beijing Jiaotong University in Volume 43, Issue 3 of the Journal of the China Railway Society in 2021, UWB original ranging information and INS are used for combined navigation, and centralized Kalman filtering is used to process multi-sensor data fusion. Although data fusion can be performed on multiple UWB ranging information at the same time, when the number of UWB ranging information changes, this method requires reconstruction of the measurement equation, and has poor flexibility. In the article "Research on UWB Enhanced Integrated Navigation Methods in GNSS Constrained Environments" published by Cao Zhuojian of Beijing Jiaotong University at the 12th China Satellite Navigation Annual Conference in 2021, a federal Kalman filtering method using UWB enhancement to assist the global navigation satellite system in a constrained environment was proposed, but this method also faces the problem of the need to reconstruct sub-filters when the number of sensors changes. Among the published patents, a patent entitled "Distributed INS / UWB Tightly Combined Navigation System Cluster Method" (patent application number: CN201710123870.1) published by Xu Yuan, Cheng Jin and others in 2017 introduced a method of fusing INS and UWB ranging information using a distributed data fusion algorithm. However, when the amount of UWB ranging information changes, this method also requires the reconstruction of the data fusion unit. Therefore, it is innovative to study an INS / UWB indoor pedestrian combined navigation method for complex indoor application scenarios with uncertain UWB ranging information. Summary of the invention

[0005] The object of the present invention is to provide an INS / UWB indoor pedestrian combined navigation method based on factor graph.

[0006] The purpose of the present invention is achieved through the following technical solutions:

[0007] Step 1: Establish the state equation of INS / UWB indoor pedestrian integrated navigation system;

[0008] Step 2: Establish the measurement equation of INS / UWB indoor pedestrian integrated navigation system;

[0009] Step 3: Establish the factor graph model of INS / UWB indoor pedestrian integrated navigation system;

[0010] Step 4: According to the system factor graph model established in step 3, the probability density function of the two-dimensional position variable and speed variable of the pedestrian to be located after the time update in the first message transmission process in the factor graph is calculated;

[0011] Step 5: According to the system factor graph model established in step 3, the probability density function of the two-dimensional position variable of the pedestrian to be located after the measurement is updated in the first message transmission process in the factor graph is calculated;

[0012] Step 6: According to Step 4 and Step 5, the probability density function of the updated two-dimensional position variable and speed variable of the pedestrian to be located in the factor graph during the second message iteration process is calculated;

[0013] Step 7: According to steps 4 and 6, the probability density function of the two-dimensional position variable and speed variable of the pedestrian to be located in the factor graph is calculated.

[0014] Furthermore, the state equation of the INS / UWB indoor pedestrian integrated navigation system in step 1 is:

[0015]

[0016]

[0017] v k,x =v k-1,x +a k-1,x t (3)

[0018] v k,y =v k-1,y +a k-1,y t (4)

[0019] Where: x k-1 ,y k-1 and x k ,y k are the x-axis and y-axis positions of the pedestrian to be located in the navigation coordinate system at time k-1 and time k respectively; v x,k-1 、v y,k-1 and v x,k 、v y,k are the x-axis and y-axis velocities of the pedestrian to be located in the navigation coordinate system at time k-1 and time k respectively; t is the sampling period. k-1,x and a k-1,y are the accelerations of the x-axis and y-axis of the navigation coordinate system at time k-1, which are obtained by projecting the specific force output of the accelerometer in the inertial navigation system in the horizontal direction, and their probability distributions are Among them, μ g and are the mean and variance of the variable g respectively;

[0020] In addition, the coupling relationship between the pedestrian position variable and the speed variable at time k is:

[0021]

[0022]

[0023] Furthermore, the measurement equation of the INS / UWB indoor pedestrian integrated navigation system in step 2 is:

[0024]

[0025] Where: is the UWB ranging information between the pedestrian to be located and the i-th (i=1, 2, …, N) base station or other pedestrians at time k, and N represents the number of valid UWB ranging information that the pedestrian to be located can obtain; and are the x-axis and y-axis positions of the navigation coordinate system of the i-th base station or other pedestrian at time k respectively; is the distance measurement noise, and its probability distribution is

[0026] Furthermore, the constraint relationship of each function node of the INS / UWB indoor pedestrian integrated navigation system established in step 3 is:

[0027] Function node f C ,f H ,f D ,f I The constraint functions are shown in equations (3)-(6), and the function nodes Describes relative position information With absolute position information (x k ,y k ) can be expressed as:

[0028]

[0029]

[0030] Similarly, function nodes Describe measurement information With relative position information The functional constraint relationship between them corresponds to the UWB measurement model:

[0031]

[0032] The remaining function nodes f A ,f B ,f J ,f K , Respectively represent the variable node v connected to it k-1,x ,x k-1 ,v k-1,y ,y k-1 , The probability density function of .

[0033] Furthermore, when the factor graph in step 4 performs the first message transmission, the average x-axis speed of the pedestrian to be located at time k after time update is Y-axis speed mean and their variances The average x-axis velocity of the pedestrian to be located can be calculated based on the k-1 moment Y-axis speed mean and their variances The mean x-axis acceleration obtained by horizontally projecting the accelerometer specific force output at time k-1 Y-axis acceleration mean and their variances Calculate, calculate The formula is:

[0034]

[0035]

[0036]

[0037]

[0038] The average x-axis position of the pedestrian to be located at time k after time update Mean y-axis position and their variances The mean x-axis position of the pedestrian to be located can be calculated based on the k-1 moment Mean y-axis position and their variances The average x-axis velocity of the pedestrian to be located calculated at time k-1 Y-axis speed mean and their variances The average x-axis speed of the pedestrian to be located at time k after time update Y-axis speed mean and their variances Calculate, calculate The formula is:

[0039]

[0040]

[0041]

[0042]

[0043] Furthermore, when the factor graph in step 5 performs the first message transmission, the average x-axis position of the pedestrian to be located at time k after the measurement is updated is Mean y-axis position and their variances The mean x-axis position of the ith base station or other pedestrian can be calculated based on time k Mean y-axis position and their variances The average x-axis position of the pedestrian to be located after time update calculated at time k Mean y-axis position and their variances The mean value of the UWB ranging information between the pedestrian to be located and the i-th base station or other pedestrians at time k and its variance Calculation; Since the system measurement equation is a nonlinear equation, in order to maintain the Gaussian characteristics of the variables, the nonlinear measurement equation needs to be Taylor expanded. In order to obtain higher accuracy, the second-order Taylor expansion is used to calculate The formula is:

[0044]

[0045]

[0046]

[0047]

[0048]

[0049] Furthermore, when the factor graph in step 6 performs the second message iteration, the average x-axis position of the pedestrian to be located at time k after the measurement is updated is Mean y-axis position and their variances The average x-axis position of the pedestrian to be located at time k after time update Mean y-axis position and their variances And the average x-axis position of the pedestrian to be located at time k after the updated measurement when the factor graph performs the first message transmission Mean y-axis position and their variances The mean x-axis position of the ith base station or other pedestrian calculated at time k Mean y-axis position and their variances The mean value of the UWB ranging information between the pedestrian to be located and the i-th base station or other pedestrians at time k and its variance Calculate, calculate The formula is:

[0050]

[0051]

[0052]

[0053]

[0054] Where: It can be calculated according to the following formula:

[0055]

[0056] The average x-axis speed of the pedestrian to be located at time k after measurement and update Y-axis speed mean and their variances The mean x-axis position of the pedestrian to be located can be calculated based on the k-1 time Mean y-axis position and their variances The average x-axis velocity of the pedestrian to be located calculated at time k-1 Y-axis speed mean and their variances The average x-axis position of the pedestrian to be located at time k after the updated measurement when the factor graph performs the second message iteration Mean y-axis position and their variances Calculate, calculate The formula is:

[0057]

[0058]

[0059]

[0060]

[0061] Where: It can be calculated according to the following formula:

[0062]

[0063] Furthermore, in step 7, the average x-axis position of the pedestrian to be located at time k is Mean y-axis position and their variances According to the x-axis position mean Mean y-axis position and their variances The average x-axis position of the pedestrian to be located at time k after the updated measurement when the factor graph performs the second message iteration Mean y-axis position and their variances Calculation, the calculation formula is as follows:

[0064]

[0065] The average x-axis velocity of the pedestrian to be located at time k Y-axis speed mean and their variances The average x-axis speed after the pedestrian to be located at time k is updated Y-axis speed mean and their variances The average x-axis speed of the pedestrian to be located at time k after the second message iteration of the factor graph Y-axis speed mean and their variances Calculate, calculate The formula is as follows:

[0066]

[0067] The beneficial effects of the present invention are:

[0068] The present invention aims at the problem of the uncertainty of the amount of UWB ranging information received by pedestrians to be located in complex indoor application scenarios, and provides an INS / UWB indoor pedestrian combined navigation method based on a factor graph. Since the factor graph model has the characteristic that the network topology structure can be dynamically changed, the INS / UWB combined navigation algorithm proposed in the present invention can integrate the UWB ranging information that is dynamically accessed and exited, and effectively solve the problem of pedestrian positioning and speed measurement in different indoor scenarios. In order to verify the beneficial effects of the present invention, a complex indoor scene is set up, and the INS / UWB indoor pedestrian combined navigation algorithm based on the factor graph in the present invention is simulated and compared with the extended Kalman filter (Extended Kalman Filter, EKF) algorithm and the particle filter (Particle Filter, PF) algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 The factor graph model of the INS / UWB indoor pedestrian integrated navigation system of the present invention;

[0070] Figure 2 A comparison diagram of pedestrian position errors based on the EKF, FG and PF algorithms in a complex indoor scene of the present invention;

[0071] Figure 3 This is a comparison diagram of pedestrian speed errors based on the EKF, FG and PF algorithms in a complex indoor scene of the present invention;

[0072] Figure 4 This is a comparison chart of the RMSE and operation time of pedestrian position and speed based on the EKF, FG and PF algorithms of the present invention. DETAILED DESCRIPTION

[0073] The present invention is further described below in conjunction with the accompanying drawings.

[0074] The present invention discloses an INS / UWB indoor pedestrian combined navigation method based on a factor graph. By setting a complex indoor scene, the INS / UWB indoor pedestrian combined navigation algorithm based on the factor graph in the present invention is simulated and compared with an extended Kalman filter (Extended Kalman Filter, EKF) algorithm and a particle filter (Particle Filter, PF) algorithm.

[0075] The simulation experiment was conducted on a computer with an Intel Core i5-11300H processor. The initial two-dimensional position of the pedestrian to be located was set to (0m, 0m), the initial speed was 1m / s, the initial heading angle was 45°, and the heading angle changed in a small sinusoidal manner. The sampling period was 0.1s, the sampling time was 60s, the gyro drift was set to 1° / h, and the accelerometer zero bias was set to 1×10 -3 g, UWB ranging noise is σ 2 =(0.1m) 2 , the sampling period is the same as that of IMU. In the period of 0-20s, the pedestrian to be positioned can only receive one UWB ranging information, in the period of 20-40s, the pedestrian to be positioned can receive three UWB ranging information, and in the period of 40-60s, the pedestrian to be positioned can receive five UWB ranging information. Considering that EKF and PF need to reconstruct the filter equations when the measurement information changes, when there is only one UWB ranging information in the period of 0-20s, EKF and PF only perform time updates without measurement updates; when there are three UWB ranging information in the period of 20-40s, EKF and PF perform time updates and measurement updates normally; when there are five UWB ranging information in the period of 40-60s, EKF and PF only fuse three of the UWB ranging information, that is, the EKF and PF algorithms cannot process the measurement information that is dynamically accessed and exited. Comparison of pedestrian positioning error and velocity error based on EKF, FG and PF algorithms in complex indoor scenes. Figure 2 and Figure 3 As shown in Figure 2, the overall positioning root mean square error (RMSE) and operation time are compared. Figure 4 shown.

[0076] according to Figures 2 to 4, in 0-20s, the INS / UWB ranging combined navigation algorithm based on EKF and the INS / UWB ranging combined navigation algorithm based on PF cannot effectively fuse a single UWB ranging information, but can only perform time update, resulting in its positioning accuracy being significantly lower than the factor graph algorithm. In 20-40s, although the INS / UWB ranging combined navigation algorithm based on EKF can perform measurement updates based on three UWB ranging information, its positioning and speed measurement accuracy are lower than the factor graph algorithm due to its first-order Taylor expansion truncation error. Although the positioning accuracy of the INS / UWB ranging combined navigation algorithm based on PF is high, its calculation amount is too large, so it is not suitable for actual engineering applications. In 40-60s, the filter structures of the INS / UWB ranging combined navigation algorithm based on EKF and the INS / UWB ranging combined navigation algorithm based on PF are fixed, and only three UWB measurement information can be fused. The factor graph algorithm proposed in the present invention can effectively fuse five UWB ranging information, so its positioning and speed measurement accuracy is high. Simulation results show that the factor graph-based INS / UWB integrated navigation algorithm proposed in the present invention can effectively integrate the UWB ranging information of dynamic random access and exit, and its positioning accuracy is improved by 91.50% and 90.97% respectively compared with the other two algorithms, and the speed measurement accuracy is improved by 67.66% and 53.65% respectively.

[0077] The present invention provides an INS / UWB indoor pedestrian combined navigation method based on factor graph, and the specific steps are as follows:

[0078] Step 1: Establish the state equation of INS / UWB indoor pedestrian integrated navigation system;

[0079]

[0080]

[0081] v k,x =v k-1,x +a k-1,x t (3)

[0082] v k,y =v k-1,y +a k-1,y t (4)

[0083] Where: x k-1 ,y k-1 and x k ,y k are the x-axis and y-axis positions of the pedestrian to be located in the navigation coordinate system at time k-1 and time k respectively; v x,k-1 、v y,k-1 and v x,k 、v y,kare the x-axis and y-axis velocities of the pedestrian to be located in the navigation coordinate system at time k-1 and time k respectively; t is the sampling period. k-1,x and a k-1,y are the accelerations of the x-axis and y-axis of the navigation coordinate system at time k-1, which are obtained by projecting the specific force output of the accelerometer in the inertial navigation system in the horizontal direction, and their probability distributions are Among them, μ g and are the mean and variance of the variable g, respectively.

[0084] In addition, the coupling relationship between the pedestrian position variable and the speed variable at time k is:

[0085]

[0086]

[0087] Step 2: Establish the measurement equation of INS / UWB indoor pedestrian integrated navigation system;

[0088]

[0089] Where: is the UWB ranging information between the pedestrian to be located and the i-th (i=1,2,…,N) base station or other pedestrians at time k, and N represents the number of valid UWB ranging information that the pedestrian to be located can obtain; and are the x-axis and y-axis positions of the navigation coordinate system of the i-th base station or other pedestrian at time k respectively; is the distance measurement noise, and its probability distribution is

[0090] Step 3: Establish the factor graph model of INS / UWB indoor pedestrian integrated navigation system, such as Figure 1 As shown in the figure, white circles are variable nodes, black circles are function nodes, and the constraint relationship of each function node is:

[0091] Function node f C ,f H ,f D ,f I The constraint functions are shown in equations (3)-(6), and the function nodes Describes relative position information With absolute position information (x k ,y k ) can be expressed as:

[0092]

[0093]

[0094] Similarly, function nodes Describe measurement information With relative position information The functional constraint relationship between corresponds to the UWB measurement model, that is,

[0095]

[0096] The remaining function nodes f A ,f B ,f J ,f K , Respectively represent the variable node v connected to it k-1,x ,x k-1 ,v k-1,y ,y k-1 , The probability density function of .

[0097] Step 4: Based on the system factor graph model established in step 3, calculate the probability density function of the two-dimensional position variable and speed variable of the pedestrian to be located after the time update in the first message transmission process in the factor graph. Since it is assumed that the messages transmitted between nodes obey the Gaussian distribution, only the corresponding mean and variance need to be considered in the message calculation and transmission process;

[0098] The average x-axis speed of the pedestrian to be located at time k after time update Y-axis speed mean and their variances The average x-axis velocity of the pedestrian to be located can be calculated based on the k-1 moment Y-axis speed mean and their variances The mean x-axis acceleration obtained by horizontally projecting the accelerometer specific force output at time k-1 Y-axis acceleration mean and their variances Specifically, calculate The formula is as follows:

[0099]

[0100]

[0101]

[0102]

[0103] The average x-axis position of the pedestrian to be located at time k after time update Mean y-axis position and their variances The mean x-axis position of the pedestrian to be located can be calculated based on the k-1 moment Mean y-axis position and their variances The average x-axis velocity of the pedestrian to be located calculated at time k-1 Y-axis speed mean and their variances The average x-axis speed of the pedestrian to be located at time k after time update Y-axis speed mean and their variances Specifically, calculate The formula is as follows:

[0104]

[0105]

[0106]

[0107]

[0108] Step 5: According to the system factor graph model established in step 3, the probability density function of the two-dimensional position variable of the pedestrian to be located after the measurement is updated in the first message transmission process in the factor graph is calculated;

[0109] The average x-axis position of the pedestrian to be located at time k after measurement update Mean y-axis position and their variances The mean x-axis position of the ith base station or other pedestrian can be calculated based on time k Mean y-axis position and their variances The average x-axis position of the pedestrian to be located after time update calculated at time k Mean y-axis position and their variances The mean value of the UWB ranging information between the pedestrian to be located and the i-th base station or other pedestrians at time k and its variance Calculation. Since the system measurement equation is a nonlinear equation, in order to maintain the Gaussian characteristics of the variables, the nonlinear measurement equation needs to be Taylor expanded. In addition, in order to obtain higher estimation accuracy, the present invention adopts a second-order Taylor expansion. Specifically, calculation The formula is as follows:

[0110]

[0111]

[0112]

[0113]

[0114] Step 6: According to Step 4 and Step 5, the probability density function of the updated two-dimensional position variable and speed variable of the pedestrian to be located in the factor graph during the second message iteration process is calculated;

[0115] The average x-axis position of the pedestrian to be located at time k after measurement update Mean y-axis position and their variances The average x-axis position of the pedestrian to be located at time k after time update Mean y-axis position and their variances And the average x-axis position of the pedestrian to be located at time k after the updated measurement when the factor graph performs the first message transmission Mean y-axis position and their variances The mean x-axis position of the ith base station or other pedestrian calculated at time k Mean y-axis position and their variances The mean value of the UWB ranging information between the pedestrian to be located and the i-th base station or other pedestrians at time k and its variance Specifically, calculate The formula is as follows:

[0116]

[0117]

[0118]

[0119]

[0120] Where: It can be calculated according to the following formula:

[0121]

[0122] The average x-axis speed of the pedestrian to be located at time k after measurement and update Y-axis speed mean and their variances The mean x-axis position of the pedestrian to be located can be calculated based on the k-1 time Mean y-axis position and their variances The average x-axis velocity of the pedestrian to be located calculated at time k-1 Y-axis speed mean and their variances The average x-axis position of the pedestrian to be located at time k after the updated measurement when the factor graph performs the second message iteration Mean y-axis position and their variances Specifically, calculate The formula is as follows:

[0123]

[0124]

[0125]

[0126]

[0127] Where: It can be calculated according to the following formula:

[0128]

[0129] Step 7: According to step 4 and step 6, the probability density function of the two-dimensional position variable and speed variable of the pedestrian to be located in the factor graph is calculated;

[0130] The mean x-axis position of the pedestrian to be located at time k Mean y-axis position and their variances According to the x-axis position mean Mean y-axis position and their variances The average x-axis position of the pedestrian to be located at time k after the updated measurement when the factor graph performs the second message iteration Mean y-axis position and their variances Calculation. Specifically, the calculation formula is as follows:

[0131]

[0132] The average x-axis velocity of the pedestrian to be located at time k Y-axis speed mean and their variances The average x-axis speed after the pedestrian to be located at time k is updated Y-axis speed mean and their variances The average x-axis speed of the pedestrian to be located at time k after the second message iteration of the factor graph Y-axis speed mean and their variances Specifically, calculate The formula is as follows:

[0133]

[0134] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. An INS / UWB indoor pedestrian combined navigation method based on factor graph, characterized by: Step 1: Establish the state equation of INS / UWB indoor pedestrian integrated navigation system; Step 2: Establish the measurement equation of INS / UWB indoor pedestrian integrated navigation system; Step 3: Establish the factor graph model of INS / UWB indoor pedestrian integrated navigation system; Step 4: According to the system factor graph model established in step 3, the probability density function of the two-dimensional position variable and speed variable of the pedestrian to be located after the time update in the first message transmission process in the factor graph is calculated; Step 5: According to the system factor graph model established in step 3, the probability density function of the two-dimensional position variable of the pedestrian to be located after the measurement is updated in the first message transmission process in the factor graph is calculated; Step 6: According to Step 4 and Step 5, the probability density function of the updated two-dimensional position variable and speed variable of the pedestrian to be located in the factor graph during the second message iteration process is calculated; Step 7: According to steps 4 and 6, the probability density function of the two-dimensional position variable and speed variable of the pedestrian to be located in the factor graph is calculated.

2. The INS / UWB indoor pedestrian combined navigation method based on factor graph according to claim 1 is characterized by: The state equation of the INS / UWB indoor pedestrian integrated navigation system in step 1 is: v k,x =v k-1,x +a k-1,x t (3) v k,y =v k-1,y +a k-1,y t (4) Where: x k-1 ,y k-1 and x k ,y k are the x-axis and y-axis positions of the pedestrian to be located in the navigation coordinate system at time k-1 and time k respectively; v x,k-1 、v y,k-1 and v x,k 、v y,k are the x-axis and y-axis velocities of the pedestrian to be located in the navigation coordinate system at time k-1 and time k respectively; t is the sampling period; a k-1,x and a k-1,y are the accelerations of the x-axis and y-axis of the navigation coordinate system at time k-1, which are obtained by projecting the specific force output of the accelerometer in the inertial navigation system in the horizontal direction, and their probability distributions are Among them, μ g and are the mean and variance of the variable g respectively; In addition, the coupling relationship between the pedestrian position variable and the speed variable at time k is: 。 3. The INS / UWB indoor pedestrian integrated navigation method based on factor graph according to claim 1 is characterized by: The measurement equation of the INS / UWB indoor pedestrian integrated navigation system in step 2 is: Where: is the UWB ranging information between the pedestrian to be located and the i-th (i=1,2,…,N) base station or other pedestrians at time k, and N represents the number of valid UWB ranging information that the pedestrian to be located can obtain; and are the x-axis and y-axis positions of the navigation coordinate system of the i-th base station or other pedestrian at time k respectively; is the distance measurement noise, and its probability distribution is 4. The INS / UWB indoor pedestrian combined navigation method based on factor graph according to claim 1 or 2, characterized in that: The constraint relationship of each function node of the INS / UWB indoor pedestrian integrated navigation system established in step 3 is: Function node f C ,f H ,f D ,f I The constraint functions are shown in equations (3)-(6), and the function nodes Describes relative position information With absolute position information (x k ,y k ) can be expressed as: Similarly, function nodes Describe measurement information With relative position information The functional constraint relationship between them corresponds to the UWB measurement model: The remaining function nodes f A ,f B ,f J ,f K ,f Zi Respectively represent the variable node v connected to it k-1,x ,x k-1 ,v k-1,y ,y k-1 , The probability density function of .

5. The INS / UWB indoor pedestrian integrated navigation method based on factor graph according to claim 1, characterized in that: When the factor graph in step 4 performs the first message transmission, the average x-axis speed of the pedestrian to be located at time k after the time is updated Y-axis speed mean and their variances The average x-axis velocity of the pedestrian to be located can be calculated based on the k-1 moment Y-axis speed mean and their variances The mean x-axis acceleration obtained by horizontally projecting the accelerometer specific force output at time k-1 Y-axis acceleration mean and their variances Calculate, calculate The formula is: The average x-axis position of the pedestrian to be located at time k after time update Mean y-axis position and their variances The mean x-axis position of the pedestrian to be located can be calculated based on the k-1 moment Mean y-axis position and their variances The average x-axis velocity of the pedestrian to be located calculated at time k-1 Y-axis speed mean and their variances The average x-axis speed of the pedestrian to be located at time k after time update Y-axis speed mean and their variances Calculate, calculate The formula is: 。 6. The INS / UWB indoor pedestrian integrated navigation method based on factor graph according to claim 1, characterized in that: When the factor graph in step 5 performs the first message transmission, the average x-axis position of the pedestrian to be located at time k after the measurement is updated Mean y-axis position and their variances The mean x-axis position of the ith base station or other pedestrian can be calculated based on time k Mean y-axis position and their variances The average x-axis position of the pedestrian to be located after time update calculated at time k Mean y-axis position and their variances The mean value of the UWB ranging information between the pedestrian to be located and the i-th base station or other pedestrians at time k and its variance Calculation; Since the system measurement equation is a nonlinear equation, in order to maintain the Gaussian characteristics of the variables, the nonlinear measurement equation needs to be Taylor expanded. In order to obtain higher accuracy, the second-order Taylor expansion is used to calculate The formula is: 。 7. The INS / UWB indoor pedestrian integrated navigation method based on factor graph according to claim 1, characterized in that: When the factor graph in step 6 performs the second message iteration, the average x-axis position of the pedestrian to be located at time k after the measurement is updated Mean y-axis position and their variances The average x-axis position of the pedestrian to be located at time k after time update Mean y-axis position and their variances And the average x-axis position of the pedestrian to be located at time k after the updated measurement when the factor graph performs the first message transmission Mean y-axis position and their variances The mean x-axis position of the ith base station or other pedestrian calculated at time k Mean y-axis position and their variances The mean value of the UWB ranging information between the pedestrian to be located and the i-th base station or other pedestrians at time k and its variance Calculate, calculate The formula is: Where: It can be calculated according to the following formula: The average x-axis speed of the pedestrian to be located at time k after measurement and update Y-axis speed mean and their variances The mean x-axis position of the pedestrian to be located can be calculated based on the k-1 time Mean y-axis position and their variances The average x-axis velocity of the pedestrian to be located calculated at time k-1 Y-axis speed mean and their variances The average x-axis position of the pedestrian to be located at time k after the updated measurement when the factor graph performs the second message iteration Mean y-axis position and their variances Calculate, calculate The formula is: Where: It can be calculated according to the following formula: 。 8. The INS / UWB indoor pedestrian integrated navigation method based on factor graph according to claim 1, characterized in that: In step 7, the average x-axis position of the pedestrian to be located at time k Mean y-axis position and their variances According to the x-axis position mean Mean y-axis position and their variances The average x-axis position of the pedestrian to be located at time k after the updated measurement when the factor graph performs the second message iteration Mean y-axis position and their variances Calculation, the calculation formula is as follows: The average x-axis velocity of the pedestrian to be located at time k Y-axis speed mean and their variances The average x-axis speed after the pedestrian to be located at time k is updated Y-axis speed mean and their variances The average x-axis speed of the pedestrian to be located at time k after the second message iteration of the factor graph Y-axis speed mean and their variances Calculate, calculate The formula is as follows: 。

Images