Humanoid robot real-time positioning and mapping method and system

Abstract

The application belongs to the technical field of humanoid robots, and particularly relates to a real-time positioning and mapping method and system for a humanoid robot, and proposes an improved Fast-LIO2 algorithm, adopts a Kalman filter to fuse the humanoid robot foot end forward kinematics and IMU data, and accurately estimates the position, attitude and speed of the robot in the world coordinate system. The obtained smooth pose trajectory is used to replace the IMU back propagation result of the point cloud de-distortion link in the traditional Fast-LIO2 algorithm, so as to ensure the space-time consistency of the point cloud in the whole scanning period. The improved method effectively overcomes the problems that the traditional Fast-LIO2 does not consider the gait characteristics and lacks external constraints in the application of the humanoid robot, significantly reduces the point cloud distortion and ghosting phenomenon, and improves the positioning robustness and mapping accuracy of the humanoid robot in a complex dynamic environment.

Description

Technical Field

[0001] This application belongs to the field of humanoid robot technology, specifically relating to a method and system for real-time localization and mapping of humanoid robots. Background Technology

[0002] With the rapid development of humanoid robot technology, robots not only need to possess humanoid locomotion capabilities but also must achieve autonomous perception and decision-making in complex and dynamic environments. Real-time localization and mapping (SLAM) technology is the core for realizing autonomous walking and environmental interaction in humanoid robots. Existing SLAM technologies mainly include vision-based SLAM and LiDAR-based SLAM. Vision SLAM lacks robustness in environments with weak textures or large changes in lighting conditions, while LiDAR, due to its strong anti-interference capabilities and high ranging accuracy, is more widely used in complex environments. The Fast-LIO2 algorithm has become a representative method in LiDAR inertial odometry. Fast-LIO2 fuses LiDAR and IMU data through tight coupling, utilizes the forward and backward propagation of the IMU to achieve point cloud distortion correction, and then combines factor graph optimization to achieve real-time estimation and mapping of the robot's state. This method has the advantages of high computational efficiency and strong real-time performance, and exhibits good robustness and accuracy in scenarios such as UAVs and unmanned vehicles. However, the traditional Fast-LIO2 algorithm has significant limitations when applied to humanoid robots. Summary of the Invention

[0003] To address the aforementioned problems, this application proposes an improved Fast-LIO2 algorithm. This algorithm uses a Kalman filter to fuse the forward kinematics of the humanoid robot's feet with IMU data, accurately estimating the robot's position, attitude, and velocity in the world coordinate system. The technical solution is as follows:

[0004] A method for real-time localization and mapping of a humanoid robot includes the following steps:

[0005] S1. Calculate the position, orientation, and velocity of the humanoid robot at time k steps, which serve as the prediction equations for the Kalman filter;

[0006] S2. The pose information of the foot end is obtained by positive kinematics calculation of the humanoid robot's legs, and the overall pose of the robot is further derived. This pose is then used as the input of the observation model of the Kalman filter to correct the state estimate.

[0007] S3. A Kalman filter is used to perform recursive optimal estimation of the system state, thereby obtaining the position, attitude and velocity of the humanoid robot;

[0008] S4. Select the reference pose and interpolate the continuous pose trajectory provided by the Kalman filter:

[0009] S5. Calculate relative pose to achieve time alignment.

[0010] Preferably, in S1, the coordinate system of the humanoid robot is divided into a body coordinate system and a world coordinate system. It is assumed that the initial position of the IMU coordinate system coincides with the origin of the body coordinate system. The IMU sensor provides three-axis acceleration, three-axis angular velocity, and Euler angles describing the rotation of the robot around its own axis. The rotation order of the Euler angles is ZYX. According to the definition of the Euler angles, a rotation matrix from the body coordinate system to the world coordinate system is constructed.

[0011] The acceleration acquired by the IMU is transformed from the body coordinate system to the world coordinate system using the following formula:

[0012] ;

[0013] ;

[0014] ;

[0015] In the formula This represents acceleration in the world coordinate system. This represents acceleration in the body coordinate system; , , These represent the accelerations along the x, y, and z axes in the body coordinate system, respectively. Represents the angular velocity in the world coordinate system; This represents the angular velocity in the body coordinate system.

[0016] Preferably, the position, orientation, and velocity of the humanoid robot at time k steps are calculated using IMU data and used as the prediction equation for the Kalman filter:

[0017] ;

[0018] ;

[0019] ;

[0020] Velocity in the world coordinate system This refers to the position in the world coordinate system. For acceleration, The time difference between two adjacent steps;

[0021] Solving the quaternion differential equation using the first-order Runge-Kutta method yields the following update equation for the quaternion:

[0022] ;

[0023] In the formula:

[0024] ;

[0025] , , This represents the angular velocity vector in the world coordinate system after IMU transformation; Update the matrix for angular velocity quaternions; The attitude in the world coordinate system.

[0026] Discretizing the above equations, we obtain the Kalman filter state equation as follows:

[0027] ;

[0028] I is the identity matrix. This is the input value at time k-1.

[0029] Preferably, the overall attitude calculation steps in step S2 are as follows:

[0030] The overall posture of the humanoid robot is obtained by using spherical linear interpolation based on the quaternions of the left and right foot postures.

[0031] ;

[0032] In the formula: and The postures of the left and right feet are shown respectively. As a weighting factor;

[0033] A complete gait cycle consists of two phases: a support phase and a swing phase. Assuming the support phase and the swing phase last for the same duration, the following formula applies:

[0034] ;

[0035] In the formula: T s It is the duration of the supporting phase, T w It is the duration of the oscillation phase, and the sum of the two should be the time of one gait cycle, where t is the current time of one gait cycle;

[0036] The formula for Slerp, a spherical linear interpolation method, is as follows:

[0037] ;

[0038] In the formula: These are interpolation parameters. for and The angle between them is calculated using the following formula:

[0039] ;

[0040] Obtain the overall posture of the humanoid robot:

[0041] ;

[0042] It is the real part; This is the imaginary part; if the time ratios of the support phase and the oscillation phase are different, the coefficients in the formula should be adjusted according to the specific gait requirements.

[0043] Preferably, the overall position calculation steps in step S2 are as follows:

[0044] Assuming the height of the humanoid robot's center of mass along the z-axis remains constant, and it continuously cycles during walking, causing the robot's center of mass to oscillate periodically between the left and right sides, the following formula can be used to estimate the humanoid robot's position along the x-axis and y-axis:

[0045] ;

[0046] ;

[0047] In the formula: Let T be the overall position of the robot, λ be the weighting factor, and T be the weighting factor. s It is the duration of the supporting phase, T w It is the duration of the oscillation phase, T c Let t be the current time of a gait cycle.

[0048] Preferably, in step S2, an observation model is established:

[0049] .

[0050] The state at time k, Let k be the observation value at time k. , , It refers to the specific values ​​contained in the observed values.

[0051] Preferably, step S3 uses a Kalman filter to perform recursive optimal estimation of the system state, thereby obtaining the position, attitude, and velocity of the humanoid robot. The specific steps are as follows:

[0052] Prediction Step:

[0053] ;

[0054] In the formula Let be the prior state estimate at time k. Let B be the optimal estimate at time k-1, and let B be the control matrix.

[0055] Update the prior estimate covariance matrix:

[0056] ;

[0057] In the formula: Let represent the prior state estimation covariance matrix at time k; It is the optimal estimated covariance matrix at time k-1; assuming the process noise is zero-mean white noise, the prediction noise covariance matrix Q can be derived according to the following formula:

[0058] ;

[0059] In the formula: Let be the covariance matrix of the process noise. The noise input matrix;

[0060] ;

[0061] In the formula: and These are the noise variances of the accelerometer and gyroscope, respectively.

[0062] ;

[0063] In the formula: It is a mapping matrix of quaternion increments;

[0064] ;

[0065] Calculate the Kalman gain:

[0066] ;

[0067] Let K be the Kalman gain at time k, and N be the observation noise covariance matrix;

[0068] Determine the observation noise covariance matrix;

[0069] ;

[0070] In the formula: For location observation noise, This is for attitude observation noise;

[0071] ;

[0072] In the formula , , and The measurement noise variances for the left and right foot positions and postures are respectively. and The variance of the model error representing position and attitude; This is a weighting factor in the overall location estimation; This is a weighting factor in the overall attitude estimation;

[0073] Calculate the optimal estimate:

[0074] ;

[0075] Update error covariance:

[0076] ;

[0077] Through the above steps, the posture, position, and velocity of the humanoid robot can be obtained based on the Kalman filter algorithm; since the posture is represented by quaternions, the quaternions must be converted into a rotation matrix R.

[0078] Preferably, step S4 involves selecting the reference pose at a specific time:

[0079] A unified reference time t is selected within the scanning period of the point cloud. ref And all point cloud data are corrected to the position at that time, the pose at the reference time is represented as:

[0080] ;

[0081] in, This represents the rotation matrix at the reference time. Indicates the position at the reference time;

[0082] To obtain the robot's pose T(t) relative to the world coordinate system at that moment j Interpolation is required based on the continuous pose trajectory provided by the Kalman filter.

[0083] Position interpolation: ;

[0084] Attitude interpolation: ;

[0085] In the formula: , t a and t b They are adjacent to t respectively j Two known moments.

[0086] Preferably, step S5 calculates the relative pose to achieve time alignment:

[0087] In order to obtain the points at sampling time t j pose unified to reference time t ref It is necessary to calculate the relative pose transformation between the two:

[0088] ;

[0089] For any radar point O j At reference time t ref The pose can be obtained in the following way:

[0090] .

[0091] A real-time localization and mapping system for humanoid robots includes a data acquisition module, a data processing module, a control module, and a display module;

[0092] Data acquisition module: Acquires motion data of the humanoid robot;

[0093] Data processing module: Using the improved Fast-LIO2 algorithm, a Kalman filter is used to fuse the forward kinematics of the humanoid robot's feet with IMU data to estimate the robot's position, attitude and velocity in the world coordinate system;

[0094] Control module: Controls the robot to move according to the predetermined settings based on the information from the data processing module;

[0095] Display module: Visualizes the motion status and data of the humanoid robot.

[0096] Compared with the prior art, the beneficial effects of this application are as follows:

[0097] Humanoid robots alternate between a support phase and a swaying phase during walking. This method combines the periodic characteristics of gait to obtain a more stable pose reference during the support phase, which is used to correct the overall state estimation. Simultaneously, continuous and smooth pose trajectories are used to distort the point cloud, ensuring spatiotemporal consistency and reducing the impact of pose fluctuations caused by gait on point cloud processing. This not only improves the stability of state estimation but also significantly enhances the accuracy and smoothness of the distorted point cloud, thereby improving the overall quality of the mapping.

[0098] This method effectively suppresses the cumulative drift problem caused by zero bias, noise, and temperature drift by incorporating foot-to-foot kinematics information and combining it with Kalman filtering to correct the IMU integration results. This improves the accuracy of pose estimation throughout the entire point cloud scanning cycle.

[0099] This method introduces foot-end positive kinematics information as an external redundant constraint, providing additional pose observations for the system. This effectively compensates for the shortcomings of LiDAR and IMU, improves the accuracy and reliability of state estimation, and ensures the robustness of the system in dynamic and complex environments. Attached Figure Description

[0100] Figure 1 Flowchart of a humanoid robot;

[0101] Figure 2 A sketch of a humanoid robot;

[0102] Figure 3 A kinematic model of the legs of a humanoid robot;

[0103] Figure 4 For the mapping results;

[0104] Figure 5 This is the location result. Detailed Implementation

[0105] The technical solution of this application will be described in detail below with reference to specific embodiments and accompanying drawings. It should be understood that the embodiments and specific features in the embodiments are detailed descriptions of the technical solution of this application, rather than limitations thereof. Specific technical features can be combined with each other.

[0106] A method for real-time localization and mapping of a humanoid robot includes the following steps:

[0107] Step 1: Calculate the position, orientation, and velocity of the humanoid robot at time k steps using IMU data, and use these as the prediction equations for the Kalman filter.

[0108] 1.1 IMU coordinate transformation:

[0109] like Figure 2 As shown, the coordinate system of the humanoid robot is divided into the body coordinate system and the world coordinate system. In this invention, since the laser radar of the humanoid robot is installed below the head and the laser radar has a built-in IMU, the installation position of the laser radar is set as the body coordinate system {O}. B The origin of the world coordinate system is defined by the coordinate system {O}. The x-axis points directly in front of the humanoid robot, the z-axis points directly above it, and the direction of the y-axis can be determined using a right-handed coordinate system. W The origin is located at the projection of the body coordinate system onto the ground, and the distance from the origin of the body coordinate system is h. It is also a right-handed coordinate system.

[0110] Since the IMU is integrated into the lidar, this paper assumes that the initial position of the IMU coordinate system coincides with the origin of the body coordinate system. The IMU sensor provides three-axis acceleration, three-axis angular velocity, and Euler angles (roll angles) describing the body's rotation about its own axis. Pitch β, yaw γ). The rotation order of Euler angles is ZYX. Based on this definition of Euler angles, a rotation matrix from the body coordinate system to the world coordinate system can be constructed.

[0111] ;

[0112] In the formula:

[0113] ;

[0114] The homogeneous transformation matrix from the body coordinate system to the world coordinate system is:

[0115] ;

[0116] The acceleration acquired by the IMU is transformed from the body coordinate system to the world coordinate system according to the following formula.

[0117] ;

[0118] ;

[0119] In the formula This represents acceleration in the world coordinate system. This represents the acceleration in the body coordinate system. , , These represent the accelerations along the x, y, and z axes in the body coordinate system, respectively.

[0120] Similarly, the angular velocity in the body coordinate system can also be... Transformed world coordinate system:

[0121] .

[0122] 1.2 Quaternion Rotation Matrix and Attitude Calculation:

[0123] In humanoid robot localization and mapping, pose is used to describe the rotational relationship between the robot's body coordinate system and the world coordinate system. Common pose representation methods include Euler angles, direction cosine matrices, and quaternions. Quaternions are widely used due to their concise expression, low computational cost, high storage efficiency, and ability to effectively avoid singularity problems. The mathematical representation of a quaternion is shown below:

[0124] ;

[0125] In the formula: It is the real part; It is the virtual part.

[0126] The above equation can be solved using quaternion differential equations:

[0127] ;

[0128] in This represents the angular velocity vector of the machine in its own coordinate system, as measured by the IMU. It can be transformed to the world coordinate system, allowing the calculation of the quaternion in the world coordinate system.

[0129] Rewrite the above equation in matrix form:

[0130] ;

[0131] The conversion formula between quaternions and rotation matrices is:

[0132] ;

[0133] After normalizing the quaternions, the rotation matrix can be calculated:

[0134] .

[0135] 1.3. Establish a prediction model:

[0136] The position, orientation, and velocity of the humanoid robot at time k steps are calculated using IMU data and used as the prediction equation for the Kalman filter. The formula is:

[0137] Location: ;

[0138] attitude: ;

[0139] speed: ;

[0140] Solving the quaternion differential equation using the first-order Runge-Kutta method yields the following update equation for the quaternion:

[0141] ;

[0142] In the formula The quaternion update matrix represents the angular velocity:

[0143] ;

[0144] Discretizing the above equations, we obtain the Kalman filter state equation as follows:

[0145] .

[0146] Step 2: Obtain the pose information of the foot end through positive kinematics calculation of the humanoid robot's legs, and further deduce the overall pose of the robot. Use this as the input of the observation model of the Kalman filter to correct the state estimate.

[0147] 2.1 Forward kinematics analysis of the humanoid robot's legs:

[0148] When performing kinematic analysis on the humanoid robot's legs, the legs are considered to be composed of six single-degree-of-freedom rotational joints connected in series. The kinematic model of the humanoid robot's legs is as follows: Figure 3 As shown in Table 1, since the left and right legs of the humanoid robot are symmetrical and calculated in the same way, this paper only derives and calculates the left leg; the calculation for the right leg is similar. A DH parameter table is established based on the relevant parameters of the humanoid robot, as shown in Table 1.

[0149] Table 1. DH Parameter Table for the Left Leg of the Humanoid Robot

[0150] .

[0151] According to the chain rule of coordinate system transformation, the transformation matrix from coordinate system {i-1} to coordinate system {i} can be written as:

[0152] ;

[0153] To reduce the complexity of the formula, s is used instead of sin and c is used instead of cos in the formula.

[0154] The final homogeneous transformation matrix of the end-effector coordinate system of the humanoid robot's left foot to the body coordinate system is:

[0155] ;

[0156] In the formula:

[0157] ;

[0158] .

[0159] The homogeneous transformation matrix obtained from the above equation can be decomposed into the pose matrix and position vector of the humanoid robot's feet, as shown in the following equation:

[0160] .

[0161] The foot posture matrix is ​​transformed from the body coordinate system to the world coordinate system according to the following formula:

[0162] .

[0163] The foot position is transformed from the body coordinate system to the world coordinate system according to the following formula:

[0164] .

[0165] 2.2 Calculate the overall pose based on the foot pose of the humanoid robot:

[0166] In gait analysis, the swing phase and the stance phase are the two main stages describing leg movement within a single gait cycle. The stance phase is when one of the robot's legs is in contact with the ground and bears part or all of the body's weight. The swing phase is when one of the robot's legs leaves the ground and swings forward. In normal gait, each leg alternates between these two phases, allowing the humanoid robot to walk normally.

[0167] 1) Overall attitude estimation:

[0168] Spherical linear interpolation (Slerp) enables a smooth transition between two rotational quaternions. Based on the pose quaternions of the left and right feet, the overall pose of the humanoid robot is obtained through spherical linear interpolation.

[0169] ;

[0170] In the formula: and These represent the postures of the left and right feet, respectively. μ is a weighting factor that can be dynamically set according to the gait phases. A complete gait cycle consists of two phases: the support phase and the swing phase. Assuming the duration of the support phase and the swing phase are the same, the following formula applies:

[0171] ;

[0172] In the formula: T s It is the duration of the supporting phase, T w It is the duration of the oscillation phase, and the sum of the two should be the time of one gait cycle, where t is the current time of one gait cycle.

[0173] Slerp's formula is:

[0174] ;

[0175] In the formula: These are interpolation parameters. for and The angle between them is calculated using the following formula:

[0176] ;

[0177] The overall posture of the humanoid robot can be obtained:

[0178] ;

[0179] If the time ratios of the support phase and the oscillating phase are different, the coefficients in the formula can be adjusted according to the specific gait requirements. For humanoid robots with variable speed motion, the weighting factor μ can be calculated in real time according to the robot's movement speed, which will not be discussed further in this invention.

[0180] 2) Overall location estimation:

[0181] During walking, the humanoid robot's center of mass changes periodically with the switching between the support and swing phases. At the beginning of the gait cycle, assuming the left leg is the support leg, the robot's center of mass is biased towards it. As the right leg enters the swing phase and gradually moves forward, the robot's center of mass gradually shifts to the right. When the right leg lands and becomes the support leg, the left leg enters the swing phase, and the robot's center of mass shifts to the left again, until the left foot lands again, completing a full gait cycle. This process repeats continuously during walking, causing the robot's center of mass to swing periodically between the left and right sides.

[0182] ;

[0183] In the formula, Let λ represent the overall position of the robot, and λ be a weighting factor. This invention assumes that the height of the humanoid robot's center of mass on the z-axis remains constant, and only uses this method to estimate the humanoid robot's position on the x-axis and y-axis.

[0184] ;

[0185] In the formula: T s It is the duration of the supporting phase, T w It is the duration of the oscillation phase, T c Let t be the current time of a gait cycle.

[0186] Establish an observation model:

[0187] .

[0188] Step 3: Use a Kalman filter to perform recursive optimal estimation of the system state, thereby obtaining the position, attitude and velocity of the humanoid robot.

[0189] Prediction Step:

[0190] ;

[0191] In the formula Let be the prior state estimate at time k. This is the optimal estimate at time k-1.

[0192] Update the prior estimate covariance matrix:

[0193] ;

[0194] In the formula: Let represent the prior state estimation covariance matrix at time k; This is the optimal estimated covariance matrix at time k-1. The system's process noise mainly originates from the measurement noise of the accelerometer and gyroscope. This invention assumes the process noise is zero-mean white noise; then, the prediction noise covariance matrix Q can be derived according to the following formula.

[0195] ;

[0196] In the formula: Let be the covariance matrix of the process noise. This is the noise input matrix.

[0197] ;

[0198] In the formula: and These are the noise variances of the accelerometer and gyroscope, respectively.

[0199] ;

[0200] In the formula: It is a mapping matrix for quaternion increments:

[0201] .

[0202] Calculate the Kalman gain:

[0203] ;

[0204] Determine the observation noise covariance matrix:

[0205] ;

[0206] In the formula: For location observation noise, This refers to attitude observation noise.

[0207] ;

[0208] In the formula , , and The measurement noise variances for the left and right foot positions and postures are respectively. and The variance of the model error represents the position and orientation.

[0209] Calculate the optimal estimate:

[0210] ;

[0211] Update error covariance:

[0212] ;

[0213] Through the above steps, the pose, position, and velocity of the humanoid robot can be obtained based on the Kalman filter algorithm. Since the pose is represented by quaternions, it is necessary to convert the quaternions into a rotation matrix R to facilitate subsequent pose calculations and point cloud distortion removal.

[0214] Step 4: Select the reference time pose:

[0215] LiDAR generates point clouds by scanning point by point; therefore, a complete frame of point cloud is actually a collection of sampling points at different times. If these points are directly considered as observations at the same moment, distortion errors will inevitably occur. To ensure the consistency of the point cloud in terms of spatiotemporal reference, a unified reference time t needs to be selected within the scanning period of the point cloud. ref And all point cloud data are corrected to the position at that time. The pose at the reference time can be represented as: .in, This represents the rotation matrix at the reference time. Indicates the position at the reference time.

[0216] Step 5: Because point cloud sampling is sequential, each point cloud O j Sampling time t j They are all different. To obtain the robot's pose T(t) relative to the world coordinate system at that moment... j Interpolation is required based on the continuous pose trajectory provided by the Kalman filter.

[0217] Position interpolation: ;

[0218] Attitude interpolation: ;

[0219] In the formula: , t a and t b They are adjacent to t respectively j Two known time points are given. By interpolation, the pose at any time point within the scanning period can be obtained, thus ensuring the continuity and smoothness of point cloud correction.

[0220] 6. Calculate relative pose to achieve time alignment:

[0221] In order to obtain the points at sampling time t j pose unified to reference time t ref It is necessary to calculate the relative pose transformation between the two:

[0222] ;

[0223] For any radar point O j At reference time tref The pose can be obtained in the following way:

[0224] ;

[0225] Through the above steps, each point is corrected to a uniform reference time, and the distortion caused by the temporal differences within the scanning cycle is effectively eliminated, thereby obtaining a spatiotemporally consistent distortion-free point cloud, which improves the accuracy and robustness of subsequent matching and mapping.

[0226] The meanings of the formula symbols are shown in Table 2-4:

[0227] Table 2 One of the meanings of the letters

[0228] .

[0229] Table 3. Meaning of Letters (Part Two)

[0230] .

[0231] Table 4. Meaning of Letters (Part 3)

[0232] .

[0233] A real-time localization and mapping system for humanoid robots includes a data acquisition module, a data processing module, a control module, and a display module;

[0234] Data acquisition module: Acquires motion data of the humanoid robot;

[0235] Data processing module: Using the improved Fast-LIO2 algorithm, a Kalman filter is used to fuse the forward kinematics of the humanoid robot's feet with IMU data to estimate the robot's position, attitude and velocity in the world coordinate system;

[0236] Control module: Controls the robot to move according to the predetermined settings based on the information from the data processing module;

[0237] Display module: Visualizes the motion status and data of the humanoid robot.

[0238] The above description is only a preferred embodiment of this application. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the technical principles of this application, and these improvements and modifications should also be considered within the scope of protection of this application.

Claims

1. A method for real-time localization and mapping of a humanoid robot, characterized in that, Includes the following steps: S1. The coordinate system of the humanoid robot is divided into the body coordinate system and the world coordinate system. The position, attitude, and velocity of the humanoid robot at time k steps are calculated using IMU data and used as the prediction equations for the Kalman filter: ； ； ； Velocity in the world coordinate system This refers to the position in the world coordinate system. For acceleration, The time difference between two adjacent steps; This represents acceleration in the world coordinate system. Solving the quaternion differential equation using the first-order Runge-Kutta method yields the following quaternion update equation: ； In the formula: ； , , This represents the angular velocity vector in the transformed world coordinate system. Update the matrix for angular velocity quaternions; The attitude in the world coordinate system; Discretizing the above equations, we obtain the Kalman filter state equation as follows: ； I is the identity matrix. This is the input value at time k-1; S2. The pose information of the foot end is obtained by positive kinematics calculation of the humanoid robot's legs, and the overall pose of the robot is further derived. This pose is then used as the input of the observation model of the Kalman filter to correct the state estimate. S3. A Kalman filter is used to perform recursive optimal estimation of the system state, thereby obtaining the position, attitude and velocity of the humanoid robot; S4. Select the reference pose and interpolate the continuous pose trajectory provided by the Kalman filter: S5. Calculate the relative pose to achieve time alignment.

2. The real-time localization and mapping method for humanoid robots according to claim 1, characterized in that, In S1, it is assumed that the initial position of the IMU coordinate system coincides with the origin of the body coordinate system. The IMU sensor provides three-axis acceleration, three-axis angular velocity, and Euler angles describing the rotation of the body about its own axis. The rotation order of the Euler angles is ZYX. Based on this Euler angle definition, a rotation matrix from the body coordinate system to the world coordinate system is constructed. ; The acceleration acquired by the IMU is transformed from the body coordinate system to the world coordinate system using the following formula: ； ； ； In the formula This represents acceleration in the world coordinate system. This represents acceleration in the body coordinate system; , , These represent the accelerations along the x, y, and z axes in the body coordinate system, respectively. Represents the angular velocity in the world coordinate system; This represents the angular velocity in the body coordinate system.

3. The real-time localization and mapping method for humanoid robots according to claim 1, characterized in that, The steps for calculating the robot's overall pose in step S2 are as follows: The overall pose of the humanoid robot is obtained by using spherical linear interpolation based on the quaternions of the left and right foot poses. ； In the formula: and The postures of the left and right feet are shown respectively. As a weighting factor; A complete gait cycle consists of two phases: a support phase and a swing phase. Assuming the support phase and the swing phase last for the same duration, the following formula applies: ； In the formula: T s It is the duration of the supporting phase, T w It is the duration of the oscillation phase, and the sum of the two should be the time of one gait cycle, where t is the current time of one gait cycle; The formula for Slerp, a spherical linear interpolation method, is: ； In the formula: These are interpolation parameters. for and The angle between them is calculated using the following formula: ； Obtain the overall posture of the humanoid robot: ； It is the real part; This is the imaginary part; if the time ratios of the support phase and the oscillation phase are different, the coefficients in the formula should be adjusted according to the specific gait requirements.

4. The real-time localization and mapping method for humanoid robots according to claim 3, characterized in that, The steps for calculating the overall position in step S2 are as follows: Assuming the height of the humanoid robot's center of mass along the z-axis remains constant, and it continuously cycles during walking, causing the robot's center of mass to oscillate periodically between the left and right sides, the following formula can be used to estimate the humanoid robot's position along the x-axis and y-axis: ； ； In the formula: Let T be the overall position of the robot, λ be the weighting factor, and T be the weighting factor. s It is the duration of the supporting phase, T w It is the duration of the oscillation phase, T c Let t be the current time of a gait cycle.

5. The real-time localization and mapping method for humanoid robots according to claim 3, characterized in that, In step S2, an observation model is established: ； The state at time k, Let k be the observation value at time k. , , These are the specific values ​​contained within the observed values.

6. The real-time localization and mapping method for humanoid robots according to claim 3, characterized in that, Step S3 uses a Kalman filter to perform recursive optimal estimation of the system state, thereby obtaining the position, orientation, and velocity of the humanoid robot. The specific steps are as follows: Prediction step: ； In the formula Let be the prior state estimate at time k. Let A be the optimal estimate at time k-1, B be the control matrix, and A be the state matrix. Update the prior estimate covariance matrix: ； In the formula: Let represent the prior state estimation covariance matrix at time k; This is the optimal estimated covariance matrix at time k-1; assuming the process noise is zero-mean white noise, the prediction noise covariance matrix can be derived from the following formula. : ； In the formula: Let be the covariance matrix of the process noise. The noise input matrix; ； In the formula: and These are the noise variances of the accelerometer and gyroscope, respectively. ； In the formula: It is a mapping matrix of quaternion increments; ； Calculate the Kalman gain: ； Let K be the Kalman gain at time k, and N be the observation noise covariance matrix; Determine the observation noise covariance matrix; ； In the formula: For location observation noise, This is for attitude observation noise; ； In the formula , , and The measurement noise variances for the left and right foot positions and postures are respectively. and The variance of the model error representing position and attitude; This is a weighting factor in the overall position estimation. This is a weighting factor in the overall attitude estimation; Calculate the optimal estimate: ； Update error covariance: ； Through the above steps, the posture, position, and velocity of the humanoid robot can be obtained based on the Kalman filter algorithm; since the posture is represented by quaternions, the quaternions must be converted into a rotation matrix R.

7. The real-time localization and mapping method for humanoid robots according to claim 1, characterized in that, Step S4: Select the reference pose at the time step. A unified reference time t is selected within the scanning period of the point cloud. ref And all point cloud data are corrected to the position at that time, the pose at the reference time is represented as: ； in, This represents the rotation matrix at the reference time. Indicates the position at the reference time; To obtain the robot's pose T(t) relative to the world coordinate system at that moment j Interpolation is required based on the continuous pose trajectory provided by the Kalman filter. Position interpolation: ; Attitude interpolation: ; In the formula: , t a and t b They are adjacent to t respectively j Two known moments.

8. The real-time localization and mapping method for humanoid robots according to claim 1, characterized in that, Step S5 calculates the relative pose to achieve time alignment: In order to obtain the points at sampling time t j pose unified to reference time t ref It is necessary to calculate the relative pose transformation between the two: ； For any radar point O j At reference time t ref The pose can be obtained in the following way: 。 9. A real-time localization and mapping system for a humanoid robot, employing the method described in any one of claims 1-8, characterized in that, It includes a data acquisition module, a data processing module, a control module, and a display module; Data acquisition module: Acquires motion data of the humanoid robot; Data processing module: Using the improved Fast-LIO2 algorithm, a Kalman filter is used to fuse the forward kinematics of the humanoid robot's feet with IMU data to estimate the robot's position, attitude and velocity in the world coordinate system; Control module: Controls the robot to move according to the predetermined settings based on the information from the data processing module; Display module: Visualizes the motion status and data of the humanoid robot.

Images