A method and system for motion control of humanoid robots based on environmental perception

By using an environment-aware motion control method, and by utilizing foot trajectory extrapolation and terrain normal difference measurement, combined with multiple ground contact feature points to calibrate the support plane equation, a humanoid robot was able to walk stably on unknown unstructured terrain. This solved the coupling problem between terrain perception and foot posture adjustment in existing technologies, and ensured the stability and continuity of the robot on complex terrain.

CN123113993APending Publication Date: 2026-11-17TIANJIN SKY STAR TECH DEV CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202611628310.7
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2026-10-21
Publication Date
2026-11-17

AI Technical Summary

Technical Problem

Existing humanoid robot walking control methods lack a unified closed-loop coupling between terrain perception information, foot trajectory prediction information, and foot posture adjustment information when facing unknown unstructured terrain. This results in the foot posture being unable to effectively respond to changes in the terrain normal direction, the foot landing angle being difficult to adaptively match the terrain slope, and insufficient walking stability.

Method used

By employing an environment-aware motion control method, which utilizes foot trajectory extrapolation, terrain normal difference measurement, and foot posture matching, terrain perception, foot motion prediction, and foot posture adjustment are integrated into the same control framework. By calibrating the support plane equation through multiple ground contact feature points, accurate estimation of contact position and contact time is achieved, and adaptive joint drive commands are generated.

Benefits of technology

It achieves stable, continuous, and low-impact walking control for humanoid robots on steep slopes and undulating, unknown unstructured terrain, ensuring the accuracy and adaptability of foot posture in contact with the ground, and eliminating the defect of lack of closed-loop coupling in each link.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN123113993A_ABST
    Figure CN123113993A_ABST
Patent Text Reader

Abstract

This invention relates to the field of motion control technology, specifically disclosing a motion control method and system for humanoid robots based on environmental perception. The method includes: during robot movement, firstly, determining the look-ahead time period based on foot speed and historical trajectory arc length, and extrapolating global foot coordinates to generate a expected trajectory curve; combining the position of the supporting foot to solve a preset supporting plane equation, and simultaneously calculating to obtain the foot contact position and contact time; calibrating the terrain normal based on the contact position, and comparing it with the foot normal of the swinging foot to obtain the expected contact force direction; calculating the adaptive motion offset vector of the foot by converting the spatial vector deviation between the contact position and the supporting foot; then matching the swinging foot posture according to the expected contact force direction to determine the adaptive foot landing angle amplitude; jointly encoding the adaptive motion offset vector and the foot landing angle amplitude, and outputting joint drive commands. This invention can improve the foot landing control accuracy and stability of humanoid robots on unknown and complex terrain.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This invention relates to the field of motion control technology, and in particular to a motion control method and system for humanoid robots based on environmental perception. Background Technology

[0002] Humanoid robots possess configurations and movement capabilities similar to humans, enabling them to adapt to unstructured terrains such as stairs, gravel, and slopes that are difficult for wheeled or tracked robots to access. They have broad application prospects in fields such as disaster relief, industrial inspection, and home services. Walking control, as the core foundation for humanoid robots to achieve the above applications, requires the robot to adjust the trajectory, landing position, and foot posture of its feet in real time according to the terrain undulations during continuous bipedal support, in order to maintain body balance and ensure the continuity and safety of walking.

[0003] Existing humanoid robot walking control methods typically employ a framework based on model prediction or gait planning. This involves pre-setting foot swing trajectories and modifying joint commands using motion state information fed back from inertial measurement units and joint encoders. However, these methods rely solely on the current foot position feedback for adjustment during foot trajectory planning, use a simplified single-point contact model for support plane calibration, depend on large-scale environmental reconstruction or fixed geometric assumptions for terrain normal acquisition, and decouple foot posture from contact force direction during posture adjustment. This results in a lack of unified closed-loop coupling between terrain perception information, foot motion prediction information, and foot posture adjustment information. These shortcomings prevent foot posture adjustment from effectively responding to changes in terrain normal direction, making it difficult to adaptively match the foot angle to terrain slope. Consequently, the robot exhibits insufficient walking stability when facing unknown unstructured terrain. Therefore, a motion control method that can collaboratively integrate terrain perception, proactive foot trajectory prediction, and adaptive foot posture matching is urgently needed. Summary of the Invention

[0004] This invention provides a motion control method and system for humanoid robots based on environmental perception, in order to solve the problems mentioned in the background art.

[0005] To achieve the above objectives, the present invention provides a motion control method for a humanoid robot based on environmental perception, comprising: S1. During robot movement, the global foot coordinates of the robot are extrapolated based on the look-ahead time period determined by the foot speed and the arc length of the historical trajectory to obtain the expected trajectory curve. S2. Based on multiple ground contact feature points on the bottom of the robot's supporting foot, calibrate the supporting plane equation, and jointly solve the expected trajectory curve and the supporting plane equation to obtain the contact position and contact time. S3. Based on multiple terrain sampling points in the neighborhood of the contact position, the spatial orientation of the terrain surface is calibrated to obtain the terrain normal. Based on the contact time, the difference between the terrain normal and the foot normal of the robot's swinging foot is measured to obtain the expected contact force direction. S4. Perform parametric conversion on the spatial vector deviation between the contact position and the supporting foot position to obtain the adaptive motion offset vector of the foot end; S5. Based on the expected contact force direction, perform spatial posture matching on the spatial posture of the swinging foot to obtain the desired foot posture, and use the landing angle parameter of the desired foot posture as the adaptive landing angle amplitude. S6. Jointly encode the adaptive motion offset vector and the adaptive foot landing angle amplitude to generate joint drive commands.

[0006] To address the aforementioned problems, the present invention also provides a humanoid robot motion control system based on environmental perception, the system comprising: The foot trajectory extrapolation module is used to extrapolate the global foot coordinates of the robot during robot movement based on the look-ahead time period determined by the foot velocity and the historical trajectory arc length, so as to obtain the expected trajectory curve. The support plane solving module is used to calibrate the support plane equation based on multiple ground contact feature points on the sole of the support foot, and to jointly solve the expected trajectory curve and the support plane equation to obtain the contact position and contact time. The contact force direction generation module is used to calibrate the spatial orientation of the terrain surface based on multiple terrain sampling points in the neighborhood of the contact position to obtain the terrain normal. Based on the contact time, the difference between the terrain normal and the foot normal of the robot's swinging foot is measured to obtain the expected contact force direction. The motion offset vector generation module is used to perform parametric conversion on the spatial vector deviation between the contact position and the supporting foot position to obtain the adaptive motion offset vector of the foot end; the foot posture matching module is used to perform spatial posture matching on the spatial posture of the swinging foot based on the expected contact force direction to obtain the desired foot posture, and the landing angle parameter of the desired foot posture is used as the adaptive landing angle amplitude. The joint drive command generation module is used to jointly encode the adaptive motion offset vector and the adaptive foot landing angle amplitude to generate joint drive commands.

[0007] Compared with the prior art, the present invention has the following beneficial effects: 1. This invention integrates terrain perception, foot motion prediction, and foot landing posture adjustment into a unified control framework through forward extrapolation of foot trajectory, measurement of the difference between terrain normal and foot normal, and foot posture matching. Forward extrapolation enables the foot trajectory to respond in advance to changes in the terrain ahead, the difference measurement provides accurate terrain basis for the generation of the expected contact force direction, and posture matching directly couples the foot posture adjustment with the contact force direction, eliminating the lack of closed-loop coupling in each link and enabling the landing angle to adaptively match the terrain slope.

[0008] 2. This invention also improves the accuracy of support plane estimation by calibrating the support plane equation based on multiple ground contact feature points. By jointly solving the support plane equation and the expected trajectory curve, the contact position and contact time can adapt to the dynamic changes of the contact point with the terrain undulations. Through joint encoding, foot position compensation and foot posture adjustment are achieved in the joint space. The above methods together ensure that when the humanoid robot walks on unknown unstructured terrain with sudden slope changes and severe undulations, the foot can make accurate contact with the ground with the ground, achieving stable, continuous, and low-impact walking control. Attached Figure Description

[0009] Figure 1 This is a flowchart illustrating a motion control method for a humanoid robot based on environmental perception, provided in an embodiment of the present invention. Figure 2 This is a functional block diagram of a humanoid robot motion control system based on environmental perception, provided as an embodiment of the present invention.

[0010] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0011] It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0012] This application provides a motion control method for a humanoid robot based on environmental perception. The execution entity of this environmental perception-based humanoid robot motion control method includes, but is not limited to, at least one of the following electronic devices that can be configured to execute the method provided in this application: a server, a terminal, etc. In other words, the environmental perception-based humanoid robot motion control method can be executed by software or hardware installed on a terminal device or a server device. The server includes, but is not limited to, a single server, a server cluster, a cloud server, or a cloud server cluster. The server can be an independent server or a cloud server that provides basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communication, middleware services, domain name services, security services, content delivery networks (CDNs), and big data and artificial intelligence platforms.

[0013] Reference Figure 1 The diagram shown is a flowchart illustrating a motion control method for a humanoid robot based on environmental perception, according to an embodiment of the present invention. In this embodiment, the motion control method for a humanoid robot based on environmental perception includes: S1. During robot movement, the global foot coordinates of the robot are extrapolated based on the look-ahead time period determined by the foot speed and the arc length of the historical trajectory to obtain the expected trajectory curve. In this embodiment of the invention, the foot velocity is the three-dimensional velocity vector of the swinging foot in the robot's base coordinate system, the trajectory arc length is the cumulative value of the historical trajectory arc length that has been walked, and the look-ahead period is the length of the future time window predicted based on the foot velocity and the cumulative value of the historical trajectory arc length.

[0014] The joint angular displacement sensing data and the body posture sensing data are aligned according to the acquisition timestamp to obtain synchronized multi-source joint motion parameters. After low-pass filtering of the parameters, the joint angle values ​​in the filtering result are used as input for forward kinematic recursion to obtain the local foot position vector of the swinging foot in the robot base coordinate system. The local foot position vector is then subjected to coordinate rotation transformation using the rotation matrix in the global pose data of the body to obtain the global foot coordinates.

[0015] Historical walking distance data of the swinging foot is collected, and the cumulative value of the arc length of the walked trajectory is extracted. Using the cumulative value and foot speed as input, a time-series recursive prediction based on linear extrapolation is performed to obtain the predicted displacement of the swinging foot within a preset future time window. The predicted displacement is divided by the magnitude of the foot speed to obtain the look-ahead time period, and the product of the look-ahead time period and the magnitude of the foot speed is used as the look-ahead step length parameter. Cubic spline interpolation fitting is performed using the global foot coordinate sequence as discrete data points to obtain a continuous displacement mapping function of the foot position relative to the trajectory arc length. Based on the look-ahead step length parameter, the domain of the function is extended recursively to generate an extrapolated discrete foot position sequence. The extrapolated discrete foot position sequence is smoothed by moving average filtering to obtain the expected trajectory curve.

[0016] The feedback parameters of the joint angles corresponding to the supporting foot are collected and forward kinematic recursion is performed to obtain the spatial position coordinate sequence of the foot sole in the robot base coordinate system. Multiple ground contact feature points in the grounding state are extracted from the sequence, and outliers with coplanarity errors exceeding a preset threshold are removed to obtain an effective coplanar feature point group. Least square plane fitting regression is performed on the feature point group to obtain the optimal parameter set. The three-axis projection components of the plane normal vector are extracted from the optimal parameter set to form a linear coefficient sequence, and the plane distance constant term is extracted as a constant offset. Each coefficient in the linear coefficient sequence is multiplied by the corresponding coordinate axis variables of the foot sole ground contact position coordinates and the sum is calculated. The sum is added to the constant offset and the algebraic sum is set to zero. The constructed plane algebraic equation is used as the supporting plane equation.

[0017] Obtain the parametric equations of the curve corresponding to the expected trajectory curve and the plane normal vector and plane intercept constant of the supporting plane equation; use the global pose data of the body output by the inertial measurement unit to unify the two into the same coordinate system; substitute the three coordinate components of the foot spatial position vector in the parametric equation into the left-hand expression of the supporting plane equation to obtain a scalar algebraic sum function with time parameter as independent variable, and set the function as the contact arrival condition scalar function; perform zero-point search iterative operation on the scalar function, select the smallest positive real root from all time roots and record it as the contact time; substitute the contact time back to... The contact position is obtained from the curve parametric equation; the swing phase termination time is the latest time that the foot can land under the kinematic constraints of the swinging foot, and the time is preset by the gait planner according to the current walking speed and stride length; in the time interval between the swing phase start time and the swing phase termination time, the first positive real root of the mathematical intersection of the expected trajectory curve and the support plane equation corresponds to the moment when the swinging foot first crosses the support plane, which is the moment when the foot first contacts the ground physically. Subsequent intersections are not physically feasible because the foot has entered the contact state and the kinematic constraints have switched to the support phase.

[0018] Discrete terrain height samples are collected within a preset spatial range centered on the contact point. The three-dimensional data points are then assembled into a covariance matrix to obtain a local terrain covariance matrix. This matrix is ​​then subjected to eigenvalue decomposition, and the unit eigenvector corresponding to the smallest eigenvalue is extracted and labeled as the terrain normal. Based on the contact moment, the quaternion of the foot posture corresponding to the swinging foot is obtained, and a rotation transformation is performed to obtain the swinging foot normal. The spatial angle between the terrain normal and the swinging foot normal is calculated as a directional deviation metric. This metric is multiplied by a preset weighted coefficient for the expected contact force normal to obtain a weighted deviation. The weighted deviation is used to correct the terrain normal direction to obtain the expected normal direction. The terrain friction coefficient corresponding to the contact point is obtained, and the friction cone boundary angle is calculated. If the angle between the expected normal direction and the terrain normal is not greater than the friction cone boundary angle, the expected normal direction is taken as the expected contact force direction; otherwise, the direction is compressed towards the terrain normal direction using the friction cone boundary angle as a limiting value, and this is taken as the expected contact force direction.

[0019] Based on the global pose data of the body, the three-dimensional coordinate vector of the supporting foot is transformed to the world coordinate system to obtain the supporting foot position vector; the three-dimensional coordinate vector of the contact position is compared with the supporting foot position vector to obtain the original spatial deviation vector; the three coordinate components of the vector are subjected to first-order low-pass filtering to obtain a smoothed deviation vector as the adaptive motion offset vector.

[0020] Obtain the quaternion of the foot posture corresponding to the swinging foot, and convert the quaternion to a rotation matrix to obtain the current foot posture rotation matrix; extract the unit vector of the third column of the matrix as the current foot normal vector, and use the unit direction vector of the expected contact force direction as the expected foot normal vector; calculate the spatial rotation axis parameter and rotation angle parameter of the current normal vector to the expected normal vector, and construct the posture adjustment rotation matrix based on the parameters; multiply the current foot posture rotation matrix and the posture adjustment rotation matrix on the left, and use the product as the expected posture rotation matrix of the swinging foot; extract the rotation angle corresponding to the rotation component of the current foot normal vector in the matrix as the rotation increment of the foot normal around the swinging foot, and set the rotation increment as the adaptive landing angle amplitude.

[0021] The system acquires real-time angle measurements of each drive joint corresponding to the swing foot and forms a current joint angle sequence according to a preset joint arrangement. The adaptive motion offset vector is decomposed into three displacement increments in the world coordinate system, and the adaptive landing angle amplitude is used as the rotation increment around the foot's plantar normal. A three-dimensional rotational change is constructed based on the rotation increments and concatenated with the three displacement increments to form a six-dimensional pose increment vector. The Jacobian matrix corresponding to the current joint angle sequence is acquired and subjected to a generalized inverse matrix operation to obtain an inverse velocity mapping matrix. The matrix is ​​left-multiplied by the pose increment vector to obtain the joint angle increment vector, which is then element-wise superimposed with the current joint angle sequence to generate the desired joint angle sequence. Position servo commands for each drive joint are configured based on the desired joint angle values ​​in the sequence and sent to the servo driver.

[0022] This invention integrates terrain perception, foot motion prediction, and foot posture adjustment into a single control framework through forward extrapolation of foot trajectory, measurement of the difference between terrain normal and foot normal, and foot posture matching, eliminating the lack of closed-loop coupling in each stage. It improves the accuracy of support plane estimation by calibrating the support plane equation based on multiple ground contact feature points; it enables the contact position and contact time to adapt to dynamic changes in the contact point with terrain undulations through joint solution of the support plane equation and the expected trajectory curve; it allows the foot trajectory to respond in advance to changes in the terrain through forward extrapolation; it provides accurate terrain basis for the generation of the expected contact force direction through difference measurement; it directly couples foot posture adjustment with the contact force direction through posture matching; and it achieves coordinated foot position compensation and foot posture adjustment in joint space through joint coding. This ensures that when the humanoid robot walks on unknown unstructured terrain with abrupt slope changes and severe undulations, the foot angle can adaptively match the terrain slope, and the foot can contact the ground with accurate posture and position, achieving stable, continuous, and low-impact walking control.

[0023] S2. Based on multiple ground contact feature points on the bottom of the robot's supporting foot, calibrate the supporting plane equation, and jointly solve the expected trajectory curve and the supporting plane equation to obtain the contact position and contact time. In this embodiment of the invention, the ground contact feature point refers to the three-dimensional spatial position point of the local area where the sole of the supporting foot makes physical contact with the terrain surface in the robot's base coordinate system; the supporting plane equation refers to the plane algebraic equation describing the spatial orientation and position of the terrain surface in the supporting foot area, obtained by fitting multiple ground contact feature points; the expected trajectory curve refers to the smooth curve in which the position of the swinging foot changes continuously with the trajectory arc length, obtained by extrapolation of the swinging foot end in the world coordinate system; the contact position refers to the three-dimensional spatial position point in the world coordinate system when the swinging foot end moves along the expected trajectory curve to the point where it first intersects with the supporting plane; and the contact time refers to the time length taken for the swinging foot end to move from the start of the swing phase to the contact position.

[0024] The joint angle feedback parameters of each joint of the supporting foot are collected, and a forward kinematic recursive solution is performed based on the robot's kinematic chain model to obtain the spatial position coordinate sequence of the foot sole in the robot's base coordinate system. Multiple ground contact feature points of the supporting foot sole in a grounded state are extracted from the foot sole spatial position coordinate sequence. The coplanarity error between each ground contact feature point is evaluated, and feature points with coplanarity errors exceeding a preset threshold are identified as outliers and removed, resulting in a set of effective coplanar feature points of the supporting foot sole. Least squares plane fitting regression is performed on the effective coplanar feature point set to find the solution that makes each... The optimal parameter set is obtained by minimizing the sum of squared residuals from feature points to the plane to be fitted. The three-axis projection components of the plane normal vector in the robot base coordinate system are extracted from the optimal parameter set. The three-axis projection components form a sequence of linear coefficients. The plane distance constant term is extracted from the optimal parameter set as a constant offset. Each coefficient in the sequence of linear coefficients is multiplied by the corresponding coordinate axis variables of the foot contact position and the sum is calculated. The sum is added to the constant offset and the resulting algebraic sum is set to zero. The constructed plane algebraic equation is used as the supporting plane equation.

[0025] Obtain the curve parametric equation corresponding to the expected trajectory curve. This equation, with a time parameter as the independent variable, outputs the spatial position vector of the swinging foot in the world coordinate system. Obtain the plane normal vector and plane intercept constant of the support plane equation. Before joint solving, use the global pose data of the body output by the inertial measurement unit to unify the support plane equation and the expected trajectory curve into the same coordinate system. Substitute the three coordinate components of the foot spatial position vector (which is a function of the time parameter) in the curve parametric equation into the left-hand side expression of the support plane equation to replace the three coordinate axis variables. A scalar algebraic sum function with time parameter as independent variable is obtained, and the scalar algebraic sum function is set as the contact arrival condition scalar function; a zero-point search iterative operation is performed on the contact arrival condition scalar function, and all time roots that make the scalar function value zero are searched within the time interval between the start time of the swing phase and the end time of the swing phase. The smallest positive real root is selected from all time roots and recorded as the contact time; the contact time is substituted back into the curve parametric equation, and the three coordinate component values ​​of the foot end spatial position vector corresponding to the contact time are calculated. The spatial point coordinates composed of the three coordinate component values ​​are used as the contact position.

[0026] This invention calibrates the support plane equation based on multiple ground contact feature points, making full use of multi-point contact information between the foot and the terrain in the estimation of the support plane. This avoids the estimation bias caused by single-point contact models and improves the calibration accuracy of the support plane equation under complex terrain conditions. By jointly solving the expected trajectory curve and the support plane equation, the acquisition of contact position and contact time can adapt to the dynamic changes of contact points with terrain undulations. Compared with fixed geometric assumption methods, it has higher terrain adaptability and solution robustness, providing an accurate spatiotemporal reference for subsequent calculation of foot movement offset vector and adaptive adjustment of foot posture.

[0027] S3. Based on multiple terrain sampling points in the neighborhood of the contact position, the spatial orientation of the terrain surface is calibrated to obtain the terrain normal. Based on the contact time, the difference between the terrain normal and the foot normal of the robot's swinging foot is measured to obtain the expected contact force direction. In this embodiment of the invention, the terrain sampling point refers to the discrete location points of the terrain surface collected within a preset spatial range around the contact position. Each sampling point includes horizontal plane coordinates and corresponding height values. The terrain normal is a unit vector that represents the spatial orientation of the terrain surface at the contact position, obtained by fitting multiple terrain sampling points in the neighborhood of the contact position. The foot normal of the swinging foot is a unit vector that represents the spatial orientation of the foot plane, obtained by solving the foot attitude quaternion output by the inertial measurement unit corresponding to the swinging foot. The expected contact force direction is the expected force direction vector that should be applied when the swinging foot contacts the terrain surface at the moment of contact.

[0028] Discrete terrain height samples are collected within a preset spatial range centered on the contact point. The horizontal coordinates of each sample point are combined with the corresponding height value to form three-dimensional data points. A covariance matrix assembly operation is performed on the three-dimensional data points to obtain a local terrain covariance matrix. Feature decomposition is performed on the local terrain covariance matrix to obtain all eigenvalues ​​and corresponding eigenvectors. The unit eigenvector corresponding to the smallest eigenvalue is extracted and calibrated as the terrain normal. Based on the foot posture quaternion output by the inertial measurement unit at the contact moment as the initial value, interpolation is performed according to the preset swing phase foot posture change law to obtain the foot posture quaternion of the swinging foot at the contact moment. A rotation transformation operation is performed on the foot posture quaternion at the contact moment to obtain the foot normal vector of the swinging foot in the robot base coordinate system, which is used as the foot normal of the swinging foot. The spatial angle between the terrain normal and the foot normal of the swinging foot is calculated. The spatial angle is used as a directional deviation metric. The directional deviation metric is multiplied by a preset weighted coefficient for the normal of the expected contact force to obtain a weighted deviation. The normal vector of the plane containing the terrain normal and the normal vector of the foot of the swinging foot is used as the rotation axis. The terrain normal is rotated around the rotation axis by an angle corresponding to the weighted deviation, and the rotated vector is normalized to obtain the expected normal direction of the foot of the swinging foot. The terrain friction coefficient corresponding to the contact position is obtained, and the friction cone boundary angle is calculated based on the terrain friction coefficient. The friction cone boundary angle is equal to the arctangent of the terrain friction coefficient. It is determined whether the angle between the expected normal direction and the terrain normal is less than or equal to the friction cone boundary angle. If it is less than or equal to, the expected normal direction is directly used as the expected contact force direction. If it is greater than, the expected normal direction is compressed towards the terrain normal direction with the friction cone boundary angle as the limiting value, so that the angle between the compressed direction and the terrain normal is equal to the friction cone boundary angle. The compressed direction is used as the expected contact force direction.

[0029] This invention achieves accurate local calibration of the spatial orientation of the terrain surface by collecting multiple terrain sampling points in the neighborhood of the contact location and performing covariance matrix eigenvalue decomposition. Compared with existing methods that rely on large-scale environmental reconstruction or fixed geometric assumptions, this invention effectively improves the estimation accuracy of terrain normals under occlusion or poor lighting conditions. By introducing friction cone constraints in the process of measuring the difference between the terrain normal and the foot normal of the swinging foot, the generation of the expected contact force direction simultaneously satisfies the dual conditions of terrain geometric orientation and foot-ground contact physical feasibility, avoiding the risk of tangential slippage or foot lateral slippage that may occur if the normal direction is directly used as the force direction. By using the difference measurement results as the basis for subsequent foot posture matching, the perception information of the terrain normal direction can be fed back to the posture adjustment stage in a closed loop, providing accurate terrain basis for the adaptive adjustment of foot posture.

[0030] S4. Perform parametric conversion on the spatial vector deviation between the contact position and the supporting foot position to obtain the adaptive motion offset vector of the foot end; In this embodiment of the invention, the contact position refers to the three-dimensional spatial position point in the world coordinate system when the swinging foot moves along the expected trajectory curve to the point of first intersection with the support plane; the support foot position refers to the position vector in the world coordinate system after coordinate transformation of the three-dimensional coordinate vector of the support foot in the robot base coordinate system; the spatial vector deviation refers to the vector difference between the three-dimensional coordinate vector corresponding to the contact position and the support foot position vector; the adaptive motion offset vector refers to the three-dimensional offset vector obtained by sequentially performing coordinate transformation, vector difference calculation, and low-pass filtering on the spatial vector deviation, which is used for foot position compensation.

[0031] The system acquires global pose data of the robot body output by the inertial measurement unit. This data includes the rotation matrix and translation vector of the robot body in the world coordinate system. Based on the rotation matrix and translation vector in the global pose data, the three-dimensional coordinate vectors of the supporting foot in the robot's base coordinate system are transformed to the world coordinate system to obtain the supporting foot position vector in the same coordinate system. The three-dimensional coordinate vector of the contact position in the world coordinate system is extracted as the first three-dimensional coordinate vector, and the supporting foot position vector is extracted as the second three-dimensional coordinate vector. A vector difference operation is performed between the first and second three-dimensional coordinate vectors, i.e., the supporting foot position vector is subtracted from the contact position vector to obtain the original spatial deviation vector. First-order low-pass filtering is applied to the three coordinate components of the original spatial deviation vector, with the filtering time constant set according to the robot control cycle. High-frequency components in the original spatial deviation vector are filtered out, while low-frequency components reflecting terrain trends are retained to obtain a smoothed deviation vector. This smoothed deviation vector is used as an adaptive motion offset vector, expressed in the world coordinate system.

[0032] This invention unifies positional information distributed in different coordinate systems into a single reference coordinate system for deviation calculation by performing coordinate transformation and vector difference calculation on the spatial vector deviation between the contact position and the supporting foot position. This eliminates errors caused by coordinate system inconsistencies, ensuring that the original spatial deviation vector accurately reflects the degree of spatial deviation between the foot landing point and the current position. By applying low-pass filtering to the original spatial deviation vector, high-frequency interference components introduced by mechanical vibration and sensor noise during foot-ground contact are filtered out, making the adaptive motion offset vector smooth and preventing high-frequency disturbances from directly coupling to joint drive commands. By expressing the adaptive motion offset vector in the world coordinate system, it can be directly co-encoded with the expected contact force direction and foot posture information in subsequent steps, participating in the generation of joint drive commands without additional coordinate transformation. This provides a unified spatial reference for the coordinated performance of foot position compensation and foot posture adjustment in joint space.

[0033] S5. Based on the expected contact force direction, perform spatial posture matching on the spatial posture of the swinging foot to obtain the desired foot posture, and use the landing angle parameter of the desired foot posture as the adaptive landing angle amplitude. In this embodiment of the invention, the expected contact force direction refers to the unit vector of the expected force direction that the swinging foot should apply when it contacts the terrain surface at the moment of contact; spatial posture matching refers to the process of adjusting the current spatial posture of the swinging foot according to the expected contact force direction so that the foot normal direction approaches the expected contact force direction; the expected foot posture refers to the target posture rotation matrix that the swinging foot should achieve after spatial posture matching; the landing angle parameter refers to the rotation angle component of the expected foot posture with respect to the foot normal direction; and the adaptive landing angle amplitude refers to using the rotation angle component as the angle adjustment amount in the landing control command.

[0034] Obtain the foot attitude quaternion output by the inertial measurement unit corresponding to the swinging foot, and perform a quaternion-to-rotation matrix conversion operation on the foot attitude quaternion to obtain the current foot attitude rotation matrix; extract the third column unit vector in the current foot attitude rotation matrix, the vector being the direction vector of the Z-axis of the foot coordinate system in the world coordinate system, representing the current foot normal vector; obtain the unit direction vector of the expected contact force direction, and use the unit direction vector as the expected foot normal vector; calculate the spatial rotation axis parameter and rotation angle parameter of rotating the current foot normal vector to the expected foot normal vector, where the rotation axis parameter... The unit vector is perpendicular to the plane containing the two normal vectors, and the rotation angle parameter is the spatial angle between the two normal vectors. The Rodriguez rotation formula is used to construct the attitude adjustment rotation matrix based on the rotation axis parameter and the rotation angle parameter. The current attitude rotation matrix of the foot is multiplied by the attitude adjustment rotation matrix on the left, and the product is used as the desired attitude rotation matrix of the swinging foot. The rotation component about the current normal vector of the foot is extracted from the desired attitude rotation matrix of the swinging foot. The rotation angle corresponding to the rotation component is the rotation increment about the foot normal of the swinging foot. The rotation increment is set as the adaptive landing angle amplitude.

[0035] This invention directly uses the unit direction vector of the expected contact force direction as the desired foot normal vector, thus directly coupling foot posture adjustment with the contact force direction. This avoids the posture mismatch problem caused by independent planning when foot posture and contact force direction are decoupled. By calculating the spatial rotation axis and rotation angle of the current foot normal vector to the desired normal vector, and constructing a posture adjustment rotation matrix based on the rotation axis and rotation angle, a complete spatial rotation transformation from the current posture to the desired posture is achieved, allowing the foot to reach the target posture through the most direct rotation path. By extracting the rotation component with respect to the foot normal direction from the desired posture rotation matrix as an adaptive landing angle amplitude, the landing angle amplitude can be adaptively updated according to changes in terrain slope. Compared with the control method of fixed landing angle, this significantly improves the posture fit between the foot and the terrain surface.

[0036] S6. Jointly encode the adaptive motion offset vector and the adaptive foot landing angle amplitude to generate joint drive commands.

[0037] In this embodiment of the invention, the adaptive motion offset vector refers to the three-dimensional position offset expressed in the world coordinate system after performing coordinate transformation, vector difference operation and low-pass filtering on the spatial vector deviation in sequence; the adaptive foot landing angle amplitude refers to the rotation angle component about the normal direction of the foot extracted from the foot's desired posture rotation matrix, representing the angle increment that the foot should adjust around the normal direction at the moment of contact; joint encoding refers to the process of uniformly converting the adaptive motion offset vector and the adaptive foot landing angle amplitude into joint space drive commands according to the robot kinematic mapping rules; the joint drive command refers to the position control command sent to each drive joint servo driver, which includes the target position and velocity limit value.

[0038] The system acquires real-time angle measurements of each driven joint corresponding to the swing foot, and arranges these measurements sequentially according to the pre-defined joint arrangement in the robot's kinematic model to form the current joint angle sequence. The adaptive motion offset vector is decomposed into components along three orthogonal axes in the world coordinate system to obtain displacement increments along these axes. The adaptive landing angle amplitude is used as the rotation increment around the foot's normal. An equivalent rotation matrix around the foot's normal direction is constructed based on the rotation increment, and this equivalent rotation matrix is ​​converted into a three-dimensional rotational change, i.e., the angle increment vector of the foot's posture. The three displacement increments are concatenated with the three-dimensional rotational change to form a six-dimensional pose increment vector, where the first three components are position increments and the last three components are pose increments. The Jacobian matrix corresponding to the current joint angle sequence is obtained. This Jacobian matrix has six rows and the number of columns is equal to the total number of driven joints of the swing foot. Each feature of the Jacobian matrix corresponds to the six-dimensional velocity change of the foot caused by the unit velocity of the joint. A generalized inverse matrix operation is performed on the Jacobian matrix. Specifically, the Jacobian matrix is ​​multiplied by its transpose, the product matrix is ​​inverted, and then the resulting inverse matrix is ​​multiplied by the transpose of the Jacobian matrix to obtain the inverse velocity mapping matrix. The inverse velocity mapping matrix is ​​then left-multiplied by the pose increment vector to obtain the joint angle increment vector. The current joint angle sequence and the joint angle increment vector are then element-wise superimposed to generate the desired joint angle sequence. Based on the desired angle values ​​corresponding to each joint in the desired joint angle sequence, position servo commands are configured for each driven joint. The position servo commands include the target position and the velocity limit value to reach the target position. The position servo commands are sent to the servo drivers of each driven joint, and the servo drivers drive the joint motors to move according to the received commands.

[0039] This invention achieves a joint expression of position compensation and attitude adjustment in the task space by decomposing the adaptive motion offset vector into three displacement increments, using the adaptive foot landing angle amplitude as the rotation increment around the foot normal, and concatenating the two into a unified six-dimensional pose increment vector. This avoids command conflicts that may occur when position control and attitude control are planned separately. By performing a generalized inverse matrix operation on the Jacobian matrix to obtain an inverse velocity mapping matrix, and mapping the pose increment vector to the joint space through a left multiplication operation of the inverse velocity mapping matrix, a precise conversion from the six-dimensional pose increment in the task space to the angle increment in the joint space is achieved without relying on complex inverse kinematics analytical solutions, thus improving computational efficiency. By superimposing the joint angle increment vector with the current joint angle sequence to generate the desired joint angle sequence, and configuring the position servo commands of each drive joint according to the desired angle value, foot position compensation and foot attitude adjustment are performed collaboratively in the joint space, enabling the foot to contact the ground with accurate attitude and position, providing a complete implementation layer guarantee for the overall control scheme.

[0040] like Figure 2 The diagram shown is a functional block diagram of a humanoid robot motion control system based on environmental perception provided in an embodiment of the present invention.

[0041] The humanoid robot motion control system 100 based on environmental perception described in this invention can be installed in an electronic device. Depending on the functions implemented, the humanoid robot motion control system 100 may include a foot trajectory extrapolation module 101, a support plane solving module 102, a contact force direction generation module 103, a motion offset vector generation module 104, a foot posture matching module 105, and a joint drive command generation module 106. The modules described in this invention can also be referred to as units, which are a series of computer program segments that can be executed by the processor of an electronic device and can perform a fixed function, and are stored in the memory of the electronic device.

[0042] In this embodiment, the functions of each module / unit are as follows: The foot trajectory extrapolation module 101 is used to extrapolate the global foot coordinates of the robot during robot movement based on the look-ahead time period determined by the foot speed and the historical trajectory arc length, so as to obtain the expected trajectory curve. The support plane solving module 102 is used to calibrate the support plane equation based on multiple ground contact feature points of the robot's supporting foot, and jointly solve the expected trajectory curve and the support plane equation to obtain the contact position and contact time. The contact force direction generation module 103 is used to calibrate the spatial orientation of the terrain surface based on multiple terrain sampling points in the neighborhood of the contact position to obtain the terrain normal, and to measure the difference between the terrain normal and the foot normal of the robot's swinging foot based on the contact time to obtain the expected contact force direction. The motion offset vector generation module 104 is used to perform parametric calculation on the spatial vector deviation between the contact position and the supporting foot position to obtain the adaptive motion offset vector of the foot end. The foot posture matching module 105 is used to perform spatial posture matching on the spatial posture of the swinging foot based on the expected contact force direction to obtain the desired foot posture, and use the landing angle parameter of the desired foot posture as the adaptive landing angle amplitude. The joint drive command generation module 106 is used to jointly encode the adaptive motion offset vector and the adaptive foot landing angle amplitude to generate joint drive commands.

[0043] In the several embodiments provided by this invention, it should be understood that the disclosed methods and systems can be implemented in other ways. For example, the system embodiments described above are merely illustrative; for instance, the division of modules is only a logical functional division, and other division methods may be used in actual implementation.

[0044] The modules described as separate components may or may not be physically separate. The components shown as modules may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs.

[0045] Furthermore, the functional modules in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or in the form of hardware plus software functional modules.

[0046] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the present invention can be implemented in other specific forms without departing from the spirit or essential characteristics of the present invention.

[0047] This application embodiment can acquire and process relevant data based on artificial intelligence technology. Artificial intelligence is the theory, method, technology, and application system that uses digital computers or machines controlled by digital computers to simulate, extend, and expand human intelligence, perceive the environment, acquire knowledge, and use that knowledge to obtain optimal results.

[0048] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention.

Claims

1. A motion control method for a humanoid robot based on environmental perception, characterized in that, The method includes: S1. During robot movement, the global foot coordinates of the robot are extrapolated based on the look-ahead time period determined by the foot speed and the arc length of the historical trajectory to obtain the expected trajectory curve. S2. Based on multiple ground contact feature points on the bottom of the robot's supporting foot, calibrate the supporting plane equation, and jointly solve the expected trajectory curve and the supporting plane equation to obtain the contact position and contact time. S3. Based on multiple terrain sampling points in the neighborhood of the contact position, the spatial orientation of the terrain surface is calibrated to obtain the terrain normal. Based on the contact time, the difference between the terrain normal and the foot normal of the robot's swinging foot is measured to obtain the expected contact force direction. S4. Perform parametric conversion on the spatial vector deviation between the contact position and the supporting foot position to obtain the adaptive motion offset vector of the foot end; S5. Based on the expected contact force direction, perform spatial posture matching on the spatial posture of the swinging foot to obtain the desired foot posture, and use the landing angle parameter of the desired foot posture as the adaptive landing angle amplitude. S6. Jointly encode the adaptive motion offset vector and the adaptive foot landing angle amplitude to generate joint drive commands.

2. The humanoid robot motion control method based on environmental perception as described in claim 1, characterized in that, In the robot's motion, based on the look-ahead time period determined by the foot velocity and the historical trajectory arc length, the global foot coordinates of the robot are extrapolated to obtain the expected trajectory curve, including: According to the acquisition timestamp, the robot's joint angular displacement sensing data and the body posture sensing data output by the inertial measurement unit are aligned to obtain synchronized multi-source joint motion parameters. The synchronized multi-source joint motion parameters are subjected to low-pass filtering to remove the sensing noise component in the multi-source joint motion parameters, and the filtered joint motion parameters are obtained. By performing a forward kinematic recursion on the filtered joint motion parameters, the local foot position vector of the robot is obtained; Based on the global pose data output by the inertial measurement unit, the local foot position vector is subjected to coordinate rotation transformation to obtain the global foot coordinates of the robot.

3. The humanoid robot motion control method based on environmental perception as described in claim 1, characterized in that, In the robot's motion, based on the look-ahead time period determined by the foot velocity and the historical trajectory arc length, the global foot coordinates of the robot are extrapolated to obtain the expected trajectory curve, including: Based on the historical walking mileage data of the robot's swinging foot, the cumulative value of the arc length of the trajectory already walked is extracted. The cumulative value of the arc length of the trajectory and the speed of the foot are used for time-series recursive prediction to generate the predicted displacement of the swinging foot in a future preset window. The time span is calculated based on the predicted displacement and the speed of the foot to obtain the look-ahead period. The step size is converted during the look-ahead period to obtain the look-ahead step size parameter of the robot's swinging foot. Interpolate and fit the global foot coordinates to obtain a continuous displacement mapping function of the foot position relative to the trajectory arc length; Based on the look-ahead step size parameter, the continuous displacement mapping function is recursively calculated to generate an extrapolated discrete foot position sequence; The extrapolated discrete foot position sequence is smoothed by moving average filtering to obtain the expected trajectory curve.

4. The humanoid robot motion control method based on environmental perception as described in claim 1, characterized in that, The calibration of the support plane equation based on multiple ground-contact feature points on the sole of the robot's supporting foot includes: By performing forward kinematic recursion on the joint angle feedback parameters corresponding to the supporting foot of the robot, the spatial position coordinates of the foot's sole are obtained. Based on the spatial coordinates of the foot, multiple ground contact feature points of the robot's supporting foot that are in a grounding state are eliminated by coplanarity error removal to obtain the effective coplanar feature point group of the supporting foot. The least squares plane fitting regression operation is performed on the effective coplanar feature point group to obtain the optimal plane parameter set of the supporting foot. The optimal plane parameter set includes the three-axis projection components of the plane normal vector in the robot base coordinate system and the plane distance constant term. Using the three-axis projection components as the first-order coefficient sequence and the plane distance constant as the constant offset, the coefficients in the first-order coefficient sequence are multiplied by the corresponding coordinate axis variables of the foot contact position coordinates and then summed. The sum is then superimposed with the constant offset to zero, and the constructed plane mathematical algebra equation is used as the supporting plane equation.

5. The humanoid robot motion control method based on environmental perception as described in claim 1, characterized in that, The step of jointly solving the expected trajectory curve and the supporting plane equation to obtain the contact position and contact time includes: Using the time parameter of the curve parametric equation corresponding to the expected trajectory curve as the independent variable, the spatial position vector of the swing foot end is calibrated, and the plane normal vector and plane intercept constant of the support plane equation are obtained. Substitute the three coordinate components of the foot spatial position vector, which is a time parameter function in the curve parametric equation, into the left-hand expression of the support plane equation, and replace the three coordinate axis variables in the support plane equation to obtain a scalar algebraic sum function with the time parameter as the independent variable. Set the scalar algebraic sum function as the contact arrival condition scalar function. The scalar function of the contact arrival condition is iteratively calculated, and all time roots that satisfy the function value of zero are searched in the time interval between the start time of the swing phase and the predicted landing time. The smallest positive real root is selected from all time roots and recorded as the contact time. Substitute the contact time back into the curve parameter equation to calculate the three coordinate components of the foot spatial position vector corresponding to the contact time, and mark the spatial point coordinates formed by the three coordinate components as the contact position.

6. The humanoid robot motion control method based on environmental perception as described in claim 1, characterized in that, The method involves calibrating the spatial orientation of the terrain surface based on multiple terrain sampling points in the neighborhood of the contact location to obtain the terrain normal. Based on the contact moment, the difference between the terrain normal and the foot normal of the robot's swinging foot is measured to obtain the expected contact force direction, including: The discrete sampled values ​​of the terrain height in the neighborhood of the contact location are matrix-assembled to obtain the local terrain covariance matrix of the contact location; The local terrain covariance matrix is ​​subjected to eigenvalue decomposition, and the unit eigenvector corresponding to the minimum eigenvalue is extracted. The unit eigenvector is then labeled as the terrain normal. Based on the contact time, the foot normal vector of the inertial attitude measurement value of the swing foot is calculated to obtain the foot normal of the swing foot. Calculate the spatial angle between the terrain normal and the foot normal of the swinging foot, use the spatial angle as the direction deviation metric, and calculate the force direction based on the direction deviation metric and the preset expected contact force normal weight coefficient to obtain the expected normal direction of the robot's swinging foot. Based on the terrain friction coefficient corresponding to the contact position, the friction cone boundary angle is calculated, and the contact force direction is mapped to the desired normal direction and the friction cone boundary angle to obtain the expected contact force direction.

7. The humanoid robot motion control method based on environmental perception as described in claim 1, characterized in that, The step of parametrically converting the spatial vector deviation between the contact position and the supporting foot position to obtain the adaptive motion offset vector at the foot end includes: Based on the global pose data of the machine body output by the inertial measurement unit, the three-dimensional coordinate vector of the foot support in the robot base coordinate system is transformed to the world coordinate system to obtain the position vector of the foot support in the same coordinate system. The original spatial deviation vector of the contact position is obtained by performing a vector difference operation between the three-dimensional coordinate vector corresponding to the contact position and the support foot position vector; The original spatial deviation vector is continuously acquired at each time point according to the control cycle to form a discrete time series of the original spatial deviation vector. The three coordinate components of the original spatial deviation vector in the discrete time series are subjected to first-order low-pass filtering respectively. The filtered coordinate component values ​​at the current time are combined to form the smooth deviation vector of the contact position. The smoothing deviation vector is calibrated as the adaptive motion offset vector, which is expressed in the world coordinate system.

8. The motion control method for a humanoid robot based on environmental perception as described in claim 1, characterized in that, The step of performing spatial attitude matching on the spatial posture of the swinging foot based on the expected contact force direction to obtain the desired foot posture, and using the landing angle parameter of the desired foot posture as the adaptive landing angle amplitude, includes: Based on the foot attitude quaternion output by the inertial measurement unit corresponding to the swinging foot, the current foot attitude rotation matrix is ​​calculated. Extract the third column unit vector from the current foot posture rotation matrix as the current foot normal vector, and use the unit direction vector of the expected contact force direction as the expected foot normal vector; Calculate the spatial rotation axis parameter and rotation angle parameter of the current normal vector of the foot to the desired normal vector of the foot, and construct the posture adjustment rotation matrix based on the spatial rotation axis parameter and the rotation angle parameter; The left-multiplication product of the current foot posture rotation matrix and the posture adjustment rotation matrix is ​​used as the desired foot posture rotation matrix. The Euler angle decomposition is performed on the desired attitude rotation matrix of the swing foot, and the angular component of the swing foot in the pitch axis direction is extracted. The angular component in the pitch axis direction is set as the adaptive foot landing angle amplitude.

9. The humanoid robot motion control method based on environmental perception as described in claim 1, characterized in that, The step of jointly encoding the adaptive motion offset vector and the adaptive foot landing angle amplitude to generate joint drive commands includes: The real-time angle measurements of the driving joints corresponding to the swing feet of the robot are arranged sequentially according to the joint arrangement order preset in the robot kinematic model to form the current joint angle sequence. The adaptive motion offset vector is decomposed into components along three orthogonal axes in the world coordinate system to obtain the displacement increments of the three orthogonal axes. The adaptive foot landing angle amplitude is used as the angle increment of the swing foot in the pitch axis direction of the world coordinate system. The angle increment is converted into a three-dimensional rotational change about the pitch axis of the world coordinate system, and the pitch axis component of the rotational change is equal to the adaptive foot angle amplitude, roll axis component and yaw axis component are all zero. The displacement increment and the three-dimensional rotational change are concatenated to obtain the pose increment vector. Obtain the Jacobian matrix corresponding to the current joint angle sequence, perform a generalized inverse matrix operation on the Jacobian matrix to obtain the inverse velocity mapping matrix, and multiply the pose increment vector by the inverse velocity mapping matrix to obtain the joint angle increment vector. The current joint angle sequence is superimposed with the joint angle increment vector to generate the desired joint angle sequence; Based on the expected angle value corresponding to each joint in the expected joint angle sequence, position servo commands for each driving joint are generated and sent to the servo driver of each driving joint.

10. A motion control system for a humanoid robot based on environmental perception, characterized in that, The system for implementing the humanoid robot motion control method based on environmental perception as described in claim 1 includes: The foot trajectory extrapolation module is used to extrapolate the global foot coordinates of the robot during robot movement based on the look-ahead time period determined by the foot velocity and the historical trajectory arc length, so as to obtain the expected trajectory curve. The support plane solving module is used to calibrate the support plane equation based on multiple ground contact feature points on the bottom of the robot's supporting foot, and to jointly solve the expected trajectory curve and the support plane equation to obtain the contact position and contact time. The contact force direction generation module is used to calibrate the spatial orientation of the terrain surface based on multiple terrain sampling points in the neighborhood of the contact position to obtain the terrain normal. Based on the contact time, the difference between the terrain normal and the foot normal of the robot's swinging foot is measured to obtain the expected contact force direction. The motion offset vector generation module is used to perform parametric conversion on the spatial vector deviation between the contact position and the supporting foot position to obtain the adaptive motion offset vector of the foot end; the foot posture matching module is used to perform spatial posture matching on the spatial posture of the swinging foot based on the expected contact force direction to obtain the desired foot posture, and the landing angle parameter of the desired foot posture is used as the adaptive landing angle amplitude. The joint drive command generation module is used to jointly encode the adaptive motion offset vector and the adaptive foot landing angle amplitude to generate joint drive commands.