Mechanical arm safety critical control method and system fused with tail end posture locking

Abstract

The invention belongs to the technical field related to mechanical arm movement, and discloses a mechanical arm safety critical control method and system fused with tail end posture locking. The method comprises the following steps: establishing a controller which takes the minimum deviation between the actual joint speed and the target joint speed at different moments as a target function and takes the condition that the tail end of the mechanical arm does not collide with an obstacle in the movement process and the movement is stable as a constraint condition; and the optimal joint speeds of the controller at different moments are solved, and the joints of the mechanical arm move according to the optimal speeds, so that the mechanical arm is controlled. By means of the method, the problem that the posture of the tail end of the mechanical arm cannot be abstracted into mathematical constraints and cannot be embedded into quadratic programming of the control obstacle function is solved.

Description

Technical Field

[0001] This invention belongs to the field of robotic arm motion technology, and more specifically, relates to a robotic arm safety-critical control method and system that integrates end-effector posture locking. Background Technology

[0002] Achieving efficient obstacle avoidance for robots using safety-critical control methods is a crucial research direction in robot sorting. Currently, domestic approaches primarily employ fixed task scheduling to adapt to different scenarios. However, when dealing with large-scale, rapid responses in complex industrial environments, robot manipulation safety remains a challenge due to factors such as workpiece characteristics, interaction quality, and open environments. Current mainstream robotic arm safety sorting technologies include path planning and reinforcement learning, employing a plan-then-execute approach. While these methods can achieve basic sorting functions, they struggle to balance safety and motion efficiency when performing trajectory planning and obstacle avoidance in complex open environments. Safety-critical control methods, on the other hand, utilize quadratic programming to ensure real-time algorithm performance, enabling efficient task completion while strictly guaranteeing the safety of sorting actions.

[0003] In robotic arm sorting tasks, to handle the complex three-dimensional geometry of the robotic arm, some studies have adopted simplified representations such as point-object, spherical envelope, or point-higher-order ellipsoid interactions. However, these approximations either completely ignore the robot's geometry or oversimplify it, leading to overly conservative control strategies. Some studies have proposed a polyhedral model (CBF) construction method with more accurate geometry, but its formula lacks generality and is difficult to extend to other shapes such as spheres and cylinders. Meanwhile, the MPC-CBF hybrid control method based on precise distance constraints can construct safety constraints by calculating the minimum distance between the robotic arm's end effector and obstacles in real time, enabling dynamic obstacle avoidance at the end effector point, but it cannot meet the obstacle avoidance requirements of the robotic arm's full linkage. To achieve precise obstacle avoidance of the robotic arm's full linkage, the designed control obstacle function can combine the advantages of geometric and distance methods, accurately describing the geometric relationship between the robotic arm and the scene while avoiding excessive constraints, ensuring a balance between safety and efficiency. This requires the control obstacle function to be mathematically continuously differentiable, highly generalizing the information of the robotic arm and the scene, and efficiently solved, making its design quite challenging. Therefore, designing efficient and accurate obstacle control functions to provide a more comprehensive and effective solution for safe sorting by robotic arms is a major issue.

[0004] In obstacle function control algorithms, integrating end-effector posture locking constraints is crucial. In industrial sorting operations, the posture stability of the end effector directly impacts gripping accuracy, operational safety, and production efficiency. When gripping plate-shaped and box-shaped workpieces, posture deviations can lead to uneven contact pressure distribution on vacuum suction cups or grippers, reducing gripping success rates and increasing collision risks. Therefore, maintaining the stability of the robotic arm's end-effector posture during sorting is essential. Existing end-effector posture control primarily employs force feedback-based active control methods. These methods utilize six-dimensional force / torque sensors to monitor the end-effector's force state in real time, combined with impedance control algorithms to dynamically adjust joint outputs. However, force feedback technology places high demands on sensors, significantly increasing system costs, and also requires stringent accuracy in the robot's contact dynamics model, making it difficult to adapt to workpieces of different materials. To address the issue of force feedback control's reliance on high-cost sensors, kinematic optimization-based solutions offer an effective alternative. By utilizing the pseudo-inverse Jacobian matrix calculation technique and incorporating posture constraints during trajectory planning, posture stability can be directly guaranteed. However, there is currently no research on safety-critical control methods that integrate posture constraints. One of the main challenges of this invention is how to abstract the problem of maintaining the posture of the robotic arm end effector into a mathematical constraint and embed it into a quadratic programming problem of controlling the obstacle function. Summary of the Invention

[0005] To address the aforementioned deficiencies or improvement needs of existing technologies, this invention provides a safety-critical control method and system for robotic arms that integrates end-effector posture locking, solving the problem of being unable to abstract the end-effector posture of a robotic arm into a mathematical constraint and embed it into a quadratic programming function for controlling obstacles.

[0006] To achieve the above objectives, according to one aspect of the present invention, a safety-critical control method for a robotic arm integrating end-effector attitude locking is provided, the method comprising the following steps: A controller is established with the objective function of minimizing the deviation between the actual joint velocity and the target joint velocity at different times, and with the constraint that the robotic arm end effector does not collide with obstacles and moves smoothly during its movement. The optimal joint velocity of the controller at different times is solved, and the joints of the robotic arm move according to the optimal velocity, thereby controlling the robotic arm.

[0007] More preferably, the objective function of the controller is as follows:

[0008] in, The velocity input is the value to be calculated at time t. It is the nominal control law. It's the joint speed of the robotic arm. It is the lower bound of the decay rate.

[0009] More preferably, the constraint that prevents the robotic arm's end effector from colliding with obstacles during its movement is a CBF constraint, which is constructed as follows: Establish a safety set to prevent collisions during robotic arm movements; Construct a control barrier function based on the security set; The control barrier function is smoothed to obtain the required CBF constraint.

[0010] More preferably, the control barrier function is as follows:

[0011] in, This is the obstacle control function, and q represents the joint state of the robotic arm. It is the closest signed distance between the robotic arm and the obstacle.

[0012] More preferably, the CBF constraint conditions are as follows:

[0013] in, It is the nearest distance direction vector, pointing from the obstacle to the robotic arm. It is the Jacobian matrix of the robotic arm. J max is the maximum value of the modulus of the Jacobian matrix. It is the maximum allowable speed of the joint.

[0014] More preferably, the constraint for smooth motion at the end effector of the robotic arm, i.e., the EPL constraint, is constructed as follows: Establish the relationship between the end-effector angular velocity and joint velocity of the robotic arm; Given that the end effector posture of the robotic arm remains unchanged, what are the values ​​of the derivative of the posture matrix? The EPL constraint is obtained by combining the relationship between the angular velocity and the joint velocity with the value of the derivative of the attitude matrix.

[0015] More preferably, the constraint conditions for smooth movement of the robotic arm's end effector are as follows:

[0016] in, Let ω be the angular velocity component of the Jacobian matrix, and q be the joint state of the robotic arm. It refers to the joint speed of the robotic arm.

[0017] More preferably, the method for determining the optimal joint speed of the controller at different times employs a quadratic programming method, and the controller is a proportional controller.

[0018] According to another aspect of the present invention, a robotic arm safety critical control system integrating end-effector attitude locking is provided, the system including an actuator for performing the above-described robotic arm safety critical control method integrating end-effector attitude locking.

[0019] According to another aspect of the present invention, a computer storage medium is provided having a computer program stored thereon, which, when executed by an actuator, is used to implement the above-described method for safety-critical control of a robotic arm with integrated end-effector posture locking.

[0020] In summary, the technical solutions conceived by this invention have the following beneficial effects compared with the prior art: 1. The controller designed in this invention integrates geometrically accurate modeling and posture stabilization control. By fusing the control obstacle function and the end effector posture locking constraint, it can coordinate the obstacle avoidance safety and operational stability of the robotic arm in real time, achieving full-link motion control in sorting scenarios. It retains the ability to accurately describe the complex geometry of the robotic arm while ensuring the operational stability of the end effector. This constraint and the CBF safety constraint are integrated in a unified manner through a quadratic programming framework to form a multi-objective optimization system. This solves the problem that the end effector posture of the robotic arm cannot be abstracted into mathematical constraints and embedded into the quadratic programming of the control obstacle function, i.e., the contradiction between geometric simplification and posture control in traditional methods. It provides a new solution for robot safety control in complex industrial scenarios.

[0021] 2. The hybrid constraint architecture proposed in this invention addresses the requirements of safe obstacle avoidance and attitude locking through a quadratic programming framework, forming an innovative control paradigm. This architecture achieves precise obstacle avoidance through a signed distance function, ensures attitude stability by utilizing the properties of the Jacobian matrix, and then organically combines the two through optimization theory, thereby enhancing the system's safety and adaptability.

[0022] 3. This invention significantly improves the operational performance and reliability of sorting robots in complex industrial environments. Through precise geometric modeling and real-time optimized control, the system can achieve high-precision attitude locking of the end effector while ensuring the safety of all linkages. The attitude locking algorithm enables the end effector to maintain attitude locking even in motion, greatly improving grasping stability. These technological advancements provide reliable technical support for the intelligent upgrading of industrial automation, significantly reducing operational risks while improving production efficiency.

[0023] 4. This invention designs a posture-maintaining constraint for the robotic arm's end effector. This constraint, based on the posture locking method using the angular velocity components of the Jacobian matrix, ensures that the derivative of the posture matrix is ​​zero, achieving precise and stable control of the robotic arm's end effector and overcoming the reliance on high-cost sensors inherent in traditional force feedback methods. This constraint, along with the CBF safety constraint, is integrated into a unified multi-objective optimization system through a quadratic programming framework. This system enables efficient robotic arm movement and safe obstacle avoidance while ensuring end-effector posture stability, significantly improving the gripping stability of plate-shaped and box-shaped workpieces and providing reliable posture stability assurance for industrial sorting operations.

[0024] 5. This invention designs a control obstacle function that accurately describes the relationship between the robotic arm and the scene. This function combines gradient optimization algorithms with the classic control obstacle function framework, aiming to simultaneously meet the hard constraints of system safety and the requirement for smooth differentiability during control optimization. The gradient descent part constructs a Lyapunov function and utilizes its gradient information to guide the system state to converge quickly to the safe region; while the control obstacle function part uses a signed distance function to accurately model the safety constraints and handles non-smooth boundary conditions through gradient decomposition techniques. In this way, the geometric relationship between the system state and the safe boundary can be accurately characterized, ensuring a sufficiently strong repulsive force when the system approaches the danger zone, while guaranteeing the continuous differentiability of the control obstacle function, achieving a smooth transition of control commands, and constructing an efficient quadratic programming solution framework, significantly improving control efficiency while ensuring safety. Attached Figure Description

[0025] Figure 1 This is the overall workflow for safety-critical control of a sorting robot constructed according to a preferred embodiment of the present invention.

[0026] Figure 2 This is a schematic diagram illustrating the convergence of the control barrier function constructed according to a preferred embodiment of the present invention. Detailed Implementation

[0027] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0028] This invention introduces an end-effector attitude maintenance constraint specifically designed for the safety control of robotic arms, and integrates it into a hybrid constraint architecture, namely the CBF-EPL controller. This architecture combines the geometric accuracy of the signed distance function with the attitude control capability of the Jacobian matrix. The "CBF" part uses the signed distance function to accurately describe the spatial relationship between the robotic arm and obstacles; the "EPL" part transforms the attitude stability requirement into a linear constraint on the joint velocity space through the angular velocity components of the Jacobian matrix. These two parts are unified and integrated through a quadratic programming framework to form a complete control scheme. The method includes the following steps: S1. When building the sorting scenario, first, a hardware platform including a UR5 robotic arm and a Robotiq Epick end effector is constructed. The moveit motion planning framework and ur_kinematics package are configured in the ROS Noetic environment, and then obstacle dimensions, positions, and other information are added. The URDF model of the robotic arm and the scenario model are then subjected to coordinate system unification and debugging. Finally, key control parameters are set, including the nominal controller proportional parameters. Maximum joint velocity Maximum value of the modulus of the Jacobian matrix wait.

[0029] S2. Construct a sorting safety set based on the sorting scenario and obstacle avoidance requirements. For a degree-of-freedom industrial robotic arm, this is based on joint angles. Assuming the system state, establish a control model:

[0030] Where v is the joint speed of the robotic arm, which is the system input.

[0031] To ensure system safety, the constraint that the robotic arm must not collide with obstacles must be met. Based on this, a safety set is defined. For: Safety set: The set of joint angles where the robotic arm does not collide with an obstacle.

[0032] in, Indicates the joint angle of the robotic arm The set of all points below, Let g represent the set of all points in g.

[0033] S3. Define the obstacle control function based on the safety set. The signed distance function describes the closest distance between two objects and can accurately describe the relative relationship between the robotic arm and the collision scene.

[0034] When the robotic arm points With environmental points When represented in a local coordinate system, a pose transformation matrix is ​​required. and Transform to the world coordinate system, and based on this, define the nearest signed distance between the robotic arm and the environment:

[0035] in This is the normal vector of the contact surface.

[0036] Based on the aforementioned signed distance function, define a control barrier function:

[0037] in, This is the obstacle control function, and q represents the joint state of the robotic arm. It is the closest signed distance between the robotic arm and the obstacle.

[0038] The system security set can then be expressed as:

[0039] To remove the formula and Operators and control barrier functions can be further equivalent to:

[0040] in, and These represent the positions of the closest points between the robotic arm and the obstacles. This represents the nearest distance direction vector, pointing from the obstacle to the robotic arm. These quantities all depend on the current pose of the robotic arm. .

[0041] like Figure 2 As shown, the figure illustrates the convergence of the control barrier function. According to the principle of forward invariance of sets, for a safe set... ,when satisfy:

[0042] Then closed set It is forward invariant. Satisfying the CBF condition, any Lipschitz continuous control law that satisfies the above conditions will make the set It is safe. This is the control barrier function.

[0043] In practical systems, for control input This is usually a control barrier function. Specify lower bound of attenuation rate To ensure the dynamic response of the control system, that is:

[0044] Under this inequality constraint, It decays rapidly in the form of a near-exponential function curve until it approaches zero infinitely. This ensures that the system converges quickly while always remaining within the safe set.

[0045] In Euclidean space, the control barrier function Except for certain special locations (such as singularities or complete expansion), it is differentiable everywhere; because In the expression - Operators, , and It is discontinuous at certain critical points. To make... Smooth throughout, using gradient decomposition method for... Processing:

[0046] in, Let the Jacobian matrix of the robotic arm be denoted as . Since the obstacle avoidance problem of the robotic arm only focuses on the position of the robotic arm rather than its orientation, we take . Let be the linear velocity component of the Jacobian matrix. This represents the disturbance term caused by changes in the extreme point. The approximate term after decomposition. Continuous in configuration space It is discontinuous in some locations.

[0047] By identifying the disturbance The upper bound can be Approximate it as a constant, and then apply CBF constraints.

[0048] Studies have shown that Upper bound:

[0049] in, It is the maximum value of the modulus of the Jacobian matrix.

[0050] Substituting the inequality into the control barrier function, we get:

[0051] in, This represents the maximum allowable joint speed.

[0052] Will Replace with a solution to a quadratic programming problem ,but:

[0053] Therefore, the final form of the CBF constraint is:

[0054] in, It is the closest distance direction vector, pointing from the obstacle to the robotic arm; Let the Jacobian matrix of the robotic arm be denoted as . Since the obstacle avoidance problem of the robotic arm only focuses on the position of the robotic arm rather than its orientation, we take . For the linear velocity components of the Jacobian matrix; It is the maximum value of the modulus of the Jacobian matrix; This represents the maximum allowable joint speed. This is the lower bound of the decay rate, used to ensure the dynamic response of the control system.

[0055] S4. The aforementioned CBF constraint enables the robotic arm to complete basic obstacle avoidance tasks. However, in industrial sorting operations, there are often tasks involving the sorting of large and heavy workpieces (such as plate-shaped structural parts, box-shaped materials, etc.). To ensure stable gripping and accurate positioning, the sorting robotic arm needs to maintain its end-effector posture as much as possible during movement. Therefore, based on the CBF safety constraint, an end-effector posture hold (EPL) constraint is introduced to construct a CBF-EPL controller.

[0056] Given the desired pose of the robotic arm's end effector Requires joint speed satisfy:

[0057] in, This represents the actual matrix pose.

[0058] The attitude stability requirement can be transformed into a linear constraint in the joint velocity space using the angular velocity components of the Jacobian matrix. Maintaining the end-effector attitude requires that the angular velocity components of the robot arm's end-effector be zero. Given the angular velocity components of the robot arm's end-effector and the joint velocities... satisfy:

[0059] in, ω represents the angular velocity component of the Jacobian matrix.

[0060] To ensure the end effector attitude remains unchanged, the derivative of the attitude matrix should be set to 0.

[0061] in This is the skew-symmetric matrix representation of the cross product. From this, we obtain strict constraints:

[0062] That is, regarding speed If satisfied This allows the robotic arm to maintain its end-effector posture.

[0063] S5. Embed the constraints constructed in S3 and S4 into a quadratic programming problem to solve for the optimal control input.

[0064] First, specify the nominal controller as a proportional controller. The reference inputs generated by the proportional controller are:

[0065] in, This represents the actual position of the robotic arm's end effector. For the target location, The desired velocity at the end point of the robotic arm.

[0066] Since the controller requires joint speed input, it is necessary to... Expanded to 6 dimensions:

[0067] in, Indicates the expanded speed. This indicates that angular velocity control is not required. Therefore:

[0068] The pseudo-inverse of the Jacobian matrix is ​​used to ultimately obtain the joint velocity command. .

[0069] Embedding the nominal controller and the dual constraints together into the quadratic programming problem yields the final form of the controller:

[0070] in, The velocity input is the solution at time t. , The nominal control law provides the desired performance target for the system. These are the control inputs of the system in an ideal, unconstrained environment; all of these quantities depend on the current pose q of the robotic arm. This is the lower bound of the decay rate, used to ensure the dynamic response of the control system. Let q be the angular velocity component of the Jacobian matrix, q be the joint state of the robotic arm, and v be the velocity input.

[0071] Solve this quadratic programming problem using OSQP to obtain the optimal solution. The goal of this problem is to minimize the deviation of the joint velocity from the nominal value, while ensuring strict locking of the end effector attitude and meeting obstacle avoidance safety requirements.

[0072] S6. Using the ROS system, the solved joint velocity values The algorithm takes input from the robotic arm and drives it to a new position, then iteratively updates the current position and proceeds to the next step of the problem-solving process based on the new position. It employs a closed-loop control architecture, generating optimal joint velocity commands by solving a quadratic programming problem in real time, thus achieving asymptotic convergence of attitude errors.

[0073] like Figure 1 The diagram illustrates the overall workflow of the safety-critical controls for the sorting robot. The system employs a closed-loop design; the algorithm first generates a reference input based on the difference between the robot's current position and the target position, using a proportional controller. ; and then based on the obtained , CBF constraints and EPL constraints are constructed using Jacobi matrices and other parameters; finally, a quadratic programming problem is constructed and solved in real time to obtain the control input. This ensures the robotic arm's end effector is locked in position and that movement is safe. The underlying motion control module will... The data is converted into joint control commands, which drive the robotic arm model to perform actions. Meanwhile, the joint sensor module transmits data from the robotic arm's hardware interface, providing real-time feedback on the joint status, thus forming a closed-loop control system.

[0074] The present invention will be further described below with reference to specific embodiments.

[0075] This experiment will specify the nominal controller proportional parameter. Set to 1, maximum joint velocity Set to 1.0, maximum value of the Jacobian matrix modulus Set to 0.9. The experimental results with only CBF constraints and CBF-EPL constraints added are as follows:

[0076] In Table 1, the trajectory enclosed area refers only to the area enclosed by the closed curve formed by the control trajectory and the algorithm trajectory. Regarding algorithm efficiency, the CBF system's solution time is in the microsecond range, while the MPC algorithm's solution time is in the millisecond range, making the CBF-EPL algorithm two orders of magnitude faster. The CBF-EPL algorithm has the fastest average solution time at 35.3 seconds. In terms of control performance, the CBF-EPL algorithm generates the most efficient trajectory, with a motion efficiency 17.65% higher than CBF. Only the robotic arm controlled by CBF-EPL maintains the locked end-effector posture. Overall, the CBF-EPL algorithm can complete sorting tasks more efficiently while maintaining the locked end-effector posture.

[0077] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A safety-critical control method for a robotic arm integrating end-effector attitude locking, characterized in that, The method includes the following steps: A controller is established with the objective function of minimizing the deviation between the actual joint velocity and the target joint velocity at different times, and with the constraint that the robotic arm end effector does not collide with obstacles and moves smoothly during its movement. The optimal joint velocity of the controller at different times is solved, and the joints of the robotic arm move according to the optimal joint velocity, thereby controlling the robotic arm.

2. The method for safety-critical control of a robotic arm integrating end-effector posture locking as described in claim 1, characterized in that, The objective function of the controller is as follows: in, The velocity input is the value to be calculated at time t. It is the nominal control law. It's the joint speed of the robotic arm. It is the lower bound of the decay rate.

3. The method for safety-critical control of a robotic arm integrating end-effector posture locking as described in claim 2, characterized in that, The constraint that prevents the robotic arm's end effector from colliding with obstacles during its movement is called the CBF constraint. The method for constructing this constraint is as follows: Establish a safety set to prevent collisions during robotic arm movements; Construct a control barrier function based on the security set; The control barrier function is smoothed to obtain the required CBF constraint.

4. The method for safety-critical control of a robotic arm integrating end-effector posture locking as described in claim 3, characterized in that, The control barrier function is as follows: in, This is the obstacle control function, and q represents the joint state of the robotic arm. It is the closest signed distance between the robotic arm and the obstacle.

5. The method for safety-critical control of a robotic arm integrating end-effector posture locking as described in claim 3, characterized in that, The CBF constraint conditions are as follows: in, It is the nearest distance direction vector, pointing from the obstacle to the robotic arm. It is the Jacobian matrix of the robotic arm. J max is the maximum value of the modulus of the Jacobian matrix. It is the maximum allowable speed of the joint.

6. The method for safety-critical control of a robotic arm integrating end-effector posture locking as described in claim 5, characterized in that, The constraint for smooth motion at the end effector of the robotic arm, namely the EPL constraint, is constructed as follows: Establish the relationship between the end-effector angular velocity and joint velocity of the robotic arm; Given that the end effector posture of the robotic arm remains unchanged, what are the values ​​of the derivative of the posture matrix? The EPL constraint is obtained by combining the relationship between the angular velocity and the joint velocity with the value of the derivative of the attitude matrix.

7. The method for safety-critical control of a robotic arm integrating end-effector posture locking as described in claim 6, characterized in that, The constraints for smooth operation of the robotic arm's end effector are as follows: in, Let q be the angular velocity component of the Jacobi matrix, q be the joint state of the robotic arm, and v be the joint velocity of the robotic arm.

8. A safety-critical control method for a robotic arm integrating end-effector posture locking as described in claim 1 or 7, characterized in that, The method for determining the optimal joint velocity of the controller at different times employs a quadratic programming approach, and the controller is a proportional controller.

9. A safety-critical control system for a robotic arm integrating end-effector attitude locking, characterized in that, The system includes an actuator for performing a robotic arm safety-critical control method with fused end-effector attitude locking as described in any one of claims 1-8.

10. A computer storage medium having a computer program stored thereon, characterized in that, When executed by the actuator, the computer program is used to implement the robotic arm safety-critical control method with integrated end-effector attitude locking as described in any one of claims 1-8.

Images