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

Abstract

The present invention belongs to the technical field of robotic arm control algorithms, and more particularly relates to an adaptive force-position hybrid control method based on an energy tank. The method comprises: redesigning the update rate of the energy tank state based on an obstacle function to compensate for the shortcomings of the force-position hybrid control and ensuring that the energy tank can adjust the system passivity based on changes in impedance parameters and null space projection. Furthermore, based on a hierarchical decoupling model, an adaptive force-position hybrid controller is designed to ensure the passivity of the closed-loop system. By dynamically optimizing the accuracy of force and position control based on the real-time state of the robotic system, high-precision requirements are met when performing independent operational tasks. Compliance is enhanced during unexpected interruptions in human-machine interaction or contact surfaces, ensuring interactive safety and adaptability. Furthermore, the robot's redundant degrees of freedom are utilized to perform secondary priority tasks, thereby enhancing its operational flexibility and adaptability.

Description

Technical Field

[0001] The present invention belongs to the technical field of robotic 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, on the other hand, are designed with direct human interaction in mind. They can quickly adapt to diverse work environments and tasks, work alongside humans in the same workspace, and assist with tasks difficult for humans. They are finding widespread application 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 unexpectedly interrupted, it can improve the flexibility 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 purpose of the present invention is 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 are achieved, as well as safe handling capabilities in the event of unexpected contact interruption, 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-of-arm posture information of the robot's manipulator arm through joint sensors;

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

[0010] Step 3) Define the task space velocity coordinates and calculate the null space basis matrix of the Jacobian matrix in the dynamic model. Expand the manipulator dynamics model into a manipulator hierarchical control model based on the defined task space velocity coordinates and the null space basis matrix.

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

[0012] Step 5) Based on the hierarchical control model, a robot controller is designed using an energy tank to ensure the robot is compliant while ensuring force and position control accuracy;

[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 manipulator, is the Coriolis force matrix of the manipulator joint space, F g (ξ)=J(q) -T G(q) is the Cartesian space gravity force of the manipulator, G(q)∈R n×1 is the gravity term in the joint space of the manipulator, J(q)∈R m×n and are the Jacobian matrix of the robot arm 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 robotic arm obtained by the sensor; τ and Fext It is the control torque of the robot arm and the external torque acting on the robot arm;

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

[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 τ The following relationship is satisfied: τ = J(q) T F τ ;

[0022] External force F acting on the end effector of the robotic arm ext Can be mapped to the joint space: τ ext =J T (q)F ext , where τ ext 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. a and i b , if i a b , then it means i a Than i b The task has 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] By 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 formula (5) are coupled with each other, a space velocity coordinate v is defined as i :

[0030]

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

[0032]

[0033] Among them, the highest priority task still remains consistent with the original Jacobian matrix, and for the sake of symbol consistency, 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's 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 the remaining degrees of freedom of the robot arm;

[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] Bringing all the above results into the robot arm dynamics model can obtain the robot arm 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 formula (5) are coupled with each other, a space velocity coordinate v is defined as i :

[0050]

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

[0052]

[0053] Among them, the highest priority task still remains consistent with the original Jacobian matrix, and for the sake of symbol consistency, 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's 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 the remaining degrees of freedom of the robot arm;

[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] Bringing all the above results into the robot arm dynamics model can obtain the robot arm 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 The controller to be designed is 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] Where 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 end 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 coordinate, 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 energy tank status update rate 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 manipulator, 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, a robot controller is designed using an energy tank to ensure that the robot has compliance while ensuring force control and position control accuracy. The controller design is performed based on the hierarchical decoupling model, and the structure of the robot controller is rewritten as follows:

[0086]

[0087] Where g is the gravity compensation, τ μ is the centripetal force compensation, F i is 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 manipulator, is the Coriolis force matrix, g(q is the gravity matrix, τ ext is the external force on the manipulator. Substituting equation (38) into the manipulator 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 follows:

[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 as follows:

[0108]

[0109] Substituting equation (44) into equation (48), we can obtain

[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 formulas (47) and (29); since V and T are positive definite, W must also be positive definite. 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 equations (54) and (36) yields:

[0123]

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

[0125]

[0126] So we have:

[0127]

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

[0129] Compared with the existing technology, the beneficial effect of the present invention is that, by introducing the controller shaping function, accurate force tracking, fully compatible impedance behavior and safe contact similarity are achieved, as well as safe handling capabilities in the event of unexpected contact interruption, 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 dynamic problems 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 skilled in the art upon reading the detailed description of the preferred embodiment below. The accompanying drawings are for illustration purposes only and are not to be considered as limiting the present invention. The same reference symbols are used throughout the drawings to represent the same components. In the drawings:

[0131] Figure 1 A schematic diagram of the trajectory change of the 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 robotic 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 showing changes in energy stored in an energy tank when executing a task with the highest priority according to an embodiment of the present invention;

[0136] Figure 6 A schematic diagram of parameter changes during a task performed by a robotic arm according to 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 following embodiments of the present invention are described in further detail with reference to 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 with reference to 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-of-arm posture information of the robot's manipulator arm through joint sensors;

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

[0143] Step 3) Define the task space velocity coordinates and calculate the null space basis matrix of the Jacobian matrix in the dynamic model. Expand the manipulator dynamics model into a manipulator hierarchical control model based on the defined task space velocity coordinates and the null space basis matrix.

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

[0145] Step 5) Based on the hierarchical control model, a robot controller is designed using an energy tank to ensure the robot is compliant while ensuring force and position control accuracy;

[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 dynamic equations of a robotic arm with n degrees of freedom are 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 manipulator, is the Coriolis force matrix of the manipulator joint space, F g (ξ)=J(q) -T G(q) is the Cartesian space gravity force of the manipulator, G(q)∈R n×1 is the gravity term in the joint space of the manipulator, J(q)∈R m×n and are the Jacobian matrix of the robot arm 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 robotic 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 generally 6. n is the number of joints in the robot arm, which is generally greater than 6 for redundant robots.

[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 τ The following relationship is satisfied: τ = J(q) T F τ .

[0158] External force F acting on the end effector of the robotic arm ext Can be mapped to the joint space: τ ext =J T(q)F ext , where τ ext The external force acting 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. a and i b , if i a b , then it means i a Than i b The task has 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 formula (5) are coupled with each other, a space velocity coordinate v is defined as i :

[0166]

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

[0168]

[0169]

[0170] Among them, the highest priority task still remains consistent with the original Jacobian matrix, and for the sake of symbol consistency, 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's 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 to allow the lowest priority task to utilize all remaining degrees of freedom of the robot arm.

[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] Bringing all the above results into the robot arm dynamics model can obtain the robot arm hierarchical control model:

[0182]

[0183]

[0184] It should be noted that although the inertia matrix Λ is a diagonal matrix, the matrix μ still has coupling. 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 formula (5) are coupled with each other, a space velocity coordinate v is defined as i :

[0189]

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

[0191]

[0192]

[0193] Among them, the highest priority task still remains consistent with the original Jacobian matrix, and for the sake of symbol consistency, 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's 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 to allow the lowest priority task to utilize all remaining degrees of freedom of the robot arm.

[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] Bringing all the above results into the robot arm dynamics model can obtain the robot arm hierarchical control model:

[0205]

[0206]

[0207] It should be noted that although the inertia matrix Λ is a diagonal matrix, the matrix μ still has coupling. 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 the high-priority task affects the low-priority task. 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 K is the controller to be designed to meet the accuracy of force control, and β1 is a constant. t1 The change rules are as follows:

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

[0215] Where 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 quantity 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 allowed error. 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 coordinate. 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 energy tank status update rate 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 manipulator, 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, τ μ is the 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 manipulator, is the Coriolis force matrix, g(q) is the gravity matrix, τ ext is the external force on the manipulator. Substituting equation (38) into the manipulator 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 follows:

[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 deriving Equation (47), we can obtain the first derivative of the energy storage function, which is the power change expression of the robot system:

[0249]

[0250] Substituting equation (44) into equation (48), we can obtain

[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 formulas (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 of the secondary priority task (i>1), 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) yields:

[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 FrankaEmika 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 robot's highest-priority task is to complete a curved surface grinding task. During the grinding process, the robot follows a pre-set trajectory and applies a constant force of 10 N. The second-priority task requires that Joint 2 of the robot be moved from an initial position of 0.37 rad to 0.9 rad, maintaining this angle during operation. The force controller parameters are Kp = 1.25, Ki = 4.0, and Kd = 0.0.

[0283] The adaptive capability of this algorithm in the presence of human interference is shown in the blue shadow in the accompanying figure. 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 in the figure, 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 arm immediately performs the task normally. During this process, the robot arm 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 this application, it should be understood that the terms "center", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating the orientation or position relationship, are based on the orientation or position relationship shown in the accompanying drawings, and are only for the convenience of describing this 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 cannot be understood as a limitation on this application.

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

[0286] In the description of this application, it should be noted that, unless otherwise expressly specified or limited, the terms "mounted," "connected," and "connected" should be understood in a broad sense. For example, they can refer to fixed connections, detachable connections, or integral connections; mechanical connections or electrical connections; direct connections or indirect connections through an intermediate medium; and internal connections between two components. Those skilled in the art will understand the specific meanings of the above terms in this application based on the specific circumstances.

[0287] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. Reference can be made to the common and similar parts between the various embodiments. For the devices disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple, and the relevant parts can be referred to the method description.

[0288] The above description of the disclosed embodiments is intended to enable one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be readily 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 is not limited to the embodiments shown herein but is intended to conform 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-of-arm posture information of the robot's manipulator arm through joint sensors; Step 2) establishing a dynamic model of the robotic arm based on the acquired sensor information; Step 3) Define the task space velocity coordinates and calculate the null space basis matrix of the Jacobian matrix in the dynamic model. Expand the manipulator dynamics model into a manipulator hierarchical control model based on the defined task space velocity coordinates and the null space basis matrix. Step 4) Design the energy tank based on the barrier function, set the energy tank state update rate, variable stiffness parameter, and adaptive amount. The energy storage function of the energy tank 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 The controller to be designed is 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) Where 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 according to the change of the robot state: in, is the error in the z-axis direction in the end 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 coordinate, 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 make the energy tank monitor the passive influence of the high priority task on the low priority task, the energy tank status update rate 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 manipulator, where K ic is the fixed stiffness, k it is the variable stiffness, ω i middle The role of is to eliminate the passive impact of high priority tasks on low priority tasks. The function is to introduce the characteristics of variable impedance parameters; Step 5) Based on the hierarchical control model, a robot controller is designed using an energy tank to ensure the robot is compliant while ensuring force and position control accuracy; 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 the energy tank according to claim 1 is characterized in that: The step 2 of establishing the dynamic model of the robotic 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 manipulator, is the Coriolis force matrix of the manipulator joint space, F g (ξ)=J(q) -T G(q) is the Cartesian space gravity of the manipulator, G(q)∈R n×1 is the gravity term in the joint space of the manipulator, J(q)∈R m×n and are the Jacobian matrix of the robot arm 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 robotic arm obtained by the sensor; τ and F ext It is the control torque of the robot arm and the external torque acting on the robot arm; m is the dimension of the end effector task space, and n is the number of joints of the robotic arm; 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 τ The following relationship is satisfied: τ = J(q) T F τ ; External force F acting on the end effector of the robotic arm ext Mapping to joint space: τ ext =J T (q)F ext , where τ ext 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. a and i b , if i a b , then it means i a Than i b The task has high priority.​ 3. The adaptive force-position hybrid control method based on the energy tank according to claim 1 is characterized in that: The calculation method of the hierarchical control model of the manipulator in step 3 includes: By 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 formula (5) are coupled with each other, a space velocity coordinate v is defined as i : is the expanded Jacobian matrix to be determined, by choosing To make it non-singular and guarantee the dynamic consistency of the null space projection, Commonly chosen as: Among them, the highest priority task still remains consistent with the original Jacobian matrix, and for the sake of symbol consistency, The remaining low-priority tasks are respectively associated 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: 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's 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 the remaining degrees of freedom of the robot arm; 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 arm dynamics model, we can obtain the robot arm 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 the energy tank according to claim 3 is characterized in that: In step 5, based on the hierarchical control model, the robot controller is designed by using 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 follows: Where g is the gravity compensation, τ μ is the centripetal force compensation, F i is the controller to be designed; The dynamic model of the robotic arm is: Among them, Λ(q) is the inertia matrix of the manipulator, is the Coriolis force matrix, g(q) is the gravity matrix, τ ext is the external force on the manipulator. Substituting equation (38) into the manipulator dynamics equation (39) yields: From formula (9), we can get So the highest priority decoupling model is: The secondary priority model is still the same as formula (40), and is obtained from formula (5) and formula (6): 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 follows: 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:

5. The adaptive force-position hybrid control method based on the energy tank according to claim 4 is characterized in that: in, In order to prove the passivity of the highest priority task closed-loop system (44), we first select the energy function that describes the highest priority closed-loop system 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 as follows: Substituting equation (44) into equation (48), we can obtain 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 formulas (47) and (29); since V and T are positive definite, W must also be positive definite. 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.

6. The adaptive force-position hybrid control method based on an energy tank according to claim 4, characterized in that: in, For the closed-loop system of the secondary priority task (i>1), 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 W i =V i +T i #(55); Taking the derivative and substituting it into equations (54) and (36) yields: Due to σ i ∈[0,1], we have: So we have: It can be seen that the secondary priority closed-loop system meets the passivity condition.

Images