Robot whole-body safety control method and system based on robust control obstacle function

Abstract

The present invention discloses a robot whole-body safety control method and system based on a robust control obstacle function. By designing a spherical envelope for the robot and obstacles, a whole-body safety constraint is constructed. On the basis of considering the robot state measurement error and the sampling and holding error, a robust control obstacle function constraint is designed for the angular velocity constraint and the safety constraint, thereby constructing a quadratic programming problem. Then, by solving the quadratic programming problem in real time, the robot safety control torque is obtained, enabling the robot to achieve whole-body obstacle avoidance and ensure the whole-body safety of the robot. The conservatism of the existing algorithm is reduced in the design of the robust control obstacle function. In addition, since the selected Lipschitz constant is only related to the robot dynamics model or safety constraint, the flexibility of the control process is increased and the adaptability of the control is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot control, and in particular relates to a robot whole-body safety control method and system based on a robust control obstacle function. Background Art

[0002] With the advancement of robotics technology, robots are increasingly being used in industries such as industry, healthcare, services, and the home. To ensure that robots can perform their tasks safely and efficiently, advanced control methods are required to cope with complex environments and task requirements. Safety is a paramount consideration, especially when robots interact with humans or other objects. Every part of a robot may come into contact with its surroundings during movement, so reliable control methods must be designed to ensure overall safety.

[0003] Safety control methods based on control obstacle functions have attracted considerable attention in recent years. However, in practical applications, robotic systems are often subject to uncertainties such as sensor errors and external disturbances, leading to inaccurate system state measurements. Furthermore, because robotic control systems typically perform calculations and execution at discrete sampling intervals, the robot's state continuously changes within the sampling interval, introducing sample-and-hold errors. Both state measurement errors and sample-and-hold errors can affect the effectiveness of safety control. Summary of the Invention

[0004] In view of this, the present invention provides a robot full-body safety control method and system based on a robust control obstacle function, which realizes the robot's full-body obstacle avoidance and ensures the robot's full-body safety.

[0005] The present invention provides a robot whole-body safety control method based on a robust control obstacle function, which specifically includes the following steps:

[0006] Step 1: Use several spheres to design envelopes for the robot and obstacles, and construct the robot's full-body safety constraints, as shown in the following formula:

[0007]

[0008] h i (q)=||X i -X0|| 2 -d 2

[0009] in, is the actual value of the robot joint angle position, X i ,1≤i≤l is the center position of several spheres attached to the robot, X0 is the center position of the obstacle sphere, d is the safety distance, γ>0 is the smoothing factor;

[0010] Design a robust control obstacle function constraint for the robot's angular velocity constraint, as shown below:

[0011]

[0012] in, is the robot joint angle position measurement value at the kth sampling moment, is the estimated value of the robot joint angular velocity at the kth sampling moment, T is the sampling period, f2 and g2 are variables related to the robot system dynamics model, A and B are auxiliary variables constructed, and f 2,i ,A i ,B i Represents the i-th element of f2, A, B vectors, g 2,i represents the i-th row of the g2 matrix, is the inertia matrix, is the inverse of the inertia matrix, is the Coriolis and centrifugal force matrix, is the gravity term, τ k is the robot control torque at the kth sampling moment, ||τ k || is τ k The second norm of Represent f2, g respectively 2,i Lipschitz constant, β is the constant coefficient of the extended K-type function, Δ1 and Δ2 represent the upper bounds of the total error of the joint angle position and joint angular velocity, respectively. is the upper bound of the robot joint angular velocity, is the upper bound of the robot joint angular acceleration, ∈1 is the upper bound of the robot joint angular position measurement error, is the upper bound of the robot joint angular velocity estimation error;

[0013] Step 3: Construct a second-order cone programming problem based on the established robust control barrier function constraint, as shown in the following formula:

[0014]

[0015] stφ i ≥0,1≤i≤n

[0016]

[0017] ψ2≥0

[0018] α2>0

[0019] -τ max ≤τ≤τ max

[0020] Among them, τ des is the nominal controller, is the expected value of α2, [-τ max ,τ max ] are the upper and lower bounds of the robot control torque, τ safe It is the control torque that ensures the robot meets speed constraints and safety constraints;

[0021] Step 4: In actual use, the joint angle position measurement value of the controlled robot is obtained as input, the second-order cone programming problem is solved in real time to obtain the robot's safety control torque, and a control instruction containing the safety control torque is sent to the robot.

[0022] Furthermore, a robust control obstacle function constraint is established based on the robot's full-body safety constraint to construct a second-order cone programming problem, as shown in the following formula:

[0023]

[0024] stφ i ≥0,1≤i≤n

[0025]

[0026] ψ2≥0

[0027] α2>0

[0028] -τ max ≤τ≤τ max

[0029] Among them, τ des is the nominal controller, is the expected value of α2, [-τ max ,τ max ] are the upper and lower bounds of the robot control torque, τ safe It is the control torque that ensures the robot meets speed constraints and safety constraints.

[0030] Furthermore, the value of the smoothing factor is 500.

[0031] The present invention provides a robot full-body safety control system based on a robust control obstacle function, comprising a perception module, a trajectory input module, a control module, an execution module, and a communication module. The perception module is used to collect the robot's current state and environmental obstacle information and transmit them to the control module. The trajectory input module is used to provide the robot's expected trajectory and transmit it to the control module. The control module is used to generate safety control instructions in real time based on the received robot's current state, environmental obstacle information, and the robot's expected trajectory. The communication module is used to transmit the safety control instructions to the execution module. The execution module is used to drive the robot to perform corresponding actions according to the control instructions.

[0032] Furthermore, the perception module integrates a position encoder, a laser radar, a depth camera and an inertial measurement unit.

[0033] Furthermore, the expected trajectory is a trajectory composed of expected trajectory points of the robot, and the expected trajectory points are a time series including joint angular positions and joint angular velocities.

[0034] Furthermore, the control module designs a safety control strategy using a robust control obstacle function, solves a second-order cone programming problem in real time, and generates a safety joint driving torque for driving the robot movement as a control instruction.

[0035] Furthermore, the communication module uses a high-speed real-time communication protocol to transmit data.

[0036] Beneficial effects:

[0037] The present invention designs a spherical envelope for the robot and obstacles to construct full-body safety constraints. Taking into account the robot's state measurement error and sample-and-hold error, robust control obstacle function constraints are designed for the angular velocity constraint and the safety constraint, thereby constructing a quadratic programming problem. This quadratic programming problem is then solved in real time to obtain the robot's safety control torque, enabling the robot to achieve full-body obstacle avoidance and ensure its full-body safety. The design of the robust control obstacle function reduces the conservatism of existing algorithms. In addition, since the selected Lipschitz constant is only related to the robot's dynamic model or safety constraints, the flexibility of the control process is increased, and the adaptability of the control is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 Schematic diagram of the spherical obstacle, the Franka Emika Panda robot, and its spherical envelope used in the embodiment.

[0039] Figure 2 Graph showing the collision result between the robot and the obstacle obtained using the nominal controller in the embodiment.

[0040] Figure 3 This is a result diagram of a robot successfully avoiding obstacles obtained by using a robot whole-body safety control method based on a robust control obstacle function provided by the present invention in an embodiment;

[0041] Figure 4 : is a control obstacle function result diagram corresponding to the whole-body safety constraint obtained by adopting a robot whole-body safety control method based on a robust control obstacle function provided by the present invention in an embodiment;

[0042] Figure 5 3 is a diagram showing the robot joint angular velocity results obtained by using a robot whole-body safety control method based on a robust control obstacle function provided by the present invention in an embodiment; DETAILED DESCRIPTION

[0043] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0044] The present invention provides a robot full-body safety control method and system based on a robust control obstacle function. The basic idea is to construct full-body safety constraints by designing a spherical envelope for the robot and obstacles. Taking into account the robot's state measurement error and sample-and-hold error, robust control obstacle function constraints are designed for angular velocity constraints and safety constraints, thereby constructing a quadratic programming problem. The quadratic programming problem is then solved in real time to obtain the robot's safety control torque, enabling the robot to achieve full-body obstacle avoidance and ensure its full-body safety.

[0045] The present invention provides a robot whole-body safety control method based on a robust control obstacle function, which specifically includes the following steps:

[0046] Step 1: Use several spheres to design envelopes for the robot and obstacles, and construct full-body safety constraints for the robot.

[0047] Specifically, a spherical envelope is designed for each link and obstacle of the robot to ensure that the robot and the obstacle are completely surrounded. The constructed robot body safety constraint is:

[0048]

[0049] h i (q)=||X i -X0|| 2 -d 2

[0050] in, is the actual value of the robot joint angle position, X i ,1≤i≤l is the center position of several spheres attached to the robot, X0 is the center position of the obstacle sphere, d is the safety distance, and γ>0 is the smoothing factor.

[0051] Step 2: Considering the influence of robot state measurement error and sampling and holding error, a robust control obstacle function constraint is designed for the robot's angular velocity constraint.

[0052] Among them, the robust control obstacle function constraint designed for the robot's angular velocity constraint is:

[0053]

[0054] in, is the robot joint angle position measurement value at the kth sampling moment, is the estimated value of the robot joint angular velocity at the kth sampling moment, T is the sampling period, f2 and g2 are variables related to the robot system dynamics model, A and B are auxiliary variables constructed, and f 2,i ,A i ,B i Represents the i-th element of f2, A, B vectors, g 2,i represents the i-th row of the g2 matrix, is the inertia matrix, is the inverse of the inertia matrix, is the Coriolis and centrifugal force matrix, is the gravity term, τ k is the robot control torque at the kth sampling moment, ||τ k || is τ k The second norm of Represent f2, g respectively 2,i Lipschitz constant, β is the constant coefficient of the extended K-type function, Δ1 and Δ2 represent the upper bounds of the total error of the joint angle position and joint angular velocity, respectively. is the upper bound of the robot joint angular velocity, is the upper bound of the robot joint angular acceleration, ∈1 is the upper bound of the robot joint angular position measurement error, It is the upper bound of the robot joint angular velocity estimation error.

[0055] Step 3: Design a robust high-order control obstacle function constraint for the robot's full-body safety constraint, as shown below:

[0056]

[0057] in, Represent the gradient and Hessian matrix of h respectively, α1, α2 are constant coefficients of the extended K class function, Represent h, The Lipschitz constant,

[0058] Step 4: Construct a second-order cone programming problem based on the robust control barrier function constraint established in step 2, as shown in the following formula:

[0059]

[0060] stφ i ≥0,1≤i≤n

[0061]

[0062] ψ2≥0

[0063] α2>0

[0064] -τ max ≤τ≤τ max

[0065] Among them, τ des is the nominal controller, is the expected value of α2, [-τ max ,τ max ] are the upper and lower bounds of the robot control torque, τ safe It is the control torque that ensures the robot meets speed constraints and safety constraints.

[0066] Step 5: In actual use, the joint angle position measurement value of the controlled robot is obtained as input, the second-order cone programming problem is solved in real time to obtain the robot's safety control torque, and a control instruction containing the safety control torque is sent to the robot.

[0067] Example:

[0068] In this example, a robot full-body safety control method based on a robust control obstacle function provided by the present invention is used to solve the full-body safety control problem of the Franka Emika Panda robot. Spherical obstacles are used as obstacles, and the control task is to ensure that the robot avoids obstacles while tracking the desired trajectory to ensure its full-body safety. The method is verified on the CoppeliaSim simulation platform. The process is as follows:

[0069] S1. Design a spherical envelope and full-body safety constraint for the seven-degree-of-freedom Franka Emika Panda robot. A certain number of spheres are attached to each link of the robot. The number of spheres is l = 13, and the coordinates of the sphere centers are X i ,1≤i≤l, the radius of the sphere is r i ,1≤i≤l; the coordinates of the center of the spherical obstacle are X0=[0.28,-0.02,0.65] T m, the radius of the sphere is r0 = 0.03m.

[0070] In order to prevent the robot from colliding with obstacles, each sphere on the robot should maintain a certain safety distance d from the obstacle sphere. i =r i +r0,1≤i≤l, so l safety constraints are constructed as follows:

[0071] h i (q)=||X i -X0|| 2 -d i 2 ≥0,1≤i≤l

[0072] in, is the actual value of the robot's joint angle position. From this, we can obtain the control obstacle function corresponding to the following full-body safety constraint:

[0073]

[0074] Among them, γ = 500 is the smoothing factor, and satisfies

[0075] S2. Considering the influence of robot state measurement error and sample-hold error, a robust control obstacle function constraint is designed for the robot's angular velocity constraint.

[0076] The dynamic model of the robot used is: in, are the actual values ​​of the robot joint angular velocity and angular acceleration, respectively. D(q) is the inertia matrix. is the Coriolis force and centrifugal force matrix, G(q) is the gravity term, and τ is the robot control torque. Assume that the upper and lower bounds of the robot joint angular velocity are The robot's joint angular acceleration is limited by the actual physical boundaries in The robot should meet the following speed constraints:

[0077]

[0078] During the simulation process, a certain measurement error is introduced into the robot joint angle position in They are the actual value and measured value of the robot joint angle position at the kth sampling moment, and the measurement error satisfies The robot sampling period is set to T = 0.01s. Assuming that the actual value of the robot joint angular velocity at the kth sampling moment is approximately expressed as The corresponding joint angular velocity estimate is Then the estimation error satisfies

[0079] On this basis, the following robust control obstacle function constraint is constructed for the robot angular velocity constraint:

[0080]

[0081] Among them, f 2,i ,A i ,B i Represents the i-th element of f2, A, B vectors, g 2,i represents the i-th row of the g2 matrix, Represent f2, g respectively 2,i The Lipschitz constant, β = 100, is the constant coefficient for the extended K-type function.

[0082] S3. Considering the influence of robot state measurement error and sampling and holding error, a robust high-order control obstacle function constraint is designed for the robot's full-body safety constraint h(q)≥0, which is expressed as:

[0083]

[0084] in, Represent the gradient and Hessian matrix of h respectively, α1, α2 are constant coefficients of the extended K class function, Represent h, The Lipschitz constant of g2,

[0085] S4. According to the robust control obstacle function constraint, construct a second-order cone programming problem. Assume that the robot's desired joint angle position is q d , the expected joint angular velocity is The tracking errors of joint angle position and joint angular velocity are q e =q d -q and Design the following nominal controller:

[0086]

[0087] Among them, K p =diag(300,300,300,300,150,150,150) and K d =diag(50,50,50,50,25,25,25) is the control gain. The simulation results under the nominal controller are shown in the attached figure. Figure 2 As shown in Figure 3, the robot collided with an obstacle while tracking the desired trajectory.

[0088] Based on the nominal controller and robust control barrier function constraints, the following second-order cone programming problem is constructed:

[0089]

[0090] stφ i ≥0,1≤i≤7

[0091]

[0092] ψ2≥0

[0093] α2>0

[0094] -τ max ≤τ≤τ max

[0095] Among them, τ, α2 are the decision variables of the optimization problem, is the expected value of α2, [-τ max ,τ max ] are the upper and lower bounds of the robot control torque, τ max =[50,50,50,50,8,8,8] T Nm. Solve the optimization problem at each sampling moment to obtain the robot's safety control torque τ safe , and apply it to the robot. The simulation results are shown in the attached Figure 3 -Attached Figure 5 As attached Figure 3 As shown in Figure 2, the robot can avoid obstacles on the desired trajectory while tracking the desired trajectory. Figure 4 Shows the control barrier function h(q) and part of h i (q) results, attached Figure 5 The results of the robot's joint angular velocity are shown, and it is obvious that the robot's safety constraints and speed constraints are met.

[0096] The present invention provides a robot full-body safety control system based on a robust control obstacle function, comprising a perception module, a trajectory input module, a control module, an execution module, and a communication module. The perception module collects information about the robot's current state and environmental obstacles, while the trajectory input module provides the robot's desired trajectory. These information are then transmitted to the control module. Based on the perception data and the desired trajectory, the control module generates obstacle avoidance and safety control instructions in real time. The control instructions output by the control module are transmitted to the execution module via the communication module, and the execution module drives the robot to perform the corresponding actions based on the control instructions. The perception module uses sensors to collect the actual state of the robot's actions (such as joint angle positions) in real time and feeds this data back to the control module. The control module dynamically adjusts the control strategy based on the received feedback information, forming a closed-loop control system that improves the robustness and accuracy of the system.

[0097] The perception module collects information about obstacles in the environment and the robot's own state data, including the position of obstacle spheres and the robot's joint angles. The perception module integrates multiple sensors, including position encoders, lidar, depth cameras, and inertial measurement units (IMUs), to obtain real-time measurements of obstacle positions and robot joint angles.

[0098] The trajectory input module is used to set the robot's desired trajectory points to form a desired trajectory, guiding the robot to move along the desired trajectory. The desired trajectory points are a time series of joint angular positions and joint angular velocities. The desired trajectory is composed of the desired trajectory points. The desired trajectory points are usually set according to the actual task requirements and include the robot's desired joint angular positions and desired joint angular velocities.

[0099] The control module is used to design a safety control strategy using the robust control obstacle function based on the environmental obstacle information provided by the perception module, the robot state information, and the desired trajectory provided by the trajectory input module. It solves the second-order cone programming problem in real time, generates the safe joint driving torque that drives the robot movement as control instructions, and sends the control instructions to the execution module to ensure that the robot avoids obstacles while tracking the reference trajectory.

[0100] The execution module is used to receive the control instructions sent by the control module and drive the robot joint torque actuator to move.

[0101] The communication module is used to realize data transmission and command interaction between modules. Specifically, it adopts high-speed real-time communication protocols (such as CAN bus or EtherCAT) to achieve rapid transmission of perception data, input of desired trajectory and timely sharing of control instructions.

[0102] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A robot whole-body safety control method based on robust control obstacle function, characterized in that: The specific steps include: Step 1: Use several spheres to design envelopes for the robot and obstacles, and construct the robot's full-body safety constraints, as shown in the following formula: h i (q)=||X i -X0|| 2 -d 2 Where q∈R n is the actual value of the robot joint angle position, X i ,1≤i≤l is the center position of several spheres attached to the robot, X0 is the center position of the obstacle sphere, d is the safety distance, γ>0 is the smoothing factor; Design a robust control obstacle function constraint for the robot's angular velocity constraint, as shown below: in, is the robot joint angle position measurement value at the kth sampling moment, is the estimated value of the robot joint angular velocity at the kth sampling moment, T is the sampling period, f2 and g2 are variables related to the robot system dynamics model, A and B are auxiliary variables constructed, and f 2,i ,A i ,B i Represents the i-th element of f2, A, B vectors, g 2,i represents the i-th row of the g2 matrix, is the inertia matrix, is the inverse of the inertia matrix, is the Coriolis and centrifugal force matrix, is the gravity term, τ k is the robot control torque at the kth sampling moment, ||τ k || is τ k The second norm of Represent f2, g respectively 2,i Lipschitz constant, β is the constant coefficient of the extended K-type function, Δ1 and Δ2 represent the upper bounds of the total error of the joint angle position and joint angular velocity, respectively. is the upper bound of the robot joint angular velocity, is the upper bound of the robot joint angular acceleration, ∈1 is the upper bound of the robot joint angular position measurement error, is the upper bound of the robot joint angular velocity estimation error; Step 2: Construct a second-order cone programming problem based on the established robust control barrier function constraint, as shown in the following formula: s.t.φ i ≥0,1≤i≤n ψ2≥0 α2>0 —t max ≤τ≤τ max Among them, τ des is the nominal controller, is the expected value of α2, [-τ max ,τ max ] are the upper and lower bounds of the robot control torque, τ safe is the control torque that ensures the robot meets the speed constraint and safety constraint, ψ2 is the high-order control obstacle function constraint; Step 3: In actual use, the joint angle position measurement value of the controlled robot is obtained as input, the second-order cone programming problem is solved in real time to obtain the robot's safety control torque, and a control instruction containing the safety control torque is sent to the robot.

2. The robot whole body safety control method according to claim 1, characterized in that: The value of the smoothing factor is 500.

3. A robot whole body safety control system based on a robust control obstacle function and adopting the robot whole body safety control method according to claim 1, characterized in that: It includes a perception module, a trajectory input module, a control module, an execution module and a communication module. Among them, the perception module is used to collect the robot's current state and environmental obstacle information and transmit them to the control module. The trajectory input module is used to provide the robot's expected trajectory and transmit it to the control module. The control module is used to generate safety control instructions in real time according to the received robot's current state, environmental obstacle information and robot's expected trajectory. The communication module is used to transmit the safety control instructions to the execution module. The execution module is used to drive the robot to achieve corresponding actions according to the control instructions.

4. The robot whole body safety control system according to claim 3, characterized in that: The perception module integrates a position encoder, a laser radar, a depth camera and an inertial measurement unit.

5. The robot whole body safety control system according to claim 3, characterized in that: The expected trajectory is a trajectory composed of expected trajectory points of the robot, and the expected trajectory points are a time series including joint angular positions and joint angular velocities.

6. The robot whole body safety control system according to claim 3, characterized in that: The control module designs a safety control strategy using a robust control obstacle function, solves a second-order cone programming problem in real time, and generates a safety joint driving torque that drives the robot movement as a control instruction.

7. The robot whole body safety control system according to claim 3, characterized in that: The communication module uses a high-speed real-time communication protocol to transmit data.

Images