A safety-critical control method and system for a robot arm with fused end pose lock

Abstract

The application belongs to the technical field of mechanical arm movement, and discloses a mechanical arm safety-related control method and system fusing end posture locking. The method comprises the following steps: establishing a controller taking the minimum deviation between actual joint speeds at different times and target joint speeds as a target function, and taking that the mechanical arm end does not collide with obstacles and moves smoothly during movement as a constraint condition; solving the optimal joint speed of the controller at different times, and the joints of the mechanical arm move according to the optimal speed, thereby realizing the control of the mechanical arm. Through the application, the problem that the mechanical arm end posture cannot be abstracted as a mathematical constraint and embedded into the quadratic programming of the control obstacle function is solved.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of mechanical arm movement, and more particularly relates to a mechanical arm safety-relevant control method and system fusing end posture locking. BACKGROUND

[0002] It is an important direction of current robot sorting research to realize efficient obstacle avoidance of robots by using safety-relevant control methods. At present, fixed task scheduling is mainly used in China to adapt to different scenes. However, when dealing with large-scale rapid response in complex industrial environments, due to various factors such as workpiece characteristics, interaction quality and open scenes, the safety of robot manipulation still faces challenges. At present, the mainstream safety sorting technology of mechanical arms includes path planning and reinforcement learning, which adopts the idea of planning first and then executing. Although this kind of method can realize the basic sorting function, it is difficult to balance safety and motion efficiency when planning trajectories and avoiding obstacles in complex open scenes. The safety-relevant control method uses quadratic programming to solve and ensure the real-time performance of the algorithm, which can efficiently complete the task while strictly ensuring the safety of sorting actions.

[0003] In the task of mechanical arm sorting, in order to deal with the complex three-dimensional geometric structure of the mechanical arm, some studies use point-object, spherical envelope or point-high-order ellipsoid interaction for simplified representation, but these approximations either completely ignore the geometric shape of the robot or simplify it too much, resulting in a too conservative control strategy. Some studies have proposed a polyhedral model CBF construction method with more accurate geometric shape, but its formula lacks universality and is difficult to extend to other shapes such as spheres and cylinders. At the same time, the MPC-CBF hybrid control method based on accurate distance constraints can construct safety constraints by real-time calculation of the minimum distance between the end of the mechanical arm and the obstacle, and realize dynamic obstacle avoidance of the end of the mechanical arm, but it cannot meet the requirements of obstacle avoidance of the whole connecting rod of the mechanical arm. In order to realize precise obstacle avoidance of the whole connecting rod of the mechanical arm, the designed control barrier function can combine the advantages of geometric method and distance method, which can accurately describe the geometric relationship between the mechanical arm and the scene, and avoid excessive constraints to ensure the balance between safety and efficiency. This requires the mathematical form of the control barrier function to be continuous and differentiable, highly generalized mechanical arm and scene information, and efficient solution, which is difficult to design. Therefore, designing an efficient and accurate control barrier function to provide a more comprehensive and effective solution for mechanical arm safety sorting is a major problem.

[0004] In the control barrier function algorithm, how to integrate the end posture locking constraint is particularly important. In industrial sorting operations, the stability of the end effector directly affects the grasping accuracy, operation safety and production efficiency. When grasping plate-shaped and box-shaped workpieces, posture deviation will cause uneven contact pressure distribution of vacuum cups or clamps, reducing the success rate of grasping and increasing the risk of collision. Therefore, it is very important to keep the stability of the end posture of the manipulator during sorting. The existing end posture control mainly adopts an active control method based on force feedback. The six-axis force / torque sensor is used to monitor the force state of the end in real time, and the impedance control algorithm is combined to dynamically adjust the joint output. However, the force feedback technology requires high-quality sensors, which significantly increases the system cost, and is also demanding on the accuracy of the robot contact dynamics model, making it difficult to adapt to workpieces of different materials. In view of the problem that force feedback control relies on high-cost sensors, a solution based on kinematics optimization provides an effective alternative. By using the pseudo-inverse solution technology of the Jacobian matrix, the posture constraint condition is added in the trajectory planning stage, which can directly ensure the stability of the posture. However, there is currently no research on safety-critical control that integrates posture constraints. How to abstract the problem of keeping the end posture of the manipulator into mathematical constraints and embed it into the quadratic programming problem of the control barrier function is one of the main challenges of the present invention. SUMMARY

[0005] In view of the above defects or improvement needs of the prior art, the present application provides a manipulator safety-critical control method and system integrating end posture locking, which solves the problem that the end posture of the manipulator cannot be abstracted into mathematical constraints and embedded into the quadratic programming of the control barrier function.

[0006] To achieve the above-mentioned purpose, according to one aspect of the present application, a manipulator safety-critical control method integrating end posture locking is provided, which comprises the following steps:

[0007] A controller is established with the minimum deviation between the actual joint speed at different times and the target joint speed as the objective function, and the constraint condition that the end of the manipulator does not collide with obstacles and moves smoothly during movement; the optimal joint speed of the controller at different times is solved, and the joints of the manipulator move according to the optimal speed, thereby controlling the manipulator.

[0008] Further preferably, the objective function of the controller is as follows:

[0009]

[0010] Wherein, is the speed input solved at time t, is the nominal control law, is the joint speed of the manipulator, is the lower limit of the decay rate.

[0011] Further preferably, a constraint that the end-effector of the robot arm does not collide with the obstacle during motion, i.e. CBF constraint, is constructed as follows:

[0012] A safe set is established in which the robot arm does not collide with the obstacle;

[0013] A control barrier function is constructed according to the safe set;

[0014] The control barrier function is smoothed to obtain the required CBF constraint.

[0015] Further preferably, the control barrier function is as follows:

[0016]

[0017] wherein, is the control barrier function, q is the joint state of the robot arm, is the signed closest distance between the robot arm and the obstacle.

[0018] Further preferably, the CBF constraint is as follows:

[0019]

[0020] wherein, is the closest distance direction vector pointing from the obstacle to the robot arm, is the Jacobian matrix of the robot arm, J max is the maximum value of the Jacobian matrix norm, is the maximum value of the joint velocity allowed.

[0021] Further preferably, a constraint that the end-effector of the robot arm moves smoothly, i.e. EPL constraint, is constructed as follows:

[0022] A relationship between the angular velocity of the end-effector of the robot arm and the joint velocity is established;

[0023] The value of the derivative of the pose matrix is determined under the condition that the pose of the end-effector of the robot arm is constant;

[0024] The EPL constraint is obtained in combination with the relationship between the angular velocity and the joint velocity and the value of the derivative of the pose matrix.

[0025] Further preferably, the constraint that the end-effector of the robot arm moves smoothly is as follows:

[0026]

[0027] wherein, is the angular velocity component of the Jacobian matrix, q is the joint state of the robot arm, is the joint velocity of the robot arm.

[0028] Further preferably, the method for solving the optimal joint velocity of the controller at different time points adopts a quadratic programming method, and the controller is a proportional controller.

[0029] According to another aspect of the present application, there is provided a safety-critical control system for a robot arm with end posture locking, comprising an executor configured to execute the safety-critical control method for a robot arm with end posture locking as described above.

[0030] According to another aspect of the present application, there is provided a computer storage medium having a computer program stored thereon, the computer program being configured to implement the safety-critical control method for a robot arm with end posture locking as described above when executed by an executor.

[0031] Overall, compared with the prior art, the above technical solutions conceived by the present application have the following beneficial effects:

[0032] 1. The controller designed by the present application combines geometric accurate modeling and posture stability control, and through the fusion of control barrier function and end posture locking constraint, the safety of obstacle avoidance and the stability of operation of the robot arm can be coordinated in real time, the whole-link motion control in the sorting scene is realized, the accurate description ability of the complex geometric shape of the robot arm is reserved, and the operation stability of the end effector is ensured, the constraint and the CBF safety constraint are integrated through the quadratic programming framework, forming a multi-objective optimization system, solving the problem that the end posture of the robot arm cannot be abstracted as a mathematical constraint and embedded into the quadratic programming of the control barrier function, i.e. the contradiction between geometric simplification and posture control in the traditional method, providing a new solution for robot safety control in complex industrial scenes.

[0033] 2. The hybrid constraint architecture proposed by the present application processes the safety obstacle avoidance and posture locking demand through the quadratic programming framework, forming an innovative control paradigm. The architecture realizes accurate obstacle avoidance through a signed distance function, ensures posture stability by using the characteristics of the Jacobian matrix, and organically combines the two through optimization theory, thereby enhancing the safety and adaptability of the system.

[0034] 3. The present application significantly improves the operation performance and reliability of sorting robots in complex industrial environments. Through accurate geometric modeling and real-time optimization control, the system can ensure the safety of the whole linkage while realizing high-precision posture locking of the end effector. The posture locking algorithm enables the end effector to maintain posture locking in the motion state, greatly improving the stability of grasping; these technological advances provide reliable technical support for the intelligent upgrading of the industrial automation field, improving production efficiency while significantly reducing operational risks.

[0035] 4. The application designs a mechanical arm end posture keeping constraint. The constraint is based on the posture locking method of the Jacobian matrix angular velocity component, ensures that the derivative of the posture matrix is zero, realizes the accurate and stable control of the mechanical arm end effector, and breaks through the dependence of the traditional force feedback method on high-cost sensors. The constraint is integrated with the CBF safety constraint through a quadratic programming framework to form a multi-objective optimization system, which can not only move the mechanical arm efficiently and avoid obstacles safely, but also ensure the stability of the end posture, greatly improving the stability of the plate and box type workpiece grabbing, and providing reliable posture stability guarantee for industrial sorting operation.

[0036] 5. The application designs a control barrier function that can accurately describe the relationship between the mechanical arm and the scene. The function combines gradient optimization algorithms with the classic control barrier function framework, aiming to meet the hard constraint requirements of system safety and the smooth differentiability requirements in the control optimization process. The gradient descent part guides the system state to quickly converge to the safety region by constructing Lyapunov-type functions and using their gradient information; while the control barrier function part uses signed distance functions to accurately model safety constraints, and uses gradient decomposition techniques to handle non-smooth boundary conditions. In this way, the geometric relationship between the system state and the safety boundary can be accurately described, ensuring that a strong repulsive effect is generated when the system approaches the dangerous area, while ensuring the continuous differentiability of the control barrier function, realizing the smooth transition of the control command, and constructing an efficient quadratic programming solution framework, which significantly improves the control efficiency while ensuring safety. BRIEF DESCRIPTION OF DRAWINGS

[0037] Figure 1 is the overall workflow of the safety-critical control of the sorting robot according to the preferred embodiment of the application.

[0038] Figure 2 is a schematic diagram of the convergence of the control barrier function according to the preferred embodiment of the application. DETAILED DESCRIPTION

[0039] In order to make the purpose, technical scheme and advantages of the application clearer, the application will be further described in detail below in combination with the drawings and examples. It should be understood that the specific embodiments described herein are only used to explain the application and do not limit the application. In addition, the technical features involved in each embodiment of the application described below can be combined with each other as long as they do not conflict with each other.

[0040] The present application introduces an end posture holding constraint specially designed for mechanical arm safety control, and integrates it into a hybrid constraint architecture, namely CBF-EPL controller. The architecture combines the geometric accuracy of signed distance function and the posture control ability of Jacobian matrix, the "CBF" part uses signed distance function to accurately describe the spatial relationship between the robot and the obstacle; the "EPL" part converts the posture stability requirement into linear constraints in joint velocity space through the angular velocity component of Jacobian matrix. The two parts are integrated through the quadratic programming framework to form a complete control scheme. The method comprises the following steps:

[0041] S1. When building the sorting scene, first build a hardware platform containing UR5 robot arm and Robotiq Epick end effector. Configure the moveit motion planning framework and ur_kinematics function package in the ROS Noetic environment, and then add information such as obstacle size and position. Coordinate system unification processing and debugging are performed on the robot arm URDF model and the scene model. Finally, set key control parameters including: nominal controller proportional parameter , maximum joint velocity , maximum value of Jacobian matrix , etc.

[0042] S2. According to the sorting scene and obstacle avoidance requirements, a sorting safety set is constructed. For a degree of freedom industrial robot, the joint angle is taken as the system state, and the control model is established:

[0043]

[0044] Wherein, v is the joint velocity of the robot arm, which is the system input.

[0045] In order to ensure the safety of the system, the constraint condition that the robot arm does not collide with the obstacle must be met. According to this, the safety set is defined as: safety set: the set of joint angles in which the robot arm does not collide with the obstacle:

[0046]

[0047] Wherein, represents the set of all points of the robot arm at joint angle , represents the set of all points in g.

[0048] S3. According to the safety set, the control barrier function is defined. The signed distance function describes the closest distance between two objects, which can accurately describe the relative relationship between the robot and the collision scene.

[0049] When the point on the robot arm collides with the environmental point In local coordinate system, the transformation matrix between the local coordinate system and the world coordinate system is needed and Based on this, the signed distance between the robot and the environment is defined:

[0050]

[0051] where is the normal vector of the contact surface.

[0052] Based on the signed distance function, the control barrier function is defined:

[0053]

[0054] where, is the control barrier function, q is the joint state of the robot, is the signed distance between the robot and the obstacle.

[0055] The system safety set can be expressed as:

[0056]

[0057] To remove the and operators in the formula, the control barrier function can be further equivalent to:

[0058]

[0059] where, and represent the position of the closest point between the robot and the obstacle, represents the direction vector of the closest distance, pointing from the obstacle to the robot, these quantities all depend on the current pose of the robot .

[0060] As shown in Figure 2 , the figure is a schematic diagram of the convergence of the control barrier function. According to the forward invariance principle of the set, for the safety set , when satisfies:

[0061]

[0062] then the closed set is forward invariant, satisfies the CBF condition, any Lipschitz continuous control law that satisfies the above condition will make the set safe. That is, the control barrier function.

[0063] In practical systems, the control input is usually specified with a lower bound on the decay rate to guarantee the dynamic response of the control system, i.e.

[0064]

[0065] Under this inequality constraint, the control barrier function will decay rapidly in the form of a near-exponential function curve until it approaches 0 at infinity. This ensures that the system converges quickly while always being within the safe set.

[0066] In Euclidean space, the control barrier function is differentiable everywhere except for some special positions (such as singular points or full expansion). Since operators , and are discontinuous at some critical points, we use the gradient decomposition method to handle :

[0067]

[0068] where represents the Jacobian matrix of the manipulator arm. Since the manipulator obstacle avoidance problem only focuses on the position of the manipulator arm and not its attitude, we take as the linear velocity component of the Jacobian matrix. represents the disturbance term caused by the change in the extreme point. The approximate term is continuous in the configuration space, but discontinuous at some positions.

[0069] By determining the upper bound of the disturbance , we can approximate as a constant and then impose the CBF constraint.

[0070] Studies have shown that the upper bound of is:

[0071]

[0072] where is the maximum value of the Jacobian matrix norm.

[0073] Then, bringing the inequality into the control barrier function gives:

[0074] ​​​

[0075] where, is the maximum value of joint velocity allowed.

[0076] Let be the solution of the quadratic programming problem then:

[0077]

[0078] Then, the final form of CBF constraint is:

[0079]

[0080] where, is the closest distance direction vector, pointing from the obstacle to the robot arm; denotes the Jacobian matrix of the robot arm, since the robot arm obstacle avoidance problem only concerns the position but not the pose of the robot arm, we take as the linear velocity component of the Jacobian matrix; is the maximum value of the Jacobian matrix norm; is the maximum value of joint velocity allowed, is the lower bound of the decay rate, used to guarantee the dynamic response of the control system.

[0081] S4. The above CBF constraint has enabled the robot arm to complete the basic obstacle avoidance task. However, in industrial sorting tasks, there are often sorting tasks for large and heavy workpieces (such as plate-shaped structural parts, box-shaped materials, etc.). In order to ensure stable grasping and accurate positioning, the end pose of the sorting robot arm needs to be kept as constant as possible during movement. Therefore, based on the CBF safety constraint, an end pose preservation (EPL) constraint is introduced to construct a CBF-EPL controller.

[0082] Given the desired end pose of the robot arm , the joint velocity satisfies:

[0083]

[0084] where, is the actual matrix pose.

[0085] Through the angular velocity component of the Jacobian matrix, the pose stability requirement can be converted into a linear constraint condition in the joint velocity space. The requirement for the end pose to remain unchanged is that the angular velocity component of the robot arm end is zero. Given that the angular velocity component of the robot arm end and the joint velocity satisfies:

[0086]

[0087] where, The angular velocity component of the Jacobian matrix.

[0088] To guarantee the end-effector pose is constant, the derivative of the pose matrix should be zero:

[0089]

[0090] where is the skew-symmetric matrix representation of the cross product. Thus, the strict constraint is:

[0091]

[0092] That is, for the velocity , if it satisfies , the end-effector pose of the manipulator can be kept.

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

[0094] First, specify the nominal controller as a proportional controller. For the reference input generated by the proportional controller, we have:

[0095]

[0096] where is the actual position of the end-effector of the manipulator, is the target position, is the desired velocity of the end-effector of the manipulator.

[0097] Since the controller needs joint velocity input, we need to extend to 6 dimensions:

[0098]

[0099] where represents the extended velocity, represents the angular velocity that does not need to be controlled. Then:

[0100]

[0101] is the pseudo-inverse of the Jacobian matrix, and the final joint velocity command is obtained.

[0102] Embed the nominal controller and the double constraints into a quadratic programming problem, and the final form of the controller is:

[0103]

[0104] where is the velocity input solved at time t , is the nominal control law, providing the desired performance target for the system, is the control input for the system in ideal unconstrained environment, which depends on the current pose q of the manipulator, is the lower bound of the damping rate, which is used to guarantee the dynamic response of the control system. is the angular velocity component of the Jacobian matrix, q is the joint state of the manipulator, v is the velocity input.

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

[0106] S6. The solved joint velocity value is transmitted to the manipulator through the ROS system, and its motion is driven to a new position, and the current position is updated again, and the next step is solved according to the new position. The algorithm adopts a closed-loop control architecture, which generates optimal joint velocity commands by solving a quadratic programming problem in real time, and realizes the gradual convergence of the pose error.

[0107] As shown in Figure 1 , the overall workflow of the safety-critical control of the sorting robot is shown. The system adopts a closed-loop design, and the algorithm first generates a reference input based on a proportional controller according to the difference between the current position and the target position of the manipulator; then constructs CBF constraints and EPL constraints according to the obtained , and Jacobian matrix, respectively; finally, a quadratic programming problem is constructed and solved in real time to obtain the control input . The end pose of the manipulator is locked and the motion is safe. The bottom motion control module converts into joint control instructions to drive the manipulator model to execute actions. The joint sensor module transmits the hardware interface data of the manipulator, and feeds back the joint state in real time to form a closed-loop control.

[0108] The application will be further described in conjunction with specific examples.

[0109] In this experiment, the nominal controller proportional parameter is set to 1, the maximum joint velocity is set to 1.0, and the maximum value of the Jacobian matrix is set to 0.9. The experimental results of adding only CBF constraints and CBF-EPL constraints are as follows:

[0110]

[0111] In Table 1, the trajectory enclosed area is only the nominal control trajectory and the closed curve formed by the algorithm trajectory encloses the area, in terms of algorithm efficiency, the solving time of CBF system is microsecond level, and the solving time of MPC algorithm is millisecond level, which is 2 orders of magnitude faster than MPC response, among which the average time of CBF-EPL is the fastest, which is 35.3 ; in terms of control effect, the trajectory generated by CBF-EPL algorithm is the most efficient, the action efficiency is 17.65% higher than that of CBF, and only the manipulator under the control of CBF-EPL maintains the locking of the end posture. In general, CBF-EPL algorithm can more efficiently complete the sorting task on the basis of locking the end posture.

[0112] Those skilled in the art can easily understand that the above description is only a preferred embodiment of the present application, and is not intended to limit the present application, and any modifications, equivalent replacements and improvements made within the spirit and principles of the present application shall be included in the protection scope of the present application.

Claims

1. A safety-critical control method for a robot arm with fused end pose locking, characterized by, The method comprises the following steps: a controller is established, which takes the minimum deviation between actual joint speed and target joint speed at different time as an objective function, and takes that the robot arm does not collide with the obstacle and moves smoothly as a constraint condition; the objective function of the controller is as follows: wherein, is the velocity input solved at time t, is the nominal control law, is the joint velocity of the robot arm, is the lower bound of the decay rate.

2. The method of claim 1, wherein the method further comprises: the constraint condition that the robot arm does not collide with the obstacle during movement, i.e. CBF constraint, is constructed as follows: a safety set in which the robot arm does not collide is established; a control barrier function is constructed according to the safety set; the control barrier function is smoothed to obtain the required CBF constraint.

3. The method of claim 2, wherein the method further comprises: the control barrier function is as follows: wherein, is a control barrier function, q is the joint state of the robot arm, is the signed closest distance between the robot arm and the obstacle.

4. The method of claim 3, wherein the method further comprises: the CBF constraint condition is as follows: wherein, is the closest distance direction vector, pointing from the obstacle to the robot arm, is the Jacobian matrix of the robot arm, J max is the maximum value of the Jacobian matrix norm, is the maximum value of the joint velocity allowed.

5. The method of safety critical control of a fused end pose-locked robotic arm of claim 4, wherein, the constraint condition that the robot arm moves smoothly, i.e. EPL constraint, is constructed as follows: a relationship between the angular velocity of the robot arm end and the joint speed is established; the value of the attitude matrix derivative is determined under the condition that the attitude of the robot arm end is unchanged; the EPL constraint is obtained by combining the relationship between the angular velocity and the joint speed and the value of the attitude matrix derivative.

6. The method of safety critical control of a fused end pose-locked robotic arm of claim 5, wherein, the constraint condition that the robot arm moves smoothly is as follows: wherein, are the angular velocity components of the Jacobian matrix, q is the joint state of the robot arm, and v is the joint velocity of the robot arm.

7. The safety-critical control method of a fused end-effort locked mechanical arm as claimed in claim 1 or 6, characterized in that, the method for solving the optimal joint speed of the controller at different time adopts a quadratic programming method, and the controller is a proportional controller.

8. A safety-critical control system for a robot arm with fused end pose locking, characterized in that, The system comprises an executor configured to execute the safety-critical control method of the robot arm integrating end attitude locking according to any one of claims 1-7.

9. A computer storage medium having stored thereon a computer program, characterized in that The computer program, when executed by the executor, is configured to implement the safety-critical control method of the robot arm integrating end attitude locking according to any one of claims 1-7.

Images