Search

CN-121989261-A - Robot motion redirection method based on two-stage optimization and multi-constraint fusion

CN121989261ACN 121989261 ACN121989261 ACN 121989261ACN-121989261-A

Abstract

The invention relates to the technical field of robot motion control, and provides a robot motion redirection method based on two-stage optimization and multi-constraint fusion, aiming at the problems of kinematic artifacts, quality and robustness in the prior art, wherein the mapping relation between a human body source framework and a robot framework is established through key body part matching; the method comprises the steps of adjusting initial gestures by means of static gesture alignment, adapting to morphological differences between a human body and a robot through non-uniform local scaling, constructing a multi-constraint optimization model, processing constraint conflicts according to a minimum violation amount principle, designing a two-stage inverse kinematics solving strategy, enhancing the spinal joint weight, optimizing the end effector gestures preferentially, relaxing the end weight, optimizing all key body part gestures, fusing three constraints, and solving by means of an augmentation Lagrangian method. The invention effectively solves the problems of gesture collapse, foot slippage, imbalance, unnatural movement and other artifacts in the movement redirection, and improves the quality and robustness of the redirection movement.

Inventors

  • XIE ZHENG
  • ZENG BINGYAO
  • DUAN XIAOJUN
  • XIAO GUILONG

Assignees

  • 开元国际数学研究院
  • 中国人民解放军国防科技大学

Dates

Publication Date
20260508
Application Date
20260408

Claims (10)

  1. 1. The robot motion redirection method based on two-stage optimization and multi-constraint fusion is characterized by comprising the following steps of: step 110, establishing a mapping relation of a key body part between a human body and a robot, and carrying out weight configuration of a key body part posture tracking error; Step 120, performing static gesture alignment on source motion data of a human body, and performing non-uniform local scaling on a key body part according to a robot shape to generate a preprocessed reference track; Step 130, based on the preprocessed reference track, minimizing an attitude tracking error of an end effector as an original objective function, and under an inverse kinematics framework, constructing a multi-constraint optimization model fusing contact constraint, centroid constraint and statistical manifold constraint by taking joint kinematics limit as a base constraint; Step 140, introducing an optimizable violation amount, and performing relaxation treatment on the multi-constraint optimization model to obtain a robust optimization model allowing slight constraint violation; Step 150, solving the robust optimization model by adopting an enhanced Lagrangian method and a Mink solver, and adopting a two-stage inverse kinematics solving strategy in the solving process, wherein the first stage is used for strengthening the weight of the spinal joint to preferentially optimize the posture of the end effector; and 160, optimizing each frame of source motion data corresponding to the preprocessed reference track frame by frame, repairing global height artifacts after obtaining a joint angle sequence, and obtaining a robot redirected motion sequence.
  2. 2. The method for redirecting robot motion based on two-stage optimization and multi-constraint fusion of claim 1, wherein said step 110 comprises: Inputting a human body source framework and a robot framework, wherein the human body source framework comprises source motion data of a human body, and the robot framework comprises form data of a robot; Defining a mapping relationship of key body parts between a human body and a robot : , Is a key body part of a human body, For the corresponding critical body parts of the robot, ; Is the total number of selected key body parts; for each pair of key body parts of the human body in the mapping relationship Critical body parts to robot Respectively configuring direction tracking error weights And position tracking error weight 。
  3. 3. The method for redirecting robot motion based on two-stage optimization and multi-constraint fusion of claim 2 wherein the key body parts comprise at least root nodes, trunk, head, shoulder, left/right arm, left/right hand, left/right leg, left/right foot as spatial references; each of the key body parts is mapped to a key rigid body in a subsequent kinematic calculation.
  4. 4. The method for redirecting motion of a robot based on two-stage optimization and multi-constraint fusion of claim 2, wherein in step 120, the performing static pose alignment on source motion data of a human body comprises: Acquiring the posture of each human body key rigid body under an original world coordinate system from source motion data of a human body, wherein the posture comprises a rotation matrix, a position vector and an original posture formed by the rotation matrix and the position vector; The direction alignment is that a unified rotation transformation is applied to the rotation torque array sequence to rotate the body direction of the human body, so that the reference direction of the human body is matched with the reference direction of the robot in the static posture; The method comprises the steps of performing position alignment, namely taking the middle point of a human body double ankle joint connecting line as a human body support reference, enabling the direction of the human body double ankle joint connecting line to be consistent with that of a robot double ankle joint connecting line, and translating the root node position of a human body to a preset reference origin to realize unified alignment of the human body and the robot in space position; Artifact alleviation, namely, reducing the artifact problems of foot slip, ground penetrating and joint shaking caused by noise, calibration errors or abrupt change of posture by carrying out smooth and constraint correction on the direction and height information of key body parts determined by the tiptoes, the knee joints and the ankle joints.
  5. 5. The method for redirecting robot motion based on two-stage optimization and multi-constraint fusion of claim 4 wherein in step 120 the non-uniform local scaling of critical body parts based on robot morphology comprises: A general scaling factor is calculated based on the height of the human source bone: , wherein, Is the height of the bone of the human body source, Is a preset reference height; Is the first Individual critical body part definition independent local scaling factors ; For key body parts that are not root nodes, the target position in the reference trajectory is calculated by the following formula: ; Wherein the method comprises the steps of Is a local scaling factor for critical body parts other than the root node, A key body part index representing non-root nodes; Is the first Source positions of key body parts of the non-root nodes after static posture alignment; representing the source position of the critical body part of the root node after the stationary pose alignment, A key body part index representing the root node, A local scaling factor of a key body part that is a root node; for key body parts of the root node, the scaling equation for acquiring the target position is simplified as: ; The generating the preprocessed reference track comprises the following steps: For each frame of source motion data, acquiring a reference posture of an original posture after static posture alignment and non-uniform local scaling; The reference poses of all frames are serialized in time sequence, and a series of reference pose sets corresponding to the time stamps are output as complete reference tracks.
  6. 6. The method for redirecting robot motion based on two-phase optimization and multi-constraint fusion of claim 5 wherein in step 130 the multi-constraint optimization model is given by: ; ; ; Wherein, the Indicating the joint angle of the robot at the current moment, Is the number of joints; representing the tracking task jacobian matrix, Is the sum of all the degrees of freedom of interest in its pose for the key body part that needs to be tracked; The joint speed of the robot at the current moment is represented; representing a reference cartesian velocity vector; three constraint conditions in a unified form of contact constraint, centroid constraint and statistical manifold constraint; Is a basic set of possible implementations, Is based on basic constraints of joint kinematics limitation, And Respectively a lower limit of the joint angle and an upper limit of the joint angle, To control the time step, the inequality sign "therein" "Is a vector inequality by component comparison; The contact constraint, which describes the rigid contact relationship between the robot and the environment in the support phase, is given by: ; Wherein, the Is a jacobian matrix of the rigid body of the foot in the Cartesian space, Is the number of degrees of freedom of the rigid motion constraint of the contact point; , ; Representation of All zero vectors in (a); The centroid constraint is used for ensuring that the robot keeps overall balance in the multi-contact or quasi-static motion process, and is given by the following formula: ; In the above-mentioned method, the step of, A centroid jacobian matrix for describing how joint velocity affects centroid velocity; representing the centroid position of the robot in the world coordinate system at the current moment; Representing a reference centroid position; Is a positive scaling gain matrix; , ; the statistical manifold constraint constructs a recovery speed field in the joint space through the learned natural gesture distribution, so that the joint motion of the robot is continuously pulled back to the low-dimensional natural motion manifold defined by human demonstration, and the statistical manifold constraint is given by the following formula: ; In the above-mentioned method, the step of, Is a unit matrix; Is a defined natural recovery speed; , ; determining three constraint conditions by using the definition type of contact constraint, centroid constraint and statistical manifold constraint In (a) and (b) And : ; 。
  7. 7. The method for redirecting robot motion based on two-stage optimization and multi-constraint fusion of claim 6, wherein said step 140 comprises: Introducing an optimizable violation amount, given by: ; Wherein, the Indicating a violation of a contact constraint; representing a violation of a centroid constraint, Representing violations of statistical manifold constraints; Relaxing the three constraint with the violation amount into an offset constraint as follows: ; obtaining a robust optimization model using the offset constraint: ; ; ; Wherein, the Indicating that the cost function is violated, In order to be a diagonal weight matrix, Representing the vector/matrix transpose.
  8. 8. The method for redirecting robot motion based on two-stage optimization and multi-constraint fusion of claim 7, wherein said step 150 comprises: Step 151, solving a robust optimization model by adopting an enhanced Lagrangian method and combining a Mink solver: Constructing an augmented lagrangian function: ; Wherein, the Is the Lagrangian multiplier vector, dimension and offset constraint The same; , representing the inner product of the vectors; Is a penalty parameter, a secondary penalty strength for controlling constraint violations, Ensure that Reversible; Represents an L2 norm; performing loop iteration solution, the first The round iteration solving process comprises the following steps: Fixing And Updating violations : ; Fixing And Updating Lagrangian multiplier vector : ; Fixing And Updating joint velocity : Will be And Substitution of the augmented Lagrangian function The constant term is ignored, and the constant term, Is equivalent to solving the following sub-problem of strictly convex quadratic programming: ; Solving the above sub-problem of the strict convex quadratic programming by using a Mink solver to obtain ; The Mink solver utilizes the sparsity of the Jacobian matrix in the sub-problem, adopts an interior point method based on sparse matrix decomposition to solve, and adopts a hot start technology to accelerate the iterative process; the loop iteration termination condition comprises the variation of the objective function value of the robust optimization model Less than a preset convergence threshold Or number of iterations Reaching the preset maximum iteration times ; In step 152, a two-stage inverse kinematics solution strategy is used to adjust and optimize the numerical solution.
  9. 9. The method for redirecting motion of a robot based on two-stage optimization and multi-constraint fusion of claim 8, wherein in step 152, a two-stage inverse kinematics solution strategy is used to adjust and optimize the numerical solution, comprising: A first stage of reinforcing spinal joint weights to preferentially optimize pose of an end effector, comprising: Obtaining an optimized model for strengthening the weight of the spinal joint: + ; ; ; Wherein, the , Representing the attitude error vector of the end effector, The number of end effectors includes at least left hand, right hand, left foot, and right foot; Jacobian matrix for end effector, diagonal weight matrix By direction tracking error weights of the first stage And position tracking error weight Constructing; Expressed in terms of Diagonal elements are L2 norms of the weighting coefficients; Solving the optimized model after strengthening the joint weight of the spine by adopting the enhanced Lagrangian method in step 151 and combining a Mink solver to obtain the optimal joint speed of the end effector in the first stage Integrated joint angle vector ; A second stage of optimizing all key body part poses based on the pose results of the end effector in the first stage, including: Expanding the gesture result obtained in the first stage to all key body parts to use The fine-tuning is performed so that, The second stage is respectively a direction tracking error weight and a position tracking error weight, and an optimization model of all the key body part postures is obtained: ; ; ; Wherein, the , Representing a posing error vector for a critical body part of the whole body; jacobian matrix for all key body parts of whole body, diagonal weight matrix By direction tracking error weights of the second stage And position tracking error weight Constructing, namely reducing the weight of the end effector and improving the weight of other key body parts not at the tail end; Expressed in terms of Diagonal elements are L2 norms of the weighting coefficients; Solving the optimization model of all the key body part postures by adopting the enhanced Lagrangian method in step 151 and combining a Mink solver to obtain the optimal joint speeds of all the key body parts of the whole body in the second stage Integrated joint angle vector 。
  10. 10. The method for redirecting robot motion based on two-stage optimization and multi-constraint fusion of claim 9, wherein said step 160 comprises: Carrying out optimal solution of step 150 on each frame of source motion data corresponding to the preprocessed reference track, wherein the first frame uses the static posture of the robot aligned by the static posture as an initial value of an optimal solution iteration process, and the following first frame is provided with a first frame and a second frame The frame of the frame is a frame of a frame, Using the first The redirection result of the frame is used as an initial value of an optimization solving iterative process; the height of the robot in each frame is calculated by utilizing forward kinematics, and the minimum height difference between each frame and the ground is subtracted from the global translation of the robot, so that the integral floating or ground penetrating artifact is eliminated.

Description

Robot motion redirection method based on two-stage optimization and multi-constraint fusion Technical Field The invention relates to the technical field of robot motion control, in particular to a robot motion redirection method based on two-stage optimization and multi-constraint fusion. Background In complex environments such as home, industry, and medical, where humans are the center, autonomous human robots are required to perform continuous operation tasks under frequent contact conditions. The robot system has the core requirements of realizing stable whole body coordination control under the multi-contact working condition, namely reasonably distributing contact force under the combined conditions of double-foot grounding, double-hand supporting or body part contact and the like, meeting the stability constraints of friction cones, supporting polygons, zero Moment Points (ZMP) and the like so as to avoid slipping, overturning or self-collision, and having high-fidelity human body action redirection capability so as to realize effective migration of human operation skills to a robot platform. To achieve the above objective, prior studies typically map human motion capture data to a robot skeleton and generate joint trajectories by inverse kinematics. However, the human body and the robot have obvious differences in the aspects of body dimension, joint freedom degree, contact mode and the like, and artifacts such as foot slippage, ground penetration, joint mutation and the like are easily caused by direct mapping, so that the naturalness and the performability of actions are reduced, and the sim-to-real migration difficulty and the safety risk are increased. From the technical route, the existing methods are mainly divided into a direct mapping method based on kinematics and a generating method based on optimization. Direct mapping relies on joint matching or coordinate transformation, is efficient in calculation, is difficult to process, is unequal to the degree of freedom, and is easy to cause posture collapse and tail end deviation. The optimization-based method combines the posture similarity, smoothness, joint limit and contact retention by constructing a loss function and a constraint term, such as introducing a staged learning rate strategy to balance approximation precision and physical feasibility, or on-line correcting the lower limb joint according to a real-time supporting state to maintain dynamic balance. However, the methods still have the limitations that most of the methods do not fully consider the differential weight of different body parts in motion coordination, especially neglect the key actions of core parts such as spines and the like on the gesture and stability, constraint models tend to be relatively fixed and lack an adaptive coordination mechanism for multi-constraint conflict, more importantly, the prior researches generally adopt multi-contact centroid control and action redirection cutting treatment, namely a centroid control method usually assumes that a contact point is fixed or decoupled from action, the real-time influence of gesture errors introduced by redirection is not fully considered, and an action redirection method focuses on the kinematic fidelity in multiple sides and lacks system modeling on centroid stability, friction constraint and contact consistency. In the complex tasks of dynamic and multi-contact, the similarity of actions and the physical stability of execution are difficult to ensure, and the reliable application of the humanoid robot in a real scene is restricted. Therefore, a method capable of deeply fusing kinematic redirection and multi-contact stability constraint and having layering optimization and constraint conflict coordination capability is needed to be developed so as to generate natural, accurate, stable and feasible robot motion in a complex interaction scene. Disclosure of Invention In view of the above, in order to solve the problems of kinematic artifacts such as foot slippage, ground penetration, joint mutation and the like, as well as quality and robustness in the prior art, the invention provides a robot motion redirection method based on two-stage optimization and multi-constraint fusion. The invention provides a robot motion redirection method based on two-stage optimization and multi-constraint fusion, which comprises the following steps: step 110, establishing a mapping relation of a key body part between a human body and a robot, and carrying out weight configuration of a key body part posture tracking error; Step 120, performing static gesture alignment on source motion data of a human body, and performing non-uniform local scaling on a key body part according to a robot shape to generate a preprocessed reference track; Step 130, based on the preprocessed reference track, minimizing an attitude tracking error of an end effector as an original objective function, and under an inverse kinematics framework, constructing a m