Method for positioning unmanned system in closed space under external interference

Abstract

The invention discloses a closed space unmanned system positioning method under external interference, and relates to the technical field of unmanned system positioning. According to the invention, a detection mechanism is designed by using a dimension-expanded mark signal to accurately identify abnormal interference, and accurate positioning estimation is carried out on the unmanned system by using forward filtering and backward smoothing based on generalized statistics. According to the method, high-precision positioning of the unmanned system can be realized in a complex scene in which the error characteristic of the wireless measurement signal shows that the non-Gaussian characteristic is coupled with unknown interference, the influence of unknown external disturbance and non-Gaussian noise on the positioning effect is reduced, and the positioning precision of the unmanned system in closed limited spaces such as a room, an underground space and a basement is improved; normal operation of the unmanned system is guaranteed, and task goals are achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned system positioning, and particularly relates to a positioning method for an unmanned system in a closed space under external interference. Background Art

[0002] When mining and other operation tasks are carried out in closed spaces such as underground and indoor, the geological environment faced is extremely complex. Wireless positioning signals will undergo reflection, refraction, and multipath phenomena, making the error characteristics of sensors no longer follow the standard Gaussian distribution. In addition, in a complex adversarial environment, wireless communication and wireless measurement are vulnerable to malicious attacks or interference from opponents, resulting in false data in the signals received by signal sensors and making them no longer reliable. It may even intermittently interrupt the measurement signal, deteriorating the positioning effect of the unmanned system. Therefore, it is particularly important to invent a positioning method for an unmanned system in a closed space under external interference.

[0003] The prior art such as the publication number: CN113324547 discloses a multi-AUV cooperative positioning method based on an iterative extended RTS smoothing filtering algorithm. This method uses the RTS optimal smoothing algorithm to correct the result of the forward extended Kalman filter, which is simple and easy to implement, and effectively improves the navigation and positioning accuracy of the following AUV. However, this method approximates the process noise and measurement noise as zero-mean Gaussian white noise, and also assumes that the transmission of the communication channel is accurate without anomalies. This is not applicable to the complex situation where the process noise and measurement noise in a closed underground space do not follow the Gaussian distribution and are very likely to be cross-coupled with network attacks.

[0004] The prior art such as the publication number: CN115615433 discloses a hybrid positioning method and system based on the extended Kalman and RTS smoothing algorithms. This system uses the RTS smoothing algorithm to smooth the position of the robot in this direction according to the change error of the system position, and takes the average of the smoothing results to obtain the optimal position estimate in this direction, effectively improving the estimation accuracy when the local direction is in a static state. However, in a complex environment with signal interference, the measurement error of the position will increase, and the best smoothed position fitting result cannot be obtained. In addition, only single Gaussian white noise is considered, without considering the non-Gaussian characteristics of sensor measurement errors in a complex restricted environment.

[0005] The prior art, such as the one with the publication number CN118921622A, discloses a method for fusing indoor positioning based on RTS-smoothed UWB and IMU. This method utilizes the complementary characteristics of UWB and IMU and adopts the extended Kalman filter to fuse positioning information. On this basis, RTS smoothing is used to more accurately estimate the system state, overcoming the limitations of a single positioning system in a closed environment and the excessive positioning error. However, in a complex closed and restricted environment, in addition to problems such as sensor offset and cumulative integration error that can lead to a decrease in positioning accuracy, the influence of non-Gaussian noise and the interference of network attacks will also cause a large position deviation when estimating an unmanned system using the positioning method.

[0006] The prior art has the following defects, specifically manifested in: 1. When the process noise and measurement noise are non-Gaussian noises and are cross-coupled with unknown external interferences, it is difficult for the prior art to achieve precise detection and forward filtering positioning. This results in inaccurate positioning of the unmanned system in a complex environment, causing frequent problems such as path planning errors and deviation from the target when the unmanned system performs tasks, making it unable to operate stably and limiting its application in some special scenarios.

[0007] 2. The prior art has limitations in terms of positioning principles and algorithms, lacking optimization means such as robust RTS smoothing technology that can effectively handle complex data and noise interference. When performing filtering positioning, its processing method for measurement data is relatively conventional, and it is unable to accurately extract accurate position information from the original data containing a large amount of noise and interference. As a result, problems such as positioning deviation and repeated calibration frequently occur during the implementation of related work, seriously affecting work efficiency and increasing labor and material costs. Summary of the Invention

[0008] The purpose of the present invention is to provide a positioning method for an unmanned system in a closed space under external interference, which solves the problems existing in the background technology.

[0009] To solve the above technical problems, the present invention adopts the following technical solutions: The present invention provides a positioning method for an unmanned system in a closed space under external interference, including: Step 1, constructing a model. Considering multi-source external interferences, introducing a flag signal that conforms to the Gaussian distribution, and constructing a state space model for the positioning problem of the unmanned system in the closed space.

[0010] Step 2, interference detection. Based on time update, obtain the predicted value of the system state at time k - 1 and the predicted error covariance P k|k-1 ; calculate the innovation of the flag signal using the one-step prediction information, and detect whether there is interference according to the chi-square statistic of the innovation, and then take corresponding measures according to the detection result.

[0011] Step 3: Measurement update. Use the proposed generalized statistic to measure the similarity between the expected quantity and the actual quantity, and calculate the system state variables and the covariance matrix P k|k for posterior estimation results, and perform state updates.

[0012] Step 4: Backward smoothing. Use the estimation results of forward filtering to calculate the recursive estimation results of the system state and P k|N under the proposed generalized statistic-based framework to obtain the corrected positioning results of the unmanned system.

[0013] Preferably, for the said Step 1: Model construction, the specific method is as follows: Step 1-1: To model the positioning problem of an unmanned system in an actual underground enclosed space under unknown disturbances, introduce binary variables and δ k indicating whether a denial of service and false data interference occur, and accordingly obtain a standard state-space model:

[0014] x k = F k x k-1 + w k ;

[0015]

[0016] where x k represents the system state at time k, z k represents the measurement value of the sensor at time k, F k and H k represent the state transition matrix and the observation matrix, the process noise w k and the measurement noise v k are independent of each other and the corresponding nominal covariance matrices are Q k and R k respectively, ω k represents the injected false data interference, and the probability of its occurrence and the probability of the denial of service disturbance are both unknown quantities. Without loss of generality, the false data interference is modeled as non-Gaussian noise with a mean of zero and a covariance matrix η k .

[0017] Step 1-2: Introduce a flag signal u k independent of the measurement noise as an extended term of the measurement model, with a mean of zero and a variance matrix B k . At this time, the pseudo-measurement model considering multi-source disturbances is expressed as:

[0018]

[0019] Among them, the pseudo-measurement matrix H j,k is the same as H k and ω j,k represents the unknown external interference.

[0020] Preferably, for the said Step 2, interference detection, the specific method is as follows: Step 2-1: According to the positioning result of the unmanned system at the previous moment and P k-1|k-1 perform time update:

[0021]

[0022] P k|k-1 = F k P k-1|k-1 F k T + Q k ;

[0023] Among them, and P k|k-1 are respectively the one-step state prediction and its prediction error covariance matrix of the system at the k-th moment.

[0024] Step 2-2: Calculate the chi-square criterion of the innovation based on the expansion term according to the prediction result, and perform outlier detection by comparing with the threshold:

[0025]

[0026] Among them, γ k is the residual based on the expansion term, is the expected covariance information when the sensor is not affected by external interference.

[0027] Step 2-3: Standardize the innovation according to the chi-square distribution, introduce the chi-square threshold with a confidence level of ζ, and judge whether there is interference by comparing with the threshold:

[0028]

[0029] Among them, θ is the threshold obtained from the chi-square distribution table according to the degrees of freedom of the measurement dimension and the confidence level.

[0030] Preferably, for the said Step 3, measurement update, the specific method is as follows: Step 3-1: Define the generalized statistic:

[0031]

[0032] Among them, β1 and β2 are the mixing coefficients of the user-defined kernel functions, and σ and ε are the kernel bandwidth and the degrees of freedom parameter respectively.

[0033] Step 3-2: Define the cost function according to the generalized statistic, and use the gradient descent solution to maximize the lower bound of the above cost function. Then, use the fixed-point iteration algorithm to obtain the optimal posterior estimate of the forward filtering:

[0034]

[0035] where and P k|k are the state posterior estimate and its estimated error covariance matrix, while K k is the forward filtering gain, and its expression is as follows:

[0036]

[0037] where B p,k|k-1 and B v,k are the Cholesky decompositions of the prediction covariance and the measurement noise covariance, I nx is the identity matrix with the same dimension as the state vector, and Π k is the adjustment matrix.

[0038] Preferably, the step 4, backward smoothing, is specifically as follows: Step 4-1: Input the initial value of the forward filtering P k|k .

[0039] Step 4-2: Define the cost function according to the generalized statistic, and use the fixed-point iteration to solve it to obtain and output the optimal smoothing estimation result:

[0040]

[0041] where and P k|N are the smoothing estimation value and its covariance information at the current moment, while G k is the smoothing gain, and its expression is as follows:

[0042]

[0043] where B w,k|k+1 and B p,k|k are respectively obtained by the Cholesky decompositions of Q k+1 and P k|k , Θ k+1 is the adjustment weight matrix of the smoothing estimation, and β is the mixing coefficient of the generalized statistic.

[0044] The beneficial effects of the present invention are as follows: 1. The positioning method of the unmanned system in a closed space based on RTS smoothing under external interference proposed by the present invention can still achieve precise detection and forward filtering positioning in a complex and harsh environment where the process noise and measurement noise are non-Gaussian noises and are cross-coupled with unknown external interference. Furthermore, the robust RTS smoothing technology is used to further improve the accuracy of filtering positioning, which can meet the application requirements with extremely high positioning accuracy, such as high-precision positioning of equipment underground. This enables the unmanned system to stably and accurately determine its position in a challenging environment, greatly expanding its application scenarios. Description of the Drawings

[0045] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0046] Figure 1 It is a flowchart of the method steps proposed by the present invention.

[0047] Figure 2 It is a comparison diagram of the position average root mean square error between the method proposed by the present invention and the existing maximum entropy filtering and its smoothing method and the classical Kalman filtering and its smoothing method.

[0048] Figure 3 It is a comparison diagram of the position average root mean square error between the method proposed by the present invention and the existing maximum entropy filtering and its smoothing method and the classical Kalman filtering and its smoothing method.

[0049] Figure 4 It is a diagram of the actual application scenario of the present invention. Detailed Embodiments

[0050] The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the drawings in the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, rather than all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0051] Refer to Figure 1 As shown, the present invention provides a positioning method for an unmanned system in a closed space under external interference, including: Step 1: Build a model. Considering multi-source external interference, introduce a flag signal that conforms to the Gaussian distribution, and build a state space model for the positioning problem of the unmanned system in the closed space.

[0052] In a specific embodiment, the step 1, constructing a model, is specifically as follows: Step 1-1: To model the positioning problem of an actual underground closed-space unmanned system under unknown disturbances, a binary variable and δ k are introduced to represent whether a denial-of-service and false data interference occur, and based on this, a standard state-space model is obtained:

[0053] x k = F k x k-1 + w k ;

[0054]

[0055] where x k represents the system state at time k, z k represents the measurement value of the sensor at time k, F k and H k represent the state transition matrix and the observation matrix, the process noise w k and the measurement noise v k are independent of each other and the corresponding nominal covariance matrices are Q k and R k , ω k represents the injected false data interference, the probability of its occurrence and the probability of the denial-of-service perturbation are both unknown quantities. Without loss of generality, the false data interference is modeled as a non-Gaussian noise with a mean of zero and a covariance matrix η k .

[0056] Step 1-2: An indicator signal u k independent of the measurement noise is introduced as an extended term of the measurement model, with a mean of zero and a variance matrix B k . At this time, the pseudo-measurement model considering multi-source perturbations is expressed as:

[0057]

[0058] where the pseudo-measurement matrix H j,k is the same as H k , and ω j,k represents the unknown external interference.

[0059] Step 2, interference detection: Based on time update, the predicted value of the system state at time k-1 and the prediction error covariance P k|k-1 are obtained; the innovation of the indicator signal is calculated using the one-step prediction information, and whether there is interference is detected based on the chi-square statistic of the innovation, and then corresponding measures are taken according to the detection result.

[0060] In a specific embodiment, in step 2, interference detection is performed as follows: Step 2-1: According to the positioning result of the unmanned system at the previous moment and P k-1|k-1 perform time update:

[0061]

[0062] P k|k-1 = F k P k-1|k-1 F k T + Q k ;

[0063] wherein, and P k|k-1 are respectively the one-step state prediction and its prediction error covariance matrix of the system at time k.

[0064] Step 2-2: Calculate the innovation chi-square criterion based on the extended term according to the prediction result, and perform outlier detection by comparing with the threshold:

[0065]

[0066] where γ k is the residual based on the extended term, is the expected covariance information when the sensor is not affected by external interference.

[0067] It should be noted that the innovation refers to the difference between the actual observation value and the predicted observation value in the Kalman filter, reflecting the new information in the observation.

[0068] Step 2-3: Standardize the innovation according to the chi-square distribution, introduce the chi-square threshold with a confidence level of ζ, and judge whether there is interference by comparing with the threshold:

[0069]

[0070] where θ is the threshold obtained from the chi-square distribution table according to the degrees of freedom of the measurement dimension and the confidence level.

[0071] Step 3, measurement update, use the proposed generalized statistic to measure the similarity between the expected quantity and the actual quantity, calculate the posterior estimation result of the system state variable and the covariance matrix P k|k and perform state update.

[0072] In a specific embodiment, in step 3, measurement update is performed as follows: Step 3-1: Define the generalized statistic:

[0073]

[0074] Among them, β1 and β2 are user-defined kernel function mixing coefficients, and σ and ε are the kernel bandwidth and degrees of freedom parameters respectively.

[0075] It should be noted that the kernel function mixing coefficients are the coefficients used when linearly combining multiple kernel functions to form a mixed kernel function. These coefficients determine the proportion of each kernel function in the mixed kernel function. By adjusting the mixing coefficients, the advantages of different kernel functions can be combined, enabling the mixed kernel function to better adapt to various complex data distributions and problem types. The kernel bandwidth is a key parameter in the kernel function, which determines the "width" or "smoothness" of the kernel function. The degrees of freedom parameter usually appears in some kernel functions or statistical models with specific distributions, used to describe the shape and characteristics of the distribution, and to adjust the complexity and robustness of the model.

[0076] Step 3-2: Define the cost function according to the generalized statistic, and use the gradient descent solution method to maximize the lower bound of the above cost function, and use the fixed-point iteration algorithm to obtain the optimal posterior estimate of the forward filtering:

[0077]

[0078] Among them, and P k|k are the state posterior estimate and its estimated error covariance matrix, while K k is the forward filtering gain, and its expression is as follows:

[0079]

[0080] Among them, B p,k|k-1 and B v,k are the Cholesky decompositions of the prediction covariance and the measurement noise covariance, I nx is the identity matrix with the same dimension as the state vector, and Π k is the adjustment matrix.

[0081] Step 4, backward smoothing: Using the estimation result of the forward filtering, calculate the recursive estimation result of the system state under the proposed generalized statistic-based framework and P k|N , and obtain the corrected positioning result of the unmanned system.

[0082] In a specific embodiment, the step 4, backward smoothing, is specifically as follows: Step 4-1: Input the initial value of the forward filtering P k|k .

[0083] Step 4-2: Define a cost function based on the generalized statistic and solve it using fixed-point iteration to obtain and output the optimal smoothing estimation result:

[0084]

[0085] where and P k|N are the smoothing estimation value and its covariance information at the current moment, while G k is the smoothing gain, and its expression is as follows:

[0086]

[0087] where B w,k|k+1 and B p,k|k are obtained by Cholesky decomposition of Q k+1 and P k|k respectively, Θ k+1 is the adjustment weight matrix of the smoothing estimation, and β is the mixing coefficient of the generalized statistic.

[0088] It should be noted that the Cholesky decomposition is a method of decomposing a positive definite matrix into the product of a lower triangular matrix and its conjugate transpose matrix.

[0089] The proposed method for positioning an unmanned system in a closed space based on RTS smoothing under external interference can still achieve accurate detection and forward filtering positioning in a complex and harsh environment where the process noise and measurement noise are non-Gaussian noises and are also cross-coupled with unknown external interference. Furthermore, the use of robust RTS smoothing technology further improves the accuracy of filtering positioning, which can meet the application requirements with extremely high positioning accuracy, such as high-precision positioning of equipment underground. This enables the unmanned system to stably and accurately determine its position in a challenging environment, greatly expanding its application scenarios.

[0090] The two-dimensional plane positioning of an unmanned system based on range and velocity sensors is selected to verify the effectiveness of the method proposed in this invention.

[0091] The state transition matrix is selected as:

[0092]

[0093] where T = 1s is the sampling time, I2 is the 2×2 identity matrix, and 02 is the 2×2 zero matrix.

[0094] The observation matrix of the linear system is selected as:

[0095] H k = [I2 02],

[0096] H j,k = [I2 02].

[0097] The system state vector is selected as:

[0098] x k =[p x,k p y,k v x,k v y,k T ,

[0099] where (p x,k , p y,k ) and (v x,k , v y,k ) are the positions and velocities in the x and y directions respectively.

[0100] The initial state is selected as:

[0101]

[0102] The initial covariance matrix is selected as:

[0103] P0 = diag[100m 2 , 100m 2 , 10m 2 s -2 , 10m 2 s -2 T .

[0104] The average root mean square error (ARMSE) is used to calculate the estimation errors of the method proposed in the present invention, the existing maximum entropy filtering and its smoothing method, the existing variational Bayesian smoothing method based on the student t-distribution, and the classical (extended) Kalman filtering and its smoothing method. The calculation formula for the average root mean square error of the position is:

[0105]

[0106] where the calculation formula for RMSE pos (k) is:

[0107]

[0108] where the subscript s is the index of the Monte Carlo simulation experiment, is the position detected by sensor k estimation of, the number of Monte Carlo simulation experiments M c = 500, and the calculation method of the average root mean square error ARMSE vel of the velocity is similar to the above method.

[0109] Reference Figure 2 、 3 ​​, respectively, the position average root mean square error and the velocity average root mean square error obtained by using the method proposed by the present invention, the existing maximum entropy filtering and its smoothing method, and the classical Kalman filtering and its smoothing method in a simulated closed complex scenario are given. Through comparison, it shows that the method proposed by the present invention is significantly more accurate than other existing methods.

[0110] Reference Figure 4 , in a closed underground roadway, the unmanned system uses sensors to measure information such as the distance and position from the tags on the equipment, and estimates and predicts the data according to the filtering algorithm proposed by the present invention to achieve precise positioning, thereby obtaining the true motion information of the unmanned system.

[0111] The above content is only an example and explanation of the concept of the present invention. Those skilled in the art of the present technology can make various modifications or supplements to the described specific embodiments or use similar methods for substitution, as long as they do not deviate from the concept of the invention or exceed the scope defined by the present invention, they should all fall within the protection scope of the present invention.

Claims

1. A closed space unmanned system positioning method under external interference, characterized in that: include: Step 1: Build a model, consider multi-source external interference, introduce a marker signal that conforms to the Gaussian distribution, and build a state space model for the positioning problem of unmanned systems in closed spaces; Step 2: Interference detection, based on time update, obtain the predicted value of the system state at time k-1 and the prediction error covariance P k|k-1 ; Use the one-step prediction information to calculate the new information of the marker signal, and detect whether there is interference based on the chi-square statistic of the new information, and then take corresponding measures based on the detection results; Step 3: Measurement update: use the proposed generalized statistics to measure the similarity between the expected quantity and the actual quantity and calculate the system state variable With the covariance matrix P k|k The posterior estimation result is used to update the state; Step 4: Backward smoothing: Using the estimation results of forward filtering, the recursive estimation results of the system state are calculated in the proposed framework based on generalized statistics. and P k|N , and obtain the corrected positioning result of the unmanned system.

2. The method for positioning an unmanned system in a closed space under external interference according to claim 1, characterized in that: The step 1, constructing the model, is specifically as follows: Step 1-1: In order to model the positioning problem of an unmanned system in an actual underground closed space with unknown interference, a binary variable that follows the Bernoulli distribution is introduced. and δ k Indicates whether denial of service and false data interference occur, and the standard state space model is obtained based on this: x k =F k x k-1 +w k ; Among them, x k represents the system state at time k, z k represents the measurement value of the sensor at time k, F k and H k represents the state transfer matrix and observation matrix, process noise w k and the measurement noise v k are independent of each other and their corresponding nominal covariance matrices are Q k and R k ,ω k Indicates the probability of injected false data interference and the probability of a denial of service disturbance All are unknown quantities. To avoid loss of generality, the false data interference is modeled as a covariance matrix η with a mean of zero k Non-Gaussian noise; Step 1-2: Introduce a marker signal u that is independent of the measurement noise k As an extension of the measurement model, its mean is the zero variance matrix B k , the pseudo-measurement model considering multi-source disturbances is expressed as: The pseudo measurement matrix H j,k With H k Same, ω j,k Indicates unknown external interference.

3. The method for positioning an unmanned system in a closed space under external interference according to claim 1, characterized in that: The specific method of step 2, interference detection, is as follows: Step 2-1: Based on the positioning result of the unmanned system at the last moment and P k-1|k-1 To update the time: P k|k-1 =F k P k-1|k-1 F k T +Q k ; in, and P k|k-1 They are the one-step state prediction of the system at time k and its prediction error covariance matrix; Step 2-2: Calculate the chi-square standard based on the prediction results and compare it with the threshold value to detect outliers: Among them, γ k is the residual based on the expansion term, It is the expected covariance information of the sensor without external interference; Step 2-3: Standardize the new information according to the chi-square distribution, introduce a chi-square threshold with a confidence level of ζ, and determine whether there is interference by comparing with the threshold: Here, θ is the threshold value obtained from the chi-square distribution table according to the degrees of freedom and confidence level of the measurement dimension.

4. The method for positioning an unmanned system in a closed space under external interference according to claim 1, characterized in that: The specific method of step 3, measurement update, is as follows: Step 3-1: Define generalized statistics: Among them, β1 and β2 are the user-defined kernel function mixing coefficients, σ ​​and ε are the kernel bandwidth and degree of freedom parameters respectively; Step 3-2: Define the cost function based on the generalized statistics, and use the gradient descent method to maximize the lower limit of the above cost function, and use the fixed point iteration algorithm to obtain the optimal posterior estimate of the forward filtering: in, and P k|k is the state posterior estimate and its estimation error covariance matrix, and K k is the forward filter gain, which is expressed as follows: Among them, B p,k|k-1 and B v,k is the Cholesky decomposition of the prediction covariance and the measurement noise covariance, I nx is the identity matrix of the same dimension as the state vector, Π k is the adjustment matrix.

5. The method for positioning an unmanned system in a closed space under external interference according to claim 1, characterized in that: The specific method of step 4, backward smoothing, is as follows: Step 4-1: Input the initial value of the forward filter P k|k ; Step 4-2: Define the cost function based on the generalized statistics, and use the fixed point iteration to obtain and output the optimal smoothing estimate result: in, and P k|N is the smoothed estimate and its covariance information at the current moment, and G k is the smoothing gain, and its expression is as follows: Among them, B w,k|k+1 and B p,k|k By Q k+1 and P k|k Cholesky decomposition gives, Θ k+1 is the adjustment weight matrix of the smoothing estimate, and β is the mixing coefficient of the generalized statistics.

Images