Path planning method for two-wheeled differential robot

Abstract

The invention discloses a path planning method for a two-wheeled differential robot. According to the method, kinematic constraints and an improved heuristic estimation method are combined to optimize path planning in a complex environment. The growth of the tree is effectively controlled by introducing kinematics constraints, so that the path generation process is ensured to accord with the physical and dynamic characteristics of the robot, and the speed in the movement process is kept stable. In addition, the provided heuristic estimation method can estimate the cost more accurately in updating of the path tree, and the optimization efficiency of the path is remarkably improved. The invention also adopts a detrapping method, and a specific detrapping sampling point is generated when a dilemma is detected, so that the operation efficiency and the success rate of the method in a complex environment can be improved.

Description

Technical Field

[0001] The invention relates to a path planning method for a robot in the technical field of path planning, in particular to a path planning method for a two-wheel differential robot. Background Art

[0002] In modern automation systems, especially in the fields of robot navigation and automatic vehicle control, path planning is one of the core technologies. Effective path planning can not only improve the operational efficiency of the robot, but also ensure that it can complete tasks safely and efficiently in complex or uncertain environments. Traditional path planning methods such as rapid random tree (RRT), probabilistic roadmap (PRM) and A-star have their own advantages and limitations. For example, the RRT method can effectively explore large-scale or unknown environments due to its non-determinism and scalability, but the path it generates is often not optimal, and there are obvious deficiencies in path optimization and computational efficiency; the PRM method is suitable for multi-query path planning in a known environment, but has poor adaptability to dynamic or real-time path planning; and the A-star method can quickly find the optimal path in a known grid, but its performance is limited in processing high-dimensional data or complex environments, and it has high resource consumption.

[0003] In addition, when faced with highly complex environments or areas with dense obstacles, existing path planning methods often face the problem of path planning failure or extremely low efficiency, especially when the path needs to be dynamically adjusted to cope with environmental changes. These methods often rely on static models and preset conditions and are difficult to adapt to rapidly changing environmental requirements. For example, on urban roads or in ever-changing disaster sites, the path planning system must be able to respond to environmental changes in real time and efficiently plan a safe path. Therefore, developing a new method that can adapt to complex and changing environments and improve path planning efficiency is crucial to improving the autonomous navigation capabilities of robots and autonomous vehicles, and effectively responding to the challenges of unknown or dynamic environments.

[0004] In view of this, a new path planning method is urgently needed. Summary of the invention

[0005] In order to solve the problems existing in the background technology, the present invention proposes an improved batch information tree (BIT*) method, which is suitable for path planning of a two-wheel differential robot. The method can enhance the adaptability to dynamic environments and the ability to handle complex path planning problems while maintaining the efficiency of path optimization, thereby meeting the needs of modern automation systems for efficient and reliable path planning.

[0006] The present invention effectively improves the efficiency and quality of path planning mainly through the following two technical improvements:

[0007] First, the invention introduces kinematic constraints to control the growth of the tree, ensuring that the path generation process conforms to the physical and dynamic characteristics of the robot and adapts to the dynamically changing environmental requirements. This improvement not only improves the practicality of the path, but also enhances the method's ability to adapt to environmental changes.

[0008] Secondly, the present invention adopts a new heuristic estimation method, which calculates the cost of the path to the target more accurately, so that the method can make more intelligent decisions when selecting the path, thereby significantly improving the optimization efficiency of the path and the computational performance of the method.

[0009] In addition, in order to solve the path planning problems that may be encountered in complex or obstacle-intensive environments, the present invention also introduces a method for getting out of trouble. This method can automatically add sampling points in the key area when the method detects a possible dilemma, so that the method has the opportunity to re-evaluate and adjust the path, effectively avoiding the path planning from falling into a local optimum or unsolvable state. This method significantly improves the operating efficiency and success rate of the method in a changeable and complex environment.

[0010] The technical solution adopted by the present invention is as follows, comprising the following steps:

[0011] 1. A path planning method for a two-wheel differential robot

[0012] S1: Obtain a work environment map;

[0013] S2: Set the starting point coordinates and the end point coordinates of the two-wheel differential robot, and use the improved batch information tree method to generate an optimal path from the starting point to the end point in the working environment map;

[0014] S3: Send the optimal path to the two-wheel differential robot, so that the two-wheel differential robot completes the automatic navigation task.

[0015] The S2 is specifically:

[0016] S21: Constructing kinematic constraints for a two-wheel differential robot;

[0017] S22: Generate a set of robot motion points according to the work environment map, and use the starting point as the initial vertex;

[0018] S23: Find the current vertex V from the robot motion point set m Take all vertices in the circle with radius r as the center and form the current vertex V m The neighborhood point set X near ;

[0019] S24: From the current neighborhood point set X nearSelect a point as a sampling point, and determine whether the current sampling point satisfies the kinematic constraint. If so, add the edge consisting of the velocity of the current vertex and the current sampling point and the sampling point to the edge set QE, and add the current sampling point as a vertex to the vertex set QV. Otherwise, execute S25;

[0020] S25: Repeat S24 to traverse and determine the neighborhood point set X near For other points in, update the edge set QE and vertex set QV;

[0021] S26: Determine whether the edge set QE or vertex set QV is empty. If so, perform the m Get out of trouble and get the current vertex V m The enlarged search radius r' is used to repeat S23-S25 to obtain an updated edge set QE or vertex set QV, otherwise, execute S27;

[0022] S27: Find the current vertex V based on the current edge set QE and vertex set QV m The optimal sampling point of the current vertex V m As the parent node of its optimal sampling point, the current vertex V m and its corresponding optimal sampling point to form a path, and the current optimal sampling point is used as the next vertex V m ;

[0023] S28: Repeat S23-S27 to find the next vertex V m The optimal sampling point is obtained, and the corresponding path and the relationship between the parent and child nodes are obtained until the end point is found. Then, the optimal path is obtained by backtracking from the end point to all parent nodes.

[0024] The S21 is specifically:

[0025] Kinematic constraints include the upper and lower limits of the radius of the moving circle. The radius of the moving circle R rotation The calculation formula is as follows:

[0026]

[0027] Among them, K v is the kinematic speed, K ω is the kinematic angular velocity, K v Satisfy[K v，min , K v，maax ], K ω Satisfy[K ω，min , K ω，max ], K v，min , K v，max are the minimum and maximum values ​​of the kinematic velocity, K ω，min , K ω，maxare the minimum and maximum values ​​of the kinematic angular velocity, respectively.

[0028] The S24 is specifically:

[0029] Set the current vertex V m The straight line with the current sampling point is recorded as the first straight line, and the straight line passing through the current vertex V m and perpendicular to the current vertex V m The straight line in the initial velocity direction is recorded as the second straight line, and the intersection of the first straight line and the second straight line is taken as the center of the moving circle. According to the center of the moving circle and the current vertex V m Calculate the radius R of the moving circle; if the radius R of the moving circle <Min{R rotation}, the current sampling point satisfies the kinematic constraint, otherwise it does not.

[0030] In S26, for the current vertex V m Get out of trouble and get the current vertex V m The expanded search radius r' is:

[0031] First calculate the current vertex V m The regional density ρ within the search radius r is calculated as follows:

[0032]

[0033] A=π×R 2

[0034] Where A is the area of ​​the circular region, N s is the number of sampling points in the area, and R is the search radius;

[0035] If the current vertex V m The regional density ρ within the search radius r is lower than the preset density threshold ρ threshold , then expand the search radius r until the regional density ρ is greater than the preset density threshold ρ threshold , get the current vertex V m The expanded search radius r'.

[0036] The S27 is specifically:

[0037] S271: Select the current vertex V from the current edge set QE m The edge that is connected and has the smallest cost, and the point corresponding to the edge is recorded as the candidate vertex X m ;

[0038] S272: Determine the starting point X star To the current vertex V m The cost of passing through the current vertex V m and the candidate vertex X mThe cost and the cost from the candidate vertex X m To the end point X goal The improved heuristic estimates whether the sum of the costs is less than the current vertex V m To the end point X goal If the estimated cost is less than , execute S273 , otherwise execute S275 ;

[0039] S273: Determine the starting point X star To the current vertex V m The cost and the cost through the current vertex V m and the candidate vertex X m Is the sum of the costs of the selected vertex X less than m To the end point X goal If the estimated cost is less than , execute S274 , otherwise execute S275 ;

[0040] S274: If the vertex X m If it does not exist in the current vertex set QV, then the candidate vertex X m Add to the current vertex set QV, and add the current vertex V m As the candidate vertex X m The parent node of the current vertex is obtained, and the vertex set QV is updated; if the vertex to be selected X m Already exists in the vertex set V, then the current vertex V m As the candidate vertex X m The parent node of the current vertex V is obtained, thereby obtaining a path of the current vertex, and the current vertex V m and the candidate vertex X m The composed edges are deleted from the edge set QE, and the edge set QE is updated;

[0041] S275: Set the current vertex V m and the candidate vertex X m The composed edges are deleted from the edge set QE and the edge set QE is updated;

[0042] S276: Repeat S275-S275 until the current vertex V is obtained m Child node of the current vertex V m A section of the path.

[0043] In S272, from the vertex X to be selected, m To the end point X goal The improved heuristic estimation cost h(X m ) is calculated as follows:

[0044] h(X m )=α×d+β×θ

[0045]

[0046]

[0047]

[0048] Among them, α and β represent two adjustment coefficients, and d represents the predicted position To the end The Euclidean distance, θ is the angle between the predicted position point and the end point. is the direction vector from the predicted position to the target point, |||| is the Euclidean distance, is the direction vector of the target point, is the vertex X to be selected m location, is the vertex X to be selected m The speed of , t is the time interval.

[0049] 2. A computer device

[0050] The device includes a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of the path planning method for a two-wheeled differential robot when executing the computer program.

[0051] 3. A computer-readable storage medium

[0052] The medium stores a computer program, which, when executed by a processor, implements the steps of a path planning method for a two-wheel differential robot.

[0053] IV. A COMPUTER PROGRAM PRODUCT

[0054] The product includes a computer program / instruction, which, when executed by a processor, implements the steps of a path planning method for a two-wheeled differential robot.

[0055] The present invention aims at the limitations of path planning methods in dealing with complex environments, especially the application problems of current methods in dynamic and uncertain environments. Although existing methods can work well under certain conditions, they are often difficult to adapt to rapidly changing environments. Therefore, the present invention proposes a method that combines an improved batch information tree (BIT*) method and an innovative escape method to optimize path planning. The present invention mainly improves the efficiency and quality of path planning through the following two technical improvements:

[0056] 1. Improved Batch Information Tree (BIT) Method*: Based on the traditional BIT* method, this invention introduces kinematic constraints and a new heuristic estimation method. The kinematic constraints ensure that the method takes into account the physical and dynamic characteristics of the robot during the path generation process, enhancing the practicality and adaptability of the path. The new heuristic estimation method calculates the cost of reaching the target more accurately, enabling the method to select paths more intelligently, significantly improving the efficiency of path optimization.

[0057] 2. Application of the escape method: In view of the problem of local optimum or path planning failure that may be encountered in complex environments, the present invention designs an escape method, which can dynamically add sampling points in the key area during the execution of the method. The addition of these sampling points is based on the current environmental information and the evaluation of the existing path, so that the method has the opportunity to re-evaluate and adjust the path, effectively avoiding the path planning failure caused by the dilemma.

[0058] Through the above-mentioned technical improvements, the present invention can significantly improve the efficiency and success rate of path planning in complex environments, and is suitable for automated navigation systems that require a high degree of autonomy and adaptability, such as autonomous driving vehicles, robot navigation, etc. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 It is a flow chart of the method of the present invention.

[0060] Figure 2 It is a simulation result diagram of running 15 times using different methods in an accessible environment in the embodiment.

[0061] Figure 3 This is a diagram of simulation results in which 15 simulations are run using different methods in a simple maze environment in the embodiment.

[0062] Figure 4 This is a simulation result diagram of 15 times running the simulation in a complex environment using different methods in the embodiment.

[0063] Figure 5 This is a simulation result diagram of 15 times running the simulation using different methods in a more complex environment in the embodiment. DETAILED DESCRIPTION

[0064] The present invention is described in detail below in conjunction with specific drawings and specific implementation examples. The following implementation examples will help those skilled in the art to further understand the present invention, but do not limit the present invention in any form.

[0065] like Figure 1 As shown, the present invention comprises the following steps:

[0066] S1: Obtain a working environment map; for example, simulate and establish a working environment map of a two-wheel differential robot; Figure 2 to Figure 5 As shown, there are four different scene diagrams, where the black rectangles are obstacles and the path planning difficulty of the scenes gradually increases.

[0067] S2: Set the starting point coordinates and the end point coordinates of the two-wheel differential robot, and use the improved batch information tree (BIT*) method to generate an optimal path from the starting point to the end point in the working environment map;

[0068] S2 is specifically:

[0069] S21: Constructing kinematic constraints for a two-wheel differential robot;

[0070] S21 is specifically:

[0071] Establish the kinematic model of the differential robot, establish the coordinate system according to the kinematic model; set the kinematic speed K v and kinematic angular velocity K ω The kinematic constraints include the upper and lower limits of the radius of the moving circle. rotation The calculation formula is as follows:

[0072]

[0073] Among them, K v is the kinematic speed, K ω is the kinematic angular velocity, K v Satisfy[K v，min , K v，max ], K ω Satisfy[K ω，min , K ω，max ], K v，min , K v，max are the minimum and maximum values ​​of the kinematic velocity, K ω，min , K ω，max are the minimum and maximum values ​​of the kinematic angular velocity, respectively.

[0074] S22: Generate robot motion point set X based on the work environment map samples , taking the starting point as the initial vertex; specifically, except for the location of obstacles, randomly generate motion points in the working environment map to obtain the robot motion point set.

[0075] S23: From the robot motion point set X samples Find the current vertex V m Take all vertices in the circle with radius r as the center and form the current vertex V m The neighborhood point set X near , that is, X near={X∈X samples |||XV m ||2≤r}, ||||2 is the Euclidean distance;

[0076] S24: From the current neighborhood point set X near Select a point as a sampling point, and determine whether the current sampling point satisfies the kinematic constraint. If so, add the edge consisting of the velocity of the current vertex and the current sampling point and the sampling point to the edge set QE, and add the current sampling point as a vertex to the vertex set QV. Otherwise, execute S25;

[0077] S24 is specifically:

[0078] Set the current vertex V m The straight line with the current sampling point is recorded as the first straight line, and the straight line passing through the current vertex V m and perpendicular to the current vertex V m The straight line in the initial velocity direction is recorded as the second straight line, and the intersection of the first straight line and the second straight line is taken as the center of the moving circle. According to the center of the moving circle and the current vertex V m Calculate the radius R of the moving circle, Cx and Cy are the horizontal and vertical coordinates of the center of the moving circle, Vx and Vy are the current vertex V m The horizontal and vertical coordinates of the moving circle; if the radius R <Min{R rotation},MinR rotation =min(K v ) / max(K ω ), the current sampling point satisfies the kinematic constraint, otherwise it does not and the sampling point will not be explored.

[0079] S25: Repeat S24 to traverse and determine the neighborhood point set X near For other points in, update the edge set QE and vertex set QV;

[0080] S26: Determine whether the edge set QE or vertex set QV is empty. If it is empty, the current vertex V m Failed to explore sampling point X, current vertex V m represents the vertex that may encounter difficulties, then for the current vertex V m Get out of trouble and get the current vertex V m The enlarged search radius r' is used to repeat S23-S25 to obtain an updated edge set QE or vertex set QV, otherwise, execute S27;

[0081] In S26, for the current vertex V m Execute the escape strategy and obtain the current vertex V m The expanded search radius r' is:

[0082] First calculate the current vertex V m The regional density ρ within the search radius r is calculated as follows:

[0083]

[0084] A=π×R 2

[0085] Where A is the area of ​​the circular region, N s is the number of sampling points in the region, and R is the search radius. If the current vertex V m The regional density ρ within the search radius r is lower than the preset density threshold ρ threshold , then expand the search radius r until the regional density ρ is greater than the preset density threshold ρ threshold , that is, the density condition is met, and the current vertex V is obtained m The expanded search radius r'.

[0086] S27: Find the current vertex V based on the current edge set QE and vertex set QV m The optimal sampling point of the current vertex V m As the parent node of its optimal sampling point, the current vertex V m and its corresponding optimal sampling point to form a path, and the current optimal sampling point is used as the next vertex V m ;

[0087] S27 is specifically:

[0088] S271: Select the current vertex V from the current edge set QE m The edge that is connected and has the smallest cost, and the point corresponding to the edge is recorded as the candidate vertex X m ; The edge connected to the starting point with the smallest cost is directly used as the path to the starting point.

[0089] S272: Determine the starting point X star To the current vertex V m The cost of passing through the current vertex V m and the candidate vertex X m The cost and the cost from the candidate vertex X m To the end point X goal The improved heuristic estimates whether the sum of the costs is less than the current vertex V m To the end point X goal If the estimated cost is less than , execute S273 , otherwise execute S275 ; the formula is as follows:

[0090] gT(V m )+c(V m , X m )+h(X m ) <gT(Xgoal )

[0091] Among them, gT(V m ) is the starting point X star To the current vertex V m The cost, c(V m , X m ) is the point through the current vertex V m and the candidate vertex X m The cost, h(X m ) is the vertex X to be selected m To the end point X goal The improved heuristic estimation cost, gT(X goal ) is the current vertex V m To the end point X goal Estimated cost of.

[0092] Among them, from the vertex X to be selected m To the end point X goal The improved heuristic estimation cost h(X m ) is calculated as follows:

[0093] h(X m )=α×d+β×θ

[0094]

[0095]

[0096]

[0097] Among them, α and β represent two adjustment coefficients, and d represents the predicted position To the end The Euclidean distance, θ is the angle between the predicted position point and the end point. is the direction vector from the predicted position to the target point, |||‖ is the calculation of the Euclidean distance, is the direction vector of the target point (i.e. the end point), is the vertex X to be selected m location, is the speed of the vertex Xm to be selected, and t is the time interval.

[0098] S273: Determine the starting point X star To the current vertex V m The cost and the cost through the current vertex V m and the candidate vertex X m Is the sum of the costs of the selected vertex X less than m To the end point X goal If the estimated cost is less than , execute S274, otherwise execute S275; the formula is as follows:

[0099] gT(V m )+c(V m , X m ) <gT(X m )

[0100] Among them, gT(V m ) is the starting point X star To the current vertex V m The cost, c(V m , X m ) is the point through the current vertex V m and the candidate vertex X m The cost of gT(X m ) is the vertex X to be selected m To the end point X goal the estimated cost of

[0101] S274: If the vertex X m If it does not exist in the current vertex set QV, then the candidate vertex X m Add to the current vertex set QV, and add the current vertex V m As the candidate vertex X m The parent node of the current vertex is obtained, and the vertex set QV is updated; if the vertex to be selected X m Already exists in the vertex set V, then the current vertex V m As the candidate vertex X m The parent node of the current vertex V is obtained, thereby obtaining a path of the current vertex, and the current vertex V m and the candidate vertex X m The composed edges are deleted from the edge set QE, and the edge set QE is updated;

[0102] S275: Set the current vertex V m and the candidate vertex X m The composed edges are deleted from the edge set QE and the edge set QE is updated;

[0103] S276: Repeat S275-S275 until the current vertex V is obtained m Child node of the current vertex V m A section of the path.

[0104] S28: Repeat S23-S27 to find the next vertex V m The optimal sampling point is obtained, and the corresponding path and the relationship between the parent and child nodes are obtained until the end point is found. Then, the optimal path is obtained by tracing back all the parent nodes from the end point. Each point in the optimal path contains the point coordinates and the speed attributes of the point.

[0105] S3: Send the optimal path to the two-wheel differential robot, so that the two-wheel differential robot completes the automatic navigation task.

[0106] In order to verify the effectiveness and feasibility of the path planning method proposed in the present invention, simulations were performed and compared with other existing methods.

[0107] Table 1 shows the operation data of the barrier-free environment algorithm

[0108]

[0109] Table 2 shows the algorithm operation data for a simple maze environment

[0110]

[0111] Table 3 shows the operation data of the algorithm in complex environment

[0112]

[0113] Table 4 shows the operation data of the algorithm in a more complex environment

[0114]

[0115] It can be seen from Tables 1 to 4 that the improvements of the present invention shorten the running time, increase the stability of the path and the angular velocity, and shorten the robot moving time compared with the unimproved method.

[0116] Figure 2 (a)~ Figure 2 (d) is the simulation result of running 15 times in an obstacle-free environment using RRT*, kinematic RRT*, batch information tree (BIT*), and improved BIT* methods. Figure 2 (e) and Figure 2 (g) are speed fluctuation diagrams of BIT* and RRT* in barrier-free environment. Figure 2 (i) and Figure 2 (k) are the angular velocity fluctuation diagrams of BIT* and RRT* in barrier-free environment; Figure 2 (f) and Figure 2 (h) are velocity fluctuation diagrams of the improved BIT* method (KBIT* of the present invention) and kinematic RRT* (KRRT*) in an obstacle-free environment, respectively. Figure 2 (j) and Figure 2 (l) are angular velocity fluctuation diagrams of the improved BIT* method (KBIT* of the present invention) and kinematic RRT* (KRRT*) in an obstacle-free environment;

[0117] Figure 3 (a)~ Figure 3(d) is the simulation result of running 15 times in a simple maze using RRT*, kinematic RRT*, batch information tree (BIT*), and improved BIT* methods. Figure 3 (e) and Figure 3 (g) are the velocity fluctuation diagrams of BIT* and RRT* in a simple maze, Figure 3 (i) and Figure 3 (k) are the angular velocity fluctuation diagrams of BIT* and RRT* in a simple maze; Figure 3 (f) and Figure 3 (h) are velocity fluctuation diagrams of the improved BIT* method (KBIT* of the present invention) and kinematic RRT* (KRRT*) in a simple maze, respectively. Figure 3 (j) and Figure 3 (l) are angular velocity fluctuation diagrams of the improved BIT* method (KBIT* of the present invention) and kinematic RRT* (KRRT*) in a simple maze;

[0118] Figure 4 (a)~ Figure 4 (d) shows the simulation results of running 15 times in a complex environment using RRT*, kinematic RRT*, batch information tree (BIT*), and improved BIT* methods. Figure 4 (e) and Figure 4 (g) are the speed fluctuation diagrams of BIT* and RRT* in complex environments. Figure 4 (i) and Figure 4 (k) are the angular velocity fluctuation diagrams of BIT* and RRT* in complex environments; Figure 4 (f) and Figure 4 (h) are velocity fluctuation diagrams of the improved BIT* method (KBIT* of the present invention) and kinematic RRT* (KRRT*) in a complex environment, respectively. Figure 4 (j) and Figure 4 (l) are angular velocity fluctuation diagrams of the improved BIT* method (KBIT* of the present invention) and kinematic RRT* (KRRT*) in a complex environment;

[0119] Figure 5 (a)~ Figure 5 (d) is the simulation result of running 15 times in a more complex environment using RRT*, kinematic RRT*, batch information tree (BIT*), and improved BIT* methods. Figure 5 (e) and Figure 5 (g) are speed fluctuation diagrams of BIT* and RRT* in more complex environments. Figure 5 (i) and Figure 5(k) are the angular velocity fluctuation diagrams of BIT* and RRT* in more complex environments; Figure 5 (f) and Figure 5 (h) are velocity fluctuation diagrams of the improved BIT* method (KBIT* of the present invention) and kinematic RRT* (KRRT*) in a more complex environment, respectively. Figure 5 (j) and Figure 5 (l) are the angular velocity fluctuation diagrams of the improved BIT* method (KBIT* of the present invention) and kinematic RRT* (KRRT*) in a more complex environment.

[0120] The present invention Figure 2 to Figure 5 The paper shows that this method is compared with the current mainstream methods RRT* and kinematic RRT*, and has great advantages in path length and path smoothness. It can be seen that this method can be applied to automated navigation systems that require high autonomy and adaptability, such as autonomous vehicles, robot navigation, etc.

Claims

1. A path planning method for a two-wheel differential robot, characterized in that: The following steps are involved: S1: Obtain a work environment map; S2: Set the starting point coordinates and the end point coordinates of the two-wheel differential robot, and use the improved batch information tree method to generate an optimal path from the starting point to the end point in the working environment map; S3: Send the optimal path to the two-wheel differential robot, so that the two-wheel differential robot completes the automatic navigation task.

2. A path planning method for a two-wheel differential robot according to claim 1, characterized in that: The S2 is specifically: S21: Constructing kinematic constraints for a two-wheel differential robot; S22: Generate a set of robot motion points according to the work environment map, and use the starting point as the initial vertex; S23: Find the current vertex V from the robot motion point set m Take all vertices in the circle with radius r as the center and form the current vertex V m The neighborhood point set X near ; S24: From the current neighborhood point set X near Select a point as a sampling point, and determine whether the current sampling point satisfies the kinematic constraint. If so, add the edge consisting of the velocity of the current vertex and the current sampling point and the sampling point to the edge set QE, and add the current sampling point as a vertex to the vertex set QV. Otherwise, execute S25; S25: Repeat S24 to traverse and determine the neighborhood point set X near For other points in, update the edge set QE and vertex set QV; S26: Determine whether the edge set QE or vertex set QV is empty. If so, perform the m Get out of trouble and get the current vertex V m The enlarged search radius r' is used to repeat S23-S25 to obtain an updated edge set QE or vertex set QV, otherwise, S27 is executed; S27: Find the current vertex V according to the current edge set QE and vertex set QV m The optimal sampling point of the current vertex V m As the parent node of its optimal sampling point, the current vertex V m and its corresponding optimal sampling point to form a path, and the current optimal sampling point is used as the next vertex V m ; S28: Repeat S23-S27 to find the next vertex V m The optimal sampling point is obtained, and the corresponding path and the relationship between the parent and child nodes are obtained until the end point is found. Then, the optimal path is obtained by backtracking from the end point to all parent nodes.

3. A path planning method for a two-wheel differential robot according to claim 2, characterized in that: The S21 is specifically: Kinematic constraints include the upper and lower limits of the radius of the moving circle. The radius of the moving circle R rotation The calculation formula is as follows: Among them, K v is the kinematic speed, K ω is the kinematic angular velocity, K v Satisfy[K v，min , K v，max ], K ω Satisfy[K ω，min , K ω，max ], K v，min , K v，max are the minimum and maximum values ​​of the kinematic velocity, K ω，min , K ω，max are the minimum and maximum values ​​of the kinematic angular velocity, respectively.

4. A path planning method for a two-wheel differential robot according to claim 2, characterized in that: The S24 is specifically: Set the current vertex V m The straight line with the current sampling point is recorded as the first straight line, and the straight line passing through the current vertex V m and perpendicular to the current vertex V m The straight line in the initial velocity direction is recorded as the second straight line, and the intersection of the first straight line and the second straight line is taken as the center of the moving circle. According to the center of the moving circle and the current vertex V m Calculate the radius R of the moving circle; if the radius R of the moving circle <Min{R rotation }, the current sampling point satisfies the kinematic constraint, otherwise it does not.

5. A path planning method for a two-wheel differential robot according to claim 2, characterized in that: In S26, for the current vertex V m Get out of trouble and get the current vertex V m The expanded search radius r' is: First calculate the current vertex V m The regional density ρ within the search radius r is calculated as follows: A=π×R 2 Where A is the area of ​​the circular region, N s is the number of sampling points in the area, and R is the search radius; If the current vertex V m The regional density ρ within the search radius r is lower than the preset density threshold ρ threshold , then expand the search radius r until the regional density ρ is greater than the preset density threshold ρ threshold , get the current vertex V m The expanded search radius r'.

6. A path planning method for a two-wheel differential robot according to claim 2, characterized in that: The S27 is specifically: S271: Select the current vertex V from the current edge set QE m The edge that is connected and has the smallest cost, and the point corresponding to the edge is recorded as the candidate vertex X m ; S272: Determine the starting point X star To the current vertex V m The cost of passing through the current vertex V m and the candidate vertex X m The cost and the cost from the candidate vertex X m To the end point X goal The improved heuristic estimates whether the sum of the costs is less than the current vertex V m To the end point X goal If the estimated cost is less than , execute S273 , otherwise execute S275 ; S273: Determine the starting point X star To the current vertex V m The cost and the cost through the current vertex V m and the candidate vertex X m Is the sum of the costs of the selected vertex X less than m To the end point X goal If the estimated cost is less than , execute S274 , otherwise execute S275 ; S274: If the vertex X m If it does not exist in the current vertex set QV, then the candidate vertex X m Add to the current vertex set QV, and add the current vertex V m As the candidate vertex X m The parent node of the current vertex is obtained, thereby obtaining a path of the current vertex and updating the vertex set QV; If the selected vertex X m Already exists in the vertex set V, then the current vertex V m As the candidate vertex X m The parent node of the current vertex V is obtained, thereby obtaining a path of the current vertex, and the current vertex V m and the candidate vertex X m The composed edges are deleted from the edge set QE, and the edge set QE is updated; S275: Set the current vertex V m and the candidate vertex X m The composed edges are deleted from the edge set QE and the edge set QE is updated; S276: Repeat S275-S275 until the current vertex V is obtained m Child node of the current vertex V m A section of the path.

7. A path planning method for a two-wheel differential robot according to claim 6, characterized in that: In S272, from the vertex X to be selected, m To the end point X goal The improved heuristic estimation cost h(X m ) is calculated as follows: h(X m )=α×d+β×θ Among them, α and β represent two adjustment coefficients, and d represents the predicted position To the end The Euclidean distance, θ is the angle between the predicted position point and the end point. is the direction vector from the predicted position to the target point, ‖ ‖ is the Euclidean distance, is the direction vector of the target point, is the vertex X to be selected m location, is the vertex X to be selected m The speed of , t is the time interval.

8. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 7 are implemented.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A computer program product comprising a computer program / instructions, characterized in that When the computer program / instructions are executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images