A dynamic trajectory planning method for mobile robots under elastic band constraints

Abstract

The present invention belongs to the field of artificial intelligence technology and is a method for dynamic trajectory planning of a mobile robot under elastic band constraints. The method comprises the following steps: initializing the robot's dynamic equations and expressing the optimal motion planning problem using a cost function; constructing a random tree dataset, combining it with a variable sampling area, performing a rewiring operation, and optimizing the random tree structure; adjusting the restricted sampling area using a target bias method; employing a layered collision detection method for obstacles, performing rough collision detection and fine detection to eliminate invalid paths; calculating the collision risk probability and removing nodes whose collision risk probability exceeds a threshold; and optimizing the robot's dynamic trajectory using an elastic band method to achieve dynamic trajectory planning for the mobile robot. The present invention detects the status and collision risk of each node during the trajectory optimization process, significantly improving the robustness and adaptability of the system. The robot can adaptively adjust its trajectory in complex environments to meet obstacle avoidance requirements.

Description

Technical Field

[0001] The present invention relates to the field of artificial intelligence technology, and in particular to a dynamic trajectory planning method for a mobile robot under elastic band constraints. Background Art

[0002] With the continuous maturity and enrichment of robotics technology, robots are increasingly being used in industrial production, medical services, logistics, and other fields, greatly improving efficiency and convenience. Robotic systems typically consist of environmental perception, decision-making, control, and communication modules. The decision-making module, at its core, is responsible for intelligent decision-making and task judgment. Path planning research occupies a central position in today's robotics field. Path planning involves finding an efficient path from an initial position to a target location in a configuration space, while avoiding obstacles and satisfying certain optimization criteria. In real-world applications, robots must follow an algorithm-generated trajectory to reach their target point without collision. Sometimes, this requires considering the constraints between multiple objectives and optimizing multiple process parameters, such as shortening arrival time, reducing resource consumption, and improving safety and robustness. It is foreseeable that trajectory planning technology will face more and more severe challenges in future applications, urgently requiring the support of innovative algorithms.

[0003] Currently, common path planning algorithms include algorithm, Algorithms, genetic algorithms, particle swarm optimization algorithms, etc. These traditional methods have been widely used, but they still have their own limitations when facing different problems. The algorithm requires a large amount of sampling and has many useless nodes, which is not very efficient. Genetic algorithms require a lot of computing power for multiple iterations to converge to the optimal solution. Particle swarm optimization is prone to falling into local optimal solutions too early when facing high-dimensional problems. Compared with the above traditional methods, the rapid randomized exploration tree (RRT) algorithm has attracted widespread attention due to its low complexity, strong adaptability, and no need for pre-modeling of the environment. However, the RRT algorithm does not contain heuristic information, the exploration space is random, and the path found is not the optimal solution. To solve this problem, The algorithm was proposed, which not only maintains the probability completeness but also achieves asymptotic optimality. However, this continuous path optimization process requires a large number of iterations, which leads to The algorithm is relatively slow in converging to the optimal solution. The sampling method is optimized and bias points are used for intelligent sampling, but this method is more dependent on the quality of the initial path. The algorithm uses the triangle inequality principle to provide a better method for reselecting parent nodes and rerouting routes, but since the range of parent nodes becomes wider, the search time will be extended.

[0004] Some improved RRT algorithms are also applied to motion planning problems in dynamic environments. These algorithms can be divided into two categories: reactive algorithms and proactive algorithms. Reactive algorithms only calculate the next action at each moment based on current conditions. When the environment changes, path replanning is required. This type of algorithm can cope with highly dynamic and unpredictable environments, but it also requires a continuous high refresh rate. Its variants focus on improving the speed of replanning. Proactive algorithms need to predict the trajectory of moving obstacles, delete possible collision paths, and actively avoid obstacles. RiskRRT is a time-based RRT algorithm that proposes a probabilistic collision risk function to guide planning and pathfinding methods, but does not consider optimizing paths.

[0005] The elastic band (EB) method can be used to optimize the current trajectory in dynamic environments. It uses physical properties as a heuristic guide to optimize the path while avoiding obstacles. The EB method requires an initial trajectory and the construction of a configuration space. In dynamic environments, the path planner must generate a feasible trajectory as quickly as possible. When working with large scenarios, such as airports, constructing the configuration space consumes significant computational resources, and quickly generating a feasible initial trajectory is difficult.

[0006] While both reactive and proactive algorithms are widely used in robotic motion planning, they focus on different areas. While various approaches have addressed some aspects of trajectory planning, they still face numerous challenges. Therefore, efficiently generating initial trajectories in dynamic environments, maintaining trajectory safety throughout the entire process, maximizing trajectory optimization speed, minimizing computing power consumption, and ensuring real-time trajectory validity remain core issues that urgently need to be addressed in the field of robotic control. Summary of the Invention

[0007] According to the technical problems raised above, a method for dynamic trajectory planning of a mobile robot under elastic band constraint is provided. A method based on target bias sampling with a variable sampling area is used to quickly obtain an initial feasible path with good quality. By combining the elastic band method (EB) with the time-based RRT algorithm, the trajectory is dynamically optimized, achieving real-time motion planning for the robot.

[0008] The technical means adopted in the present invention are as follows:

[0009] A dynamic trajectory planning method for a mobile robot under elastic band constraints, comprising:

[0010] Initialize the robot dynamics equation and use the cost function to express the optimal motion planning problem;

[0011] Construct a random tree dataset, combine it with variable sampling regions, perform rewiring operations, and optimize the random tree structure;

[0012] Use the target bias method to adjust the restricted sampling area;

[0013] A layered collision detection method is used for obstacles, with rough collision detection and fine detection to eliminate invalid paths;

[0014] Calculate the collision risk probability and remove nodes whose collision risk probability exceeds the threshold;

[0015] The elastic band method is applied to optimize the dynamic trajectory of the robot and realize dynamic trajectory planning of the mobile robot.

[0016] Furthermore, the initialization of the robot dynamics equation specifically includes:

[0017] Assume the state space is , collision space , free space , yes A proper subset of ; In a dynamic environment, the state space is time-varying, and the collision space is recorded as , the free space is denoted as ; Let the starting position be and the target position is , the target area is defined as , represents the radius of the target area, and the control space of the robot is , , the dynamics of the robot are expressed as follows:

[0018]

[0019] in, represents the state of the robot at timestamp t, Represents the dynamic function of the robot, through the state and control Derived State .

[0020] Furthermore, the optimal motion planning problem is represented by a cost function, specifically including:

[0021] The set of all feasible trajectories is expressed as , construct the cost function as follows:

[0022]

[0023]

[0024] in, is a constant, is the robot linear speed; and is the position information of the robot's two states before and after, Timestamp The linear velocity of the robot at time ; the optimal motion planning problem is expressed as:

[0025]

[0026] in, represents the feasible path with the minimum cost.

[0027] Furthermore, the construction of the random tree data set, combining the variable sampling area, performing a rewiring operation, and optimizing the random tree structure specifically includes:

[0028] Constructing a random tree dataset , represents a set of vertices in a tree, Representing the connection relationship between each vertex in the group, determining the positions of the starting point and the target point, and constructing an environment map including obstacles; before each sampling process iteration, first determining whether the initial path has been determined; if the initial path has not been determined, using the variable sampling area to optimize the sampling effect;

[0029] Generate sampling points , traverse the existing random tree and find The nearest node ,from Towards The direction is extended by a step distance to form a new node ;if and If the connected path is not blocked by obstacles, the new node Reselect the parent node through the adaptively adjusted reconnection radius and perform the rewiring operation to optimize the random tree structure; if and If the connected paths are blocked by obstacles, the sampling strategy is reselected and the algorithm continues until the set maximum number of iterations is reached and the task ends.

[0030] Furthermore, the method of adjusting the restricted sampling area by using the target bias method specifically includes:

[0031] For complex or special environments, the target bias method is used to adjust the restricted sampling area to generate the initial path:

[0032]

[0033] in, are random sampling points, is a uniformly distributed random number generated in the range (0,1). Indicates that sampling points are randomly selected in free space; is the target deviation threshold, When the path is not blocked by an obstacle, the target point is sampled and the path is extended toward the target point until an obstacle is encountered to block the path.

[0034] Furthermore, the rough collision detection uses the bounding volume technology to calculate the center position and the radius of the bounding circle of each polygonal obstacle to obtain the obstacle center. Minimum distance to a path segment , obstacle center The coordinates are , the two endpoints of the path segment and The coordinates of and , let the vector is the path segment direction vector, For endpoints arrive The vectors are:

[0035]

[0036]

[0037] Use parameters express On the line segment The projection position on is calculated by the inner product:

[0038]

[0039]

[0040] Will Restricted to Make sure the projected point is on the actual path and not on the path extension:

[0041]

[0042] According to the parameters , the projection point coordinates Expressed as:

[0043]

[0044] The minimum distance The center point of the obstacle The distance to the projection point is expressed as:

[0045]

[0046] If the minimum distance If the radius is larger than the enclosing circle, it is considered that there is no collision and the rough detection is completed.

[0047] Furthermore, the fine detection uses a linear interpolation method to generate interpolation points on the path, and traverses each point to detect whether a collision occurs;

[0048] Assume that the starting point and ending point of the path are and , through the parameters Perform linear interpolation to generate any point for:

[0049]

[0050] During the detection process, the step size is set Generate an interpolation point sequence to detect the upper, lower, left, and right boundaries of the obstacle in a given coordinate system, i.e. width and height. Expressed as:

[0051]

[0052] in, is the upper boundary position parameter of the obstacle, is the lower boundary position parameter of the obstacle, is the left boundary position parameter of the obstacle, is the right boundary position parameter of the obstacle, is the horizontal length parameter of the obstacle, is the longitudinal length parameter of the obstacle; and is the coordinate of the obstacle center. If the interpolation point Requirements: , a collision is considered to have occurred, the algorithm terminates and marks the path invalid.

[0053] Furthermore, calculating the collision risk probability and removing nodes whose collision risk probability exceeds a threshold value specifically includes:

[0054] The state of the robot is represented as , is the parent node and child node of the node, is the tree depth, Timestamp , the timestamp of the root node is , the time increment between two nodes is ; is the control input, To control the output, for The probability collision risk when , is expressed as:

[0055]

[0056]

[0057] in, represents the collision probability due to static obstacles, Indicates that due to the timestamp The collision probability of the moving obstacle at Indicates that due to the timestamp Moving obstacles The collision probability of the planned trajectory is calculated during the optimization process. If the collision probability is greater than the threshold, the corresponding node is not added to the optimized trajectory.

[0058] Furthermore, the application of the elastic band method to optimize the dynamic trajectory of the robot specifically includes:

[0059] The internal contraction force The definition is as follows:

[0060]

[0061] The node positions are updated according to the following equation:

[0062]

[0063] in, and are two positive proportional factors, the elastic band method optimization process will end when the following conditions are met:

[0064] when , is the number of nodes on the trajectory, indicating the contraction force Less than the set force is the optimal trajectory; when the dynamic and kinematic constraints that the robot needs to satisfy make the optimization process unable to proceed further, the optimization process ends; when the collision probability of the new node exceeds the threshold, that is, , then the new node Invalidation.

[0065] Compared with the prior art, the present invention has the following advantages:

[0066] The present invention provides a dynamic trajectory planning method for mobile robots under elastic band constraints, improving computational efficiency and real-time performance. This method utilizes a target-biased sampling method with a variable sampling area, which improves sampling efficiency compared to full-map random sampling, increasing the speed and quality of generating initial trajectories. After quickly generating the initial trajectory, an elastic band-based method is used to optimize the position of each node. This method maintains the structure of the original time-base tree, eliminates branching waste, and avoids the need for additional computation to modify node information in the time-base tree.

[0067] The present invention provides a dynamic trajectory planning method for a mobile robot under elastic band constraints, ensuring safety in dynamic environments. By using a probabilistic collision risk function as the external repulsive force of the trajectory, the method allows nodes on the trajectory to avoid areas where obstacles may reach, ensuring the feasibility and safety of trajectory planning.

[0068] The dynamic trajectory planning method for a mobile robot under elastic band constraints, provided by this invention, improves robustness and adaptability. Using a time-based RRT algorithm, this method detects the state and collision risk of each node during trajectory optimization, significantly improving the robustness and adaptability of the system. The robot can adaptively adjust its trajectory in complex environments to meet obstacle avoidance requirements.

[0069] Based on the above reasons, the present invention can be widely promoted in fields such as artificial intelligence. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0071] Figure 1 This is a flow chart of the dynamic trajectory planning method of a mobile robot under elastic band constraints of the present invention.

[0072] Figure 2 This is the working framework of the global planner and dynamic replanner in the embodiment of the present invention.

[0073] Figure 3 This is a flow chart of target bias sampling of variable sampling area of the present invention.

[0074] Figure 4 This is the trajectory optimization process under obstacle constraints in the present invention. DETAILED DESCRIPTION

[0075] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments of the present invention can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0076] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The following description of at least one exemplary embodiment is actually only illustrative and is in no way intended to limit the present invention and its application or use. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0077] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or combinations thereof.

[0078] Unless otherwise specifically stated, the relative arrangement of the parts and steps, numerical expressions and numerical values described in these embodiments do not limit the scope of the present invention. At the same time, it should be clear that, for ease of description, the sizes of the various parts shown in the drawings are not drawn according to the actual proportional relationship. The techniques, methods and equipment known to ordinary technicians in the relevant fields may not be discussed in detail, but where appropriate, the techniques, methods and equipment should be considered as part of the authorization specification. In all examples shown and discussed here, any specific value should be interpreted as merely exemplary and not as a limitation. Therefore, other examples of the exemplary embodiments may have different values. It should be noted that similar numbers and letters represent similar items in the following figures, so once an item is defined in one figure, it does not need to be further discussed in subsequent figures.

[0079] like Figure 1 As shown, the present invention provides a dynamic trajectory planning method for a mobile robot under elastic band constraints, comprising:

[0080] Initialize the robot dynamics equations and use the cost function to express the optimal motion planning problem.

[0081] In specific implementation, as a preferred embodiment of the present invention, the initialization of the robot dynamics equation specifically includes:

[0082] Assume the state space is , collision space , free space , yes A proper subset of ; In a dynamic environment, the state space is time-varying, and the collision space is recorded as , the free space is denoted as ; Let the starting position be and the target position is , the target area is defined as , represents the radius of the target area, and the control space of the robot is , , the dynamics of the robot are expressed as follows:

[0083]

[0084] in, represents the state of the robot at timestamp t, and are two adjacent states. Represents the dynamic function of the robot, which can be expressed by the state and control Derived State .

[0085] In specific implementation, as a preferred embodiment of the present invention, the use of a cost function to represent the optimal motion planning problem specifically includes:

[0086] The set of all feasible trajectories is expressed as , construct the cost function as follows:

[0087]

[0088]

[0089] in, is a constant, is the robot linear velocity, and is the position information of the robot's two states before and after, Timestamp The linear velocity of the robot at time . The optimal motion planning problem is expressed as:

[0090]

[0091] in, represents the feasible path with the minimum cost.

[0092] like Figure 3 As shown, a random tree dataset is constructed, variable sampling areas are combined, rewiring operations are performed, and the random tree structure is optimized.

[0093] In specific implementation, as a preferred embodiment of the present invention, the construction of a random tree data set, combining a variable sampling area, performing a rewiring operation, and optimizing the random tree structure specifically include:

[0094] Constructing a random tree dataset , represents a set of vertices in a tree, The connection relationship between each vertex in the group is represented, the positions of the starting point and the target point are determined, and an environment map including obstacles is constructed; before each sampling process iteration, it is first determined whether the initial path has been determined; if the initial path has not been determined, the variable sampling area is used to optimize the sampling effect.

[0095] Generate sampling points , traverse the existing random tree and find The nearest node ,from Towards The direction is extended by a step distance to form a new node ;if and If the connected path is not blocked by obstacles, the new node Reselect the parent node through the adaptively adjusted reconnection radius and perform the rewiring operation to optimize the random tree structure; if and If the connected paths are blocked by obstacles, the sampling strategy is reselected and the algorithm continues until the set maximum number of iterations is reached and the task ends.

[0096] The target bias method is used to adjust the restricted sampling area.

[0097] In specific implementation, as a preferred embodiment of the present invention, the use of the target bias method to adjust the restricted sampling area specifically includes:

[0098] For complex or special environments, the target bias method is used to adjust the restricted sampling area to generate the initial path:

[0099]

[0100] in, are random sampling points, is a uniformly distributed random number generated in the range (0,1). Indicates that sampling points are randomly selected in free space; is the target deviation threshold, When the path is not blocked by an obstacle, the target point is sampled and the path is extended toward the target point until an obstacle is encountered to block the path.

[0101] In practice, when sampling a new node fails due to an obstacle, the algorithm focuses on a local area centered on the new node, rather than blindly sampling the entire global environment. This increases the probability of placing the new node in a narrow local channel, enabling it to quickly bypass obstacles and reducing initial path calculation time. If local expansion fails repeatedly, the algorithm jumps out of the local minimum and performs more extensive random sampling, thus avoiding the algorithm from falling into a local optimum and maintaining probabilistic completeness.

[0102] A layered collision detection method is used to detect obstacles, performing both coarse collision detection and fine detection to eliminate invalid paths. Research has shown that collision detection is a significant factor affecting the convergence speed of trajectory planning algorithms. Therefore, this paper proposes a layered detection method that combines coarse and fine detection to improve detection efficiency.

[0103] In specific implementation, as a preferred embodiment of the present invention, the rough collision detection adopts the bounding volume technology. For each polygonal obstacle, its center position and the radius of the bounding circle are calculated to obtain the obstacle center. Minimum distance to a path segment , obstacle center The coordinates are , the two endpoints of the path segment and The coordinates of and , let the vector is the path segment direction vector, For endpoints arrive The vectors are:

[0104]

[0105]

[0106] Use parameters express On the line segment The projection position on is calculated by the inner product:

[0107]

[0108]

[0109] Will Restricted to Make sure the projected point is on the actual path and not on the path extension:

[0110]

[0111] According to the parameters , the projection point coordinates Expressed as:

[0112]

[0113] The minimum distance The center point of the obstacle The distance to the projection point is expressed as:

[0114]

[0115] If the minimum distance If the radius is larger than the enclosing circle, it is considered that there is no collision and the rough detection is completed.

[0116] In specific implementation, as a preferred embodiment of the present invention, the fine detection uses a linear interpolation method to generate interpolation points on the path, and traverses each point to detect whether a collision occurs.

[0117] Assume that the starting point and ending point of the path are and , through the parameters Perform linear interpolation to generate any point for:

[0118]

[0119] During the detection process, the step size is set Generate an interpolation point sequence to detect the upper, lower, left, and right boundaries of the obstacle in a given coordinate system, i.e. width and height. Expressed as:

[0120]

[0121] in, is the upper boundary position parameter of the obstacle, is the lower boundary position parameter of the obstacle, is the left boundary position parameter of the obstacle, is the right boundary position parameter of the obstacle, is the horizontal length parameter of the obstacle, is the longitudinal length parameter of the obstacle. and is the coordinate of the obstacle center.

[0122] If the interpolation point Requirements: , it is considered that a collision occurs, the algorithm terminates and marks the path invalid. The optimization process is as follows Figure 4 shown.

[0123] Calculate the collision risk probability and remove nodes whose collision risk probability exceeds the threshold.

[0124] In a specific implementation, as a preferred embodiment of the present invention, calculating the collision risk probability and removing nodes whose collision risk probability exceeds a threshold value specifically includes:

[0125] The state of the robot is represented as , is the parent node and child node of the node, is the tree depth, Timestamp , the timestamp of the root node is , the time increment between two nodes is ; is the control input, To control the output, for The probability collision risk when , is expressed as:

[0126]

[0127]

[0128] in, represents the collision probability due to static obstacles, Indicates that due to the timestamp The collision probability of the moving obstacle at , the trajectory of the moving obstacle is represented by a Gaussian process, Indicates that due to the timestamp Moving obstacles The collision probability of the planned trajectory is calculated during the optimization process. If the collision probability is greater than the threshold, the corresponding node is not added to the optimized trajectory.

[0129] The elastic band method is applied to optimize the dynamic trajectory of the robot and realize the dynamic trajectory planning of the mobile robot. The invention applies an internal contraction force to the nodes on the heuristic trajectory, so that the nodes continue to deform until equilibrium.

[0130] In specific implementation, as a preferred embodiment of the present invention, the application of the elastic band method to optimize the dynamic trajectory of the robot specifically includes:

[0131] The internal contraction force The definition is as follows:

[0132]

[0133] The node positions are updated according to the following equation:

[0134]

[0135] in, and are two positive proportional factors, the elastic band method optimization process will end when the following conditions are met:

[0136] when , is the number of nodes on the trajectory, indicating the contraction force Less than the set force is the optimal trajectory; when the dynamic and kinematic constraints that the robot needs to satisfy make the optimization process unable to proceed further, the optimization process ends; when the collision probability of the new node exceeds the threshold, that is, , then the new node Invalidation.

[0137] During the optimization process, the structure of the original time-base tree does not change, there is no branch waste, and no additional calculation is required to modify the node information on the tree. The optimized trajectory is recorded as The optimization process is attached. Figure 4 shown.

[0138] Example

[0139] like Figure 2 As shown in the figure, the method of the present invention constructs a global planner and a dynamic replanner to plan the trajectory of the robot. The global planner is based on the improved fast The method first uses a target bias sampling method with a variable sampling area to quickly obtain an initial feasible path with good quality during the iterative process. Then, a time-based RRT algorithm dynamic replanner combined with an elastic band method is used to dynamically optimize the trajectory (the contraction force within the trajectory and the repulsion force of external obstacles) as the environment changes, ensuring the security, probabilistic completeness and homotopic optimality of the real-time generated trajectory.

[0140] Based on the technical solution of this invention, numerical simulations, semi-physical simulations, and physical experiments were conducted to verify the effectiveness and practicality of the method. Numerical simulations were conducted using tools such as MATLAB and Python to verify the accuracy of the trajectory planning model. Semi-physical simulations were conducted on the ROS platform to verify the robustness of the method. Experiments were conducted on an actual physical platform to further debug and optimize the model and ensure that the robot could efficiently and safely complete the trajectory planning task in a real environment.

[0141] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for dynamic trajectory planning of a mobile robot under elastic band constraints, characterized in that: include: Initialize the robot dynamics equation and use the cost function to express the optimal motion planning problem; Construct a random tree dataset, combine it with variable sampling regions, perform rewiring operations, and optimize the random tree structure; Use the target bias method to adjust the restricted sampling area; The method of adjusting the restricted sampling area by using the target bias method specifically includes: For complex or special environments, the target bias method is used to adjust the restricted sampling area to generate the initial path: in, are random sampling points, is a uniformly distributed random number generated in the range (0,1). Indicates that sampling points are randomly selected in free space; is the target deviation threshold, For the target point, when the path is not blocked by an obstacle, sample at the target point and expand towards the target point until an obstacle blocks the path; A layered collision detection method is used for obstacles, with rough collision detection and fine detection to eliminate invalid paths; The rough collision detection adopts the bounding volume technology. For each polygon obstacle, its center position and the radius of the bounding circle are calculated to obtain the obstacle center. Minimum distance to a path segment , obstacle center The coordinates are , the two endpoints of the path segment and The coordinates of and , let the vector is the path segment direction vector, For endpoints arrive The vectors are: Use parameters express On the line segment The projection position on is calculated by the inner product: Will Restricted to Make sure the projected point is on the actual path and not on the path extension: According to the parameters , the projection point coordinates Expressed as: The minimum distance The center point of the obstacle The distance to the projection point is expressed as: If the minimum distance If the radius is larger than the enclosing circle, it is considered that there is no collision and the rough detection is completed; The fine detection uses a linear interpolation method to generate interpolation points on the path and traverses each point to detect whether a collision occurs; Assume that the starting point and ending point of the path are and , through the parameters Perform linear interpolation to generate any point for: During the detection process, the step size is set Generate an interpolation point sequence to detect the upper, lower, left, and right boundaries of the obstacle in a given coordinate system, i.e. width and height. Expressed as: in, is the upper boundary position parameter of the obstacle, is the lower boundary position parameter of the obstacle, is the left boundary position parameter of the obstacle, is the right boundary position parameter of the obstacle, is the horizontal length parameter of the obstacle, is the longitudinal length parameter of the obstacle; and is the coordinate of the obstacle center; if the interpolation point Requirements: , it is considered that a collision occurs, the algorithm terminates and marks the path invalid; Calculate the collision risk probability and remove nodes whose collision risk probability exceeds the threshold; The elastic band method is applied to optimize the dynamic trajectory of the robot and realize dynamic trajectory planning of the mobile robot.

2. The method for dynamic trajectory planning of a mobile robot under elastic band constraints according to claim 1, characterized in that: The initialization of the robot dynamics equation specifically includes: Assume the state space is , collision space , free space , yes A proper subset of ; In a dynamic environment, the state space is time-varying, and the collision space is recorded as , the free space is denoted as ; Let the starting position be and the target position is , the target area is defined as , represents the radius of the target area, and the control space of the robot is , , the dynamics of the robot are expressed as follows: in, represents the state of the robot at timestamp t, Represents the dynamic function of the robot, through the state and control Derived State .

3. The method for dynamic trajectory planning of a mobile robot under elastic band constraints according to claim 1, characterized in that: The optimal motion planning problem is represented by the cost function, specifically including: The set of all feasible trajectories is expressed as , construct the cost function as follows: in, is a constant, is the robot linear velocity, and is the position information of the robot's two states before and after, Timestamp The linear velocity of the robot at time ; the optimal motion planning problem is expressed as: in, represents the feasible path with the minimum cost.

4. The method for dynamic trajectory planning of a mobile robot under elastic band constraints according to claim 1, characterized in that: The random tree data set is constructed, and a rewiring operation is performed in combination with a variable sampling region to optimize the random tree structure, specifically including: Constructing a random tree dataset , represents a set of vertices in a tree, Representing the connection relationship between each vertex in the group, determining the positions of the starting point and the target point, and constructing an environment map including obstacles; before each sampling process iteration, first determining whether the initial path has been determined; if the initial path has not been determined, using the variable sampling area to optimize the sampling effect; Generate sampling points , traverse the existing random tree and find The nearest node ,from Towards The direction is extended by a step distance to form a new node ;if and If the connected path is not blocked by obstacles, the new node Reselect the parent node through the adaptively adjusted reconnection radius and perform the rewiring operation to optimize the random tree structure; if and If the connected paths are blocked by obstacles, the sampling strategy is reselected and the algorithm continues until the set maximum number of iterations is reached and the task ends.

5. The method for dynamic trajectory planning of a mobile robot under elastic band constraints according to claim 1, characterized in that: Calculating the collision risk probability and removing nodes whose collision risk probability exceeds a threshold specifically includes: The state of the robot is represented as , is the parent node and child node of the node, is the tree depth, Timestamp , the timestamp of the root node is , the time increment between two nodes is ; is the control input, To control the output, for The probability collision risk when , is expressed as: in, represents the collision probability due to static obstacles, Indicates that due to the timestamp The collision probability of the moving obstacle at Indicates that due to the timestamp Moving obstacles The collision probability of the planned trajectory is calculated during the optimization process. If the collision probability is greater than the threshold, the corresponding node is not added to the optimized trajectory.

6. The method for dynamic trajectory planning of a mobile robot under elastic band constraints according to claim 1, characterized in that: The application of the elastic band method to optimize the dynamic trajectory of the robot specifically includes: The internal contraction force The definition is as follows: The node positions are updated according to the following equation: in, and are two positive proportional factors, the elastic band method optimization process will end when the following conditions are met: when , is the number of nodes on the trajectory, indicating the contraction force Less than the set force is the optimal trajectory; when the dynamic and kinematic constraints that the robot needs to satisfy make the optimization process unable to proceed further, the optimization process ends; when the collision probability of the new node exceeds the threshold, that is, , then the new node Invalidation.

Images