Layered Safety Reinforcement Learning Control Method and System for Robotic Arm Operation

By employing a hierarchical safety reinforcement learning control method, combined with perception and control barrier functions, safety control commands are generated, solving the collision risk problem of dynamic obstacles in real robotic arm operations and achieving efficient and safe task execution.

CN123103628APending Publication Date: 2026-11-17SHANGHAI JIAOTONG UNIV
View PDF 1 Cites 0 Cited by

Patent Information

Application Number
CN202611212772.0
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2026-08-11
Publication Date
2026-11-17

AI Technical Summary

Technical Problem

Existing technologies struggle to effectively handle dynamic obstacles in real-world robotic arm operations, leading to a high risk of collisions. Furthermore, existing methods suffer from high computational burdens, poor real-time performance, and difficulty in maintaining both task execution efficiency and safety.

Method used

A hierarchical safety reinforcement learning control method is adopted. By sensing the state of obstacles, nominal control commands are generated using pre-training strategies. Safety constraints are constructed by combining control barrier functions and quadratic programming, and safety control commands are generated to achieve real-time safety control of the robotic arm.

Benefits of technology

It significantly reduced the collision rate, improved the mission success rate, maintained mission execution efficiency, and achieved safety and mission completion capability in dynamic environments.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN123103628A_ABST
    Figure CN123103628A_ABST
Patent Text Reader

Abstract

This invention provides a hierarchical safety reinforcement learning control method and system for robotic arm operation, comprising: acquiring perception information of the robotic arm's workspace, the perception information including obstacle state information; inputting the robotic arm's state information, target pose, and obstacle state information into a pre-trained policy model to generate nominal control commands; calculating distance constraints between the robotic arm and obstacles based on the robotic arm's state information and the obstacle state information; constructing safety constraints regarding the nominal control commands based on a control barrier function and the distance constraints; solving a preset optimization problem with reference to the nominal control commands, under the condition of satisfying the safety constraints, to obtain safety control commands; and outputting the safety control commands to control the robotic arm to perform operations. This invention preserves nominal trajectory quality and task execution efficiency with minimal intervention.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This invention belongs to the fields of robot motion control, robot safety control, and reinforcement learning technology. Specifically, it relates to a hierarchical safety reinforcement learning control method and system for robotic arm operation. More specifically, it is a hierarchical safety reinforcement learning control method and system for real-world robotic arm operation in dynamic obstacle environments. Background Technology

[0002] When a real robotic arm performs operations in an unstructured environment, it must make long-term motion decisions based on the target pose, and also deal with dynamic obstacles created by people, tools or other objects temporarily entering the workspace.

[0003] When obstacles suddenly approach, the target area is temporarily obscured, or the obstacle's movement deviates from the training distribution, the control system must adjust joint commands promptly to maintain task recovery capability while avoiding collisions. Reinforcement learning can learn smooth, high-performance control strategies in a simulation environment through long-term rewards, making it suitable for handling complex robotic arm arrival and grasping tasks.

[0004] However, pure reinforcement learning typically relies on soft rewards such as collision penalties and distance penalties to statistically encourage safe behavior, which cannot provide explicit safety constraints in real-world deployments. Perceptual errors, dynamic errors, and control delays between simulation and the real environment further amplify collision risks.

[0005] Model predictive control, trajectory optimization, and online planning can explicitly handle constraints, but in multi-degree-of-freedom robotic arms, 3D perception, and dynamic obstacle scenarios, it is necessary to frequently update obstacle states and repeatedly solve nonlinear optimization problems. The computational burden, model accuracy dependence, and dynamic obstacle prediction errors can affect real-time performance; when the original planned trajectory fails due to obstacle entry, the robotic arm may also exhibit conservative waiting, deviate from the target, or find the solution infeasible.

[0006] Control barrier functions provide an interpretable mathematical mechanism for online collision avoidance by constraining the forward invariance of the safety set. Control barrier functions are typically combined with quadratic programming to project nominal control commands onto a feasible domain that satisfies safety constraints. However, relying solely on local barrier constraints lacks long-term task intent and may lead to conservative motion; furthermore, if constraints are constructed for all robot and obstacle geometries, the scale of the online solution increases rapidly with the number of geometric relationships.

[0007] Patent document CN121552336A discloses a robot grasping obstacle avoidance path planning method, storage medium and computer equipment based on point cloud environment modeling. This solution requires frequent updates of obstacle states and repeated solving of nonlinear optimization problems, and does not overcome the technical problems that the robotic arm may conservatively wait, deviate from the target or have an infeasible solution.

[0008] This problem urgently needs to be solved. Summary of the Invention

[0009] To address the shortcomings of existing technologies, the purpose of this invention is to provide a hierarchical safety reinforcement learning control method and system for robotic arm operation.

[0010] A hierarchical safety reinforcement learning control method for robotic arm operation provided by the present invention includes: Sensing step: Acquire sensing information of the robotic arm's workspace, including the state information of obstacles; Decision-making steps: Input the state information of the robotic arm, the target pose, and the state information of the obstacle into the pre-trained policy model to generate nominal control commands; Safety filtering steps: Calculate the distance constraint between the robotic arm and the obstacle based on the state information of the robotic arm and the obstacle; construct safety constraints regarding the nominal control command based on the control barrier function and the distance constraint; and solve a preset optimization problem to obtain the safety control command, using the nominal control command as a reference, while satisfying the safety constraints. Execution steps: Output the safety control command to control the robotic arm to perform operations.

[0011] Preferably, in the safety filtering step, calculating the distance constraint between the robotic arm and the obstacle includes: approximating each link of the robotic arm as a first geometric shape, approximating the obstacle as a second geometric shape, and then calculating the signed distance between each first geometric shape and each second geometric shape as the distance constraint.

[0012] Preferably, the security filtering step further includes: selecting a portion of the calculated distance constraints from the plurality of distance constraints to construct an active constraint set; the security constraints are constructed based on the active constraint set.

[0013] Preferably, the preset optimization problem is a quadratic programming problem, and its objective function includes a penalty term for deviation of the safety control command from the nominal control command and a penalty term for slack variables used to alleviate constraint conflicts.

[0014] Preferably, the construction of security constraints regarding the nominal control commands includes: linearizing the distance constraints to obtain gradient information of the distance constraints with respect to the joint states of the robotic arm; Based on the gradient information and the discrete-time control barrier function condition, an affine safety constraint is constructed regarding the safety control command; The affine safety constraint is: (20) in, Indicates the first There are approximate gradient row vectors of active distance constraints relative to joint positions, where... Indicates the index of the geometric relations or distance constraints in the active constraint set; The transpose operation represents a matrix or vector; This represents the robot arm joint position vector for the next control cycle, i.e., the safety joint position command to be determined. Indicates the sequence number of the current discrete control cycle; This indicates advancing one discrete control step relative to the current control cycle; This represents the discrete-time barrier coefficient with a value range of (0,1], used to adjust the strength of the safety correction; This indicates the preset positive safety clearance, which is the minimum surface distance that the robot is required to maintain between itself and the obstacle. Indicates the first A signed surface distance function between a robot capsule and an obstacle's directed bounding box.

[0015] Preferably, in the sensing step, the sensing information includes RGB-D point clouds; The expression for the state information of the obstacle is: (12) in, Indicates the first The obstacle estimation state for each control cycle is composed of the directed bounding box pose and half-size. Indicates the first The homogeneous transformation matrix of the directed bounding box in the robot's base coordinate system for each control cycle; Indicates the first The half-size vector of the directed bounding box along the three local coordinate axes for each control cycle; In the decision-making step, the nominal control command includes a nominal joint position command; The expression for the nominal joint position command is: (2) (3) in, Indicates the first The observation vector of the input strategy model for each control cycle; Indicates the first The joint position vector of the robotic arm in each control cycle; Indicates the first The joint velocity vector of the robotic arm in each control cycle; Indicates the first The target pose for each control cycle, where the superscript "*" indicates the desired target value; Indicates the first Obstacle estimation state for each control cycle; The representation strategy model in the first The seven-degree-of-freedom nominal joint position command output in each control cycle; The parameter is The objective-conditional strategy model, in which... This represents the learnable parameters of the policy model; This represents a seven-dimensional real vector space, corresponding to the seven joint components of a seven-degree-of-freedom robotic arm.

[0016] A hierarchical safety reinforcement learning control system for robotic arm operation provided by the present invention includes: Perception module: Acquires perception information of the robotic arm's workspace, including the state information of obstacles; Decision module: Inputs the state information of the robotic arm, the target pose, and the state information of the obstacle into the pre-trained policy model to generate nominal control commands; Safety filtering module: Calculates the distance constraint between the robotic arm and the obstacle based on the state information of the robotic arm and the obstacle; constructs a safety constraint on the nominal control command based on the control barrier function and the distance constraint; and solves a preset optimization problem to obtain the safety control command, using the nominal control command as a reference, while satisfying the safety constraint. Execution module: Outputs the safety control commands to control the robotic arm to perform operations.

[0017] Preferably, in the safety filtering module, the calculation of the distance constraint between the robotic arm and the obstacle includes: approximating each link of the robotic arm as a first geometric body, approximating the obstacle as a second geometric body, and then calculating the signed distance between each first geometric body and each second geometric body as the distance constraint.

[0018] Preferably, the security filtering module further includes: selecting a portion of the calculated distance constraints to construct an active constraint set; the security constraints are constructed based on the active constraint set.

[0019] Preferably, the preset optimization problem is a quadratic programming problem, and its objective function includes a penalty term for deviation of the safety control command from the nominal control command and a penalty term for slack variables used to alleviate constraint conflicts.

[0020] Compared with the prior art, the present invention has the following beneficial effects: 1. In this invention, the learning strategy is responsible for the execution of the nominal task driven by the goal, and the online safety layer implements hard constraint correction based on the real-time perception results. Furthermore, this invention compresses the online problem into a small-scale convex quadratic programming problem that can be solved within the control cycle of a real robot by using geometric approximation, key distance screening, and distance constraint linearization.

[0021] 2. This invention decouples the nominal reinforcement learning strategy responsible for the performance of target-driven long-term tasks from the online control barrier function layer responsible for the safety constraints of real dynamic obstacles in a hierarchical manner. This allows safe deployment to be independent of the reinforcement learning training distribution's complete coverage of all obstacle movements. When the nominal joint commands satisfy all safety constraints, the secondary planning keeps the original commands unchanged and only performs local corrections when the safety gap is insufficient or there is a potential collision risk. This preserves the nominal trajectory quality and task execution efficiency with minimal intervention.

[0022] 3. This invention transforms online safety filtering into a small-scale quadratic programming problem suitable for real-time solutions by using fast distance lookup between the capsule and the directed bounding box, Top-K nearest geometric relationship filtering, and joint space linearization of nonlinear distance constraints. The preset safety distance, discrete barrier coefficient, joint weights, relaxation penalty, and Top-K number all have clear engineering meanings and can be adjusted based on perception error, control delay, and robot speed. Non-negative relaxation variables can alleviate the infeasibility problem caused by short-term conflicts or perception delays in multiple obstacle constraints, while larger relaxation penalties can limit the degree of relaxation of safety constraints. When dynamic obstacles temporarily obscure the target area, the online safety layer enables the robotic arm to maintain a safe distance; after the obstacle leaves, the nominal strategy can continue to drive the robotic arm to complete the target task.

[0023] Furthermore, the real robot experimental results provided by this invention show that, compared with the pure reinforcement learning method, this technique reduces the collision rate from 40.0% to 10.0% and increases the task success rate from 60.0% to 90.0%, verifying the comprehensive advantages of this method in terms of safety, task completion capability and execution efficiency. Attached Figure Description

[0024] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings: Figure 1 A schematic diagram of the hierarchical safety reinforcement learning robotic arm dynamic obstacle avoidance control framework provided by the present invention; Figure 2 A schematic diagram of the actual robot experimental setup provided by the present invention is shown, illustrating the Franka Panda robotic arm and the ZED-2i RGB-D camera; Figure 3A schematic diagram illustrating the process of a real robotic arm bypassing dynamic obstacles and reaching a target, as provided by this invention. Figure 4 A schematic diagram illustrating a representative process effect of a dynamic obstacle entering and temporarily obscuring the target area as provided by the present invention; Figure 5 A schematic diagram comparing the end trajectories of the three methods provided by this invention in a dynamic obstacle scenario. Detailed Implementation

[0025] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the scope of protection of the present invention.

[0026] The method provided by this invention involves RGB-D three-dimensional perception, obstacle-oriented bounding box modeling, geometric approximation of the robotic arm capsule, signed distance calculation between the capsule and the directed bounding box, control barrier function constraint generation, and online quadratic programming safety filtering. It is applicable to tasks such as robotic arm arrival, grasping, handling, and human-robot collaborative operation, and can also be extended to multiple obstacles, multiple robotic arms, mobile operation platforms, and other robot systems that need to balance task completion capability and collision safety in dynamic environments.

[0027] Furthermore, the hierarchical safety reinforcement learning method proposed in this invention can provide real-time, low-intervention, and interpretable safety corrections for real robotic arms under dynamic obstacle motion conditions that are not fully covered in the training phase, and the method's comprehensive effect on safety, task completion rate, trajectory maintenance capability, and engineering real-time performance is tested.

[0028] Among these, it verifies whether the target-conditional reinforcement learning strategy of simulation training can form smooth nominal motion oriented towards the target, and provides a joint position reference with task intent for the online safety layer.

[0029] Verify whether the online geometric perception process based on RGB-D point cloud, directed bounding box, and robotic arm capsule model can continuously obtain the signed distance, nearest point, and outward normal vector between robot links and dynamic obstacles.

[0030] Verify whether a safety filter based on a control barrier function and quadratic programming can maintain the original instruction when the nominal instruction is safe and implement the necessary minimum correction when a potential collision occurs.

[0031] The system was tested to see if it could control the scale of online constraints and adapt to a perceived control link delay of about 100 ms after activating only the nearest Top-K geometric relations.

[0032] Through comparative experiments with real robots using three methods—RL-only, Planner+CBF, and the RL+CBF method provided in this invention—the success rate, collision rate, average execution time, and average minimum distance of each link were quantified.

[0033] System overall architecture, such as Figure 1 As shown, this system includes a perception module, a decision-making module, an online safety constraint module, and a robot execution module. Each module operates in a closed loop within a discrete control cycle.

[0034] The perception module acquires point clouds using an RGB-D camera, performs segmentation, filtering, and directed bounding box fitting on the obstacle region, and transforms the obstacle pose to the robot's base coordinate system. The decision module invokes the target condition policy trained in the simulation environment to generate seven-DOF nominal joint position commands based on the robot state, target pose, and estimated obstacle state.

[0035] The online safety constraint module approximates each link of the robotic arm as a capsule, queries the surface-to-surface signed distance between the capsule and the directed bounding box of the obstacle, and selects the Top-K items that are closest to the obstacle from all candidate relationships.

[0036] Subsequently, the system constructs linear constraints for the discrete control barrier function using the nearest point normal vector and the point Jacobian, and solves a quadratic programming problem with joint limits and relaxation variables. The quadratic programming outputs safe joint position commands, which are executed by the actual robotic arm controller.

[0037] Training of the nominal objective-conditional policy. The nominal policy is trained in the Isaac Lab simulation platform, and proximal policy optimization is implemented using the RSL-RL framework. The training environment is described as a continuous-action Markov decision process, whose state space, action space, state transition, reward function, and discount factor are represented by the following equations.

[0038] (1) in, This represents a Markov decision process involving continuous actions. Representing the state space; Represents the action space; This represents the state transition kernel determined by the dynamics of the simulation environment; Represents the reward function; This represents a discount factor with a value range of (0,1); In each control cycle, the policy observation consists of the current joint position, joint velocity, target pose, and estimated obstacle state. The policy outputs the next seven-DOF nominal joint position command.

[0039] (2) (3) in, Indicates the first The observation vector of the input strategy model for each control cycle; Indicates the first The joint position vector of the robotic arm in each control cycle; Indicates the first The joint velocity vector of the robotic arm in each control cycle; Indicates the first The target pose for each control cycle, where the superscript "*" indicates the desired target value; Indicates the first Obstacle estimation state for each control cycle; The representation strategy model in the first The seven-degree-of-freedom nominal joint position command output in each control cycle; The parameter is The objective-conditional strategy model, in which... These represent the learnable parameters of the policy model, or simply policy parameters; This represents a seven-dimensional real vector space, corresponding to the seven joint components of a seven-degree-of-freedom robotic arm.

[0040] The strategy parameters are learned by maximizing the expected discount return.

[0041] (4) in, Indicates in the strategy parameters Maximize the optimization, where, These are the learnable parameters of the policy model; Representation strategy The objective function for expected discount returns; Indicates the policy Calculate the mathematical expectation of the trajectory distribution jointly induced by environmental dynamics; This represents the cumulative discounted reward obtained by summing the discounted rewards at each time step from the initial time step to the final time step, where... Indicates the sequence number of the discrete time step or control cycle. Indicates the time-domain length of a single training epoch or the termination time step; Indicates the first The scalar reward for each time step, that is, the reward corresponding to the state and action at that time step.

[0042] To improve learning efficiency under sparse task reward conditions and reduce significant modifications to nominal instructions by the online security layer, an artificial potential field-inspired guided reward is introduced during the training phase. The target attraction field is defined as follows.

[0043] (5) in, Indicates the target attraction field; Indicates a positive target attraction gain; Indicates the first The three-dimensional position vector of the end effector in the target coordinate system for each control cycle.

[0044] The end effector position is represented in the target coordinate system; the attraction gain is a positive number. The repulsion field is activated when the closest distance between the robot and the obstacle is less than the preset influence radius.

[0045] (6) in, Represents the obstacle repulsion field vector; This represents an indicator function, which takes the value 1 if the condition within the parentheses is true, and 0 otherwise. This represents the minimum marked distance between each capsule of the robot and the surface of the obstacle. This indicates the pre-defined positive influence radius of the repulsive field; Indicates a positive obstacle repulsion gain; This represents the unit repulsion direction vector in the target coordinate system; when the minimum signed distance is greater than 0, this direction points to the side away from the obstacle, and when the minimum signed distance is less than or equal to 0, it is determined based on the relative approach speed.

[0046] The desired direction of motion is obtained by adding the attractive and repulsive fields together and then normalizing them.

[0047] (7) in, This represents the resultant field vector obtained by superimposing the target attraction field and the obstacle repulsion field. This represents the desired instantaneous motion direction vector obtained by normalizing the combined field; the symbol " " represents the Euclidean norm of a vector; This represents a small positive number used to avoid zero denominators and improve numerical stability.

[0048] The end effector velocity is estimated using finite difference estimation, and then the consistency reward between the velocity direction and the desired motion direction is calculated.

[0049] (8) (9) in, Let represent the velocity vector of the end effector in the target coordinate system during the t-th control cycle, the value of which is estimated by finite difference; Indicates the previous sampling time The position vector of the end effector in the target coordinate system, where, Indicates the sampling time interval between two adjacent discrete control cycles; symbol " The "" symbol indicates a normalization flag, signifying that the corresponding vector is normalized to a unit direction vector. The reward for consistency between the velocity direction and the desired motion direction is the larger of their inner product and 0. Progress rewards are used to encourage the robotic arm to shorten the distance between the end effector and the target in the desired direction.

[0050] (10) in, This represents the target displacement vector from the current position of the end effector to the origin of the target coordinate system; It represents the reward for progress towards the target and is used to measure the degree of consistency between the target's displacement direction and the desired motion direction. The final artificial potential field-guided reward is composed of a weighted average of the directional consistency term and the goal progress term.

[0051] (11) in, Indicates the first Artificial potential field guidance rewards at each time step; The scalar weighting coefficient representing the directional consistency reward; Indicates a reward for directional consistency; This represents the scalar weighting coefficient for rewards for progress towards the target.

[0052] The aforementioned guidance rewards only shape nominal motion with a coarse obstacle avoidance tendency during simulation training and do not serve as a hard safety guarantee for real-world deployment. Dynamic obstacle safety in real-world environments is handled by the online control barrier function safety layer described below.

[0053] Obstacle perception and robot geometry modeling The obstacle point cloud output by the RGB-D camera is segmented and fitted into a directed bounding box. The obstacle state is represented by the homogeneous transformation matrix of the directed bounding box relative to the robot's base coordinate system and its half-size vector.

[0054] (12) in, Indicates the first The obstacle estimation state for each control cycle is composed of the directed bounding box pose and half-size. Indicates the first The homogeneous transformation matrix of the directed bounding box in the robot's base coordinate system for each control cycle; Indicates the first Each control cycle is a half-size vector of the directed bounding box along the three local coordinate axes.

[0055] The keyframe positions of each link are obtained based on the robot's forward kinematics, and a set of capsule bodies is constructed using predefined link endpoint pairs. For each capsule body, a nearest distance query is performed with the directed bounding box, returning the signed distance between surfaces, the nearest point on the robot side, the nearest point on the obstacle side, and the outward normal vector from the obstacle to the robot side, which serve as the distance constraints between the robot arm and the obstacle.

[0056] (13) in, Indicates the first The set of proximity relationships between all candidate capsules and directed bounding boxes for each control cycle; Indicates the first The surface-to-surface signed distance of each candidate relation; Indicates the first Among the candidate relationships, the nearest point on the robot capsule's body side is in the robot's base coordinate system. The position vector in, where the subscript Indicates the robot side; Indicates the first In each candidate relation, the obstacle has a position vector in the robot's base coordinate system b, where the subscript... o Indicates the obstacle side; Indicates the first Among the candidate relationships, the one pointing from the obstacle side to the robot side, and in the robot's base coordinate system... The outer normal unit vector is represented by the superscript. Indicates the total number of candidate neighbor relations, subscript " " indicates candidate relation index Take numbers sequentially from 1 to M.

[0057] From the calculated distance constraints, a subset of distance constraints are selected to construct an active constraint set; the security constraints are constructed based on the active constraint set.

[0058] Specifically, to reduce online computation, all candidate relations are sorted in ascending order of signed distance, and only the K closest relations are retained as the active constraint set for the current control cycle. This set is updated in real time as obstacle movement and robot configuration changes.

[0059] (14) in, Indicates the first t A set of active constraint indexes for each control cycle; This means returning the smallest value from a given distance sequence. Operations on the index of an element; This represents the number of most recent geometric relationships retained and activated in each control cycle, and is a preset positive integer.

[0060] 5.4 Distance-based control barrier function constraints Based on the control barrier function and according to the distance constraint, a security constraint is constructed regarding the nominal control command; Specifically, for each geometric relation in the active constraint set, a distance-type control barrier function is defined.

[0061] (15) in, Represents the joint position vector Next The control barrier function value for each distance constraint; This represents the vector indicating the joint position of the robotic arm. Time Signed surface distances representing the geometric relationship between a robot and obstacles; This indicates the preset positive safety clearance, which is the minimum surface distance that the robot is required to maintain between itself and the obstacle.

[0062] The default safety distance is a positive number. The safety set defined by this barrier function is as follows.

[0063] (16) in, This represents a safe set.

[0064] For discrete control at the joint position level, a discrete-time exponential control barrier condition is adopted.

[0065] (17) in, Indicates the first The first control cycle under the current joint configuration The values ​​of each control barrier function, Indicates the first The joint position vector of the robotic arm for each control cycle.

[0066] The distance function is nonlinear with respect to the next joint position. The distance constraint is linearized; specifically, to obtain a linear constraint that can be solved in real time, a first-order Taylor expansion is performed at the current joint configuration.

[0067] (18) in, Represents the joint position vector of the robotic arm Operators for finding the gradient; Indicates the first The first control cycle under the current joint configuration The possible values ​​for the signed distance; The gradient information of the distance constraint with respect to the joint state of the robotic arm is obtained. Specifically, the distance gradient is approximated by the product of the outward normal vector at the nearest point and the Jacobian of the robot point.

[0068] (19) Among them, the symbol " "Approximately equal to" Indicates the first The transpose of the outward normal unit vectors of neighboring relations, where the superscript... T Indicates transpose; Indicates the first The translation point Jacobian matrix of the nearest point on the robot side with respect to the joint position is used to map the joint space increment to the Cartesian position increment of that point; the symbol " "Indicates that it is defined as; Indicates the first The approximate gradient row vector of the active distance constraint relative to the joint position is also the... A row vector of coefficients for an affine safety constraint.

[0069] Based on the gradient information and the discrete-time control barrier function condition, an affine safety constraint is constructed regarding the safety control command; specifically, by substituting the distance linearization result into the discrete barrier condition, an affine safety constraint regarding the next joint position can be obtained.

[0070] (20) in, Indicates the first There are approximate gradient row vectors of active distance constraints relative to joint positions, where... Indicates the index of the geometric relations or distance constraints in the active constraint set; The transpose operation represents a matrix or vector; This represents the robot arm joint position vector for the next control cycle, i.e., the safety joint position command to be determined. Indicates the sequence number of the current discrete control cycle; This indicates advancing one discrete control step relative to the current control cycle; This represents the discrete-time barrier coefficient with a value range of (0,1], used to adjust the strength of the safety correction; Indicates the first A signed surface distance function between a robot capsule and an obstacle's directed bounding box.

[0071] To avoid short-term conflicts, perceived noise, or delays that could render the solution infeasible, a non-negative relaxation variable shared by all obstacle constraints is introduced.

[0072] (twenty one) 5.5 Quadratic Programming Security Filtering To simplify the quadratic programming constraints, we first define the first... The right-hand term of an affine safety constraint.

[0073] (twenty two) in, Indicates the first An affine safety constraint is a scalar on the right side of the current control cycle, determined by the current joint position, discrete-time barrier coefficient, preset safety clearance, and current signed distance.

[0074] The online safety layer uses the nominal joint position command as a reference to solve an optimization problem in each control cycle; the optimization problem is a quadratic programming problem, specifically a strictly convex quadratic programming problem.

[0075] (twenty three) in, This represents finding the minimum value of the objective function over the range of values ​​for the seven-dimensional joint position variables and the real-valued relaxation variables, where... Let represent the joint position decision variable in quadratic programming, and its optimal value serve as the safe joint position command. It represents a seven-dimensional real vector space, corresponding to the seven joint components of a seven-degree-of-freedom robotic arm; This represents a non-negative relaxation variable shared by all active obstacle constraints; Represents the set of real numbers; This represents a seventh-order positive definite diagonal matrix used to weight the cost of each joint deviating from the nominal command. This represents the seven-DOF nominal joint position command output by the strategy model; This represents the positive penalty coefficient for slack variables; This indicates that the following constraints are met; it is an abbreviation of the English word "subject to". Indicates all; Indicates the first t A set of active constraint indexes for each control cycle; This represents the lower limit vector of the allowed joint positions of the robotic arm; This represents the upper limit vector of the joint positions allowed by the robotic arm.

[0076] The weight matrix is ​​a positive definite diagonal matrix used to adjust the cost of each joint deviating from the nominal command; the relaxation variable penalty coefficient takes a large positive value so that the relaxation amount remains zero when the constraints can be satisfied simultaneously. The upper and lower limit constraints of the joints ensure that the output command is within the allowable range of the robot.

[0077] After stacking the joint variables and relaxation variables, the above quadratic programming can be interpreted as a weighted projection of the reference point onto the closed convex feasible region.

[0078] (twenty four) (25) in, Represents the decision variables based on joint position The augmented decision vector formed by stacking with the slack variable δ; Represents the joint position vector The transpose of, where the superscript T Indicates transpose; This represents the augmented reference vector formed by stacking nominal joint position commands and zero relaxation. This indicates the construction of a block diagonal matrix with a given matrix or scalar as the diagonal blocks; This represents the augmented positive definite weight matrix consisting of the joint weight matrix and the slack variable penalty coefficients; Represents a positive definite diagonal matrix used to weight the deviation of each joint from the nominal command cost; symbol " " indicates that the matrix is ​​positive definite; Indicates in the feasible region Internal augmentation variables Find the minimum value of the objective function. This represents the closed convex feasible region determined by affine safety constraints, relaxation variable nonnegativity constraints, and joint upper and lower limits; the symbol " "represents norm" Weighted notation, indicating matrix Calculate the square norm for the weights.

[0079] Since the objective function is strictly convex and the feasible region is a closed convex polyhedron, the optimal solution is unique when the feasible region is non-empty. When the nominal instructions have satisfied all safety constraints and joint limits, the safety layer keeps the nominal instructions unchanged.

[0080] (26) in, This indicates the safe joint position command output by the secondary planning. Representing slack variables The optimal value, where the superscript "*" indicates the optimal solution; the symbol " " represents the two optimal solutions that are simultaneously valid; When the first-order distance approximation holds and the relaxation is zero, the barrier constraint keeps the current safety state approximately unchanged in the discrete control; when the relaxation is non-zero, its value gives the minimum allowable degree of safety relaxation.

[0081] (27) The execution flow of a single control cycle provided by this invention includes: S100: Reads the current joint position and joint velocity of the robotic arm, and receives the target pose.

[0082] S200: Acquires RGB-D point clouds, performs obstacle segmentation, filtering, directed bounding box fitting, and robot base coordinate system transformation.

[0083] S300: Input the robot state, target pose, and obstacle estimation state into the target condition policy of the simulation training to generate nominal joint position commands.

[0084] S400: Update the position of the robotic arm capsule using forward kinematics, and calculate the signed distance, nearest point, and outward normal vector between all capsules and the directed bounding box.

[0085] S500: Filter Top-K geometric relationships in ascending order of distance to construct the active constraint set for the current period.

[0086] S600: Generates linear safety constraints in joint space based on distance-type control barrier functions, external normal vectors, and point Jacobians.

[0087] S700: Solve a quadratic programming problem with relaxation variables and joint constraints to obtain safe joint position commands.

[0088] S800: Performs joint limit checks on safety commands and sends them to the actual robotic arm controller, then proceeds to the next control cycle.

[0089] The experimental results and engineering verification are as follows: experimental platform and verification conditions.

[0090] See Figure 2 The real-world robot experiment utilized a seven-DOF Franka Emika Panda robotic arm, with point cloud data provided by a ZED-2iRGB-D camera. Obstacle bounding boxes were estimated online from the segmented point cloud. Each link of the robot was approximated by a capsule body. The computing workstation was equipped with an NVIDIA GeForce RTX 4090 GPU, and the perceived control link latency was approximately 100ms.

[0091] Ten real-world experiments were conducted on each of the three methods: RL-only, Planner+CBF, and the proposed RL+CBF. The differences between the three methods lie in the generation method of the nominal instructions and whether online safety filtering is applied: RL-only directly executes the reinforcement learning policy output; Planner+CBF uses mplib to plan joint trajectories and applies the same control barrier function for safety filtering of the tracking instructions; RL+CBF applies the proposed safety filtering to the reinforcement learning nominal instructions.

[0092] The experimental scenarios and evaluation criteria are as follows: The validation includes two representative dynamic obstacle scenarios. In the first scenario, the obstacle is already in the workspace at the start of the experiment, and the robotic arm needs to avoid the obstacle and continue to the target. In the second scenario, the initial workspace is unobstructed, but a moving obstacle subsequently enters in front of the target and temporarily blocks the path; the robotic arm needs to remain safe and wait, resuming the task after the obstacle leaves.

[0093] The instantaneous minimum surface distance between the robot and the obstacle is defined as follows.

[0094] (28) in, This represents the instantaneous minimum surface distance between the robot and the obstacle. Index representing the geometric relationships between all robots and obstacles The operation of taking the minimum value above; Indicates the first The first time step The marked surface distance between the robot capsule and the obstacle.

[0095] A collision occurs when the instantaneous minimum surface distance is less than or equal to zero.

[0096] (29) If the end effector reaches the target pose within the specified time domain and no collision occurs during the entire test, the test is considered successful. Evaluation metrics include success rate, collision rate, average execution time, and the average minimum distance from each link to the obstacle.

[0097] The quantitative experimental results are as follows: Table 1: Experimental Results

[0098] The results show that RL-only has the shortest average execution time, but its collision rate reaches 40.0% under dynamic obstacles and external perturbations of the training distribution. Planner+CBF has a collision rate of 10.0%, but its planned trajectory is prone to failure or becomes conservative after the entry of dynamic obstacles, resulting in a success rate of only 10.0% and an average execution time of 56.22 seconds. Our proposed RL+CBF technique achieves a success rate of 90.0%, a collision rate of 10.0%, and an average execution time of 41.76 seconds, demonstrating a superior overall performance in terms of safety, task completion capability, and execution efficiency.

[0099] Table 2: Shortest distance (mm) from each joint to the obstacle

[0100] Link-level distance results show that the average minimum distances at link 7 and hand for RL-only are 17.271mm and 21.745mm, respectively, significantly lower than the 63.515mm and 63.520mm of this technology. This technology increases the safety margin between critical links near the end effector and dynamic obstacles while maintaining a high mission success rate.

[0101] The present invention also provides a hierarchical safety reinforcement learning control system for robotic arm operation. The hierarchical safety reinforcement learning control system for robotic arm operation can be implemented by executing the process steps of the hierarchical safety reinforcement learning control method for robotic arm operation. That is, those skilled in the art can understand the hierarchical safety reinforcement learning control method for robotic arm operation as a preferred embodiment of the hierarchical safety reinforcement learning control system for robotic arm operation.

[0102] A hierarchical safety reinforcement learning control system for robotic arm operation provided by the present invention includes: Perception module: Acquires perception information of the robotic arm's workspace, including the state information of obstacles; Decision module: Inputs the state information of the robotic arm, the target pose, and the state information of the obstacle into the pre-trained policy model to generate nominal control commands; Safety filtering module: Calculates the distance constraint between the robotic arm and the obstacle based on the state information of the robotic arm and the obstacle; constructs a safety constraint on the nominal control command based on the control barrier function and the distance constraint; and solves a preset optimization problem to obtain the safety control command, using the nominal control command as a reference, while satisfying the safety constraint. Execution module: Outputs the safety control commands to control the robotic arm to perform operations.

[0103] Those skilled in the art will understand that, besides implementing the system and its various devices, modules, and units provided by this invention in the form of purely computer-readable program code, the same functions can be achieved entirely through logical programming of the method steps, making the system and its various devices, modules, and units of this invention function in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers. Therefore, the system and its various devices, modules, and units provided by this invention can be considered as a hardware component, and the devices, modules, and units included therein for implementing various functions can also be considered as structures within the hardware component; alternatively, the devices, modules, and units for implementing various functions can be considered as both software modules implementing the method and structures within the hardware component.

[0104] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

Claims

1. A hierarchical safety reinforcement learning control method for robotic arm operation, characterized in that, include: Sensing step: Acquire sensing information of the robotic arm's workspace, including the state information of obstacles; Decision-making steps: Input the state information of the robotic arm, the target pose, and the state information of the obstacle into the pre-trained policy model to generate nominal control commands; Safety filtering steps: Calculate the distance constraint between the robotic arm and the obstacle based on the state information of the robotic arm and the obstacle; construct safety constraints regarding the nominal control command based on the control barrier function and the distance constraint; Using the nominal control command as a reference, and under the condition of satisfying the safety constraints, a preset optimization problem is solved to obtain the safety control command; Execution steps: Output the safety control command to control the robotic arm to perform operations.

2. The hierarchical safety reinforcement learning control method for robotic arm operation according to claim 1, characterized in that, In the safety filtering step, the calculation of the distance constraint between the robotic arm and the obstacle includes: approximating each link of the robotic arm as a first geometric body, approximating the obstacle as a second geometric body, and then calculating the signed distance between each first geometric body and each second geometric body as the distance constraint.

3. The hierarchical safety reinforcement learning control method for robotic arm operation according to claim 2, characterized in that, The security filtering step further includes: selecting a portion of the calculated distance constraints from the multiple distance constraints to construct an active constraint set; the security constraints are constructed based on the active constraint set.

4. The hierarchical safety reinforcement learning control method for robotic arm operation according to claim 1, characterized in that, The preset optimization problem is a quadratic programming problem, and its objective function includes a penalty term for deviation of the safety control command from the nominal control command and a penalty term for slack variables used to alleviate constraint conflicts.

5. The hierarchical safety reinforcement learning control method for robotic arm operation according to claim 1, characterized in that, The construction of security constraints regarding the nominal control commands includes: linearizing the distance constraints to obtain gradient information of the distance constraints with respect to the joint states of the robotic arm; Based on the gradient information and the discrete-time control barrier function condition, an affine safety constraint is constructed regarding the safety control command; The affine safety constraint is: (20) in, Indicates the first There are approximate gradient row vectors of active distance constraints relative to joint positions, where... Indicates the index of the geometric relations or distance constraints in the active constraint set; The transpose operation represents a matrix or vector; This represents the robot arm joint position vector for the next control cycle, i.e., the safety joint position command to be determined. Indicates the sequence number of the current discrete control cycle; This indicates advancing one discrete control step relative to the current control cycle; This represents the discrete-time barrier coefficient with a value range of (0,1]. This indicates the preset positive safety clearance, which is the minimum surface distance that the robot is required to maintain between itself and the obstacle. Let represent the signed surface distance function between the i-th robot capsule and the directed bounding box of the obstacle.

6. The hierarchical safety reinforcement learning control method for robotic arm operation according to claim 1, characterized in that, In the sensing step, the sensing information includes RGB-D point clouds; The expression for the state information of the obstacle is: (12) in, Indicates the first The obstacle estimation state for each control cycle is composed of the directed bounding box pose and half-size. Indicates the first The homogeneous transformation matrix of the directed bounding box in the robot's base coordinate system for each control cycle; Indicates the first Each control cycle is a half-size vector of the directed bounding box along the three local coordinate axes. In the decision-making step, the nominal control command includes a nominal joint position command; The expression for the nominal joint position command is: (2) (3) in, Indicates the first The observation vector of the input strategy model for each control cycle; Indicates the first The joint position vector of the robotic arm in each control cycle; Indicates the first The joint velocity vector of the robotic arm in each control cycle; Indicates the first The target pose for each control cycle, where the superscript "*" indicates the desired target value; Indicates the first Obstacle estimation state for each control cycle; The representation strategy model in the first The seven-degree-of-freedom nominal joint position command output in each control cycle; The parameter is The objective-conditional strategy model, in which... These represent the learnable parameters of the policy model, or simply policy parameters; This represents a seven-dimensional real vector space, corresponding to the seven joint components of a seven-degree-of-freedom robotic arm.

7. A hierarchical safety reinforcement learning control system for robotic arm operation, characterized in that, include: Perception module: Acquires perception information of the robotic arm's workspace, including the state information of obstacles; Decision module: Inputs the state information of the robotic arm, the target pose, and the state information of the obstacle into the pre-trained policy model to generate nominal control commands; Safety filtering module: Calculates the distance constraint between the robotic arm and the obstacle based on the state information of the robotic arm and the obstacle; constructs a safety constraint regarding the nominal control command based on the control barrier function and the distance constraint. Using the nominal control command as a reference, and under the condition of satisfying the safety constraints, a preset optimization problem is solved to obtain the safety control command; Execution module: Outputs the safety control commands to control the robotic arm to perform operations.

8. The hierarchical safety reinforcement learning control system for robotic arm operation according to claim 7, characterized in that, In the safety filtering module, the calculation of the distance constraint between the robotic arm and the obstacle includes: approximating each link of the robotic arm as a first geometric body, approximating the obstacle as a second geometric body, and then calculating the signed distance between each first geometric body and each second geometric body as the distance constraint.

9. The hierarchical safety reinforcement learning control system for robotic arm operation according to claim 7, characterized in that, The security filtering module further includes: selecting a portion of the calculated distance constraints to construct an active constraint set; the security constraints are constructed based on the active constraint set.

10. The hierarchical safety reinforcement learning control system for robotic arm operation according to claim 7, characterized in that, The preset optimization problem is a quadratic programming problem, and its objective function includes a penalty term for deviation of the safety control command from the nominal control command and a penalty term for slack variables used to alleviate constraint conflicts.

Citation Information

Patent Citations

  • Robot grabbing obstacle avoidance path planning method based on point cloud environment modeling, storage medium and computer equipment

    CN121552336A