Fast Distributed Humanoid Robot State Estimation Method, System, Device and Medium

Abstract

The present invention relates to a method, system, device and medium for fast decentralized humanoid robot state estimation. The method includes: acquiring IMU sensor data and visual measurement data; performing non-linear attitude estimation based on the IMU sensor data and visual measurement data to obtain the attitude estimation information of the floating base at the next moment; acquiring contact information and leg joint information, performing physical constraints based on the contact information, and then performing linear moving window estimation according to the leg joint information and the attitude estimation information of the floating base at the next moment to obtain the speed and position estimation information of the floating base at the next moment; when performing linear moving window estimation, the marginalization method is used to optimize the speed estimation information and position estimation information. The present invention can improve the accuracy of state estimation and the resistance to noise and model uncertainty while reducing the computational complexity.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and particularly to a fast decentralized humanoid robot state estimation method, system, device, and medium. Background Art

[0002] When a humanoid robot walks in a complex and dynamic environment, accurate state estimation is crucial for achieving stable and efficient motion control. Currently, traditional centralized state estimation methods have many drawbacks. Their computational complexity is relatively high. When dealing with a large amount of data, they consume a large amount of computational resources and time, making it difficult for the robot to quickly make accurate state estimations when facing a complex environment. At the same time, this centralized method has poor real-time performance and cannot meet the requirements of the robot for quick response in a dynamic environment.

[0003] To overcome the deficiencies of traditional centralized state estimation methods, decentralized state estimation methods have emerged. This method can distribute computational tasks to multiple processing units, thereby effectively reducing the overall computational burden. This decentralized architecture enables the system to still maintain certain functions when facing partial unit failures, greatly improving the robustness of the system. Moreover, due to the distribution of computational tasks, each processing unit can process data in parallel, significantly improving the real-time response ability of the system, which better meets the application requirements of humanoid robots in complex dynamic environments.

[0004] However, in practical applications, how to achieve accurate and efficient state estimation under a decentralized architecture is still a challenge. On the one hand, the information interaction and cooperation mechanisms among processing units in a decentralized system are relatively complex. How to optimize these mechanisms to ensure the accurate transmission and effective fusion of information, so as to achieve accurate state estimation, is a key issue. On the other hand, while ensuring the estimation accuracy, it is also necessary to consider the computational efficiency and avoid excessive consumption of computational resources and degradation of real-time performance due to complex algorithms and excessive information interaction, which poses higher requirements for algorithm design and system architecture. Summary of the Invention

[0005] Based on this, in view of the above technical problems, it is necessary to provide a fast decentralized humanoid robot state estimation method, system, device, and medium that can reduce computational complexity and improve the accuracy of state estimation.

[0006] A fast decentralized humanoid robot state estimation method, the method comprising:

[0007] Obtain IMU sensor data and visual measurement data; perform non-linear attitude estimation based on the IMU sensor data and the visual measurement data to obtain the floating base attitude estimation information at the next moment;

[0008] Acquiring contact information and leg joint information, performing physical constraints based on the contact information, and then performing linear moving window estimation based on the leg joint information and the next moment floating base posture estimation information to obtain the next moment floating base speed and position estimation information;

[0009] When performing linear moving window estimation, a marginalization method is used to optimize the speed and position estimation information.

[0010] A fast decentralized humanoid robot state estimation system, comprising:

[0011] The attitude estimation module is used to obtain IMU sensor data and visual measurement data; perform nonlinear attitude estimation based on the IMU sensor data and the visual measurement data to obtain the floating base attitude estimation information at the next moment;

[0012] A speed and position estimation module is used to obtain contact information and leg joint information, perform physical constraints based on the contact information, and then perform linear moving window estimation based on the leg joint information and the next moment floating base posture estimation information to obtain the next moment floating base speed and position estimation information;

[0013] The optimization module is used to optimize the speed and position estimation information by using a marginalization method when performing linear moving window estimation.

[0014] A computer device includes a memory and a processor, wherein the memory stores a computer program and the processor implements the steps of the fast decentralized humanoid robot state estimation method when executing the computer program.

[0015] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps of the method for fast decentralized humanoid robot state estimation.

[0016] The above-mentioned fast decentralized humanoid robot state estimation method, system, device and medium obtain IMU sensor data and visual measurement data; perform nonlinear posture estimation based on the IMU sensor data and visual measurement data to obtain floating base posture estimation information at the next moment; obtain contact information and leg joint information, perform physical constraints based on the contact information, and then perform linear moving window estimation based on the leg joint information and the floating base posture estimation information at the next moment to obtain the speed and position estimation information of the floating base at the next moment; when performing linear moving window estimation, the marginalization method is used to optimize the speed and position estimation information.

[0017] The present invention decomposes the entire state estimation task of a humanoid robot into two sub - modules. Each module processes specific sensor data locally, achieving decentralized computing and improving the real - time performance of computing. This method can improve the accuracy of state estimation and the resistance to noise and model uncertainty while reducing the computational complexity through decentralized non - linear attitude estimation and linear moving window estimation. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0019] Figure 1 It is a schematic flowchart of the fast decentralized humanoid robot state estimation method provided in Embodiment 1;

[0020] Figure 2 It is a block diagram of the structure of the fast decentralized humanoid robot state estimation system provided in Embodiment 2;

[0021] Figure 3 It is an internal structure diagram of the computer device provided in Embodiment 3.

[0022] The realization of the object, functional features, and advantages of the present invention will be further described in combination with the embodiments with reference to the drawings. DETAILED DESCRIPTION OF THE EMBODIMENTS

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

[0024] It can be understood that the technical solutions between the various embodiments of the present invention can be combined with each other, but it must be based on the ability of those of ordinary skill in the art to implement. When the combination of technical solutions conflicts or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0025] The following will describe the embodiments of the present invention in detail with reference to the drawings in the embodiments of the present invention.

[0026] Embodiment 1

[0027] This embodiment discloses a fast decentralized humanoid robot state estimation method. By decomposing the entire state estimation task of the humanoid robot into two sub-modules, each module processes specific sensor data locally, achieving decentralized computing and improving the real-time performance of computing. This method can improve the accuracy of state estimation and the resistance to noise and model uncertainty while reducing the computational complexity through decentralized non-linear attitude estimation and linear moving window estimation.

[0028] As Figure 1 shown, the fast decentralized humanoid robot state estimation method provided in this embodiment includes the following steps:

[0029] Step 201: Obtain IMU sensor data and visual measurement data; perform non-linear attitude estimation based on the IMU sensor data and visual measurement data to obtain the floating base attitude estimation information at the next moment.

[0030] Step 202: Obtain contact information and leg joint information. After performing physical constraints based on the contact information, perform linear moving window estimation according to the leg joint information and the floating base attitude estimation information at the next moment to obtain the speed and position estimation information of the floating base at the next moment.

[0031] Step 203: When performing linear moving window estimation, use the marginalization method to optimize the speed and position estimation information.

[0032] In the specific implementation process of step 201, the IMU sensor data is obtained through a nine-axis IMU, and the IMU sensor data can provide a reasonable attitude measurement data . In addition, considering that rapid changes in acceleration will cause the attitude measurement data to have estimation drift, therefore, visual measurement data is introduced as additional calibration data. The visual measurement data is obtained through image acquisition devices such as cameras and is mainly image data. In this step, based on the projection of gravitational acceleration for calibration and by fusing additional visual measurement data, the accuracy and stability of attitude estimation can be improved, the environmental adaptability and interaction ability can be enhanced, and the accuracy and robustness of motion control can be improved.

[0033] In addition, the motion of a humanoid robot is often non-linear. Therefore, using non-linear attitude estimation for a humanoid robot can improve the estimation accuracy, enhance stability and robustness. In this embodiment, the non-linear attitude estimation is mainly completed through the Extended Kalman Filter (EKF).

[0034] Specifically, an attitude dynamics model is constructed, and preliminary nonlinear attitude estimation is performed based on the IMU sensor data to obtain the initial attitude estimation information at the next moment; the drift of the roll and pitch axes in the initial attitude estimation information at the next moment is corrected by measuring the projection of the gravitational acceleration in the body coordinate system; and the vision-related attitude measurement values in the initial attitude estimation information at the next moment are corrected through visual measurement data to obtain the final floating base attitude estimation information at the next moment.

[0035] Among them, the mathematical expression of the posture dynamics model is:

[0036] ;

[0037] In the formula, Indicates the initial posture estimation information for the next moment; Represents the unit matrix, which is a 4×4 unit matrix; represents the angular rate; Indicates converting the rotation velocity vector into a cross product matrix; Indicates the interval time; Represents the collected attitude information, which is attitude quaternion; The first-order approximate Jacobian matrix representing the exponential mapping; represents the angular velocity vector; represents the noise vector related to the angular velocity. For quaternions The first-order approximate Jacobian matrix of the exponential mapping; For the angular rate The first-order approximate Jacobian matrix of the exponential map.

[0038] The drift of the roll and pitch axes in the initial attitude estimation information at the next moment is corrected by measuring the projection of the gravitational acceleration in the body coordinate system. The calculation expression is:

[0039] ;

[0040] ;

[0041] In the formula, Represents the gravitational acceleration projection information; represents the posture matrix; represents the acceleration due to gravity; It represents the ratio of the magnitude of the acceleration vector to the magnitude of the gravity acceleration vector; represents the noise vector associated with acceleration; represents the vector transpose, where Indicates a replaceable variable.

[0042] Based on the visual measurement data, correct the visual-related attitude measurement values in the initial attitude estimation information for the next moment to obtain the final floating base attitude estimation information for the next moment. The calculation expression is as follows:

[0043] ;

[0044] In the formula, represents the floating base attitude estimation information for the next moment; represents the currently acquired attitude information; represents the measurement noise of the attitude; represents the attitude part in the estimation information; represents the estimation information including attitude and position.

[0045] It can be understood that is the attitude part of while is the noise of the attitude measurement in the visual measurement data. In the buffer of the stored IMU measurement values, when new visual measurement data arrives at time the image frame is aligned with the corresponding IMU frame at the same timestamp.

[0046] In the specific implementation process of step 202, the contact information is obtained through the foot-end force sensor, mainly including data such as contact force. The leg joint information is obtained through the joint encoder, mainly including data such as joint angle and angular velocity. When estimating the velocity and position, the constructed evolution process model, measurement model, and additional physical constraints are all represented by state constraints. In addition, to better conform to the motion characteristics of the humanoid robot, a linear moving window estimation is adopted for the position and velocity, which can quickly update the estimation of the robot's position and velocity according to the latest observation data at each new time step, with strong real-time performance and high efficiency. In this embodiment, the linear moving window estimation is mainly completed through moving horizon estimation (MHE).

[0047] Specifically, physical constraints are imposed based on the contact information, including: based on the contact information, a stable constraint model for the contact between the robot's foot-end and the ground is constructed, and the expression is:

[0048] ;

[0049] In the formula, represents the position of the foot-end at the next moment; represents the current position of the foot-end; represents the ground normal force acting on the foot.

[0050] It can be seen that when constructing the stable constraint model, the contact between the foot end and the ground is modeled as a deterministic constraint, that is, the foot will not slide on the ground, so as to construct a stable constraint model when the foot is in stable contact.

[0051] In the above stability constraint model, when using contact sensors, it is equivalent to a Boolean variable. Alternatively, the static constraint of the foot position can be constrained by the velocity constraint Alternative.

[0052] When performing linear moving window estimation, the linear state constraints generated during the constraint process are uniformly described as:

[0053] ;

[0054] In this embodiment, physical constraints provide prior knowledge of the system. By simultaneously considering both measurement and physical constraints, the MHE can better handle sensor noise and outliers, improving the accuracy and robustness of state estimation. The physical constraints enable the humanoid robot to perform well when controlling the enforced static foot contact and eliminate the need for extensive adjustments to the sliding covariance. When the humanoid robot walks on sliding terrain, the stochastic contact model in the EKF can still be enforced in the MHE.

[0055] Based on the leg joint information and the next moment floating base posture estimation information, a linear moving window is estimated to obtain the next moment floating base speed and position estimation information, including:

[0056] Construct the state vector of velocity and position, expressed as:

[0057] ;

[0058] At each discrete time, use the equivalent control input:

[0059] ;

[0060] Then, the state vectors of velocity and position are The linear dynamic evolution of the event is carried out, and the evolution process model expression is:

[0061] ;

[0062] At each time index , the above evolution process model forms a continuous state constraint when performing linear moving window estimation, which can be expressed as:

[0063] .

[0064] In addition, the state vectors of velocity and position When performing linear dynamic evolution of events, the foot position of each leg in the leg joint information is used as the measurement output to construct a measurement constraint model, which is expressed as:

[0065] ;

[0066] In the formula, represents the state vector consisting of position, velocity, foot end position and additional deviation terms; Represents the position of the humanoid robot in the world coordinate system; represents the velocity of the humanoid robot in the world coordinate system; Indicates the foot position of the humanoid robot in the world coordinate system; Indicates the acceleration offset expressed in the body coordinate system; represents the pose matrix corresponding to the pose estimate; represents the acceleration vector; represents the acceleration due to gravity; The state vector representing the velocity and position of the floating base at the next moment; Represents the state vector noise; Indicates the interval time; represents an estimate of the acceleration vector; Representation vector The measured value of The measurement matrix; Represents a vector exist The state of the moment; represents the measurement noise; Represents the N discrete moments contained in the moving window, where represents the number of discrete moments included in the moving window, Represents each discrete moment; represents the noise applied to the state vector; represents the state transition matrix; represents the state transition vector; express The noise applied to the state vector at all times.

[0067] As you can understand, measurement constraints provide real-time information from the leg joint sensors. In linear moving window estimation, measurement constraints and physical constraints together define a feasible solution space. During optimization, the optimal solution is typically sought within this space, minimizing the difference between the estimated state and the measured data while satisfying the operating conditions of the physical system.

[0068] In the specific implementation process of step 203, when the window in the linear moving window estimation slides forward, it is necessary to remove the oldest measurement data in the window from the optimization problem and add the new measurement data. The marginalization method removes the influence of the oldest measurement data from the current optimization problem by calculating the arrival cost, which includes the following steps:

[0069] Step 301: construct an optimization problem including all measurement data, the solution of which is the state estimation within the current window.

[0070] Step 302: Solve the KKT (Karush-Kuhn-Tucker) condition and calculate the marginalization result of the oldest data. In this step, the covariance matrix of the oldest data is inverted and the covariance matrix of the current state is updated.

[0071] In step 303, the marginalization result is used as the arrival cost to initialize a new optimization problem. In this way, the linear moving window estimation process is guaranteed to be equivalent to the nonlinear pose estimation process during the window sliding process.

[0072] Marginalization can effectively reduce the computational burden because the data of the entire window does not need to be recalculated. At the same time, the marginalization method ensures that the optimal state estimate can still be provided at each window update during the linear moving window estimation process.

[0073] It is worth noting that the method provided in this embodiment is applicable to a humanoid robot equipped with an IMU, leg joint encoders, and visual information input. However, it can also be applied to other humanoid robots with similar data acquisition capabilities, depending on the situation. For example, the IMU can be replaced with a motion capture system, the leg joint encoders with linear displacement sensors, and the visual information input with a lidar.

[0074] Although this embodiment Figure 1 The steps in the diagram are shown in the order indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order restriction for the execution of these steps, and these steps can be executed in other orders. In addition, Figure 1 At least part of the steps may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least part of the sub-steps or stages of other steps.

[0075] Example 2

[0076] Based on the fast decentralized humanoid robot state estimation method in Embodiment 1, this embodiment discloses a fast decentralized humanoid robot state estimation system, as Figure 2 shown. The fast decentralized humanoid robot state estimation system includes: an attitude estimation module 401, a velocity and position estimation module 402, and an optimization module 403, where:

[0077] The attitude estimation module 401 is used to obtain IMU sensor data and visual measurement data; perform non-linear attitude estimation based on the IMU sensor data and visual measurement data to obtain the floating base attitude estimation information at the next moment. That is to say, the attitude estimation module 401 can be used to process acceleration, angular velocity data, and visual measurement data.

[0078] The velocity and position estimation module 402 is used to obtain contact information and leg joint information. After performing physical constraints based on the contact information, perform linear moving window estimation according to the leg joint information and the floating base attitude estimation information at the next moment to obtain the velocity and position estimation information of the floating base at the next moment. The velocity and position estimation module 402 can be used to process multi-rate sensor data and perform optimization within a fixed window.

[0079] The optimization module 403 is used to optimize the velocity and position estimation information by using the marginalization method when performing linear moving window estimation. The optimization module 403 is based on the optimization structure of the full information filter and mainly calculates the arrival cost of the linear moving window estimation problem.

[0080] In this embodiment, the specific working processes and working principles of the attitude estimation module 401, the velocity and position estimation module 402, and the optimization module 403 are the same as those of the method in Embodiment 1, so they will not be elaborated in this embodiment. Each of these unit modules can be implemented in whole or in part by software, hardware, and their combination. Each unit module can be embedded in the processor of the computer device in hardware form or be independent of it, or be stored in the memory of the computer device in software form, so that the processor can call and execute the operations corresponding to each of these unit modules.

[0081] Embodiment 3

[0082] As Figure 3 shown, a terminal device disclosed in this embodiment includes a transmitter, a receiver, a memory, and a processor. Among them, the transmitter is used to send instructions and data, the receiver is used to receive instructions and data, the memory is used to store computer execution instructions, and the processor is used to execute the computer execution instructions stored in the memory to implement the method in Embodiment 1 above.

[0083] It should be noted that the above-mentioned memory can be either independent or integrated with the processor. When the memory is independently provided, the terminal device further includes a bus for connecting the memory and the processor.

[0084] Embodiment 4

[0085] This embodiment discloses a computer-readable storage medium in which computer-executable instructions are stored. When the processor executes the computer-executable instructions, the method in Embodiment 1 above is implemented.

[0086] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above methods. Among them, any reference to a memory, storage, database, or other medium used in the various embodiments provided in the present application can include non-volatile and / or volatile memories. Non-volatile memories can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memories can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and Rambus dynamic RAM (RDRAM), etc.

[0087] The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity of description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered as the scope described in this specification.

[0088] The above-described embodiments merely represent several implementation manners of the present invention. The description thereof is relatively specific and detailed, but it should not be construed as a limitation on the scope of the invention. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present invention, several modifications and improvements can still be made, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the present invention should be subject to the appended claims.

Claims

1. A fast distributed humanoid robot state estimation method, characterized in that, The method includes: Obtaining IMU sensor data and visual measurement data; performing non-linear attitude estimation based on the IMU sensor data and the visual measurement data to obtain the floating base attitude estimation information at the next moment; Obtaining contact information and leg joint information, performing physical constraints based on the contact information, and then performing linear moving window estimation according to the leg joint information and the floating base attitude estimation information at the next moment to obtain the speed and position estimation information of the floating base at the next moment; When performing linear moving window estimation, the marginalization method is used to optimize the speed and position estimation information; Performing non-linear attitude estimation based on the IMU sensor data and the visual measurement data to obtain the floating base attitude estimation information at the next moment, including: Constructing an attitude dynamics model, performing preliminary non-linear attitude estimation according to the IMU sensor data to obtain the initial attitude estimation information at the next moment; Correcting the drifts of the roll and pitch axes in the initial attitude estimation information at the next moment by measuring the projection of the gravitational acceleration in the body coordinate system; and correcting the attitude measurement values related to vision in the initial attitude estimation information at the next moment through the visual measurement data to obtain the final floating base attitude estimation information at the next moment; The mathematical expression of the attitude dynamics model is: ； In the formula, represents the initial attitude estimation information at the next moment; represents the identity matrix; represents the angular rate; represents converting the rotational velocity vector into a cross-product matrix; represents the time interval; represents the attitude information collected currently; represents the first-order approximate Jacobian matrix of the exponential map; represents the angular velocity vector; represents the noise vector related to the angular velocity.

2. The rapid decentralized humanoid robot state estimation method according to claim 1, characterized in that Correcting the drifts of the roll and pitch axes in the initial attitude estimation information at the next moment by measuring the projection of the gravitational acceleration in the body coordinate system, and the calculation expression is: ； wherein, represents the projection information of gravitational acceleration; represents the attitude matrix; represents the gravitational acceleration; represents the ratio of the magnitude of the acceleration vector to the magnitude of the gravitational acceleration vector; represents the noise vector related to acceleration; represents the vector transpose, wherein, represents the replaceable variable.

3. The rapid decentralized humanoid robot state estimation method according to claim 1, characterized in that, Correcting the attitude measurement values related to vision in the initial attitude estimation information at the next moment through the visual measurement data to obtain the final floating base attitude estimation information at the next moment, and the calculation expression is: ； In the formula, represents the floating base attitude estimation information at the next moment; represents the attitude information collected currently; represents the measurement noise of the attitude; represents the attitude part in the estimation information; represents the estimation information including attitude and position.

4. The rapid decentralized humanoid robot state estimation method according to any one of claims 1 to 3, characterized in that Performing physical constraints based on the contact information, including: Based on the contact information, constructing a stable constraint model for the contact between the robot foot end and the ground, and the expression is: ； In the formula, represents the position of the foot tip at the next moment; represents the current position of the foot tip; represents the normal ground force acting on the foot.

5. The fast decentralized humanoid robot state estimation method according to claim 4, characterized in that Performing linear moving window estimation according to the leg joint information and the floating base attitude estimation information at the next moment to obtain the speed and position estimation information of the floating base at the next moment, including: Constructing a state vector of speed and position, expressed as: ； At each discrete time, using an equivalent control input: ； Then, the state vectors of speed and position undergo piecewise linear dynamic evolution, and the model expression of the evolution process is as follows: ； The evolution process model forms continuous state constraints, which are: ； State vector of speed and position When performing incident linear dynamic evolution, using the leg joint information as the measurement output, a measurement constraint model is constructed, and the expression is: ； In the formula, represents the state vector composed of position, velocity, foot end position, and additional deviation term; represents the position of the humanoid robot in the world coordinate system; represents the velocity of the humanoid robot in the world coordinate system; represents the foot end position of the humanoid robot in the world coordinate system; represents the acceleration offset represented in the body coordinate system; represents the attitude matrix corresponding to the attitude estimation value; represents the acceleration vector; represents the gravitational acceleration; represents the velocity estimation and position estimation state vector of the floating base at the next moment; represents the time interval; represents the estimation of the acceleration vector; represents the vector measurement value of; represents the vector measurement matrix of; represents the vector at state at the moment; represents the measurement noise; represents N discrete moments included in the moving window; represents the noise applied to the state vector; represents the state transition matrix; represents the state transition vector; represents noise applied to the state vector at the moment.

6. A fast decentralized humanoid robot state estimation system, characterized in that, Adopting the fast decentralized humanoid robot state estimation method according to any one of claims 1 to 5, the system includes: An attitude estimation module, configured to obtain IMU sensor data and visual measurement data; perform non-linear attitude estimation based on the IMU sensor data and the visual measurement data to obtain the floating base attitude estimation information at the next moment; A speed and position estimation module, configured to obtain contact information and leg joint information, perform physical constraints based on the contact information, and then perform linear moving window estimation according to the leg joint information and the floating base attitude estimation information at the next moment to obtain the speed and position estimation information of the floating base at the next moment; An optimization module, configured to use the marginalization method to optimize the speed and position estimation information when performing linear moving window estimation.

7. A computer device, comprising a memory and a processor, the memory storing a computer program, characterized in that, When the processor executes the computer program, the steps of the fast decentralized humanoid robot state estimation method according to any one of claims 1 to 5 are implemented.

8. A computer-readable storage medium, having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, the steps of the fast decentralized humanoid robot state estimation method according to any one of claims 1 to 5 are implemented.

Images