Automatic obstacle avoidance method and system for surgical robot

Abstract

The present disclosure provides an automatic obstacle avoidance method and system for a surgical robot. The method comprises: during the process of controlling the surgical robot to move to a target position, if the surgical robot meets the obstacle avoidance condition, determining a direction vector of the surgical robot to the target position, and determining a unit vector of the shortest distance point of the surgical robot to the target obstacle; calculating the obstacle avoidance direction and speed of the surgical robot according to the direction vector and the unit vector; calculating the attractive speed of the surgical robot according to the direction vector and the pose information of the target position; the attractive speed refers to the speed component of the surgical robot pointing to the target position; calculating the resultant speed of the surgical robot according to the obstacle avoidance speed and the attractive speed, and controlling the surgical robot to realize automatic obstacle avoidance motion and target reaching through the resultant speed. The embodiment of the present disclosure can enable the surgical robot to have autonomous obstacle avoidance capability, and can safely and efficiently realize surgical pose alignment and complete the specified surgical task.

Description

TECHNICAL FIELD

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

[0002] In recent years, surgical navigation systems have been widely used in surgical operations as the most reliable assistants for surgeons to complete surgical tasks. Autonomous and safe arrival of a surgical robot at a surgical position can improve the efficiency of surgical tasks assisted by the surgical robot, and how to safely avoid obstacles is of positive significance for autonomous control of the surgical robot.

[0003] Current surgical robot systems are mainly used in orthopedics, neurosurgery, laparoscopic minimally invasive surgery, and cardiovascular surgery. In these surgeries, the surgical robot is mostly dragged by a skilled surgeon to the surgical target through master-slave teleoperation or cooperative mode. The operation mode of these two types of surgical robots is complex, and long-time surgical operation brings great physical and mental burden to the surgeon. Therefore, if the surgical robot can effectively avoid obstacles autonomously and safely, and automatically reach the desired surgical pose according to the surgical plan, the surgical efficiency can be effectively improved.

[0004] In the traditional auxiliary process, the surgical robot is mainly used for positioning and guidance to provide consistent surgical positioning capability for the surgeon, and the surgeon then holds the surgical tool to perform surgical operations through the end guiding sleeve of the surgical robot, such as positioning before spinal decompression internal fixation surgery, brain biopsy, ablation, drainage, electrode implantation, and other surgical operations in neurosurgery. However, in the alignment process of the surgical robot to automatically reach the surgical pose, the surgical robot needs to have the ability to avoid obstacles in time and safely. In addition, the surgical robot can also be used for automatic tracking of surgical instruments. For example, in a laparoscopic surgery task, the end effector of the surgical robot enters the abdominal cavity environment through a small incision. In this process, the surgical instrument often needs to be tracked in real time by a laparoscope. This requires the snake-shaped manipulator equipped with a laparoscope to have the ability of autonomous obstacle avoidance to avoid collision with important tissues in the abdominal cavity environment while providing a reliable surgical field of view. A reliable and safe real-time reactive automatic obstacle avoidance algorithm can reduce the learning curve and surgical burden of the surgeon on the surgical robot, so that the surgeon can focus on important surgical operations and improve the efficiency of the surgery.

[0005] Most of the existing obstacle avoidance motion planning is for static scenes, such as sampling and search algorithms based on global information, once the environment changes, it needs to resample or search the obstacle avoidance path, which cannot meet the real-time reaction requirements of the autonomous control of the surgical robot. The traditional artificial potential field method can achieve local obstacle avoidance and timely response according to the real-time changes of the environment, but due to the inherent defects of local obstacle avoidance area oscillation, multiple velocity field superposition easy to fall into local minimum, target point unreachable near the obstacle and other defects, it is difficult to meet the obstacle avoidance requirements in the surgical scene. SUMMARY

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

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

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

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

[0010] The attractive speed of the surgical robot is calculated according to the direction vector and the pose information of the target position; the attractive 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 attractive speed, and the automatic obstacle avoidance motion and the surgical target reaching of the surgical robot are realized by controlling the surgical robot through the combined speed.

[0012] The embodiments of the present disclosure calculate the obstacle avoidance speed and the attractive speed, and calculate the combined speed of the surgical robot according to the obstacle avoidance speed and the attractive speed, and realize the automatic obstacle avoidance motion and the surgical target reaching of the surgical robot by controlling the surgical robot through the combined speed, thereby improving the surgical efficiency. In the embodiments of the present disclosure, 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 comprises:

[0013] determine whether the surgical robot meets the angle condition, the first distance condition and the second distance condition simultaneously; the angle condition refers to an included angle between the unit vector and the direction vector being less than a preset angle; the first distance condition refers to a distance between the surgical robot and a target obstacle being less than an adaptive obstacle avoidance distance; and the second distance condition refers to a distance between the surgical robot and the target position being greater than the adaptive obstacle avoidance distance.

[0014] If the surgical robot meets the angle condition, the first distance condition and the second distance condition simultaneously, it is determined that the surgical robot meets the obstacle avoidance condition.

[0015] In the embodiments of the present disclosure, the adaptive obstacle avoidance distance is determined according to the shortest distance point of the target obstacle and the target position; the shortest distance point refers to a point on the surface of the obstacle closest to the surgical robot.

[0016] The obstacle avoidance distance in the embodiments of the present disclosure is determined according to the shortest distance point of the target obstacle and the target position, and is adaptively adjusted; the target unreachability caused by the constant distance of the traditional active obstacle avoidance function and the motion oscillation phenomenon that may occur when processing local obstacle avoidance can be effectively avoided.

[0017] In the embodiments of the present disclosure, the method further comprises:

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

[0019] establishing equivalent models of a plurality of obstacles in a surgical environment according to point cloud data obtained by a visual sensor;

[0020] calculating distances between the surgical robot and each obstacle and the shortest distance point on each obstacle according to the kinematic equivalent model and the equivalent models of the plurality of obstacles; the shortest distance point refers to a point on the surface of the obstacle closest to the surgical robot;

[0021] selecting the target obstacle from the plurality of obstacles according to the distances between the surgical robot and each obstacle.

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

[0023] constructing 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] determine the obstacle avoidance direction according to the projection vector.

[0026] In the embodiments of the present disclosure, after it is determined that the surgical robot meets the obstacle avoidance condition, the method further comprises:

[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, the closest distance of the target position to the boundary of the obstacle projection plane is calculated, and a second direction vector of the target position to the closest boundary point in the obstacle projection plane is calculated;

[0028] Based on the closest distance, the second direction vector, and the target position, a virtual target position is obtained;

[0029] The virtual target position is taken 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 automatic obstacle avoidance movement of the surgical robot and the reaching of the surgical target are achieved through the combined velocity control.

[0030] In the embodiments of the present disclosure, calculating the closest distance of the target position to the boundary of the obstacle projection plane comprises:

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

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

[0033] According to the plurality of first plane coordinate points and the second plane coordinate point, the closest distance of the target position to the boundary of the obstacle projection plane is calculated.

[0034] The embodiments of the second aspect of the present disclosure provide an automatic obstacle avoidance system of a surgical robot, comprising:

[0035] A vector determination module is configured to, in the process of controlling the surgical robot to move to a target position, if the surgical robot meets an obstacle avoidance condition, determine a direction vector of the surgical robot to the target position, and determine a unit vector of a shortest distance point of the surgical robot to a target obstacle; the target obstacle is an obstacle closest to the surgical robot among a plurality of obstacles;

[0036] An obstacle avoidance velocity determination module is configured to calculate an obstacle avoidance direction and an obstacle avoidance velocity of the surgical robot according to the direction vector and the unit vector;

[0037] An attraction speed determination module is configured to calculate an attraction speed of the surgical robot according to the direction vector and the pose information of the target position. The attraction speed refers to a speed component of the surgical robot pointing to the target position.

[0038] An automatic obstacle avoidance module is configured to calculate a resultant speed of the surgical robot according to the obstacle avoidance speed and the attraction speed, and to control the surgical robot to realize automatic obstacle avoidance movement and surgical target reaching through the resultant speed.

[0039] Embodiments of the third aspect of the present disclosure provide an electronic device, which comprises a memory and a processor in communication connection with each other, the memory stores computer instructions, and the processor executes the computer instructions to perform the automatic obstacle avoidance method of the surgical robot of the first aspect.

[0040] Embodiments of the fourth aspect of the present disclosure provide a computer readable storage medium, which stores computer instructions for causing a computer to perform the automatic obstacle avoidance method of the surgical robot of the first aspect.

[0041] Additional aspects and advantages of the present disclosure will be made apparent from the following description of the preferred embodiments. BRIEF DESCRIPTION OF DRAWINGS

[0042] Various other advantages and benefits will become apparent to those of ordinary skill in the art upon reading the following detailed description of the preferred embodiments. The accompanying drawings are included to provide a description of the preferred embodiments and are not intended to limit the scope of the present disclosure. Moreover, the same reference numerals are used throughout the same figures to designate similar or equivalent components.

[0043] In the drawings:

[0044] Figure 1 A flowchart of an automatic obstacle avoidance method of a surgical robot is shown according to an embodiment of the present disclosure;

[0045] Figure 2 A principle diagram of an automatic obstacle avoidance method of a surgical robot is shown according to an embodiment of the present disclosure;

[0046] Figure 3 An obstacle avoidance principle diagram requiring addition of a virtual surgical target is shown according to an embodiment of the present disclosure;

[0047] Figure 4 An ellipsoid wrapping convex equivalent model diagram for simplifying a surgical robot based on URDF is shown according to an embodiment of the present disclosure;

[0048] Figure 5 A schematic diagram of an obstacle avoidance path provided by the verification obstacle avoidance scheme is shown;

[0049] Figure 6 A schematic diagram of autonomous obstacle avoidance in a single obstacle environment is shown;

[0050] Figure 7 A top view schematic diagram of autonomous obstacle avoidance in a multi-obstacle environment is shown;

[0051] Figure 8 A front view schematic diagram of autonomous obstacle avoidance in a multi-obstacle environment is shown;

[0052] Figure 9 A schematic diagram of obstacle avoidance and target reaching when both the obstacle and the target position are dynamically changing is shown;

[0053] Figure 10 A schematic diagram of autonomous obstacle avoidance in a multi-obstacle environment with a snake-shaped structure feature is shown;

[0054] Figure 11 A schematic diagram of a situation in which a virtual surgical target needs to be added when the obstacle is simplified as an equivalent convex block model structure is shown;

[0055] Figure 12 A schematic diagram of another situation in which a virtual surgical target needs to be added when the obstacle is simplified as an equivalent convex block model structure is shown;

[0056] Figure 13 A schematic diagram of another situation in which a virtual surgical target needs to be added when the obstacle is simplified as an equivalent convex block model structure is shown;

[0057] Figure 14 A schematic diagram of another situation in which a virtual surgical target needs to be added when the obstacle is simplified as an equivalent convex block model structure is shown;

[0058] Figure 15 A schematic diagram of an automatic obstacle avoidance device of a surgical robot is shown;

[0059] Figure 16 A schematic diagram of an electronic device is shown;

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

[0061] Exemplary embodiments of the present disclosure will be described in detail with reference to the drawings. Although exemplary embodiments of the present disclosure are shown in the drawings, it is understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. Rather, these embodiments are provided so that the present disclosure can be more thoroughly understood, and the scope of the present disclosure can be accurately conveyed to those skilled in the art.

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

[0063] According to an embodiment of the present disclosure, 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 herein can be executed in an order different from that shown herein.

[0064] An automatic obstacle avoidance method for a surgical robot is provided in the present embodiment, Figure 1 is a flowchart of an automatic obstacle avoidance method for a surgical robot according to an embodiment of the present disclosure, as Figure 1 shown, the flow includes the following steps:

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

[0066] In an embodiment of the present disclosure, 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 model capable of wrapping the obstacle body established by a point cloud.

[0067] In the following embodiments, the kinematic equivalent model of the surgical robot is characterized by the surgical robot, the obstacle equivalent model is characterized by the obstacle, and the target obstacle equivalent model is characterized by the target obstacle, which will not be described again.

[0068] In the embodiments of the present disclosure, 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 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] wherein V Target2T_tcp represents the direction vector, P target represents the coordinate of the target position (for example, the "target point" in Figure 2 ), and P T_tcp represents the coordinate of the position of the surgical robot (for example, the "end effector center point" in Figure 2 ).

[0071] In the embodiments of the present disclosure, the unit vector of the surgical robot to the shortest distance point of the target obstacle can be determined by steps a1-a3:

[0072] Step a1: selecting the obstacle closest to the surgical robot from the plurality of obstacles in the surgical environment as the target obstacle.

[0073] Step a2: determining the point closest to the surgical robot on the surface of the target obstacle as the shortest distance point (for example, the "closest point" in Figure 2 ) of the target obstacle.

[0074] Step a3: determining 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 by the following expression:

[0075]

[0076] wherein V RO represents the unit vector of the end effector of the surgical robot to the shortest distance point of the target obstacle, P T_tcp represents the coordinate of the position of the surgical robot, and O minp represents the coordinate of the position of the shortest distance point of the target obstacle (for example, the "closest point" in Figure 2 ).

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

[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 simultaneously satisfies the angle condition, the first distance condition and the second distance condition, it is determined that the surgical robot satisfies the obstacle avoidance condition.

[0080] In the embodiments of the present disclosure, the obstacle avoidance condition is as shown in the following expression:

[0081]

[0082] wherein, represents the angle condition, which means that the included angle between the unit vector and the direction vector is less than a preset angle pi / 2, wherein pi represents radian, 180 degrees.

[0083] D O <D act represents the first distance condition, which means that the distance D O between the surgical robot and the shortest distance point of the target obstacle is less than an adaptive obstacle avoidance distance D act .

[0084] norm(P target -P T_tcp )>D act represents the second distance condition, which means that the distance between the coordinate position P T_tcp of the surgical robot and the target position P target 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, first distance condition and second distance condition are simultaneously satisfied, it can be determined that the surgical robot satisfies the obstacle avoidance condition.

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

[0087]

[0088] wherein, the function dot is the dot product operation of vectors, and the solution of Angle can ensure that the robot end effector directly moves towards the surgical target position when there is no need to avoid obstacles.

[0089] In some specific embodiments, the above D O and D act may 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] wherein, D act represents an adaptive obstacle avoidance distance, O min p represents the position coordinates of the shortest distance point of the target obstacle. D O represents the distance between the surgical robot and the shortest distance point of the target obstacle. In this way, when the distance D O between the surgical robot and the shortest distance point of the target obstacle is less than the adaptive obstacle avoidance distance D act , the end of the surgical robot starts to activate the obstacle avoidance function. In this way, the obstacle closest to the end of the surgical robot is taken as the main obstacle avoidance consideration object, which can ensure that the robot will not collide with the closest obstacle during movement, and reduce the situation of falling into local minimum value caused by superposition of traditional multiple obstacle avoidance velocity fields and failing to avoid obstacles. In addition, the adaptive obstacle avoidance distance can avoid the problem of inaccessible 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, connecting rods and other physical properties of the robot, for example: various components of the robot, including physical properties such as shape, size, mass, inertia, etc.; the connection relationship and movement mode between connecting rods, including revolute, prismatic, fixed, etc.

[0096] Step b2, establishing multiple obstacle equivalent models in the surgical environment according to the point cloud data obtained by the vision 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 surface of the obstacle 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 embodiments of the present disclosure, the robot model generated by the kinematic parameter URDF (i.e., the above-mentioned kinematic model) needs to be enveloped by an ellipsoid according to the coordinate transformation relationship and the size of the robot model to generate an obstacle avoidance model of the robot body. At the same time, the obstacle point cloud of the surgical environment obtained by the visual sensor generates a corresponding simplified convex obstacle model according to its geometric characteristics, so as to facilitate the nearest point calculation of the obstacle model and the subsequent obstacle avoidance planning by the collision detection function.

[0100] In step S102, the obstacle avoidance direction and the obstacle avoidance speed of the surgical robot are calculated 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] wherein 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 represents the unit vector of the shortest distance point of the end effector of the surgical robot to the target obstacle, V Target2T_tcp represents the direction vector.

[0104] In the embodiments of the present disclosure, the confirmation idea of the obstacle avoidance speed direction is that the direction vector of the end effector of the surgical robot to the target point of the surgery is projected onto the shortest distance point on the target obstacle and the normal plane of the straight line between the robot end effector, for example: first, a normal plane is constructed according to the unit vector; the normal plane is perpendicular to the unit vector; second, 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 according to the projection vector.

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

[0106]

[0107] wherein K r is the obstacle avoidance speed control gain, V avo represents the obstacle avoidance speed.

[0108] In step S103, the attractive speed of the surgical robot is calculated according to the direction vector and the pose information of the target position. The attractive speed refers to the speed component of the surgical robot pointing to the target position.

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

[0110]

[0111] wherein V att is the attraction speed based on the surgical target position, K a is the attraction speed control gain for the size control of the end speed of the surgical robot. P target is the Cartesian target surgical position determined by the navigation system, P T_tcp is the end point of the robot end effector, which is calculated by the forward kinematics of the robot and position error compensated by the information tracked by the navigation system.

[0112] Step S104, calculating the velocity 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 reaching through the velocity.

[0113] In some embodiments, the obstacle avoidance function of the surgical robot is only used when the obstacle avoidance condition is met, and in order to ensure the stability of the movement speed of the robot end, the velocity is calculated as follows:

[0114]

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

[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, calculating the nearest distance of the target position to the boundary of the obstacle projection plane, and calculating the second direction vector of the target position to the nearest boundary point in the obstacle projection plane.

[0117] In some embodiments, when the included angle Angle between the unit vector and the direction vector is less than the angle threshold θ t , and the surgical target position is in the plane surrounded by the obstacle orthographic 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 embodiments, calculating the nearest distance of the target position to the boundary of the obstacle projection plane comprises:

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

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

[0121] Step d3, calculating the nearest distance from the target position to the boundary of the obstacle projection plane according to the plurality of first planar coordinate points and the second planar coordinate point.

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

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

[0124] wherein 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 as the projection direction, and the coordinate point calculation formula of the point in the three-dimensional space on the projection plane is determined as:

[0125]

[0126] The nearest point and distance of the surgical target point P target projected to the projection plane to the boundary line of the obstacle projected to the projection plane are calculated, assuming that Q2d is the planar coordinate of all the vertices O i of the obstacle on the projection plane, then the specific formula of the nearest point P closet and the distance dist is:

[0127]

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

[0129] wherein i represents the sequence of the obstacle vertex, the lower right subscript of Q2d is the row sequence of all the points of the obstacle vertex on the projection plane, and N is the point vector of the obstacle vertex O i arranged in a counterclockwise cycle around the formed convex hull. p2d is the planar coordinate point of the surgical target point P target on the projection plane, which is determined according to the coordinate point calculation formula of the point in the three-dimensional space on the projection plane. t is the proportion parameter of the point on the obstacle projection boundary line segment, and the calculation formula is as follows:

[0130]

[0131] Finally, t = max(0, min(1, t)), which ensures that t is within the range of the edge line segment of the vertex of the obstacle on the projection plane.

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

[0133]

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

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

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

[0137] Where P ntarget is the virtual surgical target position, s is the distance scaling factor for adjusting the position of the virtual surgical target, dist is the closest distance from the original surgical target point to the projection boundary of the obstacle, and P ro_dir is the second direction vector of the original surgical target position to the closest boundary point in the projection plane of the obstacle.

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

[0139]

[0140] Where Q T is the coordinate point of the surgical target point to the projection plane. The co-speed of the end effector of the surgical robot 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, performing the step of determining the direction vector of the surgical robot to the target position until the automatic obstacle avoidance motion of the surgical robot and the reaching of the surgical target are achieved by the co-speed control.

[0142] In the embodiments of the present disclosure, in order to avoid the case that the end effector coordinate P T_tcp , the shortest distance point O minp on the target obstacle and the target position P target are on the same direction vector, and the mirror symmetry occurs based on the projection to determine the obstacle avoidance direction, the method of introducing the projection can be used to determine the virtual surgical target position, which can effectively avoid the failure of obstacle avoidance.

[0143] In some specific embodiments, the automatic obstacle avoidance method provided by the present disclosure is not only suitable for autonomous obstacle avoidance real-time responsiveness control of the end effector of the mechanical arm, such as obstacle avoidance safety arrival in surgical positioning, autonomous tracking of surgical instruments in laparoscopic surgery, and other applications; but also suitable for mobile robots, and situations requiring real-time responsiveness obstacle avoidance capability in the field of aircraft.

[0144] In some specific embodiments, the present disclosure also provides an ellipsoid wrapping convex equivalent model for simplifying a surgical robot 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 an obstacle avoidance scheme, for example Figure 5 as shown;

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

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

[0148] In some specific embodiments, the present disclosure also provides an obstacle avoidance and target arrival situation in which both the obstacle and the arrival target position are dynamically changing, for example Figure 9 as shown.

[0149] In some specific embodiments, the present disclosure also provides a robot configuration with a snake-shaped structure feature in a multi-obstacle environment to automatically avoid obstacles, for example Figure 10 as shown.

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

[0151] The present application calculates the attractive speed V att and the obstacle avoidance speed V avo of the surgical robot, which can make the speed obstacle avoidance planning of the end effector of the surgical robot have better obstacle avoidance guiding ability, and can also avoid the target unreachability and motion shock phenomenon that may occur when processing local obstacle avoidance due to the constant distance of traditional active obstacle avoidance function, and other innovative advantages.

[0152] The beneficial effects of the present application are that: (1) the proposed obstacle avoidance speed method is mainly described in task space, but it cannot be denied that the automatic obstacle avoidance method and system of the surgical robot proposed in the embodiment of the present disclosure can also realize the link obstacle avoidance of the joint space of the surgical robot using similar ideas, so that the obstacle avoidance of the robot is more space target-oriented and has better obstacle avoidance ability. (2) The present application can effectively overcome the problems of target unreachability and local obstacle avoidance area motion oscillation that may occur in the traditional artificial potential field vector obstacle avoidance algorithm, and the collision-free path planned has the characteristics of shorter path and smoother motion to a certain extent.

[0153] Although the present application takes the obstacle avoidance speed planning of the surgical robot as the main obstacle avoidance application case, it does not limit the present application, and the innovative method is also applicable to the field of mobile robots, ship navigation, aircraft navigation and other fields that require real-time reactive obstacle avoidance ability. The principle and idea of the proposed automatic obstacle avoidance method and system of the surgical robot can be used by any person skilled in the art to make possible changes and modifications to the technical solutions of the present application without departing from the spirit and scope of the present application, therefore, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present application, which does not deviate from the technical solutions of the present application, all belong to the protection scope of the technical solutions of the present application.

[0154] The contents not described in detail in the specification of the present application belong to the known technology of those skilled in the art.

[0155] Corresponding to the implementation mode of the above automatic obstacle avoidance method of the surgical robot, the present disclosure also provides an automatic obstacle avoidance device of a surgical robot for executing the above Figures 1-14 The automatic obstacle avoidance method of the surgical robot of any of the embodiments is shown in the schematic. As Figure 15 shown, the automatic obstacle avoidance device of the surgical robot comprises:

[0156] A vector determination module is configured to determine a direction vector of the surgical robot to the target position and a unit vector of the shortest distance point of the surgical robot to the target obstacle if the surgical robot meets the obstacle avoidance condition during the movement of the surgical robot to the target position; the target obstacle is the obstacle closest to the surgical robot among the plurality of obstacles;

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

[0158] An attraction speed determination module is configured to calculate an attraction speed of the surgical robot according to the direction vector and the pose information of the target position; the attraction speed refers to a speed component of the surgical robot pointing to the target position;

[0159] An automatic obstacle avoidance module is configured to calculate a resultant speed of the surgical robot according to the obstacle avoidance speed and the attraction speed, and to control the surgical robot to realize automatic obstacle avoidance movement and surgical target arrival through the resultant speed.

[0160] Optionally, the device further comprises:

[0161] A condition judgment module is configured to judge whether the surgical robot simultaneously satisfies an angle condition, a first distance condition and a second distance condition; the angle condition refers to an included angle between the unit vector and the direction vector being less than a preset angle; the first distance condition refers to a distance between the surgical robot and a target obstacle being less than an adaptive obstacle avoidance distance; and the second distance condition refers to a distance between the surgical robot and the target position being greater than the adaptive obstacle avoidance distance.

[0162] An obstacle avoidance condition determination module is configured to determine that the surgical robot satisfies the obstacle avoidance condition if the surgical robot simultaneously satisfies the angle condition, the first distance condition and the second distance condition.

[0163] Optionally, the device further comprises:

[0164] A kinematic model construction module is configured to construct a kinematic equivalent model according to kinematic parameters of the surgical robot;

[0165] An obstacle model establishment module is configured to establish a plurality of obstacle equivalent models in a surgical environment according to point cloud data acquired by a vision sensor;

[0166] A distance calculation module is configured to calculate distances between the surgical robot and each obstacle, and shortest distance points on each obstacle according to the kinematic equivalent model and the plurality of obstacle equivalent models; the shortest distance point refers to a point on a surface of the obstacle closest to the surgical robot;

[0167] An obstacle screening module is configured to screen the target obstacle from the plurality of obstacles according to the distances between the surgical robot and each obstacle.

[0168] Optionally, the obstacle avoidance speed determination module is further configured to: construct a normal plane according to 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 according to the projection vector.

[0169] Optionally, the device further comprises:

[0170] a nearest distance calculation module, configured to, after determining that the surgical robot meets the obstacle avoidance condition, 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, calculate the nearest distance from the target position to the boundary of the obstacle projection plane, and calculate a second direction vector of the target position to the nearest boundary point in the obstacle projection plane;

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

[0172] an obstacle avoidance module, configured to take the virtual target position as a new target position, perform the step of determining the direction vector of the surgical robot to the target position, until the automatic obstacle avoidance movement and the reaching of the surgical target are achieved by controlling the surgical robot through the resultant velocity.

[0173] Optionally, the nearest distance calculation module is further configured to: calculate first plane coordinate points of all vertices of the target obstacle on the projection plane; wherein a plurality of first plane coordinate points corresponding to all vertices form the boundary of the obstacle projection plane; calculate a second plane coordinate point of the target position on the boundary of the obstacle projection plane; and calculate the nearest distance from the target position to the boundary of the obstacle projection plane according to the plurality of first plane coordinate points and the second plane coordinate point.

[0174] The automatic obstacle avoidance device of the surgical robot provided by the above embodiments of the present disclosure and the automatic obstacle avoidance method of the surgical robot provided by the embodiments of the present disclosure have the same beneficial effects as the method adopted, run or implemented by the application program stored therein.

[0175] The embodiments of the present disclosure further provide an electronic device to execute the automatic obstacle avoidance method of the surgical robot. Please refer to Figure 16 which shows a schematic diagram of an electronic device provided by some embodiments of the present disclosure. As Figure 16 shown, the electronic device 16 comprises a processor 1600, a memory 1601, a bus 1602 and a communication interface 1603, the processor 1600, the communication interface 1603 and the memory 1601 are connected through the bus 1602; the memory 1601 stores a computer program executable on the processor 1600, and the processor 1600 executes the computer program to perform the automatic obstacle avoidance method of the surgical robot provided by any of the embodiments of the present disclosure. Figures 1-14 schematic diagram of any of the embodiments of the present disclosure.

[0176] The memory 1601 can include a random access memory (RAM) and can further include a non-volatile memory, such as at least one disk memory. 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 can be wired or wireless), and the Internet, a wide area network, a local area network, a metropolitan area network, etc. can be used.

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

[0178] The processor 1600 can be an integrated circuit chip with a signal processing capability. In the implementation process, each step of the above method can be completed by an integrated logic circuit of hardware in the processor 1600 or an instruction in the form of software. The processor 1600 described above can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; can also be a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a ready programmable gate array (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components. Each method, step and logic block diagram 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 combination with the embodiments of the present disclosure can be directly embodied as a hardware code processor for execution, or a combination of hardware and software modules in the code processor for execution. The software module can be located in a random access memory, a flash memory, a read-only memory, a programmable read-only memory, an electrically erasable programmable memory, a register, or other mature storage media in the art. The storage medium is located in the memory 1601, and the processor 1600 reads the information in the memory 1601 and combines the hardware to complete the steps of the above method.

[0179] The electronic device provided by the embodiments of the present disclosure and the automatic obstacle avoidance method of the surgical robot provided by the embodiments of the present disclosure have the same beneficial effects as the methods they employ, run or implement.

[0180] The disclosure embodiments further provide a computer readable storage medium corresponding to the automatic obstacle avoidance method of the surgical robot provided by the foregoing embodiments, please refer to Figure 17 The computer readable storage medium shown in the figure is an optical disc 30, and a computer program (i.e., a program product) is stored on the optical disc 30. When the computer program is run by a processor, the automatic obstacle avoidance method of the surgical robot provided by any of the foregoing embodiments is executed.

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

[0182] The computer readable storage medium provided by the foregoing embodiments of the disclosure has the same beneficial effects as the method adopted, run or implemented by the application program stored therein, for the same inventive concept as the automatic obstacle avoidance method of the surgical robot provided by the embodiments of the disclosure.

[0183] It should be noted that:

[0184] In the specification provided herein, a large number of specific details are described. However, it can be understood that the embodiments of the disclosure can be practiced without these specific details. In some examples, well-known structures and techniques are not shown in detail in order not to obscure the understanding of the present specification.

[0185] Similarly, it should be understood that, in order to simplify the disclosure and help understand one or more of the various inventive aspects, in the above description of the exemplary embodiments of the disclosure, various features of the disclosure are sometimes grouped together into a single embodiment, figure, or description thereof. However, the method of the disclosure should not be interpreted as reflecting the following schematic diagram: that is, the claimed disclosure requires more features than the features explicitly recorded in each claim. More precisely, as reflected in the following claims, the inventive aspects are less than all the features of the foregoing single embodiment. Therefore, the claims following the specific embodiments are hereby expressly incorporated into the specific embodiments, wherein each claim itself is a separate embodiment of the disclosure.

[0186] Furthermore, those skilled in the art will recognize that, in the following claims, the singular form "a" and "the" include plural references unless the context clearly dictates otherwise. As such, the claims following depend from claim 1 should be interpreted as including the plural forms as well.

[0187] The above description is only the preferred specific embodiments of the present disclosure, but the protection scope of the present disclosure is not limited thereto, and any changes or substitutions within the technical scope disclosed by the present disclosure can be easily thought of by those skilled in the art, which should be covered within the protection scope of the present disclosure. Therefore, the protection scope of the present disclosure should be subject to the protection scope of the claims.

Claims

1. An automatic obstacle avoidance method for a surgical robot, characterized by, The method comprises: During the process of controlling the surgical robot to move to a target position, if the surgical robot meets an obstacle avoidance condition, a direction vector of the surgical robot to the target position is determined, and a unit vector of a shortest distance point of the surgical robot to a target obstacle is determined; the target obstacle is an obstacle closest to the surgical robot among multiple obstacles; An obstacle avoidance direction and an obstacle avoidance speed of the surgical robot are calculated according to the direction vector and the unit vector; An attractive speed of the surgical robot is calculated according to the direction vector and pose information of the target position; the attractive speed refers to a speed component of the surgical robot pointing to the target position; A resultant speed of the surgical robot is calculated according to the obstacle avoidance speed and the attractive speed, and the surgical robot is controlled to realize automatic obstacle avoidance movement and surgical target reaching through the resultant speed; After determining that the surgical robot meets the obstacle avoidance condition, the method further comprises: 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, a nearest distance of the target position to a boundary of an obstacle projection plane is calculated, and a second direction vector of the target position to a nearest boundary point in the obstacle projection plane is calculated; A virtual target position is obtained based on the nearest distance, the second direction vector and the target position; The virtual target position is taken as a new target position, and the step of determining the direction vector of the surgical robot to the target position is executed until the surgical robot is controlled to realize automatic obstacle avoidance movement and the surgical target reaching through the resultant speed; The nearest distance of the target position to the boundary of the obstacle projection plane comprises: First plane coordinate points of all vertices of the target obstacle on the projection plane are calculated; wherein a plurality of first plane coordinate points corresponding to all vertices form a boundary of the obstacle projection plane; A second plane coordinate point of the target position on the boundary of the obstacle projection plane is calculated; The nearest distance of the target position to the boundary of the obstacle projection plane is calculated according to the plurality of first plane coordinate points and the second plane coordinate point.

2. The method of claim 1, wherein, The obstacle avoidance condition comprises 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 comprises: It is judged whether the surgical robot meets the angle condition, the first distance condition and the second distance condition at the same time; wherein the angle condition refers to an included angle between the unit vector and the direction vector being less than a preset angle; the first distance condition refers to a distance between the surgical robot and the target obstacle being less than an adaptive obstacle avoidance distance; the second distance condition refers to a distance between the surgical robot and the target position being greater than the adaptive obstacle avoidance distance; If the surgical robot meets the angle condition, the first distance condition and the second distance condition at the same time, it is determined that the surgical robot meets the obstacle avoidance condition.

3. The method of claim 2, wherein, The adaptive obstacle avoidance distance is determined according to 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 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; establishing equivalent models of a plurality of obstacles in the surgical environment according to point cloud data obtained by the visual sensor; calculating distances between the surgical robot and each obstacle and the shortest distance point on each obstacle according to the kinematic equivalent model and the equivalent models of the plurality of obstacles; the shortest distance point refers to the point on the surface of the obstacle closest to the surgical robot; selecting the target obstacle from the plurality of obstacles according to the distances 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 speed of the surgical robot according to the direction vector and the unit vector, comprising: constructing 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; determining the obstacle avoidance direction according to the projection vector.

6. An automatic obstacle avoidance system of a surgical robot, characterized by, The system performs the automatic obstacle avoidance method of the surgical robot according to any one of claims 1 to 5; the system comprises: a vector determination module configured to, during movement of the surgical robot to a target position, determine a direction vector of the surgical robot to the target position and a unit vector of a shortest distance point of the surgical robot to a target obstacle if the surgical robot meets an obstacle avoidance condition; the target obstacle is the obstacle closest to the surgical robot among a plurality of obstacles; an obstacle avoidance speed determination module configured to calculate an obstacle avoidance direction and speed of the surgical robot according to the direction vector and the unit vector; an attractive speed determination module configured to calculate an attractive speed of the surgical robot according to the direction vector and pose information of the target position; the attractive speed refers to a speed component of the surgical robot pointing to the target position; an automatic obstacle avoidance module configured to calculate a resultant speed of the surgical robot according to the obstacle avoidance speed and the attractive speed, and control the surgical robot to realize automatic obstacle avoidance movement and surgical target reaching through the resultant speed.

7. A computer device, comprising: comprise: a memory and a processor, which are communicatively connected to each other, and the memory stores computer instructions; the processor executes the computer instructions to perform the automatic obstacle avoidance method of the surgical robot according to any one of claims 1 to 5.

8. A computer-readable storage medium, characterized in that, The computer readable storage medium stores computer instructions for causing a computer to perform the automatic obstacle avoidance method of the surgical robot according to any one of claims 1 to 5.

Images