Robot motion re-planning method based on two-stage optimization and multi-constraint fusion

Abstract

This invention relates to the field of robot motion control technology. Addressing the kinematic artifacts and quality / robustness issues present in existing technologies, it proposes a robot motion retargeting method based on two-stage optimization and multi-constraint fusion. The method involves: establishing a mapping relationship between the human skeleton and the robot skeleton through key body part matching; adjusting the initial posture using static posture alignment; adapting to the morphological differences between the human and robot through non-uniform local scaling; constructing a multi-constraint optimization model and handling constraint conflicts using the minimum violation principle; and designing a two-stage inverse kinematics solution strategy: prioritizing the optimization of the end effector posture by strengthening the weights of the spinal joints; relaxing the weights of the end effector to optimize the posture of all key body parts; fusing the three constraints; and using the augmented Lagrangian method for solution. This invention effectively solves the artifact problems in motion retargeting, such as posture collapse, foot slippage, balance imbalance, and unnatural motion, improving the quality and robustness of the retargeted motion.

Description

Technical Field

[0001] This invention relates to the field of robot motion control technology, specifically to a robot motion redirection method based on two-stage optimization and multi-constraint fusion. Background Technology

[0002] In complex, human-centric environments such as homes, industries, and medical settings, autonomous humanoid robots must perform continuous tasks under conditions of frequent contact. This places two core requirements on the robotic system: First, it must achieve stable, coordinated full-body control under multi-contact conditions, meaning it must rationally distribute contact forces under combinations of conditions such as bipedalism, hand support, or partial body contact, satisfying stability constraints such as friction cones, support polygons, and zero-moment points (ZMPs) to avoid slippage, tipping over, or self-collision. Second, it must possess high-fidelity human motion redirection capabilities to effectively transfer human operational skills to the robotic platform.

[0003] To achieve these goals, existing research typically maps human motion capture data onto a robot skeleton and generates joint trajectories through inverse motion learning. However, there are significant differences between humans and robots in terms of body size, joint degrees of freedom, and contact patterns. Direct mapping can easily lead to artifacts such as foot slippage, ground penetration, and joint abrupt changes, which not only reduce the naturalness and feasibility of movements but also increase the difficulty and safety risks of simulator-to-real transfer.

[0004] From a technical perspective, existing methods are mainly divided into kinematic-based direct mapping and optimization-based generative methods. Direct mapping relies on joint matching or coordinate transformation, which is computationally efficient but struggles to handle scale differences and asymmetry in degrees of freedom, easily leading to posture collapse and end-effector deviation. Optimization-based methods balance posture similarity, smoothness, joint constraints, and contact maintenance by constructing loss functions and constraint terms. For example, they introduce a phased learning rate strategy to balance approximation accuracy and physical feasibility, or adjust lower limb joints online based on real-time support status to maintain dynamic balance.

[0005] However, these methods still have limitations: most do not fully consider the differentiated weights of different body parts in motion coordination, especially neglecting the crucial role of core parts such as the spine in posture and stability; constraint models are often relatively fixed, lacking adaptive coordination mechanisms for multiple constraint conflicts; more importantly, existing research usually treats multi-contact center-of-mass control and motion redirection separately—center-of-mass control methods often assume fixed contact points or decouple them from motion generation, failing to fully consider the real-time impact of posture errors introduced by redirection on contact stability; while motion redirection methods focus more on kinematic fidelity, lacking systematic modeling of center-of-mass stability, friction constraints, and contact consistency. This separation makes it difficult to simultaneously ensure motion similarity and physical stability in dynamic, multi-contact complex tasks, limiting the reliable application of humanoid robots in real-world scenarios.

[0006] Therefore, there is an urgent need to develop a method that can deeply integrate kinematic reorientation and multi-contact stability constraints, and has the ability to perform hierarchical optimization and constraint conflict coordination, so as to generate robot motion that is both natural, accurate and stable in complex interactive scenarios. Summary of the Invention

[0007] In view of this, in order to solve the kinematic artifacts such as foot slippage, ground crossing, and joint mutation in the prior art, as well as the problems of mass and robustness, this invention provides a robot motion redirection method based on two-stage optimization and multi-constraint fusion. Kinematic adaptation is achieved through key body part matching and posture alignment. Contact, center of mass, and statistical manifold constraint models are constructed, and conflicts are coordinated with the principle of minimum violation. A two-stage inverse kinematics strategy of spine priority followed by global optimization is adopted, and the augmented Lagrangian method is combined to solve the problem, so as to achieve the unity of naturalness and stability in motion redirection.

[0008] This invention provides a robot motion redirection method based on two-stage optimization and multi-constraint fusion, comprising:

[0009] Step 110: Establish the mapping relationship between key body parts of the human body and the robot, and configure the weights of the pose tracking errors of key body parts.

[0010] Step 120: Perform static pose alignment on the source motion data of the human body, and perform non-uniform local scaling on key body parts according to the robot's shape to generate a preprocessed reference trajectory.

[0011] Step 130: Based on the preprocessed reference trajectory, minimize the attitude tracking error of the end effector as the original objective function. Under the inverse kinematics framework, with joint kinematic constraints as the basic constraint, construct a multi-constraint optimization model that integrates contact constraints, centroid constraints, and statistical manifold constraints.

[0012] Step 140: Introduce an optimizable violation quantity to relax the multi-constraint optimization model and obtain a robust optimization model that allows for minor constraint violations.

[0013] Step 150: The robust optimization model is solved using the enhanced Lagrangian method combined with the Mink solver, and a two-stage inverse kinematics solution strategy is adopted in the solution process: In the first stage, the weights of the spinal joints are strengthened to prioritize the optimization of the end effector's posture; in the second stage, based on the posture results of the end effector in the first stage, the postures of all key body parts are optimized.

[0014] Step 160: Optimize each frame of source motion data corresponding to the preprocessed reference trajectory frame by frame, obtain the joint angle sequence, and then repair the global height artifact to obtain the robot's redirected motion sequence.

[0015] In summary, this invention provides a robot motion redirection method based on two-stage optimization and multi-constraint fusion. Compared with the prior art, the method of this invention has the following advantages:

[0016] (1) This invention coordinates different objectives through statistical manifold constraints and a two-stage optimization strategy, which significantly improves motion smoothness and stability. For example, the rate of change of acceleration of generated motion (NewJerk) is reduced by about 15.88%, which greatly reduces motion jitter and abrupt changes, making the robot's movements smoother and energy consumption lower.

[0017] (2) By adopting a solution strategy that integrates contact constraints and centroid constraints, the problems of foot slippage, ground crossing and balance imbalance are fundamentally solved, and typical kinematic artifacts are effectively suppressed. Experiments show that the method of this invention reduces the joint rotation error (Rot) from 2.4332 rad to 0.1081 rad, proving that the joint movement is extremely smooth and avoids unnatural posture changes from the source.

[0018] (3) The present invention achieves a good balance in ensuring motion fidelity: while ensuring the physical rationality of motion, the global (G-MPBPE) and local (L-MPBPE) trajectory errors are further reduced, achieving an excellent balance between the "similarity" in motion vision and the "physical feasibility" in execution. Attached Figure Description

[0019] Figure 1 This is a flowchart illustrating the steps of a robot motion redirection method based on two-stage optimization and multi-constraint fusion in one embodiment of the present invention.

[0020] Figure 2 This is a schematic diagram of keyframes of the simulation results obtained using existing techniques in the experiments of this invention. Figure 2 (a) Figure 2 (b) Figure 2 (c) Figure 2 (d) and Figure 2 (e) Extract keyframe images of the jumping action segment at a rate of 5 frames per second between 2 minutes 50 seconds and 2 minutes 52 seconds;

[0021] Figure 3 This is a schematic diagram of keyframes of simulation results obtained at the same time point in the experiment using the method of this invention and the prior art method, wherein, Figure 3 (a) Figure 3 (b) Figure 3 (c) Figure 3 (d) and Figure 3 (e) Extract keyframe images of the jumping action segment at a rate of 5 frames per second between 2 minutes 50 seconds and 2 minutes 52 seconds. Detailed Implementation

[0022] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are for illustrative purposes only and are not intended to limit the invention.

[0023] To address the shortcomings of existing technologies, the purpose of this invention is to propose a high-fidelity human motion redirection control method for humanoid robots that integrates multi-contact center-of-mass safety constraints. The core idea is to reshape the original feasible solution space by introducing three new constraints while maintaining the inverse kinematics (IK) solution framework, thereby obtaining a better redirection solution within the same IK framework.

[0024] In one embodiment, such as Figure 1 As shown, this invention provides a robot motion redirection method based on two-stage optimization and multi-constraint fusion, comprising:

[0025] Step 110: Establish the mapping relationship between key body parts of the human body and the robot, and configure the weights of the pose tracking errors of key body parts.

[0026] Step 120: Perform static pose alignment on the source motion data of the human body, and perform non-uniform local scaling on key body parts according to the robot's shape to generate a preprocessed reference trajectory.

[0027] Step 130: Based on the preprocessed reference trajectory, minimize the attitude tracking error of the end effector as the original objective function. Under the inverse kinematics framework, with joint kinematic constraints as the basic constraint, construct a multi-constraint optimization model that integrates contact constraints, centroid constraints, and statistical manifold constraints.

[0028] Step 140: Introduce an optimizable violation quantity to relax the multi-constraint optimization model and obtain a robust optimization model that allows for minor constraint violations.

[0029] Step 150: The robust optimization model is solved using the enhanced Lagrangian method combined with the Mink solver, and a two-stage inverse kinematics solution strategy is adopted in the solution process: In the first stage, the weights of the spinal joints are strengthened to prioritize the optimization of the end effector's posture; in the second stage, based on the posture results of the end effector in the first stage, the postures of all key body parts are optimized.

[0030] Step 160: Optimize each frame of source motion data corresponding to the preprocessed reference trajectory frame by frame, obtain the joint angle sequence, and then repair the global height artifact to obtain the robot's redirected motion sequence.

[0031] Specifically, in step 110, a mapping relationship is established between key body parts of the human body and the robot, and weights are configured to establish the correspondence between the source motion data of the human body and the humanoid robot form, providing a basis for subsequent motion retargeting.

[0032] The key body parts constitute a complete set of kinematic chains, including at least: the root node (such as the pelvis or hip) serving as the reference for the overall motion space, the torso, the head, the shoulder, the left / right arm, the left / right hand, the left / right leg, and the left / right foot; each of the key body parts is mapped to a key rigid body in subsequent kinematic calculations; in this invention, the key body part / key rigid body that contributes the most to the overall motion is selected as the constraint object.

[0033] To focus on core motion semantics, a key body part matching mechanism is specifically used to map key body parts and configure their weights, including:

[0034] Input the human source skeleton and the robot skeleton; the human source skeleton includes the source motion data of the human body, which can be obtained from a motion capture system or files in BVH or SMPL format; the robot skeleton includes the morphological data of the robot, which is usually defined and obtained from a robot description file in XML or URDF format.

[0035] User-defined mapping relationships between key body parts of the human and the robot : , It is a key part of the human body. These are the corresponding key body parts of the robot. ; This is the total number of key body parts selected;

[0036] Weighting of pose tracking errors for key body parts: Users can configure weights for the pose tracking errors of selected key body parts, allowing for differentiated treatment of the importance of different body parts during optimization; the pose includes at least orientation and position, therefore the pose tracking error includes at least orientation tracking error and position tracking error; for each pair of mapping relationships ( The weights assigned to the orientation (orientation) tracking error and the position tracking error are respectively denoted as follows: .

[0037] The above mapping relationship and weight configuration information are used for subsequent optimization and solution of the optimization problem under the inverse kinematics (IK) framework.

[0038] Furthermore, in step 120, the source motion data of the human body is aligned to a static pose, and key body parts are non-uniformly scaled locally according to the robot's shape to generate a preprocessed reference trajectory, including:

[0039] Step 121: Align the static pose of the human body's source motion data in Cartesian space to adjust the spatial pose differences between the human body and the robot in static poses, thereby reducing pose mismatch issues in subsequent motion retargeting processes.

[0040] First, the pose of the key rigid bodies of the human body in the original world coordinate system (i.e., the original pose) is obtained from the source motion data of the human body, and the precise mathematical objects required for all subsequent processing steps are extracted. These include:

[0041] For the Key rigid body of the human body (corresponding to the first) (For key body parts of the human body), spatial pose information in the world coordinate system is obtained from the source motion data of motion capture, including position vectors and rotational pose. The rotational pose of the key rigid bodies of the human body is represented in quaternion form as follows: The upper index represents the corresponding number of... A key rigid body for the human body; For rotational scalar components, The rotational vector component can be physically interpreted as the product of the direction vector and the rotation intensity when a rigid body rotates around a certain axis in space, which together describe the rotational state of the key rigid body of the human body in three-dimensional space.

[0042] Based on the quaternions, the corresponding rotation matrix can be constructed:

[0043] ;

[0044] The rotation matrix describes the rotation in the original world coordinate system of motion capture. The orientation of a key rigid body (body part) of an individual. For example, recording whether a person's legs are facing north, east, or any other direction at a given moment.

[0045] At the same time, the The position vector of the key rigid body of an individual is represented as:

[0046] ;

[0047] The position vector describes the position of the first position in the original world coordinate system of motion capture. The spatial coordinates of a key rigid body on a human body. For example, it records whether the human's feet are located at the top left corner of the stage, the center point, or any other arbitrary position.

[0048] Therefore, the key rigid body of the human body is in a special Euclidean group. The original pose can be represented as:

[0049] ;

[0050] in, This represents a 1×3 row vector with all three elements being 0. Represents the transpose of a vector / matrix.

[0051] Next, based on the precise mathematical objects in the original pose, the spatial pose difference between the human body and the robot in the static pose is adjusted by static pose alignment to solve the mismatch between the initial pose and the coordinate system.

[0052] Specifically, the rotation matrix sequence obtained through the aforementioned process is used. and position vector sequence By applying specific alignment transformation operations such as orientation alignment (e.g., multiplying by a calibration rotation matrix), position alignment (e.g., global translation), and artifact mitigation (smoothing filtering), a unified rigid body transformation is performed over time to solve the problem of inconsistency between the human body's orientation and the robot's orientation and position. Only the aligned data can be used to describe the starting point of the reference trajectory that the robot should follow.

[0053] Orientation alignment: by adjusting the rotation matrix sequence Apply a uniform rotation transformation (e.g., multiply by a calibration rotation matrix). This rotates the human body's orientation (overall orientation) to match the human body's reference orientation with the robot's reference orientation (or orientation) when it is stationary. The purpose is to eliminate the orientation difference between the human body and the robot in stationary (T-pose or A-pose) postures (i.e., the problem of inconsistent orientation, for example, the human body facing north and the robot facing east).

[0054] Perform position alignment: After completing the orientation alignment, use the midpoint of the line connecting the two ankle joints of the human body as the human body support reference, and ensure that the direction of the line connecting the two ankle joints of the human body is consistent with the direction of the line connecting the two ankle joints of the robot. Also, align the position of the root node of the human body (usually the pelvis or hip) (corresponding to the position vector). The robot is translated to the preset reference (world coordinate system) origin to achieve unified alignment of the human body and robot in space (unified global coordinate system to solve the position offset problem).

[0055] Artifact mitigation: After completing the orientation and position alignment, artifact mitigation processing is performed on the human motion data. By smoothing and constraining the orientation and height information of key body parts identified by the toes, knees and ankles, artifact problems such as foot slip, ground slip and joint shaking caused by noise, calibration errors or sudden changes in posture are reduced, providing cleaner data for subsequent scaling.

[0056] Step 122: Perform non-uniform local scaling on key body parts according to the robot's shape.

[0057] Since most artifacts are introduced when scaling the source motion, a flexible, non-uniform local scaling procedure is used to handle morphological differences between the human and the robot while avoiding common motion artifacts.

[0058] First, a general scaling factor is calculated based on the height of the human skeleton: ,in, It is the height of the human skeleton. It is the preset reference height when setting the scaling factor.

[0059] For each (the first) Define independent local scaling factors for each key body part. This is to accommodate the proportional differences between different parts of the human body and the robot. The setting of the local scaling factor allows for handling scaling differences between the lower and upper bodies.

[0060] For critical body parts that are not root nodes, the target position in the reference trajectory is calculated using the following formula:

[0061] ;

[0062] in, It is the local scaling factor for key body parts that are not root nodes. Indicates the index of the critical body parts of a non-root node; It is the first The source positions of key body parts of non-root nodes after static orientation alignment; This indicates the source position of the key body parts of the root node after static orientation alignment. This represents the index of the key body part of the root node. It is the local scaling factor for the key body parts of the root node. The target location. This refers to the planned (or desired) body position for the robot within the preprocessed reference trajectory. The source position represents the spatial coordinates of the corresponding key body parts in a unified coordinate system, serving as input for scaling calculations.

[0063] For the key body parts of the root node, the scaling equation for obtaining the target position simplifies to:

[0064] .

[0065] Step 123 involves generating a preprocessed (directly usable for robot motion retargeting) reference trajectory from the source motion data after static orientation alignment and non-uniform local scaling. This specifically includes:

[0066] For each frame of source motion data, obtain the reference pose after static pose alignment and non-uniform local scaling:

[0067] ;

[0068] in, This represents the rotation matrix after alignment at rest.

[0069] The reference poses of all frames are serialized in chronological order, and a series of reference poses corresponding to timestamps are output as a complete reference trajectory.

[0070] Furthermore, in step 130, based on the preprocessed reference trajectory, the attitude tracking error of key body parts is minimized as the original objective function. Under the inverse kinematics framework, a multi-constraint optimization model integrating contact constraints, centroid constraints, and statistical manifold constraints is constructed with joint kinematic limits as the basic constraint.

[0071] The general motion redirection (GMR) basic model is represented as follows:

[0072] ;

[0073] ;

[0074] in, This represents the robot's joint angles at the current moment (a known state variable). Number of joints; The Jacobian matrix representing the tracking task is composed of stacked Jacobian matrices of the key body parts (such as hands and feet) that need to be tracked, mapping joint velocities to end-effector Cartesian velocities. It is the sum of the degrees of freedom of all the key body parts that need to be tracked in their posture; This indicates the joint velocity of the robot at the current moment; This refers to the reference Cartesian velocity vector, derived from a human demonstration or its transformation. , It is the robot's current body position obtained using forward kinematics. and These represent the target position and velocity of the reference trajectory, respectively. It is a positive definite proportional gain matrix; It is the basic feasible set. It is a basic constraint based on joint kinematic limits. and These are the lower limit and upper limit of the joint angle, respectively. To control the time step, the inequality symbol " " is a vector inequality that compares components.

[0075] A multi-constraint optimization model is constructed by integrating contact constraints, centroid constraints, and statistical manifold constraints.

[0076] The contact constraints (hard physics) are used to describe the rigid contact relationship between the robot and the environment in the supported phase, and are mainly used to prevent the robot's feet from slipping or penetrating the ground during contact.

[0077] ;

[0078] in, It is the Jacobian matrix of the rigid body at the foot in Cartesian space. It refers to the number of degrees of freedom of the rigid body motion constraint at the contact point (usually the foot). Or 6; , ;here express The vector with all zeros in the vector. If only the translational motion of the rigid body at the contact point is constrained, then it contains only 3 translational degrees of freedom. If it is necessary to constrain the translational and rotational motions of the rigid body at the contact point, then it includes 3 translational degrees of freedom and 3 rotational degrees of freedom. .

[0079] The aforementioned center of mass constraint (equilibrium dynamics) is used to ensure that the robot maintains overall balance during multi-contact or quasi-static motion, and the mass velocity must follow the stability control law:

[0080] ;

[0081] in, This is the center-of-mass Jacobian matrix, used to describe how joint velocities affect the center-of-mass velocity; This indicates the current position of the robot's center of mass (in world coordinates). Indicates the reference (desired) centroid position; It is a positive definite proportional gain matrix, used to determine "how fast the centroid is pulled back to the reference point"; , .

[0082] The statistical manifold constraint constructs a restored velocity field in joint space by utilizing the natural pose distribution learned from human demonstration data. This continuously pulls the robot's joint motion back to the low-dimensional natural motion manifold defined by the human demonstration, thereby suppressing unnatural, unexecutable, or numerically unstable pose deviations. Specifically, it is given by the following equation:

[0083] ;

[0084] in, It is the identity matrix; The defined natural recovery rate, The joint angles in the human body demonstration follow a multivariate Gaussian distribution. , It is the expected value. It is the covariance matrix; It is the statistical manifold restoration rate gain, used to adjust how quickly the system returns to its natural attitude manifold, that is... It determines how much force is needed to pull the body back when its posture deviates from its natural posture; , .

[0085] Express the above three constraints in a unified constraint form:

[0086] ;

[0087] in, ; .

[0088] Combining the basic model of general motion redirection with the three constraints mentioned above, which integrate contact constraints, centroid constraints, and statistical manifold constraints, we obtain a multi-constraint optimization model:

[0089] ;

[0090] ;

[0091] .

[0092] Specifically, in step 140, to address the infeasibility issues that may arise from overly tight constraints in practice, an optimizable violation quantity is introduced to relax the multi-constraint optimization model, resulting in a robust optimization model that allows for minor constraint violations, including:

[0093] In real robots, the basic feasible set The constraint subspace with respect to these three constraints (contact constraint, centroid constraint, and statistical manifold constraint) { There may be cases where there is no overlap, so we introduce an optimizable (jointly optimized with the main objective function) violation quantity, which allows for slight violations of constraints but is clearly controllable.

[0094] Definition of violation quantity:

[0095] ;

[0096] in, This indicates a breach of the contact constraint (allowing minimal foot slip / lifting). This indicates a breach of the centroid constraint (allowing a temporary deviation from the most stable point). This indicates a violation of the statistical manifold constraint (allowing for transient unnaturalness).

[0097] Using violation quantities to transform the original constraints Relaxing feasible offset constraints:

[0098] ;

[0099] The original attitude tracking error optimization problem is then transformed into the following robust optimization model:

[0100] ;

[0101] ;

[0102] ;

[0103] in, This indicates a violation of the cost function. It is a diagonal weight matrix (symmetric positive definite or at least semi-positive definite).

[0104] The robust optimization model described above is a quadratic programming problem, which can be solved using standard quadratic programming (QP) solvers (such as OSQP, qpOASES, or MOSEK) to obtain the optimal joint velocities. While the aforementioned standard QP solvers are powerful and robust, they are not specifically optimized for robot inverse kinematics (IK) problems, particularly for velocity-level, real-time scenarios with special kinematic constraints. This invention addresses the mathematical structure of robot velocity-level IK problems (e.g., the specific sparsity of the Jacobian matrix, the construction method of the weight matrix), real-time requirements, and robot-specific constraints (e.g., joint limits, kinematic chains). It employs a specially customized and encapsulated QP solver for solving velocity-level inverse kinematics problems: the Mink Velocity-Level IK Solver (hereinafter referred to as the Mink Solver). This solver is deeply integrated with a two-stage inverse kinematics solution strategy to efficiently and robustly solve the aforementioned robust optimization model.

[0105] Furthermore, in step 150, the Augmented Lagrangian Method (ALM) framework is used in conjunction with the Mink solver to solve the robust optimization model through a two-stage strategy to obtain the optimal joint velocity: in the first stage, the weight of the spinal joint is strengthened in the objective function to prioritize the optimization of the pose of the end effector (left hand / right hand / left foot / right foot in the key body parts); in the second stage, based on the pose results of the end effector in the first stage, the pose of all key body parts is optimized.

[0106] To efficiently solve the robust optimization model with linear equality constraints and slack variable optimization structure, this invention uses the Enhanced Lagrange Method (ALM) as the core solution framework. ALM transforms the original constrained problem into a series of easier-to-solve unconstrained or simple constrained subproblems by introducing Lagrange multipliers and applying secondary penalties to violations, and gradually converges to the optimal solution through iteration.

[0107] Step 151: Solve the robust optimization model using the enhanced Lagrange method (ALM) combined with the Mink solver;

[0108] The augmented Lagrangian function for constructing a robust optimization model:

[0109] ;

[0110] It is a Lagrange multiplier vector, with vector dimension and offset constraints. same; , Represents the dot product of vectors; This typically represents the penalty parameter, which controls the strength of the secondary penalty for constraint violation. ,make sure reversible; This represents the L2 norm.

[0111] Perform iterative solving, where the... The round-by-round iterative solution process includes:

[0112] (1) Fixed and Update violation quantity :

[0113] ;

[0114] (2) Fixed and Update the Lagrange multiplier vector :

[0115] ;

[0116] (3) Fixed and Update joint speed :

[0117] Will and Substituting the augmented Lagrange function Ignoring the constant term, The update problem is equivalent to solving the following subproblems of strictly convex quadratic programming (QP):

[0118] ;

[0119] Solve the subproblems of the strictly convex quadratic programming (QP) problem using the Mink solver to obtain... .

[0120] The Mink solver utilizes the sparsity of the Jacobian matrix in the subproblem, employs an interior-point method based on sparse matrix decomposition for solving the problem, and uses a hot-start technique to accelerate the iteration process, thereby meeting the real-time requirements of robot control.

[0121] The iteration termination condition of ALM: when the change in the objective function value of the original robust optimization model... Less than the preset convergence threshold or number of iterations Reaching the preset maximum number of iterations The iteration terminates when the time is reached.

[0122] In one embodiment, the convergence threshold Take 0.001, or the maximum number of iterations. It is 10 times.

[0123] Step 152: A two-stage inverse kinematics solution strategy is adopted to adjust and optimize the numerical solution.

[0124] Considering the two-stage inverse kinematics solution strategy adopted by the GMR method in humanoid robot repositioning, the following problems arise: Due to the differences in kinematic structure (such as leg length ratio and spinal degrees of freedom) between the human body and the robot, when the first stage prioritizes forcing the matching of the end effector posture (such as squatting or taking a large step), the solver tends to use the redundant degrees of freedom of the robot's torso (such as the spine / lumbar region) to compensate for the height difference. As a result, the spinal joints usually lack sufficient posture maintenance constraints. This leads to the robot being prone to unnatural postures such as "collapse", "bending" or "curling" during the solution process in the first or second stage. Moreover, once collapse occurs in the first stage, it is often difficult to correct in the second stage.

[0125] Therefore, within the solution framework of the Enhanced Lagrange Method (ALM), this invention employs a two-stage inverse kinematics solution strategy to solve the robust optimization model, optimizing the robot posture in stages and with weights, specifically including:

[0126] In the first phase, the weight of the spinal joints is enhanced to prioritize the optimization of the end effector's posture.

[0127] With the primary goal of optimizing the posture of only a few end effectors that are critical to the task (such as the hands and feet), specifically, making the position and orientation of the robot's end effectors as close as possible to the corresponding key body parts in the human demonstration, thereby ensuring that the intentions of grasping, supporting, walking, and other operations are correctly executed, the following optimization model with enhanced spinal joint weights is established:

[0128] + ;

[0129] ;

[0130] ;

[0131] in, , This represents the attitude error vector of the end effector. The number of end effectors (e.g., 4 in total, for both hands and feet); The Jacobian matrix of the end effector; the diagonal weight matrix. The first stage of direction tracking error weights and position tracking error weights Construct, in the diagonal weight matrix In this process, the weight elements corresponding to the rows / columns related to spinal joint movement are significantly enhanced, thereby strongly constraining trunk posture during optimization and preventing unnatural lumbar collapse. Indicated by The diagonal elements are the L2 norms of the weighted coefficients; Indicates the time step (not necessarily the frame interval).

[0132] The above optimization model is solved iteratively using the ALM framework provided in step 151. Similarly, its augmented Lagrangian function is given. The problem involves solving the corresponding strictly convex quadratic programming (QP) subproblems, and then iteratively solving them using the Mink solver until the termination condition is met, thus obtaining the optimal joint velocity of the first-stage end effector. and the joint angles after integration .

[0133] The above process utilizes a Mink solver based on IK differences to solve for the generalized velocity. Instead of direct optimization This allows the robot to minimize the error in its target posture after the next movement.

[0134] In the second stage, based on the attitude results of the end effector in the first stage, the attitude of all key body parts is optimized.

[0135] Based on the feasible posture solution of the end effector obtained in the first stage, the second stage further introduces constraints on key body parts, including the torso, shoulders, and pelvis. By minimizing the weighted posture error of the whole body, the robot's overall motion becomes closer to a human demonstration in both visual and statistical terms.

[0136] The pose results obtained in the first stage are extended to all key body parts (torso, head, legs, feet, arms, hands, etc., not just end effectors), using different weight sets. Make fine adjustments. These are the orientation tracking error weights and position tracking error weights for the second stage, and similarly, the optimized model for the poses of all key body parts is obtained:

[0137] ;

[0138] ;

[0139] ;

[0140] in, This represents the posture error vector of key body parts, used to encode the position and orientation tracking errors of all key body parts relative to the human model target; The Jacobian matrix represents all key body parts; the diagonal weight matrix represents the weights. The second stage of direction tracking error weights and position tracking error weights The construction typically involves reducing the weight of the end effector and increasing the weight of other key body parts that are not at the end effector to achieve coordinated optimization of the whole body posture; Indicated by The diagonal elements are the L2 norm of the weighted coefficients.

[0141] Based on the ALM framework provided in step 151, the optimization model for the poses of all the key body parts mentioned above is iteratively solved. Similarly, its augmented Lagrangian function is given. The problem involves solving the corresponding strict convex quadratic programming (QP) subproblems, and then iteratively solving them using the Mink solver until the termination condition is met, thus obtaining the optimal joint velocities for all key body parts in the second stage. and the joint angles after integration .

[0142] The second stage uses the same constraints and augmented Lagrangian solution framework as the first stage, only updating the Jacobian matrix and weight configuration in the objective function. It iteratively solves for the joint velocities and updates the constraint offsets. With Lagrange multipliers This enables fine-tuning and optimization of the robot's overall posture.

[0143] To address the posture distortion problem caused by the difference in kinematic structure between humans and robots, this invention proposes a non-uniform joint weight optimization strategy in step 152 above. In the first stage of solution, a significant strong weight matrix is ​​used... The weights for the mid-spine joints prioritize matching the end effector's pose while strongly constraining the torso pose. Although the end-effector tracking task attempts to forcibly pull the robot's body to match the target point, the solver is strongly constrained, limiting excessive torso bending. The system will output an intermediate joint angle with the torso upright and only the limbs undergoing significant deformation. This effectively prevents "collapse" from occurring in the first stage. In the second stage of the solution, the normal weights of the spinal joints are restored, and the weights are reallocated to balance the optimization objectives of the end effector and other key body parts (such as the torso and pelvis). Additional contact constraints and centroid constraints can be introduced to ensure the naturalness and physical feasibility of the final posture. By combining the above two-stage inverse kinematics solution strategy with dynamic weight adjustment, this invention can effectively generate coordinated, natural, and stable robot motion while ensuring the accuracy of the end effector task.

[0144] Finally, in step 160, the source motion data corresponding to each frame of the preprocessed reference trajectory is optimized frame by frame. After obtaining the joint angle sequence, global height artifacts are repaired to obtain the robot's retargeted motion sequence. Specifically, this includes:

[0145] The optimization process in step 150 is applied to each frame of source motion data corresponding to the preprocessed reference trajectory. The first frame uses the robot's stationary pose aligned in step 120 as the initial value for the optimization iteration process; for subsequent frames... frame, , using the Frame redirection results (joint angles) () as the initial value for the optimization solution iterative process To improve solution efficiency and continuity;

[0146] After completing motion retargeting for all frames, a complete sequence of robot joint angles is obtained. The robot's overall height in each frame is calculated using forward kinematics. The minimum height difference between the robot and the ground (the height of the plane where the feet are located or other ground reference heights) in each frame is subtracted from the robot's global translation to eliminate possible overall floating or ground penetration artifacts and ensure that the generated robot motion sequence makes correct contact with the ground.

[0147] Furthermore, the effectiveness of the proposed robot motion reversal method based on two-stage optimization and multi-constraint fusion is verified through experiments.

[0148] (1) Dataset preparation

[0149] The dataset used in the experiment was the Ubisoft La Forge Animation Dataset ("LAFAN1"), which contained 5 subjects, 77 sequences, and 496,672 motion frames (30fps, approximately 4.6 hours). Each BVH file was named according to the following naming convention: [Subject][Shot Number]_[Subject ID].bvh. Sequences with the same subject and shot number were all recorded in the studio at the same time.

[0150] Themes are general descriptions of actions in the source motion data sequences. There are 13 themes in total, covering different action types and scenarios. The "Obstacles" theme contains 17 sequences describing movement on uneven terrain. The "Walking" theme has 12 sequences covering different styles of walking. The "Dance" theme contains 8 sequences referring to free dance movements. The "Fall and Get Up" theme has 6 sequences describing the action of getting up after falling. The "Aiming" theme has 5 sequences involving movement when handling or aiming a weapon. The "Ground" theme has 5 sequences describing movement when crawling and crouching. The "Multiple Actions" theme contains 4 sequences, each containing multiple different actions. The "Running" theme has 4 sequences involving jogging or running. The "Combat" theme has 3 sequences containing various combat actions. The "Jumping" theme also has 3 sequences involving single-leg and double-leg jumps. The "Combat and Movement" theme has 2 sequences involving combat and movement actions. The themes “pushing and tripping,” “push down,” and “sprinting” contain 3, 2, and 2 sequences respectively, describing actions of pushing, falling and getting up, and sprinting. Finally, the theme “push” contains 1 sequence, referring to the action of pushing an opponent.

[0151] (2) Experiment and verification

[0152] Experiments were conducted on 77 action sequences in the dataset, and the Old Jerk (672.2116) value of all samples was compared with that of the redirection method optimized by the existing two-stage inverse kinematics solution strategy (without using ALM and Mink solvers). It was found that after introducing the new method provided by this invention, the New Jerk (565.4825) value was significantly reduced, indicating that the new redirection method provided by this invention is smoother and has higher motion stability. The Jerk (jerk acceleration) value is used to directly measure the higher-order smoothness of joint motion; the lower the value, the smoother and more stable the motion.

[0153] The motion smoothness index Improvement (%) represents the percentage improvement of New Jerk compared to Old Jerk. The specific formula is as follows:

[0154] ;

[0155] Therefore, the improvement of the new method provided by this invention is approximately 15.88%.

[0156] The following three motion fidelity metrics were used as evaluation indicators for experimental verification:

[0157] G-MPBPE (Global Marker Position / Rotation Error): Measures the deviation of a robot's end effector or full-body keypoints from a reference motion in the world coordinate system, measured in mm / °. A smaller value indicates that the robot's trajectory "looks more human-like" in global space, making it the most intuitive fidelity indicator for viewers or operators.

[0158] L-MPBPE (Local Marker Position / Rotation Error): This metric represents the position / rotation error of local marker points, as well as the position / rotation error of global marker points. It is a relative posture error calculated with the robot's root node as the reference, excluding the effects of overall robot drift, slippage, or ground slippage, and only evaluating the motion accuracy of "limbs relative to the torso." This metric directly reflects the local kinematics reconstruction capability and is most sensitive to assessing the quality of joint space mapping.

[0159] Rot (Joint Rotation Error): Joint rotation error, the root mean square error (rad) of each joint angle relative to the reference orientation, focusing on rotational deviation within joint space. Lower Rot means smoother motor commands, less energy consumption and mechanical shock, while reducing the risk of strategy instability due to "large angle abrupt changes".

[0160] (3) Experimental results

[0161] The specific comparative experimental results are shown in Table 1 below.

[0162] Table 1. Comparison of experimental results between the original method before improvement and the new method provided by this invention.

[0163]

[0164] Using the sequence fightAndSports1_subject1, a segment of jumping motion from 2 minutes 50 seconds to 2 minutes 52 seconds was extracted for motion simulation visualization. For clear comparison, keyframe images were extracted at a rate of 5 frames per second. Figure 2 and Figure 3 The diagrams show a comparison of keyframes in the simulation results of the existing method and the method of this invention at the same time point. For example... Figure 2 As shown, when using existing techniques, the robot exhibits significant trunk (spine) collapse during the jump-and-squat phase. In contrast, from... Figure 3 As can be seen, after optimization by the method of the present invention, the robot's torso posture remains upright, and the problem of spinal collapse is significantly improved.

[0165] In summary, the robot motion retargeting method based on two-stage optimization and multi-constraint fusion provided by this invention comprehensively improves the quality, naturalness, stability, and robustness of robot motion retargeting through its innovative constraint model and optimization architecture. It provides an effective solution for generating efficient and reliable motion trajectories that can be directly used for actual robot execution, and solves the kinematic artifacts and quality problems existing in the prior art. Specific beneficial effects include:

[0166] (1) This invention fundamentally improves the smoothness and stability of redirected motion: by introducing statistical manifold constraints to guide posture generation and using a two-stage optimization strategy to coordinate different objectives, the rate of change of acceleration (NewJerk) of the generated motion is reduced by about 15.88% compared with the original method. This improvement directly means that the robot's motion is smoother and more natural, significantly reducing jitter and abrupt changes during the motion process, thereby reducing mechanical shock and energy consumption.

[0167] (2) This invention effectively suppresses typical kinematic artifacts such as foot slippage, ground crossing, and joint abrupt changes: the constructed contact constraint and centroid constraint models, combined with the optimized solution process, ensure the stable position of end effectors such as the feet during the motion cycle, while maintaining the overall dynamic balance of the robot. The most convincing experimental data is reflected in the joint rotation error (Rot), which has been significantly reduced from 2.4332 rad to 0.1081 rad. This order-of-magnitude improvement indicates that the generated joint angle sequence is extremely smooth and close to the reference posture, avoiding unnatural joint angle jumps from the source and providing high-quality, low-abrupt instruction input for the underlying control.

[0168] (3) The present invention achieves a good balance in ensuring motion fidelity: Experimental data shows that while successfully integrating multiple physical constraints, both global trajectory error (G-MPBPE) and local kinematic error (L-MPBPE) are further reduced. This fully demonstrates that the new method proposed in this invention not only optimizes the feasibility and robustness of motion at the physical level, but also maintains or even improves the visual similarity of motion to the reference action, achieving a unity of motion "similarity" and "physical rationality".

[0169] In one embodiment, the present invention provides a robot motion redirection device based on two-stage optimization and multi-constraint fusion, the device comprising:

[0170] The first module is used to establish the mapping relationship between key body parts of the human body and the robot, and to configure the weights of the posture tracking errors of key body parts.

[0171] The second module is used to perform static posture alignment on the source motion data of the human body, and to perform non-uniform local scaling on key body parts according to the robot's shape to generate a pre-processed reference trajectory.

[0172] The third module is used to minimize the attitude tracking error of the end effector as the original objective function based on the preprocessed reference trajectory. Under the inverse kinematics framework, with joint kinematic constraints as the basic constraint, a multi-constraint optimization model is constructed that integrates contact constraints, centroid constraints, and statistical manifold constraints.

[0173] The fourth module is used to introduce optimizable violation quantities to relax the multi-constraint optimization model, thereby obtaining a robust optimization model that allows for minor constraint violations.

[0174] The fifth module is used to solve the robust optimization model using the enhanced Lagrangian method combined with the Mink solver, and adopts a two-stage inverse kinematics solution strategy during the solution process: in the first stage, the weights of the spinal joints are strengthened to prioritize the optimization of the end effector's posture; in the second stage, based on the posture results of the end effector in the first stage, the posture of all key body parts is optimized.

[0175] The sixth module is used to optimize each frame of source motion data corresponding to the preprocessed reference trajectory frame by frame, obtain the joint angle sequence, repair the global height artifact, and obtain the motion sequence for robot retargeting.

[0176] On the other hand, the present invention provides a computer device including a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the steps of the robot motion reversal method based on two-stage optimization and multi-constraint fusion provided in any of the above embodiments. The computer device can be a server. The computer device includes a processor, a memory, a network interface, and a database connected via a system bus. The processor of the computer device provides computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and internal memory. The non-volatile storage medium stores an operating system, a computer program, and a database. The internal memory provides an environment for the operation of the operating system and computer program in the non-volatile storage medium. The database of the computer device stores sample data. The network interface of the computer device is used for communication with external terminals via a network connection.

[0177] On the other hand, the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein when the computer program is executed by a processor, it implements the steps of the robot motion reversal method based on two-stage optimization and multi-constraint fusion provided in any of the above embodiments.

[0178] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0179] Matters not covered in this invention are common knowledge. The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as the combination of these technical features does not contradict each other, it should be considered to be within the scope of this specification.

[0180] The embodiments described above are merely examples of several implementations of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention. 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 modifications and improvements all fall within the scope of protection of the present invention.

Claims

1. A robot motion redirection method based on two-stage optimization and multi-constraint fusion, characterized in that, include: Step 110: Establish the mapping relationship between key body parts of the human body and the robot, and configure the weights of the pose tracking errors of key body parts. Step 120: Perform static pose alignment on the source motion data of the human body, and perform non-uniform local scaling on key body parts according to the robot's shape to generate a preprocessed reference trajectory. Step 130: Based on the preprocessed reference trajectory, minimize the attitude tracking error of the end effector as the original objective function. Under the inverse kinematics framework, and with joint kinematic constraints as the basic constraint, construct a multi-constraint optimization model that integrates contact constraints, centroid constraints, and statistical manifold constraints. The multi-constraint optimization model is given by the following equation: ； ； ； in, This represents the joint angles of the robot at the current moment. Number of joints; The Jacobian matrix represents the tracking task. It is the sum of the degrees of freedom of all the key body parts that need to be tracked in their posture; This represents the robot's joint velocity at the current moment; Indicates the reference Cartesian velocity vector; It is a unified form of three-constraint conditions, namely contact constraints, centroid constraints, and statistical manifold constraints; It is the basic feasible set. It is a basic constraint based on joint kinematic limits. and These are the lower limit and upper limit of the joint angle, respectively. To control the time step, the inequality symbols are... , is a vector inequality that compares components; The contact constraint, used to describe the rigid contact relationship between the robot and the environment under the support phase, is given by the following equation: ； in, It is the Jacobian matrix of the rigid body at the foot in Cartesian space. It is the number of degrees of freedom of the rigid body motion constraint at the contact point; , ; express A vector of all zeros; The centroid constraint, used to ensure the robot maintains overall balance during multi-contact or quasi-static motion, is given by the following formula: ； In the above formula, This is the center-of-mass Jacobian matrix, used to describe how joint velocities affect the center-of-mass velocity; This indicates the position of the robot's center of mass in the world coordinate system at the current moment; Indicates the location of the reference centroid; It is a positive definite proportional gain matrix; , ; The statistical manifold constraint, through the learned natural pose distribution, constructs a restored velocity field in the joint space, so that the robot's joint motion is continuously pulled back to the low-dimensional natural motion manifold defined by the human demonstration, as given by the following equation: ； In the above formula, It is the identity matrix; The defined natural recovery rate; , ; Using the definitions of contact constraints, centroid constraints, and statistical manifold constraints, determine the three constraint conditions. In and : ； ； Step 140: Introduce optimizable violation quantities to relax the multi-constraint optimization model, obtaining a robust optimization model that allows for minor constraint violations; including: An optimizable violation quantity is introduced, given by the following equation: ； in, This indicates a breach of the release constraint; This indicates a breach of the centroid constraint. This indicates a violation of statistical manifold constraints; Using the violation amount, the three constraints are relaxed into the following offset constraints: ； A robust optimization model is obtained using the aforementioned offset constraints: ； ； ； in, This indicates a violation of the cost function. It is a diagonal weight matrix. Indicates the transpose of a vector / matrix; Step 150: The robust optimization model is solved using the enhanced Lagrangian method combined with the Mink solver, and a two-stage inverse kinematics solution strategy is adopted during the solution process: In the first stage, the weights of the spinal joints are strengthened to prioritize the optimization of the end effector's posture; in the second stage, based on the posture results of the end effector in the first stage, the postures of all key body parts are optimized, including: Step 151: Solve the robust optimization model using the enhanced Lagrangian method combined with the Mink solver: Constructing the augmented Lagrangian function: ； in, It is a Lagrange multiplier vector, with dimension and offset constraints. same; , Represents the dot product of vectors; It is a penalty parameter used to control the strength of the secondary penalty for constraint violation. ,make sure reversible; Represents the L2 norm; Perform iterative solving, the... The round-by-round iterative solution process includes: fixed and Update violation quantity : ； fixed and Update the Lagrange multiplier vector : ； fixed and Update joint speed : Will and Substituting the augmented Lagrange function Ignoring the constant term, The update problem is equivalent to solving the following subproblems of strictly convex quadratic programming: ； Solve the above strictly convex quadratic programming subproblems using the Mink solver to obtain... ; The Mink solver utilizes the sparsity of the Jacobian matrix in the subproblem, employs an interior-point method based on sparse matrix decomposition for solving the problem, and uses a hot-start technique to accelerate the iteration process. The termination condition for the loop iteration includes: the change in the objective function value of the robust optimization model. Less than the preset convergence threshold or number of iterations Reaching the preset maximum number of iterations ; Step 152: A two-stage inverse kinematics solution strategy is adopted to adjust and optimize the numerical solution; Step 160: Optimize each frame of source motion data corresponding to the preprocessed reference trajectory frame by frame, obtain the joint angle sequence, and then repair the global height artifact to obtain the robot's redirected motion sequence.

2. The robot motion redirection method based on two-stage optimization and multi-constraint fusion according to claim 1, characterized in that, Step 110 includes: Input human source skeleton and robot skeleton; the human source skeleton includes human source motion data; the robot skeleton includes robot morphological data; Define the mapping relationship between key body parts of the human body and the robot. : , It is a key part of the human body. These are the corresponding key body parts of the robot. ; This is the total number of key body parts selected; For each pair of key body parts in the mapping relationship Key body parts of the robot Configure the direction tracking error weights respectively. and position tracking error weights .

3. The robot motion redirection method based on two-stage optimization and multi-constraint fusion according to claim 2, characterized in that, The key body parts include at least: the root node serving as a spatial reference, the torso, the head, the shoulders, the left / right arm, the left / right hand, the left / right leg, and the left / right foot; the root node includes at least the pelvis or the hip. Each of the key body parts is mapped to a key rigid body in subsequent kinematic calculations.

4. The robot motion redirection method based on two-stage optimization and multi-constraint fusion according to claim 2, characterized in that, In step 120, the static posture alignment of the source motion data of the human body includes: The pose of each key rigid body of the human body in the original world coordinate system is obtained from the source motion data of the human body, including the rotation matrix, position vector, and the original pose composed of the rotation matrix and position vector; the rotation matrix sequence and position vector sequence of all key rigid bodies of the human body are obtained. Orientation alignment: By applying a uniform rotation transformation to the rotation matrix sequence, the body orientation of the human body is rotated so that the reference orientation of the human body matches the reference orientation of the robot when it is in a static posture. Position alignment: Using the midpoint of the line connecting the two ankle joints of the human body as the human body support reference, the direction of the line connecting the two ankle joints of the human body is kept consistent with the direction of the line connecting the two ankle joints of the robot, and the root node position of the human body is translated to the preset reference origin to achieve unified alignment of the human body and the robot in spatial position. Artifact mitigation: By smoothing and constraining the orientation and height information of key body parts identified by the toes, knees, and ankles, artifacts caused by noise, calibration errors, or sudden changes in posture are reduced, such as foot slippage, ground slippage, and joint shaking.

5. The robot motion redirection method based on two-stage optimization and multi-constraint fusion according to claim 4, characterized in that, In step 120, the non-uniform local scaling of key body parts according to the robot's shape includes: A general scaling factor is calculated based on the height of the human skeleton: ,in, It is the height of the human skeleton. This is the preset reference height; For the first Define independent local scaling factors for each key body part. ; For critical body parts that are not root nodes, the target position in the reference trajectory is calculated using the following formula: ； in It is the local scaling factor for key body parts that are not root nodes. Indicates the index of the critical body parts of a non-root node; It is the first The source positions of key body parts of non-root nodes after static orientation alignment; This indicates the source position of the key body parts of the root node after static orientation alignment. This represents the index of the key body part of the root node. It is the local scaling factor for the key body parts of the root node; For the key body parts of the root node, the scaling equation for obtaining the target position simplifies to: ； The generation of the preprocessed reference trajectory includes: For each frame of source motion data, obtain the reference pose after static pose alignment and non-uniform local scaling of the original pose; The reference poses of all frames are serialized in chronological order, and a series of reference poses corresponding to timestamps are output as a complete reference trajectory.

6. The robot motion redirection method based on two-stage optimization and multi-constraint fusion according to claim 5, characterized in that, In step 152, a two-stage inverse kinematics solution strategy is used to adjust and optimize the numerical solution, including: The first phase involves strengthening the weights of the spinal joints to prioritize optimizing the posture of the end effector, including: Obtain the optimized model after strengthening the spinal joint weights: + ； ； ； in, , This represents the attitude error vector of the end effector. The number of end effectors includes at least a left hand, a right hand, a left foot, and a right foot; The Jacobian matrix of the end effector; the diagonal weight matrix. The first stage of direction tracking error weights and position tracking error weights Build; Indicated by The diagonal elements are the L2 norms of the weighted coefficients; The optimized model with enhanced spinal joint weights, obtained by using the enhanced Lagrangian method in step 151 combined with the Mink solver, is solved to obtain the optimal joint velocity of the first-stage end effector. and the integrated joint angle vector ; In the second stage, based on the end effector's pose results from the first stage, the poses of all key body parts are optimized, including: The pose results obtained in the first stage are extended to all key body parts, using... Make fine adjustments. These are the orientation tracking error weights and position tracking error weights for the second stage, respectively, to obtain an optimized model of the pose of all key body parts: ； ； ； in, , Represents the posture error vector of key body parts; The Jacobian matrix represents all key body parts; the diagonal weight matrix represents the weights. The second stage of direction tracking error weights and position tracking error weights The design reduces the weight of the end effector and increases the weight of other key body parts that are not at the end effector. Indicated by The diagonal elements are the L2 norms of the weighted coefficients; The enhanced Lagrangian method in step 151, combined with the Mink solver, is used to solve the optimization model of the pose of all key body parts, thereby obtaining the optimal joint velocities of all key body parts in the second stage. and the integrated joint angle vector .

7. The robot motion redirection method based on two-stage optimization and multi-constraint fusion according to claim 6, characterized in that, Step 160 includes: Step 150 optimizes the source motion data corresponding to each frame of the preprocessed reference trajectory. The first frame uses the robot's stationary pose after static pose alignment as the initial value for the optimization iteration process; for subsequent frames... frame, , using the The frame redirection result is used as the initial value for the optimization solution iteration process; The robot's overall height in each frame is calculated using positive kinematics, and the minimum height difference between the robot and the ground in each frame is subtracted from the robot's global translation to eliminate overall levitation or ground penetration artifacts.

Images