A real-time obstacle avoidance method for multiple robotic arms in laparoscopic surgery

Abstract

The present invention proposes a real-time obstacle avoidance method for multiple robotic arms in laparoscopic surgery, comprising: step S1, equating the end tool of the laparoscopic surgery robotic arm to a capsule; step S2, performing kinematic modeling of the system in a discrete time domain; step S3, setting speed constraints and distance constraints for the end tools respectively according to the kinematic modeling; step S4, defining a cost function; step S5, establishing an optimization equation according to the speed constraints and distance constraints in step S3 and the cost function in step S4; and step S6, generating collision-free path points.

Description

Technical Field

[0001] The present invention relates to the technical field of medical robots, and in particular to a real-time obstacle avoidance method for multiple robotic arms in laparoscopic surgery. Background Art

[0002] Medical robots are widely used in laparoscopic surgery. In most operations, robots and navigation systems mainly play an auxiliary role. It is very difficult for robots to independently complete a series of operations such as decision-making, planning, and execution. At the same time, the absolute safety of patients during the operation needs to be guaranteed. To achieve this, the robot system needs to be able to judge the danger points in advance and make decisions quickly.

[0003] Laparoscopic surgical robots are usually equipped with multiple robotic arms, and the working spaces of the robotic arms overlap. In addition, there may be various tools, obstacles, etc. in the actual surgical scene. How to avoid collisions between the robotic arms and the external environment, and at the same time, let the robotic arms autonomously plan a collision-free executable path, has always been a hot topic for major medical surgical robot companies.

[0004] The main defects and shortcomings of the above technical methods are that traditional medical surgical robots lack autonomy, cannot judge possible collisions or dangers in advance, and cannot automatically adjust the execution path of the robotic arm after detecting a collision. Summary of the invention

[0005] The object of the present invention is to solve at least one of the technical drawbacks.

[0006] To this end, the purpose of the present invention is to provide a real-time obstacle avoidance method for multiple robotic arms in laparoscopic surgery.

[0007] In order to achieve the above object, an embodiment of the present invention provides a real-time obstacle avoidance method for multiple robotic arms in laparoscopic surgery, comprising the following steps:

[0008] Step S1, the end tool of the laparoscopic surgery robot arm is equivalent to a capsule body, wherein the middle end is a cylinder and the two ends are hemispherical bodies, and the distance between the two capsule bodies is defined as:

[0009]

[0010] in, is the distance between the two capsule axes, r i , r j are the cylinder radii of the i-th and j-th capsules respectively;

[0011] Step S2, kinematic modeling of the system in discrete time domain is as follows:

[0012] x(k+1)=x(k)+Bu(k),

[0013] in, Indicates the status of the two tools, is the coefficient matrix, Δt c is the sampling time, is the control input;

[0014] Step S3, setting speed constraints and distance constraints for the end tool respectively according to kinematic modeling;

[0015] Step S4, define the cost function as:

[0016]

[0017] Where p = N / Δt c is the number of prediction time steps, x MPC (k) is the expected state of the capsule at time k, is the expected predicted state of the capsule at time k+i;

[0018] Step S5, establishing an optimization equation according to the speed constraint and the distance constraint in step S3 and the cost function in step S4;

[0019] Step S6, generate collision-free path points. When one robot arm moves, the other robot arms are treated as obstacles. R, G, and O are used to represent the coordinate positions of the robot arm, the target point, and the obstacle in three-dimensional space respectively; define a plane through which the robot arm can reach the target point without collision, and define S i Represents a set of obstacles, scans the obstacle range, and determines whether there is interference between the obstacles and the plane;

[0020] Find the intersection point T, Z of the moving capsule and the plane i represents the point on the obstacle closest to T, then the collision-free intermediate path point w i It is a line segment An interpolation point on ; the path point is calculated once in each time step to obtain a set of path points:

[0021] W=β i w i .

[0022] Further, in step S3, the speed constraint is:

[0023]

[0024] in, is the linear maximum speed constraint, and α(k) is the confidence level coefficient.

[0025] Further, in step S3, the distance constraint is:

[0026]

[0027] Among them, d s The minimum distance between tools defined by the user.

[0028] Further, in step S5, the optimization equation is

[0029]

[0030]

[0031]

[0032]

[0033]

[0034] i=0,...,p-1

[0035] h=0,...,N 0

[0036] j∈{r,j}

[0037] in, and They are the distance between capsules and the distance between capsules and obstacles.

[0038] Further, in step S5, the determining whether there is interference between the obstacle and the plane includes:

[0039] Find two points O on the obstacle closest to R i，1， O i，2 , when these two points meet the following two conditions:

[0040] (O i，1 , O i，2 )·n=0

[0041] ((O i，1 +σ(O i，2 -O i-1 ))-R 1 )·n=0

[0042] This means that there is no interference between the obstacle and the plane, and there is a collision-free path to pass through; if there is no interference, β = 0, and if there is interference, β < 1.

[0043] Further, in step S6, the plane normal vector is set to:

[0044]

[0045] Among them, R1 and R2 are two points on the axis of the capsule body at this moment, and G1 and G2 are two points on the axis of the target capsule position.

[0046] According to the real-time obstacle avoidance method of multiple robotic arms in laparoscopic surgery according to the embodiment of the present invention, the real-time collision detection and obstacle avoidance strategy based on the MPC method can predict possible collisions and adjust the motion trajectory of the robotic arms in real time according to the position of the obstacles.

[0047] Additional aspects and advantages of the present invention will be given in part in the following description and in part will be obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] The above and / or additional aspects and advantages of the present invention will become apparent and easily understood from the description of the embodiments in conjunction with the following drawings, in which:

[0049] Figure 1 is a flow chart of a real-time obstacle avoidance method for multiple robotic arms in laparoscopic surgery according to an embodiment of the present invention;

[0050] Figure 2 a to Figure 2 c is a schematic diagram of generating collision-free path points according to an embodiment of the present invention. DETAILED DESCRIPTION

[0051] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and should not be construed as limiting the present invention.

[0052] The present invention proposes a real-time obstacle avoidance method for multiple robotic arms in laparoscopic surgery, which adopts a real-time collision detection and obstacle avoidance strategy based on the MPC method. It can predict possible collisions and adjust the motion trajectory of the robotic arms in real time according to the position of obstacles.

[0053] like Figure 1 As shown, the real-time obstacle avoidance method of multiple robotic arms in laparoscopic surgery according to an embodiment of the present invention comprises the following steps:

[0054] Step S1, the end tool of the laparoscopic surgery robot arm is equivalent to a capsule. The shape is similar to a capsule, where the middle end is a cylinder and the two ends are hemispherical. The distance between the two capsules is defined as:

[0055]

[0056] in, is the distance between the two capsule axes, r i , r j are the cylinder radii of the i-th and j-th capsules respectively.

[0057] Step S2, kinematic modeling of the system in discrete time domain is as follows:

[0058] x(k+1)=x(k)+Bu(k),

[0059] in, Indicates the status of the two tools, is the coefficient matrix, Δt c is the sampling time, is the control input.

[0060] Step S3, according to kinematic modeling, set speed constraints and distance constraints for the end tool respectively.

[0061] (1) Speed ​​constraints:

[0062]

[0063] in, is the linear maximum speed constraint, and α(k) is the confidence level coefficient.

[0064] (2) Distance constraint:

[0065]

[0066] Among them, d s is the user-defined minimum distance between tools.

[0067] Step S4, define the cost function as:

[0068]

[0069] Where p = N / Δt c is the number of prediction time steps, x MPC (k) is the expected state of the capsule at time k, is the expected predicted state of the capsule at time k+i.

[0070] Step S5, establishing an optimization equation according to the speed constraint and distance constraint in step S3 and the cost function in step S4.

[0071] Specifically, combining the above constraints and cost functions, it can be converted into an optimization problem:

[0072]

[0073]

[0074]

[0075]

[0076]

[0077] i=0,...,p-1

[0078] h=0,...,N 0

[0079] j∈{r,j}

[0080] in, and They are the distance between capsules and the distance between capsules and obstacles.

[0081] Step S6, generating collision-free path points. When one robotic arm moves, the other robotic arms are treated as obstacles.

[0082] refer to Figure 2 a to Figure 2 c. Let R, G, and O represent the coordinate positions of the robot arm, the target point, and the obstacle in three-dimensional space, respectively.

[0083] Define a plane through which the robot can reach the target point without collision. The plane normal vector can be expressed as:

[0084]

[0085] Among them, R1 and R2 are two points on the axis of the capsule body at this moment, and G1 and G2 are two points on the axis of the target capsule position. i Represents a set of obstacles, scans the obstacle range, and finds the two points O on the obstacles closest to R i，1 , O i，2 , if the two points satisfy the following two conditions:

[0086] (O i，1 , O i，2 )·n=0

[0087] ((O i，1 +σ(O i，2 -O i-1 ))-R 1 )·n=0

[0088] This means that there is no interference between the obstacle and the plane, and there is a collision-free path to pass through. If there is no interference, β = 0, and if there is interference, then β < 1.

[0089] Find the intersection point T, Z of the moving capsule and the plane i represents the point on the obstacle closest to T, then the collision-free intermediate path point w i It is a line segment An interpolation point on . The path point is calculated once in each time step to obtain a set of path points:

[0090] W=β i w i .

[0091] According to the real-time obstacle avoidance method of multiple robotic arms in laparoscopic surgery according to the embodiment of the present invention, the real-time collision detection and obstacle avoidance strategy based on the MPC method can predict possible collisions and adjust the motion trajectory of the robotic arms in real time according to the position of the obstacles.

[0092] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0093] Although the embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and cannot be understood as limiting the present invention. Those skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention without departing from the principles and purpose of the present invention. The scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A real-time obstacle avoidance method for multiple robotic arms in laparoscopic surgery, characterized in that: The steps include: Step S1, the end tool of the laparoscopic surgery robot arm is equivalent to a capsule body, wherein the middle end is a cylinder and the two ends are hemispherical bodies, and the distance between the two capsule bodies is defined as: in, is the distance between the two capsule axes, r i , r j are the cylinder radii of the i-th and j-th capsules respectively; Step S2, kinematic modeling of the system in discrete time domain is as follows: x(k+1)=x(k)+Bu(k), in, Indicates the status of the two tools, is the coefficient matrix, Δt c is the sampling time, is the control input; Step S3, setting speed constraints and distance constraints for the end tool respectively according to kinematic modeling; Step S4, define the cost function as: Where p = N / Δt c is the number of prediction time steps, x MPC (k) is the expected state of the capsule at time k, is the expected predicted state of the capsule at time k+i; Step S5, establishing an optimization equation according to the speed constraint and the distance constraint in step S3 and the cost function in step S4; Step S6, generate collision-free path points. When one robot moves, the other robots are treated as obstacles. R, G, and O are used to represent the coordinate positions of the robot, the target point, and the obstacle in three-dimensional space respectively. Define a plane through which the robot can reach the target point without collision, and define S i Represents a set of obstacles, scans the obstacle range, and determines whether there is interference between the obstacles and the plane; Find the intersection point T, Z of the moving capsule and the plane i represents the point on the obstacle closest to T, then the collision-free intermediate path point w i It is a line segment An interpolation point on ; the path point is calculated once in each time step to obtain a set of path points: In=β i In i 。 2. The real-time obstacle avoidance method for multiple robotic arms in laparoscopic surgery as claimed in claim 1, characterized in that: In step S3, the speed constraint is: in, is the linear maximum speed constraint, and α(k) is the confidence level coefficient.

3. The real-time obstacle avoidance method for multiple robotic arms in laparoscopic surgery according to claim 1, characterized in that: In step S3, the distance constraint is: Among them, d s The minimum distance between tools defined by the user.

4. The real-time obstacle avoidance method for multiple robotic arms in laparoscopic surgery according to claim 1, characterized in that: In step S5, the optimization equation is i=0,...,p-1 h=0,...,N0 j∈{r,j} in, and They are the distance between capsules and the distance between capsules and obstacles.

5. The real-time obstacle avoidance method for multiple robotic arms in laparoscopic surgery according to claim 1, characterized in that: In step S5, determining whether there is interference between the obstacle and the plane includes: Find the two points O on the obstacle closest to R i，1 , O i，2 , when these two points meet the following two conditions: (The i，1 ,The i，2 )·n=0 ((O i，1 +σ(O i，2 -O i-1 ))-R1)·n=0 This means that there is no interference between the obstacle and the plane, and there is a collision-free path to pass through; if there is no interference, β = 0, and if there is interference, β < 1.

6. The real-time obstacle avoidance method for multiple robotic arms in laparoscopic surgery according to claim 1, characterized in that: In step S6, the plane normal vector is set to: Among them, R1 and R2 are two points on the axis of the capsule body at this moment, and G1 and G2 are two points on the axis of the target capsule position.

Images