Medical robot mechanical arm path optimization method and system

Abstract

The invention relates to a medical robot mechanical arm path optimization method and system, belongs to the field of mechanical arm path optimization, and particularly comprises the steps that a working space is divided, soft constraint weights are distributed for all areas, an initial probability route map node set is constructed in a biased mode based on the soft constraint weights, and an initial global path is obtained through searching in combination with the soft constraint weights; calculating the priority according to the curvature of the path section and the soft constraint weight of the crossing area, executing inert collision detection according to a high-priority priority sequence, if collision is detected, only performing local re-planning on the collision section to generate a replacement section, updating the path, and then executing priority collision detection again until the path is completely free of collision; and converting the collision-free global path into a time sequence elastic band, mapping the soft constraint weight of the path segment into a soft constraint band of a pose point, and optimizing the form by using the soft constraint band to finally obtain an execution path.

Description

Technical Field

[0001] The present invention relates to the field of robotic arm path optimization, and specifically to a method and system for optimizing the path of a robotic arm of a medical robot. Background Art

[0002] Medical robot technology has shown great application potential in multiple fields such as surgical assistance, diagnostic imaging, rehabilitation therapy, drug delivery, etc. Especially in scenarios such as minimally invasive surgery and telemedicine, the robotic arm of a medical robot, with its advantages of high precision, high stability, repeatable operation, and the ability to filter out human hand tremors, has expanded the operating capabilities of doctors, reduced surgical risks, and improved treatment effects. The path planning of a robotic arm is to plan a collision-free, smooth, and efficient motion trajectory for the end effector of the robotic arm from the starting pose to the target pose in a complex and dynamically changing workspace. However, the working environment of a robotic arm of a medical robot usually has a high degree of complexity and uncertainty. For example, in a surgical scenario, there are not only static obstacles such as human tissues, organs, and surgical instruments in the workspace, but also dynamically changing factors such as the patient's respiratory movement and the temporary intervention of other medical devices. In addition, in addition to the basic requirement of collision-free, the path planning of a medical robot also needs to consider many additional constraints and optimization goals. For example, although some areas are not absolute physical obstacles, they may not be suitable to approach because they are close to important nerves and blood vessels, key tissues, or there is an infection risk. Such areas can be regarded as soft constraint areas. Traditional path planning methods often focus on finding the shortest path or the fastest path, and have limited ability to express and handle soft constraints, making it difficult to effectively avoid potential risk areas at the initial stage of planning. How to balance the smoothness, safety, and preference for the operating area of the path is also a problem that needs to be further solved by the existing technology. Summary of the Invention

[0003] To solve the above problems, the present invention provides a method for optimizing the path of a robotic arm of a medical robot, the method comprising the following steps: Obtain the workspace of the robotic arm of the medical robot, divide the workspace into different operating areas, and assign soft constraint weights to each area; obtain the node set of the initial probabilistic roadmap based on the soft constraint weights, and obtain the initial global path according to the node set and the soft constraint weights; Perform priority marking based on the curvature of each path segment of the initial global path and the soft constraint weights of the operating areas passed through, and sequentially perform lazy collision detection on each path segment in order from high to low priority. When the first path segment that collides with an obstacle is detected, perform path replanning on the first collision segment and generate an alternative path segment, and use the alternative path segment to update the initial global path, and then re-perform lazy collision detection based on priority marking on the updated global path until a collision-free global path is obtained; Convert the global path into a temporal elastic band according to the operation area where the path is located, map the soft constraint weights of each segment in the global path to the corresponding temporal pose points on the temporal elastic band to obtain a soft constraint band assigned to the temporal pose points, and use the soft constraint band to constrain the deformation of the temporal elastic band to obtain the final execution path.

[0004] Preferably, assign soft constraint weights to each area, specifically: Obtain the basic weight values of each operation area type, and determine the adjustment factor according to the task stage; Take the product of the basic weight value and the adjustment factor as the soft constraint weight of each area.

[0005] Preferably, obtain the node set of the initial probabilistic roadmap based on the operation area type and the soft constraint weight, specifically: Set different minimum sampling density thresholds for different operation area types; Randomly generate a candidate node in the workspace, and then generate a random number within the range. If the random number is greater than the soft constraint weight corresponding to the operation area where the candidate node is located, add the candidate node to the node set, otherwise, reject it, where is the maximum soft constraint weight of all operation areas; continuously generate candidate nodes until the number of nodes in all operation areas is greater than the minimum sampling density threshold.

[0006] Preferably, obtain the initial global path according to the node set and the soft constraint weight, specifically: Take the nodes in the node set as the vertices of the graph. For any two nodes, if the straight-line connection path between them does not collide with any physical obstacle in the three-dimensional model, establish an edge between the two nodes, and obtain the weight of the edge according to the length of the edge and the soft constraint weight of the operation area traversed by the edge; According to the weight of the edge, use Search to search for the initial global path from the starting point to the target point in the constructed graph.

[0007] Preferably, perform priority marking based on the curvature of each path segment of the initial global path and the soft constraint weight of the operation area traversed, specifically: Discretize the initial global path into path segments, and each path segment connects two adjacent path points; For each path segment , calculate the priority score , where is the length of path segment , is the soft constraint weight of the operation area traversed by path segment The maximum dynamic soft constraint weight value among all the operation areas passed through is the path segment 's average curvature is a non - negative weight coefficient

[0008] Preferably, mapping the soft constraint weights of each segment in the global path to the corresponding temporal pose points on the temporal elastic band to obtain a soft constraint band assigned to the temporal pose points, specifically: Transform the collision - free global path into a temporal elastic band composed of multiple temporal pose points, where each pose point contains position and attitude information; For each pose point, determine the path segment on the original global path corresponding to it, and obtain the soft constraint weight of the operation area associated with this path segment; Define a hyper - rectangular soft constraint band for each pose point in the configuration space, and the size of each dimension of the soft constraint band is inversely proportional to the soft constraint weight.

[0009] Preferably, using the soft constraint band to constrain the deformation of the temporal elastic band, specifically: Construct an objective function with trajectory smoothing term, obstacle avoidance term, and soft constraint band penalty term as optimization terms; Adjust the positions and attitudes of each temporal pose point in the temporal elastic band through an iterative optimization algorithm to minimize the objective function.

[0010] In the second aspect of the present invention, a path optimization system for a medical robot manipulator is provided, and the system includes the following modules: A global path generation module, which is used to obtain the working space of the medical robot manipulator, divide the working space into different operation areas, and assign soft constraint weights to each area; obtain the node set of the initial probabilistic roadmap based on the soft constraint weights, and obtain the initial global path according to the node set and the soft constraint weights; A collision detection module, which is used to perform priority marking based on the curvature of each path segment of the initial global path and the soft constraint weights of the operation areas passed through, and sequentially perform lazy collision detection on each path segment in the order of decreasing priority. When detecting the first path segment that collides with an obstacle, perform path replanning on the first collision segment and generate an alternative path segment, and use the alternative path segment to update the initial global path, and then re - perform lazy collision detection based on priority marking on the updated global path until a collision - free global path is obtained; A path optimization module, which is used to transform the global path into a temporal elastic band according to the operation area where the path is located, map the soft constraint weights of each segment in the global path to the corresponding temporal pose points on the temporal elastic band to obtain a soft constraint band assigned to the temporal pose points, and use the soft constraint band to constrain the deformation of the temporal elastic band to obtain the final execution path.

[0011] Preferably, soft constraint weights are assigned to each region, specifically as follows: Obtain the basic weight values of each operation region type, and determine the adjustment factor according to the task stage; Take the product of the basic weight value and the adjustment factor as the soft constraint weight of each region.

[0012] Preferably, a node set of the initial probability roadmap is obtained based on the operation region type and the soft constraint weight, specifically as follows: Set different minimum sampling density thresholds for different operation region types; Randomly generate a candidate node in the workspace, and then generate a random number within the range. If the random number is greater than the soft constraint weight corresponding to the operation region where the candidate node is located, add the candidate node to the node set; otherwise, reject it, where is the maximum soft constraint weight of all operation regions; continuously generate candidate nodes until the number of nodes in all operation regions is greater than the minimum sampling density threshold.

[0013] Preferably, an initial global path is obtained according to the node set and the soft constraint weight, specifically as follows: Take the nodes in the node set as the vertices of the graph. For any two nodes, if the straight-line connection path between them does not collide with any physical obstacle in the 3D model, establish an edge between the two nodes, and obtain the weight of the edge according to the length of the edge and the soft constraint weight of the operation region traversed by the edge; According to the weight of the edge, use Search for the initial global path from the starting point to the target point in the constructed graph.

[0014] Preferably, priority marking is performed based on the curvature of each path segment of the initial global path and the soft constraint weight of the operation region traversed, specifically as follows: Discretize the initial global path into path segments, and each path segment connects two adjacent path points; For each path segment , calculate the priority score , where is the length of path segment , is the maximum dynamic soft constraint weight value among all operation regions passed by path segment , is the average curvature of path segment , is a non-negative weight coefficient.

[0015] Preferably, mapping the soft constraint weights of each segment in the global path to the corresponding temporal pose points on the temporal elastic band to obtain a soft constraint band assigned to the temporal pose points, specifically: Transform the collision-free global path into a temporal elastic band composed of multiple temporal pose points, where each pose point contains position and attitude information; For each pose point, determine the path segment on the original global path corresponding to it, and obtain the soft constraint weight of the operation area associated with this path segment; Define a hyper-rectangular soft constraint band for each pose point in the configuration space, and the size of each dimension of the soft constraint band is inversely proportional to the soft constraint weight.

[0016] Preferably, use the soft constraint band to constrain the deformation of the temporal elastic band, specifically: Construct an objective function with trajectory smoothing term, obstacle avoidance term, and soft constraint band penalty term as optimization terms; Adjust the positions and attitudes of each temporal pose point in the temporal elastic band through an iterative optimization algorithm to minimize the objective function.

[0017] By introducing soft constraint weights based on the type of operation area, the present invention enables path planning to avoid undesirable areas from the very beginning, rather than simply treating them as hard obstacles. Moreover, by preferentially detecting high-risk path segments, once a collision is detected, only the collided segment is replanned and iterated quickly, improving the real-time performance and response speed of path planning. By transforming the collision-free path into a temporal elastic band and using the soft constraint band mapped from the soft constraint weights for constraint optimization, not only the smoothness and collision-freeness of the path are ensured, but also it is ensured that the path strictly follows the preset soft constraint preferences in terms of morphology, significantly enhancing the safety of the finally generated execution path. Brief Description of the Drawings

[0018] Figure 1 It is a flowchart of Embodiment 1; Figure 2 It is a schematic diagram of replacing the original path with an alternative path segment; Figure 3 It is a schematic diagram of a collision-free global path; Figure 4 It is a schematic diagram of transforming the global path into a temporal elastic band; Figure 5 It is a structural diagram of Embodiment 2. Detailed Description of the Invention

[0019] In this text, relational terms such as first and second are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the term "comprising", "including" or any other variation thereof is intended to cover non-exclusive inclusion, such that a process, method, article or device comprising a series of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article or device. Without further limitation, an element defined by the statement "comprising an..." does not exclude the presence of additional identical elements in the process, method, article or device comprising the element.

[0020] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0021] In the first embodiment of the present invention, a method for optimizing the path of a robotic arm of a medical robot is provided. As Figure 1 shown, the method includes the following steps: S1, obtain the working space of the robotic arm of the medical robot, divide the working space into different operation areas, and assign soft constraint weights to each area; obtain the node set of the initial probabilistic roadmap based on the soft constraint weights, and obtain the initial global path according to the node set and the soft constraint weights; The working space is obtained by reading a pre-established three-dimensional model file or by using the sensor system of the robot, such as a depth camera, lidar, etc., to scan and construct an environmental map in real time. In one embodiment, the working space model includes not only static obstacles, such as patient tissue and fixed equipment, but also areas that potentially affect the movement of the robot. The forms of the working space include but are not limited to point clouds, voxel grids, octrees, etc. The division of the working space is based on, for example, anatomical structures, safety distance requirements, etc. For example, a virtual cylinder or sphere with a variable radius is defined around the identified key blood vessels or nerves as a high-risk area, and several operation areas and no-go areas are defined according to the movement range and sterility requirements of the surgical instruments. A numerical soft constraint weight is assigned to each divided operation area, and the soft constraint weight represents the risk level or undesirability of the path passing through this area. In one embodiment, the closer to the hazard source or important part, the greater the soft constraint weight, for example, growing exponentially. In another embodiment, the soft constraint weight is specified by medical staff or calculated according to the density of the tissue in the area and / or the distance from the surgical target, etc.

[0022] In an alternative embodiment, a non-uniform sampling method is used to generate a set of generation nodes. During the expansion process of the Rapidly-exploring Random Tree (RRT), the random selection of new nodes or the growth direction of the tree is affected by the soft constraint weight field in the workspace. For example, the probability of expanding into a region with a low soft constraint weight is increased, or in the selection of the expansion step size, the step size towards a region with a high soft constraint weight is suppressed. In yet another embodiment, a deterministic sampling method is used to generate candidate nodes, and then, based on the soft constraint weight value at the location of the node and its connection with the existing nodes, it is determined whether to add it to the roadmap. Figure 2 A schematic diagram showing the replacement of the original path with an alternative path segment is presented.

[0023] Search for an initial path on the constructed roadmap. Preferably, the ... method is adopted. In one embodiment, the cost function during the search process includes, but is not limited to, the length of the path, the maximum soft constraint weight, etc. The obtained initial global path is a path that connects the starting point and the target point and has a relatively low cost in the graph. In an alternative embodiment, soft constraint weights are assigned to each region, specifically: Obtain the basic weight values for each type of operation region, and determine the adjustment factor according to the task stage; Take the product of the basic weight value and the adjustment factor as the soft constraint weight of each region.

[0024] Define different types of operation regions, such as high-risk no-go zones, areas adjacent to important organs, general operation areas, safe passage areas, etc., for the identified major blood vessels, nerve bundles, boundaries of important organs, and specific areas of concern marked by the surgeon according to the surgical plan. Set a standardized basic weight value for each type of region. The basic weight value is a relatively fixed value that represents the sensitivity of this type of region or the degree of undesirability for the robot to enter in general. For example, the basic weight value of the high-risk no-go zone will be significantly higher than that of the general operation area. Determine the corresponding adjustment factor according to the specific stage of the current medical robot performing the task. A complete surgical or medical operation process can be decomposed into several stages with different characteristics and requirements, such as the instrument insertion stage, the lesion exposure stage, the tissue resection / suturing stage, the instrument withdrawal stage, etc. For each task stage and for each type of operation region, preset the adjustment factor. For example, during the instrument insertion stage, the path may need to quickly pass through the general operation area, and at this time, the adjustment factor for these areas can be set to a value less than 1 to reduce their soft constraint weights and encourage the path to pass through; while during the tissue resection / suturing stage, for the areas adjacent to important organs, the adjustment factor is set to a value greater than 1 to further enhance the avoidance of these areas. Multiply the basic weight value of a specific type of operation region by the corresponding adjustment factor at the current task stage to obtain the dynamic soft constraint weight of this region at this specific moment.

[0025] In an alternative embodiment, a node set of an initial probabilistic roadmap is obtained based on the operation area type and the soft constraint weight, specifically as follows: Set different minimum sampling density thresholds for different operation area types; Randomly generate a candidate node in the workspace, and then generate a random number within the range. If the random number is greater than the soft constraint weight corresponding to the operation area where the candidate node is located, add the candidate node to the node set; otherwise, reject it, where is the maximum soft constraint weight of all operation areas; continuously generate candidate nodes until the number of nodes in all operation areas is greater than the minimum sampling density threshold.

[0026] Specifically, a minimum sampling density threshold is set for each type of operation area divided in the workspace. The minimum sampling density threshold is determined according to factors such as the importance of the area and the complexity of the internal structure of the area. For example, for a safe area with a simple internal structure, its minimum sampling density threshold can be relatively low; while for a narrow area that is not expected but may become a bottleneck path, a relatively high minimum sampling density is required to ensure connectivity. In each iteration, the position coordinates of a candidate node are randomly generated within the entire workspace of the medical robot. After generating the position coordinates of the candidate node, determine the operation area to which it belongs, obtain the current soft constraint weight corresponding to this area, and generate a random number uniformly distributed within the interval. If the random number is greater than the soft constraint weight corresponding to the operation area where the candidate node is located, it means that the random number exceeds the soft constraint repulsion degree of this area, and the candidate node is accepted and added to the node set of the PRM; otherwise, the candidate node is rejected. This process is continuously repeated, and at the same time, record the number of nodes that have been successfully added to the node set in each different operation area. When the cumulative number of nodes in all operation areas has reached or exceeded their respective preset minimum sampling density thresholds, the generation process of the node set terminates.

[0027] By combining the minimum sampling density threshold and the probabilistic node acceptance method based on the soft constraint weight, while ensuring the most basic coverage of the entire workspace by the PRM, dense sampling is preferentially performed in the safe area with a low soft constraint weight, and sparse sampling is performed in the dangerous area with a high soft constraint weight to reduce the possibility of generating paths passing through the dangerous area.

[0028] To ensure that the path search process not only pursues the shortest geometric distance, but also can actively avoid those path segments that cross the high soft constraint weight area. In an alternative embodiment, an initial global path is obtained based on the node set and the soft constraint weight, specifically as follows: Take the nodes in the node set as the vertices of the graph. For any two nodes, if their straight-line connection path does not collide with any physical obstacles in the 3D model, then establish an edge between the two nodes, and obtain the weight of the edge according to the length of the edge and the soft constraint weight of the operation area traversed by the edge; According to the weight of the said edge, use Search for an initial global path from the starting point to the target point in the constructed graph.

[0029] Take all the nodes in the node set as the vertices of the graph. For any pair of nodes in the graph , connect and If the straight-line path segment does not collide with any known physical obstacles in the 3D workspace model, then in and Establish an edge between them and calculate the weight of this edge. In one embodiment, the way to calculate the weight of the edge is , where is the effective soft constraint weight contribution of this edge. Preferably, take the maximum soft constraint weight of the operation area traversed by the edge as , is an adjustable weight coefficient. After constructing the weighted graph, use Search algorithm to find the initial global path from the specified starting node to the target node. The algorithm calculates an evaluation function f(v) = g(v) + h(v). Wherein, g(v) is the actual cumulative path cost from the starting node to the current node, and this cost is obtained by accumulating the weights of all edges on the path. g(v) reflects the length of the path and the soft constraint crossing situation. h(v) is a heuristic function used to estimate the minimum remaining cost from the current node to the target node, which is the straight-line Euclidean distance between the current node and the target node. The algorithm starts from the starting node, iteratively expands by selecting the node with the minimum f(v) value, adds its neighbor nodes to the open list until the target node is selected and taken out from the open list. By backtracking the parent node pointer chain from the target node to the starting node, construct this initial global path that comprehensively considers the length and soft constraint weight.

[0030] S2. Perform priority marking based on the curvature of each path segment of the initial global path and the soft constraint weight of the operation area traversed, and sequentially perform lazy collision detection on each path segment in the order of decreasing priority. When detecting the first path segment that collides with an obstacle, perform path replanning on the first collision segment and generate an alternative path segment. After updating the initial global path with the alternative path segment, re-perform lazy collision detection based on priority marking on the updated global path until a collision-free global path is obtained; The initial global path is segmented, for example, by a fixed length. The priority score of each path segment is calculated. In one embodiment, the priority is calculated based on, for example, the distance from the path segment to the nearest obstacle, the twist degree of the path segment itself, and the soft constraint weight of the area traversed by this segment. More specifically, the priority is the weighted sum of the SCW value and the reciprocal of the distance to the obstacle.

[0031] According to the calculated priority, accurate collision detection is performed on the path segments in descending order. The collision detection preferably uses the GJK algorithm or a method based on the bounding box hierarchy. Once the first colliding path segment is detected, subsequent detections are stopped. The colliding path segment and a small adjacent segment of the path form a local window, and local path replanning is initiated. The replanning uses a fast planning method different from the global planning, such as local adjustment of the elastic band method or running RRT in a small range near the collision point, etc. After generating a collision-free alternative path segment, the original colliding path in the path or the path within the local window is replaced with the alternative path to obtain an updated global path. Then, priority-based lazy collision detection is performed on the updated path again from the beginning or from the update point. Iterate continuously until all segments of the entire path pass the collision detection, as Figure 3 shown.

[0032] To improve the overall efficiency of collision detection and path repair. In one embodiment, priority marking is performed based on the curvature of each path segment of the initial global path and the soft constraint weight of the operating area traversed, specifically: The initial global path is discretized into path segments, and each path segment connects two adjacent path points; For each path segment , the priority score is calculated , where is the length of path segment , is the maximum dynamic soft constraint weight value among all operating areas passed by path segment , is the average curvature of path segment , is a non-negative weight coefficient.

[0033] The longer the path segment, the greater the probability of its intersection with obstacles, and the higher the soft constraint weight of the area traversed by the path segment. This area is not desired for the robot to enter. If the path must pass through it, its safety needs to be confirmed first. The greater the curvature of the path segment, the more severe the bending degree of the segment, which may require more complex attitude adjustments and is more likely to collide with itself or the environment. By performing priority collision detection on these high-priority path segments, problems can be detected and handled early. If a collision occurs and replanning is required, adjustments can be made as early as possible, thereby avoiding wasting detection resources on unnecessary subsequent path segments.

[0034] S3. Convert the global path into a timed elastic band according to the operation area where the path is located. Map the soft constraint weights of each segment in the global path to the corresponding timed pose points on the timed elastic band to obtain the soft constraint bands assigned to the timed pose points. Use the soft constraint bands to constrain the deformation of the timed elastic band to obtain the final execution path.

[0035] Use B-spline curves or polynomial splines to convert the obtained discrete and collision-free global path into a continuous trajectory representation, which includes not only the position information of the path points, but also information such as attitude, timestamp, speed, etc., to form a timed trajectory, as Figure 4 shown. Convert the soft constraint weights into a repulsive force or potential field acting on the trajectory control points, so that the higher the soft constraint weight of the area, the greater the generated repulsive force, or add a penalty term proportional to the soft constraint weight to the trajectory optimization objective function in this area. Adjust the shape of the trajectory through sequential quadratic programming (SQP) or the gradient descent method, such as moving the control points of the B-spline. The optimization objective function includes but is not limited to minimizing the curvature, maintaining a safe distance from obstacles, and / or the soft constraint penalty term. During the optimization process, under the premise of meeting the hard collision-free constraint, minimize the objective function to make the trajectory as smooth as possible and actively avoid areas with high soft constraint weights, and finally obtain a smooth trajectory for the robot to execute.

[0036] In an optional embodiment, mapping the soft constraint weights of each segment in the global path to the corresponding timed pose points on the timed elastic band to obtain the soft constraint bands assigned to the timed pose points is specifically as follows: Convert the collision-free global path into a timed elastic band composed of multiple timed pose points, where each pose point includes position and attitude information; For each pose point, determine the path segment it corresponds to on the original global path, and obtain the soft constraint weight of the operation area associated with this path segment; Define a hyper-rectangular soft constraint band for each pose point in the configuration space, and the size of each dimension of the soft constraint band is inversely proportional to the soft constraint weight.

[0037] Convert the obtained collision-free global path into the representation form of the Timing Elastic Band (TEB). The TEB consists of a sequence of multiple chronologically arranged timing pose points, including position, attitude, timestamp, etc. The pose points are connected by elastic connections to form a continuous trajectory. The conversion methods include, but are not limited to, interpolating the original path points, etc. Assign a soft constraint band to each pose point. Specifically, determine the original path segment corresponding to the pose point on the original collision-free global path, and obtain the soft constraint weight value of the operation area associated with this path segment. If multiple operation areas are associated, take the maximum soft constraint weight. Define a hyper-rectangular soft constraint band for the pose point in its configuration space. Preferably, the center of the hyper-rectangle is the current pose of the pose point, and its allowable variation range in each configuration space dimension. The constraint band is inversely proportional to the obtained soft constraint weight. For example, in the position dimension, the half-width of the soft constraint band in the x direction is , where C is the base allowable deviation constant, and the soft constraint weight is larger, the smaller it is, and the narrower the range within which the pose point is allowed to move in the x direction. Similarly, corresponding constraint ranges can also be defined in the attitude dimension.

[0038] In one embodiment, use the soft constraint band to constrain the deformation of the Timing Elastic Band. Specifically: Construct an objective function with the trajectory smoothing term, obstacle avoidance term, and soft constraint band penalty term as optimization terms; Adjust the positions and attitudes of the timing pose points in the Timing Elastic Band through an iterative optimization algorithm to minimize the objective function.

[0039] Construct an objective function with the trajectory smoothing term, obstacle avoidance term, and soft constraint band penalty term as optimization terms. Weight-sum these three types of optimization terms to obtain the total objective function. Then, use iterative optimization algorithms such as sequential quadratic programming and gradient descent method to adjust the position and attitude parameters of all timing pose points. The optimization process starts from the initial state of the TEB. In each iteration, calculate the gradient of the objective function and update the pose point parameters along the direction that can make the value of the objective function decrease until the change in the value of the objective function is less than the threshold or the maximum number of iterations is reached.

[0040] In an alternative embodiment, the trajectory smoothing term is the sum of the squares of the velocity differences or the sum of the squares of the accelerations between adjacent pose points. In yet another alternative embodiment, the trajectory smoothing term also includes attitude, and the smoothing term of angular velocity or angular acceleration can be defined in a similar manner. The obstacle avoidance term is used to penalize the situation where the trajectory is too close to physical obstacles. For each pose point, calculate its distance to the nearest physical obstacle. The obstacle avoidance term is a function whose penalty value increases when the distance is less than the safety threshold. For example 。The soft constraint band penalty term imposes a penalty based on whether the pose point is within its assigned soft constraint band or the degree of deviation. For each temporal pose point and the hyper-rectangular soft constraint band defined in the configuration space, if the pose point exceeds the boundary of its soft constraint band, a penalty is generated. In one embodiment, the magnitude of the penalty is proportional or squared to the distance beyond the boundary.

[0041] In the second embodiment of the present invention, a path optimization system for a medical robot manipulator is provided. As Figure 5 shown, the system includes the following modules: A global path generation module, configured to obtain the workspace of the medical robot manipulator, divide the workspace into different operation regions, and assign soft constraint weights to each region; obtain a node set of an initial probabilistic roadmap based on the soft constraint weights, and obtain an initial global path according to the node set and the soft constraint weights; A collision detection module, configured to perform priority marking based on the curvature of each path segment of the initial global path and the soft constraint weights of the operation regions passed through, and sequentially perform lazy collision detection on each path segment in descending order of priority. When the first path segment that collides with an obstacle is detected, perform path replanning on the first collision segment and generate an alternative path segment, and after updating the initial global path with the alternative path segment, re-perform lazy collision detection based on priority marking on the updated global path until a collision-free global path is obtained; A path optimization module, configured to convert the global path into a temporal elastic band according to the operation region where the path is located, map the soft constraint weights of each segment in the global path to the corresponding temporal pose points on the temporal elastic band to obtain soft constraint bands assigned to the temporal pose points, and use the soft constraint bands to constrain the deformation of the temporal elastic band to obtain the final execution path.

[0042] Preferably, assigning soft constraint weights to each region specifically includes: Obtain the basic weight values of each operation region type, and determine the adjustment factor according to the task stage; Take the product of the basic weight value and the adjustment factor as the soft constraint weight of each region of the region.

[0043] Preferably, obtaining the node set of the initial probabilistic roadmap based on the operation region type and the soft constraint weights specifically includes: Set different minimum sampling density thresholds for different operation region types; Randomly generate a candidate node in the workspace, and then generate a random number within a range. If the random number is greater than the soft constraint weight corresponding to the operation region where the candidate node is located, add the candidate node to the node set, otherwise, reject it, where is the maximum soft constraint weight for all operation areas; continuously generate candidate nodes until the number of nodes in all operation areas is greater than the minimum sampling density threshold.

[0044] Preferably, an initial global path is obtained according to the node set and the soft constraint weight, specifically: Take the nodes in the node set as the vertices of the graph. For any two nodes, if the straight-line connection path between them does not collide with any physical obstacle in the 3D model, establish an edge between the two nodes, and obtain the weight of the edge according to the length of the edge and the soft constraint weight of the operation area traversed by the edge; According to the weight of the edge, use Search for an initial global path from the starting point to the target point in the constructed graph.

[0045] Preferably, priority marking is performed based on the curvature of each path segment of the initial global path and the soft constraint weight of the traversed operation area, specifically: Discretize the initial global path into path segments, and each path segment connects two adjacent path points; For each path segment , calculate the priority score , where is the length of path segment , is the maximum dynamic soft constraint weight value among all operation areas passed by path segment , is the average curvature of path segment , is a non-negative weight coefficient.

[0046] Preferably, map the soft constraint weights of each segment in the global path to the corresponding temporal pose points on the temporal elastic band to obtain a soft constraint band for the temporal pose points, specifically: Convert the collision-free global path into a temporal elastic band composed of multiple temporal pose points, where each pose point contains position and orientation information; for each pose point, determine the path segment it corresponds to on the original global path, and obtain the soft constraint weight of the operation area associated with the path segment; define a hyper-rectangular soft constraint band for each pose point in the configuration space, and the size of each dimension of the soft constraint band is inversely proportional to the soft constraint weight.

[0047] Preferably, use the soft constraint band to constrain the deformation of the temporal elastic band, specifically: construct an objective function with trajectory smoothing term, obstacle avoidance term, and soft constraint band penalty term as optimization terms; adjust the positions and orientations of each temporal pose point in the temporal elastic band through an iterative optimization algorithm to minimize the objective function.

[0048] In the third embodiment of the present invention, the present invention further provides a computer-readable storage medium, on which a computer program is stored, and the computer program, when executed by a processor, implements the method of Embodiment 1.

[0049] In the fourth embodiment of the present invention, the present invention further provides a computer device, which at least includes a memory and a processor. A computer program is stored on the memory, and the computer program, when executed by the processor, implements the method of Embodiment 1.

[0050] Through the description of the above embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of adding a necessary general hardware platform. Of course, it can also be implemented by a combination of hardware and software. Based on such an understanding, the above technical solution, in essence, or the part that contributes to the prior art can be embodied in the form of a computer product. The present invention can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk memories, CD-ROMs, optical memories, etc.) containing computer-usable program codes. Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present invention, rather than limiting them. Other embodiments can also be adopted; although the present invention has been described in detail with reference to the foregoing embodiments, those ordinary skilled in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for optimizing the path of a robotic arm of a medical robot, characterized in that, The method includes the following steps: Obtain the workspace of the robotic arm of the medical robot, divide the workspace into different operation areas, and assign soft constraint weights to each area; Based on the soft constraint weights, obtain the node set of the initial probabilistic roadmap, and obtain the initial global path according to the node set and the soft constraint weights; Perform priority marking based on the curvature of each path segment of the initial global path and the soft constraint weights of the operation areas passed through, and sequentially perform lazy collision detection on each path segment in the order from high to low priority. When the first path segment that collides with an obstacle is detected, perform path replanning on the first collision segment and generate an alternative path segment. After updating the initial global path with the alternative path segment, re-perform lazy collision detection based on priority marking on the updated global path until a collision-free global path is obtained; Convert the global path into a timed elastic band according to the operation area where the path is located, map the soft constraint weights of each segment in the global path to the corresponding timed pose points on the timed elastic band to obtain a soft constraint band assigned to the timed pose points, and use the soft constraint band to constrain the deformation of the timed elastic band to obtain the final execution path.

2. The method according to claim 1, wherein Assign soft constraint weights to each area, specifically: Obtain the basic weight values of each operation area type, and determine the adjustment factor according to the task stage; Take the product of the basic weight value and the adjustment factor as the soft constraint weight of each area.

3. The method according to claim 1, wherein Based on the operation area type and the soft constraint weights, obtain the node set of the initial probabilistic roadmap, specifically: Set different minimum sampling density thresholds for different operation area types; Randomly generate a candidate node in the workspace, and then generate a random number within the range. If the random number is greater than the soft constraint weight corresponding to the operation area where the candidate node is located, add the candidate node to the node set; otherwise, reject it, where is the maximum soft constraint weight of all operation areas. Continuously generate candidate nodes until the number of nodes in all operation areas is greater than the minimum sampling density threshold.

4. The method according to claim 1, characterized in that Obtain the initial global path according to the node set and the soft constraint weights, specifically: Take the nodes in the node set as the vertices of the graph. For any two nodes, if the straight-line connection path between them does not collide with any physical obstacle in the 3D model, establish an edge between the two nodes, and obtain the weight of the edge according to the length of the edge and the soft constraint weights of the operation areas passed through by the edge; According to the weight of the said edge, Search Search for an initial global path from the starting point to the target point in the constructed graph.

5. The method according to claim 1, wherein Perform priority marking based on the curvature of each path segment of the initial global path and the soft constraint weights of the operation areas passed through, specifically: Discretize the initial global path into path segments, and each path segment connects two adjacent path points; For each path segment , calculate the priority score , where is the length of the path segment , is the maximum dynamic soft constraint weight value among all the operation areas passed by the path segment , is the average curvature of the path segment , is a non - negative weight coefficient 6. The method according to claim 1, wherein Map the soft constraint weights of each segment in the global path to the corresponding timed pose points on the timed elastic band to obtain a soft constraint band assigned to the timed pose points, specifically: Convert the collision-free global path into a timed elastic band composed of multiple timed pose points, where each pose point contains position and attitude information; For each pose point, determine the path segment corresponding to it on the original global path, and obtain the soft constraint weight of the operation area associated with the path segment; Define a hyper-rectangular soft constraint band for each pose point in the configuration space, and the size of each dimension of the soft constraint band is inversely proportional to the soft constraint weight.

7. The method according to claim 1, characterized in that, Use the soft constraint band to constrain the deformation of the timed elastic band, specifically: Construct an objective function with trajectory smoothing term, obstacle avoidance term, and soft constraint band penalty term as optimization terms; Adjust the positions and attitudes of each timed pose point in the timed elastic band through an iterative optimization algorithm to minimize the objective function.

8. A path optimization system for a medical robot manipulator, characterized in that, The system includes the following modules: A global path generation module, which is used to obtain the workspace of the robotic arm of the medical robot, divide the workspace into different operation areas, and assign soft constraint weights to each area; Based on the soft constraint weights, obtain the node set of the initial probabilistic roadmap, and obtain the initial global path according to the node set and the soft constraint weights; A collision detection module, which is used to perform priority marking based on the curvature of each path segment of the initial global path and the soft constraint weights of the operation areas passed through, and sequentially perform lazy collision detection on each path segment in the order of decreasing priority. When the first path segment that collides with an obstacle is detected, perform path replanning on the first collision segment and generate an alternative path segment. After updating the initial global path with the alternative path segment, re-perform lazy collision detection based on priority marking on the updated global path until a collision-free global path is obtained; A path optimization module, which is used to convert the global path into a temporal elastic band according to the operation area where the path is located, map the soft constraint weights of each segment in the global path to the corresponding temporal pose points on the temporal elastic band to obtain a soft constraint band assigned to the temporal pose points, and use the soft constraint band to constrain the deformation of the temporal elastic band to obtain the final execution path.

9. The system according to claim 8, wherein Assign soft constraint weights to each area, specifically: Obtain the basic weight values of each operation area type, and determine the adjustment factor according to the task stage; Take the product of the basic weight value and the adjustment factor as the soft constraint weight of each area.

10. The system according to claim 8, wherein Obtain the node set of the initial probabilistic roadmap based on the operation area type and the soft constraint weights, specifically: Set different minimum sampling density thresholds for different operation area types; Randomly generate a candidate node in the workspace, and then generate a random number within the range. If the random number is greater than the soft constraint weight corresponding to the operation area where the candidate node is located, add the candidate node to the node set; otherwise, reject it, where is the maximum soft constraint weight of all operation areas. Continuously generate candidate nodes until the number of nodes in all operation areas is greater than the minimum sampling density threshold.

Images