Mobile robot rolling time domain estimation method fused with threshold filtering

Abstract

The invention discloses a mobile robot rolling time domain estimation method fused with threshold filtering. The method comprises the following steps: 1) establishing a mobile robot system model; 2) designing a measurement equation; 3) processing measurement abnormal data; 4) establishing a cost function; 5) establishing constraint conditions; and 6) designing a rolling time domain estimator. According to the method, a self-adaptive scale factor s is introduced to correct measured abnormal data, then rolling time domain estimation is carried out, and an optimization problem is solved, so that the influence of external interference on state estimation in the moving process of the mobile robot trolley is effectively reduced, and the combined positioning capability of 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 of a mobile robot system under the condition of a large amount of external interference. Background Art

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

[0003] Traditional state estimation methods often have difficulty coping with the challenges of external interference. For example, inertial sensors are prone to data drift during long-term use, which leads to a gradual increase in the deviation of state estimation and cannot meet the requirements for state estimation accuracy. Therefore, a more advanced and effective state estimation method is needed to cope with the challenges brought by external interference. As a state estimation method based on Bayesian inference, the rolling time domain estimation method can update the posterior probability distribution based on the continuous update of the prior probability distribution and use the new observation data to achieve accurate estimation of the system state. This method can effectively deal with the noise and uncertainty caused by external interference and improve the accuracy and stability of state estimation. Therefore, it 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 integrating threshold filtering, and estimates the state of a two-wheel differential car based on a rolling time domain estimation algorithm, thereby improving the accuracy of estimating the state of the car in the prediction time domain when there is interference from external measurement values. The main steps of the whole 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 disturbed by external interference, thereby reducing the accuracy of the rolling time domain estimation algorithm. The present invention introduces an adaptive proportional factor s to correct the abnormal measurement data. Compared with the traditional SINS / UWB combined positioning method, the present method can effectively reduce the influence of external interference on the state estimation of the mobile robot car during movement, and further improve the combined positioning capability of nonlinear systems.

[0005] A mobile robot rolling time domain estimation method 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 in the whole motion process;

[0008] 3) Process the abnormal measurement data and use the distance threshold method to obtain the corrected measurement value through the real measurement value and the predicted measurement value;

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

[0010] 5) Design the estimation method constraints and combine these constraints with the cost function, finally obtaining 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 e is the mobile robot position tracking error state, θ 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 pre-process 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 measured 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 Let the cost function Minimize and satisfy the estimation constraints of the following equation (6) and the prediction constraints of the following equation (7):

[0028]

[0029] According to the cost function constructed in step 4) and the constraint conditions 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 predicted states 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 measured value: Use the above formulas (3) and (4) to calculate the corrected measured 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 the process. 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 car based on the rolling time domain estimation algorithm, thereby improving the accurate estimation of the car's state in the prediction time domain when there is interference in the external measurement value. It can solve the problem that when there is interference in the 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 external interference is reduced by the correction algorithm, 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 car during movement, and further improve the combined positioning capability of nonlinear systems. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0044] Figure 2 It 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 method embodiment of the present invention.

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

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

[0048] Reference Figure 1 to Figure 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 during 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 It is a trajectory result diagram of the method of the present invention compared with the reference trajectory; Figure 3 The error position information comparison diagram between the method of the present invention and the original measurement shows that the method of the present invention has significantly improved the results compared with the original measurement. A two-wheel differential mobile robot and a computer equipped with Intel Core i9-4060 were used in the experiment.

[0049] A mobile robot rolling time domain estimation method 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 e is the mobile robot position tracking error state, θ 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, and 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 a 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 Let the cost function Minimize and satisfy the estimation constraints of the following equation (6) and the prediction constraints of the following equation (7):

[0066]

[0067] According to the cost function constructed in step 4) and the constraint conditions 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 a rolling horizon estimator: By solving the optimization problem (8), the optimal estimator of the system can be obtained:

[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 the present 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 predicted states 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 measured value: Use the above formulas (3) and (4) to calculate the corrected measured 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 the process. If not, jump to S2.

[0081] This embodiment uses the given measurement information and the prior state 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 contents described in the embodiments of this specification are merely enumerations of implementation forms of the inventive concept and are for illustrative purposes only. The protection scope of the present invention should not be considered to be limited to the specific forms described in this embodiment, and the protection scope of the present invention also extends to equivalent technical means that can be thought of by ordinary technicians in this field based on the inventive concept.

Claims

1. A mobile robot rolling time domain estimation method integrating 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 in the whole motion process; 3) Process the abnormal measurement data and use the distance threshold method to obtain the corrected measurement value through the real measurement value and the predicted measurement value; 4) Design the cost function of the estimation method; 5) Design the estimation method constraints and combine these constraints with the cost function, finally obtaining the optimization problem of estimating the current state of the mobile robot; 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.

2. A mobile robot rolling time domain estimation method integrating threshold filtering as claimed in claim 1, characterized in that: In the above 1), the discrete time linear error model of the mobile robot path tracking system is defined as shown in the following formula (1): ξ k+1 =Aξ k +Bu k (1) in, x e and e is the mobile robot position tracking error state, θ 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 k is the input vector at the kth sampling moment.

3. A mobile robot rolling time domain estimation method integrating threshold filtering as described in claim 2, characterized in that: In the above 2), the dimension expansion method is used to pre-process the information of UWB and IMU, and the obtained measurement equation is shown in the following formula (2): z k =Cξ k +η k (2) in: η k To measure noise.

4. A mobile robot rolling time domain estimation method integrating threshold filtering as claimed in claim 3, characterized in that: In the above 3), the measured abnormal data is processed based on the distance threshold filtering, and the distance threshold filtering function is shown in the following formula (3): 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; The measurement correction data is defined as shown in the following formula (4): in, is the corrected measurement value at time k, s k is the distance threshold coefficient at time k.

5. A mobile robot rolling time domain estimation method integrating threshold filtering as claimed in claim 4, characterized in that: 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): 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.

6. A mobile robot rolling time domain estimation method integrating threshold filtering as claimed in claim 5, characterized in that: 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 Let the cost function Minimize and satisfy the estimation constraints of the following equation (6) and the prediction constraints of the following equation (7): According to the cost function constructed in step 4) and the constraint conditions set in step 5), the optimization problem of the mobile robot state estimation problem is obtained, as shown in the following formula (8):

7. A mobile robot rolling time domain estimation method integrating threshold filtering as claimed in claim 6, characterized in that: In 6), the optimal estimator of the system is obtained by solving the optimization problem (8): in, S N =diag{s k-N …s k }; 8. The mobile robot rolling time domain estimation method integrating threshold filtering as claimed in claim 7, characterized in that: In step 6), the mobile robot rolling time domain estimation method process is as follows: S1: Initialization: Initialize the sliding window size N. When k = N, given the A, B, C matrices and predicted states Weight matrix P, threshold ε, maximum number of iterations L, real measurement data sequence and the input sequence S2: Data acquisition: Acquire and update the real measurement data sequence Input sequence and predicted status S3: Calculate the corrected measured value: Use the above formulas (3) and (4) to calculate the corrected measured value 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): 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: 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 the process. If not, jump to S2.

Images