Self-adaptive force-position hybrid control method based on energy tank

Abstract

The invention belongs to the technical field of mechanical arm control algorithms, and particularly relates to a self-adaptive force-position hybrid control method based on an energy tank, which comprises the following steps of: redesigning the update rate of the state of the energy tank based on a barrier function so as to compensate the insufficiency of force-position hybrid control; and the energy tank can be ensured to adjust the system passivity according to the change of the impedance parameter and the null space projection. On the basis of a hierarchical decoupling model, a self-adaptive force and position hybrid controller is designed, the passivity of a closed-loop system is ensured, and the precision of force and position control is dynamically optimized according to the real-time state of a robot system, so that the high-precision requirement is met when an independent operation task is executed; according to the invention, flexibility is enhanced when man-machine interaction or contact surface is accidentally interrupted, interaction safety and adaptability are ensured, and sub-priority tasks are executed by using the redundant degree of freedom of the robot, so that flexibility and adaptability of operation of the robot are enhanced.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot arm control algorithms, and in particular relates to an adaptive force-position hybrid control method based on an energy tank. Background Art

[0002] Traditional industrial robots lack flexibility, collaboration capabilities, and safety. Collaborative robots are designed with direct interaction with humans in mind, can quickly adapt to different working environments and tasks, can work directly with humans in the same work area, can provide assistance in tasks that are difficult for humans to complete, and can be widely used in scenarios such as smart healthcare and smart homes.

[0003] The adaptive force-position hybrid control algorithm proposed in this application combines the advantages of position control and force control to adapt to uncertain environments and task requirements. The algorithm dynamically controls the position and applied force of the robot's end effector by monitoring the state of the robot system in real time to ensure that the accuracy of force control and position control are met at the same time. When human-machine interaction or the connection with the contact surface is accidentally interrupted, it can improve the compliance of the system, thereby ensuring the safety of the interaction process and the adaptability of the system. By dynamically optimizing the balance between force and position control, this algorithm provides strong support for the efficient and safe operation of the robot in different operating environments.

[0004] Through the analysis and research of these background technologies, the authors proposed an adaptive force-position hybrid control method based on an energy tank, and used it to design a controller that balances the contradiction between the robot's force control accuracy, position control accuracy and compliance, and proved its passivity. Summary of the invention

[0005] The present invention aims to provide an adaptive force-position hybrid control method based on an energy tank, which is equipped with an energy tank to maintain the passive characteristics of the system. This method effectively solves the challenges faced by hybrid force control, impedance control, and indirect force control based on set points. By introducing the controller shaping function, accurate force tracking, fully compatible impedance behavior and safe contact similarity, as well as safe handling capabilities in the event of unexpected contact interruptions, are achieved, while avoiding the jitter problem of switch-based control methods. In addition, the present application is able to predict the energy consumption required before performing a specific force control task. The controller is suitable for handling dynamic problems of rigid bodies and flexible joints.

[0006] In order to solve the above technical problems, the specific technical solutions of the present invention are as follows:

[0007] In some embodiments of the present application, an adaptive force-position hybrid control method based on an energy tank is provided, comprising the following steps:

[0008] Step 1) obtaining joint angle position information, joint angular velocity information, and end posture information of the robot arm through joint sensors;

[0009] Step 2) establishing a dynamic model of the robotic arm based on the acquired sensor information;

[0010] Step 3) defining the task space velocity coordinates and calculating the null space basis matrix of the Jacobian matrix in the dynamic model; expanding the manipulator dynamics model into a manipulator hierarchical control model according to the defined task space velocity coordinates and the null space basis matrix;

[0011] Step 4) designing the energy tank based on the obstacle function, setting the energy tank state update rate, variable stiffness parameter and adaptive amount;

[0012] Step 5) Based on the hierarchical control model, the robot controller is designed through the energy tank to make the robot flexible while ensuring the accuracy of force control and position control;

[0013] Step 6) The posture error is calculated using the end posture information obtained by the sensor and the expected end posture information. The control torque required for each task is calculated based on the error and the hierarchical variable impedance control model, and input into the robotic arm to realize the control of the robotic arm.

[0014] In some embodiments of the present application, establishing a dynamic model of the robotic arm in step 2 specifically includes:

[0015]

[0016] Λ(ξ)=J(q) -T M(q)J(q) -1 #(2)

[0017]

[0018] Where M(q)∈R n×n is the inertia matrix in the joint space of the robot, is the Coriolis force matrix in the joint space of the robot, F g (ξ)=J(q) -T G(q) is the Cartesian space gravity force of the robot arm, G(q)∈R n×1 is the gravity term in the joint space of the robot, J(q)∈R m×n and are the Jacobian matrix of the robot and the first-order derivative of the Jacobian matrix; q, represents the joint angle and joint angular velocity obtained by the sensor, ξ, and F is the position, velocity and acceleration of the end of the robot arm obtained by the sensor; τ and Fext It is the control torque of the robot arm and the external torque of the robot arm;

[0019] m is the dimension of the end effector task space, and n is the number of robot arm joints;

[0020] The Cartesian space coordinate ξ and the joint space coordinate q satisfy the following relationship: J(q)∈R m×n is the Jacobian matrix of the robot arm;

[0021] Joint input torque τ and input vector F τ Satisfies the following relationship: τ=J(q) T F τ ;

[0022] External force F acting on the end effector of the robot arm ext Can be mapped to the joint space: τ ext =J T (q)F ext , where τ ext is the external force on the joint;

[0023] Through the robot's forward kinematics equation, the coordinate vector of the subtask is introduced:

[0024]

[0025] Among them, r represents the number of subtasks, and the dimension of each subtask is i = 1 is the highest priority task, i = r is the lowest priority task, for any two tasks i a and i b , if i a b , then it means i a Than i b The task has a high priority.

[0026] In some embodiments of the present application, the calculation method of the hierarchical control model of the robotic arm in step 3 includes:

[0027] Through the Jacobian matrix of each subtask Complete the mapping from joint space velocity to task space velocity:

[0028]

[0029] Since the space velocities of each task described by equation (5) are coupled to each other, a space velocity coordinate v is defined as i :

[0030]

[0031] ​ is the expanded Jacobian matrix to be determined, which can be determined by choosing To make it non-singular and guarantee the dynamic consistency of the null space projection, Usually chosen as:

[0032]

[0033] Among them, the highest priority task still remains consistent with the original Jacobian matrix, and for the same symbol, The remaining low-priority tasks are combined with the augmented matrix A null space basis matrix Z with full row rank i Related to the robot's inertia matrix, the augmented matrix for:

[0034]

[0035] In order to ensure that low-priority tasks do not affect high-priority tasks, the null space basis matrix is ​​used for calculation, where the null space basis matrix Z i as follows:

[0036]

[0037] Where M+ represents the weighted pseudo-inverse of the manipulator inertia matrix, express The standard orthogonal basis of Perform singular value decomposition to obtain, is the inertia-weighted pseudo-inverse of the Jacobian matrix J1 of the highest priority task, This is to make the lowest priority task use all remaining degrees of freedom of the robot;

[0038] The extended Jacobian matrix is ​​obtained from equations (5) and (7): The inverse of:

[0039]

[0040] Substituting formula (10) into formula (6) yields:

[0041]

[0042] Get the speed of each subtask space:

[0043]

[0044] Substituting all the above results into the robot dynamics model can obtain the robot hierarchical control model:

[0045]

[0046] Among them, the inertia matrix Λ is a diagonal matrix, and the matrix μ is processed by the controller during design.

[0047] In some embodiments of the present application, the Jacobian matrix of each subtask is used. The mapping from joint space velocity to task space velocity can be done:

[0048]

[0049] Since the space velocities of each task described by equation (5) are coupled to each other, a space velocity coordinate v is defined as i :

[0050]

[0051] is the expanded Jacobian matrix to be determined, which can be determined by choosing To make it non-singular and guarantee the dynamic consistency of the null space projection, Usually chosen as:

[0052]

[0053] Among them, the highest priority task still remains consistent with the original Jacobian matrix, and for the same symbol, The remaining low-priority tasks are combined with the augmented matrix A null space basis matrix Z with full row rank i Related to the robot's inertia matrix, the augmented matrix for:

[0054]

[0055] In order to ensure that low-priority tasks do not affect high-priority tasks, a widely used null space basis matrix Z i as follows:

[0056]

[0057] Where M+ represents the weighted pseudo-inverse of the manipulator inertia matrix, express The standard orthogonal basis of Perform singular value decomposition to obtain, is the inertia-weighted pseudo-inverse of the Jacobian matrix J1 of the highest priority task, This is to make the lowest priority task use all remaining degrees of freedom of the robot;

[0058] The extended Jacobian matrix is ​​obtained from equations (5) and (7): The inverse of:

[0059]

[0060] Substituting formula (10) into formula (6) yields:

[0061]

[0062] We can further get the speed of each subtask space:

[0063]

[0064] Substituting all the above results into the robot dynamics model can obtain the robot hierarchical control model:

[0065]

[0066] Among them, the inertia matrix Λ is a diagonal matrix, and the matrix μ is processed by the controller during design.

[0067] In some embodiments of the present application, the energy storage function of the energy tank in step 4 is as follows:

[0068]

[0069] The energy tank status update rate for the highest priority task controller interconnect has been redesigned to:

[0070]

[0071] in, In order to meet the purpose of variable stiffness, F frc is the controller to be designed to meet the accuracy of force control, β1 is a constant, K t1 The change rules are as follows:

[0072] K t1 =diag[R T ,R T ] ee Kdiag[R,R]#(32)

[0073] Wherein the matrix R is the rotation matrix from the base coordinate system to the terminal coordinate system, is the desired stiffness at the end coordinate, and ρ is the adaptive quantity that can change according to the robot state:

[0074]

[0075] in, is the error in the z-axis direction in the terminal coordinate system, d_max and d_min are the maximum and minimum values ​​of the error allowed. ee f frc represents the force controller in the end coordinate system, ee f d represents the desired force in the end coordinate system, ee f ext,z Indicates the magnitude of the force at the end coordinates, F frc for:

[0076] F frc =[R,0 3×3 ] T [0,0,ρ ee f frc ] T #(34);

[0077] in, ee f frc for:

[0078]

[0079] In the formula,

[0080] In the secondary priority task, in order to enable the energy tank to monitor the passive impact of the high priority task on the low priority task, the state update rate of the energy tank is designed to be:

[0081]

[0082]

[0083] in, is the minimum energy threshold set by the energy tank. Below this energy threshold, the energy tank cannot provide energy for the variable stiffness behavior of the manipulator. σ∈[0,1] is the parameter that adjusts the damping term of the energy tank to extract energy efficiency from the manipulator system. β i >0 is a constant, Represents the upper limit of the energy storage of the energy tank, is the difference between the actual space velocity and the target space coordinate velocity;

[0084] ω i is the variable stiffness strategy of the robot arm, where K ic is the fixed stiffness, k it is the variable stiffness, ω i The first two items are used to eliminate the passive impact of high priority tasks on low priority tasks, and the last item is used to introduce the characteristics of variable impedance parameters.

[0085] In some embodiments of the present application, in step 5, based on the hierarchical control model, the robot controller is designed by the energy tank so that the robot has flexibility while ensuring the accuracy of force control and position control. The controller design is performed on the hierarchical decoupling model, and the structure of the robot controller is rewritten as follows:

[0086]

[0087] Where g is the gravity compensation, τ μ For centripetal force compensation, F i For the controller to be designed;

[0088] The dynamic model of the robotic arm is:

[0089]

[0090] Among them, Λ(q) is the inertia matrix of the robot arm, is the Coriolis force matrix, g(q is the gravity matrix, τ ext is the external force on the robot arm. Substituting equation (38) into the robot arm dynamics equation (39) yields:

[0091]

[0092] From formula (21), we can get So the highest priority decoupling model can be:

[0093]

[0094] The secondary priority model is still the same as formula (40), and from formula (17) and formula (18) we can get Therefore, the decoupling model of the highest priority task is expressed in Cartesian space as

[0095]

[0096] The controller F1 for the highest priority task is designed as:

[0097]

[0098] Formula x t1 is the state of the energy tank, ω1 is the item designed previously;

[0099] Substituting equation (43) into equation (42), the robot's highest priority closed-loop system is as follows:

[0100]

[0101] For low priority tasks (i>1), the controller is selected as:

[0102]

[0103] Substituting equation (45) into equation (40) yields the secondary priority (i>1) closed-loop system model:

[0104]

[0105] In some embodiments of the present application, in order to prove the passivity of the highest priority task closed-loop system (44), the energy function describing the highest priority closed-loop system is first selected as:

[0106]

[0107] After deriving equation (47), the first derivative of the energy storage function, i.e., the power change expression of the robot system, is obtained:

[0108]

[0109] Substituting equation (44) into equation (48), according to the properties, we can get

[0110]

[0111] The total energy storage function of the robot's highest priority closed-loop system and energy tank is selected as:

[0112]

[0113] in and T(x t1 ) has been defined in formula (47) and (29); since V and T are positive definite, W must also be positive definite. By taking the derivative of W, we can get the power expression of the entire closed-loop system as:

[0114]

[0115] Since σ1∈[0,1],

[0116]

[0117] Therefore, from equations (51) and (52), we can obtain:

[0118] Therefore, the closed-loop system satisfies the passivity condition.

[0119] In some embodiments of the present application, for the closed-loop system of the secondary priority task (i>1), the energy storage function V is selected i for:

[0120]

[0121] The energy tank is connected to the secondary priority closed-loop system through the power protection interconnection, and a total energy storage function W is selected. i W i =V i +T i #(55);

[0122] Taking the derivative and substituting it into equation (54) and equation (36) yields:

[0123]

[0124] Due to σ i ∈[0,1], we have:

[0125]

[0126] So we have:

[0127]

[0128] It can be seen that the second priority closed-loop system meets the passivity condition.

[0129] Compared with the prior art, the beneficial effect of the present invention lies in that, by introducing the controller shaping function, accurate force tracking, fully compatible impedance behavior and safe contact similarity, as well as safe handling capabilities in the event of unexpected contact interruption, are achieved, while avoiding the jitter problem based on the switch control method, and predicting the energy consumption required before performing a specific force control task. The controller is suitable for processing the dynamics of rigid bodies and flexible joints, and effectively solves the problems faced by hybrid force control, impedance control, and indirect force control based on set points. BRIEF DESCRIPTION OF THE DRAWINGS

[0130] Various other advantages and benefits will become apparent to those of ordinary skill in the art by reading the detailed description of the preferred embodiments below. The accompanying drawings are only for the purpose of illustrating the preferred embodiments and are not to be considered as limiting the present invention. Moreover, the same reference symbols are used throughout the accompanying drawings to represent the same components. In the accompanying drawings:

[0131] Figure 1 A schematic diagram of trajectory changes of a robotic arm in Cartesian space provided by an embodiment of the present invention;

[0132] Figure 2 A schematic diagram of position error provided by an embodiment of the present invention;

[0133] Figure 3 A schematic diagram of the trajectory of the tracking force of the robot arm provided in an embodiment of the present invention;

[0134] Figure 4 A schematic diagram of changes in adaptive variables provided by an embodiment of the present invention;

[0135] Figure 5 A schematic diagram of changes in energy stored in an energy tank when executing a task with the highest priority provided by an embodiment of the present invention;

[0136] Figure 6 A schematic diagram of parameter changes in a robotic arm performing a task provided by an embodiment of the present invention;

[0137] Figure 7 A schematic diagram of changes in energy stored in an energy tank when executing a second priority task provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0138] The specific implementation of the present invention is further described in detail below in conjunction with the accompanying drawings and examples. The following examples are used to illustrate the present invention, but are not intended to limit the scope of the present invention.

[0139] In order to better understand the purpose, structure and function of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings.

[0140] See attached Figure 1-7 As shown, according to some embodiments of the present application, the following steps are included:

[0141] Step 1) obtaining joint angle position information, joint angular velocity information, and end posture information of the robot arm through joint sensors;

[0142] Step 2) establishing a dynamic model of the robotic arm based on the acquired sensor information;

[0143] Step 3) defining the task space velocity coordinates and calculating the null space basis matrix of the Jacobian matrix in the dynamic model; expanding the manipulator dynamics model into a manipulator hierarchical control model according to the defined task space velocity coordinates and the null space basis matrix;

[0144] Step 4) designing the energy tank based on the obstacle function, setting the energy tank state update rate, variable stiffness parameter and adaptive amount;

[0145] Step 5) Based on the hierarchical control model, the robot controller is designed through the energy tank to make the robot flexible while ensuring the accuracy of force control and position control;

[0146] Step 6) The posture error is calculated using the end posture information obtained by the sensor and the expected end posture information. The control torque required for each task is calculated based on the error and the hierarchical variable impedance control model, and input into the robotic arm to realize the control of the robotic arm.

[0147] In order to further optimize the above technical solution, the specific process of step 1 is as follows:

[0148] The joint angle position information, joint angular velocity information and end posture information of the robot arm are obtained through joint sensors.

[0149] In order to further optimize the above technical solution, the specific process of step 2 is as follows:

[0150] The Cartesian space dynamics equation of a robotic arm with n degrees of freedom is as follows:

[0151]

[0152] Λ(ξ)=J(q) -T M(q)J(q) -1 #(2)

[0153]

[0154] Where M(q)∈R n×n is the inertia matrix in the joint space of the robot, is the Coriolis force matrix in the joint space of the robot, F g (ξ)=J(q) -T G(q) is the Cartesian space gravity force of the robot arm, G(q)∈R n×1 is the gravity term in the joint space of the robot, J(q)∈R m×n and are the Jacobian matrix of the robot and the first-order derivative of the Jacobian matrix; q, represents the joint angle and joint angular velocity obtained by the sensor, ξ, and F is the position, velocity and acceleration of the end of the robot arm obtained by the sensor; τ and F ext It is the control torque of the robot arm and the external force torque acting on the robot arm.

[0155] m is the dimension of the end effector task space, which is usually 6. n is the number of robot joints, which is usually greater than 6 for redundant robot arms.

[0156] The Cartesian space coordinate ξ and the joint space coordinate q satisfy the following relationship: J(q)∈R m×n is the Jacobian matrix of the robot arm.

[0157] Joint input torque τ and input vector F τ Satisfies the following relationship: τ=J(q) T F τ .

[0158] External force F acting on the end effector of the robot arm ext Can be mapped to the joint space: τ ext =J T(q)F ext , where τ ext The external force on the joint.

[0159] Through the robot's forward kinematics equation, the coordinate vector of the subtask is introduced:

[0160]

[0161] Among them, r represents the number of subtasks, and the dimension of each subtask is i = 1 is the highest priority task, i = r is the lowest priority task, for any two tasks i a and i b , if i a b , then it means i a Than i b The task has a high priority.

[0162] In order to further optimize the above technical solution, the specific process of step 3 is as follows:

[0163] Using the Jacobian matrix of each subtask The mapping from joint space velocity to task space velocity can be done:

[0164]

[0165] Since the space velocities of each task described by equation (5) are coupled to each other, a space velocity coordinate v is defined as i :

[0166]

[0167] is the expanded Jacobian matrix to be determined, which can be determined by choosing This makes it non-singular and guarantees the dynamic consistency of the null space projection. Usually chosen as:

[0168]

[0169]

[0170] Among them, the highest priority task still remains consistent with the original Jacobian matrix, and for the same symbol, The remaining low-priority tasks are combined with the augmented matrix A null space basis matrix Z with full row rank i Related to the robot's inertia matrix. Augmented matrix for:

[0171] ​

[0172] In order to ensure that low-priority tasks do not affect high-priority tasks, a widely used null space basis matrix Z i as follows:

[0173]

[0174] Where M+ represents the weighted pseudo-inverse of the manipulator inertia matrix, express The standard orthogonal basis of Obtained by performing singular value decomposition. is the inertia-weighted pseudo-inverse of the Jacobian matrix J1 of the highest priority task, This is done so that the lowest priority task utilizes all remaining degrees of freedom of the robot.

[0175] The extended Jacobian matrix is ​​obtained from equations (5) and (7): The inverse of:

[0176]

[0177] Substituting formula (10) into formula (6) yields:

[0178]

[0179] We can further get the speed of each subtask space:

[0180]

[0181] Substituting all the above results into the robot dynamics model can obtain the robot hierarchical control model:

[0182]

[0183]

[0184] It should be noted that although the inertia matrix Λ is a diagonal matrix, there is still coupling in the matrix μ. Although the coupling can be ignored in most cases, it will be processed during controller design in order to obtain a clear hierarchical structure model.

[0185] In order to further optimize the above technical solution, the specific process of step 3 is as follows:

[0186] Using the Jacobian matrix of each subtask The mapping from joint space velocity to task space velocity can be done:

[0187]

[0188] Since the space velocities of each task described by equation (5) are coupled to each other, a space velocity coordinate v is defined as i :

[0189]

[0190] is the expanded Jacobian matrix to be determined, which can be determined by choosing This makes it non-singular and guarantees the dynamic consistency of the null space projection. Usually chosen as:

[0191]

[0192]

[0193] Among them, the highest priority task still remains consistent with the original Jacobian matrix, and for the same symbol, The remaining low-priority tasks are combined with the augmented matrix A null space basis matrix Z with full row rank i Related to the robot's inertia matrix. Augmented matrix for:

[0194]

[0195] In order to ensure that low-priority tasks do not affect high-priority tasks, a widely used null space basis matrix Z i as follows:

[0196]

[0197] Where M+ represents the weighted pseudo-inverse of the manipulator inertia matrix, express The standard orthogonal basis of Obtained by performing singular value decomposition. is the inertia-weighted pseudo-inverse of the Jacobian matrix J1 of the highest priority task, This is done so that the lowest priority task utilizes all remaining degrees of freedom of the robot.

[0198] The extended Jacobian matrix is ​​obtained from equations (5) and (7): The inverse of:

[0199]

[0200] Substituting formula (10) into formula (6) yields:

[0201]

[0202] We can further get the speed of each subtask space:

[0203]

[0204] Substituting all the above results into the robot dynamics model can obtain the robot hierarchical control model:

[0205]

[0206]

[0207] It should be noted that although the inertia matrix Λ is a diagonal matrix, there is still coupling in the matrix μ. Although the coupling can be ignored in most cases, it will be processed during controller design in order to obtain a clear hierarchical structure model.

[0208] In order to further optimize the above technical solution, the specific process of step 4 is as follows:

[0209] In formula (12), The existence of the term is the reason why the passivity of the entire closed-loop system is destroyed, mainly because of the impact of high-priority tasks on low-priority tasks. In order to restore the passivity of the entire robot system, an energy tank is used to monitor the energy that may destroy the passivity of the system. The energy storage function of the energy tank is as follows:

[0210]

[0211] The energy tank status update rate for the highest priority task controller interconnect has been redesigned to:

[0212]

[0213] in, In order to meet the purpose of variable stiffness, F frc is the controller to be designed to meet the accuracy of force control, and β1 is a constant. K t1 The change rules are as follows:

[0214] K t1 =diag[R T ,R T ] ee Kdiag[R,R]#(32)

[0215] Wherein the matrix R is the rotation matrix from the base coordinate system to the terminal coordinate system, is the desired stiffness at the end coordinate, and ρ is an adaptive variable that can change according to the robot state and is designed as:

[0216]

[0217] in, is the error in the axis direction in the end coordinate system, and d_max and d_min are the maximum and minimum values ​​of the error allowed. In addition, ee f frc represents the force controller in the end coordinate system, ee f d represents the desired force in the end coordinate system, ee f ext,z Indicates the magnitude of the force at the end coordinates. frc Designed to:

[0218] F frc =[R,0 3×3 ] T [0,0,ρ ee f frc ] T #(34)

[0219] in, ee f frc Designed to:

[0220]

[0221] In the formula,

[0222] In the secondary priority task, in order to enable the energy tank to monitor the passive impact of the high priority task on the low priority task, the state update rate of the energy tank is designed to be:

[0223]

[0224] in, is the minimum energy threshold set by the energy tank. Below this energy threshold, the energy tank cannot provide energy for the variable stiffness behavior of the manipulator. σ∈[0,1] is the parameter that adjusts the damping term of the energy tank to extract energy efficiency from the manipulator system. β i >0 is a constant, Represents the upper limit of the energy storage of the energy tank. It is the difference between the actual space velocity and the target space coordinate velocity.

[0225] ω i is the variable stiffness strategy of the robot arm, where K ic is the fixed stiffness, k it is the variable stiffness. iThe first two items are used to eliminate the passive impact of high priority tasks on low priority tasks, and the last item is used to introduce the characteristics of variable impedance parameters.

[0226] In order to further optimize the above technical solution, the specific process of step 5 is as follows:

[0227] The controller design in this section is based on the hierarchical decoupling model. The structure of the robot controller is rewritten as:

[0228]

[0229] Where g is the gravity compensation, τ μ For centripetal force compensation, F i is the controller to be designed. The dynamic model of the robot arm is:

[0230]

[0231] Among them, Λ(q) is the inertia matrix of the robot arm, is the Coriolis force matrix, g(q) is the gravity matrix, τ ext is the external force on the robot arm. Substituting equation (38) into the robot arm dynamics equation (39) yields:

[0232]

[0233] From formula (21), we can get So the highest priority decoupling model can be:

[0234]

[0235] The secondary priority model is still the same as formula (40). From formula (17) and formula (18), we can get Therefore, the decoupling model of the highest priority task is expressed in Cartesian space as

[0236]

[0237] The controller F1 for the highest priority task is designed as:

[0238]

[0239] Formula x t1 is the state of the energy tank, and ω1 is the item designed previously.

[0240] Substituting equation (43) into equation (42), the robot's highest priority closed-loop system is as follows:

[0241]

[0242] For low priority tasks (i>1), the controller is selected as:

[0243]

[0244] Substituting equation (45) into equation (40) yields the secondary priority (i>1) closed-loop system model:

[0245]

[0246] Next, we will prove the passivity of the highest priority task closed-loop system (44). First, we select the energy function that describes the highest priority closed-loop system as:

[0247]

[0248] After taking the derivative of equation (47), we can obtain the first derivative of the energy storage function, that is, the power change expression of the robot system:

[0249]

[0250] Substituting equation (44) into equation (48), according to the properties, we can get

[0251]

[0252] The total energy storage function of the robot's highest priority closed-loop system and energy tank is selected as:

[0253]

[0254] in and T(x t1 ) has been defined in formula (47) and (29). Since V and T are positive definite, W must also be positive definite. By taking the derivative of W, we can get the power expression of the entire closed-loop system as:

[0255]

[0256] Since σ1∈[0,1], we have

[0257]

[0258] Therefore, from equations (51) and (52), we can obtain:

[0259]

[0260] Therefore, the closed-loop system satisfies the passivity condition.

[0261] For the closed-loop system with lower priority tasks (i>1), select the energy storage function V i for:

[0262]

[0263] The energy tank is connected to the secondary priority closed-loop system through the power protection interconnection, and a total energy storage function W is selected. i for:

[0264] W i =V i +T i #(55)

[0265] Taking the derivative and substituting it into equation (54) and equation (36) we can obtain:

[0266]

[0267] Due to σ i ∈[0,1], we have:

[0268]

[0269] So we have:

[0270]

[0271] This means that the second priority closed-loop system satisfies the passivity condition.

[0272] Therefore, for the entire robot closed-loop system, the total energy storage function W is selected as:

[0273]

[0274] Although the external force τ in the robot model ext It does not appear in the controller design, but it is still necessary to specify its relationship with each layer model. Since the Jacobian matrix is reversible, so τ ext and The relationship can be written as:

[0275]

[0276] From equation (6), equation (53), equation (58) and equation (60), we can get:

[0277]

[0278] because So we have:

[0279]

[0280] in This means that the robot closed-loop system meets the passivity condition. Therefore, for the robot closed-loop system, It is passive, that is, the entire closed-loop system is passive.

[0281] In order to verify the effectiveness and practicality of this algorithm. A real-machine experiment was conducted on the 7DOF Franka Emika Panda robot to verify that the robotic arm can complete trajectory tracking tasks, force control tasks, and safety during human-computer interaction. The algorithm used in the experiment was implemented on a Lenovo laptop using the ROS environment and C++ language. Communication with the Franka Emika Panda robot was completed by using the libfranka and frankaros libraries. Before the experiment began, the Franka Emika Panda robotic arm was returned to its initial position, and the joint angles were

[0282] The highest priority task of the robot arm is to complete a curved surface grinding task. During the grinding process, the robot arm follows the preset task trajectory and applies a constant force of 10N. In the second priority task, the robot arm's joint 2 is required to move from the initial position of 0.37rad to 0.9rad and keep the original angle unchanged during operation. The parameters of the force controller are Kp=1.25, Ki=4.0, Kd=0.0.

[0283] The adaptive ability of the algorithm in the case of human interference is shown in the blue shadow in the attached figure of the summary. The operator interacted with the robot arm that was performing the task from t = 44s to t = 49s and from t = 76s to t = 81s. Figure 4 As shown, when the robot arm detects the occurrence of interaction based on its own state, it will quickly cut off the force controller and reduce the stiffness of the robot arm to ensure the compliance of the robot arm. Figure 5 and Figure 7 The energy in the robot increases because it does not need to perform the task; when the interaction ends, the robot immediately performs the task normally. During this process, the robot connects the force controller and increases its own stiffness, so Figure 5 and Figure 7 The energy in is reduced.

[0284] In the description of the present application, it should be understood that the terms "center", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating orientations or positional relationships, are based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the present application.

[0285] The terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of this application, unless otherwise specified, "plurality" means two or more.

[0286] In the description of this application, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" 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 a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in this application can be understood according to specific circumstances.

[0287] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0288] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. An adaptive force-position hybrid control method based on an energy tank, characterized in that: The following steps are involved: Step 1) obtaining joint angle position information, joint angular velocity information, and end posture information of the robot arm through joint sensors; Step 2) establishing a dynamic model of the robotic arm based on the acquired sensor information; Step 3) defining the task space velocity coordinates and calculating the null space basis matrix of the Jacobian matrix in the dynamic model; expanding the manipulator dynamics model into a manipulator hierarchical control model according to the defined task space velocity coordinates and the null space basis matrix; Step 4) designing the energy tank based on the obstacle function, setting the energy tank state update rate, variable stiffness parameter and adaptive amount; Step 5) Based on the hierarchical control model, the robot controller is designed through the energy tank to make the robot flexible while ensuring the accuracy of force control and position control; Step 6) The posture error is calculated using the end posture information obtained by the sensor and the expected end posture information. The control torque required for each task is calculated based on the error and the hierarchical variable impedance control model, and input into the robotic arm to realize the control of the robotic arm.

2. The adaptive force-position hybrid control method based on an energy tank according to claim 1 is characterized in that: The step 2 of establishing the dynamic model of the robot arm specifically includes: Λ(ξ)=J(q) -T M(q)J(q) -1 #(2) Where M(q)∈R n×n is the inertia matrix in the joint space of the robot, is the Coriolis force matrix in the joint space of the robot, F g (ξ)=J(q) -T G(q) is the Cartesian space gravity force of the robot arm, G(q)∈R n×1 is the gravity term in the joint space of the robot, J(q)∈R m×n and are the Jacobian matrix of the robot and the first-order derivative of the Jacobian matrix; q, represents the joint angle and joint angular velocity obtained by the sensor, ξ, and F is the position, velocity and acceleration of the end of the robot arm obtained by the sensor; τ and F ext It is the control torque of the robot arm and the external torque of the robot arm; m is the dimension of the end effector task space, and n is the number of robot arm joints; The Cartesian space coordinate ξ and the joint space coordinate q satisfy the following relationship: J(q)∈R m×n is the Jacobian matrix of the robot arm; Joint input torque τ and input vector F τ Satisfies the following relationship: τ=J(q) T F τ ; External force F acting on the end effector of the robot arm ext Can be mapped to the joint space: τ ext =J T (q)F ext , where τ ext is the external force on the joint; Through the robot's forward kinematics equation, the coordinate vector of the subtask is introduced: Among them, r represents the number of subtasks, and the dimension of each subtask is i = 1 is the highest priority task, i = r is the lowest priority task, for any two tasks i a and i b , if i a b , then it means i a Than i b The task has a high priority.​ 3. The adaptive force-position hybrid control method based on an energy tank according to claim 1 is characterized in that: The calculation method of the hierarchical control model of the robot arm in step 3 includes: Through the Jacobian matrix of each subtask Complete the mapping from joint space velocity to task space velocity: Since the space velocities of each task described by equation (5) are coupled to each other, a space velocity coordinate v is defined as i : is the expanded Jacobian matrix to be determined, which can be determined by choosing To make it non-singular and guarantee the dynamic consistency of the null space projection, Usually chosen as: Among them, the highest priority task still remains consistent with the original Jacobian matrix, and for the same symbol, The remaining low-priority tasks are combined with the augmented matrix A nullspace basis matrix Z with full row rank i Related to the robot's inertia matrix, the augmented matrix for: In order to ensure that low-priority tasks do not affect high-priority tasks, the null space basis matrix is ​​used for calculation, where the null space basis matrix Z i as follows: Where M+ represents the weighted pseudo-inverse of the manipulator inertia matrix, express The standard orthogonal basis of Perform singular value decomposition to obtain, is the inertia-weighted pseudo-inverse of the Jacobian matrix J1 of the highest priority task, This is to make the lowest priority task use all remaining degrees of freedom of the robot; The extended Jacobian matrix is ​​obtained from equations (5) and (7): The inverse of: Substituting formula (10) into formula (6) yields: Get the speed of each subtask space: Substituting all the above results into the robot dynamics model can obtain the robot hierarchical control model: Among them, the inertia matrix Λ is a diagonal matrix, and the matrix μ is processed by the controller during design.

4. The adaptive force-position hybrid control method based on an energy tank according to claim 3 is characterized in that: Using the Jacobian matrix of each subtask The mapping from joint space velocity to task space velocity can be done: Since the space velocities of each task described by equation (5) are coupled to each other, a space velocity coordinate v is defined as i : is the expanded Jacobian matrix to be determined, which can be determined by choosing To make it non-singular and guarantee the dynamic consistency of the null space projection, Usually chosen as: Among them, the highest priority task still remains consistent with the original Jacobian matrix, and for the same symbol, The remaining low-priority tasks are combined with the augmented matrix A nullspace basis matrix Z with full row rank i Related to the robot's inertia matrix, the augmented matrix for: In order to ensure that low-priority tasks do not affect high-priority tasks, a widely used null space basis matrix Z i as follows: Where M+ represents the weighted pseudo-inverse of the manipulator inertia matrix, express The standard orthogonal basis of Perform singular value decomposition to obtain, is the inertia-weighted pseudo-inverse of the Jacobian matrix J1 of the highest priority task, This is to make the lowest priority task use all remaining degrees of freedom of the robot; The extended Jacobian matrix is ​​obtained from equations (5) and (7): The inverse of: Substituting formula (10) into formula (6) yields: We can further get the speed of each subtask space: Substituting all the above results into the robot dynamics model can obtain the robot hierarchical control model: Among them, the inertia matrix Λ is a diagonal matrix, and the matrix μ is processed by the controller during design.

5. The adaptive force-position hybrid control method based on an energy tank according to claim 1 is characterized in that: The energy storage function of the energy tank in step 4 is as follows: The energy tank status update rate for the highest priority task controller interconnect has been redesigned to: in, In order to meet the purpose of variable stiffness, F frc is the controller to be designed to meet the accuracy of force control, β1 is a constant, K t1 The change rules are as follows: K t1 =diag[R T ,R T ] ee Kdiag[R,R]#(32) Wherein the matrix R is the rotation matrix from the base coordinate system to the terminal coordinate system, is the desired stiffness at the end coordinate, and ρ is the adaptive quantity that can change according to the robot state: in, is the error in the z-axis direction in the terminal coordinate system, d_max and d_min are the maximum and minimum values ​​of the allowed error. ee f frc represents the force controller in the end coordinate system, ee f d represents the desired force in the end coordinate system, ee f ext,z Indicates the magnitude of the force at the end coordinates, F frc for: F frc =[R,0 3×3 ] T [0,0,p ee f frc ] T #(34); in, ee f frc for: Officially, In the secondary priority task, in order to enable the energy tank to monitor the passive impact of the high priority task on the low priority task, the state update rate of the energy tank is designed to be: in, is the minimum energy threshold set by the energy tank. Below this energy threshold, the energy tank cannot provide energy for the variable stiffness behavior of the manipulator. σ∈[0,1] is the parameter that adjusts the damping term of the energy tank to extract energy efficiency from the manipulator system. β i >0 is a constant, Represents the upper limit of the energy storage of the energy tank, is the difference between the actual space velocity and the target space coordinate velocity; ω i is the variable stiffness strategy of the robot arm, where K ic is the fixed stiffness, k it is the variable stiffness, ω i The first two items are used to eliminate the passive impact of high priority tasks on low priority tasks, and the last item is used to introduce the characteristics of variable impedance parameters.

6. The adaptive force-position hybrid control method based on an energy tank according to claim 1 is characterized in that: In step 5, based on the hierarchical control model, the robot controller is designed through the energy tank, so that the robot has flexibility while ensuring the accuracy of force control and position control. The controller design is carried out on the hierarchical decoupling model, and the structure of the robot controller is rewritten as: Where g is the gravity compensation, τ μ For centripetal force compensation, F i For the controller to be designed; The dynamic model of the robotic arm is: Among them, Λ(q) is the inertia matrix of the robot arm, is the Coriolis force matrix, g(q) is the gravity matrix, τ ext is the external force on the robot arm. Substituting equation (38) into the robot arm dynamics equation (39) yields: From formula (21), we can get So the highest priority decoupling model can be: The secondary priority model is still the same as formula (40), and from formula (17) and formula (18) we can get Therefore, the decoupling model of the highest priority task is expressed in Cartesian space as The controller F1 for the highest priority task is designed as: Formula x t1 is the state of the energy tank, ω1 is the item designed previously; Substituting equation (43) into equation (42), the robot's highest priority closed-loop system is as follows: For low priority tasks (i>1), the controller is selected as: Substituting equation (45) into equation (40) yields the secondary priority (i>1) closed-loop system model:

7. The adaptive force-position hybrid control method based on an energy tank according to claim 6 is characterized in that: in, In order to prove the passivity of the highest priority task closed-loop system (44), the energy function describing the highest priority closed-loop system is first selected as: After deriving equation (47), the first derivative of the energy storage function, i.e., the power change expression of the robot system, is obtained: Substituting equation (44) into equation (48), according to the properties, we can get The total energy storage function of the robot's highest priority closed-loop system and energy tank is selected as: in and T(x t1 ) has been defined in formula (47) and (29); since V and T are positive definite, W must also be positive definite. By taking the derivative of W, we can get the power expression of the entire closed-loop system as: Since σ1∈[0,1], Therefore, from equations (51) and (52), we can obtain: Therefore, the closed-loop system satisfies the passivity condition.

8. The adaptive force-position hybrid control method based on an energy tank according to claim 6 is characterized in that: in, For the closed-loop system with lower priority tasks (i>1), select the energy storage function V i for: The energy tank is connected to the secondary priority closed-loop system through the power protection interconnection, and a total energy storage function W is selected. i For: W i =V i +T i #(55); Taking the derivative and substituting it into equation (54) and equation (36) yields: Due to σ i ∈[0,1], we have: So we have: It can be seen that the second priority closed-loop system meets the passivity condition.

Images