Foot type walking anti-disturbance navigation method for humanoid robot

Abstract

The invention relates to the technical field of robot control, in particular to a humanoid robot foot type walking anti-disturbance navigation method, which is used for solving the problem of disturbance rejection in the aspect of tolerating model uncertainty by adopting the method provided by the invention. The problem of state estimation precision degradation caused by model uncertainty caused by Jacobian matrix perturbation in the first-order linear approximation process is effectively suppressed, and the first-order state estimation precision of the humanoid robot nonlinear system is remarkably improved. According to the method, real state distribution is taken as a center, a Warisstein distance fuzzy set is constructed as an allowable neighborhood, and an EKF algorithm is modeled into a game state estimation problem based on Warisstein distance fuzzy set constraint, so that the navigation and positioning precision of the humanoid robot is remarkably improved, and the method has great significance in wider application of the humanoid robot.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a humanoid robot foot-walking anti-disturbance navigation method. Background Art

[0002] With the rapid development of artificial intelligence technology, the application of humanoid robots is becoming more and more mature, such as industrial manufacturing, home services and disaster relief. Among them, high-precision navigation and positioning technology is the basis for humanoid robots to successfully complete tasks. In practical applications, the movement of humanoid robot joints, arms, and observed targets mostly show nonlinear motion characteristics. At present, the navigation and positioning technology solutions based on lidar or camera vision basically choose the traditional first-order extended Kalman filter algorithm to perform linear approximate state estimation and solution for the nonlinear system of humanoid robot navigation and positioning. This approximation process is to linearize the nonlinear system in the first order by solving the Jacobian matrix of the nonlinear state equation at each moment. In actual calculations, the Jacobian matrix obtained at each moment will have a certain difference, that is, matrix perturbation, which causes the error of subsequent state estimation to increase.

[0003] When a humanoid robot uses the first-order extended Kalman filter (EKF) algorithm to estimate the state of nonlinear systems such as motion control and action execution, the ratio of the state update amount at each moment to the entire state estimation process is usually much smaller than the state estimation accuracy of the EKF algorithm. This phenomenon shows that the first-order linear approximation of the nonlinear system is not the only reason for the low state estimation accuracy of the EKF algorithm. Theoretically, the only difference between the EKF algorithm and the classical KF algorithm is the update method of the parameter matrix. The parameter matrix of the EKF algorithm is obtained by calculating the Jacobian matrix to perform a first-order approximation on the nonlinear system, and it changes in real time. This real-time change of the parameter matrix can be called model uncertainty, that is, model perturbation. This model uncertainty (model perturbation) is the cause of the low estimation accuracy of the EKF algorithm. Summary of the invention

[0004] The purpose of the present invention is to provide a disturbance-resistant navigation method for foot-type walking of a robot to solve the above-mentioned problems in the prior art.

[0005] The present invention is achieved through the following technical solutions:

[0006] In a first aspect, the present invention provides a humanoid robot foot-walking anti-disturbance navigation method, comprising:

[0007] Initialize basic parameters;

[0008] A prediction state model is established, a one-step prediction state estimate is obtained through the prediction state model, a covariance matrix of a one-step prediction joint state estimate and a joint distribution is calculated through the one-step prediction state estimate, and a pseudo-nominal nominal distribution is obtained based on the covariance matrix of the one-step prediction joint state estimate and the joint distribution;

[0009] Get the current observation value, update the state estimation covariance matrix, build the minimax model, and obtain the probability density of the estimated value distribution of the optimal state through the minimax model;

[0010] A state estimation model is established, and the optimal state estimation and covariance matrix at the current moment are updated based on the probability density of the estimated value distribution of the optimal state.

[0011] Preferably, the establishing of the prediction state model comprises:

[0012]

[0013] In the formula, is the one-step forecast state estimate at time t, is the one-step forecast state estimate at time t-1, is the one-step prediction of the observed value at time t-1, is the first-order Jacobian matrix of the state transfer function at time t-1, is the first-order Jacobian matrix of the observation function at time t, To solve for the Jacobian matrix for the subscript function, μ t is the joint distribution, μ t,x is the state vector, μ t,y is the observation vector, ∑ t is the covariance matrix of the joint distribution, V t-1 is the estimated covariance matrix at time t-1, I n is the n*n identity matrix, Q t is the covariance matrix of the state noise, is a pseudo-nominal distribution.

[0014] Preferably, updating the state estimation covariance matrix comprises using a Frank-Wolfe algorithm;

[0015]

[0016] In the formula, is the state estimation covariance matrix, ρ is the Wasserstein fuzzy set radius, and δ is the perturbation tolerance parameter.

[0017] Preferably, the constructing of the minimax model comprises:

[0018]

[0019] Also includes Wasserstein fuzzy set constraints:

[0020]

[0021] In the formula, ψ t is a function mapping, For arrive A cluster of measurable functions in a dimensional space, is the joint distribution z t In the Gaussian distribution function distribution in the field, is the Wasserstein fuzzy set constraint function, E represents the expected solution, x t is the true state truth vector, ψ t (y t ) is the current time t based on the observed value y t The calculated state estimate, W2(,) is the type-2 Wasserstein distance, and the Wasserstein fuzzy set radius quantifies the tolerance of the time-varying perturbation of the prior Jacobian matrix.

[0022] Preferably, the type 2 Wasserstein distance comprises:

[0023]

[0024] In the formula, is the first Gaussian distribution, is the second Gaussian distribution, μ1, μ2 are the means of distributions 1 and 2 respectively, Tr is the trace of the matrix, ∑1, ∑2 are the variance matrices of distributions 1 and 2 respectively.

[0025] Preferably, the method further includes reconstructing the minimax model:

[0026]

[0027] In the formula, G t is the sensitivity matrix, g t To define the cutoff vector, c t is the mean vector, is a d-dimensional real number space, is an n-dimensional real number space, c t,x is the state estimation vector, is an m-dimensional real number space, c t,y is the observation vector, S t is a d-dimensional square matrix, represents the set of d*d dimensional positive real numbers, S t,xx is the n*n dimensional estimated covariance matrix, represents the n*n-dimensional positive real number set, S t,yyis the m*m dimensional observation covariance matrix, represents the m*m-dimensional positive real number set, S t,xy , S t,yx Represent the n*m ​​and m*n dimensional covariance matrices respectively, are all possible combinations of n×m matrices with real numbers as elements,

[0028] Preferably, it also includes obtaining by solving the first-order optimal condition constraint of the quadratic minimization problem:

[0029]

[0030] In the formula, is the unique solution for the sensitivity matrix.

[0031] Preferably, based on Through the reconstructed minimax model, we get the minimax game problem to be solved. Through equivalent substitution, we can get the natural decision problem:

[0032]

[0033] In the formula, σ is the minimum eigenvalue of the covariance matrix, I d is the d-dimensional identity matrix.

[0034] Preferably, the updating of the optimal state estimate and covariance matrix at the current moment includes:

[0035]

[0036] In the formula, is the state estimate at time t, μ t,x is the state estimation vector, K t is the Kalman gain matrix at time t, μ t,y is the observation vector, V t is the state estimation covariance at time t, I is the unit matrix, is the optimal state estimated covariance, R t is the observation noise covariance.

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

[0038] 1. The method provided by the present invention effectively suppresses the problem of state estimation accuracy degradation caused by model uncertainty due to Jacobi matrix perturbation in the first-order linear approximation process from the perspective of tolerating model uncertainty, and significantly improves the first-order state estimation accuracy of the humanoid robot nonlinear system.

[0039] 2. With the real state distribution as the center, a Wasserstein distance fuzzy set was constructed as the allowed neighborhood, and the EKF algorithm was modeled as a game state estimation problem based on the Wasserstein distance fuzzy set constraints, which significantly improved the navigation and positioning accuracy of humanoid robots, and is of great significance to the wider application of humanoid robots. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0041] Figure 1 It is a schematic diagram of the process of the present invention;

[0042] Figure 2 It is a comparison diagram of the effects of the present invention. DETAILED DESCRIPTION

[0043] 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.

[0044] The modules or submodules described independently may be physically separated or not: they may be implemented by software or hardware, and some modules or submodules may be implemented by software, and the processor may call the software to implement the functions of these modules or submodules, and other modules or submodules may be implemented by hardware, such as by hardware circuits. In addition, some or all of the modules may be selected according to actual needs to achieve the purpose of the present application.

[0045] Please refer to Figure 1 The present invention provides a humanoid robot foot-walking anti-disturbance navigation method, comprising:

[0046] First, we need to consider a nonlinear system state space:

[0047] x t =f t-1 (x t-1 )+q t

[0048] y t =h t (xt )+r t

[0049] in, and Represent the state value vector and observation value vector of the nonlinear system respectively, and each moment is expressed as f t () is the nonlinear state transfer function, h t () is the corresponding observation function, represents Gaussian noise, which is independent of the initial state distribution

[0050] represents Gaussian observation noise. Q t and R t They represent the state noise q t and observation noise r t The covariance matrix of . Constructing a joint Gaussian distribution d=n+m, at any time Define the historical observation vector as

[0051] S101: Initialize basic parameters;

[0052] Initialize the state estimate x0, the covariance matrix V0 ≥ 0, the Wasserstein fuzzy set radius ρ> 0, the perturbation tolerance parameter δ> 0, the kernel bandwidth parameter σ and a very small positive number ε.

[0053] S102: establishing a prediction state model, obtaining a one-step prediction state estimate through the prediction state model, calculating a covariance matrix of a one-step prediction joint state estimate and a joint distribution through the one-step prediction state estimate, and obtaining a pseudo-nominal nominal distribution based on the covariance matrix of the one-step prediction joint state estimate and the joint distribution;

[0054] S103: Obtain the current observation value, update the state estimation covariance matrix, construct a minimax model, and obtain the probability density of the estimated value distribution of the optimal state through the minimax model;

[0055] S104: Establish a state estimation model, and update the optimal state estimation and covariance matrix at the current moment based on the probability density of the estimated value distribution of the optimal state.

[0056] The method provided by the present invention effectively suppresses the problem of state estimation accuracy degradation caused by model uncertainty due to Jacobi matrix perturbation in the first-order linear approximation process from the perspective of tolerating model uncertainty, and significantly improves the first-order state estimation accuracy of the humanoid robot nonlinear system.

[0057] Secondly, with the true state distribution as the center, a Wasserstein distance fuzzy set was constructed as the allowed neighborhood, and the EKF algorithm was modeled as a game state estimation problem based on the Wasserstein distance fuzzy set constraints, which significantly improved the navigation and positioning accuracy of humanoid robots, and is of great significance for the wider application of humanoid robots.

[0058] In an exemplary embodiment of the present invention, establishing a prediction state model includes:

[0059]

[0060] In the formula, is the one-step forecast state estimate at time t, is the one-step forecast state estimate at time t-1, is the one-step prediction of the observed value at time t-1, is the first-order Jacobian matrix of the state transfer function at time t-1, is the first-order Jacobian matrix of the observation function at time t, To solve for the Jacobian matrix for the subscript function, μ t is the joint distribution, μ t,x is the state vector, μ t,y is the observation vector, ∑ t is the covariance matrix of the joint distribution, V t-1 is the estimated covariance matrix at time t-1, I n is the n*n identity matrix, Q t is the covariance matrix of the state noise, is a pseudo-nominal distribution.

[0061] In an exemplary embodiment of the present invention, updating the state estimation covariance matrix includes using a Frank-Wolfe algorithm;

[0062]

[0063] In the formula, is the state estimation covariance matrix, ρ is the Wasserstein fuzzy set radius, and δ is the perturbation tolerance parameter.

[0064] In an exemplary embodiment of the present invention, based on the one-step prediction pseudo-nominal distribution of the joint distribution, the corresponding minimax game problem can be constructed by introducing Wasserstein fuzzy sets to tolerate the model uncertainty of the matrix time-varying perturbation caused by the first-order approximate solution of the Jacobian matrix. The minimax problem is modeled as follows:

[0065] Building a minimax model involves:

[0066]

[0067] Also includes Wasserstein fuzzy set constraints:

[0068]

[0069] In the formula, ψ t is a function mapping, For arrive A cluster of measurable functions in a dimensional space, is the joint distribution z t In the Gaussian distribution function distribution in the field, is the Wasserstein fuzzy set constraint function, E represents the expected solution, x t is the true state truth vector, ψ t (y t ) is the current time t based on the observed value y t The calculated state estimate, W2(,) is the type-2 Wasserstein distance, and the Wasserstein fuzzy set radius quantifies the tolerance of the time-varying perturbation of the prior Jacobian matrix.

[0070] In an exemplary embodiment of the present invention, the type 2 Wasserstein distance includes:

[0071]

[0072] In the formula, is the first Gaussian distribution, is the second Gaussian distribution, μ1, ∑2 are the means of distributions 1 and 2 respectively, Tr is the trace of the matrix, ∑1, ∑2 are the variance matrices of distributions 1 and 2 respectively.

[0073] By solving the minimax problem, we can obtain the probability density function of the optimal state estimate distribution:

[0074]

[0075] In the formula, is the probability density function, is the state estimation vector at time t, V t is the covariance matrix of the state estimate.

[0076] The following equivalent transformation order is performed on the minimax problem:

[0077]

[0078] An exemplary embodiment of the present invention, wherein ψ t (y t ) can be solved by solving the conditional expectation function It is found that, without loss of generality, the set of measurable functions can be restricted to the set of affine functions parameterized by the sensitivity matrix, and the minimax model can be reconstructed:

[0079]

[0080] In the formula, G t is the sensitivity matrix, g t To define the cutoff vector, c t is the mean vector, is a d-dimensional real number space, is an n-dimensional real number space, c t,x is the state estimation vector, is an m-dimensional real number space, c t,y is the observation vector, S t is a d-dimensional square matrix, represents the set of d*d dimensional positive real numbers, S t,xx is the n*n dimensional estimated covariance matrix, represents the n*n-dimensional positive real number set, S t,yy is the m*m dimensional observation covariance matrix, represents the m*m-dimensional positive real number set, S t,xy , S t,yx Represent the n*m ​​and m*n dimensional covariance matrices respectively, are all possible combinations of n×m matrices with real numbers as elements,

[0081] Solving the internal extreme value problem analytically And substitute the optimal solution The optimal solution of the reconstructed minimax model satisfies Then the reconstructed minimax model is equivalent to:

[0082]

[0083]

[0084] Where σ is the minimum eigenvalue of the covariance matrix, I d is the d-dimensional identity matrix.

[0085] In an exemplary embodiment of the present invention, in an equivalent model of the above-mentioned reconstructed minimax model, the unconstrained quadratic minimization problem is solved in G t There is a unique solution

[0086] It also includes the first-order optimal condition constraints obtained by solving the quadratic minimization problem:

[0087]

[0088] In the formula, is the unique solution for the sensitivity matrix.

[0089] Specifically, based on Through the reconstructed minimax model, we get the minimax game problem to be solved. Through equivalent substitution, we can get the natural decision problem:

[0090]

[0091] Where σ is the minimum eigenvalue of the covariance matrix, I d is the d-dimensional identity matrix.

[0092] Furthermore, the natural decision problem can be expressed as the following Lagrangian conditional extreme value problem:

[0093]

[0094] in,

[0095]

[0096] Where γ is the Lagrange multiplier, f(S t ) is the objective function.

[0097] Among them, the objective function (S t ) meets the following conditions:

[0098]

[0099] definition is the optimal solution to the Lagrangian conditional extreme value problem, then the affine function is the optimal state estimate, and the estimated distribution is

[0100] The objective function of the Lagrangian conditional extreme value problem is replaced by a linear approximation, which can efficiently solve the problem. In this embodiment, the Lagrangian conditional extreme value problem is solved by a deformed Frank-Wolfe algorithm. To begin, the iterations are as follows:

[0101]

[0102] In the formula, represents the initialization of the covariance matrix for the 0th iteration, represents the initialization of the covariance matrix for the j+1th iteration, α j is a properly chosen step size, represents the initialization of the covariance matrix of the jth iteration, and F() is a mapping, specifically, Returns a unique solution to the direction finding problem.

[0103]

[0104] In the formula, F(S t ) is about the matrix S t Mapping, L is a d*d dimensional matrix.

[0105] In summary, the specific steps for solving the Lagrangian conditional extreme value problem through the modified Frank-Wolfe algorithm are:

[0106] Input: Covariance matrix ∑ t >0, Wasserstein radius ρ>0, perturbation tolerance parameter δ>0;

[0107] set up:

[0108] When the stopping criteria are not met, proceed as follows:

[0109] set up: Computing Gradients

[0110] set up:

[0111] ρ←Bisection algorithm(Σ t ,D,ρ,ε);

[0112] set up:

[0113] Set: j←j+1;

[0114] Output:

[0115] Among them, d is the gradient and j is a natural number.

[0116] In the above steps, the steps of the Bisection algorithm are as follows:

[0117] Input: Covariance matrix ∑ t >0, gradient matrix Wasserstein radius ρ>0, tolerance δ>0; define the maximum eigenvalue of D as λ1, v1 is the eigenvector corresponding to the eigenvalue λ1;

[0118]

[0119] Repeat steps:

[0120] Settings: Settings:

[0121] If (if) 0: h(γ) < 0, then (then): UB←γ,

[0122] And (else):

[0123] Until: h(γ)>0 and φ<0

[0124] Output: L.

[0125] Among them, the Bisection algorithm includes an auxiliary function, It is obtained by the following definition:

[0126]

[0127] Where h(γ) is the auxiliary function of the Lagrange multiplier γ, ρ t is the Wasserstein radius, is the identity matrix.

[0128] Definition * is the optimal Lagrange multiplier, which is The only solution under the condition h(γ)=0.

[0129] Finally, through the above process, we can get So we can get the corresponding suboptimal and most unfavorable distribution According to the reconstructed minimax model and Get the optimal solution to the minimax problem

[0130] In an exemplary embodiment of the present invention, updating the optimal state estimation and covariance matrix at the current moment includes:

[0131]

[0132] In the formula, is the state estimate at time t, μ t,x is the state estimation vector, K t is the Kalman gain matrix at time t, μ t,y is the observation vector, V t is the state estimation covariance at time t, I is the unit matrix, is the optimal state estimated covariance, R t is the observation noise covariance.

[0133] The present invention also embodies a humanoid robot foot-type walking anti-disturbance navigation system, comprising:

[0134] An initialization module is configured to initialize basic parameters;

[0135] A priori prediction module is configured to establish a prediction state model, obtain a one-step prediction state estimate through the prediction state model, calculate a covariance matrix of a one-step prediction joint state estimate and a joint distribution through the one-step prediction state estimate, and obtain a pseudo-nominal nominal distribution based on the covariance matrix of the one-step prediction joint state estimate and the joint distribution;

[0136] The posterior update module is configured to obtain the current observation value, update the state estimation covariance matrix, construct a minimax model, and obtain the probability density of the estimated value distribution of the optimal state through the minimax model;

[0137] A computational state estimation module is configured to establish a state estimation model and update the optimal state estimation and covariance matrix at the current moment based on the probability density of the estimated value distribution of the optimal state;

[0138] The main control module is connected with the initialization module, the prior prediction module, the a posteriori update module and the calculation state estimation module, and is used for the above-mentioned humanoid robot foot-type walking anti-disturbance navigation method.

[0139] From the minimax game problem, we can know that the Wasserstein radius ρ can tolerate the first-order linear approximation uncertainty problem of matrix perturbation caused by Jacobi matrix solution within a certain neighborhood of the true distribution. The type-2 Wasserstein distance can well measure the difference between the true distribution and the pseudo-nominal distribution and then tolerate the time-varying model uncertainty caused by matrix perturbation. This enables the MU-EKF algorithm to flexibly and well tolerate the state estimation accuracy degradation problem caused by Jacobi matrix solution in the first-order linear approximation process, greatly improving the navigation and positioning accuracy of the humanoid robot in the nonlinear state space.

[0140] The humanoid robot foot-walking anti-disturbance navigation and positioning technology based on model uncertainty extended Kalman filter (MU-EKF) proposed in this patent is based on the Wasserstein distance between two Gaussian distributions to model the model uncertainty of the first-order linear approximation.

[0141] The state estimation accuracy of the MU-EKF technical method proposed in this patent was simulated and compared with the classic extended Kalman filter (EKF) technical method, and the average steady-state mean square error (MSD) of 500 independent runs was selected for comparison.

[0142]

[0143] In the following simulation, the state equation and observation equation of the nonlinear system are as follows:

[0144] x t+1 (1) = 0.8x t (1)+x t (1)xt (2)+0.1+q t

[0145] x t+1 (2) = 1.5x t (2)-x t (1)x t (2)+0.1+q t

[0146] y t+1 =x t (2)+r t

[0147] The Gaussian distribution of state noise and observation noise is as follows:

[0148]

[0149] In the formula, q t represents the noise introduced during the state transition at time step t, r t represents the noise introduced during the observation at time step t.

[0150] refer to Figure 2 ,The comparison of the estimation accuracy of nonlinear systems between the MU-EKF algorithm proposed in this scheme and the classic EKF algorithm shows that the state estimation convergence speed of the MU-EKF method is significantly better than that of the EKF algorithm, and the estimation accuracy of the MU-EKF algorithm is significantly better than that of the EKF algorithm.

[0151] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0152] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. The computer software product is stored in a storage medium, including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods of various embodiments of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0153] 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. A humanoid robot foot-walking anti-disturbance navigation method, characterized in that: include: Initialize basic parameters; A prediction state model is established, a one-step prediction state estimate is obtained through the prediction state model, a covariance matrix of a one-step prediction joint state estimate and a joint distribution is calculated through the one-step prediction state estimate, and a pseudo-nominal nominal distribution is obtained based on the covariance matrix of the one-step prediction joint state estimate and the joint distribution; Get the current observation value, update the state estimation covariance matrix, build the minimax model, and obtain the probability density of the estimated value distribution of the optimal state through the minimax model; A state estimation model is established, and the optimal state estimation and covariance matrix at the current moment are updated based on the probability density of the estimated value distribution of the optimal state.

2. The anti-disturbance navigation method for foot-type walking of a humanoid robot according to claim 1, characterized in that: The establishment of the prediction state model comprises: In the formula, is the one-step forecast state estimate at time t, is the one-step forecast state estimate at time t-1, is the one-step prediction of the observed value at time t-1, is the first-order Jacobian matrix of the state transfer function at time t-1, is the first-order Jacobian matrix of the observation function at time t, To solve for the Jacobian matrix for the subscript function, μ t is the joint distribution, μ t,x is the state vector, μ t,y is the observation vector, ∑ t is the covariance matrix of the joint distribution, V t-1 is the estimated covariance matrix at time t-1, I n is the n*n identity matrix, Q t is the covariance matrix of the state noise, is a pseudo-nominal distribution.

3. The anti-disturbance navigation method for foot-type walking of a humanoid robot according to claim 2, characterized in that: The updating of the state estimation covariance matrix includes using a Frank-Wolfe algorithm; In the formula, is the state estimation covariance matrix, ρ is the Wasserstein fuzzy set radius, and δ is the perturbation tolerance parameter.

4. The anti-disturbance navigation method for foot-type walking of a humanoid robot according to claim 3, characterized in that: The construction of the minimax model comprises: Also includes Wasserstein fuzzy set constraints: In the formula, ψ t is a function mapping, For arrive A cluster of measurable functions in a dimensional space, is the joint distribution z t In the Gaussian distribution function distribution in the field, is the Wasserstein fuzzy set constraint function, E represents the expected solution, x t is the true state truth vector, ψ t (y t ) is the current time t based on the observed value y t The calculated state estimate, W2(,) is the type-2 Wasserstein distance, and the Wasserstein fuzzy set radius quantifies the tolerance of the time-varying perturbation of the prior Jacobian matrix.

5. The anti-disturbance navigation method for foot-walking of a humanoid robot according to claim 4, characterized in that: The Type 2 Wasserstein distance includes: In the formula, is the first Gaussian distribution, is the second Gaussian distribution, μ1, μ2 are the means of distributions 1 and 2 respectively, Tr is the trace of the matrix, ∑1, ∑2 are the variance matrices of distributions 1 and 2 respectively.

6. The anti-disturbance navigation method for foot-walking of a humanoid robot according to claim 5, characterized in that: It also includes the reconstruction of the minimax model: In the formula, G t is the sensitivity matrix, g t To define the cutoff vector, c t is the mean vector, is a d-dimensional real number space, is an n-dimensional real number space, c t，x is the state estimation vector, is an m-dimensional real number space, c t，y is the observation vector, S t is a d-dimensional square matrix, represents the set of d*d dimensional positive real numbers, S t,xx is the n*n dimensional estimated covariance matrix, represents the n*n-dimensional positive real number set, S t，yy is the m*m dimensional observation covariance matrix, represents the m*m-dimensional positive real number set, S t，xy , S t,yx Represent the n*m ​​and m*n dimensional covariance matrices respectively, are all possible combinations of n×m matrices with real numbers as elements, 7. The anti-disturbance navigation method for foot-walking of a humanoid robot according to claim 6, characterized in that: It also includes the first-order optimal condition constraints obtained by solving the quadratic minimization problem: In the formula, is the unique solution for the sensitivity matrix.

8. The anti-disturbance navigation method for foot-walking of a humanoid robot according to claim 7, characterized in that: based on Through the reconstructed minimax model, we get the minimax game problem to be solved. Through equivalent substitution, we can get the natural decision problem: In the formula, σ is the minimum eigenvalue of the covariance matrix, I d is the d-dimensional identity matrix.

9. The anti-disturbance navigation method for foot-walking of a humanoid robot according to claim 5, characterized in that: The updating of the optimal state estimation and covariance matrix at the current moment includes: In the formula, is the state estimate at time t, μ t，x is the state estimation vector, K t is the Kalman gain matrix at time t, μ t，y is the observation vector, V t is the state estimation covariance at time t, I is the unit matrix, is the optimal state estimated covariance, R t is the observation noise covariance.

Images