A mobile robot rolling horizon estimation method fusing threshold filtering

Abstract

The application discloses a mobile robot rolling horizon estimation method fusing threshold filtering, which comprises the following steps: 1) establishing a mobile robot system model; 2) designing a measurement equation; 3) processing abnormal measurement data; 4) establishing a cost function; 5) establishing a constraint condition; and 6) designing a rolling horizon estimator. The application introduces an adaptive scale factor s to correct the abnormal measurement data, and then performs rolling horizon estimation and solves an optimization problem, so that the influence of external interference on state estimation of the mobile robot car during movement is effectively reduced, and the combined positioning capacity for a nonlinear system is further improved.

Description

Technical Field

[0001] The present invention relates to a state estimation problem of a mobile robot vehicle system, and in particular to a rolling time domain estimation method for a mobile robot system in the presence of a large amount of external interference. Background Art

[0002] With the continuous advancement of science and control technology, mobile robots have been widely used in scientific research, military, industrial, civil, and logistics fields. State estimation of mobile robot systems in the face of extensive external interference is a challenging problem. With the continuous development and application of mobile robot technology, people have placed higher requirements on the accuracy and stability of its state estimation. However, mobile robots are often affected by various interferences in real environments, such as noise, vibration, and uncertainty, which have a certain impact on the accurate estimation of the system state. Accurate state estimation is crucial for mobile robot control. By introducing advanced algorithms such as threshold filtering and rolling time domain estimation, high-precision estimation of mobile robot states can be achieved, thereby improving the accuracy and stability of mobile robot control.

[0003] Traditional state estimation methods often struggle to cope with the challenges of external interference. For example, inertial sensors are prone to data drift over long periods of use, causing the deviation in state estimation to gradually increase, making it impossible to meet the requirements for state estimation accuracy. Therefore, a more advanced and effective state estimation method is needed to address the challenges posed by external interference. The rolling horizon estimation method, a state estimation method based on Bayesian inference, can continuously update the prior probability distribution and use new observation data to update the posterior probability distribution, thereby achieving accurate estimation of the system state. This method can effectively handle the noise and uncertainty caused by external interference, improve the accuracy and stability of state estimation, and therefore has important application value in mobile robot systems. Summary of the Invention

[0004] In order to overcome the shortcomings of the prior art, the present invention provides a rolling time domain estimation method for a mobile robot that integrates threshold filtering. The method estimates the state of a two-wheel differential vehicle based on a rolling time domain estimation algorithm, thereby improving the accuracy of estimating the vehicle's state in the presence of interference from external measurement values ​​in the prediction time domain. The main steps of the entire process include: establishing a suitable mobile robot system model; designing a measurement equation; processing abnormal measurement data; establishing a cost function; establishing constraints; and designing a rolling time domain estimator to obtain an optimal solution. The present invention can solve the problem that when there is interference in external measurement data, the robot is subject to external interference, thereby reducing the accuracy of the rolling time domain estimation algorithm. The present invention introduces an adaptive scaling factor s to correct abnormal measurement data. Compared with the traditional SINS / UWB combined positioning method, the present method can effectively reduce the impact of external interference on the state estimation of the mobile robot vehicle during movement, and further improve the combined positioning capability for nonlinear systems.

[0005] A rolling time domain estimation method for a mobile robot fused with threshold filtering comprises the following steps:

[0006] 1) Establish a mobile robot system model and perform discretization on it;

[0007] 2) Design the measurement equation to obtain the measurement equation during the entire motion process;

[0008] 3) Processing abnormal measurement data, using the distance threshold method, through the actual measurement value and the predicted measurement value, to obtain the corrected measurement value;

[0009] 4) Design the cost function of the estimation method;

[0010] 5) Designing the estimation method constraints and combining these constraints with the cost function, we finally get the optimization problem of estimating the current state of the mobile robot;

[0011] 6) The optimization problem is solved to obtain the optimal estimated state of the system, which is used to guide the next movement of the mobile robot controller.

[0012] Furthermore, in 1), the discrete-time linear error model of the mobile robot path tracking system is defined as shown in the following formula (1):

[0013] ξ k+1 =Aξ k +Bu k (1)

[0014] in, x e and y e is the position tracking error state of the mobile robot, θ eis the azimuth tracking error state of the mobile robot, v and w are the linear velocity and angular velocity of the mobile robot respectively, v r and w r are the reference linear velocity and reference angular velocity of the mobile robot, ξ k and ξ k+1 are the state vectors at the kth and k+1th sampling moments, u k is the input vector at the kth sampling moment.

[0015] Furthermore, in 2), the dimension expansion method is used to preprocess the information of UWB and IMU, and the obtained measurement equation is shown in the following formula (2):

[0016] z k =Cξ k +η k (2)

[0017] in: η k To measure noise.

[0018] Furthermore, in the above 3), the measurement abnormal data is processed based on the distance threshold filtering, and the distance threshold filtering function is shown in the following formula (3):

[0019]

[0020] Among them, z k-N is the actual measured value at the time kN, is the predicted measurement value at time kN, is the Euclidean distance, ε is the upper threshold;

[0021] The measurement correction data is defined as shown in the following formula (4):

[0022]

[0023] in, is the corrected measurement value at time k, s k is the distance threshold coefficient at time k.

[0024] In the above 4), the cost function in a mobile robot rolling time domain estimation method fused with threshold filtering is defined as shown in the following formula (5):

[0025]

[0026] Among them, P is the weight matrix, N is the prediction range, is the k-time pair ξ k-N The estimated state of is the predicted state at time kN, is the k-time z lk The estimated measurement of is the k-time z lk Corrected measurement.

[0027] In 5), for a given pair of known data in is the predicted state at time kN, Contains the true measurement value from time kN to time k, Contains the input values ​​from time kN to time k-1, to find the optimal estimate Make the cost function Minimize and satisfy the estimation constraints of the following formula (6) and the prediction constraints of the following formula (7):

[0028]

[0029] According to the cost function constructed in step 4) and the constraints set in step 5), the optimization problem of the mobile robot state estimation problem is obtained, as shown in the following formula (8):

[0030]

[0031] In 6), the optimal estimator of the system can be obtained by solving the optimization problem (8):

[0032]

[0033] in, S N =diag{s k-N …s k};

[0034] In step 6), the mobile robot rolling time domain estimation method process is as follows:

[0035] S1: Initialization: Initialize the sliding window size N. When k = N, given the A, B, C matrices and the predicted state Weight matrix P, threshold ε, maximum number of iterations L, real measurement data sequence and the input sequence

[0036] S2: Data acquisition: Acquire and update the real measurement data sequence Input sequence and predicted status

[0037] S3: Calculate the corrected measurement value: Use the above formulas (3) and (4) to calculate the corrected measurement value

[0038] S4: Solve the optimization problem: z k Substituting the weight matrix P into the cost function, we get the optimization problem (8). Solving the optimization problem (8), we get the optimal estimate shown in the above formula (9):

[0039] S5: State estimation: Through the above model (1), derive the optimal state at the current moment

[0040] S6: State update: By predicting the constraint (7), we get

[0041] S7: Rolling update: Let k = k + 1, measure the output Z of the mobile robot k+1 , record the input u k , determine whether the maximum number of iterations has been reached, if so, end, if not, jump to S2.

[0042] The beneficial effects of the present invention are mainly manifested in: estimating the state of the two-wheel differential vehicle based on the rolling time domain estimation algorithm, thereby improving the accuracy of estimating the vehicle's state in the prediction time domain when there is interference in external measurement values. It can solve the problem that when there is interference in external measurement data, the robot is disturbed by the outside world, thereby reducing the accuracy of the rolling time domain estimation algorithm. By introducing an adaptive proportional factor s to correct the measurement data, the correction algorithm reduces external interference, thereby improving the estimation accuracy of the rolling time domain estimation algorithm. Compared with the traditional SINS / UWB combined positioning method, the present invention can effectively reduce the impact of external interference on the state estimation of the mobile robot vehicle during movement, and further improve the combined positioning capability of nonlinear systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 Schematic diagram of the path tracking method of the present invention.

[0044] Figure 2 This is a trajectory result diagram of the method verification of the embodiment of the method of the present invention.

[0045] Figure 3 It is the error position information of X, Y and angle verified by the embodiment of the method of the present invention.

[0046] Figure 4 This is a block diagram of the implementation of the mobile robot rolling time domain estimation method integrated with threshold filtering of the present invention. DETAILED DESCRIPTION

[0047] The present invention will be further described in detail below with reference to specific embodiments.

[0048] Reference Figures 1 to 4 In this embodiment, MPC is selected as the controller to control the mobile robot to track a circular trajectory. During the tracking process, a mobile robot state estimation method based on rolling time domain estimation and threshold filtering is used. In step 1), a matching mobile robot system model is established and discretized. In step 2), the measurement equation is designed accordingly to obtain the measurement equation in the entire motion process. In step 3), the measurement abnormal data is processed, and the distance threshold method is used to obtain the corrected measurement value through the real measurement value and the predicted measurement value. In step 4), the cost function of this estimation method is designed. In step 5), the corresponding constraints of this estimation method are designed, and these constraints are combined with the cost function to finally obtain the optimization problem of the current state estimation of the mobile robot. In step 6), the optimization problem is solved to obtain the optimal estimated state of the system, which is used to guide the next movement of the mobile robot controller. The experimental results are shown in the figure. Figure 2 and Figure 3 As shown, Figure 2 The trajectory result diagram of the method of the present invention compared with the reference trajectory; Figure 3 The error position information comparison chart between the proposed method and the original measurement shows that the proposed method has significantly improved the results compared to the original measurement. The experiment used a two-wheeled differential mobile robot and a computer equipped with an Intel Core i9-4060.

[0049] A rolling time domain estimation method for a mobile robot fused with threshold filtering comprises the following steps:

[0050] 1) Establishing the mobile robot system model: The discrete-time linear error model of the mobile robot path tracking system is defined as shown in the following formula (1):

[0051] ξ k+1 =Aξ k +Bu k (1)

[0052] in, x e and y e is the position tracking error state of the mobile robot, θ e is the azimuth tracking error state of the mobile robot, v and w are the linear velocity and angular velocity of the mobile robot respectively, v r and w r are the reference linear velocity and reference angular velocity of the mobile robot, ξ k and ξ k+1 are the state vectors at the kth and k+1th sampling moments, u kis the input vector at the kth sampling moment; in this example, the target trajectory is a circle with a radius of 2m, and the corresponding reference linear velocity and reference angular velocity are 0.2m / s and 0.1m / s respectively.

[0053] 2) Design of measurement equation: The dimensional expansion method is used to preprocess the information of UWB and IMU. The obtained measurement equation is shown in the following equation (2):

[0054] z k =Cξ k +η k (2)

[0055] in: η k To measure noise;

[0056] 3) Processing of abnormal measurement data: The abnormal measurement data is processed based on distance threshold filtering. The distance threshold filtering function is shown in the following formula (3):

[0057]

[0058] Among them, z k-N is the actual measured value at the time kN, is the predicted measurement value at time kN, is the Euclidean distance, ε is the upper threshold;

[0059] The measurement correction data is defined as shown in the following formula (4):

[0060]

[0061] in, is the corrected measurement value at time k, s k is the distance threshold coefficient at time k; in this example, ε is set to 0.2.

[0062] 4) Establishing the cost function: The cost function in a mobile robot rolling time domain estimation method fused with threshold filtering is defined as shown in the following formula (5):

[0063]

[0064] Among them, P is the weight matrix, N is the prediction range, is the k-time pair ξ k-N The estimated state of is the predicted state at time kN, is the k-time z lk The estimated measurement of is the k-time z lkCorrected measurement; in this example, the prediction step size N is set to 5 and the main diagonal elements of the weight matrix P are [0.5, 0.5, 0.5].

[0065] 5) Establish constraints: For a given pair of known data in is the predicted state at time kN, Contains the true measurement value from time kN to time k, Contains the input values ​​from time kN to time k-1, to find the optimal estimate Make the cost function Minimize and satisfy the estimation constraints of the following formula (6) and the prediction constraints of the following formula (7):

[0066]

[0067] According to the cost function constructed in step 4) and the constraints set in step 5), the optimization problem of the mobile robot state estimation problem is obtained, as shown in the following formula (8):

[0068]

[0069] In this example, is [5.93651,3.36164,1.71042]; for for

[0070] 6) Design of the rolling horizon estimator: The optimal estimator of the system can be obtained by solving the optimization problem (8):

[0071]

[0072] in, S N =diag{s k-N …s k}; In this example, the solution is [5.92123,3.44236,1.74532], we get is [5.83858,3.68075,1.83259].

[0073] The mobile robot rolling time domain estimation method process of this embodiment fused with threshold filtering includes the following process:

[0074] S1: Initialization: Initialize the sliding window size N. When k = N, given the A, B, C matrices and the predicted state Weight matrix P, threshold ε, maximum number of iterations L, real measurement data sequence and the input sequence

[0075] S2: Data acquisition: Acquire and update the real measurement data sequence Input sequence and predicted status

[0076] S3: Calculate the corrected measurement value: Use the above formulas (3) and (4) to calculate the corrected measurement value

[0077] S4: Solve the optimization problem: Substituting the weight matrix P into the cost function, we get the optimization problem (8). Solving the optimization problem (8), we get the optimal estimate shown in the above formula (9):

[0078] S5: State estimation: Through the above model (1), derive the optimal state at the current moment

[0079] S6: State update: By predicting the constraint (7), we get

[0080] S7: Rolling update: Let k = k + 1, measure the output Z of the mobile robot k+1 , record the input u k , determine whether the maximum number of iterations has been reached, if so, end, if not, jump to S2.

[0081] This embodiment uses the given measurement information and prior states It can be deduced In the embodiment, the optimal estimated state at time kN is solved, and then the optimal estimated state at time k is calculated using the constraints.

[0082] The embodiments of this specification are merely examples of implementations of the invention and are provided for illustrative purposes only. The scope of protection of the present invention should not be considered limited to the specific embodiments described in these embodiments. The scope of protection of the present invention also extends to equivalent technical means that can be conceived by a person of ordinary skill in the art based on the invention.

Claims

1. A rolling time domain estimation method for a mobile robot fused with threshold filtering, characterized in that: The method comprises the following steps: 1) Establish a mobile robot system model and perform discretization on it; 2) Design the measurement equation to obtain the measurement equation during the entire motion process; 3) Process the abnormal measurement data and use the distance threshold method to obtain the corrected measurement value through the actual measurement value and the predicted measurement value; 4) Design the cost function of the estimation method; 5) Designing the estimation method constraints and combining them with the cost function ultimately leads to the optimization problem of estimating the current state of the mobile robot; 6) Solve the optimization problem to obtain the optimal estimated state of the system, which is used to guide the next movement of the mobile robot controller; In the above 3), the measurement abnormal data is processed based on the distance threshold filter, and the distance threshold filter function is shown in the following formula (3): (3) in, yes The actual measured value at the moment, is The predicted measurement value at time t, is the Euclidean distance, is the upper threshold; The measurement correction data is defined as shown in the following formula (4): (4) in, yes The measured value after time correction, yes The distance threshold coefficient at the moment; In the above 4), the cost function in a mobile robot rolling time domain estimation method fused with threshold filtering is defined as shown in the following formula (5): (5) in, is the weight matrix, is the forecast range, yes Always The estimated state of yes The predicted state at the moment, yes Always The estimated measurement of yes Always Corrected measurement of In 5), for a given pair of known data ,in yes The predicted state at the moment, Contains from Time has come The actual measured value at the moment, Contains from Time has come To find the optimal estimate of the input value at time , so that the cost function Minimize and satisfy the estimation constraints of the following formula (6) and the prediction constraints of the following formula (7): (6) (7) According to the cost function constructed in step 4) and the constraints set in step 5), the optimization problem of the mobile robot state estimation problem is obtained, as shown in the following formula (8): (8)。 2. The mobile robot rolling time domain estimation method integrating threshold filtering as claimed in claim 1, characterized in that: In 1), the discrete-time linear error model of the mobile robot path tracking system is defined as shown in the following formula (1): (1) in, , and is the position tracking error state of the mobile robot, is the state quantity of the azimuth tracking error of the mobile robot car, and are the linear velocity and angular velocity of the mobile robot, and are the reference linear velocity and reference angular velocity of the mobile robot, and They are Hedi The state vector at each sampling moment, It is The input vector at each sampling moment.

3. The mobile robot rolling time domain estimation method integrating threshold filtering as claimed in claim 2, characterized in that: In the above 2), the dimension expansion method is used to preprocess the information of UWB and IMU, and the obtained measurement equation is shown in the following formula (2): (2) in: , To measure noise.

4. The mobile robot rolling time domain estimation method integrating threshold filtering as claimed in claim 1, characterized in that: In 6), the optimal estimator of the system is obtained by solving the optimization problem (8): (9) in, , ; .

5. The mobile robot rolling time domain estimation method integrating threshold filtering as claimed in claim 4, characterized in that: In step 6), the process of the mobile robot rolling time domain estimation method is as follows: S1: Initialization: Initialize the sliding window size ,when When given Matrix, predicted state , weight matrix , threshold , maximum number of iterations , real measurement data series and the input sequence ; S2: Data acquisition: Acquire and update the real measurement data sequence , input sequence and predicted status ; S3: Calculate the corrected measurement value: Use the above formulas (3) and (4) to calculate the corrected measurement value ; S4: Solve the optimization problem: and the weight matrix Substituting into the cost function to get the optimization problem (8), solving the optimization problem (8), we get the optimal estimate shown in the above formula (9) ; S5: State estimation: Through the above model (1), derive the optimal state at the current moment ; S6: State update: By predicting the constraint (7), we get ; S7: Rolling update: , measuring the output of the mobile robot , record the input , determine whether the maximum number of iterations has been reached, if so, end, if not, jump to S2.

Images