Real-time dynamic path planning method and system for medical and nursing robots in complex dynamic scenarios

Abstract

This invention discloses a real-time dynamic path planning method and system for medical nursing robots in complex dynamic scenarios. The method includes: S1, loading a map and performing binarization processing; S2, processing the free space X... free S3. After the robot's starting point is determined, the improved real-time exploratory search tree path planning method is used to search the map for a path; S4. After the destination is determined, the robot moves towards the destination along the path at a certain speed, and the root node of the exploratory search tree moves with the robot; S5. If the target point changes during the robot's movement, the tree is rewired from the root node; S6. If obstacles are encountered during the robot's movement, the nodes in the exploratory search tree occupied by obstacles need to be blocked first to avoid the robot colliding with the obstacles.

Description

Technical Field

[0001] This invention belongs to the field of robot path planning technology, and in particular relates to a real-time dynamic path planning method and system for medical care robots in complex dynamic scenarios. Background Technology

[0002] Path planning is a hot topic in robotics research. Its goal is to find a continuous trajectory connecting the starting state and the target state for a robot in an unobstructed space. Path planning algorithms have wide applications in many fields, including service robot navigation, automatic detection systems, industrial automation, autonomous vehicles, and robotic surgery.

[0003] Path planning algorithms can be categorized by sampling method into two main types: graph search-based and sampling-based. Furthermore, the planned paths often cannot be used directly and require trajectory optimization. Trajectory optimization includes aspects such as trajectory representation methods, trajectory optimization objectives, and trajectory constraint methods.

[0004] Path planning methods based on graph search mainly include Dijkstra's algorithm and A* algorithm. However, since the robot's environment is continuous and time-varying, these methods usually cannot be directly applied to path planning and require discretization of the map. Due to the different spatial discretization methods, the effectiveness of these algorithms largely depends on the accuracy of their discretization. Typically, to improve the accuracy of the solution, efficiency must be sacrificed, and the computational cost increases exponentially with the problem size.

[0005] Sampling-based methods mainly include the Rapidly Exploring Random Tree (RAR) algorithm, the Probable Random Tree (PRM) algorithm, and the Diverse Map Wafer (DWA) algorithm. The advantage of these algorithms is that they do not require discretization of the map and can quickly find feasible solutions to complex problems (such as high-dimensional problems). However, due to the inherent randomness of their sampling methods, the solutions they obtain are asymptotically optimal. Sampling-based methods are widely used to guide the sampling process, but their generalization ability and interpretability are poor, and they cannot guarantee safe and stable operation in all situations. Unpredictable errors caused by these methods can lead to catastrophic consequences.

[0006] In summary, the aforementioned algorithms must balance the advantages and disadvantages of search paths with the length of search time. When changing the target point and adding obstacles are allowed, effectively utilizing previously sampled information to quickly replan the route is a very challenging problem. Furthermore, the path search process consumes a significant amount of time and memory searching for unnecessary paths; therefore, distinguishing and avoiding the search of similar paths is an urgent issue that needs to be addressed.

[0007] In a medical care environment, the ward scene is basically fixed, such as the position of the bed and the cabinet, that is, the map environment is basically unchanged. However, there are many dynamic obstacles, such as the movement of patients, family members and doctors. In response to the above scenarios, this invention proposes a real-time dynamic path planning method and system for medical care robots in complex dynamic scenarios. Summary of the Invention

[0008] To overcome the shortcomings of existing technologies, this invention provides a real-time dynamic path planning method and system for medical care robots in complex dynamic scenarios. It mainly relies on a map convex polygon reconstruction method and a real-time exploratory search tree method. The map convex polygon reconstruction method mainly avoids searching for the same type of path during the path planning process. The real-time exploratory search tree dynamic path planning method does not discard the previously searched path when the target point is changed or obstacles are added, thus realizing rapid route replanning.

[0009] The technical solution adopted in this invention is:

[0010] A real-time dynamic path planning method for medical care robots in complex dynamic scenarios, characterized in that the method specifically includes the following steps:

[0011] S1. Load a map and binarize it, where the black areas represent impassable regions and the white areas represent passable regions, and refer to this as free space X. free Then, polygon fitting is performed on the map to extract boundary and corner information.

[0012] S2, regarding free space X free The fitted polygons are used to reconstruct the map using convex polygons, and the free space X is used for this reconstruction. free The set is divided into multiple convex polygons. Due to the properties of convex polygons, the paths inside the convex polygons are homotopic paths, which can be regarded as similar paths. The path search can be performed directly on the boundary of the convex polygon, thereby avoiding repeated path search and improving search efficiency.

[0013] S3. Once the robot's starting point is determined, path planning begins. An improved real-time exploratory search tree path planning method is used to search the map for a path; specifically, this includes:

[0014] S31. Prioritize searching the midpoint of each cutting line of the convex polygon to quickly obtain the path from the starting point to each convex polygon. Therefore, no matter where the endpoint is, you only need to determine which convex polygon on the map the endpoint is in to quickly obtain a suboptimal path.

[0015] S32. After traversing every cutting line of all convex polygons, randomly and repeatedly sample points on the cutting lines of the convex polygons to optimize the path length.

[0016] S4. After the destination is determined, the robot moves towards the destination along the path at a certain speed, and the root node of the exploratory search tree moves with the robot, so as not to discard the previously sampled path, but also not to wait for the tree to be fully built. Therefore, the expansion of the search tree and the movement of the robot are interleaved.

[0017] S5. In the case of the target point changing during the robot's movement, since the root node of the exploratory search tree moves with the robot and the wiring is rewired from the root node of the tree, a suboptimal solution can be quickly obtained by using the previously sampled path, and the path length can be continuously optimized in the subsequent tree sampling process.

[0018] S6. In the event of obstacles encountered during robot movement, the nodes occupied by the obstacles in the exploratory search tree need to be blocked first to prevent the robot from colliding with the obstacles. Then, it is necessary to determine which convex polygons the obstacles are in. In the subsequent exploratory search tree sampling process, it is only necessary to sample the interior of the convex polygons occupied by the obstacles. Since the sampling is restricted to the interior of the convex polygons where the obstacles are located, the path search efficiency is greatly improved and obstacle avoidance is better achieved.

[0019] Furthermore, in step S1, among the extracted corner point information, it is stipulated that the outermost layer of corner points are arranged counterclockwise and the innermost layer of corner points are arranged clockwise. The arrangement is determined by the right-hand rule to determine which side the free space is on, that is, the right thumb points upward and the four fingers point in the direction of the vector, and the direction of the palm is the free space.

[0020] Furthermore, in step S2, the process of reconstructing the map convex polygon includes:

[0021] S21: Select the concave point;

[0022] Erosion and dilation operations are performed on the free space to eliminate noise. Then, polygon fitting is performed. The polygon fitting result contains the vertices of each polygon. The vertices in the polygon fitting result are traversed in order, with the vertices of the outer layer of the polygon arranged counterclockwise and the vertices of the inner layer of the polygon arranged clockwise. The cross product of the two sides of the vertex is used to determine whether the point is a concave point, that is, a point with an angle greater than 180 degrees.

[0023] Specifically, let the current point be P. now ,P now The previous point is P end The next point is P. start Vector v1 is Vector v2 is According to the properties of the cross product of vectors, when v1×v2>0, P end To the left of vector v1; according to the right-hand rule, the free space is to the right of v1, i.e., P. now An angle greater than 180 degrees indicates a concave point;

[0024] S22: Calculate the visible point of the concave point;

[0025] After obtaining all the concave points on the map, calculate the visible point for each concave point, which is the point that can be reached by a straight line without touching obstacles or other dividing lines. The specific method is as follows:

[0026] Traverse all boundaries on the map, first let V be the concave point of the visible point to be calculated for the current point. p The starting point for traversing the boundary is L. S The endpoint is L E The currently visited point is P, and the vector is... vector vector If point P is blocked, then point P is between vector v3 and vector v4, that is, to the right of v3 and to the left of v4, or to the left of v3 and to the right of v4, and (v3×v4)*(v5×v4)≤0;

[0027] If the above conditions are met, then points P and V p If not on the same side of the boundary points, then point P

[0028] Blocked, set If (v6×v7)*(v8×v7)≤0, then P and point V p Not on the same side of the boundary point;

[0029] If point P satisfies both (v3×v4)*(v5×v4)≤0 and (v6×v7)*(v8×v7)≤0, then P is blocked; otherwise, P is a visible point.

[0030] S23: Select the optimal visible point of the concave point;

[0031] Among all visible points, the optimal visible point is calculated. The optimal point is defined as one that is as close as possible to the angle bisector of the concave point, while simultaneously satisfying the condition of being as close as possible to the concave point and not intersecting with existing subdivision lines. The specific method is as follows:

[0032] Let the concave point be V. p The visible point is P1, and the angle bisector of the concave point is calculated as v9. v9 is perpendicular to V. p Starting from v and normalizing it, 10 It is V p The normalized vector with P1 as the starting point and P1 as the ending point is...

[0033] The objective function for the optimal viewpoint is defined as shown in Formula 1:

[0034]

[0035] We need to select the visible point that minimizes the objective function, prioritizing the point closest to the angle bisector v9. Negative values ​​ensure Priority ratio within the range First, to account for distance, the distance between the visible point and the concave point is calculated as L. This distance factor is then added to the objective function, resulting in the final objective function shown in Formula 2.

[0036]

[0037] The process of selecting the optimal visible point of a concave point, connecting the concave point to the optimal visible point, cutting the concave point into a convex point, and connecting the concave point to the visible point can be regarded as a map convex polygon reconstruction. The connecting line is called the partition line. If the angle of the concave point is still greater than 180 degrees after one partition, it is partitioned again until there are no more concave points. After completing one partition, it is also necessary to check whether the optimal visible point connecting the concave points is a concave point. If it is and has been partitioned into a convex point, it can be skipped directly when traversing the concave points.

[0038] The specific method for determining whether the angle after partitioning is greater than 180 degrees is as follows: Let the concave point be V. p The point preceding the concave point is P. end The next point is P. start The viewpoint is P1, and the vector is v. 11 for vector v 12 for vector v 13 for Only when (v 11 ×v 13 )≤0,(v 12 ×v 13 There are no angles greater than 180 degrees only when ) ≥ 0;

[0039] By subdividing each concave point on the map, the free space can be divided into a set of multiple convex polygons.

[0040] Furthermore, in step S2, for paths inside a convex polygon that are homotopic, they can be considered as similar paths, specifically as follows:

[0041] Let X be a topological space, denoted by P(X) for all roads in X, P(X; x0, x1) for all roads starting at x0 and ending at x1, and Ω(X, x0) for all loops with x0 as their base. Let f, g ∈ P(X; x0, x1), if there exists a continuous mapping F: (I 2 →X), such that F(s,0)=f(s), F(s,1)=g(s), F(0,t)=x0, F(1,t)=x1, then F is called path homotopy from f to g, denoted as This means that these two roads can change continuously and are of the same type.

[0042] Furthermore, in step S5, the root node of the exploratory search tree moves with the robot, and the movement of the node and the updating of the search tree alternate. In one iteration, the root node first moves with the robot, and then the tree search is performed. The tree search includes tree expansion and rewiring. Tree expansion is node sampling guided by the partition line. Tree rewiring is divided into rewiring from the root node and random rewiring. Rewiring from the root node can make good use of the previously sampled information to solve the situation of target point change and obstacle avoidance. Random rewiring is used for path optimization.

[0043] Furthermore, during the expansion of the tree, it is necessary to find the point in the search tree closest to the sampling point. A dynamic reference point tree data structure is adopted. The reference point tree selects a point v in the dataset as the "pivot" and stores the distances between other points and it in a region with a radius of μ. Then, a subset of this region is processed in the same way to form a binary tree, thus obtaining a reference point tree. Its calculation formula is shown in Formula 3:

[0044]

[0045] Where S1 and S2 are two subtrees, and d(s,v) is the formula for calculating the distance between points s and v, using Euclidean distance.

[0046] Furthermore, in step S6, as the number of obstacles increases, the sampling probability around newly added obstacles should increase, while the sampling probability around old obstacles should decrease. The specific implementation method is as follows:

[0047] Define a special sampling probability function, and set the obstacle array as Obs = [obs1, obs2, ..., obsn], where the obstacles are arranged in the order of their appearance, with the first obstacle appearing as obs1 and the nth obstacle appearing as obsn; define a probability weight array according to the order of appearance, with the first obstacle appearing having a weight of 1 and the nth obstacle appearing having a weight of n, and the weight array is weights = [1, 2, ..., n]. When calculating the probability, the probability of the i-th obstacle being selected is as shown in Formula 4:

[0048]

[0049] The sum(weights) function sums the values ​​in the weights array.

[0050] A system for implementing a real-time dynamic path planning method for medical care robots in complex dynamic scenarios is characterized by comprising: an environmental perception module, a location positioning module, a data analysis module, a data recording module, and a data transmission module.

[0051] The environmental perception module is used to acquire environmental data around the intelligent medical care robot, including map creation, semantic information, and information on static and dynamic obstacles;

[0052] The positioning module is used to acquire high-precision positioning information of the medical care robot, including odometry information, IMU information, and RTK information;

[0053] The data analysis module is used for scene recognition in the complex environment around the medical care robot, to establish rule-based decision output information, and to obtain the result of the current scene judgment.

[0054] The data recording module is used to record sensor data and initialization information of analysis results;

[0055] The data transmission module is used to transmit the analyzed data and recorded data to the main controller, including global map information, semantic information, obstacle information, odometer information, IMU information, RTK information, and scene judgment information.

[0056] Compared with the prior art, the beneficial effects of the present invention are reflected in:

[0057] 1. This invention uses a map convex polygon reconstruction method, which reduces the search for repeated paths during the exploratory search tree expansion process by sampling on the partition line; for dynamic obstacles, the obstacle avoidance effect is greatly improved by restricting the sampling points to the convex polygon occupied by the obstacle.

[0058] 2. During the robot's movement, the root node of the exploratory search tree moves with the robot and rewires from the root node. The robot's movement and rewires are carried out alternately, so the previously explored paths can be fully utilized to achieve real-time path planning and avoid dynamic obstacles. Attached Figure Description

[0059] Figure 1 This is a schematic diagram of a real-time dynamic path planning method and system for medical nursing robots in complex dynamic scenarios, as provided in Embodiment 1 of the present invention.

[0060] Figure 2a This is a diagram showing the effect of map binarization in Embodiment 1 of the present invention.

[0061] Figure 2b This is a diagram showing the map polygon fitting effect in Embodiment 1 of the present invention.

[0062] Figure 2c This is a map fitting effect diagram from Embodiment 1 of the present invention.

[0063] Figure 2d for Figure 2c Detailed diagram of the map fitting effect in region A.

[0064] Figure 3 This is a diagram showing the arrangement order of corner points after polygon fitting in Embodiment 1 of the present invention.

[0065] Figure 4 This is a diagram of the concave point determination method in Embodiment 1 of the present invention.

[0066] Figure 5 This is a diagram showing the results of extracting map concave points in Embodiment 1 of the present invention.

[0067] Figure 6 This is a schematic diagram illustrating the principle of determining whether something is blocked in Embodiment 1 of the present invention.

[0068] Figure 7 This is a schematic diagram illustrating the principle of determining whether something is blocked in Embodiment 1 of the present invention.

[0069] Figure 8 This is an image showing the extraction results of all visible points of a concave point in Embodiment 1 of the present invention.

[0070] Figure 9 This is a schematic diagram showing the relationship between the concave angle bisector and the visible point in Embodiment 1 of the present invention.

[0071] Figure 10 This is the target function graph for selecting viewpoints in Embodiment 1 of the present invention.

[0072] Figure 11 This is a diagram showing the result of a single-step map convex polygon reconstruction in Embodiment 1 of the present invention.

[0073] Figure 12 This is a schematic diagram illustrating the principle of determining whether a segmented part is a concave point in Embodiment 1 of the present invention.

[0074] Figure 13 This is a map convex polygon reconstruction result diagram in Embodiment 1 of the present invention.

[0075] Figure 14 This is a schematic diagram of the homotopy path principle in Embodiment 1 of the present invention.

[0076] Figure 15a This describes the sampling effect at the midpoint of the partition line in Embodiment 1 of the present invention. Figure 1 .

[0077] Figure 15b Figure 2 shows the sampling effect at the midpoint of the dividing line in Embodiment 1 of the present invention.

[0078] Figure 16 This is a sampling effect diagram on the dividing line in Embodiment 1 of the present invention.

[0079] Figure 17a The reference point tree principle in Embodiment 1 of the present invention Figure 1 .

[0080] Figure 17b This is the second schematic diagram of the reference point tree in Embodiment 1 of the present invention.

[0081] Figure 18a This is a diagram showing the path planning effect after the first target change in Embodiment 1 of the present invention.

[0082] Figure 18b This is a diagram showing the path planning effect after the second target change in Embodiment 1 of the present invention.

[0083] Figure 19a This is a sampling effect diagram near an obstacle in Embodiment 1 of the present invention.

[0084] Figure 19b for Figure 19a Detailed image of the sampling effect near the obstacle at point B in the middle.

[0085] Figure 20a Comparison of obstacle avoidance effects in Embodiment 1 of the present invention Figure 1 .

[0086] Figure 20b Figure 2 shows a comparison of the obstacle avoidance effects in Embodiment 1 of the present invention.

[0087] Figure 20c for Figure 20a Detailed comparison of obstacle avoidance effects at point C.

[0088] Figure 20d for Figure 20b Detailed comparison of obstacle avoidance effects at point D.

[0089] Figure 21a Comparison of dynamic obstacle avoidance effects in Embodiment 1 of the present invention Figure 1

[0090] Figure 21b Figure 2 shows a comparison of the dynamic obstacle avoidance effects in Embodiment 1 of the present invention.

[0091] Figure 21c for Figure 21a Detailed comparison of the dynamic obstacle avoidance effect at point E.

[0092] Figure 21d for Figure 21b Detailed comparison of the dynamic obstacle avoidance effect at point F.

[0093] Figure 22 This is a schematic diagram illustrating the principle of obstacle avoidance by turning the sampling point in Embodiment 1 of the present invention.

[0094] Figure 23 This is a flowchart of the environmental perception and localization mapping process in Embodiment 2 of the present invention. Detailed Implementation

[0095] The specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are for illustration and explanation only and are not intended to limit the scope of the present invention.

[0096] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other.

[0097] The present invention will now be described in detail with reference to the accompanying drawings and exemplary embodiments.

[0098] Example 1

[0099] like Figure 1 As shown in this embodiment, a real-time dynamic path planning method and system for medical nursing robots in complex dynamic scenarios mainly utilizes a map convex polygon reconstruction method to divide the free space into a set of multiple convex polygons. Paths within each convex polygon can be considered as similar paths. Real-time path planning based on an exploratory search tree can sample along the partition lines of the convex polygons, reducing the search for repeated paths and accelerating path search efficiency. Furthermore, to address situations where the target point changes or dynamic obstacles appear during robot movement, the exploratory search tree dynamically updates its sampling tree by rewiring from the root node and random rewiring, fully utilizing previous sampling information to achieve fast real-time dynamic path planning.

[0100] The basic idea of ​​the exploratory tree search algorithm is to start from the initial point, randomly sample nodes in free space, and use nearest neighbor search to find the tree node closest to that node. Then, the two nodes are connected through a join operation, and a feasibility check is performed. If the connection path is feasible, the new node is added to the tree, and the above steps are repeated until the target state is reached.

[0101] This embodiment presents a real-time dynamic path planning method for medical care robots in complex dynamic scenarios, which mainly includes the following steps:

[0102] Step 1: Load a map, perform erosion and dilation operations to remove noise, and then binarize it. The result is as follows. Figure 2a As shown, the black areas are impassable, and the white areas are passable; this is referred to as free space X. free Then, polygon fitting is performed on the map, and the result is as follows. Figure 2b As shown, the fitted comparison is as follows: Figure 2c As shown, the details of the fitting effect are as follows: Figure 2d As shown, this is mainly for extracting boundary and corner information from the map.

[0103] In the extracted corner point information, the outermost layer of corner points is arranged counterclockwise, and the innermost layer is arranged clockwise, as shown in the following arrangement: Figure 3 As shown in the figure, the outermost corner point is represented by a circle, and the inner corner point is represented by a square. The outer corner points are arranged counterclockwise, and the inner corner points are arranged clockwise. Therefore, the right-hand rule can be used to determine which side of the free space is on. The specific method is as follows: with the right thumb pointing upward and the four fingers pointing in the direction of the vector, the direction of the palm is the free space.

[0104] Step 2: Reconstruct the map using convex polygons from the fitted polygons in free space, and then reconstruct the free space X... free The set is divided into multiple convex polygons. Due to the properties of convex polygons, the paths inside the convex polygons are homotopic paths, which can be regarded as similar paths. Therefore, the path search can be performed directly on the boundary of the convex polygons, thereby avoiding repeated path searches and improving search efficiency.

[0105] The map convex polygon reconstruction process includes:

[0106] Step 2.1, select the concave point;

[0107] The process iterates through the vertices of the polygon fitting result in sequence, with vertices on the outer layer of the polygon arranged counterclockwise and those on the inner layer clockwise. A point is determined to be concave (i.e., an angle greater than 180 degrees) by calculating the cross product of the two edges of each vertex. The specific method is as follows... Figure 4Let the current point be P. now ,P now The previous point is P end The next point is P. start Vector v1 is Vector v2 is According to the properties of the cross product of vectors, when v1×v2>0, P end To the left of vector v1; based on the order of the points in step one, the free space is to the right of v1, i.e., P. now An angle greater than 180 degrees is a concave point, such as... Figure 5 These are all the concave points found on the map, represented by circles.

[0108] Step 2.2, calculate the visible point of the concave point;

[0109] After obtaining all the bumps on the map, calculate the visible point for each bump, which is the point that can be reached by a straight line without touching obstacles or other dividing lines. The specific method is as follows:

[0110] Traverse all boundaries on the map, first let V be the concave point of the visible point to be calculated for the current point. p The starting point for traversing the boundary is L. S The endpoint is L E The currently visited point is P, and the vector is... vector vector If point P is blocked, then point P lies between vectors v3 and v4, that is, to the right of v3 and to the left of v4, or to the left of v3 and to the right of v4, where (v3×v4)*(v5×v4)≤0, as shown in the following cases. Figure 6 As shown, the thin diagonal line represents the possible range of point P.

[0111] If the above conditions are met, then points P and V p If not on the same side of the boundary point, then point P is blocked. Let... If (v6×v7)*(v8×v7)≤0, then P and point V p If they are not on the same side of the boundary point, the situation is as follows: Figure 7 As shown, the area with the thin diagonal line represents the possible range of point P.

[0112] If point P simultaneously satisfies Figure 6 and Figure 7 In the case of P being blocked, P is blocked; otherwise, P is a visible point. Figure 8 As shown, the square represents a certain concave point, and the circle represents all visible points of the concave point.

[0113] Step 2.3: Select the optimal visible point of the concave point;

[0114] Among all visible points, the optimal visible point is calculated. The optimal point is defined as one that is as close as possible to the angle bisector of the concave point, while simultaneously satisfying the condition of being as close as possible to the concave point and not intersecting with existing subdivision lines. The specific method is as follows:

[0115] Let the concave point be V. p The visible point is P1, and the angle bisector of the concave point is calculated as v9. v9 is perpendicular to V. p Starting from v and normalizing it, 10 It is V p The normalized vector with P1 as the starting point and P1 as the ending point is... Two vectors as Figure 9 As shown.

[0116] The objective function for the optimal viewpoint is defined as shown in Formula 1:

[0117]

[0118] Its function graph is as follows Figure 10 As shown, we need to select the visible point that minimizes the objective function, prioritizing the point closest to the angle bisector v9. Negative values ​​ensure Priority ratio within the range First, to account for distance, the distance between the visible point and the concave point is calculated as L. This distance factor is then added to the objective function, resulting in the final objective function shown in Formula 2.

[0119]

[0120] Select the optimal visible point of the concave point, such as Figure 11 For a given concave point, the optimal viewpoint is defined as follows: squares represent convex points, circles represent viewpoints, and hollow circles represent optimal viewpoints. Connecting concave points to their optimal viewpoints cuts the concave point into a convex point. This process of connecting concave points to viewpoints can be considered a map convex polygon reconstruction, with the connecting lines called partition lines. If the angle of a concave point is still greater than 180 degrees after one partition, it is partitioned again until no more concave points remain. After one partition, it is necessary to check whether the optimal viewpoint connecting the concave points is also a concave point. If it is and has been partitioned into a convex point, it can be skipped during the traversal of concave points.

[0121] The specific method for determining whether the angle after partitioning is greater than 180 degrees is as follows: Let the concave point be V. p The point preceding the concave point is P. end The next point is P. start The viewpoint is P1, and the vector is v. 11 for vector v 12 for vector v 13 for Only when (v 11 ×v 13 )≤0,(v 12 ×v 13 There are no angles greater than 180 degrees only when ) ≥ 0. Specific details are as follows: Figure 12 As shown.

[0122] After partitioning each concave point on the map, the free space can be divided into a set of multiple convex polygons, as shown in the figure. Figure 13 The thinner line in the diagram is the dividing line.

[0123] It is worth noting that the order in which concave points are selected during the subdivision process will affect the subdivision result, but will not affect the subsequent path planning, because the free space is eventually converted into a set of convex polygons. The only difference is that the convex polygons are different. The paths inside the convex polygons are homotopic paths, which can be regarded as similar paths and will not affect the subsequent path planning.

[0124] Paths inside a convex polygon are homotopic paths and can be considered as similar paths. Let X be a topological space, denoted by P(X) for all paths in X, P(X; x0, x1) for all paths starting at x0 and ending at x1, and Ω(X, x0) for all loops with x0 as their base. Let f, g ∈ P(X; x0, x1), if there exists a continuous mapping F: (I 2 →X), such that F(s,0)=f(s), F(s,1)=g(s), F(0,t)=x0, F(1,t)=x1. Then F is called the path homotopy from f to g, denoted as This means that these two roads can change continuously between each other and are of the same type, such as... Figure 14 As shown, F(s,t0) represents the homotopic path of f(s) and g(s), which can be regarded as a similar path.

[0125] Step 3: Once the robot's starting point is determined, path planning begins. An improved real-time exploratory search tree method is used to search the map for paths. Prioritizing the search of the midpoints of each cutting line of a convex polygon, paths from the starting point to each convex polygon can be quickly obtained, such as... Figure 15a and Figure 15b As shown, the starting point is set in the upper right corner. The light-colored lines represent the partition lines, and the dark-colored lines represent the sampling tree of the exploratory search tree. In the initial stage of path planning, the sampling points of the exploratory search tree are located at the midpoints of the partition lines, enabling a rapid search of the entire map. Then, the endpoint is set. Regardless of the endpoint's location, it's sufficient to determine which convex polygon on the map the endpoint falls within to quickly obtain a suboptimal path. After traversing every cut line of all convex polygons, points on the cut lines of the convex polygons are randomly and repeatedly sampled to optimize the path length, as shown below. Figure 16As shown, after collecting the midpoints of all the subdivision lines, random sampling begins at any position on the subdivision lines.

[0126] Step 4: After the destination is determined, the robot moves towards the destination along the path at a certain speed, and the root node of the exploratory search tree moves with the robot, so as not to discard the previously sampled path, but also not to wait for the tree to be fully built, because the tree expansion and the robot's movement are interleaved.

[0127] The root node of the exploratory search tree moves with the robot, with node movement and tree updates alternating. In one iteration, the root node first moves towards the robot, followed by tree searching. Tree searching includes tree expansion and rewiring. Tree expansion involves node sampling guided by partition lines. Tree rewiring is divided into rewiring from the root node and random rewiring. The node sampling time guided by partition lines is set to 0.15s, the rewiring time from the root node is 0.1s, and the random rewiring time is 0.03s. Rewiring from the root node can effectively handle situations where the target point changes and obstacle avoidance are handled using previously sampled information, while random rewiring is used for path optimization.

[0128] During the exploratory search tree expansion process, it is necessary to find the point in the search tree closest to the sampling point. To speed up the search, a dynamic reference point tree data structure is adopted. The reference point tree selects a point v in the dataset as the "pivot" and stores the distances between other points and it within a region with a radius of μ. Its principle diagram is shown below. Figure 17a Then, the subset of this region is processed in the same way to form a binary tree, thus obtaining a reference point tree, the calculation formula of which is shown in Formula 3:

[0129]

[0130] Where S1 and S2 are two subtrees, and d(s,v) is the formula for calculating the distance between points s and v, using Euclidean distance. The schematic diagram is shown below. Figure 17b .

[0131] To accommodate applications requiring frequent additions and deletions of data points, multiple reference point trees are maintained. Elements are first added to a pool. If the pool size reaches the minimum tree size, a reference point tree is constructed using the elements in the pool. Then, trees of equal size are merged to ensure a maximum of log(log(n)) trees, with the largest tree having approximately [number missing]. There are n nodes, where n is the number of data points, and the query complexity is close to O(log n).

[0132] Step 5: For situations where the target point changes during robot movement, since the root node of the exploratory search tree moves with the robot and rewires from the root node, a suboptimal solution can be quickly obtained using previously sampled paths. The path length can then be continuously optimized during subsequent tree sampling. Figure 18a As shown in the diagram, the large circle represents the endpoint, and the thick line segment represents the planned route. When the endpoint was initially set, a path was quickly planned based on the previously searched tree, but this was clearly suboptimal. However, in subsequent steps, the endpoint position changed, and based on the previous search results, the planned path was significantly improved. Figure 18b As shown.

[0133] Step Six: To address obstacles encountered during robot movement, the nodes in the exploratory search tree occupied by the obstacle must first be blocked to prevent collisions. Then, it's necessary to determine which convex polygons contain the obstacle. During subsequent exploratory search tree sampling, the interiors of these convex polygons need to be sampled. For example... Figure 19a As shown, because the sampling is restricted to the interior of the convex polygon where the obstacle is located, the path search efficiency is greatly improved, and obstacle avoidance is better achieved. Details are as follows... Figure 19b In the diagram, the circle represents an obstacle that occupies two convex polygons. Subsequent sampling points quickly converge on these two convex polygons, without wasting sampling opportunities in other convex polygons. Figure 20a and Figure 20b The diagram shows the obstacle avoidance effect implemented by the path planning system. Light-colored circles represent the destination, dark-colored circles represent obstacles, thick lines represent the planned path, and thin lines represent the exploratory search tree. When the planned path is blocked by an obstacle, the path planning system quickly plans a suboptimal path due to the rewiring of the search tree and node sampling. Figure 20c and Figure 20d This is a detailed diagram of obstacle avoidance.

[0134] As obstacles are added, the sampling probability around newly added obstacles should increase, while the sampling probability around old obstacles should decrease. This is achieved by defining a special sampling probability function, setting the obstacle array as Obs = [obs1, obs2, ..., obsn], where obstacles are arranged in the order of their appearance, with the first obstacle appearing as obs1 and the nth obstacle appearing as obsn. A probability weight array is defined according to the order of appearance, with the first obstacle appearing having a weight of 1 and the nth obstacle appearing having a weight of n. The weight array is weights = [1, 2, ..., n]. When calculating the probability, the probability of the i-th obstacle being selected is as shown in Formula 2:

[0135]

[0136] The sum(weights) function sums the values ​​in the weights array.

[0137] For situations where obstacles are moving, because the sampling tree rewires from the root node and samples around the obstacles, the path planning system can quickly avoid dynamic obstacles and continuously optimize the path. Its effectiveness is as follows: Figure 21a Figure 21b As shown, after an obstacle blocks the path, the path planning system quickly replans a route and samples the convex polygonal region around the obstacle. This allows the path planning system to avoid dynamic obstacles as they move, while ensuring the path length is not excessive. Details of this effect are shown below. Figure 21c and Figure 21d As shown.

[0138] During sampling, if there are obstacles between a sampling point and its neighboring nodes in the exploratory search tree, the sampling point can be redirected to avoid these obstacles, thus improving search efficiency. Specifically, the process involves detecting whether there are obstacles between the sampling point and its neighboring nodes. If so, the sampling point is reset to a region where there are no obstacles on the line connecting it to its neighboring nodes. Figure 22 As shown, x nearest For the distance from the sampling point x in the exploratory search tree rand The nearest node is there, but there's an obstacle in between, so a turn is made, and the new sampling point is x. nearest A node between the obstacle and the obstacle.

[0139] Example 2

[0140] Figure 23 This is a schematic diagram of a system structure for implementing a real-time dynamic path planning method for medical nursing robots in complex dynamic scenarios, as provided in Embodiment 2 of the present invention. The implementation of path planning is based on good environmental perception and localization mapping. The system includes: an environmental perception module, a location positioning module, a data analysis module, a data recording module, and a data transmission module.

[0141] The environmental perception module is used to acquire environmental data around the intelligent medical care robot, including map creation, semantic information, static obstacles and dynamic obstacles information, etc.

[0142] The positioning module is used to acquire high-precision positioning information of the medical care robot, including odometry information, IMU information, and RTK information;

[0143] The data analysis module is used for scene recognition in the complex environment around the medical care robot, to establish rule-based decision output information, and to obtain the result of the current scene judgment.

[0144] The data recording module is used to record sensor data and initialization information of analysis results;

[0145] The data transmission module is used to transmit the analyzed data and recorded data to the main controller, including global map information, semantic information, obstacle information, odometer information, IMU information, RTK information, and scene judgment information.

[0146] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A real-time dynamic path planning method for medical care robots in complex dynamic scenarios, characterized in that, The method specifically includes the following steps: S1. Load a map and binarize it, where the black areas represent impassable regions and the white areas represent traversable regions, and refer to this as free space. Then, polygon fitting is performed on the map to extract boundary and corner information. S2, regarding free space The fitted polygons are used to reconstruct the map using convex polygons, and the free space is then used for this reconstruction. The set is divided into multiple convex polygons. Due to the properties of convex polygons, the paths inside the convex polygons are homotopic paths, which are considered similar paths. The path search is performed directly on the boundary of the convex polygons, thereby avoiding repeated path searches and improving search efficiency. S3. Once the robot's starting point is determined, path planning begins. An improved real-time exploratory search tree path planning method is used to search the map for a path; specifically, this includes: S31. Prioritize searching the midpoint of each cutting line of the convex polygon to quickly obtain the path from the starting point to each convex polygon. Therefore, no matter where the endpoint is, you only need to determine which convex polygon on the map the endpoint is in to quickly obtain a suboptimal path. S32. After traversing every cutting line of all convex polygons, randomly and repeatedly sample points on the cutting lines of the convex polygons to optimize the path length. S4. After the destination is determined, the robot moves towards the destination along the path at a certain speed, and the root node of the exploratory search tree moves with the robot, so as not to discard the previously sampled path, but also not to wait for the tree to be fully built. Therefore, the expansion of the search tree and the movement of the robot are interleaved. S5. In response to the situation where the target point changes during the robot's movement, since the root node of the exploratory search tree moves with the robot and the wiring is rewired from the root node of the tree, a suboptimal solution is quickly obtained by using the previously sampled path, and the path length is continuously optimized during the subsequent tree sampling process. S6. In the event of obstacles encountered during robot movement, the nodes occupied by the obstacles in the exploratory search tree need to be blocked first to prevent the robot from colliding with the obstacles. Then, it is necessary to determine which convex polygons the obstacles are in. In the subsequent exploratory search tree sampling process, it is only necessary to sample the interior of the convex polygons occupied by the obstacles. Since the sampling is restricted to the interior of the convex polygons where the obstacles are located, the path search efficiency is greatly improved and obstacle avoidance is better achieved. In step S5, the root node of the exploratory search tree moves with the robot, and the movement of the node and the updating of the search tree alternate. In one iteration, the root node first moves with the robot, and then the tree search is performed. The tree search includes tree expansion and rewire routing. Tree expansion is node sampling guided by the partition line. Tree rewire routing is divided into rewire routing from the root node and random rewire routing. Rewire routing from the root node uses the previously sampled information to better solve the situation of target point change and obstacle avoidance. Random rewire routing is used for path optimization. In step S6, as the number of obstacles increases, the sampling probability around newly added obstacles increases, while the sampling probability around old obstacles decreases. The specific implementation method is as follows: Define a special sampling probability function, and set the obstacle array as... The obstacles are arranged in the order they appear, with the first obstacle appearing being... The nth obstacle is Define a probability weight array according to the order of appearance. The first obstacle to appear has a weight of 1, the nth obstacle to appear has a weight of n, and the weight array is... When calculating the probability, the probability of the i-th obstacle being selected is shown in Formula 4: in To Sum the values ​​in the array.

2. The real-time dynamic path planning method for medical nursing robots in complex dynamic scenarios as described in claim 1, characterized in that, In step S1, among the extracted corner point information, it is stipulated that the outermost layer of corner points are arranged counterclockwise and the innermost layer of corner points are arranged clockwise. The arrangement is determined by the right-hand rule to determine which side of the free space is on, that is, the right thumb is pointing upwards and the four fingers are pointing in the direction of the vector. At this time, the direction of the palm is the free space.

3. The real-time dynamic path planning method for medical nursing robots in complex dynamic scenarios as described in claim 1, characterized in that, In step S2, the process of reconstructing the convex polygon of the map includes: S21: Select the concave point; Erosion and dilation operations are performed on the free space to eliminate noise. Then, polygon fitting is performed. The polygon fitting result contains the vertices of each polygon. The vertices in the polygon fitting result are traversed in order, with the vertices of the outer layer of the polygon arranged counterclockwise and the vertices of the inner layer of the polygon arranged clockwise. The cross product of the two sides of the vertex is used to determine whether the point is a concave point, that is, a point with an angle greater than 180 degrees. Specifically, let the current point be... , The previous point is The next point is ,vector for ,vector for From the properties of the cross product of vectors, we know that when hour, In vector The left side; the arrangement order obtained by the right-hand rule shows that the free space is... On the right side, that is An angle greater than 180 degrees indicates a concave point; S22: Calculate the visible point of the concave point; After obtaining all the concave points on the map, calculate the visible point for each concave point, which is the point reachable from the concave point by a straight line without touching obstacles or other dividing lines. The specific method is as follows: Traverse all boundaries on the map, first assuming the concave point of the visible point to be calculated for the current point is... The starting point for traversing the boundary is The destination is The currently visited point is ,vector ,vector ,vector If you click If blocked, then click. In vector sum vector Between, that is, in right side On the left, or in Left side Right side, ( ) ( ) 0; When the point is satisfied If blocked, then click. In vector sum vector Between, that is, in right side On the left, or in Left side Right side, ( ) ( ) After the condition of 0, if the point and points If not on the same side of the boundary point, then the point Blocked, set , , ,if( ) ( ) 0, then and points Not on the same side of the boundary point; If point Simultaneously satisfy ( ) ( ) 0 and ( ) ( ) In the case of 0, then If blocked, otherwise As the viewpoint; S23: Select the optimal visible point of the concave point; Among all visible points, the optimal visible point is calculated. The optimal point is defined as the visible point that is as close as possible to the angle bisector of the concave point, while also satisfying the condition of being as close as possible to the concave point and not intersecting with existing bisectors. The specific method is as follows: Let the concave point be The viewpoint is The angle bisector of the concave point is calculated as follows: , by Starting from this point, and unitizing it, Therefore Starting from, The normalized vector of the endpoint, i.e. , The objective function for the optimal viewpoint is defined as shown in Formula 1: We need to select the viewpoint that minimizes the objective function, prioritizing the selection of the point outside the angle bisector. The nearest point, so in [ ] is negative, ensuring [ Priority ratio within the range of [-] , ],[ [High; secondly, considering the distance factor, the distance between the visible point and the concave point is calculated as] By incorporating a distance factor into the objective function, the final objective function is shown in Formula 2: The process of selecting the optimal visible point of a concave point, connecting the concave point to the optimal visible point, cutting the concave point into a convex point, and connecting the concave point to the visible point is regarded as a map convex polygon reconstruction. The connecting line is called the partition line. If the angle of the concave point is still greater than 180 degrees after one partition, it is partitioned again until there are no more concave points. After completing one partition, it is also necessary to check whether the optimal visible point connecting the concave points is a concave point. If it is, and it has been partitioned into a convex point, it is skipped directly when traversing the concave points. The specific method for determining whether the angle after partitioning is greater than 180 degrees is as follows: Let the concave point be... The point before the concave point is The next point is The viewpoint is ,vector for ,vector for ,vector for Only when ( ) 0, ( ) There are no angles greater than 180 degrees at 0 degrees; After subdividing each concave point on the map, the free space is divided into a set of multiple convex polygons.

4. The real-time dynamic path planning method for medical nursing robots in complex dynamic scenarios as described in claim 1, characterized in that, In step S2, paths inside a convex polygon that are homotopic are considered similar paths. The specific method is as follows: set up It is a topological space. All roads are recorded as All Starting from, The road to the destination is denoted as All of them A circuit with a base point is denoted as ;set up If a continuous mapping exists , making , , , ， Then it is called From arrive The road is the same as the path, recorded as This means that these two paths change continuously over time and are of the same type.

5. The real-time dynamic path planning method for medical nursing robots in complex dynamic scenarios as described in claim 1, characterized in that, During the expansion of the tree, it is necessary to find the point in the search tree that is closest to the sampling point. A dynamic reference point tree data structure is used, and the reference point tree selects a point from the dataset. As the "pivot", the distances of other points to it are stored in a radius of . Within the region, a subset of this region is processed in the same way to form a binary tree, thus obtaining a reference point tree. Its calculation formula is shown in Formula 3: in , There are two subtrees. For point and The distance calculation formula is as follows, and the Euclidean distance is used here.

6. A system for implementing a real-time dynamic path planning method for medical care robots in complex dynamic scenarios, as described in any one of claims 1-5, characterized in that, include: The system includes an environmental sensing module, a location positioning module, a data analysis module, a data recording module, and a data transmission module. The environmental perception module is used to acquire environmental data around the intelligent medical care robot, including map creation, semantic information, and information on static and dynamic obstacles; The positioning module is used to acquire high-precision positioning information of the medical care robot, including odometry information, IMU information, and RTK information; The data analysis module is used for scene recognition in the complex environment around the medical care robot, to establish rule-based decision output information, and to obtain the result of the current scene judgment. The data recording module is used to record sensor data and initialization information of analysis results; The data transmission module is used to transmit the analyzed data and recorded data to the main controller, including global map information, semantic information, obstacle information, odometer information, IMU information, RTK information, and scene judgment information.

Images