Robot localization method based on adaptive kernel width Kalman filter

Abstract

The present invention relates to the field of special robot positioning technology, specifically, to a robot positioning method based on an adaptive kernel width Kalman filter, the steps of the method comprising: based on an inertial positioning unit and an encoder equipped with the special robot, constructing a kinematic equation and an observation equation of the special robot, wherein the kinematic equation of the special robot is characterized as theoretical position data, and the observation equation of the special robot is characterized as observed position data; bringing the theoretical position data and the observed position data into a maximum related entropy Kalman filter algorithm based on an adaptive kernel width for correction calculation, and completing the precise positioning of the special robot, wherein the adaptive kernel width is obtained by KL divergence optimization. The present invention can combine the inertial positioning unit and encoder data information, remove the influence of process noise and observation noise of external changes, and realize more accurate positioning of the special robot.

Description

Technical Field

[0001] The present invention relates to the field of robot positioning technology, and in particular to a robot positioning method based on adaptive kernel width Kalman filtering. Background Art

[0002] With the continuous innovation of science and technology, robots and related fields have been vigorously developed. Among them, special robots have gradually replaced manual labor and are widely used in warehousing, industry, medical and other fields. In a complex working environment, how special robots determine their own position information is the primary prerequisite for efficiently completing special tasks. At present, in order to overcome the shortcomings of inertial positioning and navigation methods, the technical means commonly used in the existing technology are: installing encoders and other devices to obtain another set of position information, and using the Kalman filter algorithm based on the minimum mean square error criterion to correct the position information of the inertial measurement unit. However, due to the complex working environment of special robots, their sensors will be affected by various time-varying noises, which can easily cause the algorithm performance to drop rapidly, resulting in low positioning accuracy of special robots. Based on this, we urgently need an algorithm that can more accurately locate special robots. Summary of the invention

[0003] The purpose of the present invention is to provide a robot positioning method based on an adaptive kernel width Kalman filter, which obtains the theoretical position and the observed position according to the inertial measurement unit and the encoder equipped by the special robot, and uses the Kalman filter algorithm with an adaptive kernel width to realize the estimation and correction of the position information. It can combine the inertial positioning unit and the encoder data information, remove the influence of the process noise and the observation noise of the external changes, and realize more accurate positioning of the special robot.

[0004] The embodiments of the present invention are implemented by the following technical solutions:

[0005] A robot positioning method based on adaptive kernel width Kalman filtering, the method comprising the steps of:

[0006] Based on the inertial positioning unit and encoder equipped with the special robot, the kinematic equation and observation equation of the special robot are constructed, wherein the kinematic equation of the special robot is represented by theoretical position data, and the observation equation of the special robot is represented by observed position data;

[0007] The theoretical position data and the observed position data are brought into the maximum correlation entropy Kalman filter algorithm based on adaptive kernel width for correction calculation to complete the precise positioning of the special robot, wherein the adaptive kernel width is obtained by KL divergence optimization.

[0008] Optionally, the kinematic equation of the special robot is as follows:

[0009] xk=Akxk-1+wk

[0010]

[0011] Among them, k is the kth moment, x k ∈R p×1 is the estimated value of the state at time k, A k ∈R p×p is the state transfer matrix, w k ∈R p×1 is the process noise, w k is zero mean noise, E is the expected operator, T is the transposed symbol, Q k is the covariance matrix of the process noise.

[0012] Optionally, the observation equation of the special robot is as follows:

[0013] y k =C k x k +v k

[0014]

[0015]

[0016] Among them, y k ∈R q×1 is the observed value at time k, C k ∈R q×p is the observation matrix, v k ∈R q×1 is the observation noise, v k is zero mean noise, R k is the covariance matrix of the observation noise.

[0017] Optionally, the process of solving the adaptive kernel width is as follows:

[0018] Set the initial kernel width σ, positive number ∈ and initial state estimate and the initial covariance matrix P 0|0 ;

[0019] The predicted value at the next moment is solved by the first calculation formula

[0020] The prediction error covariance matrix P of the next moment is solved by the second calculation formula k|k-1 ;

[0021] P k|k-1 With R k Perform Chulesky decomposition to obtain B pk With B rk ;

[0022] Update the parameters of the preset augmented system and determine the size relationship between k and the set time window N. If k>N, solve the kernel width through the fixed point iteration formula and proceed to the next step. Otherwise, take the initial kernel width σ and estimate the state value through fixed point iteration and proceed to the next step.

[0023] The iteration stops when the iteration condition is met, and the posterior estimated covariance matrix is ​​updated through the third calculation formula to obtain the optimal kernel width.

[0024] Optionally, the first calculation formula is specifically:

[0025]

[0026] in, is the best estimate of the state at time k-1, is the predicted value of the state at time k.

[0027] Optionally, the second calculation formula is specifically:

[0028] P k|k-1 =A k P k-1|k-1 A k T +Q k-1

[0029] Among them, P k|k-1 is the covariance matrix of the true value and the predicted value at time k, that is, the prior estimated covariance matrix, P k-1|k-1 is the covariance matrix of the true value and the estimated value at k-1 time, Q k-1 is the covariance matrix of the process noise at k-1 time.

[0030] Optionally, the P k|k-1 With R k Perform Chulesky decomposition, the calculation formula is as follows:

[0031] B pk =chol(P k|k-1 ) T

[0032] B rk =chol(R k ) T

[0033] Among them, B pk is a variable, B rk is a variable.

[0034] Optionally, the updating of the preset augmentation system parameters is specifically:

[0035] dk=Wkxk+ek

[0036]

[0037]

[0038]

[0039]

[0040] Among them, dk, Wk, Bk are all augmented system variables, and ek is the error variable of the augmented system.

[0041] Optionally, the fixed point iteration formula is specifically:

[0042]

[0043]

[0044] Among them, σ k is the Gaussian kernel function kernel width at time k, N is the sampling time window size, i is the i-th moment, G k is the intermediate variable, j is the jth moment, e i is the error size of the augmented system at the i-th moment, e j is the error size of the augmented system at the jth moment;

[0045] The specific iteration conditions are:

[0046] ||σ k+1 -σ k || / ||σ k ||≤∈ σ

[0047] Among them, ∈ σ is a positive number set;

[0048] The fixed point iterative estimation state value is specifically:

[0049]

[0050]

[0051] Among them, G σ is the Gaussian kernel function, t is the number of iterations, are the predicted value and estimated value of the state at time k after the tth iteration, K k is the Kalman filter gain, is the generalized covariance matrix of the true value and the predicted value at time k, is the generalized covariance matrix of the observation noise at time k, e k(i) is the i-th value of the augmented error at time k, and diag[] is the extracted diagonal element;

[0052] The specific iteration conditions are:

[0053]

[0054] Optionally, the third calculation formula is specifically:

[0055]

[0056] Among them, P k|k is the posterior estimated covariance matrix at time k, and I is the identity matrix.

[0057] The technical solution of the embodiment of the present invention has at least the following advantages and beneficial effects:

[0058] The embodiment of the present invention obtains the theoretical position and the observed position according to the inertial measurement unit and the encoder equipped by the special robot, and uses the Kalman filter algorithm with a self-adaptive kernel width to realize the estimation and correction of the position information. It can combine the inertial positioning unit and the encoder data information to remove the influence of the process noise and observation noise of the external changes, and realize more accurate positioning of the special robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 A schematic diagram of the overall process of a robot positioning method based on an adaptive kernel width Kalman filter provided in an embodiment of the present invention;

[0060] Figure 2 A schematic diagram of a solution process of an algorithm provided in an embodiment of the present invention;

[0061] Figure 3 A schematic diagram of observed noise distribution and adaptive kernel width values ​​provided in an embodiment of the present invention;

[0062] Figure 4 A schematic diagram of speed error calculation in an example of a maximum relevant entropy Kalman filtering method with a fixed σ=2,∞ provided in an embodiment of the present invention and a method proposed in an embodiment of the present invention;

[0063] Figure 5 A schematic diagram of displacement error calculation in an example of a maximum correlation entropy Kalman filtering method with a fixed σ=2,∞ provided in an embodiment of the present invention and a method proposed in an embodiment of the present invention. DETAILED DESCRIPTION

[0064] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings here can be arranged and designed in various different configurations.

[0065] like Figure 1 As shown, Figure 1 A schematic diagram of the overall process of a robot positioning method based on an adaptive kernel width Kalman filter provided in an embodiment of the present invention.

[0066] In some embodiments, a robot positioning method based on an adaptive kernel width Kalman filter comprises the following steps:

[0067] Based on the inertial positioning unit and encoder equipped with the special robot, the kinematic equation and observation equation of the special robot are constructed, wherein the kinematic equation of the special robot is represented by theoretical position data, and the observation equation of the special robot is represented by observed position data;

[0068] The theoretical position data and the observed position data are brought into the maximum correlation entropy Kalman filter algorithm based on adaptive kernel width for correction calculation to complete the precise positioning of the special robot, wherein the adaptive kernel width is obtained by KL divergence optimization.

[0069] In this embodiment, in order to simulate the movement of the special robot in the environment, it is necessary to establish the kinematic equation of the special robot, that is, to model the movement law of the special robot:

[0070] x k =A k x k-1 +w k

[0071] Among them, k is the kth moment, x k ∈R p×1 is the estimated value of the state at time k, A k ∈R p×p is the state transfer matrix, w k ∈R p×1 is the process noise, w k is zero mean noise, E is the expected operator, T is the transposed symbol, Q k is the covariance matrix of process noise, which can be defined as:

[0072]

[0073] In this embodiment, the observation equation is a model assumption for the encoder to obtain the position of the special robot, so it is necessary to construct the observation equation of the special robot, that is:

[0074] y k =C k x k +v k

[0075]

[0076] Among them, y k ∈R q×1 is the observed value at time k, C k ∈R q×p is the observation matrix, v k ∈R q×1 is the observation noise, v k is zero mean noise, R k is the covariance matrix of the observation noise, which can be defined as:

[0077]

[0078] like Figure 2 As shown, Figure 2 A schematic diagram of the solution process of the algorithm provided in an embodiment of the present invention.

[0079] In the specific application of this embodiment, this embodiment combines the kernel width calculation method based on KL divergence with the maximum correlation entropy Kalman filter and applies it to the positioning of special robots. The specific method is as follows:

[0080] Set the initial kernel width σ, positive number ∈ and initial state estimate and the initial covariance matrix P 0|0 ;

[0081] The predicted value at the next moment is solved by the first calculation formula

[0082] The first calculation formula is specifically:

[0083]

[0084] in, is the best estimate of the state at time k-1, is the predicted value of the state at time k.

[0085] The prediction error covariance matrix P of the next moment is solved by the second calculation formula k|k-1 ;

[0086] The second calculation formula is specifically:

[0087] P k|k-1 =A k P k-1|k-1 A k T +Q k-1

[0088] Among them, P k|k-1 is the covariance matrix of the true value and the predicted value at time k, that is, the prior estimated covariance matrix, P k-1|k-1 is the covariance matrix of the true value and the estimated value at k-1 time, Q k-1 is the covariance matrix of the process noise at k-1 time.

[0089] P k|k-1 With R k Perform Chulesky decomposition to obtain the intermediate variable B pk With B rk ;

[0090] B pk =chol(P k|k-1 ) T

[0091] B rk =chol(R k ) T

[0092] Update the default augmentation system k =W k x k +e k Parameters;

[0093] Building an augmented system,

[0094]

[0095] in,

[0096] right Perform Chulesky decomposition and get B k , the symbol E represents the expectation operator, multiply the augmented system formula by Get k =W k x k +e k .

[0097] d k =W k x k +e k

[0098]

[0099]

[0100]

[0101]

[0102] Among them, d k , W k , B k are all augmented system variables, e k is the error variable of the augmented system, e k ∈R (p+q)×1 .

[0103] Determine the size relationship between k and the set time window N. If k>N, solve the kernel width through the fixed point iteration formula and proceed to the next step. Otherwise, take the initial kernel width σ, and estimate the state value through fixed point iteration and proceed to the next step.

[0104] Specifically, in density estimation, the embodiment of the present invention uses E 1 ,E 2 ,...,E N Representing a window of N samples from a random variable of density f, the kernel density estimate of f at e is given by,

[0105]

[0106] Among them, K satisfies ∫K(s)ds=1, and the parameter h represents the bandwidth. In the maximum correlation entropy and distributed maximum correlation entropy, the Gaussian kernel function G is usually selected. σ is K, and h is the kernel width. The density estimate is the sum of the normal densities, and kernels with different bandwidths will lead to different density estimates.

[0107] In the augmented system, setting is the estimated density obtained by a window consisting of N error samples and evaluated using a Gaussian kernel,

[0108]

[0109] In this embodiment, the cost function is expressed as:

[0110]

[0111] Among them, the first term in the above formula has nothing to do with the kernel width, and minimizing the above formula is equivalent to the following cost function,

[0112]

[0113] By derivatizing the above cost function, we can obtain:

[0114]

[0115] In order to obtain an optimal kernel width, this implementation sets the above formula equal to 0 and sets a small positive value ∈ σ and time window N. When k≤N, take the initial kernel width σ value and enter the fixed point iteration to estimate the state value. When k>N, use the following fixed point iteration formula to calculate the appropriate kernel width and finally use the fixed point iteration formula to calculate:

[0116]

[0117]

[0118] When ||σ k+1 -σ k || / ||σ k ||≤∈ σ When it is established, the iteration stops, and the σ k+1 is the optimal kernel width, ∈ σ is set to a small positive value, σ k is the kernel width of the Gaussian kernel function at time k, N is the sampling time window size, e i ,e j is the error size of the augmented system at the i,jth moment.

[0119] The fixed point iterative estimation state value is specifically:

[0120]

[0121] in,

[0122]

[0123] Among them, G σ is the Gaussian kernel function, t is the number of iterations, are the predicted value and estimated value of the state at time k after the tth iteration, K k is the Kalman filter gain, is the generalized covariance matrix of the true value and the predicted value at time k, is the generalized covariance matrix of the observation noise at time k, e k (i) is the i-th value of the augmented error at time k, and diag[] is the extracted diagonal element;

[0124] when When it is established, the iteration stops and proceeds to the next step.

[0125] The iteration stops when the iteration condition is met, and the posterior estimated covariance matrix is ​​updated through the third calculation formula to obtain the optimal kernel width.

[0126] The third calculation formula is specifically:

[0127]

[0128] Where I is the identity matrix, P k|k is the posterior estimated covariance matrix at time k, which is used for iterative update at time k+1.

[0129] This implementation method evaluates the positioning performance of a special robot based on an adaptive kernel width maximum correlation entropy Kalman filter in conjunction with specific examples.

[0130] In some examples: Generally speaking, the motion state of a special robot can be modeled as a constant acceleration and uniform motion model. The constant acceleration model assumes that the special robot moves in a straight line under a certain acceleration, which generally occurs at the start and end of the task. This example takes the constant acceleration model as an example, and the time window N is 15. The adaptive kernel width maximum correlation entropy Kalman filter method of the present invention is compared with the traditional maximum correlation entropy Kalman filter when the kernel width is 2,∞.

[0131] The state space expression of the constant acceleration model is as follows,

[0132]

[0133]

[0134] Where Ts = 0.1 represents the measurement time interval, w k is the process noise that follows a Gaussian distribution with parameters (0, 0.01), r k is the observation noise that combines Gaussian noise and non-Gaussian noise, and its expression is as follows:

[0135]

[0136]

[0137]

[0138] Among them, r k (1) is non-Gaussian noise, r k (2) is Gaussian noise. After every 2000 iterations, the two noises are switched.

[0139] like Figure 3 As shown, Figure 3 A schematic diagram of observation noise distribution and adaptive kernel width value selection provided in an embodiment of the present invention.

[0140] As can be seen from the figure, the observation noise is Gaussian noise in the first 1000 iterations, 3000 to 5000 iterations, and 7000 to 9000 iterations, and non-Gaussian noise in 1000 to 3000 iterations, 5000 to 7000 iterations, and 9000 to 10000 iterations. The kernel size has different values ​​under different noises. The optimal kernel width increases under Gaussian noise, and decreases under non-Gaussian noise, which reflects the adaptability of the method proposed in this embodiment.

[0141] like Figure 4 As shown, Figure 4 The maximum correlation entropy Kalman filtering method with a fixed σ=2,∞ provided in the embodiment of the present invention and the speed error calculation schematic diagram in the example of the method proposed in the embodiment of the present invention. Figure 5 As shown, Figure 5 A schematic diagram of displacement error calculation in an example of a fixed σ=2,∞ maximum correlation entropy Kalman filtering method provided in an embodiment of the present invention and a method proposed in an embodiment of the present invention.

[0142] By comparison, it can be concluded that under Gaussian noise conditions, the maximum correlation entropy Kalman filter method with a fixed kernel width of ∞ and the method proposed in the present invention have smaller errors when processing the model; under non-Gaussian noise conditions, the maximum correlation entropy Kalman filter method with a fixed kernel width of 2 and the method proposed in this embodiment have smaller errors when processing the model. Combining the performance under the two noise conditions, it can be found that the maximum correlation entropy Kalman filter method with an adaptive kernel width proposed in this embodiment has excellent performance and can handle constantly changing noise well.

[0143] The above are only preferred embodiments of the present invention and are not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. Robot positioning method based on adaptive kernel width Kalman filter, It is characterized in that The steps of the method include: Based on the inertial positioning unit and encoder equipped with the special robot, the kinematic equation and observation equation of the special robot are constructed, wherein the kinematic equation of the special robot is represented by theoretical position data, and the observation equation of the special robot is represented by observed position data; The theoretical position data and the observed position data are brought into the maximum correlation entropy Kalman filter algorithm based on the adaptive kernel width for correction calculation to complete the precise positioning of the special robot, wherein the adaptive kernel width is obtained by KL divergence optimization; The process of solving the adaptive kernel width is as follows: Set initial kernel width , positive number and the initial state estimate and the initial covariance matrix ; The predicted value at the next moment is solved by the first calculation formula ; The prediction error covariance matrix for the next moment is solved by the second calculation formula ; Respectively and Perform the Chulesky decomposition and obtain and ; Update the parameters of the preset augmentation system and determine With the set time window If the size relationship between , then the kernel width is solved by the fixed point iteration formula and the next step is entered; otherwise, the initial kernel width is taken. , and estimate the state value through fixed point iteration and enter the next step; The iteration stops when the iteration condition is met, and the posterior estimated covariance matrix is ​​updated through the third calculation formula to obtain the optimal kernel width; The first calculation formula is specifically: in, for The best estimate of the state at time, for The predicted value of the state at the moment; The second calculation formula is specifically: in, for The covariance matrix of the true value and the predicted value at the moment, that is, the prior estimated covariance matrix, for The covariance matrix of the true value and the estimated value at the moment, for The covariance matrix of the moment process noise; The respective and Perform Chulesky decomposition, the calculation formula is as follows: in, is a variable, is a variable; The third calculation formula is specifically: in, for The posterior estimated covariance matrix at time , is the identity matrix.

2. The robot positioning method based on adaptive kernel width Kalman filtering according to claim 1, It is characterized in that The kinematic equation of the special robot is as follows: in, For the a moment, for The estimated state value at time is the state transfer matrix, is the process noise, is zero mean noise, is the expectation operator, is the transpose symbol, is the covariance matrix of the process noise.

3. The robot positioning method based on adaptive kernel width Kalman filtering according to claim 2, It is characterized in that The observation equation of the special robot is as follows: in, for The observed value at time, is the observation matrix, is the observation noise, is zero mean noise, is the covariance matrix of the observation noise.

4. The robot positioning method based on adaptive kernel width Kalman filtering according to claim 3, It is characterized in that The parameters of the updated preset augmentation system are specifically: in, , , are all augmented system variables, is the error variable of the augmented system.

5. The robot positioning method based on adaptive kernel width Kalman filtering according to claim 4, It is characterized in that The fixed point iteration formula is specifically: in, for The Gaussian kernel function kernel width at time , is the sampling time window size, For the time, is the intermediate variable, For the time, For the The error size of the system is constantly increased, For the The error magnitude of the moment-augmenting system; The specific iteration conditions are: in, is a positive number set; The fixed point iterative estimation state value is specifically: in, is the Gaussian kernel function, is the number of iterations, Respectively After iterations The predicted and estimated values ​​of the state at each moment, is the Kalman filter gain, for The generalized covariance matrix of the actual value and the predicted value at the moment, for The generalized covariance matrix of the moment-by-moment observation noise, for The moment of error augmentation values, To extract diagonal elements; The specific iteration conditions are: 。

Images