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

Abstract

The invention discloses a robot whole-body safety control method and system based on a robust control obstacle function, and the method comprises the steps: designing a sphere envelope for a robot and an obstacle, constructing a whole-body safety constraint, and carrying out the whole-body safety constraint on the basis of considering the state measurement error and sampling holding error of the robot; robust control obstacle function constraints are designed for angular velocity constraints and safety constraints, so that a quadratic programming problem is constructed, then the quadratic programming problem is solved in real time, the safety control torque of the robot is obtained, the whole-body obstacle avoidance of the robot is achieved, and the whole-body safety of the robot is guaranteed. The conservative property of an existing algorithm is reduced in the design aspect of the robust control obstacle function, in addition, the selected Lipschitz constant is only related to a robot dynamic model or safety constraint, the flexibility of the control process is improved, and the adaptability of 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 development of robotics technology, robots are increasingly used in industries such as industry, medicine, services, and homes. In order to ensure that robots can perform tasks safely and effectively, advanced control methods are needed to cope with complex environments and task requirements. Especially when robots interact with humans or other objects, safety is the primary consideration. Every part of the robot may come into contact with the surrounding environment during movement, so reliable control methods must be designed to ensure its overall safety.

[0003] In recent years, safety control methods based on control obstacle functions have attracted much attention. However, in practical applications, robot systems are often affected by uncertain factors such as sensor errors and external interference, resulting in inaccurate system state measurements. At the same time, since robot control systems usually perform calculations and executions at discrete sampling intervals, the robot's state changes continuously within the sampling interval, thereby introducing sampling and holding errors. Both state measurement errors and sampling and holding errors may 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 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, and γ>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 constructed auxiliary variables, 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, wherein the perception module is used to collect the current state of the robot and environmental obstacle information and transmit them to the control module, the trajectory input module is used to provide the expected trajectory of the robot and transmit it to the control module, the control module is used to generate safety control instructions in real time according to the received current state of the robot, environmental obstacle information and expected trajectory of the robot, the communication module is used to transmit the safety control instructions to the execution module, and the execution module is used to drive the robot to achieve 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 the obstacle, constructs a full-body safety constraint, designs a robust control obstacle function constraint for the angular velocity constraint and the safety constraint on the basis of considering the robot state measurement error and the sampling and holding error, thereby constructing a quadratic programming problem, and then solves the quadratic programming problem in real time to obtain the robot's safety control torque, so that the robot can achieve full-body obstacle avoidance and ensure the robot's full-body safety. 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 the safety constraint, 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 This is a result diagram of the robot colliding with an obstacle obtained by using a 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 using 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 is a result diagram of robot joint angular velocity 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 design a spherical envelope for the robot and obstacles to construct a full-body safety constraint. 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, so that the robot can achieve full-body obstacle avoidance and ensure the full-body safety of the robot.

[0045] The present invention provides a robot whole body safety control method based on 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 as a whole and the obstacles are completely surrounded. The safety constraints of the constructed robot are:

[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 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 constructed auxiliary variables, 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 in the following formula:

[0056]

[0057] in, Represent the gradient and Hessian matrix of h respectively, α1 and α2 are constant coefficients of the extended K-type function, Respectively 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 embodiment, 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 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 ball 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 joint angle position. From this, we get the following control obstacle function corresponding to the whole 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 sampling and holding 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 joint angular acceleration is limited by the actual physical boundaries. in The robot should meet the following speed constraints:

[0077]

[0078] Introducing a certain measurement error into the robot joint angle position during the simulation process in 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 and α2 are constant coefficients of the extended K-type function, Respectively 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 angle 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 2, the robot collided with an obstacle while tracking the desired trajectory.

[0088] Based on the nominal controller and the 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 clear 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, including a perception module, a trajectory input module, a control module, an execution module and a communication module. The perception module collects the current state of the robot and environmental obstacle information, and the trajectory input module provides the expected trajectory of the robot, and these information are transmitted to the control module. The control module generates obstacle avoidance and safety control instructions in real time based on the perception data and the expected trajectory; the control instructions output by the control module are transmitted to the execution module through the communication module, and the execution module drives the robot to perform corresponding actions according to the control instructions; the perception module collects the actual state of the robot's execution of the action (such as the joint angle position) in real time through the sensor, and feeds the data back to the control module. The control module dynamically adjusts the control strategy in combination with the received feedback information to form a closed-loop control to improve the robustness and accuracy of the system.

[0097] The perception module is used to collect obstacle information in the environment and the robot's own state data, including the position of the obstacle sphere and the robot's joint angle position. The perception module integrates a variety of sensors, including position encoders, lidar, depth cameras, inertial measurement units (IMUs), etc., to obtain obstacle positions and robot joint angle position measurements in real time.

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

[0099] The control module is used to design a safety control strategy using the robust control obstacle function according to the environmental obstacle information provided by the perception module, the robot state information and the expected trajectory provided by the trajectory input module, solve the second-order cone programming problem in real time, generate the safety joint driving torque that drives the robot movement as the control instruction, and send the control instruction 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 realize the rapid transmission of perception data, the input of desired trajectory and the 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 protection scope of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A robot full 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 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; 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 constructed auxiliary variables, 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 angle position measurement error, is the upper bound of the robot joint angular velocity estimation error; Step 3: 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 It is the control torque that ensures the robot meets speed constraints and safety constraints; 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.

2. The robot whole body safety control method according to claim 1, characterized in that: According to the robot's full body safety constraints, a robust control obstacle function constraint is established to construct a second-order cone programming problem, 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 It is the control torque that ensures the robot meets speed constraints and safety constraints.

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

4. A robot full body safety control system based on robust control obstacle function, characterized in that: It includes a perception module, a trajectory input module, a control module, an execution module and a communication module, wherein the perception module is used to collect the current state of the robot and environmental obstacle information and transmit them to the control module, the trajectory input module is used to provide the expected trajectory of the robot and transmit it to the control module, the control module is used to generate safety control instructions in real time according to the received current state of the robot, environmental obstacle information and expected trajectory of the robot, the communication module is used to transmit the safety control instructions to the execution module, and the execution module is used to drive the robot to achieve corresponding actions according to the control instructions.

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

6. The robot full body safety control system according to claim 4, 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.

7. The robot full body safety control system according to claim 4, 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 for driving the robot movement as a control instruction.

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

Images