Mechanical arm safe obstacle avoidance control method and system based on high-order control obstacle function

By using the method of using high-order control obstacle function in the multi-degree of freedom robot arm, safety constraints are provided for the control system, the safety obstacle avoidance problem of the high-relative robot control system is solved, and the safe and efficient tracking and control of the robot arm in the task trajectory is realized.

CN119635632AActive Publication Date: 2025-03-18ZHEJIANG UNIV OF TECH

Patent Information

Application Number
CN202411837262.3
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-12-13
Publication Date
2025-03-18
Estimated Expiration
2044-12-13

AI Technical Summary

Technical Problem

The prior art is difficult to effectively solve the problem of safe obstacle avoidance in high relative order robot control systems, especially when there are obstacles in the task trajectory of multi-degree of freedom robot arms.

Method used

The robot arm safety obstacle avoidance control method based on the advanced control obstacle function is adopted. By establishing a dynamic model of the multi-degree of freedom robot arm, a stable tracking controller is designed, and the control system is provided with safety constraints through the higher-order control obstacle function. Combining the control constraints and optimal control problems, the optimal control input is solved through quadratic planning.

Benefits of technology

The robot arm end effector completes optimal task trajectory tracking while meeting safety constraints, effectively reducing the computational complexity and ensuring the real-time nature of the control algorithm, and improving the safety of robot arm operations.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119635632A_ABST
    Figure CN119635632A_ABST
Patent Text Reader

Abstract

The invention relates to a mechanical arm safe obstacle avoidance control method and system based on a high-order control obstacle function, aiming at a multi-degree-of-freedom mechanical arm system with an obstacle in a task trajectory space, the high-order control obstacle function suitable for a high-relative-order system is used, and the method comprises the following steps: establishing a dynamic model of a multi-degree-of-freedom mechanical arm; designing a stable tracking controller based on the task trajectory in combination with a backstepping control method; an obstacle collision detection model is established, and safety constraints are provided for a control system through a high-order control obstacle function; and in combination with the control constraint and the optimal control problem, solving the optimal control input through quadratic programming. According to the method, the optimal task trajectory tracking can be completed on the premise that the tail end of the mechanical arm meets the safety constraint, the calculation complexity is effectively reduced, and the real-time performance is guaranteed.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a robot arm safety obstacle avoidance control method and system based on a high-order control obstacle function. Technical Background

[0002] With the widespread application of robotics technology, safety has become an important issue in the field of robotics. It is a challenge to design a control scheme that can complete the specified task without affecting safety. At present, there are many algorithms that focus on the obstacle avoidance problem of robots, such as: the global path planning algorithm provides an optimal path from the starting point to the end point, but this algorithm can only be applied to situations 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 a large amount of information storage and the controller obtained is not optimal. Hamilton Jacobi reachability analysis method is a formal method to verify the safety of dynamic systems. This method drives the dynamic system to the target set while avoiding the unsafe set from being affected by bounded uncertainty. However, reachability analysis becomes difficult to calculate as the system dimension increases.

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

[0004] Compared with traditional position control and speed control, robot torque control has broad development prospects. For the torque control of complex robot systems, it is necessary to design high-order control obstacle functions to achieve safe control of high relative order systems. Summary of the invention

[0005] In order to overcome the safety obstacle avoidance problem of high relative order robot control systems in the prior art, the present invention provides a robot arm safety obstacle avoidance control method and system based on high-order control obstacle function, so as to enable the multi-degree-of-freedom robot arm end effector to complete the optimal task trajectory tracking under the premise of satisfying safety constraints.

[0006] In order to solve the above technical problems, the present invention adopts the following technical solutions:

[0007] A first aspect of the present invention relates to a method for controlling a safe obstacle avoidance of a robotic arm based on a high-order control obstacle function, comprising the following steps:

[0008] Step 1: Establish a dynamic model of the multi-degree-of-freedom robotic arm and transform the dynamic equations of the joint space into the Cartesian space;

[0009] Step 2: Design a stable tracking controller based on the task trajectory combined with the backstepping control method, perform task trajectory planning in Cartesian space, decompose the nonlinear system into a series of subsystems that do not exceed the system order, and design the Lyapunov function step by step;

[0010] Step 3: Provide safety constraints for the control system through high-order control obstacle functions, establish a distance-based obstacle collision detection model to describe high-order control obstacle functions, define safety sets, and introduce Class function, define auxiliary functions by step-by-step derivation, and design high-order control obstacle function control constraint set;

[0011] Step 4: Combine the control constraints with the optimal control problem and solve the optimal control input through quadratic programming.

[0012] Wherein, the establishment of the dynamic model of the multi-degree-of-freedom robotic arm described in step 1 includes:

[0013] Step 1.1 Establish the dynamic equations of the robot arm joint space and Cartesian space:

[0014] For a robot with n degrees 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;

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

[0016]

[0017] in, represents the torque vector of each joint, Represent the angle, angular velocity and angular acceleration vector of each joint respectively, represents the symmetric positive definite mass matrix of the manipulator, represents the centrifugal force and Coriolis force vector, represents the gravity vector;

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

[0019]

[0020] in, represents the torque vector acting on the end effector, The Cartesian vector representing 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: Transform the dynamic equations of the robot joint space into Cartesian space:

[0022] Given the dynamic equations of the manipulator in joint space, considering that the position of the obstacle is described by Cartesian coordinates, the Jacobian matrix is ​​used to transform it into Cartesian space for easy calculation;

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

[0024]

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

[0026]

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

[0028] Step 2.1 Plan the Cartesian space mission trajectory:

[0029] In Cartesian space planning starting point x 0 To the end point x e A path is obtained to obtain the task trajectory, and the expected position, speed and acceleration of the robot arm on the path at each moment are designed;

[0030] Step 2.2 Design stable tracking controllers for each order system in combination with backstepping control:

[0031] Select the end effector position x and speed As the state vector, a state space model realization of the dynamic equation (2) is obtained:

[0032]

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

[0034] From the first-order subsystem Start to push back and change x 2 As the system virtual input, define the error e 1 =x1 -x 1d , construct the Lyapunov function Its derivative with respect to time According to Lyapunov's stability theorem, if the system is to be stable, it is necessary to ensure Introducing parameter k 1 >0, let Right now Hengcheng, design x 2 Tracking value x 2d for

[0035]

[0036] For a second-order system, define the error e 2 =x 2 -x 2d , construct the Lyapunov function V 2 Take the derivative and substitute And formula (6):

[0037]

[0038] To ensure Introducing parameter k 2 >0, let

[0039]

[0040] Also because Substitution And formula (6):

[0041]

[0042] Substituting equation (9) into equation (8), the system's stable tracking controller (also called nominal controller) is for

[0043]

[0044] where k 1 and k 2 is an adjustable control parameter and is a positive number.

[0045] Wherein, providing safety constraints for the control system through a high-order control obstacle function as described in step 3 includes:

[0046] Step 3.1 Establish a distance-based obstacle collision detection model and describe the high-order control obstacle function:

[0047] For an affine control system:

[0048]

[0049] in, is the system status, is a local Lipschitz function, (U represents a closed control constraint set), the initial state x(t 0 ), the solution of formula (11) is x(t),t≥t 0 is forward complete; in particular, in (5),

[0050] First, a simplified geometric model is established for the robot and the obstacles in its working environment. A plane or an envelope sphere is used to approximate the obstacles. The collision detection between the robot and the obstacles is transformed into a problem of judging whether the minimum distance is positive, which effectively reduces the computational complexity. A candidate high-order control obstacle function (HOCBF) is defined: m = 2nd-order continuously differentiable function b(x):

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

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

[0053] Step 3.2 Define the security collection:

[0054] Safety is constructed in the context of enforcing set invariance, i.e., not leaving a safe set; define a safe set C as a continuously differentiable function b(x): The superzero level set

[0055]

[0056] If from any x(t 0 )∈C satisfies Then the set C is forward invariant, that is, the system (11) is safe with respect to the set C;

[0057] Step 3.3 Introduction Class function, defines auxiliary functions by step-by-step derivation:

[0058] If a Lipschitz continuous function α:[0,a)→[0,∞),a>0 is strictly increasing and α(0)=0, then the function is said to belong to Class function; select Class function α 1 (·), α2 (·), by successively finding the time derivative of b(x, t), the auxiliary function is defined as

[0059]

[0060] The corresponding security set sequence C i for:

[0061]

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

[0063] If exists Class function α 1 (·), α 2 (·)make

[0064]

[0065] For all (x, t)∈C 1 (t)∩C 2 (t)∩C 3 (t)×[t 0 ,∞) holds, then the function b(x,t) is a HOCBF of the system (11), where and denotes the Lie derivatives along f and g with respect to x, respectively. O(b(x,t)) is as follows

[0066]

[0067] Represents ψ 1 As α 1 The independent variable of (·), namely α 1 (ψ 1 );

[0068] The set of HOCBF control constraints is:

[0069]

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

[0071] Wherein, the combination of control constraints and optimal control problem described in step 4, solving the optimal control input through quadratic programming, includes:

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

[0073] 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:

[0074]

[0075] 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;

[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 [t 0 ,t f ] is divided into a series of unequal time intervals

[0078] {[t 0 ,t 0 +Δt 1 ),[t 0 +Δt 1 ,t 0 +Δt 1 +Δt 2 ),...}(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 expressed as a QP sequence; 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 robot arm is

[0088] A second aspect of the present invention relates to a robot arm safety obstacle avoidance control system 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 robot arm and transform the dynamic equations of the joint space into the Cartesian space;

[0090] The stable tracking controller design module designs a stable tracking controller based on the task trajectory combined with the backstepping control method, performs task trajectory planning in Cartesian space, decomposes the nonlinear system into a series of subsystems that do not exceed the system order, and designs the Lyapunov function step by step;

[0091] The high-order control obstacle function constraint establishment module establishes a distance-based obstacle collision detection model to describe the high-order control obstacle function, provide safety constraints for the control system, define safety sets, and introduce Class function, define auxiliary functions by step-by-step derivation, and design high-order control obstacle function control constraint set;

[0092] The optimal control input solving module combines the control constraints with the optimal control problem and solves the optimal control input through quadratic programming.

[0093] The third aspect of the present invention relates to a robot arm safety obstacle avoidance control device based on a high-order control obstacle function, comprising a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, they are used to implement a robot arm safety obstacle avoidance control method based on a high-order control obstacle function of the present invention.

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

[0095] The present invention is aimed at a multi-degree-of-freedom robotic arm. When there are obstacles on the task trajectory, a robotic arm safety obstacle avoidance control method based on a high-order control obstacle function is designed in combination with a backstepping control method, so as to enable the robotic arm end effector to complete the optimal task trajectory tracking under the premise of meeting safety constraints.

[0096] The technical concept of the present invention is: for a multi-degree-of-freedom manipulator system with obstacles in the task trajectory space, a high-order control obstacle function suitable for a high relative order system is used. The specific method includes: establishing a dynamic model of a multi-degree-of-freedom manipulator; designing a stable tracking controller based on the task trajectory combined with a backstepping control method; establishing a distance-based obstacle collision detection model, and providing safety constraints for the control system through a high-order control obstacle function; combining control constraints with optimal control problems, and solving the optimal control input through quadratic programming. This method can enable the end effector of the manipulator to complete the optimal task trajectory tracking under the premise of meeting safety constraints, effectively reducing the computational complexity and ensuring real-time performance.

[0097] The beneficial effects of the present invention are: when there are obstacles in the task trajectory, it ensures that the multi-degree-of-freedom manipulator always meets the safety requirements during operation, effectively avoids collision with obstacles, and greatly improves the safety of the manipulator operation. The use of high-order obstacle function method effectively reduces the computational complexity and conservatism of safety analysis, and ensures the real-time performance of the control algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0099] Figure 1 is a schematic diagram of a time curve comparison between the actual position and the expected position of the end effector of the robot arm of the present invention;

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

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

[0102] Figure 4 is a schematic diagram of a time curve of the posture error of the end effector of the robot arm of the present invention;

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

[0104] Figure 6 It is a control flow diagram of the present invention;

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

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

[0107] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0108] Example 1

[0109] Reference Figure 1 and Figure 8 , this embodiment relates to a robot arm safety obstacle avoidance control method based on a high-order control obstacle function, comprising the following steps:

[0110] Step 1: Establish the dynamic model of the multi-degree-of-freedom robotic arm:

[0111] Step 1.1 Establish the dynamic equations of the robot arm joint space and Cartesian space:

[0112] For a robot with n degrees 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;

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

[0114]

[0115] in, represents the torque vector of each joint, Represent the angle, angular velocity and angular acceleration vector of each joint respectively,

[0116] represents the symmetric positive definite mass matrix of the manipulator, represents the centrifugal force and Coriolis force vector, represents the gravity vector;

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

[0118]

[0119] in, represents the torque vector acting on the end effector, The Cartesian vector representing 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;

[0120] Step 1.2: Transform the dynamic equations of the robot joint space into Cartesian space:

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

[0122]

[0123] Where J(q) is the Jacobian matrix; from equation (3) and

[0124] Derived, the relationship between the coefficients in formula (1) and formula (2) is:

[0125]

[0126] Step 2: Design a stable tracking controller by combining the backstepping control method:

[0127] Step 2.1 Plan the Cartesian space mission trajectory:

[0128] In Cartesian space planning starting point x 0 To the end point x e A path is obtained to obtain the task trajectory, and the expected position, speed and acceleration of the robot arm on the path at each moment are designed;

[0129] Step 2.2 Design stable tracking controllers for each order system in combination with backstepping control:

[0130] Select the end effector position x and speed As the state vector, a state space model realization of the dynamic equation (2) is obtained:

[0131]

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

[0133] From the first-order subsystem Start to push back and change x 2 As the system virtual input, define the error e 1 =x 1 -x1d , construct the Lyapunov function Its derivative with respect to time According to Lyapunov's stability theorem, if the system is to be stable, it is necessary to ensure Introducing parameter k 1 >0, let Right now Hengcheng, design x 2 Tracking value x 2d for

[0134]

[0135] For a second-order system, define the error e 2 =x 2 -x 2d , construct the Lyapunov function V 2 Take the derivative and substitute And formula (6):

[0136]

[0137] To ensure Introducing parameter k 2 >0, let

[0138]

[0139] Also because Substitution And formula (6):

[0140]

[0141] Substituting equation (9) into equation (8), the system's stable tracking controller (also called nominal controller) is for

[0142]

[0143] where k 1 and k 2 is an adjustable control parameter and is a positive number.

[0144] Step 3 provides safety constraints for the control system through a high-order control barrier function:

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

[0146] For an affine control system:

[0147]

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

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

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

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

[0152] Step 3.2 Define the security collection:

[0153] Safety is constructed in the context of enforcing set invariance, i.e., not leaving a safe set; define a safe set C as a continuously differentiable function b(x): The superzero level set

[0154]

[0155] If from any x(t 0 )∈C satisfies Then the set C is forward invariant, that is, the system (11) is safe with respect to the set C;

[0156] Step 3.3 Introduction Class function, defines auxiliary functions by step-by-step derivation:

[0157] If a Lipschitz continuous function α:[0,a)→[0,∞),a>0 is strictly increasing and α(0)=0, then the function is said to belong to Class function; select Class function α 1 (·), α 2(·), by successively finding the time derivative of b(x, t), the auxiliary function is defined as

[0158]

[0159] The corresponding security set sequence C i for:

[0160]

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

[0162] If exists Class function α 1 (·), α 2 (·)make

[0163]

[0164] For all (x, t)∈C 1 (t)∩C 2 (t)∩C 3 (t)×[t 0 ,∞) holds, then the function b(x,t) is a HOCBF of the system (11), where and denotes the Lie derivatives along f and g with respect to x, respectively. O(b(x,t)) is as follows

[0165]

[0166]

[0167] Represents ψ 1 As α 1 The independent variable of (·), namely α 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 [t 0 ,t f ] is divided into a series of unequal time intervals

[0178] {[t 0 ,t 0 +Δt 1 ),[t 0 +Δt 1 ,t 0 +Δt 1 +Δt 2 ),...} (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 expressed as a QP sequence; specifically, Solving QP:

[0179] The standard form of QP is

[0180]

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

[0182]

[0183] Ignore 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, 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 robot arm is

[0188] To illustrate the effectiveness of the proposed method, the present invention provides a simulation experiment of the Franka robot arm. The initial position of the system is, 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 , where p x represents the horizontal coordinate of the end effector. The control parameters of the safety controller are selected as follows: 1 =4, k 1 =7,α 1 =16,α 2 =64.

[0189] Combination Figure 1 and Figure 4 ,The actual position curve of the safety area is consistent with the expected position curve, and the position error is very small. It can be seen that the controller achieves satisfactory tracking performance, but the posture error exists, that is, the actual joint angle deviates from the expected joint angle. Figure 2 The right side of the trajectory curve is blocked on the left side of the plane at x = 0.4 m, which shows that the safety constraints imposed on the system by HOCBF are effective. Figure 3 The torque curve is stable without obvious jitter. Figure 5 In the example, the robot arm exhibited an over-reactive tilting behavior due to the conservativeness of the obstacle collision detection model.

[0190] In summary, for a multi-degree-of-freedom robotic arm, when there are obstacles on the task trajectory, the proposed controller can achieve stable trajectory tracking control of the robotic arm end effector while meeting the safety constraints of the system.

[0191] Example 2

[0192] This embodiment relates to a robot arm safety obstacle avoidance control system based on a high-order control obstacle function, comprising:

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

[0194] The task trajectory planning module plans the starting point x in Cartesian space 0 To the end point x e A path is used as the task trajectory, and the expected position, speed and acceleration of the robot on the path at each moment are designed;

[0195] The stable tracking controller design module designs a stable tracking controller based on the task trajectory combined with the backstepping control method. For the dynamic model of the robot arm in Cartesian space, the nonlinear system is decomposed into a series of subsystems that do not exceed the system order, and the Lyapunov function is designed step by step;

[0196] High-order control obstacle function constraint establishment module, establishes distance-based high-order control obstacle function, provides safety constraints for control systems, defines safety sets, and introduces Class function, define auxiliary functions by step-by-step derivation, and design high-order control obstacle function control constraint set;

[0197] The optimal control problem solving module combines the control constraints with the optimal control problem, solves the optimal control input through quadratic programming, and performs spatial transformation to transmit it to the robot arm;

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

[0199] Example 3

[0200] The present embodiment relates to a robot arm safety obstacle avoidance control device based on a high-order control obstacle function, comprising a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, they are used to implement a robot arm safety obstacle avoidance control method based on a high-order control obstacle function of embodiment 1.

[0201] Example 4

[0202] This embodiment relates to a computer-readable storage medium on which a program is stored. When the program is executed by a processor, a robot arm safety obstacle avoidance control method based on a high-order control obstacle function of embodiment 1 is implemented.

[0203] The above describes the excellent optimization effect shown by an embodiment of the present invention. Obviously, the present invention is not limited to the above embodiment, and various modifications can be made to it without departing from the basic spirit of the present invention and without exceeding the scope of the essential content of the present invention.

Claims

1. A safe obstacle avoidance control method for a robotic arm based on a high-order control obstacle function, characterized in that: The method comprises the steps of: Step 1: Establish a dynamic model of the multi-degree-of-freedom robotic arm and transform the dynamic equations of the joint space into the Cartesian space; Step 2: Design a stable tracking controller based on the task trajectory combined with the backstepping control method, perform task trajectory planning in Cartesian space, decompose the nonlinear system into a series of subsystems that do not exceed the system order, and design the Lyapunov function step by step; Step 3: Provide safety constraints for the control system through high-order control obstacle functions, establish a distance-based obstacle collision detection model to describe high-order control obstacle functions, define safety sets, and introduce Class function, define auxiliary functions by step-by-step derivation, and design high-order control obstacle function control constraint set; Step 4: Combine the control constraints with the optimal control problem and solve the optimal control input through quadratic programming.

2. The method according to claim 1, characterized in that The step 1 of establishing the dynamic model of the multi-degree-of-freedom robotic arm includes: Step 1.1 Establish the dynamic equations of the robot arm joint space and Cartesian space: For a robot with n degrees 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 dynamic equation of the robot arm in joint space is: in, represents the torque vector of each joint, Represent the angle, angular velocity and angular acceleration vector of each joint respectively, surface represents the symmetric positive definite mass matrix of the manipulator, represents the centrifugal force and Coriolis force vector, represents the gravity vector; The dynamic equation of the robot arm in Cartesian space is: in, represents the torque vector acting on the end effector, The Cartesian vector representing the end effector pose, Table represents the Cartesian mass matrix, represents the velocity term vector in Cartesian space, represents the gravity term vector in Cartesian space; Step 1.2: Transform the dynamic equations of the robot joint space into Cartesian space: Virtual force acting on the end effector It can be expressed by the driving force τ of the joint actuator: Where J(q) is the Jacobian matrix; from equation (3) and Derived, the relationship between the coefficients in formula (1) and formula (2) is:

3. The method according to claim 1, characterized in that The design of a stable tracking controller in combination with the backstepping control method described in step 2 includes: Step 2.1 Plan the Cartesian space mission trajectory: Plan the starting point x0 to the end point x in Cartesian space e A path is obtained to obtain the task trajectory, and the expected position, speed and acceleration of the robot on the path at each moment are designed; Step 2.2 Design stable tracking controllers for each order system in combination with backstepping control: Select the end effector position x and speed As the state vector, a state space model realization of the dynamic equation (2) is obtained: in, is the system status, is the system control input, and y = x is the system control output; u first appears in the second-order differential equation of the output x, so the relative order of the system is m = 2, which decomposes the system into a first-order subsystem and a second-order system; From the first-order subsystem Start the reverse calculation, take x2 as the virtual input of the system, and define the error e1 = x1-x 1d , construct the Lyapunov function Its derivative with respect to time According to Lyapunov's stability theorem, if the system is to be stable, it is necessary to ensure Introduce parameter k1>0, let Right now Constantly established, design the tracking value x of x2 2d for For a second-order system, define the error e2 = x2-x 2d , construct the Lyapunov function Take the derivative of V2 and substitute And formula (6): To ensure Introduce parameter k2>0, let Also because Substitution And formula (6): Substituting equation (9) into equation (8), the system's stable tracking controller (also called the nominal controller) for Where k1 and k2 are adjustable control parameters and are positive numbers.

4. The method according to claim 1, characterized in that: The step 3 provides safety constraints for the control system through a high-order control barrier function, including: Step 3.1 Establish a distance-based obstacle collision detection model and describe the high-order control obstacle function: For an affine control system: in, is the system status, is a local Lipschitz function, (U represents a closed set of control constraints), the initial state x(t0), the solution x(t), t≥t0 of equation (11) is forward complete; in particular, in equation (5), First, a simplified geometric model is established for the robot and the obstacles in its working environment. A plane or an envelope sphere is used to approximate the obstacles. The collision detection between the robot and the obstacles is transformed into a problem of judging whether the minimum distance is positive, which effectively reduces the computational complexity. A candidate high-order control obstacle function (HOCBF) is defined: m = 2nd-order continuously differentiable function b(x): b(x)=d safe -d min (x) (12) Among them, d min (x) is the minimum distance from the end effector of the robot to the obstacle, d safe is the set safety distance, and the system is safe when b(x)≥0; Step 3.2 Define the security collection: Safety is constructed in the context of enforcing set invariance, i.e., not leaving a safe set; define a safe set C as a continuously differentiable function b(x): The superzero level set If the solution starting from any x(t0)∈C satisfies Then the set C is forward invariant, that is, the system (11) is safe with respect to the set C; Step 3.3 Introduction Class function, defines auxiliary functions by step-by-step derivation: If a Lipschitz continuous function α:[0,a)→[0,∞),a>0 is strictly increasing and α(0)=0, then the function is said to belong to Class function; select Class functions α1(·), α2(·), by successively calculating the time derivative of b(x, t), the auxiliary function is defined as The corresponding security set sequence C i for: Step 3.4 Design a high-order control barrier function control constraint set: If exists Class functions α1(·), α2(·) make For all (x, t)∈C1(t)∩C2(t)∩C3(t)×[t0,∞), then the function b(x, t) is a HOCBF of the system (11), where and denotes the Lie derivatives along f and g with respect to x, respectively. O(b(x,t)) is as follows: Denote ψ1 as the independent variable of α1(·), i.e., α1(ψ1); The set of HOCBF control constraints is: In the set K hocbf The control input u within can ensure the system safety.

5. The method according to claim 1, characterized in that The combination of control constraints and optimal control problem described in step 4, solving the optimal control input through quadratic programming, includes: Step 4.1 Combine the control constraints with the optimal control problem: 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: 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; Step 4.2 solves the quadratic programming and obtains the real-time optimal control input: The problem is solved at each time step, and the time interval [t0,t f ] is divided into a series of unequal time intervals {[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 expressed as a QP sequence; specifically, Solving QP: The standard form of QP is For a symmetric positive definite matrix H and a constant vector F, since Ignore the last constant term, H = 2I n , Convert the inequality constraint into the form of Ax≤B, then 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 robot arm is 6. A robot arm safety obstacle avoidance control system based on high-order control obstacle function, characterized in that: include: The dynamic model building module is used to build the dynamic model of the multi-degree-of-freedom robot arm and transform the dynamic equations of the joint space into the Cartesian space; The stable tracking controller design module designs a stable tracking controller based on the task trajectory combined with the backstepping control method, performs task trajectory planning in Cartesian space, decomposes the nonlinear system into a series of subsystems that do not exceed the system order, and designs the Lyapunov function step by step; The high-order control obstacle function constraint establishment module establishes a distance-based obstacle collision detection model to describe the high-order control obstacle function, provide safety constraints for the control system, define safety sets, and introduce Class function, define auxiliary functions by step-by-step derivation, and design high-order control obstacle function control constraint set; The optimal control input solving module combines the control constraints with the optimal control problem and solves the optimal control input through quadratic programming.

7. A robot arm safety obstacle avoidance control device based on a high-order control obstacle function, characterized in that: It comprises a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, it is used to implement a robot arm safety obstacle avoidance control method based on a high-order control obstacle function as described in any one of claims 1-5.

8. A computer-readable storage medium, characterized in that: A program is stored thereon, and when the program is executed by a processor, a safe obstacle avoidance control method for a robotic arm based on a high-order control obstacle function as described in any one of claims 1 to 5 is implemented.

Citation Information

Patent Citations

  • Cooperative control method for full-state constraint mechanical arm trajectory tracking

    CN108519740A

  • Man-machine safety obstacle avoidance path planning method based on improved artificial potential field method

    CN110262478A

  • Self-adaptive evaluation control method for double-connecting-rod mechanical arm based on event triggering

    CN116604570A

  • Mobile robot online navigation and obstacle avoidance method based on CLF and CBF

    CN117008607A

  • Collision-free cooperative control method for man-machine cooperative work in advanced manufacturing

    CN118466186A

Cited By

  • Real-time decision-making method for safe movement of high-speed unmanned ship

    CN120370959A

  • A real-time decision-making method for safe movement of high-speed unmanned vessels

    CN120370959B

  • Data-driven enhanced control method for flexible double-arm humanoid robot

    CN121105040A

  • Data-driven enhanced control method for flexible dual-arm humanoid robot

    CN121105040B

  • Mechanical arm motion planning acceleration method and system based on conjugate gradient kinetic model

    CN121132631A