Robot teleoperation safety control method and system based on operation intention recognition and adaptive control obstacle function

Abstract

The invention belongs to the related technical field of robot teleoperation, and discloses a robot teleoperation safety control method and system for operating intention recognition and adaptive control of a barrier function. The method comprises the following steps: establishing a virtual agent between a master robot and a slave robot for teleoperation; calculating nominal control input of the virtual agent, converting the nominal control input into safety control input by taking the self-adaptive control obstacle function as a constraint, and controlling the virtual agent to track the main robot according to a preset tracking expected trajectory by using the safety control input; a slack variable in the adaptive control barrier function is set according to an interaction intention of an operator to a target; and calculating expected control input of the slave robot, and controlling the slave robot to track the virtual agent by using the expected control input so as to realize teleoperation control of the master robot on the slave robot. According to the invention, the problem that the safety control of different interactive intention functions of an operator on a target cannot be realized under a unified control framework is solved.

Description

Technical Field

[0001] The present invention belongs to the technical field related to robot teleoperation, and more specifically, relates to a robot teleoperation safety control method and system for operation intention recognition and adaptive control of obstacle functions. Background Art

[0002] In the field of robot teleoperation, safety control can effectively avoid accidental collisions and equipment damage caused by operator misoperation. Currently, the commonly used obstacle avoidance methods include artificial potential field method and control obstacle function method. However, the obstacle avoidance path generation of the artificial potential field method lacks optimization, and the obstacle avoidance effect depends on the design of the potential field function; although the control obstacle function method can generate the optimal obstacle avoidance path, it cannot identify the operator's true interaction intention and can only perform obstacle avoidance functions for all targets.

[0003] In fact, during the robot teleoperation process, facing the interactive object, the operator hopes to perform contact and obstacle avoidance actions flexibly and freely. In summary, the existing safety control methods cannot achieve both obstacle avoidance and contact functions under a unified control framework. Summary of the invention

[0004] In response to the above defects or improvement needs of the prior art, the present invention provides a robot remote operation safety control method and system for operation intention recognition and adaptive control obstacle function, which solves the problem that it is impossible to realize the safety control of the operator's different interactive intention functions to the target under a unified control framework.

[0005] To achieve the above object, according to one aspect of the present invention, a robot teleoperation safety control method for operation intention recognition and adaptive control of obstacle function is provided, characterized in that the method comprises the following steps:

[0006] Establishing a virtual agent between the teleoperated master and slave robots;

[0007] Calculating the nominal control input of the virtual agent, converting the nominal control input into a safety control input using an adaptive control obstacle function as a constraint, and using the safety control input to control the virtual agent to track the main robot according to a preset tracking desired trajectory; wherein the slack variables in the adaptive control obstacle function are set according to the operator's interaction intention with the target;

[0008] The expected control input of the slave robot is calculated, and the expected control input is used to control the slave robot to track the virtual agent, thereby realizing the teleoperation control of the master robot over the slave robot.

[0009] Further preferably, the dynamic model of the virtual agent is as follows:

[0010]

[0011] Among them, p d and is the expected position and expected velocity of the virtual agent, M p is the inertia matrix of the virtual agent, u is the safety control input of the virtual agent, n is the system degree of freedom, is the system state derivative, f(x) is the input-independent variable, and g(x) is the input-dependent variable.

[0012] Further preferably, the nominal control input is calculated using a position-force controller, and the calculation formula is as follows:

[0013] u no =u imp +u f

[0014]

[0015] u f =π(K fp (F r -F ext )+K fi ∫(F r -F ext )dt)+F ext

[0016] Among them, u no is the nominal control input of the virtual agent, u imp is the impedance control input, u f is the force control input, K p , D p are the position-related proportional and differential control gains, p r , are the reference position, velocity, and acceleration respectively, K fp , K fi are the proportional and integral control gains related to force, F r is the reference interaction force, π is an activation function, F ext For external environmental forces.

[0017] Further preferably, the desired control input of the slave robot is calculated using a proportional-differential controller, and the calculation formula is as follows:

[0018]

[0019] Among them, K q , D q are the proportional-derivative control gains in the joint space, q and are the robot joint angles and angular velocities, q dis the virtual agent position in the joint space.

[0020] Further preferably, the formula of the safety input is as follows:

[0021]

[0022] Among them, u no is the nominal control input of the virtual agent, u s is the security input, h(x) is the security certificate describing the variable constraints, is a fixed variable, Δh is a slack variable, is the derivative of the security certificate, ▽hf(x) is the input-independent Lie derivative component, ▽hg(x)u s is the input related Lie derivative component ▽hg(x)u s , α is a Class function.

[0023] Further preferably, the calculation of the slack variables in the adaptive control function is designed according to the following steps:

[0024] Determine the operator's interaction intention with the target, which includes obstacle avoidance intention and contact intention;

[0025] Calculating the time-varying weight of the interaction intention in the time domain;

[0026] The slack variables are calculated using the time-varying weights.

[0027] Further preferably, the determining of the operator's interaction intention with the target is performed according to the following steps:

[0028] Determine whether the first event is triggered, when h(x)≤h ref , the first event triggers;

[0029] Determine whether the second event is triggered. The second event triggers;

[0030] Determine whether the third event is triggered, when p d ≤p r , the third event triggers;

[0031] When the first event, the second event, and the third event are all triggered, the operator's interaction intention with the target is a contact intention, otherwise it is an obstacle avoidance intention;

[0032] Where h(x) is the security certificate describing the variable constraint, h ref is the preset reference constraint boundary, u no is the nominal control input of the virtual agent, u s For safe input, u refis the preset reference ratio, p r The default reference position of the virtual agent.

[0033] Further preferably, the time-varying weight is calculated according to the following formula:

[0034] When the operator's interaction intention to the target is contact intention, the time-varying weight is calculated according to the following formula:

[0035] TBRS k-1 =1-exp(-r*k)

[0036] TBRS k =(1-TBRS 0 )*TBRS k-1 +TBRS 0

[0037] When the operator's interaction intention with the target is obstacle avoidance, the time-varying weight is calculated according to the following formula:

[0038] TBRS k =TBRS k-1 -(1-D)*TBRS k-1 *Δk

[0039] Among them, TBRS 0 , TBRS k-1 , TBRS k are the time-varying weights of the initial moment, the previous control cycle, and the current control cycle, respectively; r is the rising rate of the time-varying weights; k is the control cycle; D describes the weight decay rate; and Δk is the unit control cycle length.

[0040] Further preferably, the slack variable is calculated according to the following formula:

[0041] Δh=Δh+β

[0042]

[0043] Among them, Δh is the slack variable, β is the change of the slack variable in each iteration cycle, and TBRS k is the time-varying weight of the current control cycle.

[0044] According to another aspect of the present invention, a robot remote operation safety control system for operation intention recognition and adaptive control of obstacle functions is provided, the system comprising an actuator for executing the above-mentioned robot remote operation safety control method for operation intention recognition and adaptive control of obstacle functions.

[0045] In general, the above technical solutions conceived by the present invention have the following beneficial effects compared with the prior art:

[0046] 1. The present invention sets a virtual agent between the master robot and the slave robot, tracks the master robot through the virtual agent, and the slave robot tracks the virtual agent. At the same time, when the virtual agent tracks the master robot, the slack variables in the adaptive control obstacle function are set according to the operator's interaction intention to the target, so as to realize the tracking of the master robot under different interaction intentions, thereby realizing the safety control problem of the operator's different interaction intention functions to the target under a unified control framework;

[0047] 2. The virtual agent used in the present invention replaces the safety constraint object from the slave robot with a preset virtual agent, avoiding the influence of the uncertainty of the slave robot model on the safety constraint and effectively improving the control performance;

[0048] 3. When solving the slack variables of the adaptive control obstacle function, the present invention first determines the operator's interaction intention with the target, then calculates the time-varying weight according to the interaction intention, and uses the time-varying weight to calculate the slack variables, so that different interaction intentions correspond to different slack variables and adaptive control obstacle function constraints. This innovation can change the degree of safety constraints in real time according to the operator's intention, and flexibly switch between obstacle avoidance and approach actions;

[0049] 4. When controlling the virtual agent to track the master robot, the present invention converts the nominal control input into a safety control input, so that the slave robot can avoid preset obstacle targets in the space while tracking the positions of the virtual agent and the master robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 The present invention is a robot teleoperation safety control framework diagram based on operation intention recognition and adaptive control obstacle function constructed according to the preferred embodiment of the present invention.

[0051] Figure 2 This is a remote operation interaction experiment scene according to a preferred embodiment of the present invention, wherein the interaction objects are three space balls.

[0052] Figure 3 This is a teleoperation interaction experiment trajectory according to a preferred embodiment of the present invention, wherein the operation intention of ball a is recognized as obstacle avoidance, the operation intention of ball b is recognized as obstacle avoidance before contact, and the operation intention of ball c is recognized as contact. DETAILED DESCRIPTION

[0053] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0054] like Figure 1 As shown in FIG. 1 , a robot teleoperation safety control framework diagram based on operation intention recognition and adaptive control obstacle function includes the following steps:

[0055] S1 establishes the dynamic model of the virtual agent and the teleoperated robot. The virtual agent is controlled by reducing the 6-DOF control of the slave robot to 3-DOF control to avoid the influence of the uncertainty of the slave robot model.

[0056] Build dynamic models of virtual agents and teleoperated robots.

[0057] Step S11: First, establish the dynamic model of the virtual agent as follows:

[0058]

[0059] where pd and is the expected position and expected velocity of the virtual agent, Mp is the inertia matrix of the virtual agent, u is the safety control input of the virtual agent, and n is the degree of freedom of the system. represents the system state derivative, f(x) represents the input-independent variable, and g(x) represents the input-dependent variable.

[0060] Step S12: Secondly, the dynamic model of the teleoperated robot is:

[0061]

[0062] where q, They are the robot joint angle, angular velocity, and angular acceleration respectively, Mq is the robot inertia matrix, Cq is the centrifugal force and Coriolis force matrix, G is the gravity matrix, J is the Jacobian matrix, τc is the robot control torque, and Fext is the environmental interaction force.

[0063] S2 uses the position-force controller to obtain the nominal control input required for the virtual agent to track the desired trajectory, and uses the adaptive control obstacle function as a constraint to convert the nominal control input into a safety control input, and uses the safety control input to control the virtual agent to track the main robot according to the desired trajectory. S21 calculates the nominal control input of the virtual agent through the impedance-force controller as:

[0064] uno=uimp+uf

[0065]

[0066] uf=π(Kfp(Fr-Fext)+Kfi∫(Fr-Fext)dt)+Fext

[0067] Where uno is the nominal control input of the virtual agent, including the impedance control input uimp and the force control input uf. In calculating the impedance control input, Kp and Dp are the position-related proportional and differential control gains, respectively. r , are the reference position, velocity, and acceleration respectively. In calculating the force control input, K fp , K fi are the proportional and integral control gains related to force, F r is the reference interaction force, and π is an activation function:

[0068]

[0069] π ensures that the desired position and the reference position are less than the activation distance d act The proportional-derivative term of the force controller takes effect only when

[0070] The specific process of converting S22 nominal control input into safety control input is as follows:

[0071] (1) Establish variable safety constraints using an adaptive control barrier function containing slack variables.

[0072] An adaptive control barrier function is constructed using slack variables and variable safety constraints, and the nominal control input is converted into a safety control input using the adaptive control barrier function. The slack variables are calculated by the interaction intention and its time-varying weight in the time domain.

[0073] The variable safety constraint established using the adaptive control barrier function is:

[0074]

[0075] where h(x) is the security certificate describing the variable constraint, which contains the fixed variables and slack variable Δh. is the derivative of the security certificate, which contains the input-independent Lie derivative component ▽hf(x) and the input-dependent Lie derivative component ▽hg(x)u s α is a Class function.

[0076] The quadratic programming algorithm can be used to calculate the control input u that meets the safety constraints and is closest to the nominal control input u no Safety control input u s :

[0077]

[0078] Quadratic programming ensures that the safety control input obtained in each control cycle is optimal.

[0079] (2) An event-triggered algorithm is used to determine whether the operator's interaction intention with the target is an obstacle avoidance intention or a contact intention.

[0080] A set of event triggering conditions affected by the current state of the system are introduced to determine whether the current operator's operation intention is obstacle avoidance intention or contact intention.

[0081] The first event is: whether the safety certificate in the adaptive control barrier function is close enough to the preset reference constraint boundary, if so, the first event is triggered, otherwise, it is not triggered;

[0082] h(x)≤h ref ?

[0083] where h ref is the reference constraint boundary, h ref This determines the trigger sensitivity of Event 1, which can be determined using a small amount of experimental testing.

[0084] The second event is: whether the ratio of the safety control input to the nominal control input is less than a preset reference ratio, if yes, the second event is triggered, otherwise, it is not triggered;

[0085]

[0086] where u ref is the reference ratio, u ref The lower it is, the more it proves that the nominal control input is affected by the safety constraints, which in turn indicates that the operator has a tendency to fight against the safety algorithm, which manifests as contact intention.

[0087] The third event is: whether the expected position of the virtual agent has crossed the preset reference position, if yes, the third event is triggered, otherwise, it is not triggered;

[0088] p d ≤p r ?

[0089] The third event increases the robustness of the operation intention judgment on the basis of the first event and the second event, and suppresses the instantaneous misjudgment caused by sensor noise.

[0090] Combining the three events, an event triggering algorithm is proposed, which uses a combination of logical judgments to determine whether the current operation intention is an obstacle avoidance intention or a contact intention:

[0091] If the first event, the second event, and the third event are all triggered, the operation intention is a contact intention;

[0092] In other cases, the operation intention is to avoid obstacles.

[0093] (3) The time-varying weights of interaction intentions in the time domain are quantified using the temporal resource sharing theory.

[0094] The time resource sharing algorithm quantifies the interaction intention as a time-varying weight. If the intention is to contact, the time-varying weight in the rising state can be calculated as follows:

[0095] TBRS k-1 =1-exp(-r*k)

[0096] TBRS k =(1-TBRS 0 )*TBRS k-1 +TBRS 0

[0097] TBRS 0 , TBRS k-1 , TBRS k They are the time-varying weights at the initial moment, the previous control cycle, and the current control cycle, r describes the rate of increase of the weight, and k is the control cycle.

[0098] If the intention is to avoid obstacles, the time-varying weight can be calculated as follows:

[0099] TBRS k =TBRS k-1 -(1-D)*TBRS k-1 *Δk

[0100] Where D describes the weight decay rate, and Δk is the unit control cycle length.

[0101] The essence of the temporal resource sharing algorithm is that when a certain operation intention (obstacle avoidance or contact) is detected by time-varying, the operator will not perform the corresponding action immediately, but will gradually increase or decrease attention over time until the time-varying weight reaches a certain threshold.

[0102] (4) The slack variables under the influence of time-varying weights are calculated according to the iterative learning algorithm, and the virtual agent safety control input in S1 under the influence of the slack variables is calculated to achieve safety control.

[0103] After obtaining the time-varying weights and trigger thresholds calculated by the time resource sharing theory, iterative learning can be used to calculate the changes in the slack variables of the current control cycle:

[0104]

[0105] Δh=Δh+β

[0106] Where β is the change of the slack variable in each iteration cycle, and one interaction intention judgment is considered as one iteration cycle. k It is a normalized value in the range of (0,1), and the trigger threshold can be selected as 0.5.

[0107] Add saturation constraints to the slack variables to increase the smoothness of the changes:

[0108]

[0109] Where Δh min and Δh max are the minimum and maximum slack variables, respectively.

[0110] S3 uses a proportional-differential controller to calculate the desired control input required for the slave robot to track the virtual agent. The desired control input is calculated using the proportional-differential controller, and the desired control input is used to control the slave robot to track the virtual agent, thereby realizing the teleoperation control of the master robot over the slave robot.

[0111] The desired control input of the teleoperated robot is calculated based on the proportional-derivative controller:

[0112]

[0113] Where K q , D q is the proportional-derivative control gain in the joint space. Since the virtual agent uses Cartesian space position control, and the teleoperated robot uses joint space position control, Figure 1 As shown, a kinematic inverse solution module needs to be added from the virtual agent to the teleoperated space robot.

[0114] In order to verify the effectiveness of the proposed operation intention recognition and adaptive control obstacle function, the accuracy is verified by using the embodiment. For this embodiment, a teleoperated robot ball obstacle avoidance and grasping task platform is built for experimental verification. Figure 2 As shown, it includes the master robot Virtuose6D TAO, the slave robot Franka, the gripper and three small balls. In the experimental test, the operator controls the Frank robot arm to pass through the center points of the three small balls in space in turn with the end gripper, and ensures that the time it stays near the small balls increases in turn.

[0115] When the method proposed in the present invention is used to perform the remote-controlled robot ball avoidance and grasping tasks, the proposed method can correctly identify the operator's operating intention and achieve the unity of obstacle avoidance and contact functions. Figure 3As shown in the figure, the position of the virtual agent is successfully tracked from the robot position. Since the first ball has the shortest residence time, it is identified as the intention of obstacle avoidance, and the slack variables have not changed, which is manifested as the spherical radius of the safety constraint has not been reduced, and the ball (red dotted line) is wrapped in the safe area, and the control system reflects the obstacle avoidance function; the second ball has a moderate residence time and is in the transition area between obstacle avoidance and contact intentions. The slack variables increase appropriately, which is manifested as the spherical radius of the safety constraint is reduced to the vicinity of the ball (red dotted line), and the control system reflects the critical function of obstacle avoidance and contact; the third ball has the longest residence time and is identified as the intention of contact, and the slack variables increase significantly, which is manifested as the spherical radius of the safety constraint is significantly reduced, and the ball (red dotted line) is exposed outside the safe area, and the control system reflects the contact function.

[0116] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A robot teleoperation safety control method for operation intention recognition and adaptive control of obstacle function, characterized in that: The method comprises the following steps: Establishing a virtual agent between the teleoperated master and slave robots; Calculating the nominal control input of the virtual agent, converting the nominal control input into a safety control input using an adaptive control obstacle function as a constraint, and using the safety control input to control the virtual agent to track the main robot according to a preset tracking desired trajectory; wherein the slack variables in the adaptive control obstacle function are set according to the operator's interaction intention with the target; The expected control input of the slave robot is calculated, and the expected control input is used to control the slave robot to track the virtual agent, thereby realizing the teleoperation control of the master robot over the slave robot.

2. A robot teleoperation safety control method for operation intention recognition and adaptive control of obstacle function as claimed in claim 1, characterized in that: The dynamic model of the virtual agent is as follows: Among them, p d and is the expected position and expected velocity of the virtual agent, M p is the virtual agent inertia matrix, u is the virtual agent's safety control input, n is the system's degree of freedom, is the system state derivative, f(x) is the input-independent variable, and g(x) is the input-dependent variable.

3. A robot teleoperation safety control method for operation intention recognition and adaptive control of obstacle function as claimed in claim 1, characterized in that: The nominal control input is calculated using a position-force controller using the following formula: in no =in imp +in f u f =π(K fp (F r -F ext )+K fi ∫(F r -F ext )dt)+F ext Among them, u no is the nominal control input of the virtual agent, u imp is the impedance control input, u f is the force control input, K p , D p are the position-related proportional and differential control gains, p r , p r , p r are the reference position, velocity, and acceleration respectively, K fp , K fi are the force-related proportional and integral control gains, F r is the reference interaction force, π is an activation function, F ext For external environmental forces.

4. A robot teleoperation safety control method for operation intention recognition and adaptive control of obstacle function as claimed in claim 1, characterized in that: The desired control input from the robot is calculated using a proportional-differential controller, and the calculation formula is as follows: Among them, K q , D q are the proportional-derivative control gains in the joint space, q and are the robot joint angles and angular velocities, q d is the virtual agent position in the joint space.

5. The robot teleoperation safety control method for operation intention recognition and adaptive control obstacle function as claimed in claim 1, characterized in that: The formula for the safety input is as follows: u s =argmin||u s -u no || 2 Among them, u no is the nominal control input of the virtual agent, u s is the security input, h(x) is the security certificate describing the variable constraints, is a fixed variable, Δh is a slack variable, is the derivative of the security certificate, is the input independent Lie derivative component, is the input related Lie derivative component α is a Class function.

6. A robot teleoperation safety control method for operation intention recognition and adaptive control of obstacle function as claimed in claim 1, characterized in that: The calculation of the slack variables in the adaptive control function is designed according to the following steps: Determine the operator's interaction intention with the target, which includes obstacle avoidance intention and contact intention; Calculating the time-varying weight of the interaction intention in the time domain; The slack variables are calculated using the time-varying weights.

7. A robot teleoperation safety control method for operation intention recognition and adaptive control of obstacle function as claimed in claim 1, characterized in that: The determination of the operator's interaction intention with the target is performed according to the following steps: Determine whether the first event is triggered, when h(x)≤h ref , the first event triggers; Determine whether the second event is triggered. The second event triggers; Determine whether the third event is triggered, when p d ≤p r , the third event triggers; When the first event, the second event, and the third event are all triggered, the operator's interaction intention with the target is a contact intention, otherwise it is an obstacle avoidance intention; Where h(x) is the security certificate describing the variable constraint, h ref is the preset reference constraint boundary, u no is the nominal control input of the virtual agent, u s For safe input, u ref is the preset reference ratio, p r The default reference position of the virtual agent.

8. The robot teleoperation safety control method for operation intention recognition and adaptive control obstacle function as claimed in claim 1, characterized in that: The time-varying weight is calculated according to the following formula: When the operator's interaction intention to the target is contact intention, the time-varying weight is calculated according to the following formula: TBRS k-1 =1-exp(-r*k) TBRS k =(1-TBRS0)*TBRS k-1 +TBRS0 When the operator's interaction intention with the target is obstacle avoidance, the time-varying weight is calculated according to the following formula: TBRS k =TBRS k-1 -(1-D)*TBRS k-1 *Δk Among them, TBRS0, TBRS k-1 , TBRS k are the time-varying weights of the initial moment, the previous control cycle, and the current control cycle, respectively; r is the rising rate of the time-varying weights; k is the control cycle; D describes the weight decay rate; and Δk is the unit control cycle length.

9. A robot teleoperation safety control method for operation intention recognition and adaptive control of obstacle function as claimed in claim 1, characterized in that: The slack variable is calculated according to the following formula: Δh=Δh+β Among them, Δh is the slack variable, β is the change of the slack variable in each iteration cycle, and TBRS k is the time-varying weight of the current control cycle.

10. A robot teleoperation safety control system with operation intention recognition and adaptive control of obstacle function, characterized in that: The system includes an actuator, which is used to execute the robot teleoperation safety control method for operation intention recognition and adaptive control obstacle function as described in any one of claims 1-9.

Images