A hierarchical variable impedance control method for a robotic arm based on an energy tank

Abstract

The present invention relates to the field of mechanical arm control technology, and in particular to a mechanical arm hierarchical variable impedance control method based on an energy tank, including obtaining joint information of a robot mechanical arm through a joint sensor, and establishing a dynamic model of the mechanical arm; then expanding the dynamic model of the mechanical arm into a hierarchical control model of the mechanical arm according to the defined task space velocity coordinates and the null space basis matrix; arranging energy tanks as energy storage elements in each workspace of the hierarchical control model, thereby expanding the hierarchical control model of the mechanical arm into a hierarchical variable impedance control model of the mechanical arm; calculating the posture error through the mechanical arm end posture information obtained by the joint sensor and the preset expected mechanical arm end posture information, and finally substituting it into the hierarchical variable impedance control model to calculate the control torque required for each task, thereby realizing the control of the mechanical arm. The present invention can quickly and accurately adjust the dynamic characteristics of the mechanical arm, thereby maintaining the safety and reliability of the operation process.

Description

Technical Field

[0001] The invention relates to the technical field of mechanical arm control, and in particular to a mechanical arm layered variable impedance control method based on an energy tank. Background Art

[0002] Robotic arms include collaborative robotic arms, which are innovative tools that work side by side with humans. The highlight of their new generation design is that they use a lightweight structure and integrate 7 degrees of freedom. This innovation not only greatly improves the flexibility and operating accuracy of the robotic arms, but also builds a solid hardware foundation for them to handle multiple tasks at the same time, which indicates that the application areas and capabilities of collaborative robotic arms will be widely expanded and deepened.

[0003] At the control method level, variable impedance control is an advanced robot control method. Its core lies in simulating the dynamic characteristics of the mass-spring-damper second-order system, so that the robot can intelligently adjust its response parameters to external forces or torques. This method gives the robot a high degree of environmental adaptability, especially when performing delicate operations in collaborative scenarios, showing unparalleled flexibility. Medical surgery collaboration is a typical example of its application. Through the force-position hybrid control method, the robotic arm can accurately perform delicate surgical operations such as cutting and puncture according to the preset impedance control law and the external force sensed in real time. All of this is completed under the remote and precise control of the operator, achieving a new level of human-machine collaboration.

[0004] However, when the robot arm uses the impedance controller to perform delicate operations or cope with complex and changing environments, if it cannot dynamically adjust the impedance parameters of each joint according to the external environment in real time, the controller will inevitably generate control torque that is difficult to predict and control. This situation will seriously weaken the operational reliability and safety of the robot arm and increase the operational risk.

[0005] Although offline adjustment of impedance parameters can improve the environmental adaptability of the robot arm to a certain extent, frequent offline parameter adjustment is obviously not flexible and efficient enough when facing a complex and unstructured working environment, and may even lead to operation failure or accidents due to delayed adjustment.

[0006] Therefore, in order to improve the adaptability and operational stability of the robotic arm in complex environments, there is an urgent need for an intelligent control method that can sense environmental changes in real time and automatically adjust impedance parameters. Summary of the invention

[0007] The purpose of the present invention is to propose a hierarchical variable impedance control method for a robotic arm based on an energy tank, to ensure that the robotic arm can quickly and accurately adjust its dynamic characteristics when performing delicate operations or facing emergencies, thereby maintaining the safety and reliability of the operation process.

[0008] To achieve this object, the present invention adopts the following technical solutions:

[0009] A method for controlling layered variable impedance of a mechanical arm based on an energy tank comprises the following steps:

[0010] S1. Obtaining joint information of the robot arm through a joint sensor, wherein the joint information includes joint angle position information, joint angular velocity information and posture information of the end of the robot arm;

[0011] S2. Establish a dynamic model of the robot arm based on the acquired joint information;

[0012] S3, defining the task space velocity coordinates, and calculating the null space basis matrix of the Jacobian matrix in the dynamic model, and then expanding the dynamic model of the manipulator into a hierarchical control model of the manipulator according to the defined task space velocity coordinates and the null space basis matrix;

[0013] S4. An energy tank is set as an energy storage element in each workspace of the hierarchical control model, and at the same time, a variable impedance characteristic of the robot arm in each workspace of the hierarchical control model is introduced through the state update rate of the energy tank;

[0014] S5, using the variable impedance characteristics obtained by the energy tank to expand the hierarchical control model of the robot arm into a hierarchical variable impedance control model of the robot arm;

[0015] S6. Calculate the posture error based on the posture information of the end of the robot arm obtained by the joint sensor and the preset expected posture information of the end of the robot arm. Substitute the posture error into the hierarchical variable impedance control model to calculate the control torque required for each task, and input it into the robot arm to realize the control of the robot arm.

[0016] Preferably, in S2, establishing a dynamic model of the robotic arm according to the acquired joint information specifically comprises the following steps:

[0017] S21. Based on the Cartesian space equation, a dynamic model of a robotic arm with n degrees of freedom is established:

[0018]

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

[0020]

[0021] Among them, q and They are respectively represented by the joint sensors to obtain the space coordinate position and coordinate velocity of the manipulator joint, ξ, and They represent the Cartesian space coordinate position, velocity and acceleration of the end of the manipulator obtained by the joint sensor; Λ(ξ) and is relative to the coordinate position ξ and the coordinate velocity The inertia matrix and centrifugal force matrix of are expressed by equations (2) and (3); F g (ξ)=J(q) -T G(q) represents the Cartesian space gravity of the robot arm, J(q)∈R m×n and They are represented as the Jacobian matrix of the robot and the first-order derivative of the Jacobian matrix, G(q)∈R n×1 It is represented as the gravity term in the joint space of the robot arm; F τ Represented as the input vector of the model, F ext It is represented as the external force acting on the end effector of the robot arm, M(q)∈R n×n Expressed as the inertia matrix in the robot joint space, It is expressed as the Coriolis force matrix of the robot joint space; S22, let the Cartesian space coordinate velocity The space coordinate velocity of the robot joint The following relations are satisfied:

[0022]

[0023] Where J(q)∈R m×n is the Jacobian matrix of the robot, m represents the dimension of the end effector task space, m is 6, and n is the number of robot joints; for redundant robot arms, n is greater than 6;

[0024] S23, let the joint input torque τ and input vector F τ The following relations are satisfied:

[0025] τ=J(q) T F τ (5)

[0026] S24, let the external force F acting on the end effector of the robot arm ext Mapped to the robot arm joint space coordinate q:

[0027] τ ext =J T (q)F ext (6)

[0028] where τ ext is the external force on the joint of the robot arm;

[0029] S25. Introduce the coordinate vector of the subtask through the robot's forward kinematics equation:

[0030]

[0031] 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 a high priority.

[0032] Preferably, in S3, the task space velocity coordinates are defined, and the null space basis matrix of the Jacobian matrix in the dynamic model is calculated, and then the dynamic model of the manipulator is expanded into a hierarchical control model of the manipulator according to the defined task space velocity coordinates and the null space basis matrix, specifically including the following steps:

[0033] S31. Using the Jacobian matrix of each subtask Complete the mapping from joint space velocity to task space velocity:

[0034]

[0035] S32. Since the joint space velocities of each subtask described by equation (8) are coupled to each other, a task space velocity coordinate v is defined i It is expressed as:

[0036]

[0037] in, is the expanded Jacobian matrix to be determined;

[0038] S33, by selecting To make it non-singular and guarantee the dynamic consistency of the null space projection, Selected as:

[0039]

[0040] Among them, the highest priority task remains consistent with the original Jacobian matrix. To unify the symbols, let The remaining low-priority tasks are respectively associated with the augmented matrix A nullspace basis matrix Z with full row rank i Related to the robot's inertia matrix;

[0041] Augmented Matrix It is expressed as:

[0042]

[0043] ​S34. In order to ensure that low-priority tasks do not affect high-priority tasks, let the null space basis matrix Z i It is expressed as:

[0044]

[0045] Among them, M+ represents the weighted pseudo-inverse of the manipulator inertia matrix, Expressed as The standard orthogonal basis of Perform singular value decomposition to obtain; Expressed as the inertia-weighted pseudo-inverse of the Jacobian matrix J1 of the highest priority task, It indicates that the lowest priority task utilizes all remaining degrees of freedom of the robot arm;

[0046] S35, from equation (8) and equation (11), we get the extended Jacobian matrix The inverse matrix of :

[0047]

[0048] S36. Substituting formula (14) into formula (9) yields:

[0049]

[0050] S36. Substitute equation (15) into equation (8) to obtain the task space speed of each subtask space:

[0051]

[0052] S37. Substitute equations (13) to (16) into equations (1) to (3) to obtain the hierarchical control model of the robot arm:

[0053]

[0054] Among them, Λ is represented as a diagonal matrix, Expressed as task space acceleration coordinates.

[0055] Preferably, in S4, energy tanks as energy storage elements are arranged in each workspace of the hierarchical control model, and variable impedance characteristics of the manipulator in each workspace of the hierarchical control model are introduced through the state update rate of the energy tank, which specifically includes the following steps:

[0056] S41. The energy storage function of the energy tank is expressed as:

[0057]

[0058] S42, the state update rate of the energy tank is designed to be:

[0059]

[0060] Among them, ∈∈R + It represents the minimum energy threshold set for the energy tank. When the energy of the energy tank is lower than the set minimum energy threshold, the energy tank cannot provide energy for the variable stiffness behavior of the manipulator. σ∈[0,1] represents the parameter for adjusting the damping term of the energy tank to extract energy efficiency from the manipulator. β i >0 represents a constant, It is expressed as the upper limit of the energy storage of the energy tank, Expressed as the difference between the actual space velocity and the target space coordinate velocity, ω i is the variable stiffness method of the robot arm, K ic is the fixed stiffness, D i is a fixed damping, k tt Variable stiffness.

[0061] Preferably, in S5, the step of expanding the hierarchical control model of the robot arm into a hierarchical variable impedance control model of the robot arm by using the variable impedance characteristic obtained by the energy tank specifically includes the following steps:

[0062] S51. Design a hierarchical variable impedance controller for the robotic arm based on the energy tank, and let the overall controller structure be expressed as:

[0063]

[0064] Among them, τ i Represented as the controller of each subtask, τ μ It represents the compensation made to counteract the Coriolis or centrifugal force, and g represents the gravity compensation;

[0065] S52, let the controllers of all subtasks τ i The control inputs have the following form:

[0066]

[0067] Among them, the position error of the end of the robot arm in the i-th task is ξ i Represented as the actual position of the end of the robot in the i-th task, ξ i,d It is expressed as the expected position of the end of the robot arm in the i-th task; the speed error of the end of the robot arm in the i-th task It is expressed as the actual speed of the end of the robot arm in the i-th task, is the desired velocity of the end of the robot in the i-th task; ω i x ti It is used to add the variable stiffness method to the control input, which is expressed by equation (23);

[0068] S53, by designing τ in formula (25) μ Feedback compensation for non-block diagonal terms in μ:

[0069]

[0070] in, It is represented as the i-th row and j-th column in μ with dimension m. i ×m j Matrix blocks of ;

[0071] S54, let μ i,j =-μ j,i T , then equation (27) is a conservative feedback, that is, the transmission power It does not actively generate energy;

[0072] S55. Due to the Jacobian matrix is reversible, then the external force τ on the joint of the robot arm ext The external force of the end effector of the manipulator in each layered variable impedance control model The relationship between them is expressed as:

[0073]

[0074] S56. Substituting equations (25) to (28) into equation (17), we obtain the decoupled closed-loop dynamic equation:

[0075]

[0076] S57. For the highest priority task (i=1), from equations (11) and (13), we can get Therefore, the highest priority closed-loop dynamic equation is expressed as:

[0077]

[0078] S58, from equation (13) and equation (16), we can get In order to prove the passivity of the entire closed-loop system, we first prove the passivity of the highest priority task (i=1), and select the energy function describing the highest priority closed-loop system as:

[0079]

[0080] S59. Taking the derivative of equation (31) yields:

[0081]

[0082] S510, connecting the energy tank to the highest priority closed-loop system through the power protection interconnection, and selecting a total energy storage function W1 to be expressed as:

[0083] W1=V1+T1 (33)

[0084] S511. Deriving equation (33) and substituting it into equations (22) and (32) can make the highest priority closed-loop system satisfy the passivity condition:

[0085]

[0086] Since σ1∈[0,1], we have:

[0087]

[0088] So we have:

[0089]

[0090] S512: For the closed-loop system of the secondary priority task (i>1), select the energy storage function V i for:

[0091]

[0092] S513, connecting the energy tank to the secondary priority closed-loop system through the power protection interconnection, and selecting a total energy storage function W i It is expressed as:

[0093] W i =V i +T i (38)

[0094] S514. Deriving equation (38) and substituting it into equation (22) and equation (38) can make the secondary priority closed-loop system satisfy the passivity condition:

[0095]

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

[0097]

[0098] So we have:

[0099]

[0100] S515. For the entire robot arm system, the total energy storage function W is selected as:

[0101]

[0102] S516. Substituting equation (9), equation (28), equation (36) and equation (41) into equation (42) can make the robot closed-loop system meet the passivity condition:

[0103]

[0104] One of the above technical solutions has the following beneficial effects: the variable impedance characteristics of the robot arm are extended to the field of multi-task control, which is achieved through the following steps: First, the joint state information is collected in real time using joint sensors, an accurate dynamic model is constructed, and the null space of the Jacobian matrix is ​​calculated to provide a theoretical basis for subsequent control. Secondly, an innovative energy storage element, the energy tank, is introduced into each workspace, which can intelligently absorb and release energy, thereby enhancing the stability and robustness of the robot arm when performing variable impedance tasks. Finally, the position error information of the robot arm is combined with the hierarchical variable impedance model to accurately calculate the control torque required for each joint, and feedback is given to the robot arm actuator in real time to achieve efficient and stable control. BRIEF DESCRIPTION OF THE DRAWINGS

[0105] Figure 1 It is a flow chart of a method for controlling layered variable impedance of a mechanical arm based on an energy tank according to the present invention;

[0106] Figure 2 It is a schematic diagram of the principle of layered variable impedance model control in a layered variable impedance control method of a mechanical arm based on an energy tank of the present invention;

[0107] Figure 3 It is a schematic diagram of the errors of three controllers in the first layer of tasks in an embodiment of a mechanical arm layered variable impedance control method based on an energy tank of the present invention;

[0108] Figure 4 It is a schematic diagram of errors of three controllers in the second-layer task in an embodiment of a mechanical arm hierarchical variable impedance control method based on an energy tank of the present invention;

[0109] Figure 5 It is a schematic diagram of errors of three controllers in the third layer task in an embodiment of a mechanical arm layered variable impedance control method based on an energy tank of the present invention;

[0110] Figure 6 It is a schematic diagram of the total energy of the system when controller 2 and controller 3 are used in an embodiment of a mechanical arm layered variable impedance control method based on an energy tank of the present invention; DETAILED DESCRIPTION

[0111] The technical solution of the present invention is further described below with reference to the accompanying drawings and through specific implementation methods.

[0112] like Figure 1 As shown, a method for controlling layered variable impedance of a mechanical arm based on an energy tank comprises the following steps:

[0113] S1. Obtaining joint information of the robot arm through a joint sensor, wherein the joint information includes joint angle position information, joint angular velocity information and posture information of the end of the robot arm;

[0114] S2. Establish a dynamic model of the robot arm based on the acquired joint information;

[0115] S3, defining the task space velocity coordinates, and calculating the null space basis matrix of the Jacobian matrix in the dynamic model, and then expanding the dynamic model of the manipulator into a hierarchical control model of the manipulator according to the defined task space velocity coordinates and the null space basis matrix;

[0116] S4. An energy tank is set as an energy storage element in each workspace of the hierarchical control model, and at the same time, a variable impedance characteristic of the robot arm in each workspace of the hierarchical control model is introduced through the state update rate of the energy tank;

[0117] S5, using the variable impedance characteristics obtained by the energy tank to expand the hierarchical control model of the robot arm into a hierarchical variable impedance control model of the robot arm;

[0118] S6. Calculate the posture error based on the posture information of the end of the robot arm obtained by the joint sensor and the preset expected posture information of the end of the robot arm. Substitute the posture error into the hierarchical variable impedance control model to calculate the control torque required for each task, and input it into the robot arm to realize the control of the robot arm.

[0119] This application focuses on the forefront of robotics technology, and in particular proposes an innovative energy tank-based hierarchical variable impedance control method for robotic arms, which aims to significantly improve the flexibility and adaptability of robots in multi-task execution environments. This method is particularly suitable for delicate work scenarios, such as medical surgery collaboration, in which the robotic arm needs to accurately respond to external forces based on the impedance control law, realize force-position hybrid control, and ensure that surgical operations (such as cutting and puncture) under remote control of the operator are accurate. Given the complexity of human tissue (including different cortical and muscle layers), the robotic arm needs to dynamically adjust its stiffness to avoid surgical failure. At the same time, during the execution of the main task, sub-tasks must also be taken into account to maintain the flexibility of the joints.

[0120] The core of this method is to extend the variable impedance characteristics of the robot arm to the field of multi-task control, which is achieved through the following steps: First, use joint sensors to collect joint state information in real time, build an accurate dynamic model, and calculate the null space of the Jacobian matrix to provide a theoretical basis for subsequent control. Secondly, introduce innovative energy storage elements in each workspace, namely energy tanks, which can intelligently absorb and release energy, thereby enhancing the stability and robustness of the robot arm when performing variable impedance tasks. Finally, combine the position error information of the robot arm with the hierarchical variable impedance model to accurately calculate the control torque required for each joint, and feed it back to the robot arm actuator in real time to achieve efficient and stable control.

[0121] This hierarchical variable impedance control method not only ensures that the robotic arm can exhibit excellent variable impedance characteristics in multiple workspaces, but also greatly broadens the application potential and possibilities of collaborative robotic arms in complex real-world scenarios, opening up new paths for the further development and application of robotics technology.

[0122] To further illustrate, in S2, establishing a dynamic model of the robot arm according to the acquired joint information specifically includes the following steps:

[0123] S21. Based on the Cartesian space equation, a dynamic model of a robotic arm with n degrees of freedom is established:

[0124]

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

[0126]

[0127] Among them, q and They are respectively represented by the joint sensors to obtain the space coordinate position and coordinate velocity of the manipulator joint, ξ, and They represent the Cartesian space coordinate position, velocity and acceleration of the end of the manipulator obtained by the joint sensor; Λ(ξ) and is relative to the coordinate position ξ and the coordinate velocity The inertia matrix and centrifugal force matrix of are expressed by equations (2) and (3); F g (ξ)=J(q) -T G(q) represents the Cartesian space gravity of the robot arm, J(q)∈R m×n and They are represented as the Jacobian matrix of the robot and the first-order derivative of the Jacobian matrix, G(q)∈R n×1 It is represented as the gravity term in the joint space of the robot arm; F τRepresented as the input vector of the model, F ext It is represented as the external force acting on the end effector of the robot arm, M(q)∈R n×n Expressed as the inertia matrix in the robot joint space, Represented as the Coriolis force matrix in the joint space of the robot.

[0128] S22, let Cartesian space coordinate velocity The space coordinate velocity of the robot joint The following relations are satisfied:

[0129]

[0130] Where J(q)∈R m×n is the Jacobian matrix of the robot, m represents the dimension of the end effector task space, m is 6, and n is the number of robot joints; for redundant robot arms, n is greater than 6;

[0131] S23, let the joint input torque τ and input vector F τ The following relations are satisfied:

[0132] τ=J(q) T F τ (5)

[0133] S24, let the external force F acting on the end effector of the robot arm ext Mapped to the robot arm joint space coordinate q:

[0134] τ ext =J T (q)F ext (6)

[0135] where τ ext is the external force on the joint of the robot arm;

[0136] S25. Introduce the coordinate vector of the subtask through the robot's forward kinematics equation:

[0137]

[0138] 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 a high priority.

[0139] ​Further explanation, in S3, the task space velocity coordinates are defined, and the null space basis matrix of the Jacobian matrix in the dynamic model is calculated, and then the dynamic model of the manipulator is expanded into a hierarchical control model of the manipulator according to the defined task space velocity coordinates and the null space basis matrix, specifically including the following steps:

[0140] S31. Using the Jacobian matrix of each subtask Complete the mapping from joint space velocity to task space velocity:

[0141]

[0142] S32. Since the joint space velocities of each subtask described by equation (8) are coupled to each other, a task space velocity coordinate v is defined i It is expressed as:

[0143]

[0144] in, is the expanded Jacobian matrix to be determined;

[0145] S33, by selecting To make it non-singular and guarantee the dynamic consistency of the null space projection, Selected as:

[0146]

[0147] Among them, the highest priority task remains consistent with the original Jacobian matrix. To unify the symbols, let The remaining low-priority tasks are respectively associated with the augmented matrix A nullspace basis matrix Z with full row rank i Related to the robot's inertia matrix;

[0148] Augmented Matrix It is expressed as:

[0149]

[0150] S34. In order to ensure that low-priority tasks do not affect high-priority tasks, let the null space basis matrix Z i It is expressed as:

[0151]

[0152] Among them, M+ represents the weighted pseudo-inverse of the manipulator inertia matrix, Expressed as The standard orthogonal basis of Perform singular value decomposition to obtain; Expressed as the inertia-weighted pseudo-inverse of the Jacobian matrix J1 of the highest priority task, It indicates that the lowest priority task utilizes all remaining degrees of freedom of the robot arm;

[0153] S35, from equation (8) and equation (11), we get the extended Jacobian matrix The inverse matrix of :

[0154]

[0155] S36. Substituting formula (14) into formula (9) yields:

[0156]

[0157] S36. Substitute equation (15) into equation (8) to obtain the task space speed of each subtask space:

[0158]

[0159] S37. Substitute equations (13) to (16) into equations (1) to (3) to obtain the hierarchical control model of the robot arm:

[0160]

[0161] Among them, Λ is represented as a diagonal matrix, Expressed as task space acceleration coordinates.

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

[0163] Further explanation, in S4, the energy tank as an energy storage element is set in each workspace of the hierarchical control model, and the variable impedance characteristics of the manipulator in each workspace of the hierarchical control model are introduced through the state update rate of the energy tank, which specifically includes the following steps:

[0164] In formula (16), 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.

[0165] S41. The energy storage function of the energy tank is expressed as:

[0166]

[0167] S42. In order to enable the energy tank to monitor the passive impact of high-priority tasks on low-priority tasks, the state update rate of the energy tank is designed to be:

[0168]

[0169] Among them, ∈∈R + It represents the minimum energy threshold set for the energy tank. When the energy of the energy tank is lower than the set minimum energy threshold, the energy tank cannot provide energy for the variable stiffness behavior of the manipulator. σ∈[0,1] represents the parameter for adjusting the damping term of the energy tank to extract energy efficiency from the manipulator. β i >0 represents a constant, It is expressed as the upper limit of the energy storage of the energy tank, Expressed as the difference between the actual space velocity and the target space coordinate velocity, ω i is the variable stiffness method of the robot arm, K ic is the fixed stiffness, D i is a fixed damping, k it Variable stiffness.

[0170] To further illustrate, in S5, the step of expanding the hierarchical control model of the robot arm into a hierarchical variable impedance control model of the robot arm by using the variable impedance characteristic obtained by the energy tank specifically includes the following steps:

[0171] S51. Design a hierarchical variable impedance controller for the robotic arm based on the energy tank, and let the overall controller structure be expressed as:

[0172]

[0173] Among them, τ i Represented as the controller of each subtask, τ μ It represents the compensation made to counteract the Coriolis or centrifugal force, and g represents the gravity compensation;

[0174] S52, let the controllers of all subtasks τ i The control inputs have the following form:

[0175]

[0176] Among them, the position error of the end of the robot arm in the i-th task is ξ i Represented as the actual position of the end of the robot in the i-th task, ξ i,d It is expressed as the expected position of the end of the robot arm in the i-th task; the speed error of the end of the robot arm in the i-th task It is expressed as the actual speed of the end of the robot arm in the i-th task, is the desired velocity of the end of the robot in the i-th task; ω i x ti It is used to add the variable stiffness method to the control input, which is expressed by equation (23);

[0177] S53. Although the inertia matrix Λ is a diagonal matrix, the matrix μ still has coupling. Although the influence of the Coriolis force μ is extremely small in most cases, in order to obtain a clear hierarchical task model, it is still necessary to design τ in equation (25) μ Feedback compensation for non-block diagonal terms in μ:

[0178]

[0179] in, It is represented as the i-th row and j-th column in μ with dimension m. i ×m j Matrix blocks of ;

[0180] S54, let μ i,j =-μ j,i T , then equation (27) is a conservative feedback, that is, the transmission power No energy is actively generated; therefore compensation can be done actively without affecting the passivity of the robot system;

[0181] S55, despite the external force τ on the robot arm joint ext It does not appear in the design of the overall controller structure, but it is still necessary to specify its external force with the end effector of the manipulator in each layered variable impedance control model. Since the Jacobian matrix is reversible, then the external force τ on the joint of the robot arm ext The external force of the end effector of the manipulator in each layered variable impedance control model The relationship between them is expressed as:

[0182]

[0183] S56. Substitute equations (25) to (28) into equation (17) to obtain the decoupled closed-loop dynamic equation:

[0184]

[0185] S57. For the highest priority task (i=1), from equations (11) and (13), we can get Therefore, the highest priority closed-loop dynamic equation is expressed as:

[0186]

[0187] S58, from equation (13) and equation (16), we can get In order to prove the passivity of the entire closed-loop system, we first prove the passivity of the highest priority task (i=1), and select the energy function describing the highest priority closed-loop system as:

[0188]

[0189] S59. Taking the derivative of equation (31) yields:

[0190]

[0191] S510, connecting the energy tank to the highest priority closed-loop system through the power protection interconnection, and selecting a total energy storage function W1 to be expressed as:

[0192] W1=V1+T1 (33)

[0193] S511. Deriving equation (33) and substituting it into equations (22) and (32) can make the highest priority closed-loop system satisfy the passivity condition:

[0194]

[0195] Since σ1∈[0,1], we have:

[0196]

[0197] So we have:

[0198]

[0199] S512: For the closed-loop system of the secondary priority task (i>1), select the energy storage function V i for:

[0200]

[0201] S513, connecting the energy tank to the secondary priority closed-loop system through the power protection interconnection, and selecting a total energy storage function W i It is expressed as:

[0202] W i =V i +T i (38)

[0203] S514. Deriving equation (38) and substituting it into equation (22) and equation (38) can make the secondary priority closed-loop system satisfy the passivity condition:

[0204]

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

[0206]

[0207] So we have:

[0208]

[0209] S515. For the entire robot arm system, the total energy storage function W is selected as:

[0210]

[0211] S516. Substituting equation (9), equation (28), equation (36) and equation (41) into equation (42) can make the robot closed-loop system meet the passivity condition:

[0212]

[0213] In order to evaluate the performance of the energy tank-based manipulator hierarchical variable impedance control method proposed in this application, a series of relevant simulation tests were performed using the Franka Emika Panda seven-degree-of-freedom manipulator (hereinafter referred to as the seven-degree-of-freedom manipulator). These tests require the seven-degree-of-freedom manipulator to complete the following three tasks:

[0214] The first level task (m1=3): impedance control in the Cartesian space (x, y, z) direction, and impedance control from the initial position P0 to P1, using fixed impedance parameters.

[0215] The second level task (m2=3): impedance control of the joint space (joint 2, joint 4, joint 5), and keeping the current position of the joint unchanged, adopting the changing impedance parameters.

[0216] The third level task (m3=1): impedance control of the joint space (joint 6), and moving from the initial angle to a specific angle, taking varying impedance parameters.

[0217] In the experiment, three controllers were used to control the seven-DOF robotic arm to perform the three required tasks:

[0218] Controller 1 (No Nullspace): Controller τ1 that only uses the first layer of tasks.

[0219] Controller 2 (No passive): Controller using the null space projection technique expressed in equation (13).

[0220] Controller 3 (Passive via Energy Tank): uses the controller proposed in this paper.

[0221] By using controller 1, we aim to verify that the proposed hierarchical task model will not affect high-priority tasks, while the purpose of using the second controller is to show that the energy tank can indeed ensure the passivity of the entire robot system.

[0222] from Figure 3 , Figure 4 and Figure 5 It can be seen that the performance of the three controllers is roughly the same on the first-level tasks, while controller 1 obviously diverges and fails to complete the assigned tasks on the second and third-level tasks. As the first-level tasks converge, controller 3 also converges on the second and third-level tasks simultaneously, verifying that the proposed hierarchical task model can complete hierarchical tasks without affecting high-priority tasks.

[0223] from Figure 6 It can be seen that the energy of the seven-degree-of-freedom robot controlled by controller 3 never exceeds the energy at t=0 as time t changes, and after the seven-degree-of-freedom robot is stable, the energy of the seven-degree-of-freedom robot controlled by controller 3 stabilizes near a constant value. However, the energy of the robot controlled by controller 2 soars at the beginning, which obviously destroys the passivity of the system. This shows that the energy tank can indeed ensure the passivity of the entire robot system, that is, the hierarchical variable impedance control method of the robot based on the energy tank of this application is creative.

[0224] The technical principle of the present invention is described above in conjunction with specific embodiments. These descriptions are only for explaining the principle of the present invention and cannot be interpreted as limiting the scope of protection of the present invention in any way. Based on the explanations herein, those skilled in the art can associate other specific embodiments of the present invention without creative work, and these equivalent variations or substitutions are all included in the scope defined by the claims of this application.

Claims

1. A hierarchical variable impedance control method for a robotic arm based on an energy tank, characterized in that: The following steps are involved: S1. Obtaining joint information of the robot arm through a joint sensor, wherein the joint information includes joint angle position information, joint angular velocity information and posture information of the end of the robot arm; S2. Establish a dynamic model of the robot arm based on the acquired joint information; S3, defining the task space velocity coordinates, and calculating the null space basis matrix of the Jacobian matrix in the dynamic model, and then expanding the dynamic model of the manipulator into a hierarchical control model of the manipulator according to the defined task space velocity coordinates and the null space basis matrix; S4. An energy tank is set as an energy storage element in each workspace of the hierarchical control model, and at the same time, a variable impedance characteristic of the robot arm in each workspace of the hierarchical control model is introduced through the state update rate of the energy tank; S5, using the variable impedance characteristics obtained by the energy tank to expand the hierarchical control model of the robot arm into a hierarchical variable impedance control model of the robot arm; S6. Calculate the posture error based on the posture information of the end of the robot arm obtained by the joint sensor and the preset expected posture information of the end of the robot arm. Substitute the posture error into the hierarchical variable impedance control model to calculate the control torque required for each task, and input it into the robot arm to realize the control of the robot arm.

2. According to the energy tank-based mechanical arm layered variable impedance control method of claim 1, it is characterized in that: In S2, the dynamic model of the robot arm is established according to the acquired joint information, which specifically includes the following steps: S21. Based on the Cartesian space equation, a dynamic model of a robotic arm with n degrees of freedom is established: Λ(ξ)=J(q) -T M(q)J(q) -1 (2) Among them, q and They are respectively represented by the joint sensors to obtain the space coordinate position and coordinate velocity of the manipulator joint, ξ, and They represent the Cartesian space coordinate position, velocity and acceleration of the end of the manipulator obtained by the joint sensor; Λ(ξ) and is relative to the coordinate position ξ and the coordinate velocity The inertia matrix and centrifugal force matrix of are expressed by equations (2) and (3); F g (ξ)=J(q) -T G(q) represents the Cartesian space gravity of the robot arm, J(q)∈R m×n and They are represented as the Jacobian matrix of the robot and the first-order derivative of the Jacobian matrix, G(q)∈R n×1 It is represented as the gravity term in the joint space of the robot arm; F τ Represented as the input vector of the model, F ext It is represented as the external force acting on the end effector of the robot arm, M(q)∈R n×n Expressed as the inertia matrix in the robot joint space, It is represented as the Coriolis force matrix in the joint space of the robot; S22, let Cartesian space coordinate velocity The space coordinate velocity of the robot joint The following relations are satisfied: Where J(q)∈R m×n is the Jacobian matrix of the robot, m represents the dimension of the end effector task space, m is 6, and n is the number of robot joints; for redundant robot arms, n is greater than 6; S23, let the joint input torque τ and input vector F τ The following relations are satisfied: τ=J(q) T F τ (5) S24, let the external force F acting on the end effector of the robot arm ext Mapped to the robot arm joint space coordinate q: τ ext =J T (q)F ext (6) where τ ext is the external force on the joint of the robot arm; S25. Introduce the coordinate vector of the subtask through the robot's forward kinematics equation: 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 a high priority.​ 3. According to claim 2, a mechanical arm layered variable impedance control method based on an energy tank is characterized in that: In S3, the task space velocity coordinates are defined, and the null space basis matrix of the Jacobian matrix in the dynamic model is calculated. Then, the dynamic model of the manipulator is expanded into a hierarchical control model of the manipulator according to the defined task space velocity coordinates and the null space basis matrix. Specifically, the following steps are included: S31. Using the Jacobian matrix of each subtask Complete the mapping from joint space velocity to task space velocity: S32. Since the joint space velocities of each subtask described by equation (8) are coupled to each other, a task space velocity coordinate v is defined i It is expressed as: in, is the expanded Jacobian matrix to be determined; S33, by selecting To make it non-singular and guarantee the dynamic consistency of the null space projection, Selected as: Among them, the highest priority task remains consistent with the original Jacobian matrix. To unify the symbols, let 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; Augmented Matrix It is expressed as: S34. In order to ensure that low-priority tasks do not affect high-priority tasks, let the null space basis matrix Z i It is expressed as: Among them, M+ represents the weighted pseudo-inverse of the manipulator inertia matrix, Expressed as The standard orthogonal basis of Perform singular value decomposition to obtain; Expressed as the inertia-weighted pseudo-inverse of the Jacobian matrix J1 of the highest priority task, It indicates that the lowest priority task utilizes all remaining degrees of freedom of the robot arm; S35, from equation (8) and equation (11), we get the extended Jacobian matrix The inverse matrix of : S36. Substituting formula (14) into formula (9) yields: S36. Substitute equation (15) into equation (8) to obtain the task space speed of each subtask space: S37. Substitute equations (13) to (16) into equations (1) to (3) to obtain the hierarchical control model of the robot arm: Among them, Λ is represented as a diagonal matrix, Expressed as task space acceleration coordinates.

4. According to claim 3, a mechanical arm layered variable impedance control method based on an energy tank is characterized in that: In S4, energy tanks as energy storage elements are arranged in each workspace of the hierarchical control model, and variable impedance characteristics of the manipulator in each workspace of the hierarchical control model are introduced through the state update rate of the energy tanks, which specifically includes the following steps: S41. The energy storage function of the energy tank is expressed as: S42, the state update rate of the energy tank is designed to be: Among them, ∈∈R + It represents the minimum energy threshold set for the energy tank. When the energy of the energy tank is lower than the set minimum energy threshold, the energy tank cannot provide energy for the variable stiffness behavior of the manipulator. σ∈[0,1] represents the parameter for adjusting the damping term of the energy tank to extract energy efficiency from the manipulator. β i >0 represents a constant, It is expressed as the upper limit of the energy storage of the energy tank, Expressed as the difference between the actual space velocity and the target space coordinate velocity, ω i is the variable stiffness method of the robot arm, K ic is the fixed stiffness, D i is a fixed damping, k it is the variable stiffness, and The role of is to eliminate the passive impact of high priority tasks on low priority tasks. The role of is to introduce the characteristics of variable impedance parameters.

5. The method for controlling layered variable impedance of a mechanical arm based on an energy tank according to claim 4, characterized in that: In S5, the variable impedance characteristic obtained by using the energy tank is used to expand the hierarchical control model of the robot arm into a hierarchical variable impedance control model of the robot arm, which specifically includes the following steps: S51. Design a hierarchical variable impedance controller for the robotic arm based on the energy tank, and let the overall controller structure be expressed as: Among them, τ i Represented as the controller of each subtask, τ μ It represents the compensation made to counteract the Coriolis or centrifugal force, and g represents the gravity compensation; S52, let the controllers of all subtasks τ i The control inputs have the following form: Among them, the position error of the end of the robot arm in the i-th task is ξ i Represented as the actual position of the end of the robot in the i-th task, ξ i,d It is expressed as the expected position of the end of the robot arm in the i-th task; the speed error of the end of the robot arm in the i-th task It is expressed as the actual speed of the end of the robot arm in the i-th task, is the desired velocity of the end of the robot in the i-th task; ω i x ti It is used to add the variable stiffness method to the control input, which is expressed by equation (23); S53, by designing τ in formula (25) μ Feedback compensation for non-block diagonal terms in μ: in, It is represented as the i-th row and j-th column in μ with dimension m. i ×m j Matrix blocks of ; S54, let μ i,j =-μ j,i T , then equation (27) is a conservative feedback, that is, the transmission power It does not actively generate energy; S55. Due to the Jacobian matrix is reversible, then the external force τ on the joint of the robot arm ext The external force of the end effector of the manipulator in each layered variable impedance control model The relationship between them is expressed as: S56. Substituting equations (25) to (28) into equation (17), we obtain the decoupled closed-loop dynamic equation: S57. For the highest priority task (i=1), from equations (11) and (13), we can get Therefore, the highest priority closed-loop dynamic equation is expressed as: S58, from equation (13) and equation (16), we can get In order to prove the passivity of the entire closed-loop system, we first prove the passivity of the highest priority task (i=1), and select the energy function describing the highest priority closed-loop system as: S59. Taking the derivative of equation (31) yields: S510, connecting the energy tank to the highest priority closed-loop system through the power protection interconnection, and selecting a total energy storage function W1 to be expressed as: W1=V1+T1 (33) S511. Deriving equation (33) and substituting it into equations (22) and (32) can make the highest priority closed-loop system satisfy the passivity condition: Since σ1∈[0,1], we have: So we have: S512: For the closed-loop system of the secondary priority task (i>1), select the energy storage function V i for: S513, connecting the energy tank to the secondary priority closed-loop system through the power protection interconnection, and selecting a total energy storage function W i It is expressed as: W i =V i +T i (38) S514. Deriving equation (38) and substituting it into equation (22) and equation (38) can make the secondary priority closed-loop system satisfy the passivity condition: So we have: S515. For the entire robot arm system, the total energy storage function W is selected as: S516. Substituting equation (9), equation (28), equation (36) and equation (41) into equation (42) can make the robot closed-loop system meet the passivity condition: and and

Images