Force - position hybrid control method and device for robot

Abstract

The present invention provides a force-position hybrid control method and device for a robot, relating to the technical field of robot control, including: determining a control target of the robot based on a desired input of the robot for operating in a preset control mode; calculating a control increment of the robot corresponding to the control target through a preset state prediction model to determine a control torque of the robot, so as to perform drive control on the robot. Among them, the control target includes at least a hybrid mode of any one of the following controls: pose control, force or torque control; the state prediction model is constructed based on a torque state equation, a pose state equation of the robot, and a Jacobian matrix of the robot, and the Jacobian matrix is constructed based on the Jacobians of the waist and double-arm subsystems of the robot. The present invention can achieve pose tracking and force / torque tracking of the robot under a unified framework, and realize force-position hybrid control of high-degree-of-freedom robots such as humanoid robots, and can more comprehensively meet complex task requirements.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and more particularly to a force-position hybrid control method and device for a robot. Background Art

[0002] With the development of robot technology, especially in the field of humanoid robots, the requirements for robot motion control are getting higher and higher. Traditional control methods usually design position control and force control separately, that is, using a position controller to track the desired position and attitude, and using a force controller to adjust the force or torque when contacting the environment. In existing solutions, position control and force control are usually designed independently and not integrated within a unified framework. This makes the control system complex and difficult to optimize, especially when dealing with complex interaction tasks. Moreover, when the position controller and the force controller work in parallel, if the position controller tries to accurately track a target position while the force controller needs to adjust the contact force to adapt to environmental changes, conflicts may occur between the two, resulting in poor control effects. Summary of the Invention

[0003] To solve the above problems, embodiments of the present invention propose a force-position hybrid control method and device for a robot, which can achieve pose tracking and force / torque tracking of the robot under a unified framework, realize force-position hybrid control of high-degree-of-freedom robots such as humanoid robots, and can more comprehensively meet complex task requirements.

[0004] In a first aspect, an embodiment of the present invention provides a force-position hybrid control method for a robot, the method comprising: obtaining an expected input to the robot for a preset control mode operation; determining a control target of the robot based on the expected input; the control target includes at least one of the following control hybrid modes: pose control, force or torque control; calculating a control increment of the robot corresponding to the control target through a preset state prediction model; wherein the state prediction model is constructed based on the torque state equation, pose state equation of the robot and the Jacobian matrix of the robot, and the Jacobian matrix is constructed based on the Jacobians of the waist and double-arm subsystems of the robot; determining a control torque of the robot based on the control increment; and performing drive control on the robot according to the control torque so that the robot meets the expected input.

[0005] Combined with the first aspect, the embodiments of the present invention provide a first implementation manner of the first aspect. Among them, the method for constructing a state prediction model includes: obtaining a preset dynamic model; the dynamic model includes a feedforward torque and a model predictive control torque, and the feedforward torque includes the Jacobian matrix of the robot; introducing an incremental control torque into the model predictive control torque of the dynamic model to construct an incremental system of the dynamic model; based on the incremental system, calculating the pose state equation and the torque state equation of the robot to generate a force-pose augmented state equation; introducing a selection matrix into the force-pose augmented state equation and combining it with the feedforward torque of the dynamic model to construct a state prediction model; wherein, the selection matrix is used to select the control dimension of the robot.

[0006] Combined with the first aspect, the embodiments of the present invention provide a second implementation manner of the first aspect. Among them, the step of calculating the pose state equation and the torque state equation of the robot based on the incremental system to generate a force-pose augmented state equation includes: discretizing the incremental system to obtain the discrete state equation of the robot; determining the pose state equation of the robot based on the discrete state equation; determining the generalized torque state equation of the robot based on a preset force closed-loop gain diagonal matrix; combining the pose state equation and the generalized torque state equation to obtain the force-pose augmented state equation of the robot.

[0007] Combined with the first aspect, the embodiments of the present invention provide a third implementation manner of the first aspect. Among them, the method for constructing the Jacobian matrix of the robot includes: constructing the world coordinate system of the robot based on the position of the robot, and constructing the base coordinate system of the robot based on the positions of the two arms and the waist of the robot; determining the arm Jacobian matrix of the end of the two arms of the robot relative to the base coordinate system, and the waist Jacobian matrix of the base coordinate system relative to the world coordinate system; constructing the Jacobian matrix of the robot according to the speed transmission relationship of the waist of the robot to the two arms and combining the arm Jacobian matrix and the waist Jacobian matrix.

[0008] Combined with the first aspect, the embodiments of the present invention provide a fourth implementation manner of the first aspect. Among them, the step of calculating the robot control increment corresponding to the control target through a preset state prediction model includes: solving the desired input of the control target through the state prediction model based on a preset optimization target to obtain the robot control increment corresponding to the desired input; wherein, the optimization target includes the state constraint and the control increment constraint corresponding to the robot.

[0009] In combination with the first aspect, an embodiment of the present invention provides a fifth implementation manner of the first aspect. The step of determining the control torque of the robot based on the control increment includes: controlling the preliminary state of the robot based on the control increment corresponding to the control target; tracking the current state of the robot to determine the optimal output sequence of the robot; determining the feedforward torque corresponding to the state prediction model based on the optimal output sequence; and calculating the control torque of the robot based on the feedforward torque and the control increment.

[0010] In combination with the first aspect, an embodiment of the present invention provides a sixth implementation manner of the first aspect. The method further includes: obtaining a linear increment system equation by using the time delay theory.

[0011] In combination with the first aspect, an embodiment of the present invention provides a seventh implementation manner of the first aspect. The number of rows and columns of the selection matrix is determined based on the control dimension of the robot. The control dimension includes one of the following hybrid dimensions: the attitude control dimension of the robot, the force or torque control dimension; the hybrid dimension includes the translational dimension and the rotational dimension of the robot's two arms; wherein, each row of the selection matrix has and only has one element as 1, and the others are 0; each column has at most one element as 1, and the others are 0.

[0012] In combination with the first aspect, an embodiment of the present invention provides an eighth implementation manner of the first aspect. The step of driving and controlling the robot according to the control torque includes: mapping the control torque to the task space corresponding to the control target to generate a joint torque command; and performing joint drive on the robot based on the joint torque command.

[0013] In a second aspect, an embodiment of the present invention provides a control device for a robot. The device includes: a data acquisition module for acquiring the desired input to the robot for a preset control mode operation; a data processing module for determining the control target of the robot based on the desired input; the control target includes at least one of the following control hybrid modes: pose control, force or torque control; a calculation module for calculating the control increment of the robot corresponding to the control target through a preset state prediction model; wherein, the state prediction model includes the Jacobian matrix of the robot, and the Jacobian of the robot's waist and two-arm subsystem is constructed; an execution module for determining the control torque of the robot based on the control increment; and a control module for driving and controlling the robot according to the control torque so that the robot meets the desired input.

[0014] The embodiments of the present invention bring the following beneficial effects: A force-position hybrid control method and device for a robot provided by the embodiments of the present invention can achieve force-position hybrid control of high-degree-of-freedom robots such as humanoid robots, and can more comprehensively meet complex task requirements. Moreover, the embodiments of the present invention use the velocity description of the Jacobian matrix to achieve the pose tracking of the robot, and can more accurately predict and control the pose change of the robot. By combining with a pre-constructed state prediction model to calculate the control torque of the robot, it is possible to achieve pose tracking and force / torque tracking of the robot under a unified framework. This not only improves the flexibility and response speed of the control system, but also enhances the stability and robustness of the system.

[0015] Other features and advantages of the present invention will be described in the following specification, and part of them will become obvious from the specification, or will be understood by implementing the present invention. The objectives and other advantages of the present invention are achieved and obtained by the structures specifically pointed out in the specification, claims and drawings.

[0016] To make the above objectives, features and advantages of the present invention more obvious and understandable, the following preferred embodiments are specifically given, and in conjunction with the accompanying drawings, the detailed description is as follows. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following will briefly introduce the drawings required for the description of the specific embodiments or the prior art. Obviously, the following drawings are some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0018] Figure 1 Shows the flowchart of a force-position hybrid control method for a robot provided by an embodiment of the present invention;

[0019] Figure 2 Shows the block diagram of a force-position hybrid control structure for a humanoid robot provided by an embodiment of the present invention;

[0020] Figure 3 Shows the flowchart of a force-position hybrid control method for a robot provided by an embodiment of the present invention;

[0021] Figure 4 Shows the schematic diagram of the coordinate system of a robot provided by an embodiment of the present invention;

[0022] Figure 5 Shows the schematic diagram of the structure of a force-position hybrid control device for a robot provided by an embodiment of the present invention;

[0023] Figure 6 Shows the schematic diagram of the structure of an electronic device provided by an embodiment of the present invention. Detailed implementation manners

[0024] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below in conjunction with the embodiments. Apparently, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0025] In the existing robotic force-position control methods, they mainly focus on the end control of the robotic arm or dedicated equipment (such as nucleic acid sampling robots) in specific application scenarios. However, in the existing technology, the position controller and the force controller are usually designed separately and simply combined to achieve force-position hybrid control. This approach fails to simultaneously implement pose control and force / torque control in a unified framework, resulting in inflexible control strategies and difficulty in adapting to complex multi-degree-of-freedom robotic systems. In addition, the existing technology combines the position controller and the force controller to ensure that the end of the robotic arm moves along a specified trajectory, and its flexibility is limited by the preset task phases and specific application scenarios, unable to meet the broader and dynamically changing task requirements, and thus has a limited scope of application.

[0026] In response to this, the embodiments of the present invention provide a force-position hybrid control method and device for a robot, which integrate pose control (such as position and attitude) and force / torque control into the same control framework, rather than simply using two independent controllers in parallel. The embodiments of the present invention allow multiple control requirements to be processed simultaneously within the same control, improving the flexibility and response speed of the system. Moreover, the embodiments of the present invention can ensure the effective constraint of the control quantity, significantly enhancing the safety and reliability of robotic operations.

[0027] For ease of understanding, first, a force-position hybrid control method for a robot provided by the embodiments of the present invention will be described. Figure 1 FIG. shows a flowchart of a force-position hybrid control method for a robot provided by the embodiments of the present invention. Referring to Figure 1 , the method includes the following steps:

[0028] Step S102, obtain the desired input for the robot for the preset control mode operation.

[0029] Step S104, based on the desired input, determine the control objective of the robot.

[0030] First, determine the specific tasks or actions that the user or the system hopes the robot to perform, such as moving to a specific position, grasping an object, etc., and then convert the high-level desired input into specific and quantifiable control objectives, including but not limited to position, attitude, force, or torque values.

[0031] In the force-position hybrid control scenario of a robot, according to the requirements of different application scenarios, the form of the desired input and the corresponding control objectives will be different. The control objectives of the embodiments of the present invention include at least one of the following control hybrid modes: pose control, force or torque control. In specific implementation, in some scenarios, pure torque control is performed on the robot, and the desired input directly specifies the force or torque that the end effector or joint of the robot needs to apply, and defines the interaction rules between the robot and the environment, such as maintaining a constant pressure or tracking a specific force curve. Correspondingly, it is necessary to ensure that each drive unit (such as a motor) can accurately generate the required torque, so that the robot can exhibit a certain compliance on the contact surface, such as providing an appropriate gripping force when grasping fragile objects without damaging the object. In some scenarios, pure pose control is performed on the robot, and the desired input includes position (x, y, z coordinates) and attitude (rotation angle, usually represented by quaternion or Euler angle). For complex action sequences, a preset trajectory or a series of key points may also be required as a reference. Correspondingly, reasonable speed and acceleration limits need to be set to ensure smooth and safe operation, and to ensure that the end effector of the robot can accurately reach the specified position and attitude. In some scenarios, force-position hybrid control is performed on the robot, and the desired input includes both force / torque values and pose information. For example, in a welding task, it may be necessary to control the welding torch to move along a predetermined path (pose control) while maintaining a certain welding pressure (force control). Among them, it can also be specified which dimensions should more strictly follow force control and which dimensions should focus on pose control. Correspondingly, the control strategy can be automatically adjusted according to the real-time feedback to cope with changes in the environment or task requirements, and strict force control is performed on some degrees of freedom, while pose accuracy is emphasized on other degrees of freedom.

[0032] In summary, the required control parameters are calculated according to the corresponding control objectives, so as to control the robot.

[0033] Step S106, calculate the robot control increment corresponding to the control objective through a preset state prediction model.

[0034] Step S108, determine the control torque of the robot based on the control increment.

[0035] Step S110, perform drive control on the robot according to the control torque so that the robot meets the desired input.

[0036] In an embodiment of the present invention, a mathematical model is used to predict how to adjust the state of a robot to achieve a control target. The embodiment of the present invention uses a state prediction model constructed by the torque state equation, pose state equation, and Jacobian matrix of the robot, and provides a new method for calculating the control increment. By constructing a task space control model based on the Jacobian matrix, pose control and force / torque control are integrated into a unified framework, rather than separately processing the position and force in the joint space, ensuring that the two can work together in the same framework.

[0037] Among them, the Jacobian matrix accurately maps the velocity in the joint space to the task space velocity of the end effector, ensuring high-precision tracking of the robot's pose; while the torque state equation provides accurate force / torque feedback, enabling the control system to adjust in real time to maintain the expected force / torque. The conventional Jacobian matrix plays a crucial role in robotics, such as being used to map the velocity in the joint space to the task space velocity of the end effector. By transposing the Jacobian matrix to obtain the torque in the joint space, it helps to determine the joint torques required to maintain a specific end effector position. The Jacobian matrix of the embodiment of the present invention is constructed based on the Jacobians of the waist and double-arm subsystems of the robot, which can better coordinate the movements of the entire upper body, ensure smooth transitions and synchronous movements between various parts, enabling the robot to quickly respond to environmental changes, such as uneven terrain or the appearance of obstacles, by flexibly adjusting the positions of the waist and arms to maintain stability and safety. Combining this Jacobian matrix, the embodiment of the present invention can handle multiple control targets (such as position accuracy, contact force, etc.) within the same framework, and find the optimal solution through an optimization algorithm, not only improving the flexibility and response speed of the control system, but also enhancing the stability and robustness of the system.

[0038] In specific implementation, a state prediction model based on the Jacobian matrix is used, combined with the torque state equation and pose state equation of the robot, to calculate the adjustment amount (i.e., control increment) of each joint or component required to achieve the control target. Based on the calculated control increment, the torque value required to be applied at each moment can be calculated in combination with dynamic analysis to ensure that these torques are within the specified range. Based on this, the embodiment of the present invention can consider the requirements of both position and force in the task space. In the unified framework, pose control and force control can be dynamically selected according to the task requirements to adapt to different application scenarios. Further, the calculated torque value is converted into specific electrical signals or commands and transmitted to the motor driver, hydraulic system, or other forms of power sources to start the drive control system, and through real-time monitoring and feedback adjustment, ensure that the robot accurately executes the task.

[0039] Through the above process, the embodiments of the present invention can dynamically adjust the control of the robot according to the current motion state and environmental conditions, ensuring good control performance under different operating conditions. For example, during walking, the swing of the waist changes continuously, and the model can be updated in real time to adapt to these changes. The Jacobian matrix of the embodiments of the present invention not only reflects the speed mapping of a single joint, but also considers that the movement of the waist will affect the position and posture of the two arms. Based on the linkage effect between different joints, the robot is controlled to comprehensively describe the robot, which helps to improve the overall control accuracy.

[0040] Further, based on the above embodiments, the embodiments of the present invention also provide another force-position hybrid control method for a robot. In the embodiments of the present invention, a pre-constructed state prediction model is used to determine the control torque of the robot. The embodiments of the present invention mainly illustrate the construction method of the state prediction model. In specific implementation, the embodiments of the present invention solve the desired input of the control target through the state prediction model based on a preset optimization target, obtain the control increment of the robot corresponding to the desired input, and then calculate the control torque of the robot. And map the calculated control torque to the task space corresponding to the control target to generate a joint torque command. Based on the joint torque command, the joints of the robot are driven to control the robot.

[0041] In the embodiments of the present invention, the above optimization target includes the state constraint and control increment constraint corresponding to the robot. In one implementation manner, the embodiments of the present invention pre-construct a force-position hybrid control optimization problem. The reference input is , then the tracking error , the standard form of the model predictive control problem:

[0042]

[0043] (1)

[0044]

[0045] Wherein: in formula (1) is the prediction step length, , and are positive definite diagonal matrices. In the equality constraint (2), , , is a constant approximation of the preset force-position augmented state equation within the prediction time domain, used to linearize the prediction system state , , , sequence. The inequality constraints (3) and (4) respectively impose constraints on the system state and the control torque increment. is the optimal control output sequence when the minimum constraint target is (1), and nearly is used for the control system, then the system control torque:

[0046] (6)

[0047] Where: and That is, the feedforward torque of the state prediction model, which is calculated from the system state under the equality constraint (2) in the optimal control output sequence is calculated and obtained.

[0048] Correspondingly, the embodiment of the present invention controls the preliminary state of the robot based on the control increment corresponding to the control target; tracks the current state of the robot to determine the optimal output sequence of the robot; determines the feedforward torque corresponding to the state prediction model based on the optimal output sequence; and calculates the control torque of the robot based on the feedforward torque and the control increment. Specifically, refer to the following steps:

[0049] The first step: Given the desired input containing the desired pose and force / torque information , set the control parameters , and .

[0050] The second step: Use the optimization problems (1)-(5) to calculate the optimal solution within the prediction time .

[0051] The third step: Use the optimization model (2) to calculate the desired robot state at the next moment.

[0052] The fourth step: Use the model (6), through the optimal solution and the desired robot state at the next moment to calculate the driving torque of the robot, and drive the robot to achieve tracking of the desired input .

[0053] The fifth step: Feed back the current state of the robot to the optimization problems (1)-(5).

[0054] The sixth step: Repeat the process from the first step to the fifth step until the control ends.

[0055] Correspondingly, Figure 2The figure shows the block diagram of the force-position hybrid control of a humanoid robot. Through the above steps, the embodiments of the present invention can achieve the force-position hybrid control of high-degree-of-freedom robots such as humanoid robots, and are applicable to the coordinated control of high-degree-of-freedom robots such as humanoid robots. Based on this, the embodiments of the present invention realize robot pose tracking and force / torque tracking in a unified control framework, fully consider the constraints of the control quantity, effectively confine the control quantity within the specified range, and improve the robustness of the control method. Moreover, the embodiments of the present invention are simple and convenient to implement, and are easy to implement and solve.

[0056] Further, in order to facilitate the understanding of the parameter meanings in the above formula, the following steps will illustrate the construction method of the state prediction model. Figure 3 The figure shows the flowchart of another force-position hybrid control method for a robot provided by the embodiments of the present invention. In specific implementation, the embodiments of the present invention decompose the dynamics into two parts: nonlinear feedforward dynamics and linear incremental dynamics. Among them, the linear incremental dynamics is an incremental system obtained based on the time-domain equivalence (TDE) method and is used to generate the state prediction model. Refer to Figure 3 , this method includes the following steps:

[0057] Step S202, obtain a preset dynamics model.

[0058] The dynamics model of the embodiments of the present invention includes feedforward torque and model predictive control torque. The feedforward torque includes the Jacobian matrix of the robot. Among them, the embodiments of the present invention rewrite the preset dynamics model into a feedforward-incremental dynamics model to form a dynamics model for constructing the state prediction model. Specifically, the preset dynamics model is as follows:

[0059]

[0060] The feedforward-incremental dynamics model is as follows:

[0061]

[0062] Among them, : The joint torque composed of the inertia matrix (M(q)), the Coriolis force and the external force . : The joint torque composed of the friction force and other torques . : The joint torque composed of the Jacobian matrix . : The total joint torque, composed of , and .

[0063] In specific implementation, the robot is divided into three parts: the left arm, the right arm, and the waist. First, a coordinate system for the dual-arm robot with a waist is established. Figure 4 The schematic diagram of the coordinate system of the robot is shown. The construction method of the Jacobian matrix in the embodiment of the present invention is as follows:

[0064] 1) Based on the position of the robot, a world coordinate system of the robot is constructed. Based on the positions of the two arms and the waist of the robot, a base coordinate system of the robot is constructed.

[0065] The O coordinate system is the world coordinate system of the dual-arm robot system and at the same time serves as the base coordinate system of the waist; the B coordinate system is the base coordinate system of the left arm and the right arm and at the same time serves as the output coordinate system of the waist. The movement of the waist affects the pose and speed of the base coordinate system B relative to the world coordinate system O, and further affects the target pose and speed of the two arms.

[0066] Among them, the attitude transformation matrix of the end of the left arm relative to the base coordinate system B is ; the position of the end of the left arm relative to the base coordinate system B is ; the attitude transformation matrix of the end of the right arm relative to the base coordinate system B is ; the position of the end of the right arm relative to the base coordinate system B is ; the transformation matrix of the base coordinate system B relative to the world coordinate system O is .

[0067] 2) Determine the Jacobian matrix of the ends of the two arms of the robot relative to the base coordinate system, and the Jacobian matrix of the waist of the base coordinate system relative to the world coordinate system.

[0068] The Jacobian description of the end of the left arm or the right arm relative to the base coordinate system B is:

[0069] , (11)

[0070] Among them: is the speed of the end of the left arm or the right arm relative to the base coordinate system B; is the Jacobian matrix of the end of the left arm or the right arm relative to the base coordinate system B; is the angular velocity of the left arm or the right arm joint.

[0071] The end of the waist is located in the base coordinate system B. The Jacobian description of the base coordinate system B relative to the world coordinate system O is:

[0072] (12)

[0073] Among them: is the speed of the coordinate system B relative to the world coordinate system O; is the Jacobian matrix of the coordinate system B relative to the world coordinate system O; is the angular velocity of the waist joint.

[0074] Definition 1: Given matrix and a column vector of dimension , define the operation ;

[0075] Property 1: For matrix , a column vector of dimension and a column vector of dimension , it satisfies .

[0076] Proof: Let denote the -th element of the column vector of dimension , that is: , then:

[0077] (13)

[0078] 3) According to the velocity transmission relationship of the robot's waist to the arms, combined with the arm Jacobian matrix and the waist Jacobian matrix, construct the robot's Jacobian matrix.

[0079] Since the movement of the waist affects the velocity of the manipulator relative to the world coordinate system O, according to the velocity transmission relationship between the waist and the left arm, we have:

[0080] (14)

[0081] According to Definition 1 and its Property 1, we know that:

[0082]

[0083] Then: (15)

[0084] Similarly, we have: (16)

[0085] From (15) and (16), we get:

[0086] where:

[0087] , ,

[0088] Then, the velocity described in the end-effector coordinate system of the two-arm robot is:

[0089]

[0090] Among them:

[0091]

[0092] That is, the Jacobian described in the end - effector coordinate system is: In summary, the Jacobian matrix of the robot is obtained.

[0093] Step S204: Introduce an incremental control torque into the model predictive control torque of the dynamic model to construct an incremental system of the dynamic model.

[0094] Among them, in the embodiment of the present invention, the sampling period is also determined as the delay time, and a deviation is introduced into the incremental system. The delay time is selected as the sampling period , and the dynamics described in (7) is differentiated at time t and time to obtain:

[0095] (17)

[0096] Among them: , is the increment of the control torque (i.e., the above - mentioned incremental control torque), is the deviation introduced by approximating the nonlinear system as an incremental system.

[0097]

[0098]

[0099]

[0100] When the sampling period is small:

[0101]

[0102]

[0103]

[0104] Then , Equation (17) is further simplified to:

[0105]

[0106] Step S206: Based on the incremental system, calculate the pose state equation and torque state equation of the robot to generate a force - pose augmented state equation.

[0107] In specific implementation, the incremental system is discretized to obtain the discrete state equation of the robot; based on the discrete state equation, the pose state equation of the robot is determined; based on the preset force closed-loop gain diagonal matrix, the generalized force / moment state equation of the robot is determined; the pose state equation and the generalized force / moment state equation are combined to obtain the force-pose augmented state equation of the robot.

[0108] Specifically, the Euler method is used to obtain the offline description of Equation (18) for calculating the joint state and outputting the end-effector velocity . Since the sampling time is small enough, the discrete error can be ignored. The discrete state equation is as follows:

[0109]

[0110] Where:

[0111] , , ,

[0112] Considering the pose closed-loop, the control state is defined, and the output state . Equations (19) and (20) are further rewritten as the pose state equation:

[0113]

[0114] Where: , , . Pose tracking is performed in the end-effector coordinate system, and the given is the full-dimensional position tracking state equation.

[0115] Let the environmental stiffness be . From the stiffness environment model, the discrete state equation of the end-effector force / moment of the dual-arm robot is:

[0116] (23)

[0117] Considering the force / moment tracking desired task objective , let the desired output , f ext be the feedback force / moment, be the desired force / moment, and K f be the force closed-loop gain diagonal matrix. Then:

[0118] (24)

[0119] Define the generalized feedback force / moment: ; the generalized desired force / moment: ; Let , the output equation can be obtained as follows:

[0120] (25)

[0121] Where: .

[0122] From (23) and (24), the generalized force / moment state equation is obtained:

[0123]

[0124] Where: .

[0125] Combining (21), (22), (26), and (27), the force-position augmented state equation is formed:

[0126] (28)

[0127] Where: , , ;

[0128] , , .

[0129] Among them, A, B, and C are the state matrix, input matrix, and output matrix of the state equation respectively, and their other specific meanings have been explained in the above derivation process.

[0130] In step S208, a selection matrix is introduced into the force-position augmented state equation, and combined with the feedforward torque of the dynamic model, a state prediction model is constructed.

[0131] Considering that pose control and force / torque control in the same dimension cannot exist simultaneously and are mutually exclusive. Therefore, and cannot exist in the same dimension. To eliminate the non-uniform dimension and ensure that only pose control or force / torque control exists in a certain dimension, for this, an embodiment of the present invention introduces a selection matrix, and the selection matrix is used to indicate the control dimension of the robot. The number of rows and columns of the selection matrix is determined based on the control dimension of the robot, and the control dimension includes one of the following hybrid dimensions: the pose control dimension of the robot, the force or torque control dimension; the hybrid dimension includes the translational dimension and rotational dimension of the robot's two arms; among them, each row of the selection matrix has and only one element as 1, and the others are 0; each column has at most one element as 1, and the others are 0.

[0132] Specifically, the control dimensions of the robot are described through the following steps: Force-position hybrid control is a control strategy for a robot system, aiming to simultaneously control the force / torque and pose at the end of the robot. In certain directions, it ensures that the end of the robot can accurately track a predetermined trajectory or reach a specific pose, and in other directions, it ensures that the robot can apply or maintain a specific force / torque. For a certain task described in the world coordinate system, especially a curved surface task, it is impossible to achieve force / torque or pose tracking in a specific direction. Therefore, the task is converted to be described in the end-effector coordinate system of the two arms to decouple the force / torque and pose control of the task. In addition, due to the diversity and non-uniqueness of attitude descriptions, the position space description operation is not applicable to attitude description operations. To ensure the tracking control of the attitude, the embodiments of the present invention will use speed description to achieve pose tracking control and realize pose tracking through real-time closed-loop planning of the speed field. In the task space of the two-arm robot in the embodiments of the present invention, there are 12 independent control dimensions, which respectively include 3 translational dimensions and 3 rotational dimensions of the left arm, and 3 translational dimensions and 3 rotational dimensions of the right arm.

[0133] According to different control strategies, the control modes are defined as follows:

[0134] Full-dimension pose control: In this mode, all 12 dimensions are used for pose control, that is, the desired spatial position and attitude are achieved by precisely adjusting the motion parameters of each dimension. This control strategy is applicable to task scenarios that require high-precision positioning.

[0135] Full-dimension force / torque control: This mode uses all 12 dimensions for the control of force or torque, aiming to apply a specific force or torque to the external environment. This control method is particularly important in tasks that require dynamic interaction with the environment, such as in the operation of flexible objects or complex contact tasks.

[0136] Full-dimension force-position hybrid control: In this mode, the 12 dimensions are divided into parts for pose control and parts for force / torque control. By combining the advantages of position and force control, this mode can provide higher flexibility and adaptability in complex tasks, especially in scenarios that require simultaneous consideration of precise positioning and force feedback.

[0137] To achieve pose tracking and force / torque tracking, the following two tasks are defined:

[0138] (1) Task space pose tracking task .

[0139] Wherein, is the pose velocity vector described in the end-effector coordinate system sub-vector of, vector dimension , For the desired pose of the workspace corresponding to denote the tracking error is a positive definite diagonal damping matrix.

[0140] (2) Task space force / moment tracking task .

[0141] Among them, is the force / moment vector described in the end coordinate system sub-vector of vector dimension , for the corresponding desired force / moment in the task space, denote the force / moment tracking error, is a positive definite diagonal stiffness matrix.

[0142] From the definitions of the above two tasks, it can be seen that both the pose description and the force / moment description are described in the end coordinate system of the robot. Obviously, it is impossible to simultaneously achieve the pose tracking goal and the force / moment tracking goal in the same dimension. In a certain dimension, it can only be set to pose tracking or force / moment tracking. That is: the force / moment control dimension is orthogonal to the pose control dimension. According to the definition of the control mode, full-dimension pose control: ; full-dimension force / moment control: ; full-dimension force-position hybrid control: .

[0143] Correspondingly, the embodiment of the present invention introduces a dimension selection matrix into the force-position augmented state equation corresponding to the dynamic model to ensure that there is only pose control or force / moment control in a certain dimension. Corresponding to the above dimensions, introduce rows, columns selection matrix , then rewrite Equation (28) as:

[0144]

[0145] Where: , satisfies the following constraints:

[0146] (1) Each row of has exactly one element as 1 and the others as 0;

[0147] (2) Each column of has at most one element as 1 and the others as 0.

[0148] In summary, Equations (29), (8), (9) and (10) together constitute the feedforward-incremental system model, where ​and As a feedforward term, The model predicts the force-position hybrid control output. Thus, the state prediction model is constructed. , , For (29) , , Constant approximation in the prediction time domain. In summary, the embodiments of the present invention can realize force-position hybrid control of high-degree-of-freedom robots such as humanoid robots. Posture tracking and force / torque tracking are realized using a unified framework. This method not only solves the problem of lack of a unified framework and integrated control in the prior art, but also adds the necessary constraint and restriction mechanism, effectively constrains the control amount within a specified range, improves the adaptability and safety of humanoid robots in complex environments, improves the robustness of the system, and enhances the stability and reliability of the system.

[0149] Furthermore, based on the above embodiment, the embodiment of the present invention also provides a force-position hybrid control device for a robot, Figure 5 A schematic diagram of the structure of a force-position hybrid control device for a robot provided by an embodiment of the present invention is shown. Figure 5 The device includes: a data acquisition module 100, used to obtain the expected input of the robot to the preset control mode operation; a data processing module 200, used to determine the control target of the robot based on the expected input; the control target includes at least one of the following control mixed modes: posture control, force or torque control; a calculation module 300, used to calculate the robot control increment corresponding to the control target through a preset state prediction model; wherein the state prediction model includes the Jacobian matrix of the robot, and the Jacobian matrix is ​​constructed based on the Jacobian of the waist and double-arm subsystems of the robot; an execution module 400, used to determine the control torque of the robot based on the control increment; a control module 500, used to drive and control the robot according to the control torque so that the robot meets the expected input.

[0150] A force-position hybrid control device for a robot provided in an embodiment of the present invention has the same technical features as a force-position hybrid control method for a robot provided in the above embodiment, and therefore can also solve the same technical problems and achieve the same technical effects.

[0151] Furthermore, an embodiment of the present invention further provides a force-position hybrid control device for another robot. The calculation module 300 is further configured to obtain a preset dynamic model. The dynamic model includes a feedforward torque and a model predictive control torque. The feedforward torque includes the Jacobian matrix of the robot. An incremental control torque is introduced into the model predictive control torque of the dynamic model to construct an incremental system of the dynamic model. Based on the incremental system, the pose state equation and the torque state equation of the robot are calculated to generate a force-position augmented state equation. A selection matrix is introduced into the force-position augmented state equation and combined with the feedforward torque of the dynamic model to construct a state prediction model. The selection matrix is used to select the control dimension of the robot. The number of rows and columns of the selection matrix is determined based on the control dimension of the robot. The control dimension includes one of the following hybrid dimensions: the pose control dimension of the robot, the force or torque control dimension. The hybrid dimension includes the translational dimension and the rotational dimension of the robot's two arms. Each row of the selection matrix has exactly one element as 1 and the others as 0. Each column has at most one element as 1 and the others as 0.

[0152] The calculation module 300 is further configured to discretize the incremental system to obtain the discrete state equation of the robot. Based on the discrete state equation, the pose state equation of the robot is determined. Based on a preset force closed-loop gain diagonal matrix, the generalized torque state equation of the robot is determined. The pose state equation and the generalized torque state equation are combined to obtain the force-position augmented state equation of the robot.

[0153] The calculation module 300 is further configured to construct the Jacobian matrix of the robot, including: constructing the world coordinate system of the robot based on the position of the robot, and constructing the base coordinate system of the robot based on the positions of the two arms and the waist of the robot; determining the Jacobian matrix of the two arms of the robot's two-arm end relative to the base coordinate system and the Jacobian matrix of the waist relative to the world coordinate system; constructing the Jacobian matrix of the robot according to the speed transmission relationship of the waist to the two arms and combining the Jacobian matrix of the two arms and the Jacobian matrix of the waist.

[0154] The calculation module 300 is further configured to solve the desired input of the control target based on a preset optimization target through the state prediction model to obtain the control increment of the robot corresponding to the desired input. The optimization target includes the state constraint and the control increment constraint corresponding to the robot.

[0155] The execution module 400 is further configured to control the initial state of the robot based on the control increment corresponding to the control target; track the current state of the robot to determine the optimal output sequence of the robot; determine the feedforward torque corresponding to the state prediction model based on the optimal output sequence; calculate the control torque of the robot based on the feedforward torque and the control increment.

[0156] The above-mentioned calculation module 300 is further configured to obtain a linear incremental system equation by using the time-delay theory. The above-mentioned control module 500 is further configured to map the control torque to the task space corresponding to the control target to generate a joint torque command; and perform joint drive on the robot based on the joint torque command.

[0157] An embodiment of the present invention further provides an electronic device, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the computer program, the steps of any of the above-mentioned Figures 1 to 3 methods are implemented. An embodiment of the present invention further provides a computer-readable storage medium, on which a computer program is stored. When the computer program is run by a processor, the steps of any of the above-mentioned Figures 1 to 3 methods are executed. An embodiment of the present invention further provides a schematic structural diagram of an electronic device. As Figure 6 shown, it is a schematic structural diagram of the electronic device. Among them, the electronic device includes a processor 61 and a memory 60. The memory 60 stores computer-executable instructions that can be executed by the processor 61. The processor 61 executes the computer-executable instructions to implement any of the above-mentioned Figures 1 to 3 methods. In Figure 6 the illustrated embodiment, the electronic device further includes a bus 62 and a communication interface 63. Among them, the processor 61, the communication interface 63, and the memory 60 are connected through the bus 62. Among them, the memory 60 may include a high-speed random access memory (RAM, Random Access Memory), and may also include a non-volatile memory, such as at least one disk memory. Through at least one communication interface 63 (which can be wired or wireless), a communication connection between the system network element and at least one other network element is realized. The Internet, wide area network, local area network, metropolitan area network, etc. can be used. The bus 62 may be an ISA (Industry Standard Architecture, industrial standard architecture) bus, a PCI (Peripheral Component Interconnect, peripheral component interconnect standard) bus, or an EISA (Extended Industry Standard Architecture, extended industrial standard structure) bus, etc. The bus 62 can be divided into an address bus, a data bus, a control bus, etc. For the convenience of representation, Figure 6It is represented only by a bidirectional arrow, but it does not mean that there is only one bus or one type of bus. The processor 61 may be an integrated circuit chip with signal processing capabilities. In the implementation process, the steps of the above method can be completed by the integrated logic circuit of the hardware in the processor 61 or the instructions in the form of software. The above-mentioned processor 61 may be a general-purpose processor, including a central processing unit (CPU for short), a network processor (NP for short), etc.; it may also be a digital signal processor (DSP for short), an application-specific integrated circuit (ASIC for short), a field-programmable gate array (FPGA for short), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc. The steps of the method disclosed in combination with the embodiments of the present application can be directly embodied as being executed and completed by a hardware decoding processor, or executed and completed by a combination of the hardware and software modules in the decoding processor. The software module may be located in a mature storage medium in the art such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory, or an electrically erasable programmable memory, a register, etc. This storage medium is located in the memory, and the processor 61 reads the information in the memory and combines its hardware to complete the foregoing Figures 1 to 3 any of the methods shown.

[0158] A computer program product of a force-position hybrid control method and device for a robot provided by an embodiment of the present invention includes a computer-readable storage medium storing program codes. The instructions included in the program codes can be used to execute the method described in the foregoing method embodiments. For specific implementation, reference can be made to the method embodiments and will not be elaborated herein. Those skilled in the art can clearly understand that for the convenience and brevity of description, the specific working processes of the above-described system and device can refer to the corresponding processes in the foregoing method embodiments and will not be elaborated herein. Additionally, in the description of the embodiments of the present invention, unless otherwise clearly specified and limited, the terms "installation", "connection", and "coupling" shall be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be directly connected or indirectly connected through an intermediate medium, and it can be the communication inside two components. For those skilled in the art, the specific meanings of the above terms in the present invention can be understood according to specific situations. If the function is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which can be a personal computer, an electronic device, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The foregoing storage medium includes: USB flash drives, mobile hard disks, read-only memories (ROMs), random access memories (RAMs), magnetic disks, or optical discs, etc., which can store program codes. In the description of the present invention, it should be noted that the orientation or positional relationship indicated by the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation of the present invention. In addition, the terms "first", "second", and "third" are only used for descriptive purposes and cannot be understood as indicating or implying relative importance.

[0159] Finally, it should be noted that the above embodiments are only specific implementation manners of the present invention, used to illustrate the technical solutions of the present invention, rather than limiting it. The protection scope of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that any person skilled in the technical field can still modify the technical solutions recorded in the foregoing embodiments, or can easily think of changes, or perform equivalent replacements on some of the technical features; and these modifications, changes or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be covered by the protection scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims

1. A force-position hybrid control method for a robot, characterized in that: The method comprises: Obtaining the expected input of the robot by the preset control mode operation; Based on the expected input, determining a control target of the robot; the control target includes at least a hybrid mode of one of the following controls: posture control, force or torque control; The robot control increment corresponding to the control target is calculated by a preset state prediction model; wherein the state prediction model is constructed based on the robot's torque state equation, posture state equation and Jacobian matrix, and the Jacobian matrix is ​​constructed based on the Jacobian of the robot's waist and dual-arm subsystems; Determining a control torque of the robot based on the control increment; According to the control torque, driving and controlling the robot so that the robot meets the expected input; The method for constructing the state prediction model comprises: Acquire a preset dynamic model; the dynamic model includes a feedforward torque and a model predictive control torque, and the feedforward torque includes a Jacobian matrix of the robot; Introducing an incremental control torque into the model prediction control torque of the dynamic model to construct an incremental system of the dynamic model; Based on the incremental system, the posture state equation and the torque state equation of the robot are calculated to generate the force-position augmented state equation; A selection matrix is ​​introduced into the force-position augmented state equation, and a state prediction model is constructed in combination with the feedforward torque of the dynamic model; wherein the selection matrix is ​​used to select the control dimension of the robot.

2. The method according to claim 1, characterized in that The step of calculating the posture state equation and the torque state equation of the robot based on the incremental system and generating the force-position augmented state equation comprises: Discretizing the incremental system to obtain a discrete state equation of the robot; Based on the discrete state equation, determining the position and posture state equation of the robot; Based on the preset force closed-loop gain diagonal matrix, the generalized torque state equation of the robot is determined; The posture state equation and the generalized torque state equation are combined to obtain the force-position augmented state equation of the robot.

3. The method according to claim 1, characterized in that The method for constructing the Jacobian matrix of the robot includes: Constructing a world coordinate system of the robot based on the position of the robot, and constructing a base coordinate system of the robot based on the positions of both arms and the waist of the robot; Determine the Jacobian matrix of the double arms of the robot relative to the base coordinate system, and the Jacobian matrix of the waist of the base coordinate system relative to the world coordinate system; According to the velocity transmission relationship between the waist of the robot and the double arms, the Jacobian matrix of the robot is constructed by combining the Jacobian matrix of the double arms and the Jacobian matrix of the waist.

4. The method according to claim 1, characterized in that The step of calculating the robot control increment corresponding to the control target by using a preset state prediction model includes: The state prediction model is used to solve the expected input of the control target based on a preset optimization target to obtain the robot control increment corresponding to the expected input; wherein the optimization target includes the state constraints and control increment constraints corresponding to the robot.

5. The method according to claim 1, characterized in that The step of determining the control torque of the robot based on the control increment comprises: Controlling a preliminary state of the robot based on a control increment corresponding to the control target; Tracking the current state of the robot to determine the optimal output sequence of the robot; Based on the optimal output sequence, determining a feedforward torque corresponding to the state prediction model; The control torque of the robot is calculated based on the feedforward torque and the control increment.

6. The method according to claim 1, characterized in that The method further comprises: Using time delay theory, the linear increment system equations are obtained.

7. The method according to claim 1, characterized in that The number of rows and columns of the selection matrix is ​​determined based on the control dimension of the robot, and the control dimension includes a mixed dimension of one of the following: a posture control dimension and a force or torque control dimension of the robot; the mixed dimension includes a translation dimension and a rotation dimension of the robot's arms; Each row of the selection matrix has only one element that is 1, and the others are 0; each column has at most one element that is 1, and the others are 0.

8. The method according to claim 1, characterized in that The step of driving and controlling the robot according to the control torque comprises: Mapping the control torque to the task space corresponding to the control target to generate a joint torque command; The joints of the robot are driven based on the joint torque commands.

9. A force-position hybrid control device for a robot, characterized in that: The device comprises: A data acquisition module, used to obtain the expected input of the robot by the preset control mode operation; A data processing module, configured to determine a control target of the robot based on the expected input; the control target includes at least a hybrid mode of one of the following controls: posture control, force or torque control; A calculation module, used to calculate the robot control increment corresponding to the control target through a preset state prediction model; wherein the state prediction model includes the Jacobian matrix of the robot, and the Jacobian matrix is ​​constructed based on the Jacobian of the waist and dual-arm subsystems of the robot; An execution module, configured to determine a control torque of the robot based on the control increment; A control module, configured to drive and control the robot according to the control torque so that the robot meets the expected input; The calculation module is also used to obtain a preset dynamic model; the dynamic model includes a feedforward torque and a model prediction control torque, and the feedforward torque includes the Jacobian matrix of the robot; an incremental control torque is introduced into the model prediction control torque of the dynamic model to construct an incremental system of the dynamic model; based on the incremental system, the posture state equation and the torque state equation of the robot are calculated to generate a force-position augmented state equation; a selection matrix is ​​introduced into the force-position augmented state equation, and a state prediction model is constructed in combination with the feedforward torque of the dynamic model; wherein the selection matrix is ​​used to select the control dimension of the robot.

Images