Force-position hybrid control method and device of robot

Abstract

The invention provides a force-position hybrid control method and device for a robot, and relates to the technical field of robot control, and the method comprises the steps: operating expected input of the robot based on a preset control mode, and determining a control target of the robot; and calculating a robot control increment corresponding to the control target through a preset state prediction model so as to determine a control torque of the robot, thereby carrying out driving control on the robot. Wherein the control target at least comprises a mixed mode of one of the following controls: pose control and force or torque control; the state prediction model is constructed based on a moment state equation and a pose state equation of the robot and a Jacobian matrix of the robot, and the Jacobian matrix is constructed based on Jacobian of waist and double arm subsystems of the robot. According to the invention, pose tracking and force / torque tracking of the robot under a unified framework can be realized, force and position hybrid control of high-degree-of-freedom robots such as humanoid robots can be realized, and complex task requirements can be met more comprehensively.

Description

Technical Field

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

[0002] With the development of robotics 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, use a position controller to track the desired position and posture, and use a force controller to adjust the force or torque when in contact with the environment. In existing schemes, position control and force control are usually designed independently and are not integrated in a unified framework. This makes the control system complex and difficult to optimize, especially when dealing with complex interactive tasks. Moreover, when the position controller and the force controller work in parallel, if the position controller tries to accurately track a target position, and the force controller needs to adjust the contact force to adapt to changes in the environment, there may be a conflict between the two, resulting in poor control effect. Summary of the invention

[0003] In order to solve the above problems, an embodiment of the present invention proposes a force-position hybrid control method and device for a robot, which can realize the robot's posture tracking and force / torque tracking under a unified framework, realize force-position hybrid control of high-degree-of-freedom robots such as humanoid robots, and can more comprehensively respond to complex task requirements.

[0004] In the first aspect, an embodiment of the present invention provides a force-position hybrid control method for a robot, the method comprising: obtaining the expected input of a preset control mode operation to the robot; based on the expected input, determining the control target of the robot; the control target includes at least one of the following control hybrid modes: posture control, force or torque control; calculating the robot control increment corresponding to the control target through 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; based on the control increment, determining the control torque of the robot; according to the control torque, driving and controlling the robot so that the robot meets the expected input.

[0005] In combination with the first aspect, an embodiment of the present invention provides a first implementation manner of the first aspect, wherein a method for constructing a state prediction model comprises: obtaining a preset dynamic model; the dynamic model comprises a feedforward torque and a model prediction control torque, and the feedforward torque comprises the 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, calculating the posture state equation and the torque state equation of the robot to generate a force-position augmented state equation; introducing a selection matrix into the force-position augmented state equation, and constructing a state prediction model in combination with the feedforward torque of the dynamic model; wherein the selection matrix is ​​used to select the control dimension of the robot.

[0006] In combination with the first aspect, an embodiment of the present invention provides a second implementation of the first aspect, wherein, based on the incremental system, the steps of calculating the posture state equation and the torque state equation of the robot to generate the force-position augmented state equation include: discretizing the incremental system to obtain the discrete state equation of the robot; determining the posture 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; and combining the posture state equation and the generalized torque state equation to obtain the force-position augmented state equation of the robot.

[0007] In combination with the first aspect, an embodiment of the present invention provides a third implementation of the first aspect, wherein a method for constructing the Jacobian matrix of a 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 position of both arms and the waist of the robot; determining the Jacobian matrix of both 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; and constructing the Jacobian matrix of the robot based on the speed transfer relationship between the waist of the robot and the arms, combining the Jacobian matrix of the both arms and the Jacobian matrix of the waist.

[0008] In combination with the first aspect, an embodiment of the present invention provides a fourth implementation of the first aspect, wherein the step of calculating the robot control increment corresponding to the control target through a preset state prediction model includes: solving the expected input of the control target based on a preset optimization target through the state prediction model 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.

[0009] In combination with the first aspect, an embodiment of the present invention provides a fifth implementation of the first aspect, wherein the step of determining the control torque of the robot based on the control increment includes: controlling the initial 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 of the first aspect, wherein the method further includes: adopting time delay theory to obtain a linear incremental system equation.

[0011] In combination with the first aspect, an embodiment of the present invention provides a seventh implementation of the first aspect, wherein 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: the posture control dimension, force or torque control dimension of the robot; the mixed dimension includes the translation dimension and rotation dimension of the robot's arms; wherein each row of the selection matrix has one and 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.

[0012] In combination with the first aspect, an embodiment of the present invention provides an eighth implementation of the first aspect, wherein 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 driving the joints of the robot based on the joint torque command.

[0013] In the second aspect, an embodiment of the present invention provides a control device for a robot, wherein the device includes: a data acquisition module, used to obtain the expected input of the robot to the preset control mode operation; a data processing module, 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, 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, the Jacobian matrix of the robot's waist and Jacobian construction of the dual-arm subsystem; an execution module, used to determine the control torque of the robot based on the control increment; a control module, used to drive and control the robot according to the control torque so that the robot meets the expected 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 realize force-position hybrid control of high-degree-of-freedom robots such as humanoid robots, and can more comprehensively cope with complex task requirements. Moreover, the embodiments of the present invention use the velocity description of the Jacobian matrix to realize the posture tracking of the robot, which can more accurately predict and control the posture changes of the robot. Combined with the pre-built state prediction model to calculate the control torque of the robot, the posture tracking and force / torque tracking of the robot can be realized under a unified framework. Not only the flexibility and response speed of the control system are improved, but also the stability and robustness of the system are enhanced.

[0015] Other features and advantages of the present invention will be described in the following description, and partly become apparent from the description, or understood by practicing the present invention. The purpose and other advantages of the present invention are realized and obtained by the structures particularly pointed out in the description, claims and drawings.

[0016] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, preferred embodiments are given below and described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order to more clearly illustrate the specific implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0018] Figure 1 A flow chart of a force-position hybrid control method of a robot provided by an embodiment of the present invention is shown; Figure 2 A block diagram of a force-position hybrid control structure of a humanoid robot provided by an embodiment of the present invention is shown; Figure 3 A flow chart of a force-position hybrid control method of a robot provided by an embodiment of the present invention is shown; Figure 4 A schematic diagram of a coordinate system of a robot provided by an embodiment of the present invention is shown; Figure 5 A schematic structural diagram of a force-position hybrid control device for a robot provided by an embodiment of the present invention is shown; Figure 6 A schematic structural diagram of an electronic device provided by an embodiment of the present invention is shown. DETAILED DESCRIPTION

[0019] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution of the present invention will be clearly and completely described in combination with the embodiments below. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0020] In the existing robot force and position control methods, the focus is mainly on the end control of the robot arm or special equipment (such as nucleic acid sampling robots) in specific application scenarios. However, the existing technology usually designs the position controller and the force controller separately, and simply combines the two to achieve force and position hybrid control. This approach fails to implement posture control and force / torque control at the same time in a unified framework, resulting in an inflexible control strategy and difficulty in adapting to complex multi-degree-of-freedom robot systems. In addition, the existing technology combines the position controller and the force controller to ensure that the end of the robot arm moves along a specified trajectory. The flexibility is limited by the preset task stage and specific application scenarios, and cannot meet the needs of a wider range of and dynamically changing tasks, and has a limited scope of application.

[0021] In this regard, an embodiment of the present invention provides a force-position hybrid control method and device for a robot, which integrates posture control (such as position and posture) and force / torque control into the same control framework, rather than simply using two independent controllers in parallel. The embodiment of the present invention allows multiple control requirements to be processed simultaneously within the same control, improving the flexibility and response speed of the system. Moreover, the embodiment of the present invention can ensure the effective constraint of the control quantity, significantly improving the safety and reliability of the robot operation.

[0022] For ease of understanding, a force-position hybrid control method for a robot provided in an embodiment of the present invention is first described. Figure 1 A flow chart of a force-position hybrid control method for a robot provided by an embodiment of the present invention is shown, referring to Figure 1 , the method comprises the following steps: Step S102, obtaining the expected input of the preset control mode operation to the robot.

[0023] Step S104, determining the control target of the robot based on the expected input.

[0024] First, determine the specific task or action that the user or system wants the robot to perform, such as moving to a specific location, grasping an object, etc., and then convert the high-level desired input into specific, quantifiable control goals, including but not limited to position, posture, force or torque values.

[0025] In the force-position hybrid control scenario of the robot, the form of the expected input and the corresponding control target will be different according to the requirements of different application scenarios. The control target of the embodiment of the present invention includes at least one of the following hybrid modes of control: posture control, force or torque control. In specific implementation, some scenarios perform pure torque control on the robot, and the expected input directly specifies the force or torque to be applied by the robot end effector or joint, 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 show a certain degree of compliance on the contact surface, such as providing appropriate grip force without damaging the object when grasping fragile objects. In some scenarios, for pure posture control of the robot, the expected input includes position (x, y, z coordinates) and posture (rotation angle, usually expressed in quaternions or Euler angles). For complex action sequences, it may also be necessary to provide a preset trajectory or a series of key points as a reference. Correspondingly, it is necessary to set reasonable speed and acceleration limits to ensure smooth and safe operation and ensure that the robot end effector can accurately reach the specified position and posture. In some scenarios, the robot is subjected to force-position hybrid control, and the expected input includes both force / torque values ​​and posture information. For example, in a welding task, it may be necessary to control the welding gun to move along a predetermined path (posture control) while maintaining a certain welding pressure (force control). Among them, it can also be clarified which dimensions should more strictly follow force control and which dimensions should focus on posture control. Correspondingly, the control strategy can be automatically adjusted according to real-time feedback to cope with changes in the environment or changes in task requirements, and strict force control can be performed on certain degrees of freedom, while focusing on posture accuracy in other degrees of freedom.

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

[0027] Step S106, calculating the robot control increment corresponding to the control target through a preset state prediction model.

[0028] Step S108, determining the control torque of the robot based on the control increment. Step S110, driving and controlling the robot according to the control torque so that the robot meets the expected input.

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

[0030] 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 posture; and the torque state equation provides accurate force / torque feedback, allowing the control system to adjust in real time to maintain the expected force / torque. Conventional Jacobian matrices play a vital role in robotics, such as being used to map the velocity in the joint space to the task space velocity of the end effector. The transposition of the Jacobian matrix is ​​converted into the torque in the joint space to help determine the joint torque required to maintain a specific end effector position. The Jacobian matrix of the embodiment of the present invention is based on the Jacobian construction of the waist and double-arm subsystem of the robot, which can better coordinate the movements of the entire upper body, ensure smooth transitions and synchronous movements between various parts, so that the robot can quickly respond to environmental changes, such as uneven terrain or obstacles, and maintain stability and safety by flexibly adjusting the position of the waist and arms. Combined with the Jacobian matrix, the embodiment of the present invention can handle multiple control objectives (such as position accuracy, contact force, etc.) in the same framework, and find the best solution through the optimization algorithm, which not only improves the flexibility and response speed of the control system, but also enhances the stability and robustness of the system.

[0031] In the specific implementation, a state prediction model based on the Jacobian matrix is ​​used, combined with the torque state equation and posture 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 the 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 position and force in the task space at the same time. In a unified framework, posture control and force control can be dynamically selected according to task requirements to adapt to different application scenarios. Furthermore, the calculated torque value is converted into a specific electrical signal or command, transmitted to the motor driver, hydraulic system or other form of power source, the drive control system is started, and real-time monitoring and feedback adjustment are performed to ensure that the robot accurately performs the task.

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

[0033] Furthermore, based on the above-mentioned 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 expected input of the control target based on the preset optimization target through the state prediction model, obtain the robot control increment corresponding to the expected 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.

[0034] In the embodiment of the present invention, the above optimization objectives include the state constraints and control increment constraints corresponding to the robot. In one embodiment, the embodiment of the present invention pre-constructs the force-position hybrid control optimization problem. The reference input is , then the tracking error , the standard form of the model predictive control problem:

[0035] (1)

[0036] Where: In formula (1) is the prediction step length, , and is a positive definite diagonal matrix. In equation (2) , , It is a constant approximation of the preset force-position augmented state equation in the prediction time domain, which is used to linearize and predict the system state. , , , The inequality constraints (3) and (4) impose constraints on the system state and control torque increment, respectively. is the optimal control output sequence when the minimum constraint objective (1) is is used in the control system, then the system control torque is: (6) in: and That is, the feedforward torque of the state prediction model is determined by the optimal control output sequence The system state calculated by the equality constraint (2) is Calculated and obtained.

[0037] Correspondingly, the embodiment of the present invention controls the initial 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: Step 1: Given the expected input containing the expected pose and force / torque information , set the control parameters , and .

[0038] Step 2: Use optimization problems (1)-(5) to calculate the optimal solution within the prediction time .

[0039] Step 3: Use the optimization model (2) to calculate the expected robot state at the next moment .

[0040] Step 4: Using model (6), find the optimal solution and the expected robot state at the next moment Calculate the robot drive torque , driving the robot to track the desired input .

[0041] Step 5: Feedback the robot's current status , which is fed back into the optimization problems (1)-(5).

[0042] Step 6: Repeat the process from step 1 to step 5 until the control is completed.

[0043] Correspondingly, Figure 2The block diagram of the force-position hybrid control structure of a humanoid robot is shown. Through the above steps, the embodiment of the present invention can realize the force-position hybrid control of a high-degree-of-freedom robot such as a humanoid robot, and is suitable for coordinated control with a high-degree-of-freedom robot such as a humanoid robot. Based on this, the embodiment of the present invention realizes robot posture tracking and force / torque tracking in a unified control framework, considers the control quantity constraint separately, effectively constrains the control quantity within a specified range, and improves the robustness of the control method. Moreover, the embodiment of the present invention is simple and convenient to implement, and is easy to implement and solve.

[0044] Furthermore, in order to facilitate the understanding of the meaning of the parameters in the above formula, the following steps illustrate the method of constructing the state prediction model. Figure 3 The flowchart of another force-position hybrid control method of a robot provided by an embodiment of the present invention is shown. In the specific implementation, the embodiment of the present invention decomposes the dynamics into two parts: nonlinear feedforward dynamics and linear incremental dynamics, wherein the linear incremental dynamics is an incremental system obtained based on the time domain equivalent (TDE) method and is used to generate a state prediction model. Figure 3 , the method comprises the following steps: Step S202, obtaining a preset dynamics model.

[0045] The dynamic model of the embodiment of the present invention includes a feedforward torque and a model predictive control torque, and the feedforward torque includes the Jacobian matrix of the robot. Among them, the embodiment of the present invention forms a dynamic model for constructing a state prediction model by rewriting the preset dynamic model into a feedforward-incremental dynamic model. Specifically, the preset dynamic model is as follows:

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

[0047] in, : From the inertia matrix (M(q)) and the Coriolis force And external forces The joint torque. : By friction and other moments The joint torque. : By the Jacobian matrix The joint torque. : The total joint torque is given by , and composition.

[0048] In the specific implementation, the robot is divided into three parts: left arm, right arm, and waist. First, the coordinate system of the double-arm robot including the waist is established. Figure 4 The schematic diagram of the coordinate system of the robot is shown. The method for constructing the Jacobian matrix of the embodiment of the present invention is as follows: 1) Build the robot's world coordinate system based on the robot's position, and build the robot's base coordinate system based on the robot's arm and waist positions.

[0049] The O coordinate system is the world coordinate system of the dual-arm robot system, and also serves as the basic coordinate system of the waist; the B coordinate system is the base coordinate system of the left and right arms, and also serves as the output coordinate system of the waist; the movement of the waist affects the posture and velocity of the base coordinate system B relative to the world coordinate system O, and thus affects the target posture and velocity of the dual arms.

[0050] Among them, the posture transformation matrix of the left arm end posture relative to the base coordinate system B is: ; The position of the left arm end relative to the base coordinate system B is ; The posture transformation matrix of the right arm end posture relative to the base coordinate system B is ; The position of the right arm end 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 .

[0051] 2) Determine the Jacobian matrix of the robot's dual arms relative to the base coordinate system, as well as the Jacobian matrix of the waist relative to the base coordinate system.

[0052] The Jacobian description of the end of the left or right arm relative to the base coordinate system B is: , (11) in: is the velocity of the end of the left or right arm relative to the base coordinate system B; is the Jacobian matrix of the end of the left or right arm relative to the base coordinate system B; is the left or right arm joint angular velocity.

[0053] 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: (12) in: is the velocity of coordinate system B relative to world coordinate system O; is the Jacobian matrix of coordinate system B relative to the world coordinate system O; is the angular velocity of the waist joint.

[0054] Definition 1: Given matrix and Dimensional column vector , define the operation ; Property 1: For matrix , Dimensional column vector and Dimensional column vector ,satisfy .

[0055] Proof: Use express Dimensional column vector No. elements, namely: ,but: (13) 3) According to the velocity transfer relationship between the robot's waist and arms, the Jacobian matrix of the arms and the Jacobian matrix of the waist are combined to construct the Jacobian matrix of the robot.

[0056] Since the movement of the waist affects the speed of the robot arm relative to the world coordinate system O, according to the speed transfer relationship between the waist and the left arm: (14) According to Definition 1 and Property 1, we know that:

[0057] but: (15) Similarly, there are: (16) From (15) and (16), we can get:

[0058] in: , ,

[0059] Then, the speed described in the dual-arm robot end coordinate system is:

[0060] in:

[0061] That is, the Jacobian described in the terminal coordinate system is: In summary, the Jacobian matrix of the robot is obtained.

[0062] Step S204, introducing an incremental control torque into the model prediction control torque of the dynamic model to construct an incremental system of the dynamic model.

[0063] The embodiment of the present invention also determines the sampling period as the delay time, which introduces a deviation to the incremental system. The delay time is selected as the sampling period , the dynamics described by (7) at time t and Always do the wrong thing: (17) in: , is the increment of the control torque (that is, the incremental control torque mentioned above), The deviation introduced by the nonlinear system approximation is an incremental system.

[0064]

[0065]

[0066]

[0067] When the sampling period When smaller:

[0068]

[0069]

[0070] but , formula (17) is further simplified as:

[0071] Step S206, based on the incremental system, calculate the posture state equation and torque state equation of the robot to generate the force-position augmented state equation.

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

[0073] Specifically, the Euler method is used to obtain equation (18) to obtain the offline description, which is used to calculate the joint state and output end speed Since the sampling time is small enough, the discretization error can be ignored. The discrete state equation is as follows:

[0074] in: , , ,

[0075] Consider the closed loop of posture and define the control state , output status , Equations (19) and (20) are further rewritten as the posture state equation:

[0076] in: , , The position tracking is performed in the terminal coordinate system, and the full-dimensional position tracking state equation is given.

[0077] Assume the environmental stiffness is , the discrete state equation of the dual-arm robot end force / torque obtained from the stiffness environment model is: (twenty three) Consider force / torque tracking of desired mission objectives , let the expected output , f ext Feedback force / torque, is the desired force / torque, K f is the force closed-loop gain diagonal matrix, then: (twenty four) Define the generalized feedback force / torque: ; Generalized expected force / torque: ;make , the output equation is: (25) in: .

[0078] From (23) and (24), we can obtain the generalized force / torque state equation:

[0079] in: .

[0080] Combining (21), (22), (26), and (27), we can form the force-potential augmented state equation: (28) in: , , ; , , .

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

[0082] Step S208, introducing a selection matrix into the force-position augmented state equation, combining the feedforward torque of the dynamics model, and constructing a state prediction model.

[0083] Considering that posture control and force / torque control cannot exist at the same time in the same dimension, they are mutually exclusive. and It is impossible to exist in the same dimension. In order to eliminate the impossible dimension and ensure that only posture control or force / torque control exists in a certain dimension, the embodiment of the present invention introduces a selection matrix, which 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 mixed dimensions: the posture control dimension, force or torque control dimension of the robot; the mixed dimension includes the translation dimension and rotation dimension of the robot's arms; wherein each row of the selection matrix has and only has 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.

[0084] Specifically, the control dimension of the robot is described by the following steps: force-position hybrid control is a control strategy of a robot system, which aims to simultaneously control the force / torque and posture of the robot end. In certain directions, it is ensured that the robot end can accurately track the predetermined trajectory or reach a specific posture, and in other directions, it is ensured that the robot can apply or maintain a specific force / torque. For a task described in the world coordinate system, especially a curved surface task, it is impossible to achieve force / torque or posture tracking in a specific direction. For this reason, the task is converted into a description in the dual-arm end coordinate system, and the force / torque and posture control of the task are decoupled. In addition, due to the diversity and non-uniqueness of the posture description, the position space description operation is not applicable to the posture description operation. In order to ensure the tracking control of the posture, the embodiment of the present invention will use the velocity description to realize the posture tracking control, and realize the posture tracking through the real-time closed-loop planning of the velocity field. In the task space of the dual-arm robot in the embodiment of the present invention, there are 12 independent control dimensions, which include 3 translational dimensions and 3 rotational dimensions of the left arm, and 3 translational dimensions and 3 rotational dimensions of the right arm.

[0085] According to different control strategies, the control mode is defined as follows: Full-dimensional posture control: In this mode, all 12 dimensions are used for posture control, that is, the desired spatial position and posture are achieved by precisely adjusting the motion parameters of each dimension. This control strategy is suitable for mission scenarios that require high-precision positioning.

[0086] Full-dimensional force / torque control: This mode uses all 12 dimensions for force or torque control, 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 the manipulation of flexible objects or complex contact tasks.

[0087] Full-dimensional force-position hybrid control: In this mode, 12 dimensions are divided into two parts for posture control and 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.

[0088] In order to achieve posture tracking and force / torque tracking, the following two tasks are defined: (1) Task space pose tracking task .

[0089] in, is the pose velocity vector described by the end coordinate system The sub-vector of Vector Dimensions , For The corresponding workspace expected pose, represents the tracking error, is a positive definite diagonal damping matrix.

[0090] (2) Task space force / torque tracking task .

[0091] in, is the force / torque vector described by the end coordinate system The sub-vector of Vector Dimensions , For The corresponding expected force / torque in the task space, represents the force / torque tracking error, Positive definite diagonal stiffness matrix.

[0092] From the definitions of the above two tasks, we can see that both the posture description and the force / torque description are described in the robot terminal coordinate system. Obviously, it is impossible to achieve posture tracking and force / torque tracking goals in the same dimension at the same time. Only posture tracking or force / torque tracking can be set in a certain dimension. That is, the force / torque control dimension is orthogonal to the posture control dimension. According to the definition of the control mode, full-dimensional posture control: ; Full-dimensional force / torque control: ; Full-dimensional force-position hybrid control: .

[0093] 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 only posture control or force / torque control exists in a certain dimension. OK, Column selection matrix , then rewrite formula (28) as:

[0094] in: , Subject to the following constraints: (1) Each row of has only one element that is 1, and the others are 0; (2) Each column of has at most one element that is 1, and the rest are 0.

[0095] In summary, equations (29), (8), (9) and (10) together form 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.

[0096] 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 5The 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.

[0097] 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.

[0098] Furthermore, the embodiment of the present invention also provides another force-position hybrid control device for a robot, wherein the above-mentioned calculation module 300 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; the 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; the selection matrix is ​​introduced into the force-position augmented state equation, and the 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. 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 posture control dimension of the robot, the force or torque control dimension; the hybrid dimension includes the translation dimension and the rotation dimension of the robot's arms; wherein each row of the selection matrix has and only has 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.

[0099] The above-mentioned calculation module 300 is also used to discretize the incremental system to obtain the discrete state equation of the robot; based on the discrete state equation, determine the posture state equation of the robot; based on the preset force closed-loop gain diagonal matrix, determine the generalized torque state equation of the robot; combine the posture state equation and the generalized torque state equation to obtain the force-position augmented state equation of the robot.

[0100] The above-mentioned calculation module 300 is also used to construct the Jacobian matrix of the robot, including: constructing the robot's world coordinate system based on the robot's position, and constructing the robot's base coordinate system based on the robot's arm position and waist position; determining the Jacobian matrix 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; and constructing the robot's Jacobian matrix based on the speed transfer relationship from the robot's waist to the two arms, combining the two-arm Jacobian matrix and the waist Jacobian matrix.

[0101] The above-mentioned calculation module 300 is also used to solve the expected input of the control target based on the preset optimization target through the state prediction model 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.

[0102] The above-mentioned execution module 400 is also used 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.

[0103] The calculation module 300 is also used to obtain the linear incremental system equation by using the time delay theory. The control module 500 is also used to map the control torque to the task space corresponding to the control target, generate the joint torque command, and drive the joints of the robot based on the joint torque command.

[0104] An embodiment of the present invention further provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the above Figures 1 to 3 The embodiment of the present invention also provides a computer-readable storage medium, on which a computer program is stored, and the computer program is executed by a processor when it is run. Figures 1 to 3 The embodiment of the present invention also provides a structural diagram of an electronic device, such as Figure 6 FIG. 1 is a schematic diagram of the structure of the electronic device, wherein 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, and the processor 61 executes the computer executable instructions to implement the above Figures 1 to 3 Any of the methods shown. Figure 6In the illustrated embodiment, the electronic device further includes a bus 62 and a communication interface 63, wherein the processor 61, the communication interface 63 and the memory 60 are connected via the bus 62. The memory 60 may include a high-speed random access memory (RAM), and may also include a non-volatile memory, such as at least one disk storage. The communication connection between the system network element and at least one other network element is achieved through at least one communication interface 63 (which may be wired or wireless), and the Internet, wide area network, local area network, metropolitan area network, etc. may be used. The bus 62 may be an ISA (Industry Standard Architecture) bus, a PCI (Peripheral Component Interconnect) bus or an EISA (Extended Industry Standard Architecture) bus, etc. The bus 62 can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 6 Only one bidirectional arrow is used in the diagram, 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, each step of the above method can be completed by the hardware integrated logic circuit in the processor 61 or the instructions in the form of software. The above-mentioned processor 61 can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it can also be a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor. The steps of the method disclosed in the embodiment of the present application can be directly embodied as a hardware decoding processor to be executed, or a combination of hardware and software modules in the decoding processor can be executed. The software module can be located in a random access memory, flash memory, read-only memory, programmable read-only memory or electrically erasable programmable memory, register or other mature storage media in the art. The storage medium is located in the memory, and the processor 61 reads the information in the memory and completes the above-mentioned Figures 1 to 3Any of the methods shown.

[0105] A computer program product of a force-position hybrid control method and device for a robot provided in an embodiment of the present invention includes a computer-readable storage medium storing a program code, and the instructions included in the program code can be used to execute the method described in the previous method embodiment. The specific implementation can refer to the method embodiment, which will not be repeated here. A person skilled in the art can clearly understand that for the convenience and simplicity of description, the specific working process of the system and device described above can refer to the corresponding process in the above method embodiment, which will not be repeated here. In addition, in the description of the embodiment of the present invention, unless otherwise clearly specified and limited, the terms "installation", "connection" and "connection" should 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 it can be indirectly connected through an intermediate medium, or it can be the internal connection of two components. For those skilled in the art, the specific meanings of the above terms in the present invention can be understood in specific circumstances. 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 understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product, which is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, an electronic device, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a disk or an optical disk, and other media that can store program codes. In the description of the present invention, it should be noted that the orientation or position relationship indicated by the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inside", "outside", etc. is based on the orientation or position relationship shown in the accompanying drawings, which 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 on 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.

[0106] Finally, it should be noted that the above embodiments are only specific implementations of the present invention, which are used to illustrate the technical solutions of the present invention, rather than to limit them. The protection scope of the present invention is not limited thereto. Although the present invention is described in detail with reference to the above embodiments, those skilled in the art should understand that any person skilled in the art can still modify the technical solutions recorded in the above embodiments within the technical scope disclosed by the present invention, or can easily think of changes, or make equivalent replacements for some of the technical features therein; 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 be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be based on 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; The robot is driven and controlled according to the control torque so that the robot meets the expected input.

2. The method according to claim 1, characterized in that: 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.

3. The method according to claim 2, 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.

4. 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.

5. 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.

6. 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.

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

8. The method according to claim 2, 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.

9. 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.

10. 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 is used to drive and control the robot according to the control torque so that the robot meets the expected input.

Images