A method for motion re-targeting of humanoid robots with contact and center of mass constraints

Abstract

The application discloses a kind of contact and centroid constraint humanoid robot motion reorientation method.The method first obtains human motion data and identifies division support phase and empty phase;Then, based on the trunk foot height ratio, the scaling ratio is calculated, and the spatial geometry scaling and the square root r frame rate resampling are synchronously executed;Subsequently, a multi-objective cost function is constructed by integrating key point pose error, joint regularization term and centroid landing alignment penalty;During the solving process, the foot end tangential zero speed and normal locking hard constraint are dynamically applied according to the contact phase, and the iterative solution in Lie group tangent space is output through two-stage damping sequence quadratic programming, and the reference joint trajectory is continuous and physically consistent.The application inhibits the problem that traditional pure kinematics reorientation lacks dynamics constraint and is easy to produce foot end slip and instability, and guarantees the physical contact authenticity and centroid balance of the reorientation output trajectory.

Description

Technical Field

[0001] This invention belongs to the fields of robot motion control, computer vision and motion data processing technology, and specifically relates to a method for redirecting the motion of a humanoid robot with contact and centroid constraints. Background Technology

[0002] With the development of humanoid robot technology, high-quality human motion reference data is needed for humanoid motion control. Compared with manually designed gait trajectories, directly using human motion capture data can more fully preserve the temporal rhythm, posture coordination, and naturalness of human movement, and has therefore become an important approach to constructing priors for humanoid motion control. However, human motion data cannot be directly used for humanoid robot control: there are significant differences between humans and humanoid robots in link length, joint degrees of freedom, joint limits, and local structures. Directly mapping human motion data to humanoid robots often leads to distorted target movements, joint overstepping, and even unreasonable foot contact relationships, specifically manifested as foot slippage, foot penetration of the ground, joint over-limits, and movement jumps. Furthermore, the height ratios of humans and humanoid robots differ. If motion data is geometrically scaled only according to height ratio, it disrupts the time scale of the center of mass dynamics. Based on the inverted pendulum model, the natural frequency of the swing motion is inversely proportional to the square root of the support height. After geometric scaling changes the equivalent support height, if the time scale of the motion is not adjusted accordingly, the gait frequency of the redirected result will not match the robot's dynamic characteristics, resulting in temporal rhythm distortion. In addition, solving the inverse kinematics problem independently for each frame in the action sequence without considering the connection between adjacent frames will lead to abrupt joint position changes and temporal discontinuities between adjacent frames.

[0003] Motion redirection based on inverse kinematics is a common technique in this field. Existing methods construct a multi-objective weighted cost function containing position and posture error terms, solve nonlinear optimization problems using sequential quadratic programming, and apply physical constraints such as joint limits and foot-to-ground contact. However, the following shortcomings still exist: geometric scaling and temporal scaling are usually handled independently, and a data layer processing mechanism that shares the same scaling ratio for geometric scaling and frame rate resampling has not yet been established. After geometric scaling, the temporal rhythm of motion data cannot be kept consistent with robot dynamics. The multi-objective inverse kinematics cost function is non-convex due to the nonlinearity of forward kinematics. Improper initial value selection can cause the solution to fall into local optima, and there is a lack of targeted initial value construction strategies, making it difficult to stably obtain an initial solution in a reasonable posture neighborhood. Pure kinematics redirection only focuses on geometric pose matching and does not explicitly model the contact state switching between the foot support phase and the airborne phase in the optimization loop. It also does not incorporate the dynamic balance constraint of the robot's center of mass into the redirection optimization, resulting in non-zero foot velocity at the moment of ground contact, obvious foot slippage and ground penetration, and a serious deviation of the center of mass from the support polygon, which can easily lead to falls.

[0004] Therefore, how to provide a method for redirecting human motion data to humanoid robots that can maintain the consistency of the time scale of the centroid dynamics after geometric scaling, incorporate the contact state and the feasible domain constraints of the centroid dynamics into the redirection optimization, avoid multi-objective optimization from getting trapped in local optima, and provide temporally continuous data has become a technical problem that urgently needs to be solved in this field. Summary of the Invention

[0005] In view of the above problems, this invention proposes a motion retargeting method for humanoid robots with contact and centroid constraints. This invention uses the kinematic characteristics shared by humans and humanoid robots as a benchmark, unifying geometric scaling and frame rate resampling to the same scaling ratio. It synchronously adjusts the spatial and temporal scales on the retargeting data layer, thereby maintaining the consistency of centroid dynamics in the retargeting results. Human motion data is divided into a support phase and a flight phase. In the support phase, hard constraints of zero tangential velocity at the foot tip and normal height locking are forcibly applied; in the flight phase, spatial trajectory smoothing tracking constraints are applied. A penalty term aligning the centroid horizontal projection point with the geometric center of the support polygon is incorporated into the multi-objective cost function, thus ensuring the physical contact realism and centroid balance of the retargeting trajectory. The sequence is decomposed into a frame-by-frame optimization problem. An initial solution is constructed through a first-stage solution that "only retains the root joint position weights and only retains the attitude weights of the remaining key points." Then, the original problem, including all position and attitude weights, is solved, thus avoiding multi-objective optimization from falling into local optima and obtaining a reference joint trajectory that is temporally continuous, physically consistent, and meets the required accuracy.

[0006] This invention provides the following solution:

[0007] A method for redirecting motion of a humanoid robot subject to contact and centroid constraints includes the following steps:

[0008] S1. Obtaining human motion data sequence and contact phase segmentation: Obtaining human motion data sequence containing root joint position, spatial pose and frame rate information of each frame; Based on foot kinematics features, dividing the human motion data sequence into periodically alternating foot support phase interval and foot airborne phase interval.

[0009] S2. Spatial geometry and temporal scale coupling scaling: The spatial scaling ratio r is determined based on the torso-foot height relationship between the human body and the humanoid robot. The spatial position of each key point is geometrically scaled according to the spatial scaling ratio r. Based on the equivalent centroid dynamic similarity law, the motion data frame rate is synchronously resampled based on the spatial scaling ratio r to maintain the centroid dynamic consistency between the spatial scale and the temporal scale.

[0010] S3. Construct a multi-objective optimization objective function that integrates the feasible domain constraint of the centroid dynamics: Using the scaled keypoint pose as the tracking reference, construct a multi-objective cost function that includes a keypoint pose weighted error term, a joint nominal configuration regularization term, and a centroid position offset penalty term; the centroid position offset penalty term is used to constrain the horizontal projection point of the humanoid robot's centroid to approach the geometric center of the current supporting polygon.

[0011] S4. Construction of dynamic constraint set dependent on contact state: For the entire action sequence, construct time-varying constraint set according to the contact phase divided in step S1: In the foot support phase interval, apply hard constraint on foot contact; in the foot air phase interval, release foot height constraint and apply spatial trajectory smoothing tracking constraint.

[0012] S5. Sequence frame-by-frame temporal decomposition and initial value setting for hot start: The entire continuous action sequence is decomposed into a frame-by-frame constrained nonlinear optimization problem. For each frame optimization problem, the generalized joint position obtained in the previous frame is used as the initial guess for the current frame optimization iteration, and a temporal smoothing term for the joint position of adjacent frames is introduced into the cost function.

[0013] S6. Two-stage optimization solution: The frame-by-frame optimization problem for each frame is solved using a two-stage method: In the first stage, only the position weight of the root node of the torso and the pose weight of the other feature points are activated to obtain a coarse initial solution; In the second stage, the coarse initial solution is used as a warm-start guess to fully activate the original problem, which includes the centroid position offset penalty term, the support phase contact hard constraint, and the full-weight pose matching, and to refine the solution, outputting the robot joint position that satisfies physical consistency for this frame.

[0014] S7. Output dynamically consistent reference joint trajectory: Iteratively output the robot joint positions obtained from each frame, generating a humanoid robot reference joint trajectory that satisfies physical contact consistency and centroid balance constraints, which is used to control the humanoid robot's motion.

[0015] As a further improvement to the above technical solution, in step S2, the spatial scaling ratio... In the formula The height of the human root joint. The height of the human foot. The height from the torso to the feet of the humanoid robot; the synchronous resampling of motion data frame rate based on the equivalent center of mass dynamic similarity law specifically involves: based on the natural frequency similarity law of a single inverted pendulum. The frame rate f before scaling is adjusted according to... Perform resampling update, where Let g be the natural frequency of the oscillation, g be the acceleration due to gravity, and h be the height of the center of mass, so that the geometrically scaled motion time rhythm matches the oscillation period of the humanoid robot's center of mass dynamics.

[0016] As a further improvement to the above technical solution, in step S1, the ground clearance height and velocity change rate of key foot points are detected frame by frame to divide the contact phase: when the ground clearance height of the foot exceeds a preset height threshold or the velocity change rate of the foot exceeds a preset velocity threshold, the frame is determined to be the foot take-off phase; otherwise, the frame is determined to be the foot support phase. In step S4, the foot contact hard constraint includes a normal height locking constraint. With tangential zero velocity constraint In the formula The height of the key point on the sole of the foot in the direction perpendicular to gravity. This is the current ground elevation. This represents the contact sliding velocity component of the foot in the horizontal plane. In frame-by-frame optimization, the tangential zero-velocity constraint is achieved by locking the horizontal position equation of the supporting foot in the current frame to the horizontal position of the supporting foot obtained from the optimization in the previous frame. .

[0017] As a further improvement to the above technical solution, the reference position of the centroid position offset penalty term in step S3 is adaptively switched according to the contact phase divided in step S1. In the bipedal support phase, the geometric center position of the current bipedal support polygon is taken, and in the unipedal support phase, the geometric center position of the unipedal support polygon corresponding to the current support foot is taken. In frame-by-frame optimization, the offset of the humanoid robot's centroid horizontal projection position relative to the reference position is penalized.

[0018] As a further improvement to the above technical solution, the position weight of the feature point k With pose weights Asymmetrical configuration based on body part, satisfying: positional weights of foot feature points. The positional weights of the feature points are higher than those of the trunk, legs, and upper limbs, and Positional weights of each feature point in the upper limb satisfy This allows upper limb movements to be retained as style features with a lower tracking weight than the feet, thereby reducing the disturbance of upper limb tracking to the lower limb balance solution while prioritizing the physical realism of foot contact.

[0019] As a further improvement to the above technical solution, the temporal smoothing term for the joint position of adjacent frames in step S5 is a second-order smoothing penalty term. The solved joint position of the previous frame and the joint position of the frame before that are taken as constants, and the deviation of the joint position to be optimized in the current frame relative to the joint position obtained by linear extrapolation of the joint position of the previous frame and the joint position of the frame before that is penalized.

[0020] As a further improvement to the above technical solution, the two-stage solution in step S6 specifically includes: First, constructing a relaxation optimization problem, resetting the position weights of all feature points except the root node of the torso to zero, retaining only the position tracking term of the root node and the attitude tracking terms of all feature points, and not applying a centroid position offset penalty term or a hard constraint on the contact between the support phase, thus obtaining a coarse initial solution for positioning. The second stage uses the coarse positioning initial solution. As an initial guess for the hot-start primal problem, the pose error of the primal problem and its Jacobian matrix are placed in the Lie group tangent space corresponding to the homogeneous transformation matrix. Linear-quadratic approximation is performed to construct a sequential quadratic programming subproblem, and the joint position is iteratively updated using a damping adjustment method until the residual change of the objective function is less than a preset threshold or the number of iterations reaches a preset upper limit.

[0021] A motion redirection device for a humanoid robot with contact and center-of-mass constraints, comprising:

[0022] Motion data parsing and phase segmentation module: used to acquire human motion data sequences and delineate the support phase and take-off phase intervals based on foot height and kinematic characteristics;

[0023] Spatiotemporal coupling scaling module: used to calculate the scaling ratio r based on the height ratio from torso to feet, geometrically scale the key point positions, and execute synchronously. Frame rate resampling;

[0024] Dynamic feasible region constraint construction module: used to establish a multi-objective inverse kinematic cost function that includes pose error, joint regularization term and centroid alignment penalty term;

[0025] Contact state dynamic constraint module: used to dynamically apply foot end tangential zero-velocity hard constraint or spatial tracking constraint according to the switching between the support phase and the airborne phase;

[0026] Two-stage sequence planning and solving module: used to perform frame-by-frame temporal decomposition. First, it obtains the initial value through the first stage coarse positioning, and then solves it through the second stage full-weighted damped sequence quadratic planning iteration with dynamics and contact constraints, outputting the temporally continuous and physically consistent reference joint trajectory.

[0027] A computer terminal includes a memory and a processor, the memory being used to store a computer program, and the processor executing the computer program to enable the terminal to implement the humanoid robot motion redirection method with contact and centroid constraints as described above.

[0028] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the contact and centroid constraint-based motion redirection method for humanoid robots as described above.

[0029] The beneficial effects of this invention are:

[0030] (1) Geometric scaling and frame rate resampling share the same scaling ratio. Geometric scaling changes the spatial scale of each key point position according to the ratio of the support height of the root joint relative to the foot end to the height from the torso to the foot. The frame rate is synchronously resampled according to the square root of this scaling ratio. Inspired by the relationship between the equivalent support height and the swing angular frequency in the inverted pendulum model, geometric scaling changes the equivalent support height. By adjusting the frame rate according to the square root of the scaling ratio, the time scale of motion can be synchronously adjusted on the retargeting data layer, so that the retargeting result is consistent with the robot's center of mass dynamics in terms of time rhythm, avoiding gait frequency distortion caused by geometric scaling, thereby improving the problems of foot slippage and motion distortion. At the same time, this processing unifies spatial scaling and temporal scaling with the same scaling ratio, eliminating the need to design separate parameters for temporal scaling. The processing flow is simple and highly consistent.

[0031] (2) The two-stage solution strategy avoids local optima. In the first stage, only the position weights of the root joint (trunk) are retained, and the other key points retain only the posture weights. The position matching task of the resulting optimization problem is focused on the whole trunk, and the posture matching task constrains the orientation of each limb, making it easy to obtain an initial solution within the neighborhood of a reasonable posture. Then, the initial solution is used as the initial guess to solve the original problem containing all position weights and posture weights. This can effectively avoid the multi-objective inverse kinematics cost function from getting trapped in local optima due to non-convexity, and improve the stability of the solution and the quality of the retargeting results.

[0032] (3) Frame-by-frame temporal decomposition and adjacent frame second-order smoothing take into account both continuity and efficiency. The action sequence is decomposed into a small-scale optimization problem frame by frame. The solution result of the previous frame is used as the initial guess of the current frame, and the joint positions of the previous and previous frames are used as constants. A second-order smoothing penalty term for the joint positions of adjacent frames is added to the cost function. While penalizing the sudden change of joint positions in adjacent frames, the jump of joint acceleration level is further suppressed. This ensures the temporal continuity of the redirection result, and makes each optimization problem small in scale and efficient in solution. It can also be used as a general redirection module for offline reference data construction and online redirection.

[0033] (4) Lie group tangent space pose error calculation enhances numerical stability. The position and attitude are unified in the tangent space of the Lie group corresponding to the homogeneous transformation matrix to calculate the error and its Jacobian, avoiding numerical problems caused by the inconsistency of the dimensions of position and attitude. Combined with the iterative update of the damping coefficient, the numerical stability of convergence is enhanced at the cost of slowing down the convergence speed.

[0034] (5) The weighting of different parts takes into account both contact stability and natural movement. The position weight of the key points of the foot is the highest, which can ensure the stability of the contact relationship between the foot and the ground and avoid foot slippage; the position weight of the key points of the upper limb is lower than that of the key points of the foot, so that the upper limb movement can be retained as a style feature, while maintaining the naturalness of the movement while ensuring the accuracy of the lower limb movement.

[0035] (6) Contact state adaptive dynamic constraints suppress physical distortion from the source. Based on the foot height threshold and velocity change rate, human motion data is divided into support phase and take-off phase. Hard constraints of foot tangential zero velocity and normal height locking are forcibly applied in the support phase. In the take-off phase, the foot height constraint is released and spatial trajectory smoothing tracking constraint is applied. Physical distortions such as foot slip, ground penetration and take-off suspension that are common in pure kinematic reorientation are suppressed from the source of constraints.

[0036] (7) Alignment of the feasible region of centroid dynamics improves the stability of downstream control. A penalty term is introduced into the cost function to align the horizontal projection point of the centroid with the geometric center of the supporting polygon. This makes the kinematic redirection implicitly embed the zero-torque balance prior. The centroid of the reference trajectory output by the redirection is always close to the supporting polygon, which improves the stability and success rate of subsequent reinforcement learning control or trajectory tracking. Attached Figure Description

[0037] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0038] Figure 1 is a schematic diagram of the overall process of the humanoid robot motion reversal method provided in an embodiment of the present invention;

[0039] Figure 2 is a schematic diagram of the contact phase adaptive constraint and centroid alignment principle provided in an embodiment of the present invention;

[0040] Figure 3 is a flowchart of the two-stage optimization and Lie group tangent space solution provided in the embodiment of the present invention;

[0041] Figure 4 is a schematic diagram comparing the motion redirection effects provided by the embodiments of the present invention. Detailed Implementation

[0042] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. The components of the embodiments of this application described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely represents selected embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without creative effort are within the scope of protection of this application.

[0043] Example 1

[0044] Referring to Figure 1, this embodiment provides a method for redirecting the motion of a humanoid robot subject to contact and centroid constraints, including the following steps:

[0045] Step S1: Acquire human motion data sequence and perform contact phase segmentation.

[0046] Referring to step S1 in Figure 1, a human motion data sequence is obtained. The human motion data sequence includes the root joint position of each frame, the position and pose of each key point, and frame rate information.

[0047] In one implementation, the human motion data sequence is derived from a publicly available motion capture dataset and / or motion capture device. The data includes at least skeletal hierarchy, skeletal length parameters, root joint positions, joint angles, and frame rate information. After parsing and forward kinematics calculation, the position and posture of each key point in the world coordinate system are obtained.

[0048] Considering scenarios targeting foot-based movements, motion data that includes foot support and transitions into airborne motion are prioritized. This allows for contact phase segmentation based on foot height and velocity change rate, providing a phase basis for subsequent adaptive constraints on contact states.

[0049] Referring to Figure 2, contact phase segmentation is performed on the acquired human motion data sequence: a threshold for foot lift-off height (0.03 m in this embodiment) and a threshold for foot velocity change rate are set, and the lift-off height and velocity change rate of key foot points are detected frame by frame; when the foot lift-off height exceeds the threshold or the foot velocity change rate exceeds the preset threshold, the frame is marked as the foot take-off phase; otherwise, it is marked as the foot support phase. This divides the entire motion sequence into periodic foot support phase intervals and foot take-off phase intervals, serving as the phase basis for subsequent dynamic constraint construction.

[0050] Step S2: Determine the scaling ratio.

[0051] Referring to Figure 2, considering the height difference between the human body and the humanoid robot, the overall scale is adjusted based on the robot's height. Let... The height of the human root joint (trunk). The height of the foot. Given the height from the robot's torso to its feet, the scaling ratio r is expressed by the following formula:

[0052] (1)

[0053] Considering that the focus of retargeting is lower limb movement, the height reference selected in this embodiment is not from head to toe, but from torso to toe, so that the scaling ratio reflects the ratio of the height from the root joint to the foot of the human body to the height from the torso to the foot of the robot, thus achieving root-to-foot scale matching.

[0054] Step S3: Perform geometric scaling according to the scaling ratio.

[0055] The positions of key points in the human motion data sequence are geometrically scaled, and the scaled key point positions are represented by the following formula:

[0056] , (2)

[0057] in, Let K be the position of feature point k in the human motion data, and K be the selected set of feature points.

[0058] Step S4: Resample the frame rate according to the scaling ratio.

[0059] Considering the consistency problem of center-of-mass dynamics: subject to the laws of the inverted pendulum model. Inspiration, among which Let g be the natural frequency of the oscillation, h be the acceleration due to gravity, and h be the height of the center of mass. Geometric scaling alters the equivalent support height, requiring corresponding adjustments to the time scale of the motion. This embodiment resamples the frame rate of the geometrically scaled motion data, as expressed by the following formula:

[0060] (3)

[0061] Where f is the frame rate before scaling. The geometric scaling (step S3) and the frame rate resampling (step S4) share the same scaling ratio r: geometric scaling changes the spatial scale of each keypoint position by r, and frame rate resampling... Adjusting the time scale of motion data allows for simultaneous adjustment of spatial and temporal scales on the retargeting data layer, maintaining the consistency of centroid dynamics in the retargeting results and avoiding temporal rhythm distortion caused by mismatch between gait frequency and robot dynamics after geometric scaling.

[0062] Step S5: Construct a multi-objective optimization objective function that incorporates the feasible region constraint of the centroid dynamics.

[0063] Using the scaled keypoint positions and poses as references, a multi-objective inverse kinematics (IK) cost function is constructed. Coordinate systems for corresponding key feature points on the human motion model and the robot model are selected. The forward kinematic data of these coordinate systems are used as features to ensure that the positions and poses of the corresponding points on the robot are as close as possible to those in the human coordinate system. The repositioning problem is defined as a multi-objective inverse kinematics problem. Considering the pose similarity of the keypoints and incorporating the feasible region constraint of the center of mass dynamics, the cost function is expressed by the following equation:

[0064] (4)

[0065] Where q is the robot's generalized position, which often includes the floating base pose and joint angles; k is the feature point number, and K is the selected set of feature points; The position weight of feature point k; Let k be the pose weight of feature point k. The position of feature point k in human motion data, relative to the world frame; Let k be the position of feature point k on the robot, relative to the world frame; Let k be the pose of feature point k in human motion data, relative to the world frame; Let k be the pose of feature point k on the robot, relative to the world frame; This represents the difference operator for the logarithmic mapping of the tangent space of a Lie group; Weights for the joint angle regularization term; This represents the robot's nominal (default) joint position; The centroid offset penalty weight; Let this be the projected position of the robot's center of mass in the horizontal plane; This represents the geometric center position of the current bipedal or monopedal supporting polygon. The joint angle regularization term is used to ensure the numerical stability of the constructed optimization problem; the centroid offset penalty term is used to constrain the horizontal projection point of the robot's centroid to approach the geometric center of the supporting polygon, so that the redirection trajectory implicitly embeds the zero-torque balance prior.

[0066] The feature points constructed in this embodiment include parts such as the torso, head, and limbs. The weight configurations for different parts are different, as shown in Table 1.

[0067] Table 1. Key Point Weight Configuration for Redirect Optimization

[0068] Key Point Types upper part of the torso 100 50 lower end of the torso 100 50 leg 100 50 foot 200 50 arm 100 0 hand 100 0 finger 50 50 head 200 50

[0069] For foot-based movements, the weighting principles for different body parts are as follows: the human torso corresponds to the floating base of the humanoid robot, therefore the position and posture weights of the key points at the upper and lower ends of the torso are relatively large; the feet are key to human dynamics interaction, and the position weights of the key points on the feet are the highest. This helps maintain physical consistency in movement and prevent slippage; key points of the upper limbs, including the arms, hands, and fingers, play an auxiliary role in balance and are mainly considered stylistic features. Therefore, their positional weight is significantly lower than that of key points of the feet, not exceeding half the positional weight of the feet (the positional weight of the arms and hands...). pose weights Finger position weight pose weights This allows upper limb movements to be preserved as style features with lower tracking weights, while reducing the impact of upper limb tracking errors on lower limb contact and balance solutions; the head, as an important reference for overall posture and orientation, has both position and posture weights set to higher values. , The above configuration satisfies the following weight hierarchy relationship between parts: the position weight of the foot feature point is higher than the position weight of each feature point of the trunk, legs and upper limbs, and the position weight of the foot is not less than twice its posture weight; the position weight of each feature point of the upper limbs does not exceed half of the position weight of the foot.

[0070] In addition, to meet the physical consistency requirement, this embodiment also imposes the following constraints when solving the frame-by-frame optimization problem:

[0071] (1) Joint limiting constraint: (Note) ,in For floating base pose, Let be the joint angle. Joint limiting constraints only apply to the joint angle component, as expressed by the following formula:

[0072] (5)

[0073] in, and These are the lower and upper bound vectors for the joint angle, respectively.

[0074] (2) Collision constraint between foot and ground: After treating the direction of gravity as the z-direction, the collision constraint between foot and ground is expressed by the following formula:

[0075] (6)

[0076] in, The height of the foot's key points in the direction of gravity; the ground position is obtained by fitting the foot position determined to be in contact.

[0077] (3) Dynamic constraints dependent on contact state: A time-varying constraint set is constructed based on the contact phases divided in step S1. When a foot is in the support phase, a hard contact constraint is applied to the foot end:

[0078] , (7)

[0079] in, The height of the key point on the sole of the foot in the direction perpendicular to gravity. This is the current ground elevation; This represents the contact sliding velocity component of the foot in the horizontal plane. In frame-by-frame optimization, a tangential zero-velocity constraint is used to support the foot's horizontal position locking constraint. The implementation involves locking the horizontal position of the supporting foot to the position of the previous frame during the support phase, thus preventing foot slippage from the source of the constraint; the normal height locking constraint ensures that the supporting foot maintains stable contact with the ground. When a foot on one side is in the air phase, the foot height constraint is released, and a smooth spatial trajectory tracking constraint is applied instead to ensure that the swinging foot moves along a smooth spatial trajectory, avoiding airborne stagnation and abrupt stomping.

[0080] Step S6: Timing decomposition.

[0081] The above steps apply to single-frame motion data, but motion consists of a continuous sequence of actions, requiring consideration of the connections between actions. This embodiment adopts a frame-by-frame decomposition approach in the temporal dimension: the decision variable for each frame's optimization problem is only the robot joint position in that frame. Frames are associated with each other through the propagation of initial guesses and the penalty terms of adjacent frames, and the size of the optimization problem does not increase with the length of the motion sequence.

[0082] The human motion data sequence is decomposed into a frame-by-frame optimization problem. For each frame, the solution result of the previous frame is used as the initial guess for the current frame, and the joint positions of the previous and the frame before that are used as constants. A second-order smoothing penalty term for the joint positions of adjacent frames is added to the cost function. Considering the continuity between actions, the optimization problem of frame t is expressed by the following equation:

[0083]

[0084] st

[0085] (8)

[0086] in, The multi-objective inverse kinematic cost function constructed in step S5, The weights of the second-order smoothing terms in adjacent frames. This represents the joint position in the current frame. , These refer to the joint positions of the previous and the frame before that, respectively. The adjacent frame second-order smoothing penalty term... Penalizing the second-order difference of joint positions at adjacent time points—that is, abrupt changes in joint acceleration levels—ensures the temporal continuity of the retargeting result. In this decomposition method, the joint positions of the previous and previous frames are constants rather than decision variables, thus guaranteeing the continuity of the action sequence. Solving a series of small-scale problems is much simpler and more efficient than solving a single large-scale problem. Furthermore, this temporal decomposition-based implementation can serve as a general-purpose retargeting module, not only for constructing offline motion reference datasets but also for online video / VR teleoperation retargeting.

[0087] Step S7: Solve and output the two-stage sequential quadratic programming damping.

[0088] Due to the nonlinearity of forward kinematics, the frame-by-frame optimization problem cannot be classified as a simple quadratic programming (QP) problem. This embodiment employs a sequential quadratic programming (SQP) method for solution. Specifically, the original problem is solved iteratively multiple times. Each time, a linear-quadratic approximation is performed in the neighborhood of the current solution. The Hessian matrix and Jacobian of the original objective function, as well as the Jacobian of the constraints, are calculated. Then, the corresponding QP subproblem is solved, and the solution of the QP subproblem is used to replace the solution of the original problem. The QP subproblem is expressed by the following equation:

[0089]

[0090] st

[0091] (9)

[0092] in, The step size for updating joint position is defined, where H is the Hessian matrix, c is the gradient vector, G and h are the inequality constraint matrix and vector, respectively, and A and b are the equality constraint matrix and vector, respectively. The inequality constraints include joint limiting constraints and foot collision constraints, and the equality constraints include support phase foot height locking and horizontal position locking constraints.

[0093] The objective function of the original problem can be viewed as the sum of multiple quadratic terms. Therefore, the Hessian matrix and gradient vector of the QP subproblem can be calculated by the following formula:

[0094]

[0095] (10)

[0096]

[0097] in, Let be the error vector of the i-th target at the current point (position error, attitude error, joint nominal deviation, second-order smoothing deviation, and centroid offset deviation). Let be the Jacobian matrix of the i-th target (position pos, pose rot, joint angle regularization nom, adjacent frame second-order smoothing smooth, centroid offset penalty com). These are the weighting coefficients for the corresponding targets.

[0098] For the calculation of the Jacobian: the Jacobian of the two objectives calculated directly in joint space—the joint angle regularization term and the second-order smoothing term of adjacent frames—is the identity matrix; however, the processing of coordinate position and attitude terms in Euclidean space is more complex. This embodiment processes position and attitude together, and the difference is calculated in the tangent space of the Lie group corresponding to homogeneous coordinates, expressed by the following formula:

[0099] (11)

[0100] in, and These are the homogeneous transformation matrices of human and robot feature points relative to the world coordinate system, respectively. For the logarithmic mapping from the Lie group to the Lie algebra, The vee operator is used for the transformation of an antisymmetric matrix into a 6-dimensional vector. The Jacobian of the pose error is also calculated in the tangent space.

[0101] The iterative method for solving the original problem is expressed by the following formula:

[0102] (12)

[0103] in, The damping adjustment coefficient set for the algorithm increases the numerical stability of convergence at the cost of slowing down the convergence speed; that is, the iteration step size multiplied by the damping coefficient. The iteration terminates when the change in the objective function residual is less than a certain threshold. Or the number of iterations reaches the preset upper limit N.

[0104] Referring to Figure 3, to avoid the algorithm getting trapped in local optima, this embodiment employs a two-stage optimization approach. The first stage constructs a relaxation optimization problem: the position weights of all feature points except the root node of the torso are reset to zero, meaning only the position weights of the root joints (torso) are retained. The remaining keypoints only have pose weights, not position weights, and no centroid offset penalty term or hard constraint on contact with the support is applied. This process yields the initial coarse localization solution. The second stage uses the coarse positioning initial solution. As an initial guess, the hot-start primal problem is: the pose error and its Jacobian matrix of the primal problem are transformed into the Lie group tangent space corresponding to the homogeneous transformation matrix according to equation (11). The linear-quadratic approximation is performed to construct the quadratic programming subproblem described in equation (9), and the damping adjustment coefficient is adjusted according to equation (12). Iterative updates continue until the change in the objective function residual is less than a threshold. Alternatively, the number of iterations may reach the upper limit N, at which point the solution result for the current frame is obtained. The two-stage solution process is as follows: for each frame, a first-stage relaxation problem is constructed (only the position weights of the root joints are retained, and the pose weights of the other keypoints are retained). The initial coarse positioning solution is obtained by solving the problem. Using this initial solution as the initial conjecture, we construct and solve the original problem, which includes all position and attitude weights, centroid offset penalty terms, and hard constraints on support phase contact. Obtain the joint positions for this frame.

[0105] After performing the above solution sequentially on each frame of the human motion data sequence, the joint positions of each frame are output iteratively to generate a humanoid robot reference joint trajectory that satisfies the physical contact consistency and center of mass balance constraints, which is used for the motion control of the humanoid robot. The redirection result in this embodiment can be used to generate reference motion data as a reference input for the humanoid robot's anthropomorphic motion control; the correspondence between the redirection result and human motion can be seen in the schematic diagram of the redirection result sequence shown in Figure 4.

[0106] Example 2

[0107] Referring to the process shown in Figure 1 and the module division of this embodiment, this embodiment provides a motion redirection device for a humanoid robot with contact and centroid constraints, including:

[0108] Motion data parsing and phase segmentation module: used to acquire human motion data sequences and delineate the support phase and take-off phase intervals based on foot height and kinematic characteristics;

[0109] Spatiotemporal coupling scaling module: used to calculate the scaling ratio r based on the height ratio from torso to feet, geometrically scale the key point positions, and execute synchronously. Frame rate resampling;

[0110] Dynamic feasible region constraint construction module: used to establish a multi-objective inverse kinematic cost function that includes pose error, joint regularization term and centroid alignment penalty term;

[0111] Contact state dynamic constraint module: used to dynamically apply foot end tangential zero-velocity hard constraint or spatial tracking constraint according to the switching between the support phase and the airborne phase;

[0112] Two-stage sequence planning and solving module: used to perform frame-by-frame temporal decomposition. First, it obtains the initial value through the first stage coarse positioning, and then solves it through the second stage full-weighted damped sequence quadratic planning iteration with dynamics and contact constraints, outputting the temporally continuous and physically consistent reference joint trajectory.

[0113] The specific processing procedures of each module described above correspond one-to-one with steps S1 to S7 in Example 1, and will not be repeated here. In one implementation, the spatiotemporal coupling scaling module includes a scaling ratio calculation submodule, a geometric scaling submodule, and a frame rate resampling submodule, which are used to calculate the scaling ratio r, perform geometric scaling of key point positions, and perform frame rate resampling, respectively; the dynamic feasible domain constraint construction module may further include a weight configuration submodule and a constraint construction submodule, which are used to configure the position weights and attitude weights of each key point according to the part, and construct joint limit constraints and foot collision constraints, respectively.

[0114] Example 3

[0115] This embodiment provides a computer terminal, including a memory and a processor. The memory is used to store a computer program, and the processor executes the computer program to enable the terminal to implement the humanoid robot motion redirection method with contact and centroid constraints as described in Embodiment 1.

[0116] Example 4

[0117] This embodiment provides a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, it implements the humanoid robot motion redirection method with contact and centroid constraints as described in Embodiment 1.

[0118] The above description is merely a partial embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these all fall within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the appended claims.

Claims

1. A method for redirecting the motion of a humanoid robot subject to contact and center-of-mass constraints, characterized in that, Includes the following steps: S1. Obtaining human motion data sequence and contact phase segmentation: Obtaining human motion data sequence containing root joint position, spatial pose and frame rate information of each frame; Based on foot kinematics features, dividing the human motion data sequence into periodically alternating foot support phase interval and foot airborne phase interval. S2. Spatial geometry and temporal scale coupling scaling: The spatial scaling ratio r is determined based on the torso-foot height relationship between the human body and the humanoid robot. The spatial position of each key point is geometrically scaled according to the spatial scaling ratio r. Based on the equivalent centroid dynamic similarity law, the motion data frame rate is synchronously resampled based on the spatial scaling ratio r to maintain the centroid dynamic consistency between the spatial scale and the temporal scale. S3. Construct a multi-objective optimization objective function that integrates the feasible domain constraint of the centroid dynamics: Using the scaled keypoint pose as the tracking reference, construct a multi-objective cost function that includes a keypoint pose weighted error term, a joint nominal configuration regularization term, and a centroid position offset penalty term; the centroid position offset penalty term is used to constrain the horizontal projection point of the humanoid robot's centroid to approach the geometric center of the current supporting polygon. S4. Construction of dynamic constraint set dependent on contact state: For the entire action sequence, construct time-varying constraint set according to the contact phase divided in step S1: In the foot support phase interval, apply hard constraint on foot contact; in the foot air phase interval, release foot height constraint and apply spatial trajectory smoothing tracking constraint. S5. Sequence frame-by-frame temporal decomposition and initial value setting for hot start: The entire continuous action sequence is decomposed into a frame-by-frame constrained nonlinear optimization problem. For each frame optimization problem, the generalized joint position obtained in the previous frame is used as the initial guess for the current frame optimization iteration, and a temporal smoothing term for the joint position of adjacent frames is introduced into the cost function. S6. Two-stage optimization solution: The frame-by-frame optimization problem is solved using a two-stage method: In the first stage, only the position weight of the root node of the torso and the pose weight of the other key points are activated to obtain the initial coarse localization solution. The second stage uses the coarse initial solution as a hot start guess, fully activates the original problem including the centroid position offset penalty term, the support phase contact hard constraint and the full weight pose matching, and performs a refined solution, outputting the robot joint position that satisfies physical consistency in this frame. S7. Output dynamically consistent reference joint trajectory: Iteratively output the robot joint positions obtained from each frame, generating a humanoid robot reference joint trajectory that satisfies physical contact consistency and centroid balance constraints, which is used to control the humanoid robot's motion.

2. The method for redirecting motion of a humanoid robot with contact and center-of-mass constraints according to claim 1, characterized in that: In step S2, the spatial scaling ratio In the formula The height of the human root joint. The height of the human foot. The height from the torso to the feet of the humanoid robot; the synchronous resampling of motion data frame rate based on the equivalent center of mass dynamic similarity law specifically involves: based on the natural frequency similarity law of a single inverted pendulum. The frame rate f before scaling is adjusted according to... Perform resampling update, where Let g be the natural frequency of the oscillation, g be the acceleration due to gravity, and h be the height of the center of mass, so that the geometrically scaled motion time rhythm matches the oscillation period of the humanoid robot's center of mass dynamics.

3. The method for redirecting motion of a humanoid robot with contact and center-of-mass constraints according to claim 1, characterized in that: In step S1, the ground clearance height and velocity change rate of key foot points are detected frame by frame to define the contact phase: when the ground clearance height of the foot exceeds a preset height threshold or the velocity change rate of the foot exceeds a preset velocity threshold, the frame is determined to be the foot take-off phase; otherwise, the frame is determined to be the foot support phase. In step S4, the foot contact hard constraint includes a normal height locking constraint. With tangential zero velocity constraint In the formula The height of the key point on the sole of the foot in the direction perpendicular to gravity. This is the current ground elevation. This represents the contact sliding velocity component of the sole of the foot in the horizontal plane; In frame-by-frame optimization, the tangential zero-velocity constraint is achieved by locking the horizontal position equation of the support foot in the current frame to the horizontal position of the support foot obtained from the optimization in the previous frame, i.e. .

4. The method for redirecting motion of a humanoid robot with contact and center-of-mass constraints according to claim 1, characterized in that: The reference position of the centroid position offset penalty term in step S3 is adaptively switched according to the contact phase divided in step S1. In the bipedal support phase, the geometric center position of the current bipedal support polygon is taken, and in the unipedal support phase, the geometric center position of the unipedal support polygon corresponding to the current support foot is taken. In frame-by-frame optimization, the offset of the humanoid robot's centroid horizontal projection position relative to the reference position is penalized.

5. The method for redirecting motion of a humanoid robot with contact and center-of-mass constraints according to claim 1, characterized in that: The position weight of feature point k With pose weights Asymmetrical configuration based on body part, satisfying: positional weights of foot feature points. The positional weights of the feature points are higher than those of the trunk, legs, and upper limbs, and Positional weights of each feature point in the upper limb satisfy This allows upper limb movements to be retained as style features with a lower tracking weight than the feet, thereby reducing the disturbance of upper limb tracking to the lower limb balance solution while prioritizing the physical realism of foot contact.

6. The method for redirecting motion of a humanoid robot with contact and center-of-mass constraints according to claim 1, characterized in that: The temporal smoothing term for the joint position of adjacent frames in step S5 is a second-order smoothing penalty term. The solved joint positions of the previous frame and the frame before that are treated as constants, and the deviation of the joint position to be optimized in the current frame relative to the joint position obtained by linear extrapolation of the joint positions of the previous frame and the frame before that is penalized.

7. The method for redirecting motion of a humanoid robot with contact and centroid constraints according to claim 1, characterized in that: The two-stage solution described in step S6 specifically includes: Phase 1: Construct a relaxation optimization problem. Reset the position weights of all feature points except the root node of the torso to zero, retain only the position tracking term of the root node and the attitude tracking terms of all feature points, and do not apply a centroid position offset penalty term or a hard constraint on the contact between the support phase. Solve to obtain the initial coarse localization solution. ; Second stage: Using the coarse positioning initial solution As an initial guess for the hot-start primal problem, the pose error of the primal problem and its Jacobian matrix are placed in the Lie group tangent space corresponding to the homogeneous transformation matrix. Linear-quadratic approximation is performed to construct a sequential quadratic programming subproblem, and the joint position is iteratively updated using a damping adjustment method until the residual change of the objective function is less than a preset threshold or the number of iterations reaches a preset upper limit.

8. A motion redirection device for a humanoid robot with contact and center-of-mass constraints, characterized in that, include: Motion data parsing and phase segmentation module: used to acquire human motion data sequences and delineate the support phase and take-off phase intervals based on foot height and kinematic characteristics; Spatiotemporal coupling scaling module: used to calculate the scaling ratio r based on the height ratio from torso to feet, geometrically scale the key point positions, and execute synchronously. Frame rate resampling; Dynamic feasible region constraint construction module: used to establish a multi-objective inverse kinematic cost function that includes pose error, joint regularization term and centroid alignment penalty term; Contact state dynamic constraint module: used to dynamically apply foot end tangential zero-velocity hard constraint or spatial tracking constraint according to the switching between the support phase and the airborne phase; Two-stage sequence planning and solving module: used to perform frame-by-frame temporal decomposition. First, it obtains the initial value through the first stage coarse positioning, and then solves it through the second stage full-weighted damped sequence quadratic planning iteration with dynamics and contact constraints, outputting the temporally continuous and physically consistent reference joint trajectory.

9. A computer terminal, comprising a memory and a processor, wherein the memory is used to store computer programs, characterized in that: The processor executes the computer program to enable the terminal to implement the motion redirection method for a humanoid robot with contact and centroid constraints as described in any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by the processor, it implements the motion redirection method for a humanoid robot with contact and centroid constraints as described in any one of claims 1 to 7.

Images