Mobile robot path planning method based on Kinodynamic RRT* algorithm

Abstract

The invention provides a mobile robot path planning method based on a Kindynamic RRT * algorithm in the field of mobile robot global path planning, which comprises the following steps of: 1, updating a global map, determining a starting point and an ending point and setting related parameters; 2, sampling by using a target deviation strategy; 3, searching for adjacent points by using a KD-Tree; 4, checking node connection possibility by using an expansion function, and expanding nodes according to a kinematic model; 5, calculating the cost between the nodes by using a cost function, and further searching a minimum cost path between the nodes; 6, pruning the final path; and 7, repeating the steps until the optimal path from the starting point to the ending point is found or after a preset condition is met, checking whether the path is completed, if so, outputting the path, otherwise, returning to the step to continue searching. According to the technical scheme, the path planning efficiency is improved, the stability and the accuracy are enhanced, and the mathematical model and the algorithm are simplified, so that the method is particularly suitable for a mobile robot platform with limited computing resources.

Description

Technical Field

[0001] The present invention relates to the technical field of robot path planning, and particularly to a mobile robot path planning method based on the Kinodynamic RRT* algorithm. Background Art

[0002] With the development of robot technology, mobile robots and automated devices are increasingly widely used in fields such as unmanned driving, warehousing logistics, and industrial production. In these dynamic and complex environments, robots need to efficiently plan a path that not only satisfies kinematic constraints but also optimizes certain performance metrics (such as time, energy, or safety). Although traditional path planning algorithms can find feasible paths, they usually ignore the dynamic limitations of the robot and are difficult to meet the actual motion constraints.

[0003] To address this challenge, a class of sampling algorithms that combine path search and kinematic constraints has been developed in recent years. For example, the main technical graph search methods (such as A*, Dijkstra, etc.) in traditional path planning discretize the environment and search for the optimal path in a grid or graph structure, which has relatively high path reliability. However, this type of method is sensitive to the degree of discretization of the environment and is difficult to handle continuous high-dimensional space problems. At the same time, the generated paths usually lack dynamic feasibility and cannot directly meet the dynamic constraints of the robot, requiring additional trajectory transformation and optimization processing. Sampling algorithms (such as RRT, RRT*) search for paths in space by rapidly expanding random trees and are suitable for dynamic and high-dimensional spaces. However, these techniques do not directly consider kinematic or dynamic constraints themselves, and the generated paths may not be actually executable by the robot, especially when dealing with complex dynamic systems. Optimization methods (such as CHOMP, STOMP, etc.) generate feasible paths by continuously optimizing the initial trajectory. These techniques can directly consider the kinematics and dynamics constraints of the robot during the planning process and generate smooth trajectories that meet the execution requirements. However, these methods are sensitive to the initial conditions, are prone to falling into local optima, and have low computational efficiency when the obstacles are dense or the environment is complex.

[0004] Therefore, when a domestic service robot needs to perform tasks in a complex, diverse, and dynamically changing domestic environment, such as delivering objects, cleaning, monitoring, etc. There are dynamic factors such as narrow passages, randomly placed furniture, and human activities in these scenarios, which pose high requirements for path planning and trajectory generation. Existing technologies are difficult to handle dynamic obstacles in complex environments in this application scenario; lack the path feasibility of dynamic constraints; are sensitive to initial conditions, and the optimization process is complex, resulting in limited computational efficiency and resources; cannot effectively handle real-time interaction scenarios; at the same time, traditional path planning technologies mostly target regular geometric shapes (such as rectangular or circular chassis), and cannot effectively handle the complexity of collision detection caused by irregular chassis (such as polygons, irregular shapes). For this reason, the present invention proposes a mobile robot path planning method based on the Kinodynamic RRT* algorithm to solve the above technical problems. Summary of the Invention

[0005] The present invention provides a path planning method that better satisfies the dynamic and kinematic constraints of a robot or system compared to RRT and RRT*. The present invention introduces a dynamic model and constraints into the planning process, so that the generated path not only satisfies the geometric constraints of the environment, but also satisfies the dynamic constraints of the robot, and the generated path gradually approaches the optimal solution during the iteration process. And by using target-biased sampling and KD-Tree to find neighboring points, the computational efficiency of path planning is improved, and real-time path planning of the robot is realized.

[0006] To achieve the above object, the present invention provides a mobile robot path planning method based on the Kinodynamic RRT* algorithm, which adopts the following technical solutions:

[0007] Step 1: Global map update: Update the map sampling space W according to the robot's perception information. W free is the free space, and W obs is the obstacle space, and the cost of all coordinates in the map from the obstacles is obtained, that is, C obs .

[0008] Step 2: Initialization: Set the starting point X start and the target point X goal , check whether both the starting point X start and the target point X goal are obstacle-free points. If so, it is determined that the path planning fails. If not, the following steps are executed. Initialize the random tree, set the parent node of the starting point X start to be an empty node, add the starting point X start to the random tree T, and set the maximum number of iterations N.

[0009] Step 3: Target point offset sampling: Sample the motion state of the robot, including position and orientation, i.e., X rand = (x, y, θ), and randomly generate a random probability value λ between 0 and 1 according to uniform probability rand , and judge the random probability value λ rand whether it is less than the initially set parameter λ bias . If the random probability value λ rand is less than the parameter λ bias , then set the target point X goal as the sampling point X rand : If the random probability value λ rand is greater than the parameter λ bias , then randomly sample in the sampling space W to obtain the sampling point X rand . Avoid the random growth and redundant calculation of the random tree.

[0010] Step 4: Use the KD-Tree to obtain the nearest point X rand to X nearest . Starting from the root node of the KD-Tree, compare the coordinates of the target point X goal with the value of the current node in the splitting dimension, and select the left subtree or the right subtree to continue traversing downwards until reaching the leaf node; during the traversal process, for each node on the access path, calculate its distance to the target point and update X nearest ; once reaching the leaf node, start backtracking upwards; for each backtracking node, check whether its sibling subtree may contain a closer node; if there is a possibility, recursively search the sibling subtree and update X nearest ; when all possible closer nodes have been considered, X nearest is the node closest to the target point. Using this method improves the path planning efficiency of the mobile robot, and thus realizes the real-time feasibility of path planning.

[0011] Step 5: Expand from the nearest neighbor node X nearest to the sampling point X rand to obtain a new node X new . Define the differential drive system in polar coordinates, and use the kinematic model of the non-holonomic wheeled mobile robot and the extended function of non-linear feedback control to obtain the trajectory of expanding a certain step P from the nearest neighbor node X nearest to the sampling point X rand and the new node X new , and judge whether the obtained trajectory is connectable and collision-free, that is, whether it is located in the free space W free . If so, execute Step 6; if not, discard the new node X new, and return to step three to repeat the execution. This makes the generated path smooth and stable and conforms to the dynamic characteristics of the mobile robot, avoiding problems such as jamming or discontinuous paths of the mobile robot. Moreover, an accurate collision detection algorithm based on the geometric model of the robot is adopted, combined with dynamic sampling expansion, to detect the feasibility of the robot in a complex environment in real time during the path planning process, ensuring that the path planning result is consistent with the actual physical form.

[0012] The kinematic model of the non-holonomic wheeled mobile robot is as follows: ; ; ;

[0013] The calculation formula for the extended function of the non-linear feedback control law is as follows: ; ; is a predefined parameter, .

[0014] ρ represents the Euclidean distance between the Cartesian coordinates of the robot pose (x, y, θ) and the target state; θ represents the angle between the x-axis of the robot reference frame X R and the x-axis of the target state coordinate system X G ; α represents the angle between the y-axis of the robot reference frame X R and the vector connecting the robot and the target position; v represents the moving speed; ω represents the angular speed of the robot.

[0015] According to the kinematic model of the non-holonomic wheeled mobile robot, the state space X and the control space U are defined, and the closed-loop forward simulation is calculated to generate the trajectory X(t) and the control U(t) for realizing the connection speed between any two motion states, providing accurate dynamic support for path planning.

[0016] Step six: Use the KD-Tree to obtain the new node X new the set X of the adjacent n nodes near , and select the parent node. Set X new the node with the minimum connection cost as X nearest , and the minimum connection cost is . Use the KD-Tree to search the random tree T to obtain the new node X new the set X of the adjacent n nodes near , and try to traverse each node X near in the set X of adjacent nodes near as the new node X newConnect to the parent node of X, and judge X through the extended function of the aforementioned non-linear feedback control and the kinematic model of the non-holonomic wheeled mobile robot near and X new Whether they can be connected and collision-free, that is, whether they are located in the free space W free If not, then give up connecting this X near as the parent node of X new ; if so, then find and calculate the cost of connecting X near and X ne w through the cost function , and the cost of connecting X nearest and X new is C min . If C near is less than C min , then update C min to C near , and set the parent node of X new to X near . If C near is greater than C min , then keep X nearest as the parent node of X new , without updating.

[0017] The calculation formula of the cost function is as follows: The cost of node connection C total includes the cost of node obstacle C obs and the cost of node movement distance C motion ; ; ; C obs is the cost of the obstacle corresponding to the coordinate in the obstacle cost map W found through the node coordinates, that is, the distance from the node to the nearest obstacle in the map; ; is a set parameter .

[0018] The cost function is the non-holonomic distance function, which also serves as the control Lyapunov function to measure the distance between the current position and the target position of the robot. And this distance function takes into account the non-holonomic constraints of the robot and can better reflect the real cost.

[0019] The node motion distance cost calculation function constructs a feedback strategy based on the control Lyapunov function to calculate the true motion cost cost between two poses (position and orientation), which truly reflects the motion cost cost of the non-holonomic kinematic model between two nodes.

[0020] Step 7: Add the connection paths of X new and X new to their parent nodes into the random tree T.

[0021] Step 8: Prune the neighboring node set X new of X near . For each neighboring node X near in the neighboring node set X near , try to replace the original parent node of its own neighbor node in the neighboring node set X new with the new node X near . Judge the reachability of the connection from X new to X near through the aforementioned expansion function. If it is not reachable, if it is reachable, then calculate whether the total cost from X new to X near is less than the current total cost of X near . If it is less, then update the total cost of X near to the total cost from X new to X near . Set the parent node of X near to X new .

[0022] Step 9: Judge whether the distance between X new and X goal is less than the threshold. If it is less, then enter Step 11; otherwise, execute Step 10.

[0023] Step 10: Judge whether the maximum number of iterations has been reached. If so, then enter Step 11; otherwise, return to Step 3 and continue to execute.

[0024] Step 11: Starting from X new , iteratively query the parent nodes of the nodes in turn until the parent node of the node is an empty node. Judge whether the final iterated node is the starting node. If it is, then judge that the planning is successful and output the path; otherwise, judge that the planning fails.

[0025] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0026] (1) Embed a kinodynamic model during the expansion process to generate kinodynamically feasible trajectories, and combine the expansion function with nonlinear feedback control to guide the expansion process, making the generated path smooth and matching the robot's dynamics, ensuring the stability and accuracy of the robot when approaching the target. During the path planning process, the feasibility of the robot in a complex environment is detected in real time to ensure that the path planning result conforms to the actual physical form. This avoids oscillations and instability phenomena during path execution;

[0027] (2) Use a parameterized closed-form distance function (cost function) to measure the distance between the current position of the robot and the target position. This ensures that the path planning algorithm can effectively handle nonholonomic constraint systems (such as unicycles). This function also serves as a control Lyapunov function to generate a stable feedback control strategy, ensuring the global stability and local optimality of the path. The simplified mathematical model and control law design make the algorithm have a low computational complexity and are suitable for real-time path planning tasks of mobile robots, especially for robot platforms with limited computational resources;

[0028] (3) Adopt a KD-Tree to quickly find neighboring points in a large-scale environment, improving the search efficiency and flexibility of path planning, avoiding a large number of redundant nearest neighbor search calculations in traditional RRT algorithms, reducing the search time, and being beneficial for real-time path planning;

[0029] (4) Adopt goal-biased sampling, and directly select the target point as the sampling point with a certain probability during sampling, thereby accelerating the expansion of the search tree towards the target area, increasing the sampling probability of sample points towards the target point, and reducing the redundancy of tree expansion. Brief Description of the Drawings

[0030] The specification drawings forming a part of this application are used to provide a further understanding of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. Obviously, for those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings. In the drawings:

[0031] Figure 1 is a flowchart of a mobile robot path planning method based on the Kinodynamic RRT* algorithm of the present invention;

[0032] Figure 2 is a schematic diagram of step six of selecting a parent node in a mobile robot path planning method based on the Kinodynamic RRT* algorithm of the present invention;

[0033] Figure 3 is a schematic diagram of step eight of pruning in a mobile robot path planning method based on the Kinodynamic RRT* algorithm of the present invention;

[0034] Figure 4 This is the output path effect diagram of a mobile robot path planning method based on the Kinodynamic RRT* algorithm of the present invention. Detailed implementation manners

[0035] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. And without conflict, the embodiments in this application and the features in the embodiments can be combined with each other. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0036] Many specific details are set forth in the following description in order to provide a thorough understanding of the present invention, but the present invention can also be implemented in other ways different from those described herein. Those skilled in the art can make similar extensions without departing from the connotation of the present invention. Therefore, the present invention is not limited by the specific embodiments disclosed below.

[0037] It should be noted that unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which this application belongs. The terms used herein are only for the purpose of describing specific embodiments and are not intended to limit the exemplary embodiments according to this application.

[0038] The present invention will be further described and illustrated below in combination with the specific implementation process:

[0039] Step 1: Global map update: Update the map sampling space W according to the robot perception information. W free is the free space, and W obs is the obstacle space. Obtain the cost of all coordinates in the map from the obstacles, that is, C obs .

[0040] Step 2: Initialization: Set the starting point X star and the target point X goal , check whether both the starting point X start and the target point X goal are obstacle-free points. If so, it is determined that the path planning fails; if not, the subsequent steps are executed. Initialize the random tree, set the parent node of the starting point X start to be an empty node, add the starting point X start to the random tree T, and set the maximum number of iterations N = 10000;

[0041] Step 3: Target point offset sampling: Sample the motion state of the robot, including position and orientation, i.e., X rand = (x, y, θ) is sampled, and a random probability value λ between 0 and 1 is randomly generated according to a uniform probability rand , and determine the random probability value λ rand whether it is less than the initially set parameter λ bias . If the random probability value λ rand is less than the parameter λ bias , then set the target point X goal to the sampled point X rand : If the random probability value λ rand is greater than the parameter λ bias , then randomly sample in the sampling space W to obtain the sampled point X rand .

[0042] Step 4: Use KD-Tree to obtain the nearest point X rand to X nearest . Starting from the root node of the KD-Tree, compare the coordinates of the target point X goal with the value of the current node in the splitting dimension, and select the left subtree or the right subtree to continue traversing downward until reaching the leaf node; during the traversal process, for each node on the access path, calculate its distance to the target point and update X nearest ; once reaching the leaf node, start backtracking upward; for each backtracking node, check whether its sibling subtree may contain a closer node; if there is a possibility, perform a recursive search on that sibling subtree and update X nearest ; when all possible closer nodes have been considered, X nearest is the node closest to the target point.

[0043] Step 5: Expand from the nearest node X nearest to the sampled point X rand to obtain a new node X new . Define a differential drive system in polar coordinates, and use the kinematic model of the non-holonomic wheeled mobile robot and the extended function of non-linear feedback control to obtain the trajectory of expanding a certain step P from the nearest node X nearest to the sampled point X rand and the new node X new , and determine whether the obtained trajectory is connectable and collision-free, that is, whether it is located in the free space W free . If so, execute Step 6; if not, discard the new node X new , and return to Step 3 to repeat the execution.

[0044] The kinematic model of the non-holonomic wheeled mobile robot is as follows: ; ; ;

[0045] The expansion function calculation formula of the non - linear feedback control law is as follows: ; ; is a predefined parameter, .

[0046] ρ represents the Euclidean distance between the Cartesian coordinates of the robot pose (x, y, θ) and the target state; θ represents the angle between the x - axis of the robot reference frame X R and the x - axis of the target state coordinate system X G ; α represents the angle between the y - axis of the robot reference frame X R and the vector connecting the robot and the target position; v represents the moving speed; ω represents the angular speed of the robot.

[0047] According to the kinematic model of the non - holonomic wheeled mobile robot, the state space X and the control space U are defined, the closed - loop forward simulation is calculated, and the trajectories X(t) and the control U(t) are generated to achieve the connection speed between any two motion states, providing accurate dynamic support for path planning.

[0048] Step six: Use the KD - Tree to obtain a new node X new The neighboring n = 5 node set X near , and select the parent node. Set X new The node with the minimum connection cost is X nearest , and the minimum connection cost is . Try to traverse each node X near in the neighboring 5 - node set X near as the parent node of the new node X new for connection. Judge whether X near and X new are connectable and collision - free, that is, whether they are located in the free space W free . If not, then give up connecting this X near as the parent node of X new . If so, then find and calculate the cost near of connecting X ne w with X , and the cost C nearest of connecting X new with X min . If C near is less than Cmin , then update C min is C near , and set X new 's parent node to X near , if C near is greater than C min , then keep X nearest as X new 's parent node and do not update.

[0049] The cost function calculation formula is as follows: The node connection cost C total includes the node obstacle cost C obs and the node movement distance cost C motion ; ; ; C obs is the obstacle cost cost corresponding to the coordinate in the obstacle cost map W found through the node coordinates; ; is a set parameter, .

[0050] The cost function is the nonholonomic distance function, which also serves as the control Lyapunov function to measure the distance between the current position and the target position of the robot, and this distance function takes into account the nonholonomic constraints of the robot and can better reflect the true cost.

[0051] The node movement distance cost calculation function constructs a feedback strategy based on the control Lyapunov function to calculate the true movement cost cost between two poses (position and orientation), which truly reflects the movement cost cost of the nonholonomic kinematic model between two nodes.

[0052] Step Seven: Add the connection paths of X new and X near to its parent node to the random tree T.

[0053] Step Eight: Prune the neighboring node set X new of X near . Each neighboring node X near in the neighboring node set X near tries to replace the original parent node of its own neighbor node in the neighboring node set X new with the new node X near . Judge the reachability of the connection from X new to X near through the aforementioned expansion function. If it is not reachable, if it is reachable, then calculate the distance from X new to Xnear Whether the total cost is less than X near The current total cost, if less, then X near Update the total cost to X new To X near The total cost of, X near Set the parent node of to X new .

[0054] Step Nine: Judge X new And X goal Whether the distance between them is less than the threshold value of 0.01m. If less, then enter Step Eleven; otherwise, execute Step Ten.

[0055] Step Ten: Judge whether the maximum number of iterations is reached. If so, then enter Step Eleven; otherwise, return to Step Three and continue to execute.

[0056] Step Eleven: Starting from X new Iteratively query the parent nodes of the nodes in turn until the parent node of the node is a null node. Judge whether the final iterated node is the starting node. If so, then judge that the planning is successful and output the path; otherwise, judge that the planning fails, return to Step Three and continue to search until the path planning is completed.

[0057] In this article, specific examples are used to elaborate on the principles and implementation methods of the present invention. The descriptions of the above embodiments are only used to help understand the method and its core idea of the present invention; at the same time, for those of ordinary skill in the art, according to the idea of the present invention, there will be changes in the specific implementation methods and application scopes. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention. In summary, the content of this specification should not be construed as a limitation to the present invention.

Claims

1. A mobile robot path planning method based on Kinodynamic RRT* algorithm, characterized in that: The steps include: Step 1: Global map update: Update the map sampling space W, W according to the robot's perception information free is the free space, W obs For the obstacle space, get the cost of all coordinates in the map from the obstacle; Step 2: Initialization: Set the starting point X start With the target point X goal , check the starting point X start With the target point X goal Are all points without obstacles? If so, it is determined that the path planning has failed; if not, the subsequent steps are executed. Initialize the random tree and set the starting point X start The parent node is an empty node, and the starting point X start Add to the random tree T and set the maximum number of iterations N; Step 3: Target point bias sampling: Sample the robot's motion state, including position and orientation, i.e. X rand = (x, y, θ), randomly generates a random probability value λ between 0 and 1 according to uniform probability rand , determine the random probability value λ rand Is it less than the initial setting parameter λ? bias If the random probability value λ rand Less than parameter λ bias , then the target point X goal Set as sampling point X rand :If the random probability value λ rand Greater than parameter λ bias , then randomly sample in the sampling space W to obtain the sampling point X rand ; Step 4: Use KD-Tree to obtain X rand Nearest point X nearest ; Step 5: From the nearest node X nearest Towards sampling point X rand Expand to get new node X new Define the differential drive system in polar coordinates, use the nonholonomic wheeled mobile robot kinematic model and the extended function of nonlinear feedback control to obtain the nearest node X nearest Towards sampling point X rand Expand the trajectory of a certain step length P and the new node X new , determine whether the obtained trajectory is connectable and collision-free, that is, whether it is located in the free space W free If yes, execute step 6; if no, discard the new node X. new , and return to step 3 to repeat; The kinematic model of the nonholonomic wheeled mobile robot is as follows: ； ； ； The calculation formula of the extended function of the nonlinear feedback control law is as follows: ； ； are predefined parameters; ρ represents the Euclidean distance between the Cartesian coordinates of the robot posture (x, y, θ) and the target state; θ represents the angle between the x-axis of the robot reference system XR and the x-axis of the target state coordinate system XG; α represents the angle between the y-axis of the robot reference system XR and the vector connecting the robot and the target position; v represents the moving speed; ω represents the robot angular speed; Step 6: Use KD-Tree to obtain the new node X new The set of n neighboring nodes X near , select the parent node. Set X new The node with the minimum connection cost is X nearest , the minimum connection cost is . Try to traverse the new node X new The set of n neighboring nodes X near Each node X near As new node X new The parent node of X is connected to the extended function of the nonlinear feedback control and the kinematic model of the nonholonomic wheeled mobile robot. near and X new Whether it is connectable and collision-free, that is, whether it is located in the free space W free If not, give up on the X near As X new If so, the cost function is used to find and calculate X near With X new The cost of connection , and X nearest With X new The cost of connection C min , if C near Less than C min , then update C min C near , and X new The parent node is set to X near , if C near Greater than C min , then keep X nearest For X new The parent node of is not updated; The cost function calculation formula is as follows: Node connection cost C total Contains node obstacle cost C obs and node movement distance cost C motion ; ； ； C obs To find the corresponding coordinate obstacle cost cost in the obstacle cost map W through the node coordinates; ； To set parameters; Step 7: X new and X new The path connecting to its parent node is added to the random tree T; Step 8: X new Neighboring node set X near Prune. Neighboring node set X near Each neighboring node X near Try using new node X new Replace the neighboring node set X near The original parent node of its own neighbor node in X is determined by the aforementioned expansion function. new To X near Connection reachability, if not reachable, if reachable, calculate from X new To X near Is the total cost less than X? near The current total cost, if less than, will be X near The total cost is updated to X new To X near The total cost, X near The parent node is set to X new ; Step 9: Determine X new With X goal Is the distance between them less than the threshold? If so, proceed to step 11; otherwise, proceed to step 10. Step 10: Determine whether the maximum number of iterations has been reached. If so, proceed to step 11; otherwise, return to step 3 to continue execution; Step 11: From X new Start to iterate and query the parent node of the node in sequence until the parent node of the node is an empty node. Determine whether the final iterated node is the starting node. If it is, the planning is successful and the path is output. Otherwise, the planning fails and returns to step 3 to continue searching until the path planning is completed.

Images