A positioning and tracking method for an indoor uncertain system

Abstract

The present invention belongs to the technical field of indoor target tracking, and specifically relates to a positioning and tracking method for an indoor uncertain system. The present invention combines a model of a second-order nonlinear system under measurement noise and multi-source interference. First, a prior predicted value of the position information is obtained by the least squares method for calibrating a subsequent state estimator. Then, a state estimator based on a radial basis neural network is designed to estimate both the lumped interference suffered by the system and all states of the system, obtaining a more smooth and accurate positioning result relative to the prior prediction value. Finally, a backstepping control law is designed using the system state output by the estimator and the predicted value of the lumped interference to drive the system to track a preset trajectory. The simulation results show that the proposed method can obtain accurate and continuous positioning and tracking results under measurement noise and multi-source interference.

Description

Technical Field

[0001] The invention belongs to the technical field of indoor target tracking, and in particular relates to a positioning and tracking method for an indoor uncertain system. Background Art

[0002] The development of industry has put forward broader requirements for the application scenarios of unmanned systems, such as GNSS-denied environments such as field rescue and indoor navigation. In these complex environments, accurate location information cannot be provided by systems such as GPS and BDS in most scenarios, and a separately designed positioning system is required. With the low power consumption and simple deployment characteristics of ultra-wideband signals, a positioning system can be designed based on ultra-wideband (UWB) signals and time difference of arrival (TDOA) algorithms. However, the propagation of communication signals is inevitably affected by multipath and measurement noise, which greatly reduces the positioning results based on UWB. In particular, the introduction of high-frequency noise in a closed-loop system is not conducive to the completion of the subsequent tracking tasks of the system, and more seriously may cause the divergence of the system and fail to achieve the predetermined tracking effect. However, most of the existing indoor positioning-related results focus on positioning accuracy and ignore the continuity of positioning results. Therefore, taking into account the accuracy and continuity of positioning results is a necessary condition for enabling the system to complete subsequent tasks.

[0003] The Kalman filter can achieve the optimal estimation of the state by integrating the dynamic laws of the system and the measurement results. However, the dynamics of nonlinear systems are naturally affected by system uncertainty, modal coupling, external interference, etc. When these unknown dynamics exist, the Kalman filter cannot be used to directly predict the system state. Therefore, it is necessary to design an interference prediction and compensation mechanism while designing the positioning system to overcome this problem. Summary of the invention

[0004] The purpose of the present invention is to design a positioning and tracking system for a nonlinear system with multi-source interference and measurement noise to overcome the above shortcomings. The system adopts a radial basis function neural network (RBFNN) to adaptively predict the lumped interference of the system, compensate it and design a state predictor, and calibrate the position of the system using the TDOA signal obtained by the UWB sensor, estimate all states of the nonlinear system in real time, and obtain smoother and more accurate position information. Finally, the backstepping method is used to drive the nonlinear system to track the preset trajectory, ensuring the global consistent and stable convergence of all dynamics of the closed-loop system, ensuring the positioning and tracking accuracy while improving the feasibility of the algorithm and broadening the application scenarios.

[0005] The technical solution of the present invention is:

[0006] A positioning and tracking method for an indoor uncertain system, comprising the following steps:

[0007] S1. Model a non-linear system under multi-source interference, and use the following second-order non-linear uncertain system model:

[0008]

[0009] where x p (t) = [x p，x , x p，y T , x v (t) = [x v，x , x v，y T are respectively the displacement and velocity states of the system, represents the differential of x with respect to time, x p，x , x p，y respectively represent the displacement states of the system along the x-axis and y-axis, x v，x , x v，y represent the velocity states of the system along the x-axis and y-axis, f(x p (t), x v (t)) is the unknown system non-linear dynamics, caused by inter-modal coupling and parameter uncertainty, d(t) represents the external interference received by the system; define F(t) = f(x p (t), x v (t)) + d(t) to represent the lumped interference received by the system; m(t) is to collect the time difference of arrival TDOA signal at frequency f s , p(x p (t)) represents the ideal TDOA measurement result of the position information x p (t), n(t) is the measurement noise without prior information;

[0010] S2. Utilize the TDOA signal obtained in S1, and adopt the least square method to calculate the prior pre-estimation value of the system position to provide calibration information for the subsequent state estimator;

[0011] S3. For the unknown lumped interference F(t) in S1, utilize the universal approximation theorem of neural networks to approximate it, and at the same time design a state estimator, combine the dynamic law of the system and the prior pre-estimation value obtained in S2 for calibration, and obtain a more accurate and smooth positioning result through iterative update; the state estimator is:

[0012]

[0013] where and​​ The predicted value representing the state and respectively represent and the differential with respect to time, l p , l v is the gain of the estimator is the estimated value of the lumped disturbance using the universal approximation property of the neural network is the weight vector of the neural network is the Gaussian basis function with center c j and base width b represents the input of the neural network, m = x, y

[0014] S4. For the weight vector of the neural network in S3, design the following adaptive weight update law using the tracking error of the system:

[0015]

[0016] where represents the differential with respect to time, Γ is the adaptive gain, κ is the drift parameter for suppressing overfitting, and e(t) is the tracking error of the system

[0017] S5. Based on the predicted value output by the estimator in S3 as the input of the tracking controller, and based on the Lyapunov stability theory, adopt the backstepping control strategy to achieve uniformly bounded convergence of the system error, thereby completing the predetermined tracking task

[0018] Furthermore, in step S1, m(t) is obtained through the UWB positioning system. The specific method is as follows:

[0019] Deploy UWB target points on the controlled system and deploy more than 3 UWB anchor points in the task execution environment, and then collect the time difference of arrival TDOA signals m(t) = [m s (t), m 1 (t), …, m 2 (t)] ∈ R q ; where q represents the number of TDOA signals that can be obtained, and m q (t) represents the i-th TDOA measurement result at time t, satisfying i represents the ideal time difference when the UWB signal arrives at the i-th base station and the designated base station [x 0 , y 0 T T without noise

[0020] ​Further, in step S2, the prior estimated value of the system position is calculated by using the least square method The specific method is as follows:

[0021] It is defined that the number of base stations is greater than 3, and the coordinates of the target to be measured are x p (t)=[x p，x , x p，y T , and the coordinates of the i-th base station are [x i , y i T , where i = 0 represents the TDOA reference base station; the following error vector is established:

[0022] ∈(t)=h(t)-G(t)z(t)

[0023] where h(t) and G(t) are the measurement result and the channel matrix respectively, and are expressed as

[0024]

[0025] where c represents the speed of light, is the vector to be solved; the following result is obtained by using the least square method:

[0026] z(t)=arg min{(h(t)-Gz(t)) T (h(t)-Gz(t))}=(G T G) -1 G T h(t)

[0027] Thus, the positioning result of TDOA is obtained, that is, the prior position estimate of the target is

[0028] Further, in step S3, the specific method for approximating F(t) by using the universal approximation theorem of neural network is as follows:

[0029] The radial basis function neural network RBFNN is adopted, and the prior estimated value of the state is used as the input of the RBFNN to estimate the lumped interference as follows:

[0030]

[0031] By adjusting different weights, any continuous function can be approximated; The specific expression of

[0032]

[0033] In order to obtain a more continuous positioning result ​​The estimated value of the system's high-order unmeasurable state Based on the prior estimated value and the system interference estimated in real time while using the observation error for calibration to obtain a state estimator

[0034] Furthermore, in step S5, the specific method for completing the predetermined tracking task is as follows

[0035] For the tracking task r(t) of the predetermined trajectory, first use the tracking error of displacement to design a virtual control law α(t) to connect the displacement loop and the velocity loop, and then construct the tracking error of the velocity loop and design a feedback controller for interference compensation in combination with the estimated result of the lumped interference. The specific control law design is as follows

[0036]

[0037] where k p =[k p，x ,k p，y T ,k v =[k v，x ,k v，y T are the control gains of the displacement loop and the velocity loop respectively

[0038] The beneficial effects of the present invention are as follows: The present invention adopts a state estimator based on RBFNN and uses the TDOA positioning result for calibration to obtain a reliable and smooth positioning effect. Then, based on the output of the estimator, a backstepping tracking control law is designed to complete the positioning and tracking tasks. This method can be widely applied to dynamic environments with GNSS denial, multi-source interference, and measurement noise, and has high engineering significance Description of the Drawings

[0039] Figure 1 is the block diagram of the positioning and tracking system based on the state estimator

[0040] Figure 2 is the density distribution of the measurement noise

[0041] Figure 3 is the positioning and tracking effect diagram

[0042] Figure 4 is the comparison diagram of the state prediction between the background technology method and the method of the present invention Detailed Embodiments

[0043] The technical solutions of the present invention will be described in detail below in conjunction with the drawings and embodiments ​​

[0044] Step 1. Model the nonlinear system under multi-source interference, using the following second-order nonlinear uncertain system:

[0045]

[0046] where, x p (t) = [x p，x , x p，y T , x v (t) = [x v，x , x v，y T are the displacement and velocity states of the system respectively, represents the differentiation of x with respect to time, x p，x , x p，y represent the displacement states of the system along the x-axis and y-axis respectively, x v，x , x v，y represent the velocity states of the system along the x-axis and y-axis. f(x p (t), x v (t)) is the unknown system nonlinear dynamics, usually caused by inter-modal coupling and parameter uncertainty. d(t) represents the external interference received by the system. Define F(t) = f(x p (t), x v (t)) + d(t) to represent the lumped interference received by the system. Many practical systems, such as mechanical systems and pendulum systems, are modeled as formula (1).

[0047] Use the UWB positioning system to assist in obtaining state information. Deploy UWB target points on the controlled system, and deploy a certain number (more than 3) of UWB anchor points in the task execution environment. Then collect the time difference of arrival (TDOA) signals m(t) = [m s (t), m 1 (t), …, m 2 (t)] ∈ R q at frequency f q . Where q represents the number of TDOA signals that can be obtained. Where m i (t) represents the i-th TDOA measurement result at time t, satisfying represents the ideal time difference when the UWB signal arrives at the i-th base station and the designated base station [x 0 , y 0 T without noise. p(x p (t)) represents the ideal TDOA measurement result of the position information x p (t). n(t) = [n 1 (t), n 2 (t), …, n q ​​​(t)] T is the measurement noise without prior information.

[0048] The objective of the present invention is to accurately and continuously estimate the state x p (t), x v (t) of the system in (1), and then design a controller based on the estimated value to drive the system to accurately track the preset reference trajectory r(t).

[0049] Step 2. Estimate the system position using the least squares method. Specifically, taking two-dimensional coordinates as an example, when the number of base stations is more than 3, the coordinates of the target to be measured are x p (t) = [x p，x , x p，y T , and the coordinates of the i-th base station are [x i , y i T , where i = 0 represents the TDOA reference base station. Establish the following error vector

[0050] ∈(t) = h(t) - G(t)z(t) (3)

[0051] where h(t) and G(t) are the measurement result and the channel matrix respectively, and are specifically expressed as

[0052]

[0053] where c represents the speed of light, is the vector to be solved. Solve using the least squares method

[0054] z(t) = arg min{(h(t) - Gz(t)) T (h(t) - Gz(t))} = (G T G) -1 G T h(t) (4)

[0055] Thus, the positioning result of TDOA, that is, the prior position estimate of the target, is

[0056] Step 3. Design of the state estimator

[0057] For the unknown time-varying lumped interference in the system in Step 1, the present invention uses the estimated value of the state as the input of the radial basis function neural network RBFNN, and uses the universal approximation of the neural network to estimate the lumped interference as follows:

[0058]

[0059] where ​​is the weight vector. By adjusting different weights, (5) can approximate any continuous function. is a Gaussian function with center c j and base width b, which can be expressed as

[0060]

[0061] where represents the input of the neural network. To obtain a more continuous positioning result and the predicted value of the system's high-order unmeasurable state Based on the prior predicted value and the system interference estimated in real time while using the observation error for calibration, design the following state predictor:

[0062]

[0063] where and represent the predicted values of the state, and respectively represent and the differentials with respect to time, l p , l v is the gain of the predictor.

[0064] Step 4. Online adaptive update of neural network weights

[0065] To achieve the online accurate estimation of the lumped interference in the above steps, an appropriate weight vector needs to be selected. Using the differential characteristics of the system feedback loop, the present invention adaptively updates the weights using the tracking error of the system as follows:

[0066]

[0067] where, represents the differential with respect to time, Γ is the adaptive gain, and κ is the drift parameter for suppressing overfitting. e(t) is the tracking error of the system. The differential equation (8) can adaptively update the value in real time, thus achieving the online approximation of the lumped interference in (5).

[0068] Step 5. Based on the predicted value obtained in Step 3 design a backstepping controller to complete the tracking task of the predetermined trajectory r(t). First, use the tracking error of the displacement to design the virtual control law α(t) to connect the displacement loop and the velocity loop, and then construct the tracking error of the velocity loop Design a feedback controller for interference compensation based on the estimated result of the total interference of the union set. The specific control law design is as follows

[0069]

[0070] where k p = [k p，x , k p，y T , k v = [k v，x , k v，y T are the control gains of the displacement loop and the velocity loop respectively.

[0071] The block diagram of the positioning and tracking system based on the state estimator constructed by the above method is as Figure 1 shown.

[0072] Example

[0073] This example is tested based on the MATLAB / SIMULINK simulation platform to obtain the positioning and tracking curves of the nonlinear system, and compare them with the state curves without using the state estimator, so as to verify the effectiveness and superiority of the proposed positioning and tracking method for indoor uncertain systems. The specific implementation process is given below

[0074] In a 30m * 30m indoor environment, 8 UWB anchors are deployed at the four corners and the midpoints of the four sides. At the same time, a UWB target point is deployed on the target system to receive TDOA signals. Design a feedback compensation controller based on the estimated state and interference under the condition of mixed measurement noise and multi-source interference, and drive the target to track an "8" - shaped trajectory. The TDOA measurement results of UWB contain ST noise, and the distribution of the noise is as Figure 2 shown.

[0075] In the example, the external interference and the nonlinear dynamics are set as

[0076]

[0077] The reference trajectory for the desired target tracking is r(t) = [10sin(4.17t), 12sin(8.2t)] T . The controller gains of the displacement loop and the velocity loop are set as k p = [12, 28] T , k v = [150, 120] T . The adaptive gain of the neural network is Γ = [1.2e 7 , 1.2e 7 T , and the drift parameter is κ = [2, 2]​​​T , the parameters of the estimator are set to l p = 28, l v = 38.

[0078] Based on the above parameters, by using the positioning and tracking method in the present invention, the positioning and tracking trajectories as shown in Figure 3 can be obtained. In addition, the comparison effect with the method without using the state estimator is as shown in Figure 4 . Obviously, although the positioning accuracies are similar, there are small high-frequency components in the method without using the state observer, and huge errors will be generated after differentiation when solving high-order states, making it difficult to apply to subsequent tracking feedback tasks. On the contrary, in the method of the present invention, although there is a mixed environment with multi-source interference and measurement noise coexisting, with the help of an effective compensation mechanism and the design of the estimator, we can obtain accurate and continuous positioning and tracking results, greatly improving the feasibility of the algorithm and broadening the application scenarios.

Claims

1. A positioning and tracking method for an indoor uncertain system, characterized in that, it includes the following steps: S1. Model the nonlinear system under multi-source interference, and use the following second-order nonlinear uncertain system model: where, x p (t) = [x p，x , x p，y T , x v (t) = [x v，x , x v，y T are the displacement and velocity states of the system, respectively, represents the differentiation of x with respect to time, x p，x , x p，y represent the displacement states of the system along the x-axis and y-axis, respectively, x v，x , x v，y represent the velocity states of the system along the x-axis and y-axis, f(x p (t), x v (t)) is the unknown system nonlinear dynamics, caused by the coupling between modes and parameter uncertainties, d(t) represents the external disturbance received by the system; define F(t) = f(x p (t), x v (t)) + d(t) to represent the lumped disturbance received by the system; m(t) is to collect the time difference of arrival TDOA signal at frequency f s , p(x p (t)) represents the ideal TDOA measurement result of the position information x p (t), n(t) is the measurement noise without prior information;​​ S2. Using the TDOA signals obtained in S1, calculate the prior pre-estimated value of the system position by using the least squares method to provide calibration information for the subsequent state estimator; S3. For the unknown lumped interference F(t) in S1, use the universal approximation theorem of neural networks to approximate it. At the same time, design a state estimator, and combine the dynamic laws of the system and the prior estimate value obtained in S2 Obtain a more accurate and smooth positioning result through an iterative update method; the state estimator is as follows: where and represent the predicted values of the state, and represent respectively and the differentials with respect to time, l p , l v is the gain of the estimator, is the estimated value of the lumped disturbance using the universal approximation property of the neural network, is the weight vector of the neural network, is a Gaussian basis function centered at c j with a basis width of b; S4. For the weight vector of the neural network in S3 Design the following adaptive weight update law using the tracking error of the system: wherein, denotes the differentiation with respect to time, Γ is the adaptive gain, κ is the drift parameter for suppressing overfitting, and e(t) is the tracking error of the system; S5. Based on the estimated value output by the estimator in S3 As the input of the tracking controller, based on the Lyapunov stability theory, the backstepping control strategy is adopted to achieve uniformly bounded convergence of the system error, thereby completing the predetermined tracking task.

2. According to the positioning and tracking method for an indoor uncertain system described in claim 1, characterized in that, in step S1, m(t) is obtained through the UWB positioning system, and the specific method is: Deploy UWB targets on the controlled system and deploy more than 3 UWB anchors in the environment where the task is executed, and then at frequency f s Collect the time difference of arrival TDOA signal m(t) = [m 1 (t), m 2 (t),..., m q (t)] ∈ R q : where q represents the number of TDOA signals that can be obtained, and m i (t) represents the i-th TDOA measurement result at time t, satisfying represents the ideal time difference when the UWB signal arrives at the i-th base station and arrives at the specified base station [x 0 , y 0 T .​ 3. According to the positioning and tracking method for an indoor uncertain system described in claim 2, characterized in that, In step S2, the prior estimated value of the system position is calculated by using the least square method The specific method is as follows: It is defined that the number of base stations is greater than 3, and the coordinates of the target to be measured are x p (t) = [x p，x , x p，y T , the coordinates of the i-th base station are [x i , y i T , when i = 0, it represents the TDOA reference base station; establish the following error vector:​​ ∈(t) = h(t) - G(t)z(t) where h(t) and G(t) are the measurement result and the channel matrix respectively, expressed as where c represents the speed of light, is the vector to be solved; the following results are obtained using the least squares method: z(t) = argmin{(h(t) - Gz(t)) T (h(t) - Gz(t))} =(G T G) -1 G T h(t) Thus, the positioning result of TDOA is obtained, that is, the prior position estimate of the target is 4. According to the positioning and tracking method for an indoor uncertain system described in claim 3, characterized in that, in step S3, the specific method for approximating F(t) using the universal approximation theorem of neural networks is: Adopt a radial basis function neural network RBFNN, use the predicted value of the state as the input of the RBFNN, and estimate the lumped interference as follows: By adjusting different weights, it can approximate any continuous function; The specific expression of To obtain a more continuous positioning result and the estimated value of the system's high-order unmeasurable state Based on the prior estimated value and the system interference estimated in real time At the same time, use the observation error for calibration to obtain a state estimator.

5. According to the positioning and tracking method for an indoor uncertain system described in claim 3, characterized in that, in step S5, the specific method for completing the predetermined tracking task is: For the tracking task r(t) of a predefined trajectory, first, the tracking error of displacement is used to design a virtual control law α(t) to connect the displacement loop and the velocity loop. Then, the tracking error of the velocity loop is constructed and a feedback controller for disturbance compensation is designed based on the estimation result of the lumped disturbance. The specific control law design is as follows: where k p = [k p，x , k p，y T , k v = [k v，x , k v，y T are the control gains of the displacement loop and the velocity loop, respectively.​​

Images