A variable impedance control method for a robotic arm based on an energy tank containing an obstacle function

Abstract

The present invention relates to the field of mechanical arm control technology, and proposes a mechanical arm variable impedance control method based on an energy tank containing an obstacle function, comprising the steps of: S1: establishing an Euler-Lagrangian model of a mechanical arm with n degrees of freedom, and establishing a mechanical arm dynamics equation in Cartesian space according to the Euler-Lagrangian model; S2: establishing an impedance model based on the mechanical arm dynamics equation according to the error between the current position of the end effector of the mechanical arm and the ideal equilibrium position; S3: adding an energy tank to the impedance model to generate a variable stiffness impedance model corresponding to the mechanical arm; S4: obtaining the state information of the mechanical arm, generating the joint torque of the mechanical arm, and controlling the mechanical arm according to the joint torque. By designing the energy tank and controlling the obstacle function, it can be ensured that the energy stored in the energy tank does not exceed the predetermined boundary, extracting the required energy from the energy tank, compensating for the non-passive behavior, and thus maintaining the overall passivity of the system.

Description

Technical Field

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

[0002] The control of the robotic arm is divided into fixed impedance control and variable impedance control. The traditional impedance control method is challenging in unstructured environments because factors such as the mass and friction of the object will affect the control force. Variable impedance control (VIC) has significant advantages. Variable impedance control (VIC) can adjust the impedance parameters of the robot in real time to cope with different motion requirements and external forces.

[0003] While many studies have addressed the challenge of obtaining impedance parameters, these studies have demonstrated the practicality of VIC and proposed several different methods to obtain impedance parameter information. When performing tasks, it is a generally desired goal to adhere to predefined impedance parameters while ensuring the passivity of the system. Some studies have proposed a passive variable stiffness method based on an energy tank, which can make the impedance parameters change in a prescribed form, but the excessive accumulation of energy in the energy tank and its rapid release during human-computer interaction may cause a series of safety issues. Some studies have proposed a passivity assurance method based on energy tank control and control barrier function. Whenever some desired control actions may destroy the passivity of the system, the designed control barrier function will modify the behavior to force passivity. This method avoids system instability caused by the sudden increase of tank energy to a certain extent, but sacrifices the performance of the system in performing tasks, which is unacceptable in some cases. Therefore, how to maintain the passivity of the system and the energy stored in the energy tank does not exceed the predetermined boundaries has become an urgent problem to be solved. Summary of the invention

[0004] In view of the above-mentioned defects, the purpose of the present invention is to propose a variable impedance control method for a robotic arm based on an energy tank containing an obstacle function, aiming to maintain the passivity of the system and the energy stored in the energy tank not exceeding a predetermined limit, thereby maintaining stability.

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

[0006] A variable impedance control method for a mechanical arm based on an energy tank containing an obstacle function comprises the steps of:

[0007] S1: Establishing an Euler-Lagrangian model of a manipulator with n degrees of freedom, and establishing a dynamic equation of the manipulator in Cartesian space according to the Euler-Lagrangian model, wherein the dynamic equation of the manipulator includes an input vector in Cartesian space;

[0008] S2: establishing an impedance model based on the dynamic equation of the robotic arm according to the error between the current position of the end effector of the robotic arm and the ideal equilibrium position;

[0009] S3: adding an energy tank to the impedance model to generate a variable stiffness impedance model corresponding to the robotic arm, wherein the energy tank prevents energy from crossing the boundary through a barrier function, and the energy tank is used to store energy dissipated by the robotic arm;

[0010] S4: Acquire state information of the robotic arm, generate a joint torque of the robotic arm according to the state information, the robotic arm dynamics equation and the variable stiffness impedance model, and control the robotic arm according to the joint torque.

[0011] Preferably, in step S1, the Euler-Lagrangian model is as shown in formula (1):

[0012]

[0013] Where q is the joint configuration, the inertia matrix M(q) is a positive definite symmetric matrix, and the Coriolis force and centrifugal force matrices are Satisfy the formula g(q) is the gravity torque vector, the generalized force τ is the joint torque of the robot arm, τ ext is the external torque vector, represents the change of the moment of inertia of the system over time, and T represents the transpose of the matrix;

[0014] The mechanical arm dynamics equation established in Cartesian space according to the Euler-Lagrangian model is shown in formula (2):

[0015]

[0016] Where ξ is the coordinate of the end effector configuration of the robot arm in the Cartesian coordinate system, is the speed of the end effector of the robot arm, is the acceleration of the end effector of the robot arm, F ext is the generalized external force vector, F τ is the input vector, through τ=J(q) T F τ Associated with τ, J(q) represents the Jacobian matrix of the robot, F g (ξ) is the equivalent gravity vector, and g(q) = J(q) T F g (ξ), the matrix Λ(ξ) and The inertia matrix and the Coriolis force / centrifugal force matrix about the coordinate ξ, respectively, where:

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

[0018]

[0019] Among them, M(q) -1 is the inverse matrix of the inertia matrix M(q), J(q) T represents the transpose of the Jacobian matrix of the robot, J(q) -T represents the inverse matrix of the transposed Jacobian matrix of the robot, Represents the derivative of the Jacobian matrix of the robot.

[0020] Preferably, in step S2, the current position of the end effector of the robotic arm is ξ, and the ideal equilibrium position of the end effector of the robotic arm is ξ d , the error between the current position of the end effector of the robot and the ideal equilibrium position is When the generalized external force vector F ext When it can be obtained through detection, an impedance model is established based on the mechanical arm dynamics equation as shown in formula (3):

[0021]

[0022] Where K d >0,D d >0,Λ d >0 are the stiffness matrix, damping matrix and inertia matrix required to achieve the ideal impedance behavior, that is, K d is the impedance parameter;

[0023] When the generalized external force vector F ext When it cannot be obtained through detection, an impedance model is established based on the mechanical arm dynamics equation as shown in formula (4):

[0024]

[0025] Among them, the matrix Λ(ξ) is the inertia matrix about the coordinate ξ, ξ is the coordinate of the end effector configuration of the manipulator in the Cartesian coordinate system, is the speed of the end effector of the robot arm, is the error velocity between the current position of the end effector of the robot and the ideal equilibrium position, is the error acceleration between the current position of the end effector of the robot and the ideal equilibrium position, is the Coriolis force / centrifugal force matrix.

[0026] Preferably, in step S3, the energy tank E(x t ) is shown in formula (5):

[0027]

[0028] Among them, x t is the state of the energy tank, t is the time;

[0029] Energy changes of energy tanks As shown in formula (6):

[0030]

[0031] in, ∈ is the threshold value at which energy cannot be extracted from the energy tank under variable stiffness, ω is a term related to variable stiffness, is the energy boundary of the energy tank, is the error velocity between the current position of the end effector of the robot and the ideal equilibrium position, is the transpose of the error velocity, β is the adjustment variable, Dd is the damping matrix required to achieve the ideal impedance behavior, ω T is the transpose of the term associated with variable stiffness;

[0032] The energy tank E(x t ) with respect to time is Combining formula (6), the barrier function is:

[0033] Furthermore, before generating the variable stiffness impedance model corresponding to the robot arm in step S3, the input control model is also designed. The input control model is shown in formula (7):

[0034]

[0035] Among them, F g (ξ) is the equivalent gravity vector, ξ is the coordinate of the end effector configuration of the manipulator in the Cartesian coordinate system, is the speed of the end effector of the robot arm, is the acceleration of the end effector of the robot arm, the matrix Λ(ξ) and The inertia matrix and the Coriolis force / centrifugal force matrix about the coordinate ξ, K c ≥0 is a constant stiffness gain, D d To achieve the desired impedance behavior, the damping matrix x t is the state of the energy tank, and ω is a term related to the variable stiffness.

[0036] Furthermore, in step S3, the variable stiffness impedance model corresponding to the robot arm is generated as follows: Combining formula (2) and formula (7), the variable stiffness impedance model corresponding to the robot arm is generated as shown in formula (8):

[0037]

[0038] Among them, the matrix Λ(ξ) and The inertia matrix and the Coriolis force / centrifugal force matrix about the coordinate ξ, D d To achieve the desired impedance behavior, the damping matrix x t is the state of the energy tank, ω is the term related to variable stiffness, is the error between the current position of the end effector of the robot and the ideal equilibrium position, is the error velocity between the current position of the end effector of the robot and the ideal equilibrium position, is the error acceleration between the current position of the end effector of the robot and the ideal equilibrium position, K c ≥0 means constant stiffness gain.

[0039] Preferably, the state information includes the joint positions, joint velocities, ideal equilibrium positions and the position of the end effector of the robot arm;

[0040] In step S4, the joint torque of the robotic arm is generated according to the state information, the robotic arm dynamics equation and the variable stiffness impedance model as follows:

[0041] Substitute the state information into the robot arm dynamics equation in the Cartesian space and the variable stiffness impedance model to obtain the input vector F in the Cartesian space τ , according to the relationship τ=J(q) T F τ , calculate the input joint torque τ;

[0042] Where J(q) represents the Jacobian matrix of the robot arm, J(q) T is the transpose of the Jacobian matrix of the robot.

[0043] A variable impedance control system for a robotic arm based on an energy tank containing an obstacle function, the control system comprising:

[0044] A module for establishing the dynamic equations of a robotic arm, used to establish an Euler-Lagrangian model of a robotic arm with n degrees of freedom, and to establish the dynamic equations of the robotic arm in Cartesian space according to the Euler-Lagrangian model;

[0045] An impedance model building module, used to build an impedance model based on the mechanical arm dynamics equation according to the error between the current position of the end effector of the mechanical arm and the ideal equilibrium position;

[0046] A variable stiffness impedance model generation module is used to add an energy tank to the impedance model to generate a variable stiffness impedance model corresponding to the robotic arm, wherein the energy tank prevents energy from crossing the boundary through a barrier function, and the energy tank is used to store energy dissipated by the robotic arm;

[0047] The robot arm control module is used to obtain the state information of the robot arm, generate the joint torque of the robot arm according to the state information, the robot arm dynamics equation and the variable stiffness impedance model, and control the robot arm according to the joint torque.

[0048] A terminal device comprises: a memory, a processor and a program stored in the memory and executable on the processor, wherein the program is configured to implement any one of the steps of the variable impedance control method for a robotic arm as described above.

[0049] A storage medium, characterized in that a robot arm control program is stored on the storage medium, and when the robot arm control program is executed by a processor, the steps of the robot arm variable impedance control method as described above are implemented.

[0050] One of the above technical solutions has the following advantages or beneficial effects:

[0051] The required energy can be extracted from the energy tank to compensate for non-passive behavior, thereby maintaining the overall passivity of the system. By designing the energy tank and controlling the barrier function, it can be ensured that the energy stored in the energy tank does not exceed the predetermined boundary, which helps to improve the safety of the system and reduce the impact of uncertainty factors on the stability of the system. While maintaining passivity and performance during mission execution, it avoids instability caused by energy mutations. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0053] Figure 1 is a structural schematic diagram of a variable impedance control method for a mechanical arm provided by an embodiment of the present invention;

[0054] Figure 2 is a schematic structural diagram of a mechanical arm variable impedance control system provided by an embodiment of the present invention;

[0055] Figure 3 is a diagram of expected trajectory and actual trajectory along the y direction of the standard impedance tracking performance of the variable impedance control method for a manipulator provided by an embodiment of the present invention;

[0056] Figure 4 It is a diagram of expected trajectory and actual trajectory along the z direction of the standard impedance tracking performance of the variable impedance control method of the manipulator provided by an embodiment of the present invention;

[0057] Figure 5 is a tracking error diagram along the y and z directions of the standard impedance tracking performance of the variable impedance control method for a manipulator provided by an embodiment of the present invention;

[0058] Figure 6 It is a diagram of the expected trajectory and the actual trajectory along the y direction of the variable impedance tracking performance of the energy tank based on the obstacle function of the variable impedance control method of the manipulator provided by the embodiment of the present invention;

[0059] Figure 7 It is a diagram of the expected trajectory and the actual trajectory along the z direction of the variable impedance tracking performance of the energy tank based on the obstacle function of the variable impedance control method of the manipulator provided by the embodiment of the present invention;

[0060] Figure 8 is a tracking error diagram along the y and z directions of the variable impedance tracking performance of the energy tank based on the obstacle function in the variable impedance control method of the manipulator provided by the embodiment of the present invention;

[0061] Fig. 9 is a time variation diagram of the impedance stiffness using standard impedance control of the variable impedance control method of the manipulator provided by an embodiment of the present invention;

[0062] Fig.10 is a time variation diagram of the impedance stiffness of an energy tank used in the variable impedance control method of a mechanical arm provided by an embodiment of the present invention;

[0063] Fig.11 is a time variation diagram of the energy of an energy tank based on an obstacle function in a variable impedance control method for a mechanical arm provided by an embodiment of the present invention;

[0064] Fig.12 It is a time variation diagram of the energy of the energy tank of the variable impedance control method of the robot arm provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0065] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be understood as limiting the present invention.

[0066] In the description of the present invention, it is to be understood that the terms “center”, “longitudinal”, “lateral”, “length”, “width”, “thickness”, “up”, “down”, “left”, “right”, “vertical”, “horizontal”, “top”, “bottom”, “inside”, “outside”, “axial”, “radial”, “circumferential”, etc., indicating orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the present invention.

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

[0068] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a direct connection, or an indirect connection through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0069] like Figure 1 As shown, a preferred embodiment of the present application includes the steps of:

[0070] S1: Establishing an Euler-Lagrangian model of a manipulator with n degrees of freedom, and establishing a dynamic equation of the manipulator in Cartesian space according to the Euler-Lagrangian model, wherein the dynamic equation of the manipulator includes an input vector in Cartesian space;

[0071] Among them, degrees of freedom refer to the number of independent directions or independent parameters that can move freely in a system. In a robotic arm, the degrees of freedom refer to the number of joints of the robotic arm, that is, the number of joints that can be independently controlled. In three-dimensional space, the degrees of freedom of a robotic arm usually refer to the number of independent movements that can be achieved, including rotation and translation. For example, a six-degree-of-freedom robotic arm may include three rotational joints and three translational joints, each of which can move freely in a specific direction. Such a robotic arm can achieve more flexible and complex movements in three-dimensional space.

[0072] Specifically, the Euler-Lagrangian model is used to describe the dynamic characteristics of the mechanical system, including factors such as the mass, inertia, and constraints of the robot, as well as the external forces and torques applied to the robot. Through the Euler-Lagrangian method, the dynamic equations of the robot in the joint space can be obtained to describe the motion state of each joint of the robot and the external torque it is subjected to. By establishing the Euler-Lagrangian model in Cartesian space, the motion description of the robot can be converted into the motion description of the end effector in the Cartesian coordinate system, so as to more intuitively understand and control the motion of the robot. In many application scenarios, tasks and control requirements are often based on descriptions in Cartesian space. Therefore, establishing the dynamic equations of the robot in Cartesian space can more conveniently connect with task requirements and controller design. The dynamic equations of the robot in Cartesian space can be used as the basis for controller design. By controlling information such as the position, velocity, and acceleration of the end effector, precise control of the robot can be achieved.

[0073] S2: establishing an impedance model based on the dynamic equation of the robotic arm according to the error between the current position of the end effector of the robotic arm and the ideal equilibrium position;

[0074] Among them, the error between the current position of the end effector of the robot and the ideal equilibrium position is a measure used to evaluate the deviation between the current state of the robot and the desired state. The existence of this error enables the control system to identify and adjust the robot to keep it close to or maintain the desired position. This error is used as the input of the controller, and the controller generates corresponding control commands according to the size and rate of change of the error to adjust the motion state of the robot.

[0075] Specifically, the goal of the impedance model is to make the robot arm have a certain flexibility and stability to external disturbances while being able to stay near the desired motion trajectory. Through impedance control, the robot arm can show controllable softness when interacting with the outside world, so as to adapt to various tasks and working environments. By comparing the current position error of the robot arm with the desired position and establishing an impedance model based on the dynamic equation of the robot arm, the goal of the impedance model is to make the robot arm have a certain flexibility and stability to external disturbances while being able to stay near the desired motion trajectory. By comparing the current position error of the robot arm with the desired position and establishing an impedance model based on the dynamic equation of the robot arm, the robot arm can make an appropriate response to external disturbances and achieve the desired motion control.

[0076] S3: adding an energy tank to the impedance model to generate a variable stiffness impedance model corresponding to the robotic arm, wherein the energy tank prevents energy from crossing the boundary through a barrier function, and the energy tank is used to store energy dissipated by the robotic arm;

[0077] Specifically, the energy tank is used to store the energy dissipated by the robot arm. During the movement of the robot arm, the robot arm will dissipate energy due to various factors. At this time, the energy tank acts as an energy storage device, which can inject or absorb energy into the robot arm when needed to maintain the stability of the system. By using a barrier function to prevent energy from going out of bounds, the energy tank ensures that the energy of the robot arm remains within a safe range, which helps prevent the robot arm from causing unstable or unpredictable behavior due to excessive dissipation or energy out of control.

[0078] The variable stiffness impedance model combines the concepts of impedance control and energy tanks to achieve flexibility control of the robot arm. By adjusting the impedance parameters in the model and changing them in a prescribed form, the stiffness and softness of the robot arm in different working scenarios can be adjusted. This allows the robot arm to exhibit high stiffness when needed to provide precise control, while exhibiting softness when interacting with human operations or irregular environments to reduce the risk of injury to the external environment or personnel. The variable stiffness impedance model can also achieve active adjustment and response to the interaction force between the robot arm and the external environment, improving its applicability and flexibility.

[0079] S4: acquiring state information of the robotic arm, generating a joint torque of the robotic arm according to the state information, the robotic arm dynamics equation and the variable stiffness impedance model, and controlling the robotic arm according to the joint torque;

[0080] After establishing the relevant model, in the control of the manipulator, the acquisition of state information is the key first step, which usually includes information such as the position, velocity, acceleration and target position of the manipulator. This information is acquired in real time through sensors and provided to the control system. After acquiring the state information, the state information is first substituted into the variable stiffness impedance model to calculate the impedance of the current manipulator, and then this impedance is substituted into the manipulator dynamics equation in Cartesian space, and the impedance is converted to the joint space through the Jacobian matrix, so as to calculate the torque of the joint and control the manipulator.

[0081] In another preferred embodiment of the present application, in step S1, the Euler-Lagrangian model is as shown in formula (1):

[0082]

[0083] Where q∈R n is the joint configuration, the inertia matrix M(q)∈R n×n is a positive definite symmetric matrix, the Coriolis force and centrifugal force matrices satisfy g(q)∈R n is the gravitational moment vector, the generalized force τ∈R n is the joint torque of the robot arm, τ ext ∈Rn is the external torque vector, represents the change of the moment of inertia of the system over time, T represents the transpose of the matrix, R n is an n-dimensional real vector space;

[0084] Specifically, q is the joint configuration, which is an important descriptive parameter of the robot. It provides information about the position of each joint and is the basis for kinematic and dynamic analysis. The Euler-Lagrangian model is described by joint coordinates, which are also included in the state information of the robot. The goal of impedance control is to achieve a specific dynamic relationship between the robot motion and the external torque. Therefore, it is necessary to rewrite the model in Cartesian coordinates.

[0085] The mechanical arm dynamics equation established in Cartesian space according to the Euler-Lagrangian model is shown in formula (2):

[0086]

[0087] where ξ=f(q)∈R m is the coordinate of the end effector configuration of the robot arm in the Cartesian coordinate system, is the speed of the end effector of the robot arm, is the acceleration of the end effector of the robot arm, F ext ∈R m is the generalized external force vector, F τ ∈R m is the input vector, through τ=J(q) T F τ Associated with τ, J(q) represents the Jacobian matrix of the robot, F g (ξ)∈R m is the equivalent gravity vector, and g(q)=J(q) T F g (ξ), matrix Λ(ξ)∈R m×m and The inertia matrix and the Coriolis force / centrifugal force matrix about the coordinate ξ, respectively, where:

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

[0089]

[0090] Specifically, ξ is the position of the end effector of the robot arm, the derivative of ξ is the end velocity of the robot arm effector, and the derivative of ξ is the end acceleration of the robot arm effector. τ Also called the force vector in Cartesian space, we get the generalized external force vector Fext Then the force vector F in Cartesian space can be calculated τ , through the Jacobian matrix J(q) of the robot, the force vector F in Cartesian space is transformed τ Converted to joint torque τ in joint space.

[0091] Furthermore, in step S2, the current position of the end effector of the robotic arm is ξ, and the ideal equilibrium position of the end effector of the robotic arm is ξ d , the error between the current position of the end effector of the robot and the ideal equilibrium position is When the generalized external force vector F ext When it can be obtained through detection, an impedance model is established based on the mechanical arm dynamics equation as shown in formula (3):

[0092]

[0093] Where K d >0,D d >0,Λ d >0 are the stiffness matrix, damping matrix and inertia matrix required to achieve the ideal impedance behavior, that is, K d is the impedance parameter;

[0094] When the generalized external force vector F ext When it cannot be obtained through detection, an impedance model is established based on the mechanical arm dynamics equation as shown in formula (4):

[0095]

[0096] Among them, the matrix Λ(ξ) is the inertia matrix about the coordinate ξ, ξ is the coordinate of the end effector configuration of the manipulator in the Cartesian coordinate system, is the speed of the end effector of the robot arm, is the error velocity between the current position of the end effector of the robot and the ideal equilibrium position, is the error acceleration between the current position of the end effector of the robot and the ideal equilibrium position, is the Coriolis force / centrifugal force matrix.

[0097] Specifically, the generalized external force vector F extAlso known as external torque, external torque can be measured by a force sensor installed at the end of the robot arm. However, not all robot arms can be equipped with force sensors. For example, due to space limitations, shape limitations or other mechanical structural limitations, the design and structure of some robot arms may not be able to accommodate external force sensors. Force sensors usually have a certain weight and volume. If the load capacity or balance of the robot arm is limited, then adding a force sensor may cause imbalance or overweight problems. Therefore, when the end of the robot arm is equipped with a force sensor, the standard impedance model established is shown in formula (3):

[0098]

[0099] Stiffness matrix K d Determines the degree of response of the robot arm to position error. Higher stiffness means that the system will correct the position error faster. d Determines the robot's response to velocity errors. Appropriate damping helps absorb vibrations and stabilize the system and inertia matrix Λ d Determines the acceleration response characteristics of the system. A larger inertia means that the system requires a greater force to change its state of motion.

[0100] When the end of the robot arm is not equipped with a force sensor, that is, the external torque F ext When it cannot be obtained through detection, the impedance model needs to be deformed. The deformed impedance model is shown in formula (4):

[0101]

[0102] In this way, the external torque can be directly calculated through the dynamic model parameters of the robot arm. d When (ξ)=Λ(ξ), F can be avoided ext , there is no need to equip force sensors and it does not rely on specific sensor configurations, so that the same control scheme can be applied to robotic arms of different types and structures, avoiding system failures caused by sensor failures, thereby improving overall reliability.

[0103] In order to verify the performance of the impedance model, this application uses the Franka Emika Panda 7-DOF robot to test the performance of the model. The test results are as follows: Figure 3-5 As shown, Figure 3-5 It is expressed as the difference between the position of the end effector of the robot and the expected trajectory of the impedance control along the y-axis and z-axis directions. It can be seen that the tracking performance of the ordinary impedance model is unstable, so the impedance model needs to be improved.

[0104] In another embodiment of the present application, in order to ensure that the robot arm can maintain its passive nature when performing tasks, while controlling the impact of non-passive control actions, and maintaining the stability and safety of the entire system operation, in step S3, the energy tank E(x t ) is shown in formula (5):

[0105]

[0106] Among them, x t is the state of the energy tank, t is the time;

[0107] Energy changes of energy tanks As shown in formula (6):

[0108]

[0109] in, ∈ ∈R + is the threshold value at which energy cannot be extracted from the energy tank under variable stiffness, ω is a term related to variable stiffness, is the energy boundary of the energy tank, is the error velocity between the current position of the end effector of the robot and the ideal equilibrium position, is the transpose of the error velocity, β is the adjustment variable, D d To achieve the desired impedance behavior, the damping matrix, ω T is the transpose of the term associated with variable stiffness;

[0110] The energy tank E(x t ) with respect to time is Combining formula (6), the barrier function is:

[0111] Among them, the energy change of the energy tank It can also be called the state update function of the energy tank. By analyzing the total energy in the controlled system, the non-passivity of the system can be easily verified. The total energy of the controlled system is regarded as an energy storage function:

[0112]

[0113] Among them, the time derivative of the energy storage function is expressed as:

[0114]

[0115] Obviously, because The sign of is uncertain, so a variable stiffness matrix will result in a loss of passivity. Assume When K d When (t) increases, unstable energy is injected into the system, causing the system to become unstable.

[0116] For this purpose, an energy tank E(x t ) is used to store the unstable energy generated when the impedance changes, the barrier function is used to prevent the energy from crossing the boundary, and the energy tank ensures that the energy of the robot arm remains within a safe range. This conclusion is proved by two parts in this application, among which:

[0117] Lemma: If and ω is bounded, then

[0118] Theorem: For the energy tank E(x t ), under any initial energy storage condition The state update function of the energy tank ensures that the energy of the energy tank remains consistently bounded.

[0119] First, we will prove that the energy storage function of the energy tank And the state update function Construct a homogeneous system in the maximum existence interval [0, δ), and use proof by contradiction to prove that δ = +∞.

[0120] Part I: Defining open sets:

[0121]

[0122] Obviously x t (0)∈Ξ, in addition, and δ are bounded. According to the uniqueness theorem of solutions to differential equations, for any initial condition x t (0), energy tank status update function There is a unique solution x on the maximum existence interval [0, δ) t In other words, for t∈(0,δ), the inequality Established.

[0123] Part 2: In this part, we will use proof by contradiction to prove that δ = +∞. To avoid contradiction, we assume that δ < +∞ and replace the energy tank's state update function with , then the power function of the energy tank can be expressed as:

[0124]

[0125] in

[0126] As shown in the first part, it has been determined According to the extreme value theorem, we can conclude that:

[0127]

[0128] From this formula, we can see that there is a positive constant So that when Furthermore, we can deduce in So for all t∈[0,ω), we can derive:

[0129]

[0130] Therefore, there exists a compact subset Make the state update function of the energy tank There is a unique solution x on the interval [0, δ) t ∈Ξ′. We can conclude that δ=+∞, that is, for all t∈[0,+∞), it means

[0131] The addition of the barrier function keeps the energy of the energy tank within the boundary, that is, When the energy of the energy tank is close to the upper bound of the energy tank, the value corresponding to the barrier function will be very large, thus limiting the energy of the energy tank below the upper bound. The change of parameter ∈ will avoid singular points and prevent x t The singularity caused by ∈ is 0, and the change of stiffness will not affect the value of parameter ∈. Through the above approach, the smooth change of energy tank energy can be guaranteed.

[0132] Further, before generating the variable stiffness impedance model corresponding to the robot arm in step S3, the input control model is also designed. The input control model is shown in formula (7):

[0133]

[0134] Among them, F g (ξ)∈R m is the equivalent gravity vector, ξ=f(q)∈R m is the coordinate of the end effector configuration of the robot arm in the Cartesian coordinate system, is the speed of the end effector of the robot arm, is the acceleration of the end effector of the robot arm, and the matrix Λ(ξ)∈R m×m and The inertia matrix and the Coriolis force / centrifugal force matrix about the coordinate ξ, K c ≥0 is a constant stiffness gain, D d To achieve the desired impedance behavior, the damping matrix x t is the state of the energy tank, and ω is a term related to the variable stiffness.

[0135] Specifically, F τIt can be regarded as a controller including an energy tank. ω can also be regarded as a control input. Through ω, the energy exchange between the variable stiffness impedance model and the energy tank can be controlled. The energy acquisition and release of the energy tank are related to the change of stiffness and the error and error change of the robot arm actuator. The passivity of the system is guaranteed by introducing the energy tank. The additional energy generated by the variable stiffness is stored in the energy tank. This is consistent with the role of ω mentioned above. By controlling the input ω and the state x of the energy tank t Through precise energy management and dynamic stiffness adjustment, the robot arm can move in the expected way while ensuring the stability and safety of the system. τ In some cases, it absorbs energy, while in other cases, it may provide energy. The specific energy flow depends on the current control needs and the state of the robot arm.

[0136] To further illustrate, in step S3, the variable stiffness impedance model corresponding to the robot arm is generated as follows: Combining formula (2) and formula (7), the variable stiffness impedance model corresponding to the robot arm is generated as shown in formula (8):

[0137]

[0138] Among them, the matrix Λ(ξ)∈R m×m and The inertia matrix and the Coriolis force / centrifugal force matrix about the coordinate ξ, D d To achieve the desired impedance behavior, the damping matrix x t is the state of the energy tank, ω is the term related to variable stiffness, is the error between the current position of the end effector of the robot and the ideal equilibrium position, is the error velocity between the current position of the end effector of the robot and the ideal equilibrium position, is the error acceleration between the current position of the end effector of the robot and the ideal equilibrium position.

[0139] Specifically, the expression of ω is:

[0140]

[0141] The impedance parameter K is obtained from this d (t) = K c +K′(t),K c ≥0 means constant stiffness gain.

[0142] To demonstrate passivity, the total kinetic energy is given by:

[0143]

[0144] Therefore, we can derive the time derivative as:

[0145]

[0146] Further we can get:

[0147]

[0148] Then, define an energy storage function:

[0149]

[0150] Since v and E are positive definite, W is also positive definite. The derivative of W with respect to time is:

[0151]

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

[0153]

[0154] This means that the generated variable stiffness impedance model satisfies the passivity condition:

[0155]

[0156] According to the passivity theory, by selecting the parameters of the energy tank so that the entire system dissipates energy independently, the energy dissipation associated with passivity naturally ensures the closed-loop stability and tracking performance of the system.

[0157] Figure 6-8 represents the variable impedance tracking performance of the energy tank based on the obstacle function, i.e., the difference between the position of the end effector of the manipulator and the expected trajectory along the y-axis and z-axis directions under standard impedance control, Figure 9-10 Represents the change of impedance stiffness with time. They all show a sharp increase in stiffness around t=50. Figure 6-10 What is described is that when the system runs for 50s, the stiffness of the robot arm increases sharply from 100N / m to 1105N / m, and the standard impedance control is unstable, while the variable impedance control with the energy tank based on the obstacle function is stable. Fig.11 and Fig.12 It represents the evolution of energy in the energy tank over time. The energy of an ordinary energy tank will exceed the predetermined energy limit over time, while the energy tank with the barrier function added will not exceed the predetermined energy limit.

[0158] It is further explained that the state information includes the joint positions, joint velocities, ideal equilibrium positions and the position of the end effector of the robot arm;

[0159] In step S4, the joint torque of the robotic arm is generated according to the state information, the robotic arm dynamics equation and the variable stiffness impedance model as follows:

[0160] Substitute the state information into the robot arm dynamics equation in the Cartesian space and the variable stiffness impedance model to obtain the input vector F in the Cartesian space τ , according to the relationship τ=J(q) T F τ , calculate the input joint torque τ;

[0161] Where J(q) represents the Jacobian matrix of the robot arm, J(q) T is the transpose of the Jacobian matrix of the robot.

[0162] Specifically, first obtain the state information of the robot arm and substitute the state information into the equations and models established previously. In Cartesian space, the control of the position and force of the end effector of the robot arm is usually more intuitive than directly in the joint space. Using the dynamic equations of Cartesian space, the desired end effector behavior (such as position, velocity, and force) can be easily mapped to the actual robot arm operation.

[0163] Through the Jacobian matrix, the force in Cartesian space is converted into the torque in joint space, so that force control and motion control can be effectively combined, and the control input in Cartesian space is converted into the control input in joint space, so that the robot arm can better adapt to the dynamically changing environment, and respond to external disturbances and uncertainties by adjusting the control strategy in real time, and finally generate the joint torque τ to control the robot arm.

[0164] like Figure 2 As shown, a variable impedance control system 6 for a manipulator based on an energy tank including an obstacle function comprises:

[0165] A mechanical arm dynamics equation establishing module 601 is used to establish an Euler-Lagrangian model of a mechanical arm with n degrees of freedom, and to establish a mechanical arm dynamics equation in Cartesian space according to the Euler-Lagrangian model;

[0166] An impedance model building module 602 is used to build an impedance model based on the mechanical arm dynamics equation according to the error between the current position of the end effector of the mechanical arm and the ideal equilibrium position;

[0167] A variable stiffness impedance model generation module 603 is used to add an energy tank to the impedance model to generate a variable stiffness impedance model corresponding to the manipulator, wherein the energy tank prevents energy from crossing the boundary through a barrier function, and the energy tank is used to store energy dissipated by the manipulator;

[0168] The robot control module 604 is used to obtain the state information of the robot, generate the joint torque of the robot according to the state information, the robot dynamics equation and the variable stiffness impedance model, and control the robot according to the joint torque.

[0169] This embodiment implements the variable impedance control method and implementation process of the robot arm. Please refer to the above embodiments and will not be described in detail here.

[0170] In addition, an embodiment of the present invention further proposes a terminal device, which includes: a memory, a processor, and a program stored in the memory and executable on the processor, wherein the program is configured to implement the steps of the robotic arm variable impedance control method as described above.

[0171] Since the program is configured to implement the steps of the variable impedance control method of the robot arm as described above, the program at least has all the beneficial effects brought about by all the technical solutions of all the aforementioned embodiments, which will not be described one by one here.

[0172] In addition, a storage medium is provided, on which a robot arm control program is stored, and when the robot arm control program is executed by a processor, the steps of the robot arm variable impedance control method as described above are implemented.

[0173] Since all the technical solutions of all the aforementioned embodiments are adopted when the robot arm control program is executed by the processor, it has at least all the beneficial effects brought by all the technical solutions of all the aforementioned embodiments, which will not be described one by one here.

[0174] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "illustrative embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0175] Although the embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the claims and their equivalents.

Claims

1. A variable impedance control method for a robotic arm based on an energy tank containing an obstacle function, characterized in that: Includes steps: S1: Establishing an Euler-Lagrangian model of a manipulator with n degrees of freedom, and establishing a dynamic equation of the manipulator in Cartesian space according to the Euler-Lagrangian model, wherein the dynamic equation of the manipulator includes an input vector in Cartesian space; S2: establishing an impedance model based on the dynamic equation of the robotic arm according to the error between the current position of the end effector of the robotic arm and the ideal equilibrium position; S3: adding an energy tank to the impedance model to generate a variable stiffness impedance model corresponding to the robotic arm, wherein the energy tank prevents energy from crossing the boundary through a barrier function, and the energy tank is used to store energy dissipated by the robotic arm; S4: acquiring state information of the robotic arm, generating a joint torque of the robotic arm according to the state information, the robotic arm dynamics equation and the variable stiffness impedance model, and controlling the robotic arm according to the joint torque; In step S1, the Euler-Lagrangian model is as shown in formula (1): Where q is the joint configuration, the inertia matrix M(q) is a positive definite symmetric matrix, and the Coriolis force and centrifugal force matrices are Satisfy the formula g(q) is the gravity torque vector, the generalized force τ is the joint torque of the robot arm, τ ext is the external torque vector, represents the change of the moment of inertia of the system over time, and T represents the transpose of the matrix; The mechanical arm dynamics equation established in Cartesian space according to the Euler-Lagrangian model is shown in formula (2): Where ξ is the coordinate of the end effector configuration of the robot arm in the Cartesian coordinate system, is the speed of the end effector of the robot arm, is the acceleration of the end effector of the robot arm, F ext is the generalized external force vector, F τ is the input vector, through τ=J(q) T F τ Associated with τ, J(q) represents the Jacobian matrix of the robot, F g (ξ) is the equivalent gravity vector, and g(q) = J(q) T F g (ξ), the matrix Λ(ξ) and The inertia matrix and the Coriolis force / centrifugal force matrix about the coordinate ξ, respectively, where: Λ(ξ)=(J(q)M(q) -1 J(q) T ) -1 Among them, M(q) -1 is the inverse matrix of the inertia matrix M(q), J(q) T represents the transpose of the Jacobian matrix of the robot, J(q) -T represents the inverse matrix of the transposed Jacobian matrix of the robot, Derivatives of the Jacobian matrix representing the manipulator; In step S2, the current position of the end effector of the robot arm is ξ, and the ideal equilibrium position of the end effector of the robot arm is ξ d , the error between the current position of the end effector of the robot and the ideal equilibrium position is When the generalized external force vector F ext When it can be obtained through detection, an impedance model is established based on the mechanical arm dynamics equation as shown in formula (3): Where K d >0,D d >0,Λ d >0 are the stiffness matrix, damping matrix and inertia matrix required to achieve the ideal impedance behavior, that is, K d is the impedance parameter; When the generalized external force vector F ext When it cannot be obtained through detection, an impedance model is established based on the mechanical arm dynamics equation as shown in formula (4): Among them, the matrix Λ(ξ) is the inertia matrix about the coordinate ξ, ξ is the coordinate of the end effector configuration of the manipulator in the Cartesian coordinate system, is the speed of the end effector of the robot arm, is the error velocity between the current position of the end effector of the robot and the ideal equilibrium position, is the error acceleration between the current position of the end effector of the robot and the ideal equilibrium position, is the Coriolis force / centrifugal force matrix; In step S3, the energy tank E(x t ) is shown in formula (5): Among them, x t is the state of the energy tank, t is the time; Energy changes of energy tanks As shown in formula (6): in, ∈ is the threshold value at which energy cannot be extracted from the energy tank under variable stiffness, ω is a term related to variable stiffness, is the energy boundary of the energy tank, is the error velocity between the current position of the end effector of the robot and the ideal equilibrium position, is the transpose of the error velocity, β is the adjustment variable, Dd is the damping matrix required to achieve the ideal impedance behavior, ω T is the transpose of the term associated with variable stiffness; The energy tank E(x t ) with respect to time is Combining formula (6), the barrier function is:

2. The variable impedance control method of a robotic arm according to claim 1, characterized in that: Before generating the variable stiffness impedance model corresponding to the robot arm in step S3, the input control model is also designed. The input control model is shown in formula (7): Among them, F g (ξ) is the equivalent gravity vector, ξ is the coordinate of the end effector configuration of the manipulator in the Cartesian coordinate system, is the speed of the end effector of the robot arm, is the acceleration of the end effector of the robot arm, the matrix Λ(ξ) and The inertia matrix and the Coriolis force / centrifugal force matrix about the coordinate ξ, K c ≥0 is a constant stiffness gain, D d To achieve the desired impedance behavior, the damping matrix x t is the state of the energy tank, and ω is a term related to the variable stiffness.

3. The variable impedance control method of a mechanical arm according to claim 2, characterized in that: In step S3, the variable stiffness impedance model corresponding to the robot arm is generated as follows: Combining formula (2) and formula (7), the variable stiffness impedance model corresponding to the robot arm is generated as shown in formula (8): Among them, the matrix Λ(ξ) and The inertia matrix and the Coriolis force / centrifugal force matrix about the coordinate ξ, D d To achieve the desired impedance behavior, the damping matrix x t is the state of the energy tank, ω is the term related to variable stiffness, is the error between the current position of the end effector of the robot and the ideal equilibrium position, is the error velocity between the current position of the end effector of the robot and the ideal equilibrium position, is the error acceleration between the current position of the end effector of the robot and the ideal equilibrium position, K c ≥0 means constant stiffness gain.

4. The variable impedance control method of a mechanical arm according to claim 3, characterized in that: The state information includes the joint positions, joint velocities, ideal equilibrium positions and the position of the end effector of the robot arm; In step S4, the joint torque of the robotic arm is generated according to the state information, the robotic arm dynamics equation and the variable stiffness impedance model as follows: Substitute the state information into the robot arm dynamics equation in the Cartesian space and the variable stiffness impedance model to obtain the input vector F in the Cartesian space τ , according to the relationship τ=J(q) T F τ , calculate the input joint torque τ; Where J(q) represents the Jacobian matrix of the robot arm, J(q) T is the transpose of the Jacobian matrix of the robot.

5. A variable impedance control system for a robotic arm based on an energy tank containing an obstacle function, characterized in that: The control system comprises: A module for establishing the dynamic equations of a robotic arm, used to establish an Euler-Lagrangian model of a robotic arm with n degrees of freedom, and to establish the dynamic equations of the robotic arm in Cartesian space according to the Euler-Lagrangian model; An impedance model building module, used to build an impedance model based on the mechanical arm dynamics equation according to the error between the current position of the end effector of the mechanical arm and the ideal equilibrium position; A variable stiffness impedance model generation module is used to add an energy tank to the impedance model to generate a variable stiffness impedance model corresponding to the robotic arm, wherein the energy tank prevents energy from crossing the boundary through a barrier function, and the energy tank is used to store energy dissipated by the robotic arm; A robotic arm control module, used to obtain state information of the robotic arm, generate a joint torque of the robotic arm according to the state information, the robotic arm dynamics equation and the variable stiffness impedance model, and control the robotic arm according to the joint torque; The Euler-Lagrangian model is shown in formula (1): Where q is the joint configuration, the inertia matrix M(q) is a positive definite symmetric matrix, and the Coriolis force and centrifugal force matrices are Satisfy the formula g(q) is the gravity torque vector, the generalized force τ is the joint torque of the robot arm, τ ext is the external torque vector, represents the change of the moment of inertia of the system over time, and T represents the transpose of the matrix; The mechanical arm dynamics equation established in Cartesian space according to the Euler-Lagrangian model is shown in formula (2): Where ξ is the coordinate of the end effector configuration of the robot arm in the Cartesian coordinate system, is the speed of the end effector of the robot arm, is the acceleration of the end effector of the robot arm, F ext is the generalized external force vector, F τ is the input vector, through τ=J(q) 丁 F τ Associated with τ, J(q) represents the Jacobian matrix of the robot, F g (ξ) is the equivalent gravity vector, and g(q) = J(q) T F g (ξ), the matrix Λ(ξ) and The inertia matrix and the Coriolis force / centrifugal force matrix about the coordinate ξ, respectively, where: Λ(ξ)=(J(q)M(q) -1 J(q) T ) -1 Among them, M(q) -1 is the inverse matrix of the inertia matrix M(q), J(q) T represents the transpose of the Jacobian matrix of the robot, J(q) -T represents the inverse matrix of the transposed Jacobian matrix of the robot, Derivatives of the Jacobian matrix representing the manipulator; The current position of the end effector of the robot arm is ξ, and the ideal equilibrium position of the end effector of the robot arm is ξ d , the error between the current position of the end effector of the robot and the ideal equilibrium position is When the generalized external force vector F ext When it can be obtained through detection, an impedance model is established based on the mechanical arm dynamics equation as shown in formula (3): Where K d >0,D d >0,Λ d >0 are the stiffness matrix, damping matrix and inertia matrix required to achieve the ideal impedance behavior, that is, K d is the impedance parameter; When the generalized external force vector F ext When it cannot be obtained through detection, an impedance model is established based on the mechanical arm dynamics equation as shown in formula (4): Among them, the matrix Λ(ξ) is the inertia matrix about the coordinate ξ, ξ is the coordinate of the end effector configuration of the manipulator in the Cartesian coordinate system, is the speed of the end effector of the robot arm, is the error velocity between the current position of the end effector of the robot and the ideal equilibrium position, is the error acceleration between the current position of the end effector of the robot and the ideal equilibrium position, is the Coriolis force / centrifugal force matrix; The energy tank E(x t ) is shown in formula (5): Among them, x t is the state of the energy tank, t is the time; Energy changes of energy tanks As shown in formula (6): Where, ∈ is the threshold at which energy cannot be extracted from the energy tank under variable stiffness, ω is a term related to variable stiffness, is the energy boundary of the energy tank, is the error velocity between the current position of the end effector of the robot and the ideal equilibrium position, is the transpose of the error velocity, β is the adjustment variable, D d To achieve the desired impedance behavior, the damping matrix, ω 丁 is the transpose of the term associated with variable stiffness; The energy tank E(x t ) with respect to time is Combining formula (6), the barrier function is:

6. A terminal device, characterized in that: The terminal device comprises: a memory, a processor, and a program stored in the memory and executable on the processor, wherein the program is configured to implement the steps of the variable impedance control method for a robotic arm as described in any one of claims 1 to 4.

7. A storage medium, characterized in that: The storage medium stores a robot arm control program, and when the robot arm control program is executed by the processor, the steps of the robot arm variable impedance control method according to any one of claims 1 to 4 are implemented.

Images