Automatic obstacle avoidance method and system for surgical robot

Abstract

The invention provides an automatic obstacle avoidance method and system for a surgical robot. The method comprises the following steps: in the process of controlling a surgical robot to move to a target position, if the surgical robot meets an obstacle avoidance condition, determining a direction vector from the surgical robot to the target position, and determining a unit vector of a shortest distance point from the surgical robot to a target obstacle; calculating an obstacle avoidance direction and an obstacle avoidance speed of the surgical robot according to the direction vector and the unit vector; according to the direction vector and the pose information of the target position, the attraction speed of the surgical robot is calculated; the suction speed refers to a speed component of the surgical robot pointing to the target position; and calculating the resultant speed of the surgical robot according to the obstacle avoidance speed and the attraction speed, and controlling the surgical robot to realize automatic obstacle avoidance movement and surgical target arrival through the resultant speed. According to the embodiment of the invention, the surgical robot can have the autonomous obstacle avoidance capability, and surgical pose alignment can be safely and efficiently realized and a specified surgical task can be completed.

Description

Technical Field

[0001] The present disclosure relates to the technical field of surgical navigation, and in particular to an automatic obstacle avoidance method and system for a surgical robot. Background Art

[0002] In recent years, surgical navigation systems have been widely used in surgical operations as the most effective assistant to assist surgeons in completing surgical tasks. The autonomous and safe arrival of surgical robots at the surgical site can improve the efficiency of surgical robot-assisted surgical tasks, among which how to safely avoid obstacles has a positive significance for the autonomous control of surgical robots.

[0003] Current surgical robot systems are mainly used in orthopedics, neurosurgery, minimally invasive abdominal surgery, and cardiovascular surgery. In these surgeries, the surgical robots are mostly operated by skilled surgeons through master-slave teleoperation and dragging in collaborative mode to reach the surgical target. The operation methods of these two types of surgical robots are complicated, and long-term surgical operations bring great physical and mental burdens to surgeons. Therefore, if the surgical robot can effectively achieve autonomous and safe obstacle avoidance and automatically reach the desired surgical position according to the surgical plan, it can effectively improve the surgical efficiency.

[0004] In traditional assisted procedures, surgical robots are mainly used for positioning and guidance to provide surgeons with more consistent surgical positioning capabilities. Surgeons then hold surgical tools through the end guide sleeve of the surgical robot to perform surgical operations, such as spinal decompression and internal fixation, brain biopsy, ablation, drainage, electrode implantation and other pre-operative positioning in neurosurgery. However, the surgical robot needs to have timely and safe obstacle avoidance operation capabilities during the alignment process of automatically reaching the surgical posture. In addition, surgical robots can also be used to automatically track surgical instruments. For example, in laparoscopic surgery tasks, the end effector of the surgical robot enters the abdominal environment through a tiny incision. During this process, surgical instruments often require real-time tracking by laparoscopes. This requires the snake manipulator equipped with a laparoscope to have the ability to autonomously avoid obstacles so as to avoid collisions with important tissues in the abdominal environment while providing a reliable surgical field of view. Reliable and safe real-time reactive automatic obstacle avoidance algorithms can reduce the doctor's learning curve and surgical burden on the operation of the surgical robot, allowing surgeons to focus on important surgical operations and improve surgical efficiency.

[0005] Most of the existing obstacle avoidance motion planning is aimed at static scenes. For example, sampling and search algorithms need to be performed based on global information. Once the environment changes, resampling or searching for obstacle avoidance paths is required, which cannot meet the real-time responsiveness requirements of autonomous control of surgical robots. The traditional artificial potential field method can achieve local obstacle avoidance and respond promptly to the real-time changing environment. However, due to the inherent defects of local area oscillation, multiple velocity fields superposition easily falling into local minimum, and unreachable target points near obstacles, it is difficult to meet the obstacle avoidance requirements in surgical scenarios. Summary of the invention

[0006] In view of this, the present disclosure provides an automatic obstacle avoidance method and system for a surgical robot to address the deficiencies of existing obstacle avoidance technologies. The method enables the surgical robot to better utilize the free movement space of the surgical environment in a complex surgical environment according to the spatial position relationship between the surgical target and obstacles, and to achieve obstacle avoidance planning and surgical target arrival in real time in an efficient and simple manner.

[0007] A first aspect of the present disclosure provides an automatic obstacle avoidance method for a surgical robot, the method comprising:

[0008] In the process of controlling the surgical robot to move to a target position, if the surgical robot meets the obstacle avoidance condition, the direction vector of the surgical robot to the target position is determined, and the unit vector of the shortest distance point from the surgical robot to the target obstacle is determined; the target obstacle is the obstacle closest to the surgical robot among multiple obstacles;

[0009] Calculating the obstacle avoidance direction and obstacle avoidance speed of the surgical robot according to the direction vector and the unit vector;

[0010] Calculating the suction speed of the surgical robot according to the direction vector and the position information of the target position; the suction speed refers to the speed component of the surgical robot pointing to the target position;

[0011] The combined speed of the surgical robot is calculated according to the obstacle avoidance speed and the attraction speed, and the surgical robot is controlled by the combined speed to achieve automatic obstacle avoidance movement and reach the surgical target.

[0012] The disclosed embodiment calculates the obstacle avoidance speed and the attraction speed, and calculates the combined speed of the surgical robot based on the obstacle avoidance speed and the attraction speed, and controls the surgical robot through the combined speed to achieve automatic obstacle avoidance movement and reach the surgical target, thereby improving surgical efficiency. In the disclosed embodiment, the obstacle avoidance condition includes an angle condition, a first distance condition, and a second distance condition; before determining the direction vector of the surgical robot to the target position, the method further includes:

[0013] Determine whether the surgical robot satisfies an angle condition, a first distance condition, and a second distance condition at the same time; wherein the angle condition means that the angle between the unit vector and the direction vector is less than a preset angle; the first distance condition means that the distance between the surgical robot and the target obstacle is less than an adaptive obstacle avoidance distance; the second distance condition means that the distance between the surgical robot and the target position is greater than the adaptive obstacle avoidance distance;

[0014] If the surgical robot satisfies the angle condition, the first distance condition, and the second distance condition at the same time, it is determined that the surgical robot satisfies the obstacle avoidance condition.

[0015] In the disclosed embodiment, the adaptive obstacle avoidance distance is determined based on the shortest distance point of the target obstacle and the target position; the shortest distance point refers to the point on the surface of the obstacle that is closest to the surgical robot.

[0016] The obstacle avoidance distance in the disclosed embodiment is determined based on the shortest distance point of the target obstacle and the target position, and is adaptively adjusted; it can effectively avoid the target being unreachable due to the constant distance of the traditional obstacle avoidance function activation and avoid the motion oscillation phenomenon that may occur when processing local obstacle avoidance.

[0017] In the embodiment of the present disclosure, the method further includes:

[0018] Constructing a kinematic equivalent model according to the kinematic parameters of the surgical robot;

[0019] Establish multiple obstacle equivalent models in the surgical environment based on the point cloud data obtained by the visual sensor;

[0020] Calculating the distance between the surgical robot and each obstacle, and the shortest distance point on each obstacle according to the kinematic equivalent model and the multiple obstacle equivalent models; the shortest distance point refers to the point on the surface of the obstacle that is closest to the surgical robot;

[0021] The target obstacle is selected from the plurality of obstacles according to the distance between the surgical robot and each obstacle.

[0022] In the embodiment of the present disclosure, the obstacle avoidance direction and obstacle avoidance speed of the surgical robot are calculated according to the direction vector and the unit vector, including:

[0023] Construct a normal plane according to the unit vector; the normal plane is perpendicular to the unit vector;

[0024] Projecting the direction vector onto the normal plane to obtain a projection vector of the direction vector on the normal plane;

[0025] The obstacle avoidance direction is determined according to the projection vector.

[0026] In the embodiment of the present disclosure, after determining that the surgical robot meets the obstacle avoidance condition, the method further includes:

[0027] If the position coordinates of the surgical robot, the shortest distance point of the target obstacle and the target position are on the same direction vector, then calculating the shortest distance from the target position to the boundary of the obstacle projection surface, and calculating a second direction vector from the target position to the nearest boundary point in the obstacle projection surface;

[0028] Obtaining a virtual target position based on the closest distance, the second direction vector and the target position;

[0029] The virtual target position is used as a new target position, and the step of determining the direction vector of the surgical robot to the target position is performed until the surgical robot is controlled by the combined speed to achieve automatic obstacle avoidance movement and reach the surgical target.

[0030] In the embodiment of the present disclosure, calculating the shortest distance from the target position to the boundary of the obstacle projection surface includes:

[0031] Calculating the first plane coordinate points of all vertices of the target obstacle on the projection surface; wherein a plurality of first plane coordinate points corresponding to all vertices constitute the boundary of the obstacle projection surface;

[0032] Calculate a second plane coordinate point of the target position on the boundary of the obstacle projection surface;

[0033] The shortest distance from the target position to the boundary of the obstacle projection surface is calculated according to the multiple first plane coordinate points and the second plane coordinate points.

[0034] An embodiment of a second aspect of the present disclosure provides an automatic obstacle avoidance system for a surgical robot, comprising:

[0035] A vector determination module is used to determine the direction vector of the surgical robot to the target position and the unit vector of the shortest distance point from the surgical robot to the target obstacle during the process of controlling the surgical robot to move to the target position, if the surgical robot meets the obstacle avoidance condition; the target obstacle is the obstacle closest to the surgical robot among multiple obstacles;

[0036] An obstacle avoidance speed determination module, used to calculate the obstacle avoidance direction and obstacle avoidance speed of the surgical robot according to the direction vector and the unit vector;

[0037] An attraction speed determination module, used for calculating the attraction speed of the surgical robot according to the direction vector and the position information of the target position; the attraction speed refers to the speed component of the surgical robot pointing to the target position;

[0038] The automatic obstacle avoidance module is used to calculate the combined speed of the surgical robot according to the obstacle avoidance speed and the attraction speed, and control the surgical robot through the combined speed to achieve automatic obstacle avoidance movement and reach the surgical target.

[0039] An embodiment of the third aspect of the present disclosure provides an electronic device, which includes a memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the automatic obstacle avoidance method of the surgical robot described in the first aspect by executing the computer instructions.

[0040] An embodiment of the fourth aspect of the present disclosure provides a computer-readable storage medium, on which computer instructions are stored, and the computer instructions are used to enable a computer to execute the automatic obstacle avoidance method for a surgical robot described in the first aspect.

[0041] Additional aspects and advantages of the present disclosure will be given in part in the following description and in part will become apparent from the following description or be learned through practice of the present disclosure. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] By reading the detailed description of the preferred embodiment below, various other advantages and benefits will become clear to those of ordinary skill in the art. The accompanying drawings are only used for the purpose of illustrating the preferred embodiment and are not considered to be limitations of the present disclosure. In addition, the same reference symbols are used to represent the same components throughout the accompanying drawings.

[0043] In the attached picture:

[0044] Figure 1 A schematic diagram showing a flow chart of an automatic obstacle avoidance method for a surgical robot provided by an embodiment of the present disclosure is shown;

[0045] Figure 2 A schematic diagram showing the principle of an automatic obstacle avoidance method for a surgical robot provided by an embodiment of the present disclosure is shown;

[0046] Figure 3 A schematic diagram of the obstacle avoidance principle requiring the addition of a virtual surgical target provided by an embodiment of the present disclosure is shown;

[0047] Figure 4 A schematic diagram of an equivalent model of an ellipsoid-wrapped convex body for surgical robot simplification based on URDF provided in an embodiment of the present disclosure is shown;

[0048] Figure 5 A schematic diagram showing an obstacle avoidance path for verifying an obstacle avoidance solution provided by an embodiment of the present disclosure is shown;

[0049] Figure 6 A schematic diagram showing autonomous obstacle avoidance in a single obstacle environment provided by an embodiment of the present disclosure is shown;

[0050] Figure 7 A schematic top view of autonomous obstacle avoidance in a multi-obstacle environment provided by an embodiment of the present disclosure is shown;

[0051] Figure 8 A front view schematic diagram of autonomous obstacle avoidance in a multi-obstacle environment provided by an embodiment of the present disclosure is shown;

[0052] Fig. 9 A schematic diagram showing an obstacle avoidance and target arrival situation in which both the obstacle and the target arrival position are dynamically changed provided by an embodiment of the present disclosure;

[0053] Fig.10 A schematic diagram showing a situation in which a robot configuration with snake-like structure characteristics automatically avoids obstacles in a multi-obstacle environment provided by an embodiment of the present disclosure;

[0054] Fig.11 A schematic diagram showing a situation where a virtual surgical target needs to be added when an obstacle is simplified to an equivalent convex block model structure provided by an embodiment of the present disclosure;

[0055] Fig.12 A schematic diagram showing another situation in which a virtual surgical target needs to be added when an obstacle is simplified to an equivalent convex block model structure provided by an embodiment of the present disclosure;

[0056] Fig.13 A schematic diagram showing another situation in which a virtual surgical target needs to be added when an obstacle is simplified to an equivalent convex block model structure provided by an embodiment of the present disclosure;

[0057] Fig.14 A schematic diagram showing another situation in which a virtual surgical target needs to be added when an obstacle is simplified to an equivalent convex block model structure provided by an embodiment of the present disclosure;

[0058] Fig.15 A schematic structural diagram of an automatic obstacle avoidance device for a surgical robot provided by an embodiment of the present disclosure is shown;

[0059] Fig.16 A schematic diagram of the structure of an electronic device provided by an embodiment of the present disclosure is shown;

[0060] Fig.17A schematic diagram of a storage medium provided by an embodiment of the present disclosure is shown. DETAILED DESCRIPTION

[0061] The exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although the exemplary embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments described herein. On the contrary, these embodiments are provided in order to enable a more thorough understanding of the present disclosure and to fully convey the scope of the present disclosure to those skilled in the art.

[0062] It should be noted that, unless otherwise specified, the technical terms or scientific terms used in the present disclosure should have the common meanings understood by those skilled in the art to which the present disclosure belongs.

[0063] According to an embodiment of the present disclosure, an embodiment of an automatic obstacle avoidance method for a surgical robot is provided. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0064] In this embodiment, a method for automatic obstacle avoidance of a surgical robot is provided. Figure 1 is a flow chart of an automatic obstacle avoidance method for a surgical robot according to an embodiment of the present disclosure, such as Figure 1 As shown, the process includes the following steps:

[0065] Step S101, in the process of controlling the surgical robot to move to a target position, if the surgical robot meets the obstacle avoidance condition, the direction vector of the surgical robot to the target position and the unit vector of the shortest distance point from the surgical robot to the target obstacle are determined.

[0066] In the disclosed embodiment, the surgical robot can be characterized by a kinematic equivalent model, which is constructed based on the kinematic parameters of the surgical robot. The obstacle can be characterized by an obstacle equivalent model, which is a convex body model that can wrap the obstacle body and is built from a point cloud.

[0067] In the following embodiments, a surgical robot is used to characterize the kinematic equivalent model of the surgical robot, an obstacle is used to characterize the obstacle equivalent model, and a target obstacle is used to characterize the target obstacle equivalent model, which will not be repeated here.

[0068] In the disclosed embodiment, the surgical robot can be specifically defined as the end effector of the surgical robot; the target position refers to the surgical position, which can be determined according to the preoperative planning and intraoperative navigation system. The unit direction vector of the surgical robot to the target position can be determined by the following expression:

[0069]

[0070] Among them, V Target2T_tcp represents the direction vector, P target The coordinates representing the target location (e.g. Figure 2 The "target point" in T_tcp represents the position coordinates of the surgical robot (e.g. Figure 2 end effector center point in the image).

[0071] In the disclosed embodiment, the unit vector of the shortest distance point from the surgical robot to the target obstacle can be determined through steps a1 to a3:

[0072] Step a1, selecting the obstacle closest to the surgical robot from multiple obstacles in the surgical environment as the target obstacle.

[0073] Step a2: determine the point closest to the surgical robot on the surface of the target obstacle as the shortest distance point of the target obstacle (e.g. Figure 2 'Nearest point' in .

[0074] Step a3: Determine the above unit vector according to the coordinate position of the shortest distance point of the target obstacle and the coordinate position of the end effector of the surgical robot through the following expression:

[0075]

[0076] Among them, V RO The unit vector representing the shortest distance point from the surgical robot end effector to the target obstacle, P T_tcp represents the position coordinates of the surgical robot, O minp The position coordinates of the point with the shortest distance to the target obstacle (e.g. Figure 2 'Nearest point' in .

[0077] In some specific embodiments, the obstacle avoidance conditions include but are not limited to: an angle condition, a first distance condition, and a second distance condition; before the above step S101, the method further includes:

[0078] Step a1, determining whether the surgical robot satisfies the angle condition, the first distance condition and the second distance condition at the same time.

[0079] Step a2: If the surgical robot satisfies the angle condition, the first distance condition and the second distance condition at the same time, it is determined that the surgical robot satisfies the obstacle avoidance condition.

[0080] In the embodiment of the present disclosure, the obstacle avoidance condition is expressed as follows:

[0081]

[0082] in, The angle condition means that the angle between the unit vector and the direction vector is less than a preset angle pi / 2, where pi represents radians, 180 degrees.

[0083] D O <D act The first distance condition refers to the distance D between the surgical robot and the shortest distance point of the target obstacle. O Less than the adaptive obstacle avoidance distance D act .

[0084] norm(P target -P T_tcp )>D act The second distance condition refers to the coordinate position P of the surgical robot. T_tcp With the target position P target The distance between them is greater than the adaptive obstacle avoidance distance D act .

[0085] It can be seen from the expression of the above obstacle avoidance condition that only when the above angle condition, the first distance condition and the second distance condition are met at the same time can it be determined that the surgical robot meets the obstacle avoidance condition.

[0086] In some specific embodiments, the angle Angle between the unit vector and the direction vector can be determined according to the following expression:

[0087]

[0088] The function dot is a dot product operation of a vector, and the solution of Angle can ensure that the robot end effector moves directly toward the surgical target position when obstacle avoidance is not required.

[0089] In some specific embodiments, the above D O and D act It can be determined according to the following expression:

[0090] D act =norm(O min p -P target )

[0091] D O =norm(O min p -P T_tcp )

[0092] Among them, D act represents the adaptive obstacle avoidance distance, O min p Indicates the position coordinates of the point with the shortest distance to the target obstacle. O represents the distance between the surgical robot and the shortest distance point of the target obstacle. O Less than the adaptive obstacle avoidance distance D act When the robot reaches the end of the robot, the obstacle avoidance function is activated. This ensures that the robot will not collide with the closest obstacle during movement, while reducing the situation where the robot falls into the local minimum value and fails to avoid obstacles due to the superposition of multiple traditional obstacle avoidance velocity fields. In addition, the adaptive obstacle avoidance distance can avoid the problem of unreachable surgical targets.

[0093] In some specific embodiments, the method further comprises:

[0094] Step b1, constructing a kinematic equivalent model according to the kinematic parameters of the surgical robot.

[0095] In the embodiments of the present disclosure, the kinematic parameters include the structure, joints, links and other physical properties of the robot, for example: the various components of the robot, including physical properties such as shape, size, mass, inertia, etc.; the connection relationship and movement mode between the links, including revolute joints, prismatic joints, fixed joints, etc.

[0096] Step b2: establishing multiple obstacle equivalent models in the surgical environment based on the point cloud data acquired by the visual sensor.

[0097] Step b3, calculating the distance between the surgical robot and each obstacle and the shortest distance point on each obstacle according to the kinematic equivalent model and the multiple obstacle equivalent models. The shortest distance point refers to the point on the obstacle surface that is closest to the surgical robot.

[0098] Step b4: selecting the target obstacle from the multiple obstacles according to the distance between the surgical robot and each obstacle.

[0099] In the disclosed embodiment, the robot model generated by the kinematic parameter URDF (i.e., the above-mentioned kinematic model) needs to use an ellipsoid to envelop the robot link according to the coordinate transformation relationship and the size of the robot model to generate an obstacle avoidance model for the robot body. At the same time, the point cloud of the surgical environment obstacle obtained by the visual sensor generates a corresponding simplified convex obstacle model according to its geometric features, so as to facilitate the collision detection function to calculate the closest point of the obstacle model and subsequent obstacle avoidance planning.

[0100] Step S102: calculating the obstacle avoidance direction and obstacle avoidance speed of the surgical robot according to the direction vector and the unit vector.

[0101] In some specific embodiments, the obstacle avoidance direction of the surgical robot can be determined by the following expression:

[0102]

[0103] Among them, Dir avo Represents the obstacle avoidance direction of the surgical robot, which can be understood as the unit vector of the obstacle avoidance speed direction; V RO The unit vector representing the shortest distance from the end effector of the surgical robot to the target obstacle, V Target2T_tcp Represents a direction vector.

[0104] In the disclosed embodiment, the idea of ​​confirming the obstacle avoidance speed direction is: the direction vector from the surgical robot end effector to the surgical target point is projected onto the normal plane of the straight line between the shortest distance point on the target obstacle and the robot end effector, for example: first, a normal plane is constructed based on the unit vector; the normal plane is perpendicular to the unit vector; secondly, the direction vector is projected onto the normal plane to obtain the projection vector of the direction vector on the normal plane; finally, the obstacle avoidance direction is determined based on the projection vector.

[0105] In some specific embodiments, in order to ensure that the obstacle avoidance speed is related to the distance between the surgical robot and the shortest distance point of the target obstacle when the obstacle avoidance speed is activated, so that the surgical robot generates a larger obstacle avoidance speed when it is closer to the obstacle, the calculation formula of the obstacle avoidance speed is:

[0106]

[0107] Among them, K r is the obstacle avoidance speed control gain, V avo Indicates the obstacle avoidance speed.

[0108] Step S103, calculating the suction speed of the surgical robot according to the direction vector and the position information of the target position. The suction speed refers to the speed component of the surgical robot pointing to the target position.

[0109] In some specific embodiments, the suction speed of the surgical robot can be determined by the following expression:

[0110]

[0111] Among them, V att is the suction speed based on the surgical target location, K a It is the suction speed control gain, which is used to control the speed of the surgical robot end. target is the Cartesian target surgical position determined by the navigation system, P T_tcp It is the end point of the robot end effector, and the position error is compensated by the information calculated by the robot's forward kinematics and tracked by the navigation system.

[0112] Step S104, calculating a combined speed of the surgical robot according to the obstacle avoidance speed and the attraction speed, and controlling the surgical robot by the combined speed to achieve automatic obstacle avoidance movement and reach the surgical target.

[0113] In some specific embodiments, the obstacle avoidance function of the surgical robot is used only when the obstacle avoidance conditions are met. In addition, in order to ensure the stability of the moving speed of the robot end, the combined speed is calculated as follows:

[0114]

[0115] In some specific embodiments, after determining that the surgical robot meets the obstacle avoidance condition, the method further includes:

[0116] Step c1, if the position coordinates of the surgical robot, the shortest distance point of the target obstacle and the target position are on the same direction vector, then calculate the shortest distance from the target position to the boundary of the obstacle projection surface, and calculate the second direction vector from the target position to the nearest boundary point in the obstacle projection surface.

[0117] In some specific embodiments, when the angle Angle between the unit vector and the direction vector is less than the angle threshold θ t , and when the surgical target position is in the plane surrounded by the obstacle's positive projection, it can be determined that the position coordinates of the surgical robot, the shortest distance point of the target obstacle and the target position are on the same direction vector.

[0118] In some specific embodiments, calculating the shortest distance from the target position to the boundary of the obstacle projection surface includes:

[0119] Step d1, calculating the first plane coordinate points of all vertices of the target obstacle on the projection surface; wherein a plurality of first plane coordinate points corresponding to all vertices constitute the boundary of the obstacle projection surface.

[0120] Step d2, calculating a second plane coordinate point of the target position on the boundary of the obstacle projection surface;

[0121] Step d3, calculating the shortest distance from the target position to the boundary of the obstacle projection surface according to the multiple first plane coordinate points and the second plane coordinate points.

[0122] In the embodiments of the present disclosure, Figure 3 As shown, the normal vector of the projection surface is n = V Target2T_tcp , the calculation formula of the coordinate points of the surgical target point and the vertex of the simplified convex obstacle to the projection surface is:

[0123] Q=P-dot(n,P)n

[0124] Where P is the point to be projected, which can be the surgical target point P target Or the obstacle vertex O i ; Q is the calculated projection point coordinate, Q = [Q X ,Q Y ,Q Z ]. In order to calculate the distance relationship between the projection points, the projection points in the three-dimensional space are simplified to the two-dimensional plane, the coordinate axis corresponding to the maximum component of the normal vector is selected to act in the projection direction, and the coordinate point calculation formula of the point in the three-dimensional space on the projection plane is determined as follows:

[0125]

[0126] Calculate the surgical target point P target When projected onto the projection surface, the nearest point and distance to the boundary line of the obstacle projected onto the projection surface. Assume that Q2d is the distance between all vertices O of the obstacle. i The plane coordinates on the projection surface, then the nearest point P closet The specific formula of the distance dist is:

[0127]

[0128] dist=norm(p2d-P closest )

[0129] Among them, i represents the obstacle vertex sequence, the lower right corner of Q2d is the row sequence of all points of the obstacle vertex in the projection plane, and N is the vertex O of the obstacle in the projection plane. i The point vectors are arranged counterclockwise around the formed convex hull. p2d is the surgical target point P target The plane coordinate point on the projection surface is determined according to the above three-dimensional space coordinate point calculation formula on the projection plane. t is the scale parameter of the point on the obstacle projection edge segment, and the calculation formula is as follows:

[0130]

[0131] Finally, t = max(0, min(1, t)), ensuring that t is within the range of the vertex edge segments of the obstacle on the projection surface.

[0132] The coordinates of the closest point of the surgical target to the obstacle boundary line calculated on the projection surface are converted into three-dimensional space. The specific conversion formula is as follows:

[0133]

[0134] Step c2, obtaining a virtual target position based on the closest distance, the second direction vector and the target position.

[0135] In some specific embodiments, the virtual target position may be determined by the following expression:

[0136] P ntarget =P target +s*dist*P ro_dir

[0137] Among them, P ntarget is the virtual surgical target position, s is the distance scaling factor used to adjust the position of the virtual surgical target, dist is the shortest distance from the original surgical target point to the obstacle projection boundary, P ro_dir It is the second direction vector from the original surgical target position to the nearest boundary point in the obstacle projection surface.

[0138] In some specific embodiments, the calculation formula of the second direction vector is:

[0139]

[0140] Among them, Q T is the coordinate point from the surgical target point to the projection surface. The combined speed of the surgical robot end effector is calculated according to the position of the virtual surgical target point.

[0141] Step c3, taking the virtual target position as a new target position, executing the step of determining the direction vector of the surgical robot to the target position, until the surgical robot is controlled by the combined speed to achieve automatic obstacle avoidance movement and reach the surgical target.

[0142] In the embodiment of the present disclosure, in order to avoid the surgical robot end effector coordinate P T_tcp , the shortest distance point O on the target obstacle minp and the target position P target When the obstacle avoidance direction is determined based on projection and mirror symmetry occurs on the same direction vector, the projection method can be introduced to determine the virtual surgical target position, which can effectively avoid the obstacle avoidance failure.

[0143] In some specific embodiments, the automatic obstacle avoidance method provided by the present invention is not only applicable to the real-time reactive control of autonomous obstacle avoidance of the end effector of the robotic arm, such as safe arrival by obstacle avoidance in surgical positioning, autonomous tracking of surgical instruments in laparoscopic surgery, etc.; it is also suitable for situations in the field of mobile robots and aircraft that require real-time reactive obstacle avoidance capabilities.

[0144] In some specific embodiments, the present disclosure also provides an ellipsoid-wrapped convex body equivalent model for surgical robot simplification based on URDF, for example Figure 4 As shown;

[0145] In some specific embodiments, the present disclosure also provides an obstacle avoidance path for verifying the obstacle avoidance solution, such as Figure 5 As shown;

[0146] In some specific embodiments, the present disclosure also provides a schematic diagram of autonomous obstacle avoidance in a single obstacle environment, for example Figure 6 shown.

[0147] In some specific embodiments, the present disclosure also provides obstacle avoidance in a multi-obstacle environment, such as Figure 7 , Figure 8 shown.

[0148] In some specific embodiments, the present disclosure also provides obstacle avoidance and target arrival situations in which both obstacles and target arrival positions are dynamically changing, for example Fig. 9 shown.

[0149] In some specific embodiments, the present disclosure also provides a robot configuration with snake-like structure characteristics that automatically avoids obstacles in a multi-obstacle environment, such as Fig.10 shown.

[0150] In some specific embodiments, the present disclosure also provides a situation where a virtual surgical target needs to be added when an obstacle is simplified to an equivalent convex block model structure, for example Figure 11-Figure 14 shown.

[0151] The present invention calculates the suction speed V of the surgical robot att and obstacle avoidance speed V avo It can make the speed obstacle avoidance planning of the surgical robot's end effector more capable of obstacle avoidance guidance, and can also avoid the unreachable target caused by the constant distance of the traditional activation of the obstacle avoidance function and avoid the motion oscillation phenomenon that may occur when dealing with local obstacle avoidance and other innovative advantages.

[0152] The beneficial effects of the present invention are as follows: (1) The proposed obstacle avoidance speed method is mainly described in detail in the task space, but it is undeniable that the automatic obstacle avoidance method and system of the surgical robot proposed in the embodiment of the present disclosure can also adopt a similar idea to realize the link obstacle avoidance in the joint space of the surgical robot, so that the robot's obstacle avoidance has more spatial target orientation and obstacle avoidance capabilities. (2) The present invention can also effectively overcome the problems of unreachable targets and oscillating motion in the local obstacle avoidance area that may occur in the traditional artificial potential field vector obstacle avoidance algorithm, and the planned collision-free path has the characteristics of shorter paths and smoother motion to a certain extent.

[0153] Although the present invention takes the obstacle avoidance speed planning of surgical robots as the main obstacle avoidance application case, it is not intended to limit the present invention, and its innovative method is also applicable to situations in the fields of mobile robots, ship navigation, aircraft navigation, etc. that require real-time reactive obstacle avoidance capabilities. The principles and ideas of the proposed automatic obstacle avoidance method and system for surgical robots can be used by any technical personnel in this field without departing from the spirit and scope of the present invention to make possible changes and modifications to the technical solution of the present invention using the methods and technical contents disclosed above. Therefore, any simple modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solution of the present invention are within the protection scope of the technical solution of the present invention.

[0154] The contents not described in detail in the specification of the present invention belong to the common knowledge of those skilled in the art.

[0155] Corresponding to the implementation of the above automatic obstacle avoidance method of the surgical robot, the embodiment of the present disclosure also provides an automatic obstacle avoidance device for a surgical robot, which is used to implement the above automatic obstacle avoidance method. Figures 1 to 14 The automatic obstacle avoidance method of the surgical robot described in any of the embodiments shown in the figure. Fig.15 As shown, the automatic obstacle avoidance device of the surgical robot includes:

[0156] A vector determination module is used to determine the direction vector of the surgical robot to the target position and the unit vector of the shortest distance point from the surgical robot to the target obstacle during the process of controlling the surgical robot to move to the target position, if the surgical robot meets the obstacle avoidance condition; the target obstacle is the obstacle closest to the surgical robot among multiple obstacles;

[0157] An obstacle avoidance speed determination module, used to calculate the obstacle avoidance direction and obstacle avoidance speed of the surgical robot according to the direction vector and the unit vector;

[0158] An attraction speed determination module, used for calculating the attraction speed of the surgical robot according to the direction vector and the position information of the target position; the attraction speed refers to the speed component of the surgical robot pointing to the target position;

[0159] The automatic obstacle avoidance module is used to calculate the combined speed of the surgical robot according to the obstacle avoidance speed and the attraction speed, and control the surgical robot through the combined speed to achieve automatic obstacle avoidance movement and reach the surgical target.

[0160] Optionally, the device further comprises:

[0161] a condition judgment module, used to judge whether the surgical robot satisfies an angle condition, a first distance condition and a second distance condition at the same time; wherein the angle condition means that the angle between the unit vector and the direction vector is less than a preset angle; the first distance condition means that the distance between the surgical robot and the target obstacle is less than an adaptive obstacle avoidance distance; the second distance condition means that the distance between the surgical robot and the target position is greater than the adaptive obstacle avoidance distance;

[0162] The obstacle avoidance condition determination module is used to determine that the surgical robot meets the obstacle avoidance condition if the surgical robot meets the angle condition, the first distance condition and the second distance condition at the same time.

[0163] Optionally, the device further comprises:

[0164] A kinematic model building module, used to build a kinematic equivalent model according to the kinematic parameters of the surgical robot;

[0165] An obstacle model building module is used to build multiple obstacle equivalent models in the surgical environment based on the point cloud data acquired by the visual sensor;

[0166] a distance calculation module, used to calculate the distance between the surgical robot and each obstacle, and the shortest distance point on each obstacle according to the kinematic equivalent model and the multiple obstacle equivalent models; the shortest distance point refers to the point on the surface of the obstacle that is closest to the surgical robot;

[0167] The obstacle screening module is used to screen out the target obstacle from the multiple obstacles according to the distance between the surgical robot and each obstacle.

[0168] Optionally, the obstacle avoidance speed determination module is also used to: construct a normal plane based on the unit vector; the normal plane is perpendicular to the unit vector; project the direction vector onto the normal plane to obtain a projection vector of the direction vector on the normal plane; and determine the obstacle avoidance direction based on the projection vector.

[0169] Optionally, the device further comprises:

[0170] A closest distance calculation module, configured to calculate the closest distance from the target position to the boundary of the obstacle projection surface, and calculate a second direction vector from the target position to the nearest boundary point in the obstacle projection surface, if the position coordinates of the surgical robot, the shortest distance point of the target obstacle and the target position are on the same direction vector after determining that the surgical robot meets the obstacle avoidance condition;

[0171] A virtual target position determination module, configured to obtain a virtual target position based on the closest distance, the second direction vector and the target position;

[0172] The obstacle avoidance module is used to use the virtual target position as a new target position and execute the step of determining the direction vector of the surgical robot to the target position until the surgical robot is controlled by the combined speed to achieve automatic obstacle avoidance movement and reach the surgical target.

[0173] Optionally, the shortest distance calculation module is also used to: calculate the first plane coordinate points of all vertices of the target obstacle on the projection surface; wherein a plurality of first plane coordinate points corresponding to all vertices constitute the boundary of the obstacle projection surface; calculate the second plane coordinate point of the target position on the boundary of the obstacle projection surface; and calculate the shortest distance from the target position to the boundary of the obstacle projection surface based on the plurality of first plane coordinate points and the second plane coordinate points.

[0174] The automatic obstacle avoidance device for a surgical robot provided in the above-mentioned embodiments of the present disclosure and the automatic obstacle avoidance method for a surgical robot provided in the embodiments of the present disclosure are based on the same inventive concept and have the same beneficial effects as the methods adopted, run or implemented by the application programs stored therein.

[0175] The disclosed embodiment also provides an electronic device to implement the automatic obstacle avoidance method of the surgical robot. Fig.16 , which shows a schematic diagram of an electronic device provided by some embodiments of the present disclosure. Fig.16 As shown, the electronic device 16 includes: a processor 1600, a memory 1601, a bus 1602 and a communication interface 1603, wherein the processor 1600, the communication interface 1603 and the memory 1601 are connected via the bus 1602; the memory 1601 stores a computer program that can be run on the processor 1600, and the processor 1600 executes the aforementioned Figures 1 to 14 An automatic obstacle avoidance method for a surgical robot is provided in any of the illustrated embodiments.

[0176] The memory 1601 may include a high-speed random access memory (RAM), and may also include a non-volatile memory (non-volatile memory), such as at least one disk storage. The communication connection between the system network element and at least one other network element is realized through at least one communication interface 1603 (which may be wired or wireless), and the Internet, wide area network, local area network, metropolitan area network, etc. may be used.

[0177] The bus 1602 may be an ISA bus, a PCI bus, or an EISA bus. The bus may be divided into an address bus, a data bus, a control bus, etc. The memory 1601 is used to store programs. After receiving the execution instruction, the processor 1600 executes the program. Figures 1 to 14 The automatic obstacle avoidance method for a surgical robot disclosed in any of the illustrated embodiments may be applied to the processor 1600 or implemented by the processor 1600 .

[0178] The processor 1600 may be an integrated circuit chip with signal processing capabilities. In the implementation process, each step of the above method can be completed by the hardware integrated logic circuit or software instructions in the processor 1600. The above processor 1600 may be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it can also be a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components. The methods, steps and logic block diagrams disclosed in the embodiments of the present disclosure can be implemented or executed. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor. The steps of the method disclosed in conjunction with the embodiments of the present disclosure can be directly embodied as a hardware decoding processor to be executed, or the hardware and software modules in the decoding processor can be executed. The software module can be located in a mature storage medium in the field such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory or an electrically erasable programmable memory, a register, etc. The storage medium is located in the memory 1601, and the processor 1600 reads the information in the memory 1601 and completes the steps of the above method in combination with its hardware.

[0179] The electronic device provided in the embodiment of the present disclosure and the automatic obstacle avoidance method of the surgical robot provided in the embodiment of the present disclosure are based on the same inventive concept and have the same beneficial effects as the methods adopted, operated or implemented therein.

[0180] The present disclosure also provides a computer-readable storage medium corresponding to the automatic obstacle avoidance method of the surgical robot provided in the aforementioned embodiment. Fig.17 The computer-readable storage medium shown is a CD 30 on which a computer program (i.e., a program product) is stored. When the computer program is run by the processor, it will execute the automatic obstacle avoidance method of the surgical robot provided by any of the aforementioned embodiments.

[0181] It should be noted that examples of the computer-readable storage medium may also include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other optical or magnetic storage media, which are not listed here one by one.

[0182] The computer-readable storage medium provided in the above-mentioned embodiments of the present disclosure and the automatic obstacle avoidance method for a surgical robot provided in the embodiments of the present disclosure are based on the same inventive concept and have the same beneficial effects as the methods adopted, run or implemented by the application programs stored therein.

[0183] It should be noted that:

[0184] In the description provided herein, a large number of specific details are described. However, it is understood that the embodiments of the present disclosure can be practiced without these specific details. In some instances, well-known structures and technologies are not shown in detail so as not to obscure the understanding of this description.

[0185] Similarly, it should be understood that in order to streamline the present disclosure and aid in understanding one or more of the various inventive aspects, in the above description of the exemplary embodiments of the present disclosure, the various features of the present disclosure are sometimes grouped together into a single embodiment, figure, or description thereof. However, the disclosed method should not be interpreted as reflecting the following schematic diagram: the claimed disclosure requires more features than the features explicitly recited in each claim. More specifically, as reflected in the claims below, the inventive aspects lie in less than all the features of the single embodiment disclosed above. Therefore, the claims that follow the specific embodiment are hereby expressly incorporated into the specific embodiment, with each claim itself serving as a separate embodiment of the present disclosure.

[0186] In addition, those skilled in the art will appreciate that, although some embodiments described herein include certain features included in other embodiments but not other features, the combination of features of different embodiments is meant to be within the scope of this disclosure and form different embodiments. For example, in the claims below, any one of the claimed embodiments may be used in any combination.

[0187] The above is only a preferred specific implementation of the present disclosure, but the protection scope of the present disclosure is not limited thereto. Any changes or substitutions that can be easily thought of by any technician familiar with the technical field within the technical scope disclosed in the present disclosure should be included in the protection scope of the present disclosure. Therefore, the protection scope of the present disclosure should be based on the protection scope of the claims.

Claims

1. An automatic obstacle avoidance method for a surgical robot, characterized in that: The method comprises: In the process of controlling the surgical robot to move to a target position, if the surgical robot meets the obstacle avoidance condition, the direction vector of the surgical robot to the target position is determined, and the unit vector of the shortest distance point from the surgical robot to the target obstacle is determined; the target obstacle is the obstacle closest to the surgical robot among multiple obstacles; Calculating the obstacle avoidance direction and obstacle avoidance speed of the surgical robot according to the direction vector and the unit vector; Calculating the suction speed of the surgical robot according to the direction vector and the position information of the target position; the suction speed refers to the speed component of the surgical robot pointing to the target position; The combined speed of the surgical robot is calculated according to the obstacle avoidance speed and the attraction speed, and the surgical robot is controlled by the combined speed to achieve automatic obstacle avoidance movement and reach the surgical target.

2. The method according to claim 1, characterized in that The obstacle avoidance condition includes an angle condition, a first distance condition, and a second distance condition; before determining the direction vector of the surgical robot to the target position, the method further includes: Determine whether the surgical robot satisfies an angle condition, a first distance condition, and a second distance condition at the same time; wherein the angle condition means that the angle between the unit vector and the direction vector is less than a preset angle; the first distance condition means that the distance between the surgical robot and the target obstacle is less than an adaptive obstacle avoidance distance; the second distance condition means that the distance between the surgical robot and the target position is greater than the adaptive obstacle avoidance distance; If the surgical robot satisfies the angle condition, the first distance condition, and the second distance condition at the same time, it is determined that the surgical robot satisfies the obstacle avoidance condition.

3. The method according to claim 2, characterized in that The adaptive obstacle avoidance distance is determined based on the shortest distance point of the target obstacle and the target position; the shortest distance point refers to the point on the surface of the obstacle that is closest to the surgical robot.

4. The method according to claim 1 or 2, characterized in that: The method further comprises: Constructing a kinematic equivalent model according to the kinematic parameters of the surgical robot; Establish multiple obstacle equivalent models in the surgical environment based on the point cloud data obtained by the visual sensor; Calculating the distance between the surgical robot and each obstacle, and the shortest distance point on each obstacle according to the kinematic equivalent model and the multiple obstacle equivalent models; the shortest distance point refers to the point on the surface of the obstacle that is closest to the surgical robot; The target obstacle is selected from the plurality of obstacles according to the distance between the surgical robot and each obstacle.

5. The method according to claim 1 or 2, characterized in that: Calculating the obstacle avoidance direction and obstacle avoidance speed of the surgical robot according to the direction vector and the unit vector includes: Construct a normal plane according to the unit vector; the normal plane is perpendicular to the unit vector; Projecting the direction vector onto the normal plane to obtain a projection vector of the direction vector on the normal plane; The obstacle avoidance direction is determined according to the projection vector.

6. The method according to claim 2, characterized in that After determining that the surgical robot meets the obstacle avoidance condition, the method further includes: If the position coordinates of the surgical robot, the shortest distance point of the target obstacle and the target position are on the same direction vector, then calculating the shortest distance from the target position to the boundary of the obstacle projection surface, and calculating a second direction vector from the target position to the nearest boundary point in the obstacle projection surface; Obtaining a virtual target position based on the closest distance, the second direction vector and the target position; The virtual target position is used as a new target position, and the step of determining the direction vector of the surgical robot to the target position is performed until the surgical robot is controlled by the combined speed to achieve automatic obstacle avoidance movement and reach the surgical target.

7. The method according to claim 6, characterized in that Calculating the shortest distance from the target position to the boundary of the obstacle projection surface includes: Calculating the first plane coordinate points of all vertices of the target obstacle on the projection surface; wherein a plurality of first plane coordinate points corresponding to all vertices constitute the boundary of the obstacle projection surface; Calculate a second plane coordinate point of the target position on the boundary of the obstacle projection surface; The shortest distance from the target position to the boundary of the obstacle projection surface is calculated according to the multiple first plane coordinate points and the second plane coordinate points.

8. An automatic obstacle avoidance system for a surgical robot, characterized in that: The device comprises: A vector determination module is used to determine the direction vector of the surgical robot to the target position and the unit vector of the shortest distance point from the surgical robot to the target obstacle during the process of controlling the surgical robot to move to the target position, if the surgical robot meets the obstacle avoidance condition; the target obstacle is the obstacle closest to the surgical robot among multiple obstacles; An obstacle avoidance speed determination module, used to calculate the obstacle avoidance direction and obstacle avoidance speed of the surgical robot according to the direction vector and the unit vector; An attraction speed determination module, used for calculating the attraction speed of the surgical robot according to the direction vector and the position information of the target position; the attraction speed refers to the speed component of the surgical robot pointing to the target position; The automatic obstacle avoidance module is used to calculate the combined speed of the surgical robot according to the obstacle avoidance speed and the attraction speed, and control the surgical robot through the combined speed to achieve automatic obstacle avoidance movement and reach the surgical target.

9. A computer device, characterized in that: include: A memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the automatic obstacle avoidance method of the surgical robot according to any one of claims 1 to 7 by executing the computer instructions.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a computer to execute the automatic obstacle avoidance method for a surgical robot according to any one of claims 1 to 7.

Images