Mobile robot path planning method combining kinematics constraint and variable density sampling

Abstract

The invention discloses a mobile robot path planning method combining kinematics constraint and variable density sampling, which comprises the following steps: setting a starting point and a target point, adding the starting point into a vertex set, adding the target point into a sampling set, initializing a node priority queue and an edge priority queue, and establishing kinematics constraint and environment parameters at the same time; searching a main circulation path; if the vertex cost is better, performing vertex expansion and generating candidate edges, and if the edge cost is better, performing feasibility verification and accessing the path tree; when the search is stagnated or trapped in local optimum, an escape mechanism is triggered, and the exploration capability is enhanced through radius expansion and supplementary sampling; an improved heuristic function and a local cost function are established, search convergence is guided, and track smoothness and performability are improved; and when a termination condition is reached, outputting a path planning result. According to the method, the sampling efficiency and the searching speed are improved while the path feasibility is ensured, so that the navigation requirement of the mobile robot in a complex dynamic environment is met.

Description

Technical Field

[0001] This invention belongs to the field of mobile robots and relates to mobile robot path planning technology, specifically to a mobile robot path planning method that combines kinematic constraints and variable density sampling. Background Technology

[0002] With the widespread application of mobile robots in industrial production, warehousing and logistics, indoor and outdoor inspection and service, their autonomous navigation and path planning capabilities have become crucial factors affecting operational efficiency and safety. Existing path planning algorithms are mainly divided into sampling-based methods (such as RRT, RRT*, BIT*, etc.) and search-based methods (such as A*, Dijkstra, etc.). Among them, sampling-based algorithms have good scalability in high-dimensional spaces and complex environments, but under the presence of kinematic constraints, the problems of low sampling efficiency and unstable path generation quality remain prominent.

[0003] Existing BIT* algorithms improve path search efficiency through batch sampling and heuristic search. However, in practical applications of mobile robots, robots are often constrained by kinematic constraints such as turning radius, maximum curvature, and velocity acceleration, which leads to a large number of invalid sampling points and increases the computational load. Furthermore, uniformly distributed sampling strategies cannot target key areas for focused searching, easily resulting in low path planning efficiency and slow convergence speed. Summary of the Invention

[0004] Purpose of the invention: To address the problems of low sampling efficiency, high invalid sampling rate, non-smooth paths, and non-compliance with kinematic constraints in existing mobile robot path planning, this invention provides a mobile robot path planning method that combines kinematic constraints and variable density sampling. This method improves sampling efficiency and search speed while ensuring path feasibility, thus adapting to the navigation needs of mobile robots in complex dynamic environments.

[0005] Technical Solution: To achieve the above objectives, this invention provides a mobile robot path planning method combining kinematic constraints and variable density sampling, comprising the following steps:

[0006] S1: Based on the acquired environmental map information, set the starting point and the target point, add the starting point to the vertex set, add the target point to the sampling set, initialize the node priority queue and the edge priority queue, and establish kinematic constraints and environmental parameters.

[0007] S2: Search for the main loop path based on kinematic constraints and environmental parameters;

[0008] S3: Compare the optimal elements in the vertex queue and the edge queue. If the vertex cost is better, perform vertex expansion and generate candidate edges. If the edge cost is better, perform feasibility verification and connect to the path tree.

[0009] S4: When the search stalls or gets stuck in a local optimum, the escape mechanism is triggered, which enhances the exploration capability through radius expansion and supplementary sampling;

[0010] S5: Establish an improved heuristic function and local cost function, comprehensively considering factors such as path length, direction, and curvature, to guide the search convergence and improve trajectory smoothness and executability;

[0011] S6: Output the path planning results when the termination condition is met.

[0012] Furthermore, the establishment of kinematic constraints in step S1 includes: calculating the minimum turning radius and the maximum curvature range based on the robot chassis type and steering characteristics; eliminating points that do not meet the kinematic conditions during the node generation stage; verifying curvature continuity and acceleration constraints during the edge connection stage, and discarding edges that do not meet the conditions.

[0013] Further, step S1 specifically includes:

[0014] A1: Initialize the node set, set the node set V = {x start} contains only the starting point, which serves as the root node of the search tree; edge set

[0015] A2: Initialize the sampling set, set the sampling set X samples ={x goal}, containing only the endpoint, forming a goal-oriented bias to accelerate early convergence; providing starting samples for subsequent high-density exploration of the endpoint neighborhood;

[0016] A3: Priority queue initialization, edge priority queue Dequeue vertices based on their total cost, from smallest to largest; Vertex priority queue. Dequeue the vertex heuristic from smallest to largest;

[0017] A4: Kinematic constraints and environmental parameter settings, setting the minimum linear velocity v min With maximum angular velocity ω max And based on this, the minimum turning radius is defined:

[0018]

[0019] Establish a collision detection model (occupancy grid or distance field); record the dimension d, and set the constant γ and the environment coefficient k for radius adaptation.

[0020] A5: State representation and neighborhood index, node state adopts pose (x,y,θ); pre-built KD-Tree to support neighborhood query with radius r, complexity O(logn).

[0021] Furthermore, during the main loop path search process in step S2, resampling, pruning, and radius adaptation are performed. The specific process includes:

[0022] B1: Stasis Detection

[0023] like The search is deemed stalled or trapped in a local optimum.

[0024] B2: Escape strategy triggered

[0025] Execute EscapeStrategy() to generate breakthrough samples in blocked or sparse regions;

[0026] B3: Pruning

[0027] With the current optimal target cost g T (x goal Let be the threshold. For any node x, if the pruning condition is satisfied:

[0028] g T (x)+ImpH(x)≥g T (x goal (2)

[0029] Then delete the node and its associated edges;

[0030] B4: Variable density sampling

[0031] Call Sample(m,g) T (x goal Generate m new samples X sample_i Sampling density adapts to regional potential: sparse sampling in low-cost / accessible areas, and appropriately dense sampling in areas with dense obstacles; sampling points must pass the collision-free test of the grid / distance field.

[0032] B5: Set and Queue Refresh

[0033] Record V old ←V, and update V←V∪X sample_i The updated V is heuristically sorted and then pushed into Q in batches. V ;

[0034] B6: Radius Adaptive

[0035] Update the search radius r to (BIT * theoretical term + environmental adaptation term):

[0036]

[0037] Where γ is a constant, d is the spatial dimension, and k is appropriately increased as the obstacle density increases, so as to balance coverage and efficiency.

[0038] Further, step S3 includes:

[0039] C1: Take Q respectively V Q E The first element of the queue, if min(Q) V )≤min(Q E If the vertex is not fully expanded, proceed to step C2 to perform vertex expansion; otherwise, proceed to step C3 to perform edge expansion.

[0040] C2: Perform vertex expansion from Q V Select the optimal vertex P A ;

[0041] C3: Perform edge expansion from Q E Select the optimal edge (v) m ,x m );

[0042] C4: The pruning rule applies during each round of expansion or stagnation recovery if any node x∈V satisfies

[0043] g T (x)+ImpH(x)≥g T (x goal (4)

[0044] Then delete the node and its associated edges; corresponding obviously redundant or high-curvature inferior edges are deleted simultaneously; pruning causes the search domain to monotonically shrink as the optimal solution improves.

[0045] Furthermore, the vertex expansion process in step C2 includes:

[0046] C2-1: Neighborhood Search: Using KD-Tree to retrieve the candidate sampling point set X near KD-Tree can complete nearest neighbor search in O(logn) time; for any candidate sampling point P B ∈X near Calculate its Euclidean distance to the current vertex:

[0047]

[0048] At the same time, combined with the heuristic function h(P) B Perform pre-sorting;

[0049] C2-2: Set the minimum turning radius based on the allowable range of the robot's linear velocity v and angular velocity ω. Where v is the linear velocity, ω max The maximum angular velocity is used; the motion circle method is employed to determine whether candidate points satisfy the constraints.

[0050] C2-3: Enqueuing candidate edges: For candidate points P selected through kinematic constraints BGenerate edge (P) A ,P B ), and calculate its total cost:

[0051] f(P B ) = g T (P A )+c(P A ,P B )+ImpH(P B (6)

[0052] Wherein: g T (P A c(P) represents the cumulative cost of the current vertex; A ,P B ImpH(P) represents the local connection cost from a vertex to a candidate vertex. B ) represents an improved heuristic function; candidate edges (P) A ,P B According to the total cost f(P) B After sorting in ascending order, add to edge queue Q. E .

[0053] Furthermore, the process of determining whether candidate points satisfy the constraints using the motion circle method in step C2-2 includes:

[0054] 1) At the current vertex P A Draw a normal line perpendicular to the direction of velocity at that point;

[0055] 2) Move vertex P A With candidate point P B Connect the dots;

[0056] 3) The intersection of the two straight lines is the center C of the circle, from which the turning radius can be obtained:

[0057] 4) If R <R min If the candidate point is deemed infeasible, it will be deleted.

[0058] 5) If R ≥ R min If the candidate point is selected, it is retained and added to the candidate edge set.

[0059] Furthermore, the edge expansion process in step C3 includes:

[0060] C3-1: The criterion can be improved if the following condition is met:

[0061] g T (v m )+c(v m ,x m )+ImpH(x m )≤g T (xgoal (7)

[0062] Among them, g T (x goal If the cost of the edge is the currently known optimal solution, then it is considered that the edge may improve the optimal solution, and proceed to the next step;

[0063] If the condition is not met, then discard the edge.

[0064] C3-2: Non-inferiority criterion, if the following is satisfied:

[0065] g(v m )+c(v m ,x m )≤g T (V) (8)

[0066] If the condition is not met, the edge is considered superior to an existing path and can be connected to the tree structure; otherwise, the edge is discarded.

[0067] C3-3: After the above two steps of screening, collision detection is performed. If edge (v,x) has no collision and satisfies the kinematic constraints, it is added to the explicit tree, and the cumulative cost g is updated. T (v);

[0068] Collision detection employs a discrete sampling + continuous segment interpolation method to check whether the trajectory from vertex v to candidate point x intersects with an obstacle. If a collision exists, the edge is discarded; otherwise, the edge (v,x) is added to the explicit tree structure (V,E), and the cumulative cost g of the target node is updated. T (v), and simultaneously push it into the vertex queue Q. V .

[0069] Furthermore, in step S4 when If no improvement is made in multiple consecutive rounds, an escape mechanism is triggered. The execution of the escape mechanism includes:

[0070] D1: Set radius expansion r new =λr, λ∈[1.2,1.5], expanding the explorable domain;

[0071] D2: Density assessment and resampling

[0072] calculate If ρ < ρ th Then supplement One sample, and add X samples ;

[0073] D3: After completing the supplementary sampling in conjunction with the main loop, return to steps B5 and B6 to refresh the set and radius, and continue the search adaptively.

[0074] Furthermore, the improved heuristic function in step S5 is defined as follows:

[0075]

[0076] Among them, ||P B -P goal || represents the Euclidean distance from the candidate point to the target point; θ represents the angle between the candidate point's direction of movement and the target direction, normalized to [0,π]; λ is the weight coefficient for direction correction; Formula (9) shows that when the candidate point and the target point are in the same direction, the heuristic mainly depends on the straight-line distance; when the candidate point's direction deviates from the target, the cost is amplified, thus guiding the search to expand in a more reasonable direction;

[0077] The improved local cost function is defined as follows:

[0078] Local connection cost for candidate edge (P) A ,P B Its local cost is defined as:

[0079] c(P A ,P B ) = w L ·L+w θ ·Δθ+w κ ·κ (10)

[0080] Where L represents the connection length, Δθ represents the directional deflection angle of the edge, and κ represents the local curvature; w L ,w θ ,w κ The weights are for length, direction, and curvature, respectively. Formula (10) shows that the local cost consists of three parts: the longer the path, the greater the cost; the greater the directional deflection angle, the greater the cost, encouraging smoother paths; and the greater the curvature, the greater the cost, in order to avoid sharp turns.

[0081] The variable density sampling strategy in this invention includes:

[0082] The sampling space is divided into regions, and the sampling density is dynamically allocated based on the heuristic cost function to the target point (the calculation of the heuristic cost function will affect the cost estimation of the path, and the magnitude of the cost will directly affect the sampling density); the sampling probability is increased in areas with obstacles, narrow passages and areas restricted by kinematic constraints; the sampling probability is reduced in open or low-cost areas to reduce redundant calculations.

[0083] The node expansion process in this invention includes:

[0084] Search for a feasible set of sampling points in the neighborhood of the current node; calculate the cost of candidate edges, including path length, turning cost, obstacle avoidance cost and smoothness penalty; filter edges that satisfy kinematic constraints and have acceptable costs, and add them to the edge queue according to priority.

[0085] Furthermore, the termination conditions of step S6 include: successfully finding a path that satisfies the kinematic constraints; reaching the maximum number of iterations or the time limit; and the path cost converging to a set threshold range in multiple consecutive iterations.

[0086] The method of this invention introduces a kinematic constraint model during the sampling process and combines it with an adaptive sampling density control strategy based on environmental features to achieve high efficiency in the path search process, smoothness of the path results, and executability, thereby improving the robot's navigation and obstacle avoidance capabilities in complex environments.

[0087] The method of this invention solves the problems of non-executable paths and low search efficiency in existing mobile robot path planning by introducing kinematic constraints and a variable density sampling mechanism. It has the advantages of fast computing speed and high path quality, and is particularly suitable for autonomous mobile robot navigation in complex obstacle environments, showing good application prospects.

[0088] Beneficial effects: Compared with the prior art, the present invention has the following advantages:

[0089] 1. High efficiency and accuracy: By combining batch sampling with variable density sampling, the number of invalid sampling points is reduced, search efficiency is improved, and the generated path is ensured to be smoother and more accurate.

[0090] 2. Comprehensive consideration of multiple constraints: Kinematic constraints, obstacle distribution and environmental structure features are introduced simultaneously during the path search process, which improves the feasibility of the path and its adaptability to complex scenarios;

[0091] 3. Practicality and Flexibility: The algorithm framework is highly versatile and can flexibly adjust parameters according to different robot platforms and application scenarios, ensuring effective application in indoor, outdoor, and complex dynamic environments. Attached Figure Description

[0092] Figure 1 This is a flowchart of the method of the present invention;

[0093] Figure 2 A schematic diagram illustrating the selection of kinematic sampling points;

[0094] Figure 3 This is a schematic diagram of the BIT* path planning;

[0095] Figure 4 This is a schematic diagram of K-BIT* path planning;

[0096] Figure 5 A comparative diagram of BIT* and K-BIT* path planning. Detailed Implementation

[0097] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that these embodiments are for illustrative purposes only and are not intended to limit the scope of the invention. After reading this invention, any modifications of the invention in various equivalent forms by those skilled in the art will fall within the scope defined by the appended claims.

[0098] like Figure 1 As shown, this invention provides a mobile robot path planning method that combines kinematic constraints and variable density sampling, comprising the following steps:

[0099] S1: Based on the acquired environmental map information, set the starting point and the target point, add the starting point to the vertex set, add the target point to the sampling set, initialize the node priority queue and the edge priority queue, and establish kinematic constraints and environmental parameters.

[0100] S2: Search for the main loop path based on kinematic constraints and environmental parameters;

[0101] S3: Compare the optimal elements in the vertex queue and the edge queue. If the vertex cost is better, perform vertex expansion and generate candidate edges. If the edge cost is better, perform feasibility verification and connect to the path tree.

[0102] S4: When the search stalls or gets stuck in a local optimum, the escape mechanism is triggered, which enhances the exploration capability through radius expansion and supplementary sampling;

[0103] S5: Establish an improved heuristic function and local cost function, comprehensively considering factors such as path length, direction, and curvature, to guide the search convergence and improve trajectory smoothness and executability;

[0104] S6: Output the path planning results when the termination condition is met.

[0105] The establishment of kinematic constraints in step S1 includes: calculating the minimum turning radius and maximum curvature range based on the robot chassis type and steering characteristics; eliminating points that do not meet the kinematic conditions during the node generation stage; verifying curvature continuity and acceleration constraints during the edge connection stage, and discarding edges that do not meet the conditions.

[0106] Step S1 specifically includes:

[0107] A1: Initialize the node set, set the node set V = {x start} contains only the starting point, which serves as the root node of the search tree; edge set

[0108] A2: Initialize the sampling set, set the sampling set Xsamples ={x goal}, containing only the endpoint, forming a goal-oriented bias to accelerate early convergence; providing starting samples for subsequent high-density exploration of the endpoint neighborhood;

[0109] A3: Priority queue initialization, edge priority queue Dequeue according to total cost (see equation (5)) from smallest to largest; Vertex priority queue Dequeue the vertex heuristic from smallest to largest;

[0110] A4: Kinematic constraints and environmental parameter settings, setting the minimum linear velocity v min With maximum angular velocity ω max And based on this, the minimum turning radius is defined:

[0111]

[0112] Establish a collision detection model (occupancy grid or distance field); record the dimension d, and set the constant γ and the environment coefficient k for radius adaptation.

[0113] A5: State representation and neighborhood index, node state adopts pose (x,y,θ); pre-built KD-Tree to support neighborhood query with radius r, complexity O(logn).

[0114] During the main loop path search in step S2, resampling, pruning, and radius adaptation are performed. The specific process includes:

[0115] B1: Stasis Detection

[0116] like The search is deemed stalled or trapped in a local optimum.

[0117] B2: Escape strategy triggered

[0118] Execute EscapeStrategy() to generate breakthrough samples in blocked or sparse regions;

[0119] B3: Pruning

[0120] With the current optimal target cost g T (x goal Let be the threshold. For any node x, if the pruning condition is satisfied:

[0121] g T (x)+ImpH(x)≥g T (x goal (2)

[0122] Then delete the node and its associated edges;

[0123] B4: Variable density sampling

[0124] Call Sample(m,g) T (x goal Generate m new samples X sample_i Sampling density adapts to regional potential: denser sampling is used in low-cost / accessible areas, and sparser sampling is used in areas with dense obstacles; sampling points must pass the collision-free test of the grid / distance field.

[0125] B5: Set and Queue Refresh

[0126] Record V old ←V, and update V←V∪X sample_i The updated V is heuristically sorted and then pushed into Q in batches. V ;

[0127] B6: Radius Adaptive

[0128] Update the search radius r to (BIT * theoretical term + environmental adaptation term):

[0129]

[0130] Where γ is a constant, d is the spatial dimension, and k is appropriately increased as the obstacle density increases, so as to balance coverage and efficiency.

[0131] In step S3, decision-making and expansion are used to dynamically balance global advancement and local verification, and kinematic filtering is used to ensure executability. The expansion process is as follows: Figure 2 As shown, it specifically includes:

[0132] C1: Take Q respectively V Q E The first element of the queue, if min(Q) V )≤min(Q E If the vertex is not fully expanded, proceed to step C2 to perform vertex expansion; otherwise, proceed to step C3 to perform edge expansion.

[0133] C2: Perform vertex expansion from Q V Select the optimal vertex P A ;

[0134] The execution process of vertex expansion includes:

[0135] C2-1: Neighborhood Search: Using KD-Tree to retrieve the candidate sampling point set X near KD-Tree can complete nearest neighbor search in O(logn) time; for any candidate sampling point P B ∈X near Calculate its Euclidean distance to the current vertex:

[0136]

[0137] At the same time, combined with the heuristic function h(P) B Perform pre-sorting;

[0138] C2-2: Set the minimum turning radius based on the allowable range of the robot's linear velocity v and angular velocity ω. Where v is the linear velocity, ω max The maximum angular velocity is used; the motion circle method is employed to determine whether candidate points satisfy the constraints.

[0139] 1) At the current vertex P A Draw a normal line perpendicular to the direction of velocity at that point;

[0140] 2) Move vertex P A With candidate point P B Connect the dots;

[0141] 3) The intersection of the two straight lines is the center C of the circle, from which the turning radius can be obtained:

[0142] 4) If R <R min If the candidate point is deemed infeasible, it will be deleted.

[0143] 5) If R ≥ R min If the candidate point is selected, it is retained and added to the candidate edge set.

[0144] C2-3: Enqueuing candidate edges: For candidate points P selected through kinematic constraints B Generate edge (P) A ,P B ), and calculate its total cost:

[0145] f(P B ) = g T (P A )+c(P A ,P B )+ImpH(P B (5)

[0146] Wherein: g T (P A c(P) represents the cumulative cost of the current vertex; A ,P B ) represents the local connection cost from a vertex to a candidate vertex (length, curvature penalty, etc., obtained from the improved local cost function); ImpH(P B ) represents the improved heuristic function, the definition of which is given in step S5; the candidate edge (P) is... A ,P B According to the total cost f(P) B After sorting in ascending order, add to edge queue Q. E .

[0147] C3: Perform edge expansion from Q E Select the optimal edge (v) m ,x m );

[0148] The edge expansion process includes:

[0149] C3-1: The criterion can be improved if the following condition is met:

[0150] g T (v m )+c(v m ,x m )+ImpH(x m )≤g T (x goal (6)

[0151] Among them, g T (x goal If the cost of the edge is the currently known optimal solution, then it is considered that the edge may improve the optimal solution, and proceed to the next step;

[0152] If the condition is not met, then discard the edge.

[0153] C3-2: Non-inferiority criterion, if the following is satisfied:

[0154] g(v m )+c(v m ,x m )≤g T (V) (7)

[0155] If the condition is not met, the edge is considered superior to an existing path and can be connected to the tree structure; otherwise, the edge is discarded.

[0156] C3-3: After the above two steps of screening, collision detection is performed. If edge (v,x) has no collision and satisfies the kinematic constraints, it is added to the explicit tree, and the cumulative cost g is updated. T (v);

[0157] Collision detection employs a discrete sampling + continuous segment interpolation method to check whether the trajectory from vertex v to candidate point x intersects with an obstacle. If a collision exists, the edge is discarded; otherwise, the edge (v,x) is added to the explicit tree structure (V,E), and the cumulative cost g of the target node is updated. T (v), and simultaneously push it into the vertex queue Q. V .

[0158] C4: The pruning rule applies during each round of expansion or stagnation recovery if any node x∈V satisfies

[0159] g T (x)+ImpH(x)≥g T(x goal (8)

[0160] Then delete the node and its associated edges; corresponding obviously redundant or high-curvature inferior edges are deleted simultaneously; pruning causes the search domain to monotonically shrink as the optimal solution improves.

[0161] In step S4 when If no improvement is made in multiple consecutive rounds, an escape mechanism is triggered. The execution of the escape mechanism includes:

[0162] D1: Set radius expansion r new =λr, λ∈[1.2,1.5], expanding the explorable domain;

[0163] D2: Density assessment and resampling

[0164] calculate If ρ < ρ th Then supplement One sample, and add X samples ;

[0165] D3: After completing the supplementary sampling in conjunction with the main loop, return to steps B5 and B6 to refresh the set and radius, and continue the search adaptively.

[0166] The improved heuristic function in step S5 is defined as follows:

[0167]

[0168] Among them, ||P B -P goal || represents the Euclidean distance from the candidate point to the target point; θ represents the angle between the candidate point's direction of movement and the target direction, normalized to [0,π]; λ is the weight coefficient for direction correction; Formula (9) shows that when the candidate point and the target point are in the same direction, the heuristic mainly depends on the straight-line distance; when the candidate point's direction deviates from the target, the cost is amplified, thus guiding the search to expand in a more reasonable direction;

[0169] The improved local cost function is defined as follows:

[0170] Local connection cost for candidate edge (P) A ,P B Its local cost is defined as:

[0171] c(P A ,P B ) = w L ·L+w θ ·Δθ+w κ ·κ (10)

[0172] Where L represents the connection length, Δθ represents the directional deflection angle of the edge, and κ represents the local curvature; w L ,w θ ,w κ The weights are for length, direction, and curvature, respectively. Formula (10) shows that the local cost consists of three parts: the longer the path, the higher the cost; the larger the directional deflection angle, the higher the cost, encouraging smoother paths; and the larger the curvature, the higher the cost, to avoid sharp turns. To balance convergence and executability, this embodiment recommends a weight of w. L =1.0,w θ =0.5,w κ =0.2. Furthermore, the curvature must satisfy k ≤ k max The minimum turning radius of the robot is determined to ensure that the trajectory is kinematically executable.

[0173] In this embodiment, the termination conditions of step S6 include (any one of them needs to be satisfied): (1) End point x goal (1) Successfully access tree T; (2) Improvement rate of optimal cost for multiple consecutive rounds < 0.1%; (3) Reach the maximum number of iterations or time budget.

[0174] The method of this invention introduces a kinematic constraint model during the sampling process and combines it with an adaptive sampling density control strategy based on environmental features to achieve high efficiency in the path search process, smoothness of the path results, and executability, thereby improving the robot's navigation and obstacle avoidance capabilities in complex environments.

[0175] The method of this invention solves the problems of non-executable paths and low search efficiency in existing mobile robot path planning by introducing kinematic constraints and a variable density sampling mechanism. It has the advantages of fast computing speed and high path quality, and is particularly suitable for autonomous mobile robot navigation in complex obstacle environments, showing good application prospects.

[0176] Example 2:

[0177] To verify the effectiveness and efficacy of the method of the present invention, a simulation comparison experiment was conducted in this embodiment, as follows:

[0178] In this embodiment, the existing BIT* algorithm and the K-BIT* algorithm proposed in this invention are used for mobile robot path planning in the same environment. Figure 3 For BIT* path planning, Figure 4 For K-BIT* path planning, Figure 5 A comparison of BIT* and K-BIT* path planning.

[0179] Combination Figures 3-5It is evident that existing BIT* algorithms generate paths with numerous polylines and uneven trajectories. In contrast, the K-BIT* algorithm of this invention produces smoother, shorter paths under the same conditions, and conforms to kinematic constraints, demonstrating higher convergence efficiency and path quality.

Claims

1. A path planning method for a mobile robot that combines kinematic constraints and variable density sampling, characterized in that, Includes the following steps: S1: Based on the acquired environmental map information, set the starting point and the target point, add the starting point to the vertex set, add the target point to the sampling set, initialize the node priority queue and the edge priority queue, and establish kinematic constraints and environmental parameters. S2: Search for the main loop path based on kinematic constraints and environmental parameters; S3: Compare the optimal elements in the vertex queue and the edge queue. If the vertex cost is better, perform vertex expansion and generate candidate edges. If the edge cost is better, perform feasibility verification and connect to the path tree. S4: When the search stalls or gets stuck in a local optimum, the escape mechanism is triggered, which enhances the exploration capability through radius expansion and supplementary sampling; S5: Establish improved heuristic functions and local cost functions to guide search convergence and improve trajectory smoothness and executability; S6: Output the path planning results when the termination condition is met.

2. The mobile robot path planning method combining kinematic constraints and variable density sampling according to claim 1, characterized in that, The establishment of kinematic constraints in step S1 includes: calculating the minimum turning radius and maximum curvature range based on the robot chassis type and steering characteristics; eliminating points that do not meet the kinematic conditions during the node generation stage; verifying curvature continuity and acceleration constraints during the edge connection stage, and discarding edges that do not meet the conditions.

3. The mobile robot path planning method combining kinematic constraints and variable density sampling according to claim 2, characterized in that, Step S1 specifically includes: A1: Initialize the node set, set the node set V = {x start } contains only the starting point, which serves as the root node of the search tree; edge set A2: Initialize the sampling set, set the sampling set X samples ={x goal }, containing only the endpoint, forming a goal-oriented bias to accelerate early convergence; providing starting samples for subsequent high-density exploration of the endpoint neighborhood; A3: Priority queue initialization, edge priority queue Dequeue vertices based on their total cost, from smallest to largest; Vertex priority queue. Dequeue the vertex heuristic from smallest to largest; A4: Kinematic constraints and environmental parameter settings, setting the minimum linear velocity v min With maximum angular velocity ω max And based on this, the minimum turning radius is defined: Establish a collision detection model; record the dimension d, and set the constant γ and the environmental coefficient k for radius adaptation. A5: State representation and neighborhood index, node state adopts pose (x,y,θ); pre-built KD-Tree to support neighborhood query with radius r, complexity O(logn).

4. The mobile robot path planning method combining kinematic constraints and variable density sampling according to claim 3, characterized in that, During the main loop path search process in step S2, resampling, pruning, and radius adaptation are performed. The specific process includes: B1: Stasis Detection like The search is deemed stalled or trapped in a local optimum. B2: Escape strategy triggered Execute EscapeStrategy() to generate breakthrough samples in blocked or sparse regions; B3: Pruning With the current optimal target cost g T (x goal Let be the threshold. For any node x, if the pruning condition is satisfied: g T (x)+ImpH(x)≥g T (x goal ) (2) Then delete the node and its associated edges; B4: Variable density sampling Call Sample(m,g) T (x goal Generate m new samples X sample_i Sampling density adapts to regional potential: sparse sampling in low-cost / accessible areas, and appropriately dense sampling in areas with dense obstacles; sampling points must pass the collision-free test of the grid / distance field. B5: Set and Queue Refresh Record V old ←V, and update V←V∪X sample_i The updated V is heuristically sorted and then pushed into Q in batches. V ; B6: Radius Adaptive Update the search radius r to: Where γ is a constant and d is the spatial dimension.

5. The mobile robot path planning method combining kinematic constraints and variable density sampling according to claim 4, characterized in that, Step S3 includes: C1: Take Q respectively V Q E The first element of the queue, if min(Q) V )≤min(Q E If the vertex is not fully expanded, proceed to step C2 to perform vertex expansion; otherwise, proceed to step C3 to perform edge expansion. C2: Perform vertex expansion from Q V Select the optimal vertex P A ; C3: Perform edge expansion from Q E Select the optimal edge (v) m ,x m ); C4: The pruning rule applies during each round of expansion or stagnation recovery if any node x∈V satisfies g T (x)+ImpH(x)≥g T (x goal ) (4) Then delete the node and its associated edges; corresponding obviously redundant or high-curvature inferior edges are deleted simultaneously.

6. The mobile robot path planning method combining kinematic constraints and variable density sampling according to claim 5, characterized in that, The vertex expansion process in step C2 includes: C2-1: Neighborhood Search: Using KD-Tree to retrieve the candidate sampling point set X near KD-Tree can complete nearest neighbor search in O(logn) time; for any candidate sampling point P B ∈X near Calculate its Euclidean distance to the current vertex: At the same time, combined with the heuristic function h(P) B Perform pre-sorting; C2-2: Set the minimum turning radius based on the allowable range of the robot's linear velocity v and angular velocity ω. Where v is the linear velocity, ω max The maximum angular velocity is used; the motion circle method is employed to determine whether candidate points satisfy the constraints. C2-3: Enqueuing candidate edges: For candidate points P selected through kinematic constraints B Generate edge (P) A ,P B ), and calculate its total cost: f(P B )=g T (P A )+c(P A ,P B )+ImpH(P B ) (6) Wherein: g T (P A c(P) represents the cumulative cost of the current vertex; A ,P B ImpH(P) represents the local connection cost from a vertex to a candidate vertex; B ) represents an improved heuristic function; candidate edges (P) A ,P B According to the total cost f(P) B After sorting in ascending order, add to edge queue Q. E .

7. The mobile robot path planning method combining kinematic constraints and variable density sampling according to claim 6, characterized in that, The process of determining whether candidate points satisfy the constraints using the motion circle method in step C2-2 includes: 1) At the current vertex P A Draw a normal line perpendicular to the direction of velocity at that point; 2) Move vertex P A With candidate point P B Connect the dots; 3) The intersection of the two straight lines is the center C of the circle, from which the turning radius can be obtained: 4) If R <R min If the candidate point is deemed infeasible, it will be deleted. 5) If R ≥ R min If the candidate point is selected, it is retained and added to the candidate edge set.

8. The mobile robot path planning method combining kinematic constraints and variable density sampling according to claim 5, characterized in that, The edge expansion process in step C3 includes: C3-1: The criterion can be improved if the following condition is met: g T (v m )+c(v m ,x m )+ImpH(x m )≤g T (x goal ) (7) Among them, g T (x goal If the cost of the edge is the currently known optimal solution, then it is considered that the edge may improve the optimal solution, and proceed to the next step; If the condition is not met, then discard the edge. C3-2: Non-inferiority criterion, if the following is satisfied: g(v m )+c(v m ,x m )≤g T (V) (8) If the condition is not met, the edge is considered superior to an existing path and can be connected to the tree structure; otherwise, the edge is discarded. C3-3: After the above two steps of screening, collision detection is performed. If edge (v,x) has no collision and satisfies the kinematic constraints, it is added to the explicit tree, and the cumulative cost g is updated. T (v); Collision detection employs a discrete sampling + continuous segment interpolation method to check whether the trajectory from vertex v to candidate point x intersects with an obstacle. If a collision exists, the edge is discarded; otherwise, the edge (v,x) is added to the explicit tree structure (V,E), and the cumulative cost g of the target node is updated. T (v), and push it into the vertex queue QV.

9. A mobile robot path planning method combining kinematic constraints and variable density sampling according to claim 5, characterized in that, In step S4 when If no improvement is made in multiple consecutive rounds, an escape mechanism is triggered. The execution of the escape mechanism includes: D1: Set radius expansion r new =λr, λ∈[1.2,1.5], expanding the explorable domain; D2: Density assessment and resampling calculate If ρ < ρ th Then supplement One sample, and add X samples ; D3: After completing the supplementary sampling in conjunction with the main loop, return to steps B5 and B6 to refresh the set and radius, and continue the search adaptively.

10. A mobile robot path planning method combining kinematic constraints and variable density sampling according to claim 9, characterized in that, The improved heuristic function in step S5 is defined as follows: Among them, ||P B -P goal || represents the Euclidean distance from the candidate point to the target point; θ represents the angle between the candidate point's direction of motion and the target direction, normalized to [0,π]; λ is the weighting coefficient for direction correction. The improved local cost function is defined as follows: Local connection cost for candidate edge (P) A ,P B Its local cost is defined as: c(P A ,P B )=in L ·L+w θ ·Δθ+w κ ·κ (10) Where L represents the connection length, Δθ represents the directional deflection angle of the edge, and κ represents the local curvature; w L ,w θ ,w κ The weights are for length, direction, and curvature, respectively.

Images