Mechanical arm operation obstacle avoidance method and system based on probe obstacle collection

Abstract

The invention discloses a mechanical arm operation obstacle avoidance method and system based on probe obstacle collection and a real-time collision detection and singular point avoidance mechanism, the risk that a mechanical arm collides with the body surface of a patient in the operation process is remarkably reduced, optimal path planning is achieved through an A * algorithm, collision detection of a Bullet physical engine is combined, and the obstacle avoidance efficiency is improved. And the smoothness and continuity of movement of the mechanical arm are ensured, the precision of the obstacle is ensured while the cost is reduced, and the precision of surgical path planning is ensured through high-precision point cloud collection and 3D model reconstruction.

Description

Technical Field

[0001] The present invention relates to the field of electromechanical equipment, and in particular to a method and system for avoiding obstacles in robotic surgery based on probe acquisition of obstacles. Background Art

[0002] Currently, surgical robots have been widely used to improve surgical accuracy in spinal surgery. However, in practical applications, existing surgical robot systems still face some challenges when handling complex spinal surgeries. Existing path planning systems usually only consider geometric paths, while ignoring the complex structure of the patient's body surface, which easily leads to collisions between the robotic arm and the patient's body. In addition, the existing system lacks an effective singularity avoidance mechanism, which causes the robotic arm to pause or discontinuously move at the singularity point, increasing the risk of surgery. Unless a high-precision depth camera is used to update the obstacle position in real time and reconstruct the model, the depth camera is relatively expensive.

[0003] In summary, a robotic arm surgery obstacle avoidance method and system based on probe acquisition obstacles is needed to solve the deficiencies in the prior art. Summary of the invention

[0004] In view of the deficiencies in the prior art, the present invention provides a robotic arm surgery obstacle avoidance method and system based on probe acquisition of obstacles, aiming to solve the above problems.

[0005] To achieve the above-mentioned purpose, the present invention provides the following technical solutions: a robotic arm surgery obstacle avoidance system based on probe acquisition of obstacles, comprising a point cloud data acquisition module, a data processing module, a 3D model reconstruction module, a fixed-point method adjacent expansion module, an obstacle update module and a path planning and collision detection module; Point cloud data acquisition module, used to collect all surface data of the surgical area to form point cloud data; A data processing module, used for preprocessing the collected point cloud data; 3D model reconstruction module, used to process the smoothed point cloud through the VTK library to generate a 3D surface model of the surgical area; Vertex normal vector expansion module: used to calculate the normal vector of each point in the point cloud, and multiply the calculated normal vector direction by an expansion factor to expand the 3D model; Obstacle update module: used to gradually add the obstacle model of the Coriolis wire during the operation and add it to the previous obstacle model as a new collision detection obstacle; Path planning and collision detection module: used to achieve optimal path planning through the A* algorithm, and combined with the Bullet physics engine for real-time collision detection and singularity avoidance.

[0006] Optionally, the point cloud data acquisition module further includes: Sample point interval control unit: used to reduce the sampling points that are too close to each other according to the distance interval, thereby reducing the amount of data; Noise filtering unit: used to smooth the collected point cloud to make the overall shape of the point cloud complete and accurate.

[0007] Optionally, the 3D model reconstruction module includes: Point cloud connection unit: used to connect each point in the smoothed point cloud data to generate a 3D surface model of the surgical area; Model optimization unit: used to further optimize the generated 3D surface model.

[0008] Optionally, the vertex normal vector expansion module includes: Normal vector calculation unit: used to calculate the normal vector of each point in the point cloud; Expansion processing unit: used to multiply the calculated normal vector direction by an expansion factor and add the result to the original vertex coordinates to expand the 3D model.

[0009] Optionally, the obstacle updating module includes: Coriolis wire model generation unit: used to generate a new Coriolis wire obstacle model after each surgical screw position is completed; Model merging unit: used to add the new Coriolis needle obstacle model to the previous obstacle model as a new collision detection obstacle.

[0010] Optionally, the path planning and collision detection module includes: A* algorithm path planning unit: used to achieve optimal path planning through A* algorithm; Collision detection unit: used to perform real-time collision detection through the Bullet physics engine to make the robot arm move smoothly and continuously.

[0011] A robotic arm surgery obstacle avoidance method based on probe-based obstacle collection, using the robotic arm surgery obstacle avoidance system based on probe-based obstacle collection, comprising the following steps: Step S1: point cloud data collection. The operator holds a probe and continuously touches the part of the patient that needs surgery. The probe head collects one position every short period of time. After covering the entire surface of the surgical area, the collection ends to form point cloud data. Step S2: data processing, pre-processing the collected point cloud data, including reducing the sampling points that are too closely spaced and smoothing the point cloud to remove noise and abnormal points; Step S3: 3D model reconstruction, processing the smoothed point cloud through the VTK library to generate a 3D surface model of the surgical area; Step S4: Vertex normal vector expansion, calculating the normal vector of each point in the point cloud, and multiplying the calculated normal vector direction by an expansion factor to expand the 3D model; Step S5: obstacle update, gradually adding the obstacle model of the Coriolis wire during the operation, and adding it to the previous obstacle model as a new collision detection obstacle; Step S6: Path planning and collision detection: The optimal path planning is achieved through the A* algorithm, and the Bullet physics engine is combined to perform real-time collision detection and singularity avoidance.

[0012] Optionally, the normal vector calculation in step S4 is performed in the following manner: Step S41: Select N domain points and construct them into a matrix; Step S42: Calculate the covariance matrix of the matrix: , , , , In the formula, is the average value of the X coordinates, is the average value of the Y coordinate, is the average value of the Z coordinate, Pj is the coordinate of the surrounding area point cloud of the target point cloud, C: covariance matrix, pj is the jth sample vector, is the mean vector of all sample vectors, T is the transpose operator, is the covariance between random variables a and b, a, b is any pair of x, y, z, aj is the value of variable a in the jth sample; Step S43: perform eigenvalue decomposition on the covariance ratio, and the eigenvector corresponding to the minimum value is the normal vector of the point.

[0013] Optionally, the path planning in step S6 is performed in the following manner: Step S61: adding the starting point to the open list and setting the heuristic function value of the starting point to zero; Step S62: Rotate the node with the lowest total cost from the open list; Step S63: Generate all child nodes for the current node; Step S64: Calculate the heuristic function, perform collision detection on the child nodes, and update the open list until the target node is found or the open list is empty; Step S65: Go through the target node, trace back the path, and generate the optimal path.

[0014] Beneficial effects of the present invention: 1. In the present invention, usually, obstacle collection requires the use of a high-precision depth camera. For a human body shape that remains basically unchanged, the use of a probe can still ensure obstacle accuracy while reducing costs. The accuracy of surgical path planning is ensured through high-precision point cloud collection and 3D model reconstruction.

[0015] 2. In the present invention, safety is enhanced, and the real-time collision detection and singular point avoidance mechanism significantly reduce the risk of collision between the robotic arm and the patient's body surface during surgery. The optimal path planning is achieved through the A* algorithm, combined with the collision detection of the Bullet physics engine to ensure the smoothness and continuity of the robotic arm's movement. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 The present invention is a flow chart of a method.

[0017] Figure 2 A system structure diagram of the present invention. DETAILED DESCRIPTION

[0018] In order to more clearly illustrate the embodiments of the invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0019] like Figure 1 and Figure 2 As shown, a robotic arm surgery obstacle avoidance system based on probe acquisition of obstacles includes a point cloud data acquisition module, a data processing module, a 3D model reconstruction module, a fixed point method adjacent expansion module, an obstacle update module and a path planning and collision detection module; Point cloud data acquisition module, used to collect all surface data of the surgical area to form point cloud data; A data processing module, used for preprocessing the collected point cloud data; 3D model reconstruction module, used to process the smoothed point cloud through the VTK library to generate a 3D surface model of the surgical area; Vertex normal vector expansion module: used to calculate the normal vector of each point in the point cloud, and multiply the calculated normal vector direction by an expansion factor to expand the 3D model; Obstacle update module: used to gradually add the obstacle model of the Coriolis wire during the operation and add it to the previous obstacle model as a new collision detection obstacle; Path planning and collision detection module: used to achieve optimal path planning through the A* algorithm, and combined with the Bullet physics engine for real-time collision detection and singularity avoidance.

[0020] The point cloud data acquisition module also includes: Sample point interval control unit: used to reduce the sampling points that are too close to each other according to the distance interval, thereby reducing the amount of data; Noise filtering unit: used to smooth the collected point cloud to make the overall shape of the point cloud complete and accurate.

[0021] The 3D model reconstruction module includes: Point cloud connection unit: used to connect each point in the smoothed point cloud data to generate a 3D surface model of the surgical area; Model optimization unit: used to further optimize the generated 3D surface model.

[0022] The vertex normal vector expansion module includes: Normal vector calculation unit: used to calculate the normal vector of each point in the point cloud; Expansion processing unit: used to multiply the calculated normal vector direction by an expansion factor and add the result to the original vertex coordinates to expand the 3D model.

[0023] The obstacle update module includes: Coriolis wire model generation unit: used to generate a new Coriolis wire obstacle model after each surgical screw position is completed; Model merging unit: used to add the new Coriolis needle obstacle model to the previous obstacle model as a new collision detection obstacle.

[0024] The path planning and collision detection modules include: A* algorithm path planning unit: used to achieve optimal path planning through A* algorithm; Collision detection unit: used to perform real-time collision detection through the Bullet physics engine to make the robot arm move smoothly and continuously.

[0025] A robotic arm surgery obstacle avoidance method based on probe-based obstacle collection, using the robotic arm surgery obstacle avoidance system based on probe-based obstacle collection, comprising the following steps: Step S1: point cloud data collection. The operator holds a probe and continuously touches the part of the patient that needs surgery. The probe head collects one position every short period of time. After covering the entire surface of the surgical area, the collection ends to form point cloud data. Step S2: data processing, pre-processing the collected point cloud data, including reducing the sampling points that are too closely spaced and smoothing the point cloud to remove noise and abnormal points; Step S3: 3D model reconstruction, processing the smoothed point cloud through the VTK library to generate a 3D surface model of the surgical area; Step S4: Vertex normal vector expansion, calculating the normal vector of each point in the point cloud, and multiplying the calculated normal vector direction by an expansion factor to expand the 3D model; The normal vector is calculated as follows: Step S41: Select N domain points and construct them into a matrix; Step S42: Calculate the covariance matrix of the matrix; Step S43: perform eigenvalue decomposition on the covariance, and the eigenvector corresponding to the minimum value is the normal vector of the point; Step S5: obstacle update, gradually adding the obstacle model of the Coriolis wire during the operation, and adding it to the previous obstacle model as a new collision detection obstacle; Step S6: Path planning and collision detection: A* algorithm is used to achieve optimal path planning, and Bullet physics engine is used to perform real-time collision detection and singular point avoidance. The path planning in step S6 is performed in the following manner: Step S61: adding the starting point to the open list and setting the heuristic function value of the starting point to zero; Step S62: Rotate the node with the lowest total cost from the open list; Step S63: Generate all child nodes for the current node; Step S64: Calculate the heuristic function, perform collision detection on the child nodes, and update the open list until the target node is found or the open list is empty; Step S65: Go through the target node, trace back the path, and generate the optimal path.

[0026] Point cloud data collection and 3D model reconstruction: Point cloud data collection: The operator holds the probe and continuously touches the part of the patient that needs surgery. The probe head collects a position every short period of time. When these positions cover the entire surface of the surgical area, the collection ends and the point cloud data is formed.

[0027] In order to reduce the amount of data collected by the point cloud data, the sampling points with too close intervals are reduced according to the distance interval of the initial point cloud data.

[0028] The collected point cloud may contain noise and abnormal points. To ensure the accuracy of the 3D model, the point cloud needs to be smoothed.

[0029] Smoothing: Gaussian filtering: Use the Gaussian kernel function to perform weighted averaging on the point cloud, smooth out sharp mutation points, and maintain the overall shape of the point cloud. The main principle of Gaussian weighted averaging is to calculate the Gaussian weight w based on each target point cloud and the surrounding field points, and then multiply it by each target point cloud after normalization to achieve a smoothing effect.

[0030] 3D model reconstruction: The smoothed point cloud is processed by the VTK library, each point is connected, and a 3D surface model of the surgical area is generated. This model will be used as the basis for obstacle avoidance during surgery.

[0031] Vertex normal expansion: Normal vector calculation: For each point in the point cloud, use the points in its neighborhood to calculate the normal vector of the point. The specific steps are as follows: Neighborhood selection: Select the k neighboring points closest to the point.

[0032] Covariance matrix: The coordinates of these neighborhood points are constructed into a matrix and the covariance matrix of the matrix is ​​calculated.

[0033] Step S42: Calculate the covariance matrix of the matrix: , In the formula, is the average value of the X coordinates, is the average value of the Y coordinate, is the average Z coordinate, describing the To find the average value.

[0034] , The calculation steps are as follows: Calculate the mean vector of all sample vectors , for each sample vector pj, subtract the mean vector , and get the centered vector .

[0035] Perform the outer product operation on the centered vector with itself, that is , Repeat the above process for all sample vectors and sum the results. Finally, divide the sum by the number of samples n.

[0036] , , In the formula, is the average value of the X coordinates, is the average value of the Y coordinate, is the average value of the Z coordinate, Pj is the coordinate of the surrounding area point cloud of the target point cloud, C: covariance matrix, pj is the jth sample vector, is the mean vector of all sample vectors, T is the transpose operator, is the covariance between random variables a and b, a, b is any pair of x, y, z, aj is the value of variable a in the jth sample, and the covariance matrix C is a 3×3 symmetric matrix.

[0037] Eigenvalue decomposition: Perform eigenvalue decomposition on the covariance matrix (decompose it into eigenvalues ​​and eigenvectors), and the eigenvector corresponding to the minimum eigenvalue is the normal vector of the point.

[0038] Expansion operation: The calculated normal vector direction is multiplied by an expansion factor and the result is added to the original vertex coordinates to achieve the expansion of the 3D model. This process ensures that the robotic arm does not touch the real human body during surgical path planning.

[0039] Obstacle Updates: During the operation, an extra Coriolis wire will be left at each screw position, and the new Coriolis wire does not exist in the initial obstacle model. Therefore, the system will gradually add obstacle models of Coriolis wires according to the order of surgical screws and add them to the last updated obstacle model as new collision detection obstacles.

[0040] Path planning and collision detection: Collision detection in the Bullet physics engine: Collision detection principle: Bullet physics engine uses the generalized separating axis theorem (GJK) to perform collision detection on convex polygons. The specific principles are as follows: Support function: For each object, define a support function that returns the largest point of the object in a specified direction.

[0041] Minkowski difference: By calculating the Minkowski difference of two objects, the collision problem is transformed into the question of whether the origin is in the difference.

[0042] GJK algorithm: gradually build a simple shape (usually a triangle or tetrahedron) and determine whether the shape contains the origin. If it does, it means that the two objects collide.

[0043] Mathematical formula: The support function calculation formula of the GJK algorithm is: , in, is the maximum point of object A in direction d, is a point on A, and d is the detection direction.

[0044] A* algorithm path planning: Initialization: Add the starting point to the open list and set the heuristic function value of the starting point to 0.

[0045] Select a node: Select the node with the lowest total cost (heuristic function value + cost from the starting point) from the open list.

[0046] Generate child nodes: For the current node, generate all possible child nodes (that is, all possible next step positions).

[0047] Calculate the heuristic function: The heuristic function, h(n), is usually calculated using Euclidean distance or Manhattan distance. The formula is as follows:

[0048] Collision detection: Perform collision detection between the generated child node and the obstacle (using the Bullet physics engine). If a collision occurs or the point is at a singular point, the node is discarded.

[0049] Update the open list: If the child node has not been discarded, add it to the open list and update its total cost.

[0050] Repeat: Repeat the above until the target node is found or the open list is empty.

[0051] Path generation: After finding the target node, backtrack the path to generate the final optimal path The present invention generally requires the use of a high-precision depth camera for obstacle collection. For a human body shape that remains basically unchanged, the use of a probe can still ensure obstacle accuracy while reducing costs. The accuracy of surgical path planning is ensured through high-precision point cloud collection and 3D model reconstruction. The real-time collision detection and singular point avoidance mechanism significantly reduce the risk of collision between the robotic arm and the patient's body surface during surgery. The optimal path planning is achieved through the A* algorithm, combined with the collision detection of the Bullet physics engine to ensure the smoothness and continuity of the robotic arm's movement.

[0052] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modification, equivalent substitution or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A robotic arm surgery obstacle avoidance system based on probe acquisition obstacles, characterized in that: It includes point cloud data acquisition module, data processing module, 3D model reconstruction module, fixed-point adjacent expansion module, obstacle update module and path planning and collision detection module; Point cloud data acquisition module, used to collect all surface data of the surgical area to form point cloud data; A data processing module, used for preprocessing the collected point cloud data; 3D model reconstruction module, used to process the smoothed point cloud through the VTK library to generate a 3D surface model of the surgical area; Vertex normal vector expansion module: used to calculate the normal vector of each point in the point cloud, and multiply the calculated normal vector direction by an expansion factor to expand the 3D model; Obstacle update module: used to gradually add the obstacle model of the Coriolis wire during the operation and add it to the previous obstacle model as a new collision detection obstacle; Path planning and collision detection module: used to achieve optimal path planning through the A* algorithm, and combined with the Bullet physics engine for real-time collision detection and singularity avoidance.

2. The robotic arm surgery obstacle avoidance system based on probe obstacle collection according to claim 1, characterized in that: The point cloud data acquisition module also includes: Sample point interval control unit: used to reduce the sampling points that are too close to each other according to the distance interval, thereby reducing the amount of data; Noise filtering unit: used to smooth the collected point cloud to make the overall shape of the point cloud complete and accurate.

3. The robotic arm surgery obstacle avoidance system based on probe obstacle collection according to claim 1, characterized in that: The 3D model reconstruction module includes: Point cloud connection unit: used to connect each point in the smoothed point cloud data to generate a 3D surface model of the surgical area; Model optimization unit: used to further optimize the generated 3D surface model.

4. The robotic arm surgery obstacle avoidance system based on probe obstacle collection according to claim 1, characterized in that: The vertex normal vector expansion module includes: Normal vector calculation unit: used to calculate the normal vector of each point in the point cloud; Expansion processing unit: used to multiply the calculated normal vector direction by an expansion factor and add the result to the original vertex coordinates to expand the 3D model.

5. The robotic arm surgery obstacle avoidance system based on probe obstacle collection according to claim 1, characterized in that: The obstacle updating module comprises: Coriolis wire model generation unit: used to generate a new Coriolis wire obstacle model after each surgical screw position is completed; Model merging unit: used to add the new Coriolis needle obstacle model to the previous obstacle model as a new collision detection obstacle.

6. The robotic arm surgery obstacle avoidance system based on probe obstacle collection according to claim 1, characterized in that: The path planning and collision detection module includes: A* algorithm path planning unit: used to achieve optimal path planning through A* algorithm; Collision detection unit: used to perform real-time collision detection through the Bullet physics engine to make the robot arm move smoothly and continuously.

7. A robotic arm surgery obstacle avoidance method based on probe-based obstacle collection, using the robotic arm surgery obstacle avoidance system based on probe-based obstacle collection as claimed in claim 1, characterized in that: The following steps are involved: Step S1: point cloud data collection. The operator holds a probe and continuously touches the part of the patient that needs surgery. The probe head collects one position every short period of time. After covering the entire surface of the surgical area, the collection ends to form point cloud data. Step S2: data processing, pre-processing the collected point cloud data, including reducing the sampling points that are too closely spaced and smoothing the point cloud to remove noise and abnormal points; Step S3: 3D model reconstruction, processing the smoothed point cloud through the VTK library to generate a 3D surface model of the surgical area; Step S4: Vertex normal vector expansion, calculating the normal vector of each point in the point cloud, and multiplying the calculated normal vector direction by an expansion factor to expand the 3D model; Step S5: obstacle update, gradually adding the obstacle model of the Coriolis wire during the operation, and adding it to the previous obstacle model as a new collision detection obstacle; Step S6: Path planning and collision detection: The optimal path planning is achieved through the A* algorithm, and the Bullet physics engine is combined to perform real-time collision detection and singularity avoidance.

8. The robot arm surgery obstacle avoidance method based on probe acquisition obstacle according to claim 7, characterized in that: The normal vector calculation in step S4 is performed in the following manner: Step S41: Select N domain points and construct them into a matrix; Step S42: Calculate the covariance matrix of the matrix: ， ， ， ， In the formula, is the average value of the X coordinates, is the average value of the Y coordinate, is the average value of the Z coordinate, Pj is the coordinate of the surrounding area point cloud of the target point cloud, C: covariance matrix, pj is the jth sample vector, is the mean vector of all sample vectors, T is the transpose operator, is the covariance between random variables a and b, a, b is any pair of x, y, z, aj is the value of variable a in the jth sample; Step S43: perform eigenvalue decomposition on the covariance ratio, and the eigenvector corresponding to the minimum value is the normal vector of the point.

9. The robot arm surgery obstacle avoidance method based on probe acquisition obstacle according to claim 7, characterized in that: The path planning in step S6 is carried out in the following manner: Step S61: adding the starting point to the open list and setting the heuristic function value of the starting point to zero; Step S62: Rotate the node with the lowest total cost from the open list; Step S63: Generate all child nodes for the current node; Step S64: Calculate the heuristic function, perform collision detection on the child nodes, and update the open list until the target node is found or the open list is empty; Step S65: Go through the target node, trace back the path, and generate the optimal path.

Images