CN114454157A - Local track adjustment and man-machine sharing control method and system suitable for robot - Google Patents

Abstract

The invention provides a local track adjustment and man-machine sharing control method and system suitable for a robot, which are used for improving the autonomy of a surgical robot and converting the relation between the robot and the robot from a master-slave mode to a cooperative mode. When the difference between the instruction of the human and the reference track of the robot is large, the robot can locally and actively adjust the reference track of the robot by combining the virtual interaction force of the human; when the difference between the human and the robot is small, the instructions of the human and the robot are comprehensively considered, the human-computer mixed cost function is dynamically adjusted based on the system safety evaluation index, the optimal control quantity is calculated, and the human-computer sharing control is realized. The invention also provides a corresponding computer program storage medium and a robot.

Description

Local track adjustment and man-machine sharing control method and system suitable for robot

Technical Field

The invention relates to the technical field of teleoperation surgical robots, in particular to a local track adjustment and man-machine sharing control method and system suitable for a robot. In particular to a local track adjustment and man-machine sharing control method and system suitable for teleoperation and suitable for a robot and a surgical robot thereof.

Background

The minimally invasive surgery robot integrates advanced intelligent robot technology into clinical surgery, fully exerts the advantages of high stability, flexible operability, motion accuracy and the like of the robot in surgery tasks, greatly reduces surgery intensity of surgeons, and avoids the risk of improving misoperation probability caused by continuous high-intensity work.

The minimally invasive surgery robot is always the key input direction of all countries in the world, and related research results are continuously promoted to be new: the da Vinci surgical robot system is the most famous, and is continuously optimized and updated, so that the performance of the da Vinci surgical robot system is more remarkable in the aspects of the operation dexterity, the safety interaction and the like of a mechanical arm. Surgical robot development in china has focused on this decade: such as "Shen Jian Hua Tuo" minimally invasive surgery robot developed by Shanghai university of transportation; "Miaomanus" series of robots developed by Tianjin university; "Huaqun-II" type minimally invasive surgery robot developed by Harbin university of industry, etc.

In the aspect of control of surgical robots, most of the current surgical robot systems are in a master-slave mode, that is, a doctor remotely controls the motion of a slave-end mechanical arm by operating a teleoperation rod, so that the automation degree is relatively low, the workload of the doctor is relatively high, and the technical level of the doctor is still relatively high. On the other hand, due to the complexity and diversity of surgical tasks, it is not possible to perform surgical tasks in a short time using fully automatically controlled robots, so the concept of man-machine sharing is more applicable to current surgical robot systems, which changes the relationship of human and robot from master-slave to cooperative, with the motion of the robot being determined by both human and robot.

The existing idea of man-machine sharing is largely applied to the control aspect, i.e. shared control. One limitation of current systems, however, is that humans cannot influence the future desired trajectory that the robot originally set through the teleoperational device, which also indicates that the robot is not sufficiently predictive of human intent. In addition, the prior art still can not adjust the control ratio of people and robot according to actual conditions developments well, and the human-computer control has not been fused with higher degree of automation yet.

Disclosure of Invention

Aiming at the defects in the prior art, the invention aims to provide a local track adjustment and man-machine sharing control method and system suitable for a robot.

The invention provides a local track adjustment and man-machine sharing control method suitable for a robot, which comprises any one or more of the following steps:

step S0, determining an initial reference trajectory based on the one-time trajectory plan: generating a feasible trajectory set based on the desired end position and the ambient environment information obtained from the sensor data; screening an optimal track from the current feasible track set as a reference track for one-time planning;

step S1, a step of local trajectory re-planning taking into account human intent: the method comprises the steps that a human being transmits a motion instruction to a robot through teleoperation equipment, whether human intention is strong or not is judged through virtual interaction force, and when the human intention is strong, the robot adjusts a local reference track;

step S2, the step of adjusting the human-computer control weight based on the system safety evaluation index: evaluating the safety of the current robot configuration through sensor data, constructing an evaluation index representing the safety of the system, and dynamically adjusting the human-computer control weight based on the evaluation index;

step S3, model prediction control based on man-machine mixed cost function: constructing a hybrid cost function based on the control cost of the robot and the human; and calculating to obtain an optimal control instruction through a model prediction controller of a hybrid cost function, so as to realize man-machine sharing control.

Preferably, the step S1 includes:

step S1.1: establishing a virtual force model representing human interaction force;

step S1.2: judging whether the human intention is strong or not through the virtual interaction force; if so, the robot does not adjust the reference track; (ii) a If not, the robot adjusts the local reference track;

the step S2 includes:

step 2.1: sensing environmental information in real time based on data of a sensor, and establishing an index representing the safety of the system; wherein the environment information comprises a distance from a boundary and a distance from an obstacle;

step S2.2: taking the system safety index as a basis for adjusting the man-machine control weight;

the step S3 includes:

step S3.1: establishing a state space expression of the slave end system based on a dynamic model of the slave end system;

step S3.2: respectively calculating error vectors of the robot and the human according to expected instructions of the robot and the human;

step S3.3: at any moment k, calculating a robot cost function;

step S3.4: at any time k, calculating a human cost function;

step S3.5: establishing the k mixing cost function at the moment based on the index representing the system safety;

step S3.6: forming a control problem based on a model prediction control framework: at any time k, the optimized time domain is t ═ k, k +1, the time domain is divided into P discrete time periods with equal time step, and the objective of the rolling optimization is to minimize the cumulative mixed cost function in the time domain; the prediction model is a state space expression established based on a dynamic model of the slave end system; the system safety constraint is that the distance between the position of the current robot and the nearest barrier is equal to the distance between the current robot and the nearest boundary;

step S3.7: and solving the optimization problem in each optimization time domain t ═ k, k +1, and calculating the optimal control quantity input at the time k.

Preferably, in the step S0:

determining a target configuration of a robot in Cartesian space as x_finalInitial position x₀Desired trajectory duration T, with the goal of planning a path from initial configuration to target configuration x_finalThe optimum trajectory is the reference trajectory x_d(t), t is a time variable;

obtaining obstacle position distribution in vivo environment by sensing device

And a motion boundary

(ii) a Generation of feasible trajectories using fast-expanding random tree RRT algorithmCollection

(ii) a An optimal track is selected from the feasible track set as a reference track x for one-time planning through the following optimization targets_d(t)：

Wherein p represents a feasible track, and the optimization index is composed of three parts respectively used for representing the shortest path, avoiding obstacles to the maximum degree, avoiding boundaries to the maximum degree, and alpha_l,α_o,α_bAll are normal numbers and are used for adjusting the proportion of the three parts.

Preferably, the step S1 includes:

step S1.1: establishing a virtual force model representing human interaction force:

M_m,D_m,K_mrespectively representing an inertia matrix, a damping matrix and a rigidity matrix;

represents the current position of the end of the robot arm;

is the corresponding expected value;

F_hcharacterizing a virtual interaction force applied by a human to the slave robot through the teleoperational device;

step S1.2: judging whether the human intention is strong through the virtual interaction force, wherein the judgment method comprises the following steps:

setting a threshold value delta_iI 1.. m, which is the corresponding virtual interaction force F_hThe lower limit of the ith component,real-time monitoring of virtual interaction force F applied by a human to a slave robot via a teleoperational device_hThe value of (d);

1) if it is

F_h≤δ_iThe robot does not adjust the reference trajectory;

2) if it is

F_h＞δ_iThen the robot carries out local reference track adjustment;

in step S1.2, the step of adjusting the local reference trajectory includes:

step S1.2.1: determining a local track range t epsilon [ t ] to be adjusted_s,t_f]T is a time variable, t_s、t_fRespectively representing the starting time and the ending time of the local track;

step S1.2.2: will be the original track x_d(t) discretization into locally discrete trajectories

Wherein the content of the first and second substances,

x_dii is 1, …, m is

x_d(t) the ith component, δ being the time interval of the selected discrete point;

step S1.2.3: for the distance from the current position to x_d(t_f) The local track is re-planned to generate a local feasible track set

Step S1.2.4: for a local feasible trajectory gamma_dLocal trace energy E (γ)_d) Is gamma_dAdjusted trajectory energy:

is the original local trace energy;

α is a normal number;

r is a positive definite symmetric matrix;

from the set of feasible trajectories based on the representation adjusted trajectory energy index

Selecting a feasible track with the minimum energy of the adjusting track as an optimal track gamma_dAs an adjusted reference trajectory;

the step S2 includes:

step S2.1: sensing environmental information in real time based on data of a sensor, and establishing an index representing the safety of the system; wherein the environment information comprises a distance from a boundary and a distance from an obstacle;

step S2.2: taking the system safety index as a basis for adjusting the man-machine control weight;

the step S2.1 comprises the steps of:

step S2.1.1: obstacle position distribution in-vivo environment obtained based on sensing data

And a motion boundary

Subscript i represents a serial number;

step S2.1.2: constructing a current moment representation movable margin vector d_res(k)：

γ_d(k-1) is the reference trajectory position at the previous time k-1, and if it is determined in step S1.2 that the human intention is strong, the reference trajectory γ is_dTo the adjusted desired trajectory, otherwise, to the reference trajectory gamma_dObtaining an initial reference track for one-time planning; k represents a time;

step S2.1.3: calculating an actual offset d (k) as a current position configuration x (k-1) of the robot and a corresponding expected value gamma_dDistance of (k-1):

d(k)＝||x(k-1)-γ_d(k-1)||

step S2.1.4: defining a saturation function d_sat:[0,d_res]→(0,d_res)

μ₁,μ₂,θ₂And xi is a parameter of the Richards curve;

d_max(k)＝min{d(k),d_res(k) the parameter value determines the shape of the curve and is a preset value set according to the actual situation;

step S2.1.5: establishing an index lambda (k) representing the safety of the system:

the step S3 includes:

step S3.1: establishing a slave end system state space expression based on a dynamic model of the slave end system:

M_x,C_x,G_xthe method comprises the following steps that an inertia matrix, a Coriolis force centrifugal force matrix and a gravity matrix of the end robot in a Cartesian space are respectively set, and J is a Jacobian matrix from a robot joint space to the Cartesian space; the control input is the joint moment input tau of the slave end robot; wherein the state variable

The system configuration and its derivatives;

step S3.2: respectively calculating error vectors of the robots according to expected instructions of the robots, wherein the error vector of the robot is delta x_r＝x-γ_dThe human error vector is Δ x_h＝x-x_hd(ii) a Wherein, γ_dIs the desired cartesian spatial configuration position of the robot; x is the number of_hdThe configuration position in the expected Cartesian space of the person is obtained by converting force input of the person through the main-end interaction device;

step S3.3: at any time k, a robot cost function C is calculated_r(k)：

Q_1r,Q_2r,Q_3rAre all positive definite matrixes;

step S3.4: at any time k, a human cost function is computed:

Q_1h,Q_2hare all positive definite matrixes;

step S3.5: establishing a k-time hybrid cost function based on an index lambda (k) representing the system safety:

C(k)＝λ(k)C_h(k)+(1-λ(k))C_r(k)

step S3.6: based on the model predictive control framework, the following control problem is formed:

z(k+1|k)＝A(k)z(k)+B(k)τ(k)+C(k)

the control problem is: at any time k, the time domain is optimized to be t ═ k, k +1]Dividing the time-domain-based data into P discrete time periods with equal time step, wherein the goal of rolling optimization is to minimize the cumulative mixed cost function in the time domain; the prediction model is a state space expression established based on a dynamic model of the slave end system, and A (k), B (k) and C (k) are corresponding discrete coefficient matrixes; the system security constraint is h_oAnd h_bRespectively representing the distance between the position of the current robot and the nearest barrier and the nearest boundary, and ensuring that the distance is not less than the corresponding threshold value delta_o,δ_b；

Step S3.7: and solving the optimization problem in each optimization time domain t ═ k, k +1, and calculating the optimal control quantity input at the time k.

The invention provides a local track adjustment and man-machine sharing control system suitable for a robot, which comprises any one or more of the following modules:

module M0, step of determining an initial reference trajectory based on a trajectory plan: generating a feasible trajectory set based on the desired end position and the ambient environment information obtained from the sensor data; screening an optimal track from the current feasible track set as a reference track for one-time planning;

module M1, step of local trajectory re-planning taking into account the person's intention: the method comprises the steps that a human being transmits a motion instruction to a robot through teleoperation equipment, whether human intention is strong or not is judged through virtual interaction force, and when the human intention is strong, the robot adjusts a local reference track;

module M2, step of human-machine control weight adjustment based on system security assessment indicators: evaluating the safety of the current robot configuration through sensor data, constructing an evaluation index representing the safety of the system, and dynamically adjusting the human-computer control weight based on the evaluation index;

module M3, model predictive control step based on human-machine hybrid cost function: constructing a hybrid cost function based on the control cost of the robot and the human; and calculating to obtain an optimal control instruction through a model prediction controller of a hybrid cost function, so as to realize man-machine sharing control.

Preferably, said module M1 comprises:

module M1.1: establishing a virtual force model representing human interaction force;

module M1.2: judging whether the human intention is strong or not through the virtual interaction force; if so, the robot does not adjust the reference track; (ii) a If not, the robot adjusts the local reference track;

the module M2 includes:

step 2.1: sensing environmental information in real time based on data of a sensor, and establishing an index representing the safety of the system; wherein the environment information comprises a distance from a boundary and a distance from an obstacle;

module M2.2: taking the system safety index as a basis for adjusting the man-machine control weight;

the module M3 includes:

module M3.1: establishing a state space expression of the slave end system based on a dynamic model of the slave end system;

module M3.2: respectively calculating error vectors of the robot and the human according to expected instructions of the robot and the human;

module M3.3: at any moment k, calculating a robot cost function;

module M3.4: at any time k, calculating a human cost function;

module M3.5: establishing the k mixing cost function at the moment based on the index representing the system safety;

module M3.6: forming a control problem based on a model prediction control framework: at any time k, the optimized time domain is t ═ k, k +1, the time domain is divided into P discrete time periods with equal time step, and the objective of the rolling optimization is to minimize the cumulative mixed cost function in the time domain; the prediction model is a state space expression established based on a dynamic model of the slave end system; the system safety constraint is that the distance between the position of the current robot and the nearest barrier is equal to the distance between the current robot and the nearest boundary;

module M3.7: and solving the optimization problem in each optimization time domain t ═ k, k +1, and calculating the optimal control quantity input at the time k.

Preferably, in said module M0:

determining a target configuration of a robot in Cartesian space as x_finalInitial position x₀Desired trajectory duration T, with the goal of planning a path from initial configuration to target configuration x_finalThe optimum trajectory is the reference trajectory x_d(t), t is a time variable;

obtaining obstacle position distribution in vivo environment by sensing device

And a motion boundary

(ii) a Generation of feasible trajectory sets using fast-spanning random tree RRT algorithm

(ii) a An optimal track is selected from the feasible track set as a reference track x for one-time planning through the following optimization targets_d(t)：

Wherein, p isPossible tracks are shown, and the optimization index is composed of three parts which are respectively used for representing the shortest path, avoiding obstacles to the maximum degree and avoiding boundaries to the maximum degree, alpha_l,α_o,α_bAll are normal numbers and are used for adjusting the proportion of the three parts.

Preferably, said module M1 comprises:

module M1.1: establishing a virtual force model representing human interaction force:

M_m,D_m,K_mrespectively representing an inertia matrix, a damping matrix and a rigidity matrix;

represents the current position of the end of the robot arm;

is the corresponding expected value;

F_hcharacterizing a virtual interaction force applied by a human to the slave robot through the teleoperational device;

module M1.2: whether the human intention is strong or not is judged through the virtual interaction force, and the judgment system is as follows:

setting a threshold value delta_iI 1.. m, which is the corresponding virtual interaction force F_hThe lower limit of the ith component monitors the virtual interaction force F applied to the slave robot by the human through the teleoperation equipment in real time_hThe value of (d);

1) if it is

F_h≤δ_iThe robot does not adjust the reference trajectory;

2) if it is

F_h＞δ_iThen the robot carries out local reference track adjustment;

in block M1.2, the step of local reference trajectory adjustment comprises:

module M1.2.1: determining a local track range t epsilon [ t ] to be adjusted_s,t_f]T is a time variable, t_s、t_fRespectively representing the starting time and the ending time of the local track;

module M1.2.2: will be the original track x_d(t) discretization into locally discrete trajectories

Wherein the content of the first and second substances,

are respectively

x_d(t) the ith component, δ being the time interval of the selected discrete point;

module M1.2.3: for the distance from the current position to x_d(t_f) Re-planning the local track to generate a local feasible track set

Module M1.2.4: for a local feasible trajectory gamma_dLocal trace energy E (γ)_d) Is gamma_dThe adjusted track energy is:

is the original local trace energy;

α is a normal number;

r is a positive definite symmetric matrix;

from the set of feasible trajectories based on the representation adjusted trajectory energy index

Selecting a feasible track with the minimum energy of the adjusting track as an optimal track gamma_dAs an adjusted reference trajectory;

the module M2 includes:

module M2.1: sensing environmental information in real time based on data of a sensor, and establishing an index representing the safety of the system; wherein the environment information comprises a distance from a boundary and a distance from an obstacle;

module M2.2: taking the system safety index as a basis for adjusting the man-machine control weight;

the module M2.1 comprises the following steps:

module M2.1.1: obstacle position distribution in-vivo environment obtained based on sensing data

And a motion boundary

Subscript i represents a serial number;

module M2.1.2: constructing a current moment representation movable margin vector d_res(k)：

γ_d(k-1) is the reference track position at the previous time k-1, and if the human intention is judged to be strong in the module M1.2, the reference track gamma is_dTo the adjusted desired trajectory, otherwise, to the reference trajectory gamma_dObtaining an initial reference track for the primary planning; k represents a time;

module M2.1.3: calculating an actual offset d (k) as a current position configuration x (k-1) of the robot and a corresponding expected value gamma_dDistance of (k-1):

d(k)＝||x(k-1)-γ_d(k-1)||

module M2.1.4: defining a saturation function d_sat:[0,d_res]→(0,d_res)

μ₁,μ₂,θ₂And xi is a parameter of the Richards curve;

d_max(k)＝min{d(k),d_res(k) the parameter value determines the shape of the curve and is a preset value set according to the actual situation;

module M2.1.5: establishing an index lambda (k) for representing the safety of the system:

the module M3 includes:

module M3.1: establishing a slave end system state space expression based on a dynamic model of the slave end system:

M_x,C_x,G_xthe method comprises the following steps that an inertia matrix, a Coriolis force centrifugal force matrix and a gravity matrix of the end robot in a Cartesian space are respectively set, and J is a Jacobian matrix from a robot joint space to the Cartesian space; the control input is the joint moment input tau of the slave end robot; wherein the state variable

The system configuration and its derivatives;

module M3.2: respectively calculating error vectors of the robots according to expected instructions of the robots, wherein the error vector of the robot is delta x_r＝x-γ_dThe human error vector is Δ x_h＝x-x_hd(ii) a Wherein, γ_dIs the desired cartesian spatial configuration position of the robot; x is the number of_hdThe configuration position in the expected Cartesian space of the person is obtained by converting force input of the person through the main-end interaction device;

module M3.3: at any time k, a robot cost function C is calculated_r(k)：

Q_1r,Q_2r,Q_3rAre all positive definite matrixes;

module M3.4: at any time k, a human cost function is computed:

Q_1h,Q_2hare all positive definite matrixes;

module M3.5: establishing a k-time hybrid cost function based on an index lambda (k) representing the system safety:

C(k)＝λ(k)C_h(k)+(1-λ(k))C_r(k)

module M3.6: based on the model predictive control framework, the following control problems are formed:

z(k+1|k)＝A(k)z(k)+B(k)τ(k)+C(k)

the control problem is: at any time k, the time domain is optimized to t ═ k, k +1]Dividing the time-domain-based data into P discrete time periods with equal time step, wherein the goal of rolling optimization is to minimize the cumulative mixed cost function in the time domain; the prediction model is a state space expression established based on a dynamic model of the slave end system, and A (k), B (k) and C (k) are corresponding discrete coefficient matrixes; the system security constraint is h_oAnd h_bRespectively representing the distance between the position of the current robot and the nearest barrier and the nearest boundary, and ensuring that the distance is not less than the corresponding threshold value delta_o,δ_b；

Module M3.7: and solving the optimization problem in each optimization time domain t ═ k, k +1, and calculating the optimal control quantity input at the time k.

According to the present invention, there is provided a computer readable storage medium storing a computer program, which when executed by a processor, implements the steps of the method for robot local trajectory adjustment and human-machine sharing control.

According to the invention, the robot comprises the local track adjustment and man-machine sharing control system suitable for the robot, or comprises the computer readable storage medium storing the computer program.

Compared with the prior art, the invention has the following beneficial effects:

1. the invention can further improve the autonomy of the surgical robot and change the relation between the robot and the robot from a master-slave mode to a cooperative mode.

2. In the invention, when the motion instruction provided by the person to the slave robot through the teleoperation device is greatly different from the reference track of the robot, the robot locally and actively adjusts the reference track of the robot by combining the virtual interaction force of the person, and simultaneously avoids the following two possible situations: firstly, the robot still masters a larger control right and ignores the intention of the person; secondly, the palm of a person holds a larger weight, continuous active operation is carried out, the workload of an operator is increased, and meanwhile, the safety in the operation cannot be well ensured;

3. the invention comprehensively considers the instructions of both the human and the machine, evaluates the safety degree of the current slave end mechanical arm configuration in real time, constructs the safety of a corresponding index quantification system, and dynamically adjusts the control proportion of the human and the robot: when the safety is higher, the human is given a larger weight, otherwise, the robot has a larger weight, and the human-computer control is fused with a higher automation degree.

Drawings

Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments with reference to the following drawings:

fig. 1 is a schematic diagram of the principle of the present invention.

Fig. 2 is an overall block diagram of the present invention.

Fig. 3 is a Richards curve.

FIG. 4 shows the system safety indexes λ and d_satThe mapping relationship of (2).

FIG. 5 is a block diagram of a shared control module.

Detailed Description

The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the invention, but are not intended to limit the invention in any way. It should be noted that it would be obvious to those skilled in the art that various changes and modifications can be made without departing from the spirit of the invention. All falling within the scope of the present invention.

The invention provides a local track adjustment and man-machine sharing control method suitable for a robot, which comprises the following steps:

step S0: determining an initial reference trajectory based on the one-time trajectory plan;

step S1: a step of local trajectory re-planning taking into account the intention of the person;

step S2: adjusting the human-computer control weight based on the system safety evaluation index;

step S3: a step of model predictive control based on a man-machine hybrid cost function;

the invention can further improve the autonomy of the surgical robot and change the relation between the robot and the robot from a master-slave mode to a cooperative mode. When the difference between the instruction of the person and the reference track of the robot is large, the robot can locally and actively adjust the reference track of the robot by combining the virtual interaction force of the person; when the difference between the human and the robot is small, instructions of both the human and the robot are comprehensively considered, a human-computer mixed cost function is dynamically adjusted based on the system safety evaluation index, the optimal control quantity is calculated, and human-computer sharing control is realized.

In this embodiment, a schematic diagram and an overall block diagram of the method are respectively shown in fig. 1 and fig. 2, and the method includes the following steps:

a step of determining an initial reference trajectory based on a trajectory plan, denoted as step S0, specifically: determining the target configuration of the robot in Cartesian space as x according to the requirements of the surgical task_finalInitial position x₀If the desired trajectory duration T is desired, then the goal of this step is to plan a path from the initial configuration to the target configuration x_finalThe optimum trajectory is the reference trajectory x_dAnd (t), wherein t is a time variable and has a unit of seconds. Firstly, the position distribution of obstacles (organs, tissues, blood clots and the like) in the internal environment is obtained through sensing equipment such as an external CT, an ultrasonic probe, an endoscope and the like

And the boundary of motion (trachea, blood vessel, etc.)

Generating a set of feasible trajectories using a fast-expanding random tree (RRT) algorithm

(ii) a Optimizing the objective byScreening out an optimal track from the feasible track set as a reference track x for one-time planning_d(t)：

Wherein p represents a feasible track, and the optimization index is composed of three parts respectively used for representing the shortest path, avoiding obstacles to the maximum degree, avoiding boundaries to the maximum degree, and alpha_l,α_o,α_bAll are normal numbers and are used for adjusting the proportion of the three parts.

The step of local trajectory re-planning, which takes human intent into consideration, is denoted as step S1, specifically: the method comprises the steps that a human transmits virtual interaction force to a slave-end robot by operating a master-end teleoperation device, whether the operation intention of the current human is strong or not is judged by a virtual interaction force system, when the virtual interaction force is larger than a certain threshold value, the human control intention is strong, and the robot locally adjusts an original reference track;

a step of adjusting the human-computer control weight based on the system security evaluation index, which is denoted as step S2, specifically: evaluating whether the current robot configuration is safe in the environment through sensor data, constructing a system safety index based on the concept of movable allowance, quantifying the safety degree of the current system, and dynamically adjusting the human-computer control weight based on the index;

the model predictive control step based on the human-computer hybrid cost function is denoted as step S3, and specifically: constructing a discrete state space expression based on a slave end system dynamic model; respectively calculating the cost of the robot and the human according to the expectation of the robot and the human, constructing a mixed cost function by combining with the system safety index, further forming a model prediction control problem, and realizing human-computer sharing control by iteratively solving an optimal control instruction.

Step S0 is to obtain an initial reference trajectory of the robot through a planning method, and the initial reference trajectory may also be directly set manually according to actual task requirements.

The step S1 includes:

step S1.1: establishing a virtual force model representing human interaction force:

wherein the content of the first and second substances,

representing the current position of the end of the robot arm,

to the corresponding desired value. F_hA virtual interaction force applied by a human to the slave robot through the teleoperational device is characterized. M is a group of_m,D_m,K_mRespectively representing an inertia matrix, a damping matrix and a rigidity matrix.

Step S1.2: judging whether the human intention is strong through the virtual interaction force, wherein the judgment method comprises the following steps:

setting a threshold value delta_iI 1.. m, which is the corresponding virtual interaction force F_hThe lower limit of the ith component monitors the virtual interaction force F applied to the slave robot by the human through the teleoperation equipment in real time_hThe value of (d);

1) if it is

F_h≤δ_iIf so, the deviation between the current expected command of the person and the expected motion of the robot is small, and the robot does not adjust the reference track;

2) if it is

F_h＞δ_iIf the difference between the current expected command of the robot and the expected motion of the robot is large, the robot adjusts the local reference track.

In step S1.2, the step of adjusting the local reference trajectory includes:

step S1.2.1: determining a local track range t epsilon [ t ] to be adjusted_s,t_f]T is timeThe time variable, in seconds, t_s、t_fRespectively representing the starting time and the ending time of the local track;

step S1.2.2: will be the original track x_d(t) discretization into locally discrete trajectories

Wherein the content of the first and second substances,

x_dii is 1, …, m is

x_d(t) the ith component, δ being the time interval of the selected discrete point;

step S1.2.3: for the distance from the current position to x_d(t_f) The local track is re-planned, and a local feasible track set is searched and generated through an RRT algorithm

Step S1.2.4: for a local feasible trajectory gamma_dLocal trace energy E (γ)_d) Is gamma_dThe adjusted track energy is:

wherein the content of the first and second substances,

in order to obtain the original local energy of the track,

is a column vector of all 1, f_hi、γ_diAre respectively F_h、γ_dα is a positive constant, and R is a positive definite symmetric matrix. It can be observed that the adjusted trajectory energy consists of three parts, namely a first original local trajectory energy, a second adjusted trajectory energy which is a work done by a human being, and a third adjusted trajectory energy which is a square norm of the matrix R;

from the set of feasible trajectories based on the representation adjusted trajectory energy index

Selecting an optimal track gamma_dAs adjusted reference trajectories, the screening criteria were as follows:

it can be seen that the screening criterion is actually to select a feasible trajectory with the minimum energy of the adjustment trajectory, since the optimization objective contains invariant

And removing all terms contained in the invariant quantity is the screening standard. The selection of the matrix R can be determined by actual conditions, and here, we provide a selection mode based on a Minimum-jerk model:

because the Minimum-jerk model can more accurately express the motion trail of the human body, the selection mode ensures that the adjusted trail is more in line with the motion habit of the human body.

The step S2 includes:

step S2.1: sensing environmental information in real time based on data of a sensor, and establishing an index representing the safety of a system; wherein the environment information comprises a distance from a boundary and a distance from an obstacle;

s2.2, taking the system safety index as a basis for adjusting the man-machine control weight;

the step S2.1 comprises the steps of:

step S2.1.1: location distribution of obstacles (organs, tissues, blood clots, etc.) in an in vivo environment based on sensed data

And the boundary of motion (trachea, blood vessel, etc.)

；

Step S2.1.2: constructing a current moment representation movable margin vector d_res(k)：

Wherein, gamma is_d(k-1) is the reference trajectory position at the previous time, and if it is determined in step S1.2 that the human intention is strong, the reference trajectory γ is_dTo the adjusted desired trajectory, otherwise, to the reference trajectory gamma_dObtaining an initial reference track for one-time planning; k represents a time;

step S2.1.3: calculating the actual offset as the distance between the current position configuration of the robot and the corresponding expected value:

d(k)＝||x(k-1)-γ_d(k-1)||

step S2.1.4: a saturation function d is defined using the Richards curve as shown in FIG. 3_sat:[0,d_res]→(0,d_res)

Wherein d is_max(k)＝min{d(k),d_res(k)}，μ₁,μ₂,θ₂Xi is the parameter of the Richards curve, and the parameter value determines the shape of the curve according toActual situation set predetermined value. Theta₂The rising rate of the saturation function is determined for positive real numbers, xi is positive real numbers, the degree of curvature of the curve is determined, mu₁Is a normal number close to 1, determines the position of the upper asymptote of the curve, mu₂Is a normal number, the length of the curve front end lag;

step S2.1.5: establishing an index lambda (k) representing the safety of the system:

the index and d_satThe mapping relationship of (2) is shown in fig. 4.

The step S3 is that the structure of the human-machine sharing control module is shown in fig. 5, and includes:

step S3.1: establishing a slave end system state space expression based on a dynamic model of the slave end system:

wherein the state variable

The system configuration and its derivatives; the control input is the joint moment input tau of the slave end robot; m is a group of_x,C_x,G_xThe method comprises the following steps that an inertia matrix, a Coriolis force centrifugal force matrix and a gravity matrix of the end robot in a Cartesian space are respectively set, and J is a Jacobian matrix from a robot joint space to the Cartesian space;

step S3.2, error vectors of the robot and the human are respectively calculated according to expected instructions of the robot and the human, wherein the error vector of the robot is delta x_r＝x-γ_dThe human error vector is Δ x_h＝x-x_hd(ii) a Wherein，γ_dThe configuration position in the expected Cartesian space of the robot can be obtained by the paths of track planning and the like; x is the number of_hdIs the expected configuration position in the Cartesian space of the human, and is obtained by the force input conversion of the human through the main-end interaction device.

Step S3.3: at any time k, a robot cost function is calculated:

the above equation is composed of two parts of a quadratic form (first two terms) of the error vector of the robot and its derivative and a quadratic form (third term) of the robot control input, where Q_1r,Q_2r,Q_3rAre all positive definite matrixes;

step S3.4: at any time k, a human cost function is computed:

the above equation consists of a quadratic form of the human error vector and its derivatives, where Q_1h,Q_2hAre all positive definite matrixes;

step S3.5: establishing a k-time hybrid cost function based on an index lambda (k) representing the system safety:

C(k)＝λ(k)C_h(k)+(1-λ(k))C_r(k)

step S3.6: based on the model predictive control framework, the following control problem is formed:

z(k+1|k)＝A(k)z(k)+B(k)τ(k)+C(k)

the control problem is: at any time k, the time domain is optimized to t ═ k, k +1]Dividing the time-domain-based data into P discrete time periods with equal time step, wherein the goal of rolling optimization is to minimize the cumulative mixed cost function in the time domain; the prediction model is a state space expression established based on a dynamic model of the slave end system, and A (k), B (k) and C (k) are corresponding discrete coefficient matrixes; the system security constraint is h_oAnd h_bRespectively representing the distance between the position of the current robot and the nearest barrier and the nearest boundary, and ensuring that the distance is not less than the corresponding threshold value delta_o,δ_b；

And step 3.7, solving the optimization problem in each optimization time domain t ═ k, k +1, and calculating the optimal control quantity input at the time k. There are many alternative methods for solving the Quadratic Programming (QP) optimization problem, including active set, interior point method, first-order optimization method, etc., and there are also many open QP solution libraries that can be directly called at present.

The invention further provides a local trajectory adjusting and man-machine sharing control system suitable for the robot, and a person skilled in the art can realize the local trajectory adjusting and man-machine sharing control system suitable for the robot by executing the step flow of the local trajectory adjusting and man-machine sharing control method suitable for the robot, namely, the method can be understood as a preferred embodiment of the local trajectory adjusting and man-machine sharing control system suitable for the robot.

The invention provides a local track adjustment and man-machine sharing control system suitable for a robot, which comprises any one or more of the following modules:

module M0, step of determining an initial reference trajectory based on a trajectory plan: generating a feasible trajectory set based on the desired end position and the ambient environment information obtained from the sensor data; screening an optimal track from the current feasible track set as a reference track for one-time planning;

module M1, step of local trajectory re-planning taking into account the person's intention: the method comprises the steps that a human being transmits a motion instruction to a robot through teleoperation equipment, whether human intention is strong or not is judged through virtual interaction force, and when the human intention is strong, the robot adjusts a local reference track;

module M2, step of human-machine control weight adjustment based on system security assessment indicators: evaluating the safety of the current robot configuration through sensor data, constructing an evaluation index representing the safety of the system, and dynamically adjusting the human-computer control weight based on the evaluation index;

module M3, model predictive control step based on human-machine hybrid cost function: constructing a hybrid cost function based on the control cost of the robot and the human; and calculating to obtain an optimal control instruction through a model prediction controller of a hybrid cost function, so as to realize man-machine sharing control.

Preferably, said module M1 comprises:

module M1.1: establishing a virtual force model representing human interaction force;

module M1.2: judging whether the human intention is strong or not through the virtual interaction force; if so, the robot does not adjust the reference track; (ii) a If not, the robot adjusts the local reference track;

the module M2 includes:

step 2.1: sensing environmental information in real time based on data of a sensor, and establishing an index representing the safety of the system; wherein the environment information comprises a distance from a boundary and a distance from an obstacle;

module M2.2: taking the system safety index as a basis for adjusting the man-machine control weight;

the module M3 includes:

module M3.1: establishing a state space expression of the slave end system based on a dynamic model of the slave end system;

module M3.2: respectively calculating error vectors of the robot and the human according to expected instructions of the robot and the human;

module M3.3: at any moment k, calculating a robot cost function;

module M3.4: at any time k, calculating a human cost function;

module M3.5: establishing the k mixing cost function at the moment based on the index representing the system safety;

module M3.6: forming a control problem based on a model prediction control framework: at any time k, the optimized time domain is t ═ k, k +1, the time domain is divided into P discrete time periods with equal time step, and the objective of the rolling optimization is to minimize the cumulative mixed cost function in the time domain; the prediction model is a state space expression established based on a dynamic model of the slave end system; the system safety constraint is that the distance between the position of the current robot and the nearest barrier is equal to the distance between the current robot and the nearest boundary;

module M3.7: and solving the optimization problem in each optimization time domain t ═ k, k +1, and calculating the optimal control quantity input at the time k.

Preferably, in said module M0:

determining a target configuration of a robot in Cartesian space as x_finalInitial position x₀Desired trajectory duration T, with the goal of planning a path from initial configuration to target configuration x_finalThe optimum trajectory is the reference trajectory x_d(t), t is a time variable;

obtaining obstacle position distribution in vivo environment by sensing device

And a motion boundary

(ii) a Generation of feasible trajectory sets using fast-spanning random tree RRT algorithm

(ii) a An optimal track is selected from the feasible track set as a reference track x for one-time planning through the following optimization targets_d(t)：

Wherein, p isPossible tracks are shown, and the optimization index is composed of three parts which are respectively used for representing the shortest path, avoiding obstacles to the maximum degree and avoiding boundaries to the maximum degree, alpha_l,α_o,α_bAll are normal numbers and are used for adjusting the proportion of the three parts.

Preferably, said module M1 comprises:

module M1.1: establishing a virtual force model representing human interaction force:

M_m,D_m,K_mrespectively representing an inertia matrix, a damping matrix and a rigidity matrix;

represents the current position of the end of the robot arm;

is the corresponding expected value;

F_hcharacterizing a virtual interaction force applied by a human to the slave robot through the teleoperational device;

module M1.2: whether the human intention is strong or not is judged through the virtual interaction force, and the judgment system is as follows:

setting a threshold value delta_iI 1.. m, which is the corresponding virtual interaction force F_hThe lower limit of the ith component is used for monitoring the virtual interaction force F applied to the slave robot by the human through the teleoperation equipment in real time_hThe value of (d);

1) if it is

F_h≤δ_iThe robot does not adjust the reference trajectory;

2) if it is

F_h＞δ_iThen the robot carries out local reference track adjustment;

in module M1.2, the step of adjusting the local reference trajectory comprises:

module M1.2.1: determining a local track range t epsilon [ t ] to be adjusted_s,t_f]T is a time variable, t_s、t_fRespectively representing the starting time and the ending time of the local track;

module M1.2.2: will be the original track x_d(t) discretization into locally discrete trajectories

Wherein the content of the first and second substances,

x_dii is 1, …, m is

x_d(t) the ith component, δ being the time interval of the selected discrete point;

module M1.2.3: for the distance from the current position to x_d(t_f) Re-planning the local track to generate a local feasible track set

Module M1.2.4: for a local feasible trajectory gamma_dLocal trace energy E (γ)_d) Is gamma_dThe adjusted track energy is:

is the original local trace energy;

α is a normal number;

r is a positive definite symmetric matrix;

from the set of feasible trajectories based on the representation adjusted trajectory energy index

Selecting a feasible track with the minimum energy of the adjusting track as an optimal track gamma_dAs an adjusted reference trajectory;

the module M2 includes:

module M2.1: sensing environmental information in real time based on data of a sensor, and establishing an index representing the safety of the system; wherein the environment information comprises a distance from a boundary and a distance from an obstacle;

module M2.2: taking the system safety index as a basis for adjusting the man-machine control weight;

the module M2.1 comprises the following steps:

module M2.1.1: obstacle position distribution in-vivo environment obtained based on sensing data

And a motion boundary

Subscript i represents a serial number;

module M2.1.2: constructing a representative movable margin vector d at the current moment_res(k)：

γ_d(k-1) is the reference track position at the previous time k-1, and if the human intention is judged to be strong in the module M1.2, the reference track gamma is_dTo the adjusted desired trajectory, otherwise, the reference trajectory gamma_dObtaining an initial reference track for one-time planning; k represents a time;

module M2.1.3: calculating an actual offset d (k) as a current position configuration x (k-1) of the robot and a corresponding expected value gamma_dDistance of (k-1):

d(k)＝||x(k-1)-γ_d(k-1)||

module M2.1.4: defining a saturation function d_sat:[0,d_res]→(0,d_res)

μ₁,μ₂,θ₂And xi is a parameter of the Richards curve;

d_max(k)＝min{d(k),d_res(k) the parameter value determines the shape of the curve and is a preset value set according to the actual situation;

module M2.1.5: establishing an index lambda (k) representing the safety of the system:

the module M3 includes:

module M3.1: establishing a slave end system state space expression based on a dynamic model of the slave end system:

M_x,C_x,G_xthe method comprises the following steps that an inertia matrix, a Coriolis force centrifugal force matrix and a gravity matrix of the end robot in a Cartesian space are respectively set, and J is a Jacobian matrix from a robot joint space to the Cartesian space; the control input is the joint moment input tau of the slave end robot; wherein the state variable

The system configuration and its derivatives;

module M3.2: respectively calculating error vectors of the robots according to expected instructions of the robots, wherein the error vector of the robot is delta x_r＝x-γ_dThe human error vector is Δ x_h＝x-x_hd(ii) a Wherein, γ_dIs the desired cartesian spatial configuration position of the robot; x is the number of_hdThe configuration position in the expected Cartesian space of the person is obtained by converting the force input of the person through the main-end interaction device;

module M3.3: at any time k, a robot cost function C is calculated_r(k)：

Q_1r,Q_2r,Q_3rAre all positive definite matrixes;

module M3.4: at any time k, a human cost function is computed:

Q_1h,Q_2hare all positive definite matrixes;

module M3.5: establishing a k-time hybrid cost function based on an index lambda (k) representing the system safety:

C(k)＝λ(k)C_h(k)+(1-λ(k))C_r(k)

module M3.6: based on the model predictive control framework, the following control problem is formed:

z(k+1|k)＝A(k)z(k)+B(k)τ(k)+C(k)

the control problem is: at any time k, the time domain is optimized to t ═ k, k +1]Dividing the time-domain-based data into P discrete time periods with equal time step, wherein the goal of rolling optimization is to minimize the cumulative mixed cost function in the time domain; the prediction model is a state space expression established based on a dynamic model of the slave end system, and A (k), B (k) and C (k) are corresponding discrete coefficient matrixes; the system security constraint is h_oAnd h_bRespectively representing the distance between the position of the current robot and the nearest barrier and the nearest boundary, and ensuring that the distance is not less than the corresponding threshold value delta_o,δ_b；

Module M3.7: and solving the optimization problem in each optimization time domain t ═ k, k +1, and calculating the optimal control quantity input at the time k.

Those skilled in the art will appreciate that, in addition to implementing the systems, apparatus, and various modules thereof provided by the present invention in purely computer readable program code, the same procedures can be implemented entirely by logically programming method steps such that the systems, apparatus, and various modules thereof are provided in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like. Therefore, the system, the device and the modules thereof provided by the present invention can be considered as a hardware component, and the modules included in the system, the device and the modules thereof for implementing various programs can also be considered as structures in the hardware component; modules for performing various functions may also be considered to be both software programs for performing the methods and structures within hardware components.

According to the present invention, there is provided a computer readable storage medium storing a computer program, which when executed by a processor, implements the steps of the method for robot local trajectory adjustment and human-machine sharing control.

According to the invention, the robot comprises the local track adjustment and man-machine sharing control system suitable for the robot, or comprises the computer readable storage medium storing the computer program.

The foregoing description of specific embodiments of the present invention has been presented. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes or modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention. The embodiments and features of the embodiments of the present application may be combined with each other arbitrarily without conflict.

Claims (10)

1. A local track adjustment and man-machine sharing control method suitable for a robot is characterized by comprising any one or more of the following steps:

step S0, determining an initial reference trajectory based on the one-time trajectory plan: generating a feasible trajectory set based on the desired end position and the ambient environment information obtained from the sensor data; screening an optimal track from the current feasible track set as a reference track for one-time planning;

step S1, a step of local trajectory re-planning taking into account human intent: the method comprises the steps that a human being transmits a motion instruction to a robot through teleoperation equipment, whether human intention is strong or not is judged through virtual interaction force, and when the human intention is strong, the robot adjusts a local reference track;

step S2, the step of adjusting the human-computer control weight based on the system safety evaluation index: evaluating the safety of the current robot configuration through sensor data, constructing an evaluation index representing the safety of the system, and dynamically adjusting the human-computer control weight based on the evaluation index;

step S3, model prediction control based on man-machine mixed cost function: constructing a hybrid cost function based on the control cost of the robot and the human; and calculating to obtain an optimal control instruction through a model prediction controller of a hybrid cost function, so as to realize man-machine sharing control.

2. The method for robot local trajectory adjustment and human-machine sharing control according to claim 1,

the step S1 includes:

step S1.1: establishing a virtual force model representing human interaction force;

step S1.2: judging whether the human intention is strong or not through the virtual interaction force; if so, the robot does not adjust the reference track; (ii) a If not, the robot adjusts the local reference track;

the step S2 includes:

step 2.1: sensing environmental information in real time based on data of a sensor, and establishing an index representing the safety of the system; wherein the environment information comprises a distance from a boundary and a distance from an obstacle;

step S2.2: taking the system safety index as a basis for adjusting the man-machine control weight;

the step S3 includes:

step S3.1: establishing a state space expression of the slave end system based on a dynamic model of the slave end system;

step S3.2: respectively calculating error vectors of the robot and the human according to expected instructions of the robot and the human;

step S3.3: at any moment k, calculating a robot cost function;

step S3.4: at any time k, calculating a human cost function;

step S3.5: establishing the k mixing cost function at the moment based on the index representing the system safety;

step S3.6: forming a control problem based on a model prediction control framework: at any time k, the optimized time domain is t ═ k, k +1, the time domain is divided into P discrete time periods with equal time step, and the objective of the rolling optimization is to minimize the cumulative mixed cost function in the time domain; the prediction model is a state space expression established based on a dynamic model of the slave end system; the system safety constraint is that the distance between the position of the current robot and the nearest barrier is equal to the distance between the current robot and the nearest boundary;

step S3.7: and solving the optimization problem in each optimization time domain t ═ k, k +1, and calculating the optimal control quantity input at the time k.

3. The method for robot local trajectory adjustment and human-machine sharing control according to claim 2, wherein in step S0:

determining the target configuration of the robot in Cartesian space as x_finalInitial position x₀Desired trajectory duration T, with the goal of planning a path from initial configuration to target configuration x_finalThe optimum trajectory is the reference trajectory x_d(t), t is a time variable;

obtaining obstacle position distribution in vivo environment by sensing device

And a motion boundary

Generation of feasible trajectory sets using fast-spanning random tree RRT algorithm

An optimal track is selected from the feasible track set as a reference track x for one-time planning through the following optimization targets_d(t)：

Wherein p represents a feasible track, and the optimization index comprises three parts, which are respectively used for representing the shortest path, avoiding obstacles to the maximum degree, avoiding boundaries to the maximum degree, and alpha_l,α_o,α_bAll are normal numbers and are used for adjusting the proportion of the three parts.

4. The method for robot local trajectory adjustment and human-machine sharing control according to claim 2,

the step S1 includes:

step S1.1: establishing a virtual force model representing human interaction force:

M_m,D_m,K_mrespectively representing an inertia matrix, a damping matrix and a rigidity matrix;

represents the current position of the end of the robot arm;

is the corresponding expected value;

F_hcharacterizing a virtual interaction force applied by a human to the slave robot through the teleoperational device;

step S1.2: judging whether the human intention is strong through the virtual interaction force, wherein the judgment method comprises the following steps:

setting a threshold value delta_iI 1.. m, which is the corresponding virtual interaction force F_hThe lower limit of the ith component monitors the virtual interaction force F applied to the slave robot by the human through the teleoperation equipment in real time_hThe value of (d);

1) if it is

F_h≤δ_iThe robot does not adjust the reference trajectory;

2) if it is

F_h＞δ_iThen the robot carries out local reference track adjustment;

in step S1.2, the step of adjusting the local reference trajectory includes:

step S1.2.1:determining a local track range t epsilon [ t ] to be adjusted_s,t_f]T is a time variable, t_s、t_fRespectively representing the starting time and the ending time of the local track;

step S1.2.2: will be the original track x_d(t) discretization into locally discrete trajectories

Wherein the content of the first and second substances,

are respectively

x_d(t) the ith component, δ being the time interval of the selected discrete point;

step S1.2.3: for the distance from the current position to x_d(t_f) Re-planning the local track to generate a local feasible track set

Step S1.2.4: for a local feasible trajectory gamma_dLocal trace energy E (γ)_d) Is gamma_dThe adjusted track energy is:

is the original local trace energy;

α is a normal number;

r is a positive definite symmetric matrix;

from the set of feasible trajectories based on the representation adjusted trajectory energy index

Selecting a feasible track with the minimum energy of the adjusting track as an optimal track gamma_dAs an adjusted reference trajectory;

the step S2 includes:

step S2.1: sensing environmental information in real time based on data of a sensor, and establishing an index representing the safety of the system; wherein the environment information comprises a distance from a boundary and a distance from an obstacle;

step S2.2: taking the system safety index as a basis for adjusting the man-machine control weight;

the step S2.1 comprises the steps of:

step S2.1.1: obstacle position distribution in-vivo environment obtained based on sensing data

And a motion boundary

Subscript i represents a serial number;

step S2.1.2: constructing a current moment representation movable margin vector d_res(k)：

γ_d(k-1) is the reference trajectory position at the previous time k-1, and if it is determined in step S1.2 that the human intention is strong, the reference trajectory γ is_dTo the adjusted desired trajectory, otherwise, to the reference trajectory gamma_dObtaining an initial reference track for one-time planning; k represents a time;

step S2.1.3: calculating an actual offset d (k) as a current position configuration x (k-1) of the robot and a corresponding expected value gamma_dDistance of (k-1):

d(k)＝||x(k-1)-γ_d(k-1)||

step S2.1.4: defining a saturation function d_sat:[0,d_res]→(0,d_res)

μ₁,μ₂,θ₂And xi is a parameter of the Richards curve;

d_max(k)＝min{d(k),d_res(k) the parameter value determines the shape of the curve and is a preset value set according to the actual situation;

step S2.1.5: establishing an index lambda (k) representing the safety of the system:

the step S3 includes:

step S3.1: establishing a slave end system state space expression based on a dynamic model of the slave end system:

M_x,C_x,G_xthe method comprises the following steps that an inertia matrix, a Coriolis force centrifugal force matrix and a gravity matrix of the end robot in a Cartesian space are respectively set, and J is a Jacobian matrix from a robot joint space to the Cartesian space; the control input is the joint moment input tau of the slave end robot; wherein the state variable

The system configuration and its derivatives;

step S3.2: respectively calculating error vectors of the robots according to expected instructions of the robots, wherein the error vector of the robot is delta x_r＝x-γ_dThe human error vector is Δ x_h＝x-x_hd(ii) a Wherein, γ_dIs the desired cartesian spatial configuration position of the robot; x is the number of_hdThe configuration position in the expected Cartesian space of the person is obtained by converting force input of the person through the main-end interaction device;

step S3.3: at any time k, a robot cost function C is calculated_r(k)：

Q_1r,Q_2r,Q_3rAre all positive definite matrixes;

step S3.4: at any time k, a human cost function is computed:

Q_1h,Q_2hare all positive definite matrixes;

step S3.5: establishing a k-time hybrid cost function based on an index lambda (k) representing the system safety:

C(k)＝λ(k)C_h(k)+(1-λ(k))C_r(k)

step S3.6: based on the model predictive control framework, the following control problem is formed:

z(k+1|k)＝A(k)z(k)+B(k)τ(k)+C(k)

s.t.

the control problem is: at any time k, the time domain is optimized to t ═ k, k +1]Dividing the time-domain-based data into P discrete time periods with equal time step, wherein the goal of rolling optimization is to minimize the cumulative mixed cost function in the time domain; the prediction model is a state space expression established based on a dynamic model of the slave end system, and A (k), B (k) and C (k) are corresponding discrete coefficient matrixes; the system security constraint is h_oAnd h_bRespectively representing the distance between the position of the current robot and the nearest barrier and the nearest boundary, and ensuring that the distance is not less than the corresponding threshold value delta_o,δ_b；

Step S3.7: and solving the optimization problem in each optimization time domain t ═ k, k +1, and calculating the optimal control quantity input at the time k.

5. A local track adjustment and man-machine sharing control system suitable for a robot is characterized by comprising any one or more of the following modules:

module M0, step of determining an initial reference trajectory based on a trajectory plan: generating a feasible trajectory set based on the desired end position and the ambient environment information obtained from the sensor data; screening an optimal track from the current feasible track set as a reference track for one-time planning;

module M1, step of local trajectory re-planning taking into account the person's intention: the method comprises the steps that a human being transmits a motion instruction to a robot through teleoperation equipment, whether human intention is strong or not is judged through virtual interaction force, and when the human intention is strong, the robot adjusts a local reference track;

module M2, step of human-machine control weight adjustment based on system security assessment indicators: evaluating the safety of the current robot configuration through sensor data, constructing an evaluation index representing the safety of the system, and dynamically adjusting the human-computer control weight based on the evaluation index;

module M3, model predictive control step based on human-machine hybrid cost function: constructing a hybrid cost function based on the control cost of the robot and the human; and calculating to obtain an optimal control instruction through a model prediction controller of a hybrid cost function, so as to realize man-machine sharing control.

6. The system for robot local trajectory adjustment and human-machine sharing control according to claim 1,

the module M1 includes:

module M1.1: establishing a virtual force model representing human interaction force;

module M1.2: judging whether the human intention is strong or not through the virtual interaction force; if so, the robot does not adjust the reference track; (ii) a If not, the robot adjusts the local reference track;

the module M2 includes:

step 2.1: sensing environmental information in real time based on data of a sensor, and establishing an index representing the safety of the system; wherein the environment information comprises a distance from a boundary and a distance from an obstacle;

module M2.2: taking the system safety index as a basis for adjusting the man-machine control weight;

the module M3 includes:

module M3.1: establishing a state space expression of the slave end system based on a dynamic model of the slave end system;

module M3.2: respectively calculating error vectors of the robot and the human according to expected instructions of the robot and the human;

module M3.3: at any moment k, calculating a robot cost function;

module M3.4: at any time k, calculating a human cost function;

module M3.5: establishing the k mixing cost function at the moment based on the index representing the system safety;

module M3.6: forming a control problem based on a model prediction control framework: at any time k, the optimized time domain is t ═ k, k +1, the time domain is divided into P discrete time periods with equal time step, and the objective of the rolling optimization is to minimize the cumulative mixed cost function in the time domain; the prediction model is a state space expression established based on a dynamic model of the slave end system; the system safety constraint is the distance between the position of the current robot and the nearest barrier and the nearest boundary;

module M3.7: and solving the optimization problem in each optimization time domain t ═ k, k +1, and calculating the optimal control quantity input at the time k.

7. The system for robot local trajectory adjustment and human-machine sharing control according to claim 6, wherein in said module M0:

determining a target configuration of a robot in Cartesian space as x_finalInitial position x₀Desired trajectory duration T, with the goal of planning a path from initial configuration to target configuration x_finalThe optimum trajectory is the reference trajectory x_d(t), t is a time variable;

obtaining obstacle position distribution in vivo environment by sensing device

And a motion boundary

Generation of feasible trajectory sets using fast-expanding random tree RRT algorithm

An optimal track is selected from the feasible track set as a reference track x for one-time planning through the following optimization targets_d(t)：

Wherein p represents a feasible track, the optimization index consists of three parts which are respectively used for representing the shortest path, avoiding barriers to the maximum extent and avoiding boundaries to the maximum extent,α_l,α_o,α_ball are normal numbers and are used for adjusting the proportion of the three parts.

8. The system for robot local trajectory adjustment and human-machine sharing control according to claim 6,

the module M1 includes:

module M1.1: establishing a virtual force model representing human interaction force:

M_m,D_m,K_mrespectively representing an inertia matrix, a damping matrix and a rigidity matrix;

represents the current position of the end of the robot arm;

is the corresponding expected value;

F_hcharacterizing a virtual interaction force applied by a human to the slave robot through the teleoperational device;

module M1.2: whether the human intention is strong or not is judged through the virtual interaction force, and the judgment system is as follows:

setting a threshold value delta_iI 1.. m, which is the corresponding virtual interaction force F_hThe lower limit of the ith component monitors the virtual interaction force F applied to the slave robot by the human through the teleoperation equipment in real time_hThe value of (d);

1) if it is

F_h≤δ_iThe robot does not adjust the reference trajectory;

2) if it is

F_h＞δ_iThen the robot carries out local reference track adjustment;

in block M1.2, the step of local reference trajectory adjustment comprises:

module M1.2.1: determining a local track range t epsilon [ t ] to be adjusted_s,t_f]T is a time variable, t_s、t_fRespectively representing the starting time and the ending time of the local track;

module M1.2.2: will be the original track x_d(t) discretization into locally discrete trajectories

Wherein the content of the first and second substances,

are respectively

x_d(t) the ith component, δ being the time interval of the selected discrete point;

module M1.2.3: for the distance from the current position to x_d(t_f) Re-planning the local track to generate a local feasible track set

Module M1.2.4: for a local feasible trajectory gamma_dLocal trace energy E (γ)_d) Is gamma_dThe adjusted track energy is:

is the original local trace energy;

α is a normal number;

r is a positive definite symmetric matrix;

from the set of feasible trajectories based on the representation adjusted trajectory energy index

Selecting a feasible track with the minimum energy of the adjusting track as an optimal track gamma_dAs an adjusted reference trajectory;

the module M2 includes:

module M2.1: sensing environmental information in real time based on data of a sensor, and establishing an index representing the safety of the system; wherein the environment information comprises a distance from a boundary and a distance from an obstacle;

module M2.2: taking the system safety index as a basis for adjusting the man-machine control weight;

the module M2.1 comprises the following steps:

module M2.1.1: obstacle position distribution in-vivo environment obtained based on sensing data

And a motion boundary

Subscript i represents a serial number;

module M2.1.2: constructing a current moment representation movable margin vector d_res(k)：

γ_d(k-1) is the reference track position at the previous time k-1 ifIf the module M1.2 judges that the human intention is stronger, the reference track gamma is determined_dTo the adjusted desired trajectory, otherwise, to the reference trajectory gamma_dObtaining an initial reference track for one-time planning; k represents a time;

module M2.1.3: calculating an actual offset d (k) as a current position configuration x (k-1) of the robot and a corresponding expected value gamma_dDistance of (k-1):

d(k)＝||x(k-1)-γ_d(k-1)||

module M2.1.4: defining a saturation function d_sat:[0,d_res]→(0,d_res)

μ₁,μ₂,θ₂And xi is a parameter of the Richards curve;

d_max(k)＝min{d(k),d_res(k) the parameter value determines the shape of the curve and is a preset value set according to the actual situation;

module M2.1.5: establishing an index lambda (k) representing the safety of the system:

the module M3 includes:

module M3.1: establishing a slave end system state space expression based on a dynamic model of the slave end system:

M_x,C_x,G_xrespectively, the slave robot is in Cartesian spaceAn inertia matrix, a Coriolis force centrifugal force matrix and a gravity matrix among the three matrixes, wherein J is a Jacobian matrix from a robot joint space to a Cartesian space; the control input is the joint moment input tau of the slave end robot; wherein the state variable

The system configuration and its derivatives;

module M3.2: respectively calculating error vectors of the robots according to expected instructions of the robots, wherein the error vector of the robot is delta x_r＝x-γ_dThe human error vector is Δ x_h＝x-x_hd(ii) a Wherein, gamma is_dIs the desired cartesian spatial configuration position of the robot; x is the number of_hdThe configuration position in the expected Cartesian space of the person is obtained by converting force input of the person through the main-end interaction device;

module M3.3: at any time k, a robot cost function C is calculated_r(k)：

Q_1r,Q_2r,Q_3rAre all positive definite matrixes;

module M3.4: at any time k, a human cost function is computed:

Q_1h,Q_2hare all positive definite matrixes;

module M3.5: establishing a k-time hybrid cost function based on an index lambda (k) representing the system safety:

C(k)＝λ(k)C_h(k)+(1-λ(k))C_r(k)

module M3.6: based on the model predictive control framework, the following control problem is formed:

z(k+1|k)＝A(k)z(k)+B(k)τ(k)+C(k)

s.t.

the control problem is: at any time k, the time domain is optimized to t ═ k, k +1]Dividing the time-domain-based data into P discrete time periods with equal time step, wherein the goal of rolling optimization is to minimize the cumulative mixed cost function in the time domain; the prediction model is a state space expression established based on a dynamic model of the slave end system, and A (k), B (k) and C (k) are corresponding discrete coefficient matrixes; the system security constraint is h_oAnd h_bRespectively representing the distance between the position of the current robot and the nearest barrier and the nearest boundary, and ensuring that the distance is not less than the corresponding threshold value delta_o,δ_b；

Module M3.7: and solving the optimization problem in each optimization time domain t ═ k, k +1, and calculating the optimal control quantity input at the time k.

9. A computer-readable storage medium storing a computer program, wherein the computer program, when executed by a processor, implements the steps of the method for robot local trajectory adjustment and human-machine sharing control of any one of claims 1 to 4.

10. A robot comprising the local trajectory adjustment and human-machine sharing control system for a robot according to any one of claims 5 to 8, or comprising the computer-readable storage medium of claim 9 having a computer program stored thereon.

Images