A medical robot arm path optimization method and system

Abstract

The present invention relates to a medical robot manipulator path optimization method and system, belonging to the field of manipulator path optimization. Specifically, a workspace is divided and soft constraint weights are assigned to each area. An initial probabilistic roadmap node set is constructed based on the soft constraint weights. An initial global path is obtained by searching in combination with the soft constraint weights. Priorities are calculated according to the curvature of the path segment and the soft constraint weights of the traversed area. Inert collision detection is performed in a high-priority order. If a collision is detected, only the collision segment is locally replanned to generate an alternative segment. After the path is updated, priority collision detection is re-executed until the path is completely collision-free. The collision-free global path is converted into a temporal elastic band, and the soft constraint weights of the path segment are mapped into a soft constraint band of the pose point. The soft constraint band is used to optimize the morphology to finally obtain the execution path.

Description

Technical Field

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

[0002] Medical robotics technology demonstrates tremendous potential in diverse fields, including surgical assistance, diagnostic imaging, rehabilitation therapy, and drug delivery. In particular, in minimally invasive surgery and telemedicine, medical robotic arms, with their high precision, high stability, repeatable operation, and ability to filter out hand tremors, expand surgeons' operational capabilities, reduce surgical risks, and improve treatment outcomes. Path planning for a robotic arm involves planning a collision-free, smooth, and efficient trajectory for the end effector from a starting position to a target position within a complex, dynamically changing workspace. However, the working environment of a medical robotic arm is often highly complex and uncertain. For example, in surgical scenarios, the workspace contains not only static obstacles such as human tissue, organs, and surgical instruments, but also dynamic factors such as the patient's respiratory movement and the temporary insertion of other medical equipment. Furthermore, beyond the basic collision-free requirement, medical robotic path planning must consider numerous additional constraints and optimization objectives. For example, while not necessarily physical obstacles, certain areas may be inaccessible due to proximity to vital nerves, blood vessels, critical tissue, or infection risks. These areas can be considered soft constraints. Traditional path planning methods often focus on finding the shortest or fastest path. Their ability to express and process soft constraints is limited, making it difficult to effectively avoid potential risk areas in the early stages of planning. Balancing path smoothness, safety, and preferred operating areas is also a challenge that existing technologies need to address. Summary of the Invention

[0003] In order to solve the above problems, the present invention provides a method for optimizing the path of a medical robot manipulator, the method comprising the following steps:

[0004] Obtaining a workspace of the medical robot arm, dividing the workspace into different operating areas, and assigning soft constraint weights to each area; obtaining a node set of an initial probabilistic roadmap based on the soft constraint weights, and obtaining an initial global path based on the node set and the soft constraint weights;

[0005] Priority marking is performed on each path segment of the initial global path based on the curvature and the soft constraint weight of the traversed operating area, and lazy collision detection is performed on each path segment in descending order of priority. When the first path segment collides with an obstacle is detected, the path of the first collision segment is replanned and an alternative path segment is generated. After the initial global path is updated with the alternative path segment, lazy collision detection based on the priority marking is re-performed on the updated global path until a collision-free global path is obtained;

[0006] The global path is converted into a timing elastic band according to the operation area where the path is located, and the soft constraint weights of each segment in the global path are mapped to the corresponding timing pose points on the timing elastic band to obtain a soft constraint band assigned to the timing pose points. The soft constraint band is used to constrain the deformation of the timing elastic band to obtain the final execution path.

[0007] Preferably, a soft constraint weight is assigned to each region, specifically:

[0008] Obtain the basic weight value of each operation area type and determine the adjustment factor according to the mission stage;

[0009] The product of the basic weight value and the adjustment factor is used as the soft constraint weight of each area in the region.

[0010] Preferably, the node set of the initial probability roadmap is obtained based on the operation area type and the soft constraint weight, specifically:

[0011] Set different minimum sampling density thresholds for different operating area types;

[0012] Randomly generate a candidate node in the workspace, and then generate a A random number in the range, if the random number is greater than the soft constraint weight corresponding to the operation area where the candidate node is located, the candidate node is added to the node set, otherwise, it is rejected, where is the maximum soft constraint weight of all operating areas; candidate nodes are continuously generated until the number of nodes in all operating areas is greater than the minimum sampling density threshold.

[0013] Preferably, the initial global path is obtained according to the node set and the soft constraint weights, specifically as follows:

[0014] The nodes in the node set are used as vertices of the graph. For any two nodes, if the straight line connecting the two nodes does not collide with any physical obstacles in the three-dimensional model, an edge is established between the two nodes. The weight of the edge is obtained according to the length of the edge and the soft constraint weight of the operation area traversed by the edge.

[0015] According to the weight of the edge Search searches for an initial global path from the starting point to the target point in the constructed graph.

[0016] 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:

[0017] Discretizing the initial global path into path segments, each path segment connecting two adjacent path points;

[0018] For each path segment , calculate the priority score ,in, For path segments length, For path segments The maximum dynamic soft constraint weight value of all the operation areas passed, Is a path segment The mean curvature of is a non-negative weight coefficient.

[0019] Preferably, the soft constraint weights of each segment in the global path are mapped to the corresponding temporal pose points on the temporal elastic band to obtain a soft constraint band assigned to the temporal pose points, specifically:

[0020] The collision-free global path is converted into a temporal elastic band consisting of multiple temporal pose points, where each pose point contains position and attitude information;

[0021] 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;

[0022] A hyper-rectangular soft constraint band is defined 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.

[0023] Preferably, the soft constraint belt is used to constrain the deformation of the timing elastic belt, specifically:

[0024] Construct an objective function with trajectory smoothing term, obstacle avoidance term, and soft constraint band penalty term as optimization terms;

[0025] The position and posture of each temporal pose point in the temporal elastic band are adjusted through an iterative optimization algorithm to minimize the objective function.

[0026] In a second aspect of the present invention, a medical robot arm path optimization system is provided, the system comprising the following modules:

[0027] A global path generation module is configured to obtain a workspace of the medical robot arm, divide the workspace into different operating areas, and assign soft constraint weights to each area; obtain a node set of an initial probabilistic roadmap based on the soft constraint weights, and obtain an initial global path based on the node set and the soft constraint weights;

[0028] a collision detection module for assigning priorities to each path segment of the initial global path based on the curvature and the soft constraint weights of the traversed operating area, and performing lazy collision detection on each path segment in descending order of priority; when the first path segment colliding with an obstacle is detected, replanning the path of the first collision segment and generating an alternative path segment; after updating the initial global path with the alternative path segment, re-performing lazy collision detection based on the priority marking on the updated global path until a collision-free global path is obtained;

[0029] The path optimization module is used to convert the global path into a temporal elastic band according to the operation area in which 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, and assign a soft constraint band to the temporal pose points. The soft constraint band is used to constrain the deformation of the temporal elastic band to obtain the final execution path.

[0030] Preferably, a soft constraint weight is assigned to each region, specifically:

[0031] Obtain the basic weight value of each operation area type and determine the adjustment factor according to the mission stage;

[0032] The product of the basic weight value and the adjustment factor is used as the soft constraint weight of each area in the region.

[0033] Preferably, the node set of the initial probability roadmap is obtained based on the operation area type and the soft constraint weight, specifically:

[0034] Set different minimum sampling density thresholds for different operating area types;

[0035] Randomly generate a candidate node in the workspace, and then generate a A random number in the range, if the random number is greater than the soft constraint weight corresponding to the operation area where the candidate node is located, the candidate node is added to the node set, otherwise, it is rejected, where is the maximum soft constraint weight of all operating areas; candidate nodes are continuously generated until the number of nodes in all operating areas is greater than the minimum sampling density threshold.

[0036] Preferably, the initial global path is obtained according to the node set and the soft constraint weights, specifically as follows:

[0037] The nodes in the node set are used as vertices of the graph. For any two nodes, if the straight line connecting the two nodes does not collide with any physical obstacles in the three-dimensional model, an edge is established between the two nodes. The weight of the edge is obtained according to the length of the edge and the soft constraint weight of the operation area traversed by the edge.

[0038] According to the weight of the edge Search searches for an initial global path from the starting point to the target point in the constructed graph.

[0039] 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:

[0040] Discretizing the initial global path into path segments, each path segment connecting two adjacent path points;

[0041] For each path segment , calculate the priority score ,in, For path segments length, For path segments The maximum dynamic soft constraint weight value of all the operation areas passed, Is a path segment The mean curvature of is a non-negative weight coefficient.

[0042] Preferably, the soft constraint weights of each segment in the global path are mapped to the corresponding temporal pose points on the temporal elastic band to obtain a soft constraint band assigned to the temporal pose points, specifically:

[0043] The collision-free global path is converted into a temporal elastic band consisting of multiple temporal pose points, where each pose point contains position and attitude information;

[0044] 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;

[0045] A hyper-rectangular soft constraint band is defined 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.

[0046] Preferably, the soft constraint belt is used to constrain the deformation of the timing elastic belt, specifically:

[0047] Construct an objective function with trajectory smoothing term, obstacle avoidance term, and soft constraint band penalty term as optimization terms;

[0048] The position and posture of each temporal pose point in the temporal elastic band are adjusted through an iterative optimization algorithm to minimize the objective function.

[0049] The present invention introduces soft constraint weights based on the type of operating area, so that path planning can avoid unexpected areas from the beginning, rather than just treating them as hard obstacles. In addition, the present invention prioritizes the detection of high-risk path segments. Once a collision is detected, only the collision segment is replanned and quickly iterated, thereby improving the real-time performance and response speed of path planning. By converting the collision-free path into a time-series elastic band and using the soft constraint band mapped from the soft constraint weights for constraint optimization, not only is the smoothness and collision-free nature of the path guaranteed, but it also ensures that the path strictly follows the preset soft constraint preferences in terms of morphology, so that the security of the final generated execution path is significantly improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 This is a flow chart of Example 1;

[0051] Figure 2 A schematic diagram of replacing the original path with an alternative path segment;

[0052] Figure 3 is a schematic diagram of the collision-free global path;

[0053] Figure 4 Schematic diagram of converting global path into temporal elastic band;

[0054] Figure 5 This is a structural diagram of Example 2. DETAILED DESCRIPTION

[0055] In this document, relational terms such as first and second, etc., are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "comprises," "comprising," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus comprising a series of elements includes not only those elements, but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of additional identical elements in the process, method, article, or apparatus comprising the element.

[0056] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making any creative efforts shall fall within the scope of protection of the present invention.

[0057] In a first embodiment of the present invention, a method for optimizing the path of a medical robot arm is provided. Figure 1 As shown, the method includes the following steps:

[0058] S1: Obtain the workspace of the medical robot arm, divide the workspace 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 based on the node set and soft constraint weights;

[0059] The workspace is acquired by reading a pre-established 3D model file or by using the robot's sensor systems, such as depth cameras and lidar, to construct an environmental map through real-time scanning. In one embodiment, the workspace model includes not only static obstacles, such as patient tissue and fixtures, but also areas that could potentially affect the robot's motion. The workspace can be divided into formats including, but not limited to, point clouds, voxel grids, and octrees. The workspace can be divided based on, for example, anatomical structures or safety distance requirements. For example, a virtual cylinder or sphere with a variable radius can be defined around identified critical blood vessels or nerves as a high-risk area. Several operating areas and no-go areas can be defined based on the range of motion of surgical instruments and sterility requirements. Each divided operating area is assigned a numerical soft constraint weight, which represents the risk level or undesirability of a path traversing that area. In one embodiment, the closer to the source of danger or critical area, the greater the soft constraint weight, for example, increasing exponentially. In another embodiment, the soft constraint weight is assigned by medical personnel or calculated based on, for example, the density of tissue within the area and / or the distance to the surgical target.

[0060] In an optional embodiment, a non-uniform sampling method is used to generate a node set. During the expansion process of the Rapidly Expanding 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 to a region with low soft constraint weights is increased, or the step size of the expansion step is suppressed to a region with high soft constraint weights. In another embodiment, a deterministic sampling method is used to generate candidate nodes. Then, based on the soft constraint weight value of the node's location and its connection with existing nodes, a decision is made as to whether to add it to the roadmap. Figure 2 A schematic diagram showing the replacement of an original path with an alternative path segment is shown.

[0061] Search for the initial path on the constructed roadmap, preferably using In one embodiment, the cost function in the search process includes, but is not limited to, the length of the path, the maximum soft constraint weight, etc. The initial global path obtained is a path connecting the starting point and the target point with a relatively low cost in the graph. In an optional embodiment, soft constraint weights are assigned to each region, specifically:

[0062] Obtain the basic weight value of each operation area type and determine the adjustment factor according to the mission stage;

[0063] The product of the basic weight value and the adjustment factor is used as the soft constraint weight of each area in the region.

[0064] Different types of operating areas are defined based on the identified major blood vessels, nerve bundles, boundaries of vital organs, and specific areas of concern marked by the surgeon according to the surgical plan, such as high-risk no-go areas, areas adjacent to vital organs, general operating areas, and safe passage areas. A standardized base weight value is assigned to each area type. The base weight value is a relatively fixed value that represents the sensitivity of that type of area under normal circumstances or the degree to which the robot is not expected to enter. For example, the base weight value of a high-risk no-go area will be significantly higher than that of a general operating area. The corresponding adjustment factor is determined based on the specific stage of the current medical robot's task. A complete surgical or medical operation process can be broken down into several stages with different characteristics and requirements, such as the instrument introduction stage, lesion exposure stage, tissue resection / suturing stage, and instrument withdrawal stage. For each mission phase, an adjustment factor is preset for each type of operation area. For example, during the instrument introduction phase, the path may need to quickly pass through the general operation area. In this case, the adjustment factor of these areas can be set to a value less than 1 to reduce their soft constraint weight and encourage the path to pass through. During the tissue resection / suturing phase, the adjustment factor for the areas adjacent to important organs is set to a value greater than 1 to further enhance avoidance of these areas. The base weight value of a specific operation area type is multiplied by the corresponding adjustment factor for the current mission phase to obtain the dynamic soft constraint weight of the area at that specific moment.

[0065] In an optional embodiment, the node set of the initial probability roadmap is obtained based on the operation area type and the soft constraint weight, specifically:

[0066] Set different minimum sampling density thresholds for different operating area types;

[0067] Randomly generate a candidate node in the workspace, and then generate a A random number in the range, if the random number is greater than the soft constraint weight corresponding to the operation area where the candidate node is located, the candidate node is added to the node set, otherwise, it is rejected, where is the maximum soft constraint weight of all operating areas; candidate nodes are continuously generated until the number of nodes in all operating areas is greater than the minimum sampling density threshold.

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

[0069] By combining the minimum sampling density threshold and the probabilistic node acceptance method based on soft constraint weights, while ensuring that PRM has the most basic coverage of the entire workspace, dense sampling is prioritized in safe areas with low soft constraint weights, while sparse sampling is performed in dangerous areas with high soft constraint weights to reduce the possibility of generating paths that pass through dangerous areas.

[0070] To ensure that the path search process not only pursues the shortest geometric distance, but also actively avoids path segments that pass through areas with high soft constraint weights, in an optional embodiment, the initial global path is obtained based on the node set and the soft constraint weights, specifically:

[0071] The nodes in the node set are regarded as the vertices of the graph. For any two nodes, if the straight line connecting them does not collide with any physical obstacles in the 3D model, an edge is established between the two nodes. The weight of the edge is obtained based on the length of the edge and the soft constraint weight of the operation area traversed by the edge.

[0072] According to the weight of the edge Search searches for an initial global path from the starting point to the target point in the constructed graph.

[0073] Take all nodes in the node set as vertices of the graph, and for any pair of nodes in the graph ,connect and The straight path segment does not collide with any known physical obstacles in the 3D workspace model. and Create an edge between them and calculate the weight of this edge. In one embodiment, the edge weight is calculated as follows ,in is the effective soft constraint weight contribution of the edge. Preferably, the maximum soft constraint weight of the operating area through which the edge passes is taken as , is an adjustable weight coefficient. After constructing the weighted graph, The search algorithm finds an initial global path from a specified start node to a target node. The algorithm calculates an evaluation function f(v) = g(v) + h(v). Here, g(v) is the actual cumulative path cost from the starting node to the current node, calculated by summing the weights of all edges along the path. g(v) reflects the path length and soft constraint traversal. h(v) is a heuristic function that estimates the minimum remaining cost from the current node to the target node, representing the straight-line Euclidean distance between the current and target nodes. The algorithm begins at the starting node and iteratively expands by selecting the node with the smallest f(v) value, adding its neighbor nodes to the open list, until the target node is selected and removed from the open list. By backtracing the parent node pointer chain from the target node to the starting node, an initial global path is constructed that takes into account both length and soft constraint weights.

[0074] S2: Prioritize each path segment of the initial global path based on its curvature and the soft constraint weights of the traversed operating area, and 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, replan the path of the first collision segment and generate an alternative path segment. After updating the initial global path with the alternative path segment, re-execute lazy collision detection based on the priority tag on the updated global path until a collision-free global path is obtained.

[0075] The initial global path is segmented, for example, into fixed lengths. A priority score is calculated for each path segment. In one embodiment, the priority is calculated based on factors such as the distance from the path segment to the nearest obstacle or the torsion of the path segment itself, as well as the soft constraint weights of the region traversed by the segment. More specifically, the priority is a weighted sum of the SCW value and the inverse of the distance to the obstacle.

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

[0077] In order 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 traversed operation area, specifically:

[0078] Discretize the initial global path into path segments, each of which connects two adjacent path points;

[0079] For each path segment , calculate the priority score ,in, For path segments length, For path segments The maximum dynamic soft constraint weight value of all the operation areas passed, Is a path segment The mean curvature of is a non-negative weight coefficient.

[0080] The longer the path segment, the greater the probability of intersecting an obstacle. The soft constraint weight of the area traversed by the path segment is higher, indicating that the robot is not allowed to enter this area. If the path must be traversed, its safety must be prioritized. The greater the curvature of the path segment, the more severe the segment's curvature, which may require more complex posture adjustments and increase the likelihood of collision with the robot or the environment. By prioritizing collision detection for these high-priority path segments, problems can be discovered and addressed early. If a collision occurs and replanning is required, adjustments can be made as soon as possible, thus avoiding wasting detection resources on unnecessary subsequent path segments.

[0081] S3, convert the global path into a timing 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 timing pose points on the timing elastic band, and assign a soft constraint band to the timing pose points. Use the soft constraint band to constrain the deformation of the timing elastic band to obtain the final execution path.

[0082] The obtained discrete and collision-free global path is converted into a continuous trajectory representation using B-spline curves or polynomial splines, which contains not only the position information of the path points, but also the posture, timestamp, speed and other information to form a time series trajectory, such as Figure 4 As shown. The soft constraint weight is converted into a repulsive force or potential field acting on the trajectory control point, so that the area with higher soft constraint weight has a greater repulsive force, or a penalty term proportional to the soft constraint weight is added to the trajectory optimization objective function in this area. The shape of the trajectory is adjusted by sequential quadratic programming SQP or gradient descent method, such as moving the control point of the B-spline. The optimization objective function includes but is not limited to minimizing curvature, maintaining a safe distance from obstacles and / or soft constraint penalty terms. Under the premise of satisfying the hard no-collision constraint, the optimization process minimizes the objective function to make the trajectory as smooth as possible, and actively avoids areas with high soft constraint weights, and finally obtains a smooth trajectory that can be executed by the robot.

[0083] In an optional embodiment, the soft constraint weights of each segment in the global path are mapped to the corresponding temporal pose points on the temporal elastic band to obtain a soft constraint band assigned to the temporal pose points, specifically:

[0084] The collision-free global path is converted into a temporal elastic band consisting of multiple temporal pose points, where each pose point contains position and attitude information;

[0085] 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;

[0086] A hyper-rectangular soft constraint band is defined in the configuration space for each pose point, and the size of each dimension of the soft constraint band is inversely proportional to the soft constraint weight.

[0087] The obtained collision-free global path is converted into a representation of a temporal elastic band TEB. TEB consists of a sequence of multiple temporal pose points arranged in chronological order, including position, posture, timestamp, etc. The pose points are connected elastically to form a continuous trajectory. The conversion method includes but is not limited to interpolation of the original path points. A soft constraint band is assigned to each pose point. Specifically, the original path segment corresponding to the pose point on the original collision-free global path is determined, and the soft constraint weight value of the operating area associated with the path segment is obtained. If multiple operating areas are associated, the maximum soft constraint weight is taken. A super-rectangular soft constraint band is defined for the pose point in its configuration space. Preferably, the center of the super-rectangle is the current pose of the pose point, and its allowed 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 The bigger, The smaller it is, the narrower the range in which the pose point is allowed to move in the x-direction. Similarly, the corresponding constraint range can be defined in the pose dimension.

[0088] In one embodiment, a soft constraint band is used to constrain the deformation of the temporal elastic band, specifically:

[0089] Construct an objective function with trajectory smoothing term, obstacle avoidance term, and soft constraint band penalty term as optimization terms;

[0090] The position and posture of each temporal pose point in the temporal elastic band are adjusted through an iterative optimization algorithm to minimize the objective function.

[0091] An objective function is constructed with trajectory smoothing, obstacle avoidance, and soft constraint band penalty as optimization terms. These three optimization terms are weighted and summed to obtain the overall objective function. Then, an iterative optimization algorithm, such as sequential quadratic programming or gradient descent, is used to adjust the position and attitude parameters of all sequential pose points. The optimization process begins with the initial state of the TEB. At each iteration, the gradient of the objective function is calculated, and the pose point parameters are updated in a direction that decreases the objective function value until the change in the objective function value is less than a threshold or the maximum number of iterations is reached.

[0092] In an optional 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 another optional embodiment, the trajectory smoothing term also includes the posture, and the smoothing term of the angular velocity or angular acceleration can also be defined in a similar manner. The obstacle avoidance term is used to penalize situations where the trajectory is too close to a physical obstacle. For each pose point, the distance to the nearest physical obstacle is calculated. The obstacle avoidance term is a function whose penalty value increases when the distance is less than a 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 its 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 size of the penalty is proportional to or squared with the distance beyond the boundary.

[0093] In a second embodiment of the present invention, a medical robot arm path optimization system is provided. Figure 5 As shown, the system includes the following modules:

[0094] The global path generation module is used to obtain the workspace of the medical robot arm, divide the workspace into different operation areas, and assign soft constraint weights to each area. The node set of the initial probabilistic roadmap is obtained based on the soft constraint weights, and the initial global path is obtained based on the node set and soft constraint weights.

[0095] A collision detection module is used to prioritize each path segment of the initial global path based on the curvature and the soft constraint weights of the traversed operating area, and to 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, the path of the first collision segment is replanned and an alternative path segment is generated. After the initial global path is updated with the alternative path segment, lazy collision detection based on the priority tag is re-performed on the updated global path until a collision-free global path is obtained.

[0096] The path optimization module is used to convert the global path into a timing 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 timing pose points on the timing elastic band, and assign soft constraint bands to the timing pose points. The soft constraint bands are used to constrain the deformation of the timing elastic band to obtain the final execution path.

[0097] Preferably, a soft constraint weight is assigned to each region, specifically:

[0098] Obtain the basic weight value of each operation area type and determine the adjustment factor according to the mission stage;

[0099] The product of the basic weight value and the adjustment factor is used as the soft constraint weight of each area in the region.

[0100] Preferably, the node set of the initial probability roadmap is obtained based on the operation area type and the soft constraint weight, specifically:

[0101] Set different minimum sampling density thresholds for different operating area types;

[0102] Randomly generate a candidate node in the workspace, and then generate a A random number in the range, if the random number is greater than the soft constraint weight corresponding to the operation area where the candidate node is located, the candidate node is added to the node set, otherwise, it is rejected, where is the maximum soft constraint weight of all operating areas; candidate nodes are continuously generated until the number of nodes in all operating areas is greater than the minimum sampling density threshold.

[0103] Preferably, the initial global path is obtained according to the node set and the soft constraint weights, specifically:

[0104] The nodes in the node set are regarded as the vertices of the graph. For any two nodes, if the straight line connecting them does not collide with any physical obstacles in the 3D model, an edge is established between the two nodes. The weight of the edge is obtained based on the length of the edge and the soft constraint weight of the operation area traversed by the edge.

[0105] According to the weight of the edge Search searches for an initial global path from the starting point to the target point in the constructed graph.

[0106] 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:

[0107] Discretize the initial global path into path segments, each of which connects two adjacent path points;

[0108] For each path segment , calculate the priority score ,in, For path segments length, For path segments The maximum dynamic soft constraint weight value of all the operation areas passed, Is a path segment The mean curvature of is a non-negative weight coefficient.

[0109] Preferably, the soft constraint weights of each segment in the global path are mapped to the corresponding temporal pose points on the temporal elastic band to obtain a soft constraint band assigned to the temporal pose points, specifically:

[0110] The collision-free global path is converted into a temporal elastic band consisting of multiple temporal pose points, where each pose point contains position and attitude information. For each pose point, the path segment corresponding to it on the original global path is determined, and the soft constraint weight of the operation area associated with the path segment is obtained. For each pose point, a hyperrectangular soft constraint band is defined in the configuration space, and the size of each dimension of the soft constraint band is inversely proportional to the soft constraint weight.

[0111] Preferably, a soft constraint band is used to constrain the deformation of the temporal elastic band, specifically: constructing an objective function with trajectory smoothing term, obstacle avoidance term, and soft constraint band penalty term as optimization terms; adjusting the position and posture of each temporal pose point in the temporal elastic band through an iterative optimization algorithm to minimize the objective function.

[0112] In a third embodiment of the present invention, the present invention further provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the method of the first embodiment is implemented.

[0113] In a fourth embodiment of the present invention, the present invention further provides a computer device, which includes at least a memory and a processor. A computer program is stored in the memory, and when the computer program is executed by the processor, the method of the first embodiment is implemented.

[0114] Through the above description of the embodiments, those skilled in the art will clearly understand that each embodiment can be implemented using a general-purpose hardware platform, or alternatively, through a combination of hardware and software. Based on this understanding, the essence of the above technical solution, or the portion that contributes to the prior art, can be embodied in the form of a computer product. The present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0115] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it, and other embodiments may also be used. Although the present invention has been described in detail with reference to the aforementioned embodiments, ordinary technicians in this field should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A method for optimizing the path of a medical robot arm, characterized in that: The method comprises the following steps: Obtain the workspace of the medical robot arm, divide the workspace into different operation areas, and assign soft constraint weights to each area; Obtaining a node set of an initial probabilistic roadmap based on the soft constraint weights, and obtaining an initial global path based on the node set and the soft constraint weights; Priority marking is performed on each path segment of the initial global path based on the curvature and the soft constraint weight of the traversed operating area, and lazy collision detection is performed on each path segment in descending order of priority. When the first path segment collides with an obstacle is detected, the path of the first collision segment is replanned and an alternative path segment is generated. After the initial global path is updated with the alternative path segment, lazy collision detection based on the priority marking is re-performed on the updated global path until a collision-free global path is obtained; The global path is converted into a timing elastic band according to the operation area where the path is located. The soft constraint weights of each segment in the global path are mapped to the corresponding timing pose points on the timing elastic band to obtain a soft constraint band assigned to the timing pose points. The soft constraint band is used to constrain the deformation of the timing elastic band to obtain the final execution path.

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

3. The method according to claim 1, wherein The node set of the initial probability roadmap is obtained based on the operation area type and soft constraint weight, specifically: Set different minimum sampling density thresholds for different operating area types; Randomly generate a candidate node in the workspace, and then generate a A random number in the range, if the random number is greater than the soft constraint weight corresponding to the operation area where the candidate node is located, the candidate node is added to the node set, otherwise, it is rejected, where is the maximum soft constraint weight of all operating areas; candidate nodes are continuously generated until the number of nodes in all operating areas is greater than the minimum sampling density threshold.

4. The method according to claim 1, wherein The initial global path is obtained according to the node set and the soft constraint weights, specifically: The nodes in the node set are taken as vertices of the graph. For any two nodes, if the straight line connecting the two nodes does not collide with any physical obstacles in the three-dimensional model, an edge is established between the two nodes. The weight of the edge is obtained 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 Search searches 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 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: Discretizing the initial global path into path segments, each path segment connecting two adjacent path points; For each path segment , calculate the priority score ,in, For path segments length, For path segments The maximum dynamic soft constraint weight value of all the operation areas passed, Is a path segment The mean curvature of is a non-negative weight coefficient.

6. The method according to claim 1, wherein The soft constraint weights of each segment in the global path are mapped to the corresponding temporal pose points on the temporal elastic band to obtain the soft constraint band assigned to the temporal pose points, specifically: The collision-free global path is converted into a temporal elastic band consisting of multiple temporal pose points, where each pose point contains 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 the path segment; A hyper-rectangular soft constraint band is defined 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, wherein The soft constraint band is used 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; The position and posture of each temporal pose point in the temporal elastic band are adjusted through an iterative optimization algorithm to minimize the objective function.

8. A medical robot arm path optimization system, characterized in that: The system includes the following modules: The global path generation module is used to obtain the workspace of the medical robot arm, divide the workspace into different operation areas, and assign soft constraint weights to each area; Obtaining a node set of an initial probabilistic roadmap based on the soft constraint weights, and obtaining an initial global path based on the node set and the soft constraint weights; a collision detection module for assigning priorities to each path segment of the initial global path based on the curvature and the soft constraint weights of the traversed operating area, and performing lazy collision detection on each path segment in descending order of priority; when the first path segment colliding with an obstacle is detected, replanning the path of the first collision segment and generating an alternative path segment; after updating the initial global path with the alternative path segment, re-performing lazy collision detection based on the priority marking on the updated global path until a collision-free global path is obtained; The path optimization module is used to convert the global path into a temporal elastic band according to the operation area in which 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 region, specifically: Obtain the basic weight value of each operation area type and determine the adjustment factor according to the mission stage; The product of the basic weight value and the adjustment factor is used as the soft constraint weight of each area in the region.

10. The system according to claim 8, wherein The node set of the initial probability roadmap is obtained based on the operation area type and soft constraint weight, specifically: Set different minimum sampling density thresholds for different operating area types; Randomly generate a candidate node in the workspace, and then generate a A random number in the range, if the random number is greater than the soft constraint weight corresponding to the operation area where the candidate node is located, the candidate node is added to the node set, otherwise, it is rejected, where is the maximum soft constraint weight of all operating areas; candidate nodes are continuously generated until the number of nodes in all operating areas is greater than the minimum sampling density threshold.

Images