A mechanical arm safety obstacle avoidance control method and system based on a high-order control obstacle function

Abstract

The application relates to a mechanical arm safety obstacle avoidance control method and system based on a high-order control obstacle function, and is suitable for a multi-degree-of-freedom mechanical arm system with obstacles in a task trajectory space. A high-order control obstacle function suitable for a high relative order system is used. The method comprises the following steps: a dynamics model of the multi-degree-of-freedom mechanical arm is established; a stable tracking controller is designed based on a task trajectory and a backstepping control method; an obstacle collision detection model is established, and the high-order control obstacle function is used to provide safety constraints for the control system; and the optimal control input is solved through quadratic programming by combining the control constraints and the optimal control problem. The application can realize optimal task trajectory tracking of the mechanical arm end under the premise of meeting safety constraints, effectively reduce the calculation complexity and guarantee real-time performance.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot technology, in particular to a mechanical arm safety obstacle avoidance control method and system based on high-order control barrier function. TECHNICAL BACKGROUND

[0002] With the wide application of robot technology, safety has become an important issue in the field of robots, and it is a challenge to design a control scheme that can complete the specified task without affecting safety. At present, many algorithms have focused on the obstacle avoidance problem of robots, such as global path planning algorithm which provides an optimal path from the starting point to the end point, however, this algorithm can only be applied to the case where global information is known. The virtual force field method based on artificial potential field and grid method can realize real-time obstacle avoidance of robots, but this method has large information storage and the obtained controller is not optimal. Hamilton Jacobi reachability analysis method is a formal method for verifying the safety of dynamic systems, which drives the dynamic system to the target set under the premise of avoiding the influence of bounded uncertainty on the unsafe set, however, the reachability analysis becomes difficult to calculate with the increase of system dimension.

[0003] The barrier function method can efficiently solve the safety analysis and control problem of nonlinear control system, and can avoid calculating the state trajectory or reachable set of the system. The safety analysis of nonlinear system can be transformed into a convex optimization problem through barrier function, which can effectively reduce the computational complexity and conservativeness of safety analysis; and for safety control design, the safety constraints can be embedded into the nominal controller through control barrier function, which can effectively guarantee the real-time performance of the control algorithm.

[0004] Compared with traditional position control and speed control, torque control of robots has wide development prospects. For torque control of complex robot systems, high-order control barrier function needs to be designed to realize safety control of high relative order systems. SUMMARY

[0005] In order to overcome the safety obstacle avoidance problem of high relative order robot control system existing in the prior art, the present application provides a mechanical arm safety obstacle avoidance control method and system based on high-order control barrier function, which realizes optimal task trajectory tracking of multi-degree-of-freedom robot end effector under the premise of meeting safety constraints.

[0006] In order to solve the above technical problems, the technical scheme adopted by the present application is as follows:

[0007] The first aspect of the present application relates to a mechanical arm safety obstacle avoidance control method based on high-order control barrier function, comprising the following steps:

[0008] Step 1, a dynamic model of a multi-degree-of-freedom robot arm is established, and the dynamic equation in joint space is transformed into Cartesian space;

[0009] Step 2, a stable tracking controller is designed based on the task trajectory and the backstepping control method, the task trajectory is planned in Cartesian space, the nonlinear system is decomposed into a series of subsystems not exceeding the order of the system, and Lyapunov functions are designed step by step;

[0010] Step 3, a distance-based obstacle collision detection model is established to describe the high-order control barrier function, define a safe set, introduce function, define auxiliary functions by step-by-step derivation, and design a high-order control barrier function to control the constraint set;

[0011] Step 4, combining control constraints and optimal control problems, the optimal control input is solved by quadratic programming.

[0012] The dynamic model of the multi-degree-of-freedom robot arm in step 1 comprises:

[0013] Step 1.1, the dynamic equations in joint space and Cartesian space of the robot arm are established:

[0014] For a robot arm with n degrees of freedom, the motion control mode is torque control, and the dynamic model has been identified, and the time-varying dynamic parameters can be obtained online;

[0015] The dynamic equation in joint space of the robot arm is

[0016]

[0017] wherein, represents the torque vector of each joint, represents the angle, angular velocity and angular acceleration vector of each joint, represents the symmetric positive definite mass matrix of the robot arm, represents the centrifugal force and Coriolis force vector, represents the gravity vector;

[0018] The dynamic equation in Cartesian space of the robot arm is

[0019]

[0020] wherein, represents the torque vector acting on the end effector, represents the Cartesian vector of the end effector pose, represents the Cartesian mass matrix, represents the velocity term vector in Cartesian space, represents the gravity term vector in Cartesian space;

[0021] Step 1.2 transforms the dynamics equation in joint space of the manipulator to Cartesian space:

[0022] Given the dynamics equation in joint space of the manipulator, considering the position of the obstacle described in Cartesian coordinates, it is transformed to Cartesian space through the Jacobian matrix for the convenience of calculation;

[0023] Virtual force acting on the end effector It can be expressed by the driving force τ of the joint driver:

[0024]

[0025] Where J(q) is the Jacobian matrix; from equation (3) and Derivation, the relationship between the coefficients in equation (1) and equation (2) is:

[0026]

[0027] Wherein, the combination of the backstepping control method described in step 2 to design a stable tracking controller includes:

[0028] Step 2.1 plans a task trajectory in Cartesian space:

[0029] A path from the starting point x0 to the terminal point x e in Cartesian space is planned to obtain a task trajectory, and the desired position, velocity and acceleration of the manipulator at each time on the path are designed;

[0030] Step 2.2 designs a stable tracking controller for each order system by combining backstepping control:

[0031] The position x and velocity of the end effector are selected as the state vector, and a state space model of equation (2) is realized:

[0032]

[0033] Wherein, is the system state, is the system control input, y=x is the system control output; u first appears in the second-order differential equation of the output x, so the relative order m=2 of the system decomposes the system into a first-order subsystem and a second-order system;

[0034] Starting from the first-order subsystem , x2 is defined as a virtual input of the system, the error e1=x1-x 1d is constructed, and the Lyapunov function its derivative with respect to time According to Lyapunov stability criterion theorem, if the system is stable, it is necessary to ensure that Introducing the parameter k1>0, let That is Always true, design x2 tracking value x 2d For

[0035]

[0036] For a second-order system, define the error e2=x2-x 2d , construct Lyapunov function Derive V2 and substitute And equation (6):

[0037]

[0038] In order to ensure Introducing the parameter k2>0, let

[0039]

[0040] And since Substitute And equation (6):

[0041]

[0042] Substitute equation (9) into equation (8), the stable tracking controller of the system (also known as the nominal controller) For

[0043]

[0044] Where k1 and k2 are adjustable control parameters and are positive.

[0045] Wherein, the high-order control barrier function described in step 3 provides safety constraints for the control system, including:

[0046] Step 3.1 establishes a distance-based barrier collision detection model to describe the high-order control barrier function:

[0047] For an affine control system:

[0048]

[0049] Where, is the system state, is a local Lipschitz function, (U denotes a closed control constraint set), the initial state x(t0), the solution x(t) of equation (11) for t≥t0 is forward complete; in particular, the solution x(t) of equation (5) for t≥t0 is forward complete if the initial state x(t0) is in the set C defined by equation (10).

[0050] First, a simplified geometric model of the robot arm and obstacles in its working environment is established, and a plane or an envelope sphere is used to approximate the obstacles, so that the collision detection between the robot arm and the obstacles is converted into a problem of judging whether the minimum distance is positive or not, effectively reducing the computational complexity; a candidate high-order control barrier function (HOCBF) is defined: m=2 order continuous differentiable function b(x):

[0051] b(x)=d safe -d min (x) (12)

[0052] where d min (x) is the minimum distance from the end effector of the robot arm to the obstacle, and d safe is the safety distance set, and the system is safe when b(x)≥0;

[0053] Step 3.2 defines the safety set:

[0054] Safety is constructed in the context of forced set invariance, i.e. not leaving the safety set; the safety set C is defined as the super zero level set of the continuous differentiable function b(x):

[0055]

[0056] If the solution starting from any x(t0)∈C satisfies then the set C is forward invariant, i.e. the system (11) is safe for the set C;

[0057] Step 3.3 introduces class functions, and auxiliary functions are defined by successive differentiation:

[0058] If a Lipschitz continuous function α:[0,a)→[0,∞),a>0 is strictly increasing, and α(0)=0, then the function belongs to class function; select class functions α1(·), α2(·), and define auxiliary functions as

[0059]

[0060] The corresponding safety set sequence C i is:

[0061] ​

[0062] Step 3.4 Design high-order control barrier function control constraint set:

[0063] If there exist Class functions α1(·), α2(·) such that

[0064]

[0065] For all (x, t) ∈ C1(t)∩C2(t)∩C3(t)×[t0, ∞), the function b(x, t) is a HOCBF of system (11), where And Respectively, along f and g, the Lie derivative with respect to x, O(b(x, t)) is as follows

[0066]

[0067] Indicates that ψ1 is the argument of α1(·), that is, α1(ψ1);

[0068] The HOCBF control constraint set is:

[0069]

[0070] In the control input u in the set K hocbf , the safety of the system can be guaranteed.

[0071] Wherein, the combination of the control constraint and the optimal control problem described in step 4 is solved by quadratic programming to obtain the optimal control input, including:

[0072] Step 4.1 Combination of control constraint and optimal control problem:

[0073] In order to minimize the change of control input to track the task trajectory, the constraint (18) is combined with the optimal control problem to obtain HOCBF-QP:

[0074]

[0075] Wherein, u * is the objective function, satisfies the inequality constraint, u is the optimization variable to be solved, u norm is the control input of the nominal controller; When the system state is in the safe set C, the solution of the problem is u * = u norm , which indicates that the system control input is determined by the nominal controller; When the system state leaves the safe set C, the forced control constraint is added, and the tracking performance is sacrificed to meet the safety until the manipulator is far away from the dangerous area;

[0076] Step 4.2 solves the quadratic programming and obtains the real-time optimal control input:

[0077] The problem is solved at each time step, and the time interval [t0,t f ] is divided into a series of unequal time intervals

[0078] {[t0,t0+Δt1),[t0+Δt1,t0+Δt1+Δt2),...}(20)where Δt i >0; in each interval Holding the state constant at its value at the beginning of the interval and assuming that the control is constant, the optimization problem is formulated as a sequence of QPs; specifically, Solving QP:

[0079] The standard form of QP is

[0080]

[0081] For a symmetric positive definite matrix H and a constant vector F, since

[0082]

[0083] Ignore the last constant term, H=2I n ,

[0084] Convert the inequality constraint into the form of Ax≤B, then

[0085]

[0086]

[0087] Substitute H, F, A, and B into the quadratic programming solver, and the resulting solution vector is the virtual control torque of the end effector of the manipulator in the Cartesian space in the current time period. Then convert it to the joint space, which is the optimal control input torque τ of the joint actuator * , that is, the safety controller of the robotic arm is

[0088] A second aspect of the present invention relates to a safe obstacle avoidance control system for a robotic arm based on a high-order control obstacle function, comprising:

[0089] The dynamic model building module is used to build the dynamic model of the multi-degree-of-freedom robotic arm and transform the dynamic equations in the joint space into the Cartesian space;

[0090] A stable tracking controller design module designs a stable tracking controller based on a task trajectory and a backstepping control method, plans a task trajectory in a Cartesian space, decomposes a nonlinear system into a series of subsystems not exceeding the order of the system, and designs a Lyapunov function step by step;

[0091] A high-order control barrier function constraint establishment module establishes a distance-based obstacle collision detection model for describing a high-order control barrier function, provides a safety constraint for a control system, defines a safety set, introduces a barrier function, defines an auxiliary function by step-by-step derivation, and designs a high-order control barrier function control constraint set;

[0092] An optimal control input solving module combines a control constraint and an optimal control problem, and solves an optimal control input through quadratic programming.

[0093] A third aspect of the application relates to a mechanical arm safety obstacle avoidance control device based on a high-order control barrier function, comprising a memory and one or more processors, the memory stores executable code, and the one or more processors execute the executable code to implement a mechanical arm safety obstacle avoidance control method based on a high-order control barrier function.

[0094] A fourth aspect of the application relates to a computer-readable storage medium having a program stored thereon, which, when executed by a processor, implements a mechanical arm safety obstacle avoidance control method based on a high-order control barrier function.

[0095] The application is directed to a multi-degree-of-freedom mechanical arm, and in the presence of obstacles in the task trajectory, a mechanical arm safety obstacle avoidance control method based on a high-order control barrier function is designed in combination with a backstepping control method, so that the end effector of the mechanical arm can complete optimal task trajectory tracking under the premise of meeting safety constraints.

[0096] The technical concept of the application is that for a multi-degree-of-freedom mechanical arm system with obstacles in the task trajectory space, a high-order control barrier function suitable for high relative order systems is used, and the specific method includes: establishing a dynamics model of the multi-degree-of-freedom mechanical arm; designing a stable tracking controller based on a task trajectory and a backstepping control method; establishing a distance-based obstacle collision detection model to provide a safety constraint for the control system through a high-order control barrier function; combining a control constraint and an optimal control problem, and solving an optimal control input through quadratic programming. This method can realize optimal task trajectory tracking of the end effector of the mechanical arm under the premise of meeting safety constraints, effectively reduce the computational complexity, and ensure real-time performance.

[0097] The beneficial effect of the present application is that in the case of existence of obstacles in the task trajectory, the multi-degree-of-freedom robot arm always meets the safety requirements in the running process, effectively avoids collision with obstacles, and greatly improves the safety of the robot arm operation. Using the high-order obstacle function method, the calculation complexity and conservativeness of safety analysis are effectively reduced, and the real-time performance of the control algorithm is ensured. BRIEF DESCRIPTION OF DRAWINGS

[0098] The drawings described herein are used to provide further understanding of the present application, and form a part of the present application. The illustrative embodiments of the present application and their descriptions serve to explain the present application, and do not constitute an improper limitation on the present application. In the drawings:

[0099] Figure 1 is a schematic diagram of a comparison between the actual position and the desired position of the end effector of the robot arm of the present application over time;

[0100] Figure 2 is a schematic diagram of the xoy plane position trajectory of the end effector of the robot arm of the present application;

[0101] Figure 3 is a schematic diagram of the actual joint driving torque component of the robot arm of the present application over time;

[0102] Figure 4 is a schematic diagram of the pose error of the end effector of the robot arm of the present application over time;

[0103] Figure 5 is a schematic diagram of the simulation trajectory of the robot arm of the present application;

[0104] Figure 6 is a schematic diagram of the control flow of the present application;

[0105] Figure 7 is a schematic diagram of the system of the present application;

[0106] Figure 8 is a schematic diagram of the method of the present application. DETAILED DESCRIPTION

[0107] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0108] Embodiment 1

[0109] Reference Figure 1 and Figure 8The embodiment relates to a mechanical arm safety obstacle avoidance control method based on a high-order control obstacle function, and comprises the following steps.

[0110] Step 1: establishing a dynamics model of a multi-degree-of-freedom mechanical arm

[0111] Step 1.1: establishing dynamics equations of a mechanical arm joint space and a Cartesian space

[0112] For a mechanical arm with n degrees of freedom, the motion control mode is torque control, and the dynamics model has been identified, and time-varying dynamics parameters can be obtained online.

[0113] The dynamics equation of the mechanical arm in the joint space is

[0114]

[0115] wherein, indicates torque vectors of each joint, indicates angle, angular velocity and angular acceleration vectors of each joint,

[0116] indicates a symmetric positive definite mass matrix of the mechanical arm, indicates a centrifugal force and Coriolis force vector, indicates a gravity vector;

[0117] The dynamics equation of the mechanical arm in the Cartesian space is

[0118]

[0119] wherein, indicates torque vectors acting on an end effector, indicates a Cartesian vector of a pose of the end effector, indicates a Cartesian mass matrix, indicates a velocity term vector in the Cartesian space, indicates a gravity term vector in the Cartesian space;

[0120] Step 1.2: transforming the dynamics equation of the mechanical arm joint space to the Cartesian space

[0121] Virtual force acting on the end effector The driving force τ of the joint driver can be indicated as

[0122]

[0123] wherein J (q) is a Jacobian matrix; the virtual force τ is indicated as formula (3) and

[0124] The relationship between the coefficients of each term in formula (1) and formula (2) is derived as follows:

[0125]

[0126] Step 2 combines the backstepping control method to design a stable tracking controller:

[0127] Step 2.1 plans a Cartesian space task trajectory:

[0128] A path from the starting point x0 to the terminal point x e in the Cartesian space is planned to obtain a task trajectory, and the desired position, velocity and acceleration of the manipulator at each time on the path are designed;

[0129] Step 2.2 combines backstepping control to design a stable tracking controller for each order system:

[0130] The end effector position x and velocity are selected as the state vector to obtain a state space model implementation of the dynamic equation (2):

[0131]

[0132] wherein, is the system state, is the system control input, y=x is the system control output; u first appears in the second-order differential equation of the output x, so the relative order m=2 of the system decomposes the system into a first-order subsystem and a second-order system;

[0133] Starting from the first-order subsystem , x2 is taken as a virtual input of the system, an error e1=x1-x 1d is defined, and a Lyapunov function is constructed, and the derivative of the Lyapunov function with respect to time is According to the Lyapunov stability criterion theorem, if the system is to be stable, it is necessary to ensure A parameter k1>0 is introduced, and let that is, is always true, and the tracking value x 2d of x2 is designed as

[0134]

[0135] For the second-order system, an error e2=x2-x 2d is defined, and a Lyapunov function is constructed, and the derivative of V2 is taken and substituted into and formula (6):

[0136]

[0137] In order to ensure Introduce parameter k2>0, let

[0138]

[0139] Again, since Substitute and equation (6):

[0140]

[0141] Substitute equation (9) into equation (8), the stable tracking controller of the system (also called nominal controller) is

[0142]

[0143] where k1 and k2 are adjustable control parameters and are positive numbers.

[0144] Step 3 provides safety constraints for the control system by high-order control barrier functions:

[0145] Step 3.1 establishes a distance-based obstacle collision detection model to describe the high-order control barrier function:

[0146] For an affine control system:

[0147]

[0148] where, is the system state, is a local Lipschitz function, (U represents a closed control constraint set), the initial state x(t0), and the solution x(t), t≥t0 of equation (11) is forward complete; in particular, in equation (5),

[0149] First, a simplified geometric model is established for the robot arm and obstacles in its working environment. A plane or an envelope sphere is used to approximate the obstacles, and the collision detection between the robot arm and the obstacles is converted into the problem of determining whether the minimum distance is positive, effectively reducing the computational complexity. A candidate high-order control barrier function (HOCBF) is defined: a continuous and differentiable function b(x) of order m=2:

[0150] b(x) = d safe -d min (x) (12)

[0151] where d min (x) is the minimum distance from the end effector of the robot arm to the obstacle, and d safe is the safety distance set. When b(x)≥0, the system is safe.

[0152] Step 3.2 Define the safety set:

[0153] Security is built in the context of enforcing set invariance, i.e. not leaving the safety set; define the safety set C as the super zero level set of the continuously differentiable function b(x):

[0154]

[0155] If the solution starting from any x(t0)∈C satisfies Then the set C is forward invariant, i.e. the system (11) is safe for the set C;

[0156] Step 3.3 Introduce Class functions, define auxiliary functions by successive differentiation:

[0157] If a Lipschitz continuous function a: [0, a)→[0, ∞), a>0 is strictly increasing, and a(0)=0, then the function is called Class function; select Class functions a1(·), a2(·), define auxiliary functions as

[0158]

[0159] The corresponding safety set sequence C i Is:

[0160]

[0161] Step 3.4 Design high-order control barrier function to control the constraint set:

[0162] If there exist Class functions a1(·), a2(·) such that

[0163]

[0164] For all (x, t)∈C1(t)∩C2(t)∩C3(t)×[t0, ∞) holds, then the function b(x, t) is a HOCBF of the system (11), where And Respectively represent the Lie derivative of f and g with respect to x, O(b(x, t)) is as follows

[0165]

[0166]

[0167] ​Denote ψ1 as the independent variable of α1(·), i.e., α1(ψ1);

[0168] The set of HOCBF control constraints is:

[0169]

[0170] In the set K hocbf The control input u within can ensure the system safety.

[0171] Step 4 combines the control constraints with the optimal control problem and solves the optimal control input through quadratic programming:

[0172] Step 4.1 Combine the control constraints with the optimal control problem:

[0173] In order to minimize the change of control input to track the task trajectory, constraint (18) is combined with the optimal control problem to obtain HOCBF-QP:

[0174]

[0175] Among them, u * is the objective function, satisfying the inequality constraint, u is the optimization variable to be solved, u norm is the control input of the nominal controller; when the system state is within the safety set C, the solution to the problem is u * =u norm , which shows that the system control input is determined by the nominal controller; when the system state leaves the safety set C, a mandatory control constraint is added, sacrificing tracking performance to meet safety until the manipulator is away from the dangerous area;

[0176] Step 4.2 solves the quadratic programming and obtains the real-time optimal control input:

[0177] The problem is solved at each time step, and the time interval [t0,t f ] is divided into a series of unequal time intervals

[0178] {[t0,t0+Δt1),[t0+Δt1,t0+Δt1+Δt2),...} (20) where Δt i >0; in each interval Holding the state constant at its value at the beginning of the interval and assuming that the control is constant, the optimization problem is formulated as a sequence of QPs; specifically, Solving QP:

[0179] The standard form of QP is

[0180]

[0181] For symmetric positive definite matrix H and constant vector F, since

[0182]

[0183] Neglecting the last constant term, H = 2I n ,

[0184] Convert the inequality constraint into the form of Ax <= B, then

[0185]

[0186]

[0187] Substitute H, F, A, B into the quadratic programming solver, and the solution vector is the virtual control torque of the end effector of the manipulator in the Cartesian space at the current time period Convert it to the joint space, which is the optimal control input torque τ of the joint driver * , that is, the safety controller of the manipulator is

[0188] To illustrate the effectiveness of the proposed method, the simulation experiment of Franka manipulator is given. The initial position of the system is that the joint angle q = [0, -45°, 0, -135°, 0, 90°, 45°] T . The task trajectory is a circle with a radius of 0.15m and the initial position as the center. The obstacle model is a plane, and the HOCBF is defined as b(x) = 0.4-p x , wherein p x represents the horizontal coordinate of the end effector. The control parameters of the safety controller are selected as follows: k1 = 4, k1 = 7, alpha1 = 16, alpha2 = 64.

[0189] Combining Figure 1 and Figure 4 , the actual position curve of the safety region is consistent with the expected position curve, and the position error is small, which can be seen that the controller achieves satisfactory tracking performance, but the attitude error exists, that is, the actual joint angle deviates from the expected joint angle. Figure 2 The trajectory curve is blocked on the left side of the plane x = 0.4m, which shows that the safety constraint applied to the system by the HOCBF is effective. Figure 3 The torque curve is stable and has no obvious jitter phenomenon. Figure 5 In the above, due to the conservativeness of the obstacle collision detection model, the manipulator produces an over-reaction tilting behavior.

[0190] In summary, for a multi-degree-of-freedom manipulator, the controller can realize stable trajectory tracking control of the manipulator end effector in the presence of obstacles in the task trajectory, while meeting the safety constraints of the system.

[0191] Embodiment 2

[0192] The embodiment relates to a manipulator safety obstacle avoidance control system based on a high-order control barrier function, comprising:

[0193] The obstacle collision detection model establishment module identifies obstacles through a depth camera, and obtains obstacle position and depth information to establish a distance-based obstacle collision detection model.

[0194] The task trajectory planning module plans a path from a starting point x0 to a terminal point x e in the Cartesian space as a task trajectory, and designs the expected position, velocity and acceleration of the manipulator on the path at each moment;

[0195] The stable tracking controller design module designs a stable tracking controller based on the task trajectory and a backstepping control method, decomposes the nonlinear system into a series of subsystems not exceeding the order of the system for the dynamics model of the manipulator in the Cartesian space, and designs Lyapunov functions step by step.

[0196] The high-order control barrier function constraint establishment module establishes a distance-based high-order control barrier function to provide safety constraints for the control system, defines a safety set, introduces a similar function, defines an auxiliary function by step-by-step derivation, and designs a high-order control barrier function control constraint set.

[0197] The optimal control problem solving module combines the control constraints and the optimal control problem, solves the optimal control input through quadratic programming, and performs spatial transformation to deliver to the manipulator.

[0198] The host computer and the lower computer communication module realizes real-time state monitoring of the lower computer by the host computer through data transmission and image transmission.

[0199] Embodiment 3

[0200] The embodiment relates to a manipulator safety obstacle avoidance control device based on a high-order control barrier function, comprising a memory and one or more processors, wherein the memory stores executable code, and the one or more processors execute the executable code to implement the manipulator safety obstacle avoidance control method based on the high-order control barrier function in the embodiment 1.

[0201] Embodiment 4

[0202] The embodiment relates to a computer readable storage medium, which stores a program, and the program is executed by a processor to realize a mechanical arm safety obstacle avoidance control method based on a high-order control obstacle function.

[0203] The above is the excellent optimization effect of an embodiment provided by the application. Apparently, the application is not limited to the above embodiment, and various modifications can be made to the application without departing from the basic spirit of the application and without exceeding the scope of the application.

Claims

1. A safety obstacle avoidance control method for a robot arm based on a high-order control barrier function, characterized in that, The method comprises the steps of: Step 1, establishing a dynamics model of the multi-degree-of-freedom robot arm, and transforming a dynamics equation in a joint space to a Cartesian space; Step 2, designing a stable tracking controller based on a task trajectory and a backstepping control method, planning a task trajectory in the Cartesian space, decomposing a nonlinear system into a series of subsystems not more than a system order, and designing a Lyapunov function step by step; Step 3, provide safety constraints for the control system through high-order control barrier functions, establish a distance-based obstacle collision detection model, define a safety set, introduce a class function, define auxiliary functions through step-by-step differentiation, and design high-order control barrier functions to control the constraint set; Step 4, combining a control constraint and an optimal control problem, and solving an optimal control input through quadratic programming; The dynamics model of the multi-degree-of-freedom robot arm in step 1 comprises: Step 1.1, establishing dynamics equations in a joint space and a Cartesian space of the robot arm: For a manipulator with one degree of freedom , its motion control mode is torque control, and its dynamic model has been identified, and the time-varying dynamic parameters can be obtained online. The dynamics equation in the joint space of the robot arm is （1） wherein, denotes a torque vector of each joint, denotes an angle, an angular velocity and an angular acceleration vector of each joint, respectively, denotes a symmetric positive definite mass matrix of the robot arm, denotes a centrifugal and coriolis force vector, denotes a gravity vector; The dynamics equation in the Cartesian space of the robot arm is （2） wherein, denotes a torque vector acting on the end effector, denotes a Cartesian vector of the end effector pose, denotes a Cartesian mass matrix, denotes a vector of velocity terms in Cartesian space, denotes a vector of gravity terms in Cartesian space; Step 1.2, transforming the dynamics equation in the joint space of the robot arm to the Cartesian space: Virtual forces acting on an end effector Driving force of the joint driver Indicated: （3） wherein is the Jacobian matrix; derived from equation (3) and The relationship between the coefficients of each term in equation (1) and equation (2) is derived as: （4） The stable tracking controller designed based on the backstepping control method in step 2 comprises: Step 2.1, planning a task trajectory in the Cartesian space: Starting from Cartesian space planning To the end point A path is obtained to obtain the task trajectory, and the expected position, velocity and acceleration of the manipulator on the path at each moment are designed; Step 2.2, designing a stable tracking controller of each order system in combination with the backstepping control: Selecting end effector position and velocity As state vector, one state space model implementation of the dynamics equation (2) is obtained: （5） where, is the system state, is the system control input, is the system control output; first appears in the output second derivative equation, so the relative order decomposes the system into a first-order subsystem and a second-order system; From the first-order subsystem Start pushing back As a virtual input of the system, define the error , construct the Lyapunov function , whose derivative with respect to time is According to Lyapunov's stability theorem, if the system is to be stable, it is necessary to ensure ;Introduce parameters ,make ,Right now Hengcheng, design Tracking value of for （6） For a second order system, define the error , construct a Lyapunov function , take the derivative with respect to time , and substitute and equation (6): （7） To ensure , the parameter is introduced, so that （8） Further, since , substituting and equation (6): （9） Substituting equation (9) into equation (8), the stable tracking controller (also referred to as the nominal controller) of the system is is （10） wherein and are adjustable control parameters and are positive numbers; The control system is provided with a safety constraint through the high-order control barrier function in step 3, comprising: Step 3.1, establishing a distance-based barrier collision detection model to describe the high-order control barrier function: For an affine control system: （11） wherein is the system state, , is a local Lipschitz function, (U denotes a closed control constraint set), initial state , the solution of equation (11) is forward complete; in particular, equation (5) in which , ; First, a simplified geometric model of the robot arm and obstacles in its working environment is established, and a plane or an envelope sphere is used to approximate the obstacles, so that the collision detection between the robot arm and the obstacles is converted into the problem of judging whether the minimum distance is positive or not, effectively reducing the computational complexity; a candidate high-order control barrier function (HOCBF) is defined: a function that is continuously differentiable : （12） wherein, is the minimum distance of the end effector of the robot to the obstacle, is the safety distance set, when the system is safe. Step 3.2, defining a safety set: Security is built in the context of mandatory set invariance, i.e. not leaving the secure set; the secure set is defined as the super zero level set of a continuously differentiable function : ​ （13） If the solution starting from any satisfies then the set is forward invariant, i.e. the system (11) is safe with respect to the set ; Step 3.3 Introduction Class functions, define auxiliary functions by successive differentiation: If a Lipschitz continuous function is strictly increasing, and then the function is said to belong to class of functions; select class of functions , , by taking the time derivative of successively, define the auxiliary function as （14） Corresponding secure set sequence Is: （15） Step 3.4, designing a high-order control barrier function control constraint set: if present class function , make （16） For all is a HOCBF of the system (11) if is a HOCBF of the system (11) if and denote the Lie derivatives along and with respect to is given by​ （17） representing as the argument of ; The HOCBF control constraint set is: （18） In the set of control inputs , the system safety can be guaranteed; The optimal control input is solved through quadratic programming in combination with the control constraint and the optimal control problem in step 4, comprising: Step 4.1, combining the control constraint and the optimal control problem: In order to minimize the change of the control input to track the task trajectory, the constraint (18) is combined with the optimal control problem to obtain the HOCBF-QP: （19） where, is the objective function, satisfying inequality constraints, is the optimization variable to be solved, is the control input of the nominal controller; when the system state is within the safe set , the solution of the problem is , which indicates that the system control input is determined by the nominal controller; when the system state leaves the safe set , the forced control constraints are added, sacrificing the tracking performance to meet safety until the robot arm is far away from the dangerous area; Step 4.2, solving the quadratic programming to obtain a real-time optimal control input: The problem is solved at each time step, dividing the time interval into a series of unequal time intervals （20） where ; at each interval , the state is held constant at the value it had at the beginning of its interval, and assuming constant control, the optimization problem is represented as a sequence of QPs; specifically, at , the QP is solved: The standard form of the QP is （21） For symmetric positive definite matrices H and constant vector F since （22） ignoring the last constant term, , ; Transforming the inequality constraint into the form of then （23） （24） Will Substitute it into the quadratic programming solver, and the solution vector obtained is the virtual control torque of the end effector of the manipulator in the Cartesian space in the current time period. , and then convert it to the joint space, which is the optimal control input torque of the joint actuator , that is, the safety controller of the robotic arm is .

2. A robot safety obstacle avoidance control system based on high-order control barrier function, realizing a robot safety obstacle avoidance control method based on high-order control barrier function as claimed in claim 1, characterized in that, Comprise: The dynamics model establishing module is used for establishing a dynamics model of the multi-degree-of-freedom robot arm, and transforming a dynamics equation in a joint space to a Cartesian space; The stable tracking controller designing module is used for designing a stable tracking controller based on a task trajectory and a backstepping control method, planning a task trajectory in the Cartesian space, decomposing a nonlinear system into a series of subsystems not more than a system order, and designing a Lyapunov function step by step; The high-order control obstacle function constraint establishing module establishes a distance-based obstacle collision detection model, is used for describing the high-order control obstacle function, providing safety constraints for the control system, defining a safety set, and introducing A class function, an auxiliary function is defined by step-by-step derivation, and a high-order control obstacle function control constraint set is designed. The optimal control input solving module is used for combining a control constraint and an optimal control problem, and solving an optimal control input through quadratic programming.

3. A safety obstacle avoidance control device for a robot arm based on a high order control barrier function, characterized by, The device comprises a memory and one or more processors, the memory stores executable code, and the one or more processors execute the executable code to implement the robot arm safety obstacle avoidance control method based on the high-order control barrier function.

4. A computer-readable storage medium, characterized in that, The device has a program stored thereon, and the program is executed by a processor to implement the robot arm safety obstacle avoidance control method based on the high-order control barrier function.

Images