PDRUWB indoor fusion positioning method based on UKF-FGO algorithm

Abstract

The invention provides a PDRUWB indoor fusion positioning method based on a UKF-FGO algorithm. The method comprises the following steps: collecting indoor motion data of a target person; determining a first positioning result and a second positioning result based on the motion data; wherein the first positioning result is determined based on a PDR algorithm, and the second positioning result is determined based on a UWB algorithm; inputting the first positioning result into a first equation, and determining a first target; inputting the second positioning result into a second equation, and determining a second target; and based on a UKF-FGO algorithm, fusing the first target and the second target to obtain a target equation, and solving the target equation to obtain an indoor fusion positioning result of the target person. Compared with a traditional method, the method has the advantages that the positioning precision can be improved by combining the advantages of two different positioning algorithms; and for the limitation between the two positioning results, a fusion algorithm is adopted to process the two obtained positioning results again, so that the positioning data is corrected, the deviation is reduced, and the accuracy of the indoor positioning result is further improved.

Description

Technical Field

[0001] The present invention relates to the field of indoor positioning technology, and in particular to a PDRUWB indoor fusion positioning method based on a UKF-FGO algorithm. Background Art

[0002] Buildings and obstacles in indoor environments limit the penetration of Global Navigation Satellite System (GNSS) signals, resulting in reduced signal quality. In addition, multipath effects are prone to occur in indoor environments, where signals are reflected and refracted inside buildings, causing the receiver to receive signals on multiple paths, thus affecting positioning accuracy. These factors have resulted in GNSS being unable to meet people's needs for indoor positioning.

[0003] To solve this technical problem, ultra-wideband technology (UWB) and pedestrian dead reckoning (PDR) are currently used to achieve indoor positioning. For UWB technology, it uses the arrival time of the pulse signal between the base station and the mobile tag for positioning, and has high positioning accuracy, but is easily affected by various factors such as non-line-of-sight and multipath effects. The PDR algorithm mainly uses inertial measurement elements, such as accelerometers, gyroscopes, magnetometers, etc., to locate indoor personnel based on parameters such as step number, step length, and direction. The positioning has high short-term accuracy and good stability, but there is an integral error in long-distance positioning.

[0004] That is, the indoor positioning algorithm currently used is affected by various factors, which causes deviations in the final positioning data, thereby causing the final indoor positioning result to be inaccurate. Summary of the invention

[0005] The embodiment of the present invention provides a PDRUWB indoor fusion positioning based on the UKF-FGO algorithm to solve the problem of inaccurate indoor positioning results caused by the deviation of the final determined positioning data at the current stage.

[0006] In a first aspect, an embodiment of the present invention provides a PDRUWB indoor fusion positioning method based on a UKF-FGO algorithm, including:

[0007] Collect the target person's indoor movement data;

[0008] Determine a first positioning result and a second positioning result based on the motion data; wherein the first positioning result is determined based on the PDR algorithm, and the second positioning result is determined based on the UWB algorithm;

[0009] Inputting the first positioning result into the first equation to determine the first target;

[0010] Inputting the second positioning result into the second equation to determine the second target;

[0011] Based on the UKF-FGO algorithm, the first target and the second target are fused to obtain the target equation. The target equation is solved to obtain the fused positioning result of the target person indoors.

[0012] In a possible implementation, the first target and the second target are fused to obtain a target equation, and the target equation is solved to obtain a fused positioning result of the target person indoors, including:

[0013] The motion data is input into the factor graph optimization algorithm integrated with the unscented Kalman filter to obtain the position information of the target person output by UKF at each moment;

[0014] Input the location information of the target personnel output by UKF at each moment into the third process to determine the third target;

[0015] The first target, the second target and the third target are integrated to obtain the target equation, and the target equation is solved to obtain the fused positioning result of the target person in the room.

[0016] In a possible implementation, the third program is:

[0017]

[0018] Among them, e ukf is the error term of the UKF vertex; is the location information of the target person output by UKF at time k; p k is the motion data input by UKF at time k; Ω ukf is the weight matrix; is the observation noise.

[0019] In a possible implementation, the first positioning result includes the step length, heading angle, and pose variable vertex at each moment of the target person output by the PDR algorithm;

[0020] The second positioning result includes the coordinates corresponding to the target person at each moment output by the UWB algorithm.

[0021] In one possible implementation, the first equation is:

[0022]

[0023] Among them, e pdr is the error term of the PDR pose vertex; q k-1 is the error term and the pose variable at time k-1; qk is the pose variable at time k; Ω pdr is the weight matrix of the squared error term; L k is the change of posture at time k; x k is the horizontal coordinate of the two-dimensional position at time k; x k-1 is the horizontal coordinate of the two-dimensional position at time k-1; y k is the longitudinal coordinate of the two-dimensional position at time k; y k-1 is the longitudinal coordinate of the two-dimensional position at time k-1; θ k is the heading angle at time k; θ k-1 is the heading angle at time k-1; s k is the step size corresponding to time k; Δθ k is the change of heading angle at time k;

[0024] The second equation is:

[0025]

[0026] e uwb (u k ,B n ,d kn )=norm(B n -u k )-d kn

[0027] Among them, e uwb is the error function formed by the UWB measurement value; u k is the coordinate B of the target person at time k n is the coordinates of the nth base station; d kn is the distance from the target person to the nth base station at time k; Ω uwb is the weight matrix; norm(·) represents the modulus of the vector; norm(B n -u k ) represents the predicted value, which is used to predict the distance from the target person to the nth base station at time k;

[0028] The objective equation is:

[0029] F(·)=F pdr (·)+F uwb (·)+F ukf (·).

[0030] In a possible implementation, the motion data includes acceleration and two-dimensional position corresponding to each moment;

[0031] Determining a first positioning result based on the motion data includes:

[0032] According to the acceleration corresponding to each moment, the gait data corresponding to each step of the target person is determined; wherein the gait data includes the starting moment, the middle moment and the acceleration corresponding to each moment of each step;

[0033] Determine the step length of the target person based on the gait data;

[0034] The acceleration and gravitational acceleration corresponding to each moment are converted into coordinate systems respectively to obtain the standard acceleration and gravitational acceleration corresponding to each moment in the b-system coordinates;

[0035] Multiply the standard acceleration and the gravity acceleration to get the standard acceleration error;

[0036] Input the standard acceleration error into the PI filter to obtain the acceleration compensation term;

[0037] Based on the acceleration compensation term, the acceleration corresponding to each moment is corrected to obtain an updated heading angle;

[0038] Based on the updated heading angle and two-dimensional position, the vertex of the pose variable at each moment is determined.

[0039] In a possible implementation manner, determining a second positioning result based on the motion data includes:

[0040] For each moment, the electromagnetic wave flight time between the kth base station and the target person is calculated using the bilateral two-way ranging method;

[0041] Calculate the distance between the kth base station and the target person based on the electromagnetic wave flight time;

[0042] Based on the distance between the target person and a preset number of base stations, a nonlinear equation system is obtained;

[0043] Solving the nonlinear equation group at least twice and obtaining positioning errors corresponding to at least the two solutions;

[0044] When the positioning error is less than a preset threshold, the average value of the initial coordinates obtained by the at least two calculations is used as the coordinates corresponding to the target person at each moment output by the UWB algorithm.

[0045] In a second aspect, an embodiment of the present invention provides a PDRUWB indoor fusion positioning device based on a UKF-FGO algorithm, comprising:

[0046] A collection module is used to collect the movement data of the target person indoors;

[0047] A positioning module, used to determine a first positioning result and a second positioning result based on the motion data; wherein the first positioning result is determined based on the PDR algorithm, and the second positioning result is determined based on the UWB algorithm;

[0048] A determination module, used for inputting the first positioning result into a first equation to determine a first target;

[0049] The determination module is further used to input the second positioning result into the second equation to determine the second target;

[0050] The fusion module is used to fuse the first target and the second target based on the UKF-FGO algorithm to obtain the target equation, solve the target equation, and obtain the fusion positioning result of the target person indoors.

[0051] In a third aspect, an embodiment of the present invention provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the processor implements the steps of the method described in the first aspect or any possible implementation manner of the first aspect.

[0052] In a fourth aspect, an embodiment of the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the method described in the first aspect or any possible implementation method of the first aspect are implemented.

[0053] The embodiment of the present invention provides a PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm. Compared with the traditional method, the embodiment of the present invention adopts two different positioning algorithms to determine two positioning results. By combining the advantages of the two, the positioning accuracy can be improved. For the limitations between the two, a fusion algorithm is used to reprocess the two positioning results to correct the positioning data, reduce the deviation, and thereby improve the accuracy of the indoor positioning results. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings required for use in the embodiments or related technical descriptions are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0055] Figure 1 It is a flow chart of an implementation of a PDRUWB indoor fusion positioning method based on a UKF-FGO algorithm provided in an embodiment of the present invention;

[0056] Figure 2 A flowchart for implementing gait detection of a PDRUWB indoor fusion positioning method based on a UKF-FGO algorithm provided in an embodiment of the present invention;

[0057] Figure 3A flow chart of heading angle estimation of a PDRUWB indoor fusion positioning method based on a UKF-FGO algorithm provided in an embodiment of the present invention;

[0058] Figure 4 It is a factor graph optimization flow chart of the fused UKF of the PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm provided in an embodiment of the present invention;

[0059] Figure 5 A top view of a test site for the PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm provided in an embodiment of the present invention;

[0060] Figure 6 A fusion algorithm positioning trajectory diagram of the PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm provided in an embodiment of the present invention;

[0061] Figure 7 A fusion algorithm positioning error diagram of the PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm provided in an embodiment of the present invention;

[0062] Figure 8 It is a structural diagram of a PDRUWB indoor fusion positioning device based on a UKF-FGO algorithm provided in an embodiment of the present invention;

[0063] Fig. 9 is a schematic diagram of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0064] In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present invention. However, it should be clear to those skilled in the art that the present invention may be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present invention.

[0065] In order to make the purpose, technical solutions and advantages of the present invention more clear, specific embodiments will be described below in conjunction with the accompanying drawings.

[0066] Figure 1 The following is a flow chart of the implementation of the PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm provided in the embodiment of the present invention. Figure 1 As shown, the method may include:

[0067] Step 110: Collect the target person's indoor movement data.

[0068] In this embodiment, the motion data may include the acceleration corresponding to the target person at each moment measured by the acceleration sensor, the turning angle, posture and direction corresponding to the target person at each moment measured by the gyroscope, the geomagnetic intensity corresponding to the target person at each moment measured by the magnetometer, the angular velocity corresponding to the target person at each moment measured by the gyroscope, the ground coordinates of the target person at each moment measured by positioning technology, and other data.

[0069] Step 120: Determine a first positioning result and a second positioning result based on the motion data; wherein the first positioning result is determined based on the PDR algorithm, and the second positioning result is determined based on the UWB algorithm.

[0070] In this embodiment, two different algorithms can respectively select the PDR algorithm and the UWB algorithm as two different positioning algorithms to obtain the first positioning result and the second positioning result. Accordingly, the first positioning result can be determined by the PDR algorithm according to the acceleration and two-dimensional position corresponding to each moment in the motion data; the second positioning result can be determined by the UWB algorithm according to the ground coordinates of the target person in the motion data at each moment; wherein the first positioning result may include the step length, heading angle and pose variable vertex of the target person at each moment output by the PDR algorithm; the second positioning result may include the coordinates corresponding to the target person at each moment output by the UWB algorithm.

[0071] The process of determining the first positioning result and the second positioning result is described below through an optional embodiment:

[0072] In an optional embodiment, determining the first positioning result based on the motion data may include:

[0073] According to the acceleration corresponding to each moment, the gait data corresponding to each step of the target person is determined; wherein the gait data includes the starting moment, the middle moment and the acceleration corresponding to each moment of each step.

[0074] The step length of the target person is determined based on the gait data.

[0075] The acceleration and gravitational acceleration corresponding to each moment are converted into coordinate systems respectively to obtain the standard acceleration and gravitational acceleration corresponding to each moment in the b-system coordinates.

[0076] Multiply the standard acceleration and the acceleration due to gravity to get the standard acceleration error.

[0077] The standard acceleration error is input into the PI filter to obtain the acceleration compensation term.

[0078] The acceleration corresponding to each moment is corrected based on the acceleration compensation term to obtain an updated heading angle.

[0079] Based on the updated heading angle and two-dimensional position, the vertex of the pose variable at each moment is determined.

[0080] Figure 2 The gait detection implementation flow chart of the PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm provided in the embodiment of the present invention is as follows: Figure 2 , how to determine the gait data corresponding to each step of the target person recorded in this embodiment is explained.

[0081] In this embodiment, the target person's stepping action can be divided into a finite number of states based on the peak detection method of the finite state machine. When the pedestrian completes all states, it is considered that the step is completed. Taking any step of the target person as an example, in this embodiment, the target person's stepping action is divided into six states, namely from S1 to S6. S1 to S6 are described in detail below:

[0082] State S1: This state assumes that the target person is in a stationary standing state, and the acceleration fluctuates and continuously crosses the zero point. The zero point is divided into an upper zero point and a lower zero point. The upper zero point is the zero point through which the acceleration signal turns from a negative value to a positive value, that is, the starting point of the rising trend of the acceleration signal during a single step; the lower zero point is the zero point through which the acceleration signal turns from a positive value to a negative value, that is, the middle point of the falling trend of the acceleration signal during a single step.

[0083] The judgment formula of upper zero point and lower zero point is as follows:

[0084] szero={+1|acc k ≥0&&acc k-1 ≤0}(1)

[0085] xzero={+1|acc k ≤0&&acc k-1 ≥0}(2)

[0086] Among them, szero is the count of the upper zero point, xzero is the count of the lower zero point, and acc k is the total acceleration at time k, acc k-1 is the total acceleration at time k-1. In this state, set szero and xzero to zero.

[0087] When the zero mark satisfies formula (3), it is determined that the target person enters the S2 state.

[0088] szero-xzero=1(3)

[0089] State S2: In this state, the target person is considered to have started walking. At this time, the heel begins to leave the ground and the overall acceleration is on an upward trend. According to formula (4), confirm whether the acceleration is on an upward trend. According to formula (5), confirm whether the acceleration is greater than the fluctuation threshold accT. If formula (5) is satisfied, it is determined to enter state S3. Among them, formulas (4) and (5) are as follows:

[0090] acc i >acc i-1 (4)

[0091] acc i >accT(5)

[0092] State S3: In this state, it is considered that the target person's feet leave the ground and the acceleration acc reaches the maximum, that is, it satisfies formula (6), and peakacc=acc is recorded at this time i Next, the formula (7) is judged. If the formula (7) is satisfied, the state record S4 is entered. Among them, the formulas (6) and (7) are as follows:

[0093] peak i ={acc i |acc i >acc i-1 &&acc i >acc i+1}(6)

[0094] szero-xzero=0(7)

[0095] State S4: In this state, it is considered that the feet of the target person start to drop from the highest point, and formula (8) is satisfied at this time. When formula (8) is satisfied, it is confirmed whether the acceleration is greater than the fluctuation threshold accD according to formula (9). If formula (9) is satisfied, it is determined to enter state S5. Among them, formulas (8) and (9) are as follows:

[0096] acc i <acc i-1 (8)

[0097] acc i ≤accD(9)

[0098] State S5: In this state, it is considered that the target person's feet are close to the ground and the acceleration acc reaches the minimum value, that is, it satisfies formula (10), and valleyacc = acc is recorded at this time i According to formula (11), if the peak-to-valley difference is greater than the threshold σ, the state S6 is entered. Formulas (10) and (11) are as follows:

[0099] valleyj ={acc j |acc j <acc j-1 &&acc j <acc j+1} (10)

[0100] peakacc-valleyacc>σ (11)

[0101] State S6: In this state, the target person completes a step, the step count increases by one, and returns to S1.

[0102] Based on this state, the gait data corresponding to each step of the target person can be obtained, that is, the acceleration at the starting moment and the acceleration at the middle moment of each step, as well as the state and acceleration corresponding to any other moment.

[0103] After determining the gait data of the target person, the step length of the target person can be calculated according to the acceleration at the starting moment, the acceleration at the middle moment, and the state and acceleration corresponding to any other moment in the gait data. The step length calculation formula can be:

[0104]

[0105] Among them, S k is the step length at time k; p is the weight factor, which can be set to 0.4 in this embodiment; S k-1 is the step length at time k-1; K1 and K2 are constants, which are determined by testing in the experiment; a max 、a min are the maximum acceleration and minimum acceleration in the gait corresponding to time k; w k is the variance of acceleration in gait corresponding to time k, f is the step frequency; t s and t x Represents the starting time and middle time of a single step, that is, the upper zero point and lower zero point in gait detection.

[0106] Figure 3 The heading angle estimation flow chart of the PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm provided in the embodiment of the present invention is as follows: Figure 3 , the determination of the heading angle in this embodiment is specifically described.

[0107] In this embodiment, the heading angle estimation can be performed based on PI complementary filtering. Specifically, the output value of the acceleration sensor in the b system (the geodetic dimension in the geodetic coordinate system) can be defined as a b , normalize it and get the standard acceleration data in the b system:

[0108] ab =[a x a y a z ] T (14)

[0109] Convert the gravitational acceleration from the n system to the b system:

[0110]

[0111] The accelerations converted by equations (14) and (15) are regarded as standard acceleration and gravitational acceleration, respectively.

[0112] to a b and g b Doing vector cross product, we can get the error e a It is used as the input of the PI filter, where the error e a The calculation formula is as follows:

[0113]

[0114] Use PI (proportional-integral) regulator to adjust the error e a Filtering is performed to eliminate drift errors. The expression of the compensation value δ is as follows:

[0115] δ=(k p +k i )·e a (17)

[0116] In the formula, k p is the proportional coefficient, which controls the strength of the system correction. i It is used to adjust the error integral to correct the steady-state error.

[0117] Compare δ with the angular velocity data w output by the gyroscope g The angular velocity correction value w based on acceleration can be obtained as follows:

[0118] w=w g +δ(18)

[0119] Use w to update the quaternion and finally find the heading angle updated in real time.

[0120] Afterwards, based on the updated heading angle and two-dimensional position, the pose variable vertex at each moment can also be determined, where the pose variable vertex can be expressed as [x k ,y k ,θ k ] T .

[0121] After obtaining the first positioning result, the process of determining the second positioning result is described below. Accordingly, in an optional embodiment, determining the second positioning result based on the motion data may include:

[0122] For each moment, the electromagnetic wave flight time between the kth base station and the target person is calculated using the bilateral two-way ranging method.

[0123] The distance between the kth base station and the target person is calculated based on the electromagnetic wave flight time.

[0124] Based on the distance between the target person and a preset number of base stations, a nonlinear equation group is obtained.

[0125] The nonlinear equation group is solved at least twice, and positioning errors corresponding to at least the two solutions are obtained.

[0126] When the positioning error is less than a preset threshold, the average value of the initial coordinates obtained by the at least two calculations is used as the coordinates corresponding to the target person at each moment output by the UWB algorithm.

[0127] In this embodiment, when actually used, multiple UWB base stations need to be set up, and the location of each UWB base station is different. The target pedestrian carries a tag for positioning detection. Specifically, under the premise that the target pedestrian carries a tag, the electromagnetic wave flight time between the kth base station and the target person is calculated using the bilateral two-way ranging method, and the distance between the kth base station and the target person is determined based on the electromagnetic wave flight time. Among them, the distance between the kth base station and the target person can be calculated by the following method: k =t k *c, c is the speed of electromagnetic wave.

[0128] The preset number of base stations can be set to 4 base stations. Accordingly, when the number of base stations detecting tags is not less than 4, the TDOA algorithm is used to obtain a nonlinear equation group based on the distance between the target person and the preset number of base stations:

[0129]

[0130] To ensure the accuracy of the positioning result, the least square method can be used to calculate formula (19) at least twice, and at least two solution results can be obtained. The positioning errors corresponding to the two solutions are calculated based on the two results. When the positioning error is less than the preset threshold, the average value of the initial coordinates obtained by the two solutions is used as the coordinates corresponding to the target person at each moment output by the UWB algorithm.

[0131] That is, when When the UWB algorithm outputs the target person at each moment, the average value of the initial coordinates obtained from the two solutions is used as the coordinates of the target person at each moment.

[0132] Among them, (x 01 ,y 01 ,z 01 ) and (x 02 ,y 02 ,z 02 ) are the initial coordinates obtained from the two settlements, D thr is a preset threshold, which can be set to 0.3 m in this embodiment.

[0133] After obtaining the first positioning result and the second positioning result, the first target may be determined through step 130, and the second target may be determined through step 140. For details, reference may be made to the following corresponding embodiments:

[0134] Step 130: Input the first positioning result into the first equation to determine the first target.

[0135] In this embodiment, in order to perform indoor fusion positioning, this embodiment adopts the factor graph optimization (Unscented Kalman Filter Factor Graph Optimization, UKF-FGO) algorithm fused with unscented Kalman filter. Based on this, this embodiment sets a PDR constraint equation for the first positioning result, that is, the first equation.

[0136] The pose variable vertex contains three factors: two-dimensional position and rotation angle, that is, [x k ,y k ,θ k ] T The posture change of each step of the target person is usually characterized by two parameters: step length and heading angle in the direction of travel. It is obtained by processing and analyzing the data collected by the PDR system. Since the PDR data is derived from the direct measurement of the sensor, the posture change parameters calculated based on this data can be regarded as observations. The posture change L k It can be written as follows:

[0137]

[0138] Among them, L k is the change of posture at time k; s k is the step size corresponding to time k; Δθ k is the change of heading angle at time k.

[0139] The PDR constraint is used to describe the relationship between adjacent poses in the PDR algorithm. Its error term is related to the pose variable at the previous moment, the current pose variable, and the observed pose change. pdr The definition is as follows:

[0140]

[0141] Among them, q k-1 is the error term and the pose variable at time k-1; q k is the pose variable at time k; x k is the horizontal coordinate of the two-dimensional position at time k; x k-1 is the horizontal coordinate of the two-dimensional position at time k-1; y k is the longitudinal coordinate of the two-dimensional position at time k; y k-1 is the longitudinal coordinate of the two-dimensional position at time k-1; θ k is the heading angle at time k; θ k-1 is the heading angle at time k-1.

[0142] Correspondingly, the error term e based on the PDR pose vertex pdr , the first equation in least squares form can be:

[0143]

[0144] Among them, Ω pdr is the weight matrix of the squared error term.

[0145] Step 140: Input the second positioning result into the second equation to determine the second target.

[0146] Since the UWB system can measure the distance from the mobile tag to the UWB base station through the double-sided two-way ranging (DS-TWR) method, the distance constraint equation between the UWB base station and the mobile tag can be constructed using the distance information, which is the second equation.

[0147] Correspondingly, at time k, the coordinates of the target person are u k , the coordinates of the nth base station are B n =(x n ,y n The measured value obtained by the UWB sensor at time k is the distance from the target person (tag) to the nth base station, denoted as d kn The error function e formed by the UWB measurement value uwb It can be defined as:

[0148] e uwb (u k ,B n ,d kn )=norm(B n -u k )-d kn (twenty four)

[0149] Among them, e uwbis the error function formed by the UWB measurement value; u k is the coordinate B of the target person at time k n is the coordinates of the nth base station; d kn is the distance from the target person to the nth base station at time k; norm(·) represents the norm of the vector; norm(B n -u k ) represents the predicted value, which is used to predict the distance from the target person to the nth base station at time k.

[0150] Based on the error function e uwb , the second equation can be expressed as:

[0151]

[0152] Among them, Ω uwb is the weight matrix.

[0153] Step 150: Based on the UKF-FGO algorithm, the first target and the second target are fused to obtain a target equation, and the target equation is solved to obtain a fused positioning result of the target person indoors.

[0154] In this embodiment, since this embodiment adopts the factor graph optimization algorithm of fusion unscented Kalman filtering for fusion positioning, accordingly, in an optional embodiment, the first target and the second target are fused to obtain the target equation, and the target equation is solved to obtain the fusion positioning result of the target person in the room, which may include:

[0155] The motion data is input into the factor graph optimization algorithm fused with the unscented Kalman filter to obtain the location information of the target person output by UKF at each moment.

[0156] The location information of the target person output by UKF at each moment is input into the third program to determine the third target.

[0157] The first target, the second target and the third target are integrated to obtain the target equation, and the target equation is solved to obtain the fused positioning result of the target person in the room.

[0158] Figure 4 The factor graph optimization flow chart of the fused UKF of the PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm provided in the embodiment of the present invention is as follows. Figure 4 This embodiment is described.

[0159] In this embodiment, the method provided in this embodiment includes two processes: positioning information processing and fusion positioning. Among them, the positioning information processing can refer to step 120, and the fusion positioning can refer to steps 130-150. Steps 130 and 140 have already described the first equation (PDR constraint) and the second equation (UWB constraint). In step 150 below, the third equation (UKF constraint) and the target equation and their solution are described.

[0160] Accordingly, in this embodiment, the location information of the target person output by UKF at each moment can be expressed as:

[0161]

[0162] in, is the location information of the target person output by UKF at time k; p k The motion data input to UKF at time k; is the observation noise.

[0163] The third equation (UKF constraint) is used to describe the relationship between the state variables and the measured values ​​in the algorithm, and its error term is related to the state parameters. Therefore, the error term of the UKF vertex is defined as follows:

[0164]

[0165] Correspondingly, the third-party program can be expressed as:

[0166]

[0167] Among them, Ω ukf is the weight matrix.

[0168] After determining the three equations (constraint equations), the corresponding values ​​are input into the equations to obtain the first target, the second target and the third target; the target equation established based on the first target, the second target and the third target can be expressed as:

[0169] F(·)=F pdr (·)+F uwb (·)+F ukf (·)(29)

[0170] The target equation can be solved by the Levenberg-Marquardt algorithm to obtain the optimal estimate of the system state, and the optimal estimate is used as the fusion positioning result of the target person indoors.

[0171] In summary, the embodiment of the present invention provides a PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm. This method has clearer gait cycle information for the traditional PDR algorithm, improves the accuracy of gait detection and step length estimation, has the advantages of strong resistance to signal noise and interference, and further improves the accuracy and robustness of indoor positioning with the help of the fusion algorithm. Specifically, this embodiment uses an accelerometer, a gyroscope, and a magnetometer to collect motion data, uses a PDR algorithm to detect the gait of the target person, and estimates the step length and heading angle; in the case of small interference from factors such as non-line-of-sight, UWB positioning is performed based on the Time Difference of Arrival (TDOA) algorithm; the PDR and UWB positioning results are fused using the factor graph optimization (UKF-FGO) algorithm fused with the unscented Kalman filter to obtain the optimal positioning result of the person. It can be seen that compared with the traditional method, the embodiment of the present invention adopts two different positioning algorithms to determine two positioning results. By combining the advantages of the two, the positioning accuracy can be improved. As for the limitations between the two, a fusion algorithm is used to reprocess the two positioning results to correct the positioning data, reduce the deviation, and thus improve the accuracy of the indoor positioning results.

[0172] The effectiveness of the PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm provided in the embodiment of the present invention is verified by an example below. Figure 5-Figure 7 . Figure 5 A top view of a test site for the PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm provided in an embodiment of the present invention; Figure 6 A fusion algorithm positioning trajectory diagram of the PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm provided in an embodiment of the present invention; Figure 7 A fusion algorithm positioning error diagram of the PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm provided in an embodiment of the present invention.

[0173] exist Figure 5 In the experiment, the corridor of the selected test site is 3.6m wide and 15m long. Monitoring equipment can be set at the four corners. The target person walks from point ① to point ② at his normal speed; from point ② to point ③ at his accelerated speed; from point ③ to point ④ at his slowed speed. In this process, the movement data of the target person is collected through the monitoring equipment set at the four corners.

[0174] The results of motion data processing can be found in Figure 6 , Figure 6 The horizontal axis represents the width of the corridor, and the vertical axis represents the length of the corridor. All values ​​are positive. Point ① is the origin. Figure 6There are three different trend curves in the middle. The first section represents stages ①-②, the second section represents stages ②-③, and the third section represents stages ③-④. Figure 6 In the figure, the cross, asterisk, triangle, hollow circle and square represent PDR, UWB, UKF, factor graph route trajectory and the trajectory of this embodiment respectively, while the black line segment represents the real trajectory. The three stages ①-②, ②-③ and ③-④ are partially enlarged respectively. From the partial enlarged diagrams of the first ①-② and ②-③ stages, it can be seen that the algorithm trajectory provided by this embodiment is closer to the real trajectory than other trajectories. In the partial enlarged diagram of the ③-④ stage, the UKF trajectory is slightly better than the algorithm trajectory provided by this embodiment.

[0175] exist Figure 7 In the fusion algorithm positioning error diagram, the horizontal axis is the number of steps and the vertical axis is the error. Steps 0-22 are stages ①-②, steps 22-31 are stages ②-③, and steps 31-40 are stages ③-④. Figure 7 The black square line in the middle represents the PDR positioning error. In the ②-③ stage, due to the faster pace, the vibration frequency of the equipment increases, and the error reaches the maximum value when turning at point ②. In the ③-④ stage, due to the change in gait, the step length becomes smaller, resulting in the initial error being slightly smaller than normal speed walking. The star line represents the UWB positioning error. Since it is near the base station in the initial stage, the error is small. As it moves away from the base station, the error begins to increase, and the error reaches a maximum of 0.2m at the midpoint between point 1 and point 2. The triangle line and the circle line represent the errors of the factor graph and UKF fusion positioning, respectively. The initial error curve of UKF is better than the factor graph error curve. After 30 steps, the factor graph error curve is better than the UKF error curve. The box line represents the error of this embodiment. Compared with the errors of the factor graph and UKF fusion positioning, the curve of this embodiment has better robustness and the positioning error is reduced.

[0176] It should be understood that the order of execution of the steps in the above embodiment does not necessarily mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of the present invention.

[0177] The following is an embodiment of the device of the present invention. For details not described in detail therein, reference may be made to the corresponding method embodiment described above.

[0178] Figure 8 The structural diagram of the PDRUWB indoor fusion positioning device based on the UKF-FGO algorithm provided in an embodiment of the present invention is shown. For the convenience of explanation, only the part related to the embodiment of the present invention is shown, which is described in detail as follows:

[0179] like Figure 8 As shown, the PDRUWB indoor fusion positioning device 8 based on the UKF-FGO algorithm includes:

[0180] A collection module 81 is used to collect the movement data of the target person indoors;

[0181] A positioning module 82, configured to determine a first positioning result and a second positioning result based on the motion data; wherein the first positioning result is determined based on a PDR algorithm, and the second positioning result is determined based on a UWB algorithm;

[0182] A determination module 83, configured to input the first positioning result into a first equation to determine a first target;

[0183] The determination module 83 is further used to input the second positioning result into the second equation to determine the second target;

[0184] The fusion module 84 is used to fuse the first target and the second target based on the UKF-FGO algorithm to obtain a target equation, solve the target equation, and obtain a fusion positioning result of the target person in the room.

[0185] In a possible implementation, the fusion module 84 is specifically configured to:

[0186] The motion data is input into the factor graph optimization algorithm integrated with the unscented Kalman filter to obtain the position information of the target person output by UKF at each moment;

[0187] Input the location information of the target personnel output by UKF at each moment into the third process to determine the third target;

[0188] The first target, the second target and the third target are integrated to obtain the target equation, and the target equation is solved to obtain the fused positioning result of the target person in the room.

[0189] In a possible implementation, the third program is:

[0190]

[0191] Among them, e ukf is the error term of the UKF vertex; is the location information of the target person output by UKF at time k; p k is the motion data input by UKF at time k; Ω ukf is the weight matrix; is the observation noise.

[0192] In a possible implementation, the first positioning result includes the step length, heading angle, and pose variable vertex at each moment of the target person output by the PDR algorithm;

[0193] The second positioning result includes the coordinates corresponding to the target person at each moment output by the UWB algorithm.

[0194] In one possible implementation, the first equation is:

[0195]

[0196] Among them, e pdr is the error term of the PDR pose vertex; q k-1 is the error term and the pose variable at time k-1; q k is the pose variable at time k; Ω pdr is the weight matrix of the squared error term; L k is the change of posture at time k; x k is the horizontal coordinate of the two-dimensional position at time k; x k-1 is the horizontal coordinate of the two-dimensional position at time k-1; y k is the longitudinal coordinate of the two-dimensional position at time k; y k-1 is the longitudinal coordinate of the two-dimensional position at time k-1; θ k is the heading angle at time k; θ k-1 is the heading angle at time k-1; s k is the step size corresponding to time k; Δθ k is the change of heading angle at time k;

[0197] The second equation is:

[0198]

[0199] e uwb (u k ,B n ,d kn )=norm(B n -u k )-d kn

[0200] Among them, e uwb is the error function formed by the UWB measurement value; u k is the coordinate B of the target person at time k n is the coordinates of the nth base station; d kn is the distance from the target person to the nth base station at time k; Ω uwb is the weight matrix; norm(·) represents the modulus of the vector; norm(B n -u k ) represents the predicted value, which is used to predict the distance from the target person to the nth base station at time k;

[0201] The objective equation is:

[0202] F(·)=F pdr (·)+F uwb (·)+F ukf (·).

[0203] In a possible implementation, the motion data includes acceleration and two-dimensional position corresponding to each moment;

[0204] The positioning module 82 is specifically used for:

[0205] According to the acceleration corresponding to each moment, the gait data corresponding to each step of the target person is determined; wherein the gait data includes the starting moment, the middle moment and the acceleration corresponding to each moment of each step;

[0206] Determine the step length of the target person based on the gait data;

[0207] The acceleration and gravitational acceleration corresponding to each moment are converted into coordinate systems respectively to obtain the standard acceleration and gravitational acceleration corresponding to each moment in the b-system coordinates;

[0208] Multiply the standard acceleration and the gravity acceleration to get the standard acceleration error;

[0209] Input the standard acceleration error into the PI filter to obtain the acceleration compensation term;

[0210] Based on the acceleration compensation term, the acceleration corresponding to each moment is corrected to obtain an updated heading angle;

[0211] Based on the updated heading angle and two-dimensional position, the vertex of the pose variable at each moment is determined.

[0212] In a possible implementation, the positioning module 82 is specifically configured to:

[0213] For each moment, the electromagnetic wave flight time between the kth base station and the target person is calculated using the bilateral two-way ranging method;

[0214] Calculate the distance between the kth base station and the target person based on the electromagnetic wave flight time;

[0215] Based on the distance between the target person and a preset number of base stations, a nonlinear equation system is obtained;

[0216] Solving the nonlinear equation group at least twice and obtaining positioning errors corresponding to at least the two solutions;

[0217] When the positioning error is less than a preset threshold, the average value of the initial coordinates obtained by the at least two calculations is used as the coordinates corresponding to the target person at each moment output by the UWB algorithm.

[0218] Fig. 9 Schematic diagram of an electronic device provided by an embodiment of the present invention. Fig. 9As shown, the electronic device 9 of this embodiment includes: a processor 90, a memory 91, and a computer program 92 stored in the memory 91 and executable on the processor 90. When the processor 90 executes the computer program 92, the steps in the above-mentioned PDRUWB indoor fusion positioning method embodiment based on the UKF-FGO algorithm are implemented, for example Figure 1 Alternatively, when the processor 90 executes the computer program 92, the functions of each module / unit in the above-mentioned device embodiments are realized, for example, Figure 8 The functions of each module / unit are shown.

[0219] Exemplarily, the computer program 92 may be divided into one or more modules / units, which are stored in the memory 91 and executed by the processor 90 to implement the present invention. The one or more modules / units may be a series of computer program instruction segments capable of implementing specific functions, which are used to describe the execution process of the computer program 92 in the electronic device 9. For example, the computer program 92 may be divided into Figure 8 The modules / units shown.

[0220] The electronic device 9 may be a computing device such as a desktop computer, a notebook, a PDA, or a cloud server. The electronic device 9 may include, but is not limited to, a processor 90 and a memory 91. Those skilled in the art will appreciate that Fig. 9 It is only an example of the electronic device 9 and does not constitute a limitation of the electronic device 9. It may include more or fewer components than shown in the figure, or a combination of certain components, or different components. For example, the electronic device may also include input and output devices, network access devices, buses, etc.

[0221] The processor 90 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc.

[0222] The memory 91 may be an internal storage unit of the electronic device 9, such as a hard disk or memory of the electronic device 9. The memory 91 may also be an external storage device of the electronic device 9, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. equipped on the electronic device 9. Further, the memory 91 may also include both an internal storage unit of the electronic device 9 and an external storage device. The memory 91 is used to store the computer program and other programs and data required by the electronic device. The memory 91 may also be used to temporarily store data that has been output or is to be output.

[0223] The technicians in the relevant field can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In practical applications, the above-mentioned function allocation can be completed by different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiment can be integrated in a processing unit, or each unit can exist physically separately, or two or more units can be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of this application. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method embodiment, which will not be repeated here.

[0224] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0225] Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0226] In the embodiments provided by the present invention, it should be understood that the disclosed devices / electronic devices and methods can be implemented in other ways. For example, the device / electronic device embodiments described above are only schematic. For example, the division of the modules or units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0227] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0228] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0229] If the integrated module / unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present invention implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor, the steps of the above-mentioned PDRUWB indoor fusion positioning method embodiment based on the UKF-FGO algorithm can be implemented. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable medium may include: any entity or device that can carry the computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signal, telecommunication signal and software distribution medium.

[0230] The embodiments described above are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. Such modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be included in the protection scope of the present invention.

Claims

1. A PDRUWB indoor fusion positioning method based on UKF-FGO algorithm, characterized in that: include: Collect the target person's indoor movement data; Determine a first positioning result and a second positioning result based on the motion data; wherein the first positioning result is determined based on a PDR algorithm, and the second positioning result is determined based on a UWB algorithm; Inputting the first positioning result into a first equation to determine a first target; Inputting the second positioning result into a second equation to determine a second target; Based on the UKF-FGO algorithm, the first target and the second target are fused to obtain a target equation, and the target equation is solved to obtain a fused positioning result of the target person indoors.

2. The PDRUWB indoor fusion positioning method based on the UKF-FGO algorithm according to claim 1 is characterized in that: The step of fusing the first target and the second target to obtain a target equation, and solving the target equation to obtain a fusion positioning result of the target person indoors includes: Input the motion data into a factor graph optimization algorithm fused with an unscented Kalman filter to obtain the position information of the target person output by UKF at each moment; Input the location information of the target person outputted by UKF at each moment into the third program to determine the third target; The first target, the second target and the third target are integrated to obtain a target equation, and the target equation is solved to obtain a fused positioning result of the target person indoors.

3. The PDRUWB indoor fusion positioning method based on UKF-FGO algorithm according to claim 2 is characterized in that: The third-party procedure is: Among them, e uk f is the error term of the UKF vertex; is the location information of the target person output by UKF at time k; k is the motion data input by UKF at time k; Ω ukf is the weight matrix; is the observation noise.

4. The PDRUWB indoor fusion positioning method based on UKF-FGO algorithm according to claim 1 is characterized in that: The first positioning result includes the step length, heading angle and pose variable vertex of the target person at each moment output by the PDR algorithm; The second positioning result includes the coordinates corresponding to the target person at each moment output by the UWB algorithm.

5. The PDRUWB indoor fusion positioning method based on UKF-FGO algorithm according to claim 1 is characterized in that: The first equation is: Among them, e pdr is the error term of the PDR pose vertex; q k-1 is the error term and the pose variable at time k-1; q k is the pose variable at time k; Ω pdr is the weight matrix of the squared error term; L k is the change of posture at time k; x k is the horizontal coordinate of the two-dimensional position at time k; x k-1 is the horizontal coordinate of the two-dimensional position at time k-1; y k is the longitudinal coordinate of the two-dimensional position at time k; y k-1 is the longitudinal coordinate of the two-dimensional position at time k-1; θ k is the heading angle at time k; θ k-1 is the heading angle at time k-1; s k is the step size corresponding to time k; Δθ k is the change of heading angle at time k; The second equation is: e uwb (u k ,B n ,d kn )=norm(B n -u k )-d kn Among them, e uwb is the error function formed by the UWB measurement value; u k is the coordinate B of the target person at time k n is the coordinates of the nth base station; d kn is the distance from the target person to the nth base station at time k; Ω uwb is the weight matrix; norm(·) represents the modulus of the vector; norm(B n -u k ) represents a predicted value, which is used to predict the distance from the target person to the nth base station at time k; The objective equation is: F(·)=F pdr (·)+F uwb (·)+F ukf (·)。 6. The PDRUWB indoor fusion positioning method based on UKF-FGO algorithm according to claim 1 is characterized in that: The motion data includes acceleration and two-dimensional position corresponding to each moment; The determining a first positioning result based on the motion data includes: Determine the gait data corresponding to each step of the target person according to the acceleration corresponding to each moment; wherein the gait data includes the starting moment, the middle moment and the acceleration corresponding to each moment of each step; Determining the step length of the target person based on the gait data; The acceleration and gravitational acceleration corresponding to each moment are converted into coordinate systems respectively to obtain the standard acceleration and gravitational acceleration corresponding to each moment in the b-system coordinates; Multiplying the standard acceleration and the gravity acceleration to obtain a standard acceleration error; Inputting the standard acceleration error into the PI filter to obtain an acceleration compensation term; Correcting the acceleration corresponding to each moment based on the acceleration compensation term to obtain an updated heading angle; Based on the updated heading angle and two-dimensional position, the vertex of the pose variable at each moment is determined.

7. The PDRUWB indoor fusion positioning method based on UKF-FGO algorithm according to claim 1 is characterized in that: The determining a second positioning result based on the motion data includes: At each moment, the electromagnetic wave flight time between the kth base station and the target person is calculated using the bilateral two-way ranging method; Calculating the distance between the kth base station and the target person based on the electromagnetic wave flight time; Based on the distance between the target person and a preset number of base stations, a nonlinear equation group is obtained; Solving the nonlinear equation group at least twice and obtaining positioning errors corresponding to the at least two solutions; When the positioning error is less than a preset threshold, the average value of the initial coordinates obtained by the at least two calculations is used as the coordinates corresponding to the target person at each moment output by the UWB algorithm.

8. A PDRUWB indoor fusion positioning device based on UKF-FGO algorithm, characterized in that: include: A collection module is used to collect the movement data of the target person indoors; A positioning module, configured to determine a first positioning result and a second positioning result based on the motion data; wherein the first positioning result is determined based on a PDR algorithm, and the second positioning result is determined based on a UWB algorithm; A determination module, configured to input the first positioning result into a first equation to determine a first target; The determination module is further used to input the second positioning result into a second equation to determine a second target; The fusion module is used to fuse the first target and the second target based on the UKF-FGO algorithm to obtain a target equation, solve the target equation, and obtain a fusion positioning result of the target person indoors.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method as claimed in any one of claims 1 to 7 are implemented.

Images