Motion Control Method, Device, Program Product, Robot, and Storage Medium

Abstract

The present disclosure relates to a motion control method, device, program product, robot, and storage medium. The method includes: during the process of controlling the end effector of a robot to operate a target object based on a first desired trajectory, obtaining a desired interaction torque and an actual interaction torque collected by the end effector; adjusting the first desired trajectory according to the desired interaction torque and the actual interaction torque to obtain a second desired trajectory; and controlling the end effector of the robot to operate the target object based on the second desired trajectory.

Description

Technical Field

[0001] The present disclosure relates to the technical field of robotics, and in particular, to a motion control method and apparatus, a program product, a robot, and a storage medium. Background Art

[0002] In recent years, robotics has been continuously developing, becoming more and more intelligent and automated, and the richness, stability, and flexibility of movements have been improved to varying degrees. Robots can replace users to perform specific tasks in users' production and life, thus bringing convenience to users; for example, robots can perform tasks such as manufacturing assembly, daily cleaning, and kitchen utensil operation. There are a large number of physical interaction scenarios in these tasks, so robots need to be able to autonomously adjust their motion trajectories according to information such as vision, interaction force, and position, and ensure their own balance during the interaction process.

[0003] However, in related technologies, when the information of the object to be interacted with is known, the robot can complete the task more precisely; while when the information of the object to be interacted with is unknown, the robot cannot complete the task more precisely, or even cannot complete the task, or cannot maintain balance, resulting in damage to the robot. Summary of the Invention

[0004] To overcome the problems in related technologies, embodiments of the present disclosure provide a motion control method and apparatus, a program product, a robot, and a storage medium to solve the defects in related technologies.

[0005] According to a first aspect of embodiments of the present disclosure, a motion control method is provided, the method including:

[0006] During the process of controlling the end of the robot's arm to operate a target object based on a first desired trajectory, obtaining a desired interaction torque and an actual interaction torque collected at the end of the arm;

[0007] Adjusting the first desired trajectory according to the desired interaction torque and the actual interaction torque to obtain a second desired trajectory;

[0008] Controlling the end of the robot's arm to operate the target object based on the second desired trajectory.

[0009] In some embodiments of the present disclosure, the adjusting the first desired trajectory according to the desired interaction torque and the actual interaction torque to obtain a second desired trajectory includes:

[0010] Adjusting the first desired trajectory according to the difference between the desired interaction torque and the actual interaction torque, and the first desired trajectory, the first desired speed, and the first desired acceleration to obtain a second desired trajectory.

[0011] In some embodiments of the present disclosure, adjusting the first desired trajectory according to the difference between the desired interaction torque and the actual interaction torque, and the first desired trajectory, first desired velocity, and first desired acceleration to obtain a second desired trajectory includes:

[0012] Adjusting the first desired trajectory according to the difference between the desired interaction torque and the actual interaction torque, the first desired trajectory, first desired velocity, first desired acceleration, and stiffness parameters, damping parameters, and inertia parameters to obtain a second desired trajectory;

[0013] Wherein, at different trajectory points of the first desired trajectory, the stiffness parameters are the same or different;

[0014] Wherein, at different trajectory points of the first desired trajectory, the damping parameters are the same or different;

[0015] Wherein, at different trajectory points of the first desired trajectory, the inertia parameters are the same or different.

[0016] In some embodiments of the present disclosure, at different trajectory points of the first desired trajectory, the desired interaction forces are the same or different.

[0017] In some embodiments of the present disclosure, controlling the end of the robot's arm to operate on the target object based on the second desired trajectory includes:

[0018] Taking maintaining the balance of the robot's body as a high-priority task and tracking the second desired trajectory as a low-priority task, and controlling the end of the robot's arm to operate on the target object based on the hierarchical optimization-based whole-body coordination control technology.

[0019] In some embodiments of the present disclosure, taking maintaining the balance of the robot's body as a high-priority task and tracking the second desired trajectory as a low-priority task, and controlling the end of the robot's arm to operate on the target object based on the hierarchical optimization-based whole-body coordination control technology includes:

[0020] Under the constraint of maintaining the balance of the robot's body, optimizing the floating-base acceleration and the acceleration of each joint based on the Jacobian matrix in the robot's floating-base coordinate system to obtain an initial optimization result of the floating-base acceleration and an initial optimization result of the acceleration of each joint;

[0021] Under the constraint of tracking the second desired trajectory, and within the range of the initial optimization result of the floating base acceleration and the initial optimization result of the acceleration of each joint, the floating base acceleration and the accelerations of each joint are optimized based on the Jacobian matrix at the end of the robotic arm and the error between the actual trajectory and the second desired trajectory, so as to obtain the target optimization result of the floating base acceleration and the target optimization result of the acceleration of each joint;

[0022] Based on the target optimization result of the floating base acceleration and the target optimization result of the acceleration of each joint, the optimal torque of each joint is determined, and the corresponding joint is controlled to move based on the optimal torque of each joint, so as to control the end of the robotic arm to operate on the target object.

[0023] In some embodiments of the present disclosure, the constraint conditions for maintaining the balance of the robotic body include at least one of the following:

[0024] The angle of each joint of the robot is within the angular range;

[0025] The angular velocity of each joint of the robot is within the angular velocity range;

[0026] The relative pose between each joint of the robot and the floating base meets the pose stability requirements.

[0027] In some embodiments of the present disclosure, the constraint conditions for tracking the second desired trajectory include at least one of the following:

[0028] The angle of each joint of the robot is within the angular range;

[0029] The angular velocity of each joint of the robot is within the angular velocity range;

[0030] The relative pose between each joint of the robot and the floating base meets the pose stability requirements.

[0031] According to a second aspect of the embodiments of the present disclosure, there is provided a motion control device, the device includes:

[0032] An acquisition module, configured to acquire an expected interaction torque and an actual interaction torque collected at the end of the arm during the process of controlling the end of the robotic arm to operate on a target object based on a first desired trajectory;

[0033] An adjustment module, configured to adjust the first desired trajectory according to the expected interaction torque and the actual interaction torque to obtain a second desired trajectory;

[0034] A tracking module, configured to control the end of the robotic arm to operate on the target object based on the second desired trajectory.

[0035] In some embodiments of the present disclosure, the adjustment module is configured to:

[0036] Adjust the first desired trajectory according to the difference between the desired interaction torque and the actual interaction torque, as well as the first desired trajectory, the first desired velocity, and the first desired acceleration, to obtain a second desired trajectory.

[0037] In some embodiments of the present disclosure, when the adjustment module is configured to adjust the first desired trajectory according to the difference between the desired interaction torque and the actual interaction torque, as well as the first desired trajectory, the first desired velocity, and the first desired acceleration, to obtain a second desired trajectory, it is configured to:

[0038] Adjust the first desired trajectory according to the difference between the desired interaction torque and the actual interaction torque, the first desired trajectory, the first desired velocity, the first desired acceleration, as well as the stiffness parameter, the damping parameter, and the inertia parameter, to obtain a second desired trajectory;

[0039] Wherein, at different trajectory points of the first desired trajectory, the stiffness parameter is the same or different;

[0040] Wherein, at different trajectory points of the first desired trajectory, the damping parameter is the same or different;

[0041] Wherein, at different trajectory points of the first desired trajectory, the inertia parameter is the same or different.

[0042] In some embodiments of the present disclosure, at different trajectory points of the first desired trajectory, the desired interaction force is the same or different.

[0043] In some embodiments of the present disclosure, the tracking module is configured to:

[0044] Taking maintaining the balance of the robot body as a high-priority task and tracking the second desired trajectory as a low-priority task, control the end of the robot's arm to operate on the target object based on the hierarchical optimization whole-body coordination control technology.

[0045] In some embodiments of the present disclosure, the tracking module is configured to:

[0046] Under the constraint of maintaining the balance of the robot body, optimize the floating-base acceleration and the accelerations of each joint based on the Jacobian matrix in the robot's floating-base coordinate system to obtain an initial optimized result of the floating-base acceleration and an initial optimized result of the acceleration of each joint;

[0047] Under the constraint of tracking the second desired trajectory, and within the range of the initial optimization result of the floating base acceleration and the initial optimization result of the acceleration of each joint, the floating base acceleration and the accelerations of each joint are optimized based on the Jacobian matrix at the end of the robotic arm and the error between the actual trajectory and the second desired trajectory, so as to obtain the target optimization result of the floating base acceleration and the target optimization result of the acceleration of each joint;

[0048] Based on the target optimization result of the floating base acceleration and the target optimization result of the acceleration of each joint, the optimal torque of each joint is determined, and the corresponding joint is controlled to move based on the optimal torque of each joint, so as to control the end of the robotic arm to operate on the target object.

[0049] In some embodiments of the present disclosure, the constraint conditions for maintaining the balance of the robotic body include at least one of the following:

[0050] The angle of each joint of the robot is within the angular range;

[0051] The angular velocity of each joint of the robot is within the angular velocity range;

[0052] The relative pose between each joint of the robot and the floating base meets the requirements of pose stability.

[0053] In some embodiments of the present disclosure, the constraint conditions for tracking the second desired trajectory include at least one of the following:

[0054] The angle of each joint of the robot is within the angular range;

[0055] The angular velocity of each joint of the robot is within the angular velocity range;

[0056] The relative pose between each joint of the robot and the floating base meets the requirements of pose stability.

[0057] According to the third aspect of the embodiments of the present disclosure, there is provided a computer program product, including a computer program / instructions, and when the computer program / instructions are executed by a processor, the steps of the method according to any one of the first aspect are implemented.

[0058] According to the fourth aspect of the embodiments of the present disclosure, there is provided a robot, the robot includes a memory and a processor, the memory is used to store computer instructions that can be run on the processor, and the processor is used to implement the motion control method according to any one of the above embodiments when executing the computer instructions.

[0059] According to the fifth aspect of the embodiments of the present disclosure, there is provided a computer-readable storage medium, on which a computer program is stored, and when the program is executed by a processor, the method according to any one of the above embodiments is implemented.

[0060] The technical solutions provided by the embodiments of the present disclosure may include the following beneficial effects:

[0061] In the process of controlling the end of the robot arm to operate the target object based on the first desired trajectory, the motion control method provided by the embodiments of the present disclosure acquires the desired interaction torque and the actual interaction torque collected at the end of the arm; and adjusts the first desired trajectory according to the desired interaction torque and the actual interaction torque to obtain a second desired trajectory; and then controls the end of the robot arm to operate the target object based on the second desired trajectory. Since the actual interaction torque is more accurate and has a smaller error than visual information, after adjusting the desired trajectory based on the desired interaction torque and the actually collected torque, this method can enable the end of the robot arm to adapt to the information of the target object, especially the weight of the target object, thereby improving the operation accuracy of the robot, as well as the reliability and safety of the robot, so that it can complete the task of interacting with the object in the case of unknown object information, such as unknown object weight. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] The accompanying drawings herein are incorporated into the specification and constitute a part of this specification, showing embodiments consistent with the present invention and used together with the specification to explain the principles of the present invention.

[0063] Figure 1 It is a schematic diagram of a scenario where a robot transports a box shown in an exemplary embodiment of the present disclosure;

[0064] Figure 2 It is a flowchart of a motion control method shown in an exemplary embodiment of the present disclosure;

[0065] Figure 3 It is a control logic diagram of a motion control method shown in an exemplary embodiment of the present disclosure;

[0066] Figure 4 It is a schematic structural diagram of a motion control device shown in an exemplary embodiment of the present disclosure;

[0067] Figure 5 It is a structural block diagram of a robot shown in an exemplary embodiment of the present disclosure. DETAILED DESCRIPTION

[0068] Here, the exemplary embodiments will be described in detail, and the examples are shown in the accompanying drawings. When the following description refers to the accompanying drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present disclosure. On the contrary, they are merely examples of devices and methods consistent with some aspects of the present disclosure as detailed in the appended claims.

[0069] The terms used in this disclosure are for the sole purpose of describing particular embodiments and are not intended to limit the disclosure. The singular forms "a", "the", and "said" used in this disclosure and the appended claims are also intended to include the plural forms unless the context clearly dictates otherwise. It should also be understood that the term "and / or" as used herein refers to and encompasses any and all possible combinations of one or more of the associated listed items.

[0070] It should be understood that although the terms first, second, third, etc. may be used in this disclosure to describe various information, such information should not be limited to these terms. These terms are only used to distinguish information of the same type from each other. For example, without departing from the scope of this disclosure, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the word "if" as used herein may be interpreted as "when" or "while" or "in response to determining".

[0071] Based on the technical problems mentioned in the background art, in a first aspect, at least one embodiment of this disclosure provides a motion control method, which can be applied to a robot, such as a bipedal robot (humanoid robot); it should be understood that a bipedal robot has two lower limbs for walking and two upper limbs for operating objects - namely arms, the ends of its lower limbs, i.e., feet, are used to contact the ground for walking, and the ends of its upper limbs, i.e., hands, are used to operate items.

[0072] This method can be applied to operation scenarios where a robot performs operations such as carrying, assembling, and disassembling objects, such as the Figure 1 scenario of a humanoid robot carrying a box as shown. The humanoid robot in the above scenario can meet the following requirements: each joint driver can support torque control at 1 kHz, the on-board computing device of the robot can solve the quadratic optimal control problem at a frequency of 500 Hz; the head is equipped with a vision sensor to obtain the pose information of the interaction object, and the position error accuracy is within 1 cm; the wrist and ankle are equipped with six-axis torque sensors to obtain the end torque of the robot, and the torque error is within 1 N; each joint has an encoder that can return the joint angle and angular velocity; the robot's waist is equipped with an inertial measurement unit to estimate the position and velocity of the robot's floating base.

[0073] Please refer to the attached Figure 2 , which exemplarily shows the flow of this method, including steps S201 to S203.

[0074] In step S201, during the process of controlling the end of the robot's arm to operate the target object based on the first desired trajectory, the desired interaction torque and the actual interaction torque collected at the end of the arm are obtained.

[0075] In the motion control of a robot, control instructions are output frame by frame at a certain period, such as the joint torques of each joint, so as to achieve continuous control. The desired trajectory includes at least one trajectory point, on which the poses, velocities, accelerations, etc. of each joint of the robot and the floating base are marked, and the robot controls the end of the arm to track these trajectory points one by one to make it move along the desired trajectory. The first desired trajectory may refer to the entire desired trajectory, or the desired pose of the trajectory point that needs to be tracked in the current frame of the desired trajectory.

[0076] Among them, the actual interaction torque at the end of the arm can be collected by a six-dimensional torque sensor equipped on the robot wrist, and the frequency of this sensor collecting the actual interaction torque is equal to or higher than the instruction output frequency of the robot control.

[0077] Exemplarily, taking maintaining the balance of the robot body as a high-priority task and tracking the first desired trajectory as a low-priority task, the end of the robot arm is controlled to operate on the target object based on the hierarchical optimization-based whole-body coordination control technology.

[0078] Among them, the constraint conditions for maintaining the balance of the robot body include at least one of the following:

[0079] The angle of each joint of the robot is within the angular range;

[0080] The angular velocity of each joint of the robot is within the angular velocity range;

[0081] The relative pose between each joint of the robot and the floating base meets the pose stability requirements.

[0082] Among them, the constraint conditions for tracking the second desired trajectory include at least one of the following:

[0083] The angle of each joint of the robot is within the angular range;

[0084] The angular velocity of each joint of the robot is within the angular velocity range;

[0085] The relative pose between each joint of the robot and the floating base meets the pose stability requirements.

[0086] In step S202, the first desired trajectory is adjusted according to the desired interaction torque and the actual interaction torque to obtain a second desired trajectory.

[0087] Exemplarily, the first desired trajectory is adjusted according to the difference between the desired interaction torque and the actual interaction torque, and the first desired trajectory, the first desired velocity, and the first desired acceleration to obtain a second desired trajectory.

[0088] For example, according to the difference between the desired interaction torque and the actual interaction torque, the first desired trajectory, the first desired velocity, the first desired acceleration, and the stiffness parameter, the damping parameter, and the inertia parameter, the first desired trajectory is adjusted to obtain a second desired trajectory. That is, the second desired trajectory is determined according to the following formula:

[0089]

[0090] In the above formula, F represents the actual interaction torque, and F r represents the desired interaction torque, r d represents the second desired trajectory, and r r represents the first desired trajectory. is the second desired velocity, is the first desired velocity, is the second desired acceleration, is the first desired acceleration, K s , K d , K m represent the stiffness, damping, and inertia parameters of the controller. The larger these parameters are, the more sensitive the corresponding position, velocity, and acceleration are to the interaction force. At this time, the same interaction force will produce a larger deviation in the robot trajectory movement. Therefore, they can be selected by the engineer according to experience. Specifically, first, the interaction force F, the preset desired force F r and the first desired trajectory r r are obtained, and then an adjusted second desired trajectory r d is calculated according to the model represented by the above formula.

[0091] Among them, at different trajectory points of the first desired trajectory, the stiffness parameter may be the same or different;

[0092] Among them, at different trajectory points of the first desired trajectory, the damping parameter may be the same or different;

[0093] Among them, at different trajectory points of the first desired trajectory, the inertia parameter may be the same or different.

[0094] One advantage of the admittance controller represented by the above formula is that engineers can design appropriate desired torques and admittance parameters according to the robot application scenario, namely, stiffness parameters, damping parameters, and inertia parameters. Taking the example of a robot opening a hatch, when lifting the hatch, the movement of the robotic arm will be affected by a sudden interaction force. Then, during the process of lifting the hatch, the interaction force between the hatch and the robotic arm decreases as the lifting angle of the hatch increases. Finally, when withdrawing the robotic arm, the controller needs to adaptively adjust the end posture according to the contact force to ensure as small a collision as possible. For the above scenario, this project analyzes the influence of parameters such as compliant control stiffness, damping, mass, and desired interaction force on the operation process, and designs a compliant control parameter adjustment strategy for operating objects of unknown weight. Specifically, in the stage of lifting the hatch, a desired interaction torque F is preset for the admittance controller r to accelerate the process of the robotic arm lifting the hatch, and this desired torque should be greater than or equal to the weight of the hatch. At the same time, a high damping is designed to reduce the impact of the sudden change in the interaction force on the movement of the robotic arm. In the stage of lifting the hatch, the desired interaction torque is gradually reduced according to the lifting angle to adapt to the lifting process. In the withdrawal stage, a low stiffness in the direction of the end posture of the robotic arm is maintained to reduce the collision between the robotic arm and the hatch.

[0095] In step S203, based on the second desired trajectory, control the end of the robot's arm to operate on the target object.

[0096] Exemplarily, taking maintaining the balance of the robot body as a high-priority task and tracking the second desired trajectory as a low-priority task, control the end of the robot's arm to operate on the target object based on the hierarchical optimization-based whole-body coordination control technology.

[0097] Next, a detailed introduction to the hierarchical optimization-based whole-body coordination control technology and the specific implementation method of this step will be given.

[0098] Hierarchical optimization-based whole-body coordination control technology

[0099] Assume that a set of optimal solutions that satisfy the first p-layer tasks is Then, when solving the (p + 1)-th task, certain principles need to be followed, that is, solving the (p + 1)-th layer task shall not violate the equality constraints and inequality constraints of the first p-layer tasks.

[0100] To ensure that the equality constraints of the first p-layer tasks are not violated, the feasible region of the optimization variables in the (p + 1)-th layer QP problem (quadratic programming problem) is expressed as:

[0101]

[0102] In the above formula, N p is the null space projection matrix of the first p-layer tasks; z p+1 is a free vector. Thanks to the characteristics of the null space projection, when zp+1 When taking any value, x p+1 will not violate the equality constraints of the first p layers of tasks. N p The expression of can be obtained by iterative calculation using the following formula:

[0103]

[0104] In the formula, N p is equal to the null space projection matrix N of the first p - 1 layers of tasks p-1 multiplied by the null space projection matrix of the (p - 1)-th layer of tasks of the product. represents the task coefficient matrix A of the (p - 1)-th layer of tasks p-1 after being mapped by the null space projection matrix, the new task coefficient matrix, represents the pseudo-inverse matrix.

[0105] To ensure that the inequality constraints of the first p layers of tasks are not violated, when constructing the (p + 1)-th layer QP problem, it is necessary to add the inequality constraints of the first p layers of tasks to the constraints, specifically as follows:

[0106]

[0107] …

[0108]

[0109] In the formula, etc. are the terms to be constrained, f1, f p , f p+1 are the constraint values of the terms to be constrained, v p+1 is the constraint variable.

[0110] At this time, the goal of the (p + 1)-th layer optimization problem is to find a set of z p+1 , v p+1 such that, on the premise of satisfying the inequality constraints of the first p + 1 layers of tasks, the optimization objective function is minimized. According to the optimization result of the (p + 1)-th layer problem the total output of the first p + 1 layers of tasks can be obtained as:

[0111]

[0112] Finally, the solver will be based on output the joint torques of the robot

[0113] Accordingly, this step can be executed in the following manner:

[0114] First, under the constraint conditions for maintaining the balance of the robot's body, the floating-base acceleration and the accelerations of each joint are optimized based on the Jacobian matrix in the floating-base coordinate system of the robot, so as to obtain the initial optimization results of the floating-base acceleration and the initial optimization results of the acceleration of each joint.

[0115] Among them, the task of maintaining the balance of the robot's body can be expressed as:

[0116]

[0117] In the above formula, u is the generalized velocity variable, which includes the linear velocity and angular velocity of the floating-base coordinate system of the robot and the rotational velocities of all joints. x d is the decision variable to be optimized and solved, representing the linear acceleration and angular acceleration of the floating-base coordinate system of the robot, as well as the joint accelerations and joint torques (i.e., the feedforward torques related to the desired trajectory). n represents the dimension of the joint torques of the robot. represents the Jacobian matrix in the floating-base coordinate system of the robot, which can be obtained according to the kinematic model of the robot.

[0118] Among them, the constraint conditions for maintaining the balance of the robot's body include at least one of the following:

[0119] The angle of each joint of the robot is within the angular range;

[0120] The angular velocity of each joint of the robot is within the angular velocity range;

[0121] The relative pose between each joint of the robot and the floating base meets the pose stability requirements.

[0122] Next, under the constraint conditions for tracking the second desired trajectory, and within the range of the initial optimization results of the floating-base acceleration and the initial optimization results of the acceleration of each joint, the floating-base acceleration and the accelerations of each joint are optimized based on the Jacobian matrix at the end of the robot arm and the error between the actual trajectory and the second desired trajectory, so as to obtain the target optimization results of the floating-base acceleration and the target optimization results of the acceleration of each joint.

[0123] Among them, the task of tracking the second desired trajectory can be expressed as:

[0124]

[0125] In the above formula, represents the Jacobian matrix in the coordinate system at the end of the robot, which can be obtained according to the kinematic model of the robot. The state error is defined as that is, the error between the actual trajectory r and the desired trajectory r d KP , K D is the gain parameter for controlling the error change, which can be adjusted by engineers according to experience. Other tasks can be added according to requirements, such as dynamic constraints, output torque limits, etc.

[0126] Among them, the constraint conditions for tracking the second desired trajectory include at least one of the following:

[0127] The angle of each joint of the robot is within the angular range;

[0128] The angular velocity of each joint of the robot is within the angular velocity range;

[0129] The relative pose between each joint of the robot and the floating base meets the requirements of pose stability.

[0130] Finally, based on the target optimization results of the floating base acceleration and the target optimization results of the acceleration of each joint, the optimal torque of each joint is determined, and the corresponding joint is controlled to move based on the optimal torque of each joint to control the end of the robot's arm to operate on the target object.

[0131] For example, based on the target optimization results of the floating base acceleration, and integrating the velocity of the floating base in the previous frame to obtain the velocity of the floating base in the current frame; then based on the velocity of the floating base in the current frame, and integrating the pose of the floating base in the previous frame to obtain the pose of the floating base in the current frame.

[0132] For example, for each joint, based on the target optimization results of the joint acceleration, and integrating the angular velocity of the joint in the previous frame to obtain the angular velocity of the joint in the current frame; then based on the angular velocity of the joint in the current frame, and integrating the angle of the joint in the previous frame to obtain the angle of the joint in the current frame. Finally, PD (Proportional-Derivative Control) control is performed based on the angle, angular velocity, and angular acceleration of each joint in the current frame to obtain the optimal torque of each joint.

[0133] Controlling each joint of the robot based on the optimal torque can thus achieve the tracking of the second desired trajectory by the end of the arm and can adapt to the weight of the target object.

[0134] Combining the above various embodiments, it can be seen that this method can be attached Figure 3The control strategy shown completes the control. First, admittance control is performed based on the initial reference trajectory and the interaction force (the actual interaction force and the desired interaction force), that is, step S201 and step S202 are executed; then, based on the corrected reference trajectory output by the admittance control, that is, the state feedback of the robot (i.e., the states of each joint and the state of the floating base, etc.), whole-body control is performed to output joint torques to each joint of the robot, so that each joint tracks the corrected reference trajectory by adjusting the joint torque, thereby completing the interaction with an object of unknown weight.

[0135] It should be understood that this method can be executed once for each frame of the robot motion control. During the execution process, the current desired trajectory is adjusted, and the adjusted desired trajectory is tracked to interact with the target object, so that real-time torque control adaptable to external forces can be achieved, enabling the robot to stably, safely, and smoothly complete the object interaction task.

[0136] In the process of controlling the end effector of the robot arm to operate the target object based on the first desired trajectory, the motion control method provided by the embodiments of the present disclosure obtains the desired interaction torque and the actual interaction torque collected at the end effector of the arm; and adjusts the first desired trajectory according to the desired interaction torque and the actual interaction torque to obtain a second desired trajectory; and then controls the end effector of the robot arm to operate the target object based on the second desired trajectory. Since the actual interaction torque is more accurate and has a smaller error than visual information, after the method adjusts the desired trajectory based on the desired interaction torque and the actually collected torque, the end effector of the robot arm can adapt to the information of the target object, especially the weight of the target object, thereby improving the operation accuracy of the robot, as well as the reliability and safety of the robot, enabling it to complete the task of interacting with the object in the case of unknown object information, such as the weight of the unknown object.

[0137] According to the second aspect of the embodiments of the present disclosure, a motion control device is provided. Please refer to the appendix Figure 4 , the device includes:

[0138] An acquisition module 401, configured to obtain the desired interaction torque and the actual interaction torque collected at the end effector of the arm during the process of controlling the end effector of the robot arm to operate the target object based on the first desired trajectory;

[0139] An adjustment module 402, configured to adjust the first desired trajectory according to the desired interaction torque and the actual interaction torque to obtain a second desired trajectory;

[0140] A tracking module 403, configured to control the end effector of the robot arm to operate the target object based on the second desired trajectory.

[0141] In some embodiments of the present disclosure, the adjustment module is configured to:

[0142] Adjust the first desired trajectory according to the difference between the desired interaction torque and the actual interaction torque, as well as the first desired trajectory, the first desired velocity, and the first desired acceleration, to obtain a second desired trajectory.

[0143] In some embodiments of the present disclosure, when the adjustment module is configured to adjust the first desired trajectory according to the difference between the desired interaction torque and the actual interaction torque, as well as the first desired trajectory, the first desired velocity, and the first desired acceleration, to obtain a second desired trajectory, it is configured to:

[0144] Adjust the first desired trajectory according to the difference between the desired interaction torque and the actual interaction torque, the first desired trajectory, the first desired velocity, the first desired acceleration, as well as the stiffness parameter, the damping parameter, and the inertia parameter, to obtain a second desired trajectory;

[0145] Wherein, at different trajectory points of the first desired trajectory, the stiffness parameter is the same or different;

[0146] Wherein, at different trajectory points of the first desired trajectory, the damping parameter is the same or different;

[0147] Wherein, at different trajectory points of the first desired trajectory, the inertia parameter is the same or different.

[0148] In some embodiments of the present disclosure, at different trajectory points of the first desired trajectory, the desired interaction force is the same or different.

[0149] In some embodiments of the present disclosure, the tracking module is configured to:

[0150] Regarding maintaining the balance of the robot body as a high-priority task and tracking the second desired trajectory as a low-priority task, control the end of the robot's arm to operate on the target object based on the hierarchical optimization whole-body coordination control technology.

[0151] In some embodiments of the present disclosure, the tracking module is configured to:

[0152] Under the constraint of maintaining the balance of the robot body, optimize the floating-base acceleration and the accelerations of each joint based on the Jacobian matrix in the floating-base coordinate system of the robot, to obtain an initial optimization result of the floating-base acceleration and an initial optimization result of the acceleration of each joint;

[0153] Under the constraint of tracking the second desired trajectory, and within the range of the initial optimization result of the floating base acceleration and the initial optimization result of the acceleration of each joint, the floating base acceleration and the accelerations of each joint are optimized based on the Jacobian matrix at the end of the robotic arm and the error between the actual trajectory and the second desired trajectory, so as to obtain the target optimization result of the floating base acceleration and the target optimization result of the acceleration of each joint;

[0154] Based on the target optimization result of the floating base acceleration and the target optimization result of the acceleration of each joint, the optimal torque of each joint is determined, and the corresponding joint is controlled to move based on the optimal torque of each joint, so as to control the end of the robotic arm to operate on the target object.

[0155] In some embodiments of the present disclosure, the constraint conditions for maintaining the balance of the robotic body include at least one of the following:

[0156] The angle of each joint of the robot is within the angular range;

[0157] The angular velocity of each joint of the robot is within the angular velocity range;

[0158] The relative pose between each joint of the robot and the floating base meets the pose stability requirements.

[0159] In some embodiments of the present disclosure, the constraint conditions for tracking the second desired trajectory include at least one of the following:

[0160] The angle of each joint of the robot is within the angular range;

[0161] The angular velocity of each joint of the robot is within the angular velocity range;

[0162] The relative pose between each joint of the robot and the floating base meets the pose stability requirements.

[0163] Regarding the device in the above embodiments, the specific manner in which each module performs operations has been described in detail in the embodiments of the method in the first aspect, and will not be elaborated here.

[0164] According to a third aspect of the embodiments of the present disclosure, there is provided a computer program product, including a computer program / instructions, and when the computer program / instructions are executed by a processor, the steps of the method described in any embodiment of the first aspect are implemented.

[0165] According to a fourth aspect of the embodiments of the present disclosure, there is provided a robot, please refer to the attached Figure 5, the robot includes a memory and a processor. The memory is used to store computer instructions that can run on the processor, and the processor is used to implement the motion control method provided by any embodiment of the first aspect when executing the computer instructions.

[0166] In a sixth aspect, at least one embodiment of the present disclosure provides a computer-readable storage medium, on which a computer program is stored. When the program is executed by a processor, it implements the method described in any one of the first aspects.

[0167] Those skilled in the art will readily conceive of other embodiments of the present disclosure after considering the specification and practicing the disclosure herein. This application is intended to cover any variations, uses, or adaptations of the present disclosure, which follow the general principles of the present disclosure and include known common knowledge or conventional technical means in the technical field not disclosed by the present disclosure. The specification and embodiments are only regarded as exemplary, and the true scope and spirit of the present disclosure are pointed out by the following claims.

[0168] It should be understood that the present disclosure is not limited to the exact structures described above and shown in the drawings, and various modifications and changes can be made without departing from its scope. The scope of the present disclosure is only limited by the appended claims.

Claims

1. A motion control method, characterized in that, The method includes: During the process of controlling the end - effector of the robot arm to operate the target object based on the first desired trajectory, obtaining the desired interaction torque and the actual interaction torque collected by the end - effector of the arm; Adjusting the first desired trajectory according to the desired interaction torque and the actual interaction torque to obtain a second desired trajectory; Controlling the end - effector of the robot arm to operate the target object based on the second desired trajectory, including: taking maintaining the balance of the robot body as a high - priority task and tracking the second desired trajectory as a low - priority task, and controlling the end - effector of the robot arm to operate the target object based on the hierarchical - optimization whole - body coordination control technology; Wherein, taking maintaining the balance of the robot body as a high - priority task and tracking the second desired trajectory as a low - priority task, and controlling the end - effector of the robot arm to operate the target object based on the hierarchical - optimization whole - body coordination control technology includes: under the constraint of maintaining the balance of the robot body, optimizing the floating - base acceleration and the accelerations of each joint based on the Jacobian matrix in the robot's floating - base coordinate system to obtain the initial optimization results of the floating - base acceleration and the initial optimization results of the accelerations of each joint; under the constraint of tracking the second desired trajectory and within the range of the initial optimization results of the floating - base acceleration and the initial optimization results of the accelerations of each joint, optimizing the floating - base acceleration and the accelerations of each joint based on the Jacobian matrix of the robot arm end - effector and the error between the actual trajectory and the second desired trajectory to obtain the target optimization results of the floating - base acceleration and the target optimization results of the accelerations of each joint; determining the optimal torque of each joint based on the target optimization results of the floating - base acceleration and the target optimization results of the accelerations of each joint, and controlling the corresponding joint to move based on the optimal torque of each joint to control the end - effector of the robot arm to operate the target object.

2. The motion control method according to claim 1, wherein The adjusting the first desired trajectory according to the desired interaction torque and the actual interaction torque to obtain a second desired trajectory includes: Adjusting the first desired trajectory according to the difference between the desired interaction torque and the actual interaction torque, and the first desired trajectory, the first desired velocity, and the first desired acceleration to obtain a second desired trajectory.

3. The motion control method according to claim 2, wherein The adjusting the first desired trajectory according to the difference between the desired interaction torque and the actual interaction torque, and the first desired trajectory, the first desired velocity, and the first desired acceleration to obtain a second desired trajectory includes: Adjusting the first desired trajectory according to the difference between the desired interaction torque and the actual interaction torque, the first desired trajectory, the first desired velocity, the first desired acceleration, and the stiffness parameter, the damping parameter, and the inertia parameter to obtain a second desired trajectory; Wherein, at different trajectory points of the first desired trajectory, the stiffness parameter is the same or different; Wherein, at different trajectory points of the first desired trajectory, the damping parameter is the same or different; Among them, at different trajectory points of the first desired trajectory, the inertial parameters are the same or different.

4. The motion control method according to claim 1, wherein At different trajectory points of the first desired trajectory, the desired interaction torques are the same or different.

5. The motion control method according to claim 1, characterized in that The constraint conditions for maintaining the balance of the robot body include at least one of the following: The angle of each joint of the robot is within the angular range; The angular velocity of each joint of the robot is within the angular velocity range; The relative pose between each joint of the robot and the floating base meets the pose stability requirements.

6. The motion control method according to claim 1, characterized in that, The constraint conditions for tracking the second desired trajectory include at least one of the following: The angle of each joint of the robot is within the angular range; The angular velocity of each joint of the robot is within the angular velocity range; The relative pose between each joint of the robot and the floating base meets the pose stability requirements.

7. A motion control device, characterized in that, The device includes: An acquisition module, configured to acquire the desired interaction torque and the actual interaction torque collected at the end of the robot arm during the process of controlling the end of the robot arm to operate the target object based on the first desired trajectory; An adjustment module, configured to adjust the first desired trajectory according to the desired interaction torque and the actual interaction torque to obtain a second desired trajectory; A tracking module, configured to control the end of the robot arm to operate the target object based on the second desired trajectory, including: taking maintaining the balance of the robot body as a high-priority task and tracking the second desired trajectory as a low-priority task, and controlling the end of the robot arm to operate the target object based on the hierarchical optimization-based whole-body coordination control technology; Among them, the method of taking maintaining the balance of the robot body as a high-priority task, tracking the second desired trajectory as a low-priority task, and controlling the end of the robot arm to operate the target object based on the hierarchical optimization-based whole-body coordination control technology includes: under the constraint conditions of maintaining the balance of the robot body, optimizing the floating base acceleration and the acceleration of each joint based on the Jacobian matrix in the robot floating base coordinate system to obtain the initial optimization results of the floating base acceleration and the acceleration of each joint; under the constraint conditions of tracking the second desired trajectory and within the range of the initial optimization results of the floating base acceleration and the acceleration of each joint, optimizing the floating base acceleration and the acceleration of each joint based on the Jacobian matrix at the end of the robot arm and the error between the actual trajectory and the second desired trajectory to obtain the target optimization results of the floating base acceleration and the acceleration of each joint; determining the optimal torque of each joint based on the target optimization results of the floating base acceleration and the acceleration of each joint, and controlling the corresponding joint to move based on the optimal torque of each joint to control the end of the robot arm to operate the target object.

8. A computer program product, comprising a computer program / instructions, characterized in that, When the computer program / instructions are executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

9. A robot, characterized in that, The robot includes a memory and a processor. The memory is used to store computer instructions that can run on the processor, and the processor is used to implement the method described in any one of claims 1 to 6 when executing the computer instructions.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, the method described in any one of claims 1 to 6 is implemented.

Images