Robot motion planning method based on rolling search and optimization and related devices

Abstract

The application discloses a robot motion planning method based on rolling search and optimization and related equipment, which allows an autonomous mobile robot with sensor constraints to dynamically plan an obstacle avoidance trajectory in an unknown complex and dense obstacle environment. The method comprises the following steps: firstly, an initial feasible path satisfying the dynamics of the autonomous mobile robot is planned through two process searches; secondly, a nonlinear optimization problem considering path safety and smoothness is solved to improve the path smoothness, with path points as variables; then, the feasible path is time-parameterized by using a uniform B-spline rolling method, and control points are optimized to obtain a safe and dynamically feasible minimum time trajectory; finally, the trajectory risk is evaluated according to the distance relationship between the B-spline trajectory and the obstacle, so that the trajectory time is adjusted. The application is verified through simulation and physical experiment, and can realize risk perception of the autonomous mobile robot in the dense obstacle environment and adaptive speed adjustment.

Description

Technical Field

[0001] This invention relates to the field of robot motion planning technology, and in particular to a robot motion planning method and related equipment based on rolling search and optimization. Background Technology

[0002] Autonomous mobile robots with autonomous navigation capabilities have numerous applications in military, exploration and surveying, and search and rescue missions. However, many technical challenges in autonomous navigation in complex environments limit their application. For autonomous mobile robots with limited perception capabilities, such as limited sensing range and low accuracy, meeting dynamic constraints and ensuring navigation safety in complex environments is a significant technical challenge. Autonomous mobile robots equipped with good motion planners should be able to navigate at slower speeds in environments with dense obstacles, while navigating at higher speeds in reasonably safe and open environments.

[0003] Path planning methods are mainly divided into two types: sampling and search. For sampling methods, Randomized Randomized Tracing (RRT) is a classic method with many variations and applications. However, due to its random sampling nature, the feasible paths planned by RRT are random, making it unsuitable for the rolling motion planning of autonomous mobile robots. For search methods, the A* algorithm is a classic method for searching for obstacle-free paths in three-dimensional discrete space, also with many variations and improved real-time performance. Another type of method utilizes the differential flatness property of autonomous mobile robots to search for motion primitives, which can be used for the rolling motion planning of autonomous mobile robots.

[0004] However, the above methods do not take into account the dynamic characteristics of autonomous mobile robots. In semi-enclosed complex environments, the search for motion primitives is time-consuming and costly, which makes autonomous mobile robots generally lack the risk-adaptive ability of motion planning and unable to make good adjustments between risk avoidance and navigation efficiency in various scenarios.

[0005] Therefore, existing technologies still need to be improved and developed. Summary of the Invention

[0006] The main objective of this invention is to provide a robot motion planning method, system, terminal, and computer-readable storage medium based on rolling search and optimization, aiming to solve the problem that autonomous mobile robots in the prior art lack the risk adaptive capability of motion planning and cannot make good adjustments between risk avoidance and navigation efficiency in various scenarios.

[0007] To achieve the above objectives, the present invention provides a robot motion planning method based on rolling search and optimization, which includes the following steps:

[0008] The map is acquired and the target point is determined. A new planned speed is calculated based on the current speed and yaw direction of the autonomous mobile robot. The current position and the planned speed are converted into the initial node of the search process. The first search process is performed based on the planned speed and the initial node to obtain the path, end position and end speed to the target point.

[0009] A second search process is performed based on the path, the terminal position, and the terminal speed to obtain a first target path that minimizes the cost function. The first target path is then optimized to obtain a second target path.

[0010] The second target path is parameterized using a uniform B-spline to obtain a preliminary parameterized uniform B-spline trajectory. The uniform B-spline trajectory is then nonlinearly optimized to obtain the target trajectory.

[0011] The risk of the trajectory is determined based on the distance relationship between all control points on the target trajectory and obstacles in the map, and the trajectory time of the target trajectory is dynamically adjusted based on the risk.

[0012] Using a rolling replanning strategy, a new second target path and a new target trajectory after adjusting the trajectory time are obtained.

[0013] Optionally, the robot motion planning method based on rolling search and optimization, wherein calculating the new planned speed based on the current speed and yaw direction of the autonomous mobile robot specifically includes:

[0014] Obtain the current speed V of the autonomous mobile robot cur and yaw direction cur According to the formula Calculate the virtual velocity V in the yaw direction. yaw Where r is the major axis radius of the autonomous mobile robot, and V max It is the maximum scalar velocity that satisfies the navigation of an autonomous mobile robot, where α is a constant and θ is the yaw direction. cur The angle with the horizontal direction;

[0015] The virtual speed V taw With the speed V cur Horizontal component velocity V hcur By performing synthesis, a new planned velocity V is obtained. init If ||V init ||2>V max Then make ||V init ||2=V max and maintain V inir The direction remains unchanged.

[0016] Optionally, the robot motion planning method based on rolling search and optimization, wherein converting the current position and the planned velocity into initial nodes for the search process, and performing a first search process based on the planned velocity and the initial nodes to obtain the path, end position, and end velocity to the target point, specifically includes:

[0017] The current position P cur and the planned speed V init Transform into the initial node n of the search process init Initialize the open set O, the closed set I, and the flag reach-horizon, and set the initial node n. init Add to the open set O, where the open set O is used to store the expanded nodes, the closed set I is used to store all the traversed nodes, and the flag reach-horizon is used to determine whether the search node exceeds the preset planning range H.

[0018] Iterate through the main loop, outputting the minimum cost node n from the open set O. cur And add it to the closed set I, and use the Expand() method to process the minimum cost node n. cur The extension is performed to obtain the extended node n. i The EdgeCost() and Heuristic() methods are used to calculate the value of each extended node n. i The actual cost and heuristic cost are calculated, and the RetrievePath() method is used based on the minimum cost node n. cur With extended node n i Tracing back through all the path nodes, we obtain path F. path End position P ads and terminal velocity V ads .

[0019] Optionally, the robot motion planning method based on rolling search and optimization, wherein the second search process based on the path, the end-effector position, and the end-effector velocity to obtain the first target path that minimizes the cost function specifically includes:

[0020] According to the formula f(P) i )=μ1f e (P i )+μ2f d (P i )+μ3f s (P i )+μ4h(P i Calculate the end position P ads To the current position P i The total cost of f(P) is calculated to obtain f(P). iThe set of path points that yields the minimum value is taken as the first target path;

[0021] Where, f(P) i ) is the end position P ads To the current position P i The entire cost, P i For the path F path For any point in, f e (P i ) represents the cost of the path length, f d (P i ) represents the cost in the location direction, f s (P i h(P) represents the cost of maintaining a safe distance. i ) represents the current position P i The heuristic cost of the diagonal distance to the target point, μ1, μ2, μ3, and μ4, are f e (P i ), f d (P i ), f s (P i ) and h(P i The weight of );

[0022] Cost f of positional orientation d (P i The calculation process is as follows: for each position P i Assign a direction attribute and define the end position P. ads To P i The direction is P i The direction of the terminal velocity V ads Direction and P i The angle θ (P) between the directions i Considering the directional cost f d (P i In ), f d (P i ) represents Wherein, θ(P) i ) is the terminal velocity V ads Direction and P i The angle between the directions, d d (P i ) represents position P i With end position P ads The Euclidean distance;

[0023] The cost of safe distance f s (P i The calculation process is as follows: based on position P i The shortest Euclidean distance d to the obstacle in the maps (P i ), and location P i The cost of calculating the safe distance from the safe distance d0 of the obstacles in the map is expressed as:

[0024] Optionally, in the robot motion planning method based on rolling search and optimization, the optimization of the first target path to obtain the second target path specifically includes:

[0025] While ensuring path safety, the bending characteristics are used to analyze the n+1 path points {P0, P1, ..., P...} on the first target path. n} Optimization processing is performed according to the path optimization formula. Calculate the optimization variables {p1, p2, ..., p} n-1} as the second target path, where, For position P i The cost of the belt balance force, f s Let λ1 and λ2 be the weights of the corresponding penalty terms, where λ1 and λ2 are the safe distance cost of the first target path.

[0026] Optionally, the robot motion planning method based on rolling search and optimization, wherein the parameterization of the second target path using uniform B-splines to obtain a preliminary parameterized uniform B-spline trajectory, and the nonlinear optimization of the uniform B-spline trajectory to obtain the target trajectory, specifically includes:

[0027] Obtain the current position of the autonomous mobile robot. Starting from the current position, obtain M+1 consecutive path points in the direction of the target point as the model points of the uniform B-spline trajectory. Query the maximum distance d between two adjacent path points. s and calculate the time interval.

[0028] Connect two adjacent path points p i and p i+1 Replacement control points Q of uniform B-spline i and Q i+1 Calculate the initial velocity and acceleration between two adjacent path points, and the velocity and acceleration at their final positions. Then, using these initial and final velocities and accelerations as boundary conditions, inversely calculate the control points {Q0, Q1, ..., Q...}. N}, through formula The uniform B-spline trajectory P(t) is obtained, where Q i At time t i control points, t iThe node vector U = {t0, t1, ..., t} M}, M = N + p b +1, N represents the number of control points; M represents the number of time intervals required for the entire B-spline trajectory; p b This represents the degree of the B-spline trajectory. These are the corresponding basis functions;

[0029] Nonlinear optimization is performed on the uniform B-spline trajectory P(t) according to the optimization formula. The goal is to find the minimum trajectory time Δt to obtain the target trajectory, where f m It is the cost of smoothing, f s It is the cost of maintaining a safe distance, f v The constraint is on speed, f a The constraint is on acceleration, f T It is a function that minimizes the overall time of the trajectory, where β1, β2, β3, and β4 are the parameter factors that balance smoothness, safety, dynamics, and time minimization, respectively. N-3} is the subset of control points after removing the first and last 6 control points.

[0030] Optionally, the robot motion planning method based on rolling search and optimization, wherein obtaining the trajectory risk based on the distance relationship between all control points on the target trajectory and obstacles in the map, and dynamically adjusting the trajectory time of the target trajectory based on the risk, specifically includes:

[0031] Based on all control points Q on the target trajectory i Distance d from the obstacle in the map c (Q i Determine whether there exists a condition d. c (Q i ) <d thr d c (Q i If it exists, it indicates that the target trajectory has a risk, and the condition d will be met. c (Q i The average distance d is obtained by summing the results. mean , where d thr It is the radius of the bubble centered at any control point;

[0032] According to d the and d mean Through the risk assessment function η nisk The risk of obtaining the trajectory is represented as Where ω is an adjustable parameter;

[0033] Based on minimizing the trajectory time Δt and the trajectory risk η nisk The time to minimize the trajectory is readjusted, expressed as Δt′=(η nisk +1)Δt, where Δt′ is the trajectory time of the dynamically adjusted target trajectory.

[0034] Furthermore, to achieve the above objectives, the present invention also provides a robot motion planning system based on rolling search and optimization, wherein the robot motion planning based on rolling search and optimization includes:

[0035] The path planning module is used to acquire a map and determine the target point, calculate a new planned speed based on the current speed and yaw direction of the autonomous mobile robot, and convert the current position and the planned speed into the initial node of the search process. Based on the planned speed and the initial node, a first search process is performed to obtain the path, end position and end speed to the target point.

[0036] The path optimization module is used to perform a second search process based on the path, the end position, and the end speed to obtain a first target path that minimizes the cost function, and to optimize the first target path to obtain a second target path.

[0037] The trajectory generation module is used to parameterize the second target path using uniform B-splines to obtain a preliminary parameterized uniform B-spline trajectory, and to perform nonlinear optimization on the uniform B-spline trajectory to obtain the target trajectory.

[0038] The trajectory time adjustment module is used to obtain the risk of the trajectory based on the distance relationship between all control points on the target trajectory and obstacles in the map, and dynamically adjust the trajectory time of the target trajectory based on the risk.

[0039] The replanning module is used to obtain a new second target path and a new target trajectory after adjusting the trajectory time using a rolling replanning strategy.

[0040] Furthermore, to achieve the above objectives, the present invention also provides a terminal, wherein the terminal includes: a memory, a processor, and a robot motion planning program based on scrolling search and optimization stored in the memory and executable on the processor, wherein when the robot motion planning program based on scrolling search and optimization is executed by the processor, it implements the steps of the robot motion planning method based on scrolling search and optimization as described above.

[0041] Furthermore, to achieve the above objectives, the present invention also provides a computer-readable storage medium, wherein the computer-readable storage medium stores a robot motion planning program based on scrolling search and optimization, and the robot motion planning program based on scrolling search and optimization, when executed by a processor, implements the steps of the robot motion planning method based on scrolling search and optimization as described above.

[0042] This invention allows sensor-constrained autonomous mobile robots to dynamically plan obstacle avoidance trajectories in unknown, complex, and densely obstacle-filled environments. The method includes: first, searching and planning an initial feasible path that satisfies the dynamics of the autonomous mobile robot through a two-process search; second, improving path smoothness by solving a nonlinear optimization problem considering path safety and smoothness with path points as variables; then, parameterizing the feasible path time using a uniform B-spline rolling method and optimizing the control points to obtain a safe and dynamically feasible minimized time trajectory; finally, assessing the trajectory risk based on the relationship between the B-spline trajectory and obstacle distance, and adjusting the trajectory time accordingly. This invention has been verified through simulation and physical experiments, enabling autonomous mobile robots to perceive risks and adaptively adjust speed in densely obstacle-filled environments. Attached Figure Description

[0043] Figure 1 This is a flowchart of a preferred embodiment of the robot motion planning method based on rolling search and optimization of the present invention;

[0044] Figure 2 This is a schematic diagram of a preferred embodiment of the robot motion planning method based on rolling search and optimization of the present invention.

[0045] Figure 3 This is a schematic diagram illustrating the calculation principle of the new planning speed in the robot motion planning method based on rolling search and optimization of the present invention;

[0046] Figure 4 This is a schematic diagram of path planning for the first and second search processes in the robot motion planning method based on rolling search and optimization of the present invention;

[0047] Figure 5 This is a schematic diagram illustrating the distance between the control point and the obstacle in the robot motion planning method based on rolling search and optimization of this invention;

[0048] Figure 6 This is a schematic diagram of the replanning strategy in the robot motion planning method based on rolling search and optimization in this invention;

[0049] Figure 7 This is the simulation result of end-to-end autonomous navigation in a simulated forest scene in the robot motion planning method based on rolling search and optimization of this invention;

[0050] Figure 8 This is an experimental result of autonomous navigation with the target point behind the autonomous mobile robot in the robot motion planning method based on rolling search and optimization of this invention;

[0051] Figure 9 This is a diagram showing the results of outdoor autonomous navigation in the robot motion planning method based on rolling search and optimization of this invention;

[0052] Figure 10 This invention provides a position and velocity map for outdoor autonomous navigation in a robot motion planning method based on rolling search and optimization.

[0053] Figure 11 This is an outdoor autonomous navigation experiment diagram of the robot motion planning method based on rolling search and optimization in this invention, in which the target is behind the autonomous mobile robot.

[0054] Figure 12 This is a schematic diagram illustrating the principle of a preferred embodiment of the robot motion planning system based on rolling search and optimization of the present invention.

[0055] Figure 13 This is a schematic diagram of the operating environment of a preferred embodiment of the terminal of the present invention. Detailed Implementation

[0056] This application provides a robot motion planning method and related equipment based on rolling search and optimization. To make the purpose, technical solution, and effects of this application clearer and more explicit, the following detailed description is provided with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only for explaining this application and are not intended to limit this application.

[0057] It will be understood by those skilled in the art that, unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains. It should also be understood that terms such as those defined in general dictionaries should be understood to have the same meaning as in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless specifically defined as herein.

[0058] Furthermore, if the embodiments of this invention involve descriptions such as "first" or "second," these descriptions are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, a feature defined with "first" or "second" may explicitly or implicitly include at least one of those features. Additionally, the technical solutions of the various embodiments can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. If the combination of technical solutions is contradictory or impossible to implement, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed by this invention.

[0059] The preferred embodiment of the robot motion planning method based on rolling search and optimization described in this invention, such as... Figure 1 and Figure 2 As shown, the robot motion planning method based on rolling search and optimization includes the following steps:

[0060] Step S100: Obtain the map and determine the target point. Calculate a new planned speed based on the current speed and yaw direction of the autonomous mobile robot. Convert the current position and the planned speed into the initial node of the search process. Perform a first search process based on the planned speed and the initial node to obtain the path, end position, and end speed to the target point.

[0061] like Figure 1 and Figure 2 As shown, a map of the location of the autonomous mobile robot is obtained using a depth camera, and the target point to be reached by the autonomous mobile robot is determined in the map.

[0062] Obtain the current speed and yaw direction of the autonomous mobile robot, and calculate the new planned speed based on the current speed and yaw direction of the autonomous mobile robot.

[0063] Conventional motion primitive search algorithms do not consider the initial yaw angle during motion primitive generation, leading to the initial path direction coinciding with the initial velocity direction. When the angle between the velocity direction and the perception direction of an autonomous mobile robot is large, the robot often collides because it cannot obtain environmental information in the velocity direction in time. Therefore, this invention defines a custom RecalculateV() method, which calculates a new initial planned velocity using the initial yaw angle and initial velocity, aiming to reduce the variation in the initial yaw angle. The calculation principle of RecalculateV() is that if the initial velocity in the horizontal direction and the yaw angle are inconsistent, a new planned velocity is recalculated; otherwise, the original initial velocity is returned.

[0064] Specifically, obtain the current speed V of the autonomous mobile robot.cur and yaw direction cur According to the formula Calculate the virtual velocity V in the yaw direction. yaw Where r is the major axis radius of the autonomous mobile robot, and V max α is the maximum scalar velocity required for autonomous mobile robot navigation. α is a constant; if more emphasis is placed on the yaw direction during planning, the value of α can be increased, and vice versa. θ represents the yaw direction. cur The angle with the horizontal direction.

[0065] like Figure 3 As shown in (a) and (b) in the figure, the virtual velocity V yaw With the speed V cur Horizontal component velocity V hcur By performing synthesis, a new planned velocity V is obtained. init The z-axis component is zero, such as... Figure 3 As shown in (c), if ||V inir ||2>V max Then make ||V init ||2=V max and maintain V init The direction remains unchanged, such as Figure 3 As shown in (d) in the figure.

[0066] Furthermore, such as Figure 4 As shown, the current position of the autonomous robot and the planned speed are converted into the initial node of the search process. Based on the planned speed and the initial node, a first search process (MPS) is performed to obtain the path, end position, and end speed to the target point. The flowchart of the first search process is shown in Table 1 below:

[0067] Table 1. Flowchart of the First Search Algorithm

[0068]

[0069] Specifically, the current position P cur and the planned speed V init Transform into the initial node n of the search process init Initialize the open set O, the closed set I, and the flag reach-horizon, and set the initial node n. init Add to the open set O, where the open set O is used to store the expanded nodes, the closed set I is used to store all the nodes that have been traversed, and the flag reach-horizon (the value of reach-horizon can be true or false) is used to determine whether the search node exceeds the preset planning range H.

[0070] Iterate through the main loop, outputting the minimum cost node n from the open set O. cur And add it to the closed set I, and use the Expand() method to process the minimum cost node n. cur The extension is performed to obtain the extended node n. i The EdgeCost() and Heuristic() methods are used to calculate the value of each extended node n. i The actual cost and heuristic cost are calculated, and the RetrievePath() method is used based on the minimum cost node n. cur With extended node n i Tracing back through all the path nodes, we obtain path F. path End position P ads and terminal velocity V ads .

[0071] The actual cost is the distance from the starting point to node n. i The actual cost, which is typically represented by the Euclidean distance from the starting point to the current node, is the heuristic cost of node n. i The cost of reaching the target point is usually represented by the sum of the control input and time required for the robot to reach the target point. This means that the cost is still required for the robot to reach the target point from the current node.

[0072] Step S200: Perform a second search process based on the path, the end position, and the end speed to obtain a first target path that minimizes the cost function; optimize the first target path to obtain a second target path.

[0073] After completing the first search process, the second search process (ADS*) is completed using the improved ADS* algorithm. Considering that safety factors are also very important in path planning, and that the standard ADS algorithm only considers collision-free paths, the planned path is close to the obstacle, which can easily lead to collisions between the autonomous mobile robot and the obstacle. Therefore, the distance between the path and the obstacle needs to be considered in path planning.

[0074] The second search process first needs to consider the terminal velocity V output by the first search process. ads This allows the path planned by ADS* to follow V. ads The system develops in the desired direction and then gradually moves towards the target. Secondly, the costs of location direction and safe distance are incorporated into the objective function f(P) of ADS*. i )middle.

[0075] According to the formula f(P) i )=μ1f e (P i )+μ2f d (P i )+μ3fs (P i )+μ4h(P i Calculate the end position P ads To the current position P i The total cost of f(P) is calculated to obtain f(P). i The set of path points that yields the minimum value is taken as the first target path.

[0076] Where, f(P) i ) is the end position P ads To the current position P i The entire cost, P i For the path F path For any point in, f e (P i ) represents the cost of the path length, f d (P i ) represents the cost in the location direction, f s (P i h(P) represents the cost of maintaining a safe distance. i ) represents the current position P i The heuristic cost of the diagonal distance to the target point, μ1, μ2, μ3, and μ4, are f e (P i ), f d (P i ), f s (P i ) and h(P i The weight of ).

[0077] Regarding the cost of position orientation, if the orientation angle is simply taken as the orientation cost, the orientation cost will increase over time, leading to increased search time and unnecessary path length. To reduce the change in the initial angle of the autonomous mobile robot and to gradually reduce the role of orientation cost in the search process, this invention uses position P... i relative to the starting point P ads Euclidean distance d d (P i This is also included in the calculation of location and orientation costs.

[0078] Cost f of positional orientation d (P i The calculation process is as follows: for each position P i Assign a direction attribute and define the end position P. ads To P i The direction is P i The direction of the terminal velocity V ads Direction and P i The angle θ (P) between the directions iConsidering the directional cost f d (P i In ), f d (P i ) represents Wherein, θ(P) i ) is the terminal velocity V ads Direction and P i The angle between the directions, d d (P i ) represents position P i With end position P ads The Euclidean distance.

[0079] The safety factor of the path is also very important in the cost of the safe distance. It is generally believed that the farther away from the obstacle, the lower the probability of collision and the safer it is. This invention uses a grid map that can describe the relationship between the environment and the distance to the obstacle, which can easily take the distance relationship between the path point location and the obstacle into the cost function. Therefore, the cost of the safe distance of the location is calculated by the shortest Euclidean distance between the location and the obstacle and the safe distance.

[0080] The cost of safe distance f s (P i The calculation process is as follows: based on position P i The shortest Euclidean distance d to the obstacle in the map s (P i ), and location P i The cost of calculating the safe distance from the safe distance d0 of the obstacles in the map is expressed as:

[0081] The larger the safety distance d0 is, the larger the area where the safety distance cost needs to be considered, and the path will be relatively farther away from obstacles. However, this will also increase the path length and search time. Therefore, d0 is an important constant in balancing path safety and real-time performance.

[0082] Thus, the first search process (MPS) and the second search process (ADS*) are completed. A path planning algorithm that combines motion primitives and improved A* is adopted, taking into account the dynamics and safety of the autonomous mobile robot, ensuring the feasibility of the initial path, and obtaining a path that satisfies the initial feasibility of the autonomous mobile robot's dynamics, namely the first target path.

[0083] Since the path planned by the ADS* algorithm is not smooth enough, directly parameterizing the path planned by ADS* will increase the time of trajectory optimization. Therefore, under the premise of ensuring path safety, this invention treats the path as an elastic band and smooths the first target path, making the path smoother by using the bending characteristics of the elastic band.

[0084] Specifically, on the premise of ensuring path safety, n + 1 path points {P0, P1,......, P n} on the first target path are optimized according to the path optimization formula to calculate the optimization variables {p1, p2,......, p n-1} as the second target path. Among them, is the cost of the cartridge balance force at position P i , f s is the safety distance cost of the first target path, λ1 and λ2 are the weights of the corresponding penalty terms. When d s f(P i ) < d0, it will result in a higher safety cost. The main role of optimization is to push the path points away from obstacles. When d s f(P i ) ≥ d0, the main role of optimization is to smooth the path.

[0085] Step S300: Parametrize the second target path using a uniform B-spline to obtain a preliminarily parameterized uniform B-spline trajectory, and perform non-linear optimization on the uniform B-spline trajectory to obtain the target trajectory.

[0086] Although the optimized second target path is obtained, the obtained path does not consider time and is not yet suitable for an autonomous mobile robot to track and navigate. Therefore, it is necessary to parameterize the set of path points into a trajectory. Any control point set of a B-spline (a special representation of a spline curve in the sub-discipline of numerical analysis in mathematics) can ensure the generation of a smooth trajectory, and the B-spline has good local characteristics. The change of a single control point only affects part of the trajectory. Therefore, the present invention uses a uniform B-spline to parameterize the path into a trajectory.

[0087] Specifically, obtain the current position of the autonomous mobile robot. Starting from the current position, obtain M + 1 consecutive path points in the direction of the target point as the shape value points of the uniform B-spline trajectory, query the maximum distance d s between two adjacent path points, and calculate the time interval

[0088] Replace two adjacent path points p i and p i+1 with the control points Q i and Q i+1 of the uniform B-spline, calculate the velocity and acceleration at the initial position and the velocity and acceleration at the end position between two adjacent path points, and use the velocity and acceleration at the initial position and the velocity and acceleration at the end position as boundary conditions to inversely calculate the control points {Q0, Q1,......, QN}, through formula The uniform B-spline trajectory P(t) is obtained, where Q i At time t i control points, t i The node vector U = {t0, t1, ..., t} M}, M = N + p b +1, N represents the number of control points; M represents the number of time intervals required for the entire B-spline trajectory; p b This represents the degree of the B-spline trajectory. These are the corresponding basis functions.

[0089] Nonlinear optimization is performed on the uniform B-spline trajectory P(t) according to the optimization formula. The goal is to find the minimum trajectory time Δt to obtain the target trajectory, where f m It is the cost of smoothing, f s It is the cost of maintaining a safe distance, f v The constraint is on speed, f a The constraint is on acceleration, f T It is a function that minimizes the overall time of the trajectory, where β1, β2, β3, and β4 are the parameter factors that balance smoothness, safety, dynamics, and time minimization, respectively. N-3} is the subset of control points after removing the first and last 6 control points.

[0090] It can be seen that the present invention designs a two-step optimization technique that combines path optimization and trajectory optimization, which can obtain a safe and dynamically feasible trajectory.

[0091] Step S400: Obtain the risk of the trajectory based on the distance relationship between all control points on the target trajectory and obstacles in the map, and dynamically adjust the trajectory time of the target trajectory based on the risk.

[0092] Although the above process yields a target trajectory that satisfies the motion dynamics of an autonomous mobile robot and is collision-free, it does not fully consider the safety of the target trajectory.

[0093] The present invention further considers the distance relationship between the target trajectory and the obstacle. When the target trajectory is close to the obstacle, the trajectory is considered to be more risky, and vice versa. Therefore, the trajectory time is dynamically adjusted according to the distance between the target trajectory and the obstacle.

[0094] in accordance with Figure 5The diagram showing the distance between control points and obstacles illustrates the risk assessment of the trajectory based on the distance relationship between control points and obstacles. Specifically, since the trajectory is parameterized over time using uniform B-splines, the safety of the trajectory can be guaranteed, and therefore the risk of the control points can be used to assess the risk of the trajectory.

[0095] Based on all control points Q on the target trajectory i Distance d from the obstacle in the map c (Q i Determine whether there exists a condition d. c (Q i ) <d thr d c (Q i If it exists, it indicates that the target trajectory has a risk, and the condition d will be met. c (Q i The average distance d is obtained by summing the results. mean , where d thr It is the radius of the bubble centered at any control point.

[0096] According to d thr and d mean Through the risk assessment function η nisk The risk of obtaining the trajectory is represented as Here, ω is an adjustable parameter.

[0097] Furthermore, the trajectory time is adjusted based on the risk of the trajectory and the current trajectory time. When there is a risk of collision at a control point, the overall trajectory time is adjusted to reduce the sailing speed in order to ensure safe navigation.

[0098] Based on minimizing the trajectory time Δt and the trajectory risk η nisk The time to minimize the trajectory is readjusted, expressed as Δt′=(η nisk +1)Δt, where Δt′ is the trajectory time of the dynamically adjusted target trajectory.

[0099] Based on the B-spline trajectory time adjustment method, the autonomous mobile robot adaptively adjusts its travel speed according to the surrounding obstacle environment. When the control point ahead senses a risk, the trajectory risk η is minimized because the trajectory time Δt is a constant. nisk The higher the value of Δt′, the larger the value of Δt′. By adjusting the time of the overall trajectory, the speed of the autonomous mobile robot can be reduced in advance, which helps to ensure the safe navigation of the autonomous mobile robot.

[0100] Step S500: Use a rolling replanning strategy to obtain a new second target path and a new target trajectory after adjusting the trajectory time.

[0101] Replanning strategies are crucial for the safety of autonomous navigation. Replanning strategies that cannot work in real time are prone to causing collisions in autonomous mobile robots and leading to discontinuity in navigation. Therefore, this invention designs a rolling replanning strategy that performs replanning in two aspects: path replanning and trajectory replanning.

[0102] Specifically, such as Figure 6 As shown in (a), based on the sensing range H of the autonomous mobile robot's sensor... r Determine the range for trajectory updates. When the global target point is outside the update range, connect the global target point to the current position of the autonomous mobile robot, selecting a distance of H from the current position. r The intermediate target point is used as the target point of the trajectory planning module to plan the trajectory. Steps S100 to S400 are repeated to obtain the new target trajectory after adjusting the trajectory time. The new target trajectory after adjusting the trajectory time is then sent to the trajectory server, which sends it to the trajectory tracking controller to control the autonomous mobile robot to move according to the new target trajectory after adjusting the trajectory time.

[0103] During the process of the autonomous mobile robot following the target trajectory after the new trajectory adjustment time, the perception of the visual sensor will be updated to the map at a certain frequency. Because the map has a good map query interface, the replanning frequency and the map update frequency are not synchronized. The path and trajectory are queried separately at a certain frequency.

[0104] The path replanning will be triggered when the query result is one of two cases: First, when a collision between the path or trajectory and the environment is detected, such as... Figure 6 As shown in (d), steps S100 to S200 are repeated for path replanning to obtain a new second target path; secondly, when the autonomous mobile robot's position has insufficient remaining path points, such as Figure 6 As shown in (c), repeat steps S100 to S200 to perform path replanning and obtain a new second target path.

[0105] Trajectory replanning mainly involves planning the optimized path in a rolling manner. This occurs when the autonomous mobile robot's execution time is close to the overall time or when the robot's position is nearing the end of the trajectory. Figure 6 As shown in (b), path points are reselected for parameterization and optimization. If there are insufficient points at the end of the path, path replanning will be triggered, and the remaining path points will be parameterized.

[0106] Furthermore, taking the vision-based autonomous mobile robot as an experimental platform for end-to-end autonomous navigation as an example, this invention verifies the proposed robot motion planning method based on rolling search and optimization. The method includes an end-to-end autonomous navigation simulation experiment (Experiment 1) and a physical experiment (Experiment 2).

[0107] Experiment 1: First, the performance of the replanning strategy and motion planning framework in autonomous navigation is analyzed. The autonomous navigation simulation experiment is conducted on five randomly generated simulated forest maps, each 30m x 30m x 5m in size. Next, the influence of the initial velocity mentioned in Step 1 on autonomous navigation is analyzed. The target point is set behind the autonomous mobile robot; that is, the initial stationary position of the autonomous mobile robot is set at (0.0, 0.0, 1.0), the target position is (-13.0, -13.0, 2.0), and the initial yaw angle is...

[0108] Figure 7 The simulation results of autonomous navigation in front of an autonomous mobile robot, based on the objective of this invention, are presented. Due to time adjustments, the overall trajectory speed is relatively slow. Because the trajectory is far from obstacles, it curves at corners, resulting in a significant decrease in speed. However, the trajectory always maintains a certain distance from obstacles, and the time adjustment considering a safe distance allows for better control of navigation, ensuring higher speeds in relatively open areas and reducing speed in areas with dense obstacles.

[0109] Figure 8 The simulation results of autonomous navigation behind an autonomous mobile robot, demonstrating the objective of this invention, are presented. Figure 8 In practice, for example, the purple arrow can be used to represent the synthesis rate V. init The trajectory initially follows the initial direction yaw. cur Then it gradually turns toward the target point. This illustrates that the change in the initial yaw angle in autonomous navigation can be reduced (the yaw angle direction of the planned trajectory is always along the tangent direction of the trajectory).

[0110] Experiment 2: Two sets of autonomous navigation experiments were conducted with targets in different directions: the target was located directly in front of the autonomous mobile robot's starting point and the target point was located to the side and rear of the autonomous mobile robot.

[0111] First, an autonomous navigation experiment was conducted with the target point directly in front of the autonomous mobile robot. The global target was set 45m in front of the starting point of the autonomous mobile robot, and the maximum speed and acceleration were set to 1m / s and 0.5m / s, respectively. Figure 9 The results of an autonomous navigation experiment show that the autonomous mobile robot can perceive the unknown environment in a timely manner and plan a trajectory that does not cause a collision, and the trajectory maintains a certain distance from obstacles. Figure 10 What is being shown is Figure 9 The position and speed, as well as the planned and navigating speeds, of the autonomous navigation experiment shown were all within the set range. At approximately 30 seconds of navigation time, the speed was reduced due to the increased risk of approaching an obstacle.

[0112] Secondly, an autonomous navigation experiment was conducted with the target point located to the side and rear of the autonomous mobile robot. A target point was randomly assigned to the side and rear, while other parameters remained unchanged. Figure 11 The experimental results show that the target point is to the left rear and to the right rear. The trajectory does not start in the direction of the target, but gradually approaches the target point along the yaw direction.

[0113] The experimental results show that the vision-based autonomous mobile robot navigation system can perceive the environment, successfully plan a safe, dynamic, feasible trajectory that avoids obstacles, and navigate to the target point completely autonomously.

[0114] Furthermore, such as Figure 12 As shown, based on the above-described robot motion planning method based on rolling search and optimization, the present invention also provides a robot motion planning system based on rolling search and optimization, wherein the robot motion planning system based on rolling search and optimization includes:

[0115] The path planning module 51 is used to acquire a map and determine a target point, calculate a new planned speed based on the current speed and yaw direction of the autonomous mobile robot, convert the current position and the planned speed into the initial node of the search process, and perform a first search process based on the planned speed and the initial node to obtain the path, end position and end speed to the target point.

[0116] Path optimization module 52 is used to perform a second search process based on the path, the end position and the end speed to obtain a first target path that minimizes the cost function, and to optimize the first target path to obtain a second target path;

[0117] The trajectory generation module 53 is used to parameterize the second target path using a uniform B-spline to obtain a preliminary parameterized uniform B-spline trajectory, and to perform nonlinear optimization on the uniform B-spline trajectory to obtain the target trajectory.

[0118] The trajectory time adjustment module 54 is used to obtain the risk of the trajectory based on the distance relationship between all control points on the target trajectory and obstacles in the map, and dynamically adjust the trajectory time of the target trajectory based on the risk.

[0119] The replanning module 55 is used to obtain a new second target path and a new target trajectory after adjusting the trajectory time using a rolling replanning strategy.

[0120] Furthermore, such as Figure 13 As shown, based on the above-mentioned robot motion planning method and system based on rolling search and optimization, the present invention also provides a terminal, which includes a processor 10, a memory 20 and a display 30. Figure 13 Only some of the terminal components are shown; however, it should be understood that it is not required to implement all of the components shown, and more or fewer components may be implemented instead.

[0121] In some embodiments, the memory 20 may be an internal storage unit of the terminal, such as a hard disk or memory. In other embodiments, the memory 20 may be an external storage device of the terminal, such as a plug-in hard disk, smart media card (SMC), secure digital card (SD), flash card, etc. Further, the memory 20 may include both internal and external storage devices. The memory 20 is used to store application software and various types of data installed on the terminal, such as the program code installed on the terminal. The memory 20 can also be used to temporarily store data that has been output or will be output. In one embodiment, the memory 20 stores a robot motion planning program 40 based on scrolling search and optimization, which can be executed by the processor 10 to implement the robot motion planning method based on scrolling search and optimization in this application.

[0122] In some embodiments, the processor 10 may be a central processing unit (CPU), a microprocessor, or other data processing chip, used to run program code stored in the memory 20 or process data, such as executing the robot motion planning method based on rolling search and optimization.

[0123] In some embodiments, the display 30 may be an LED display, a liquid crystal display, a touch-sensitive liquid crystal display, or an OLED (Organic Light-Emitting Diode) touchscreen. The display 30 is used to display information on the terminal and to display a visual user interface. The components 10-30 of the terminal communicate with each other via a system bus.

[0124] In one embodiment, the robot motion planning method based on scrolling search and optimization is implemented when the processor 10 executes the robot motion planning program 40 based on scrolling search and optimization in the memory 20.

[0125] The present invention also provides a computer-readable storage medium, wherein the computer-readable storage medium stores a robot motion planning program based on scrolling search and optimization, and the robot motion planning program based on scrolling search and optimization, when executed by a processor, implements the steps of the robot motion planning method based on scrolling search and optimization as described above.

[0126] In summary, this invention proposes a motion planning method for autonomous mobile robots based on rolling search and optimization. This method comprises three levels: path planning, path optimization, and trajectory optimization. At the path planning level, a path based on a motion primitive search algorithm and an improved ADS* algorithm is used to obtain a path that satisfies the initial dynamic feasibility of the autonomous mobile robot. At the path optimization level, soft constraint optimization is performed on the path using path points as optimization variables, improving the smoothness of the path while avoiding contact with obstacles. At the trajectory optimization level, a trajectory is obtained by rolling cubic B-spline interpolation of the path. By solving optimization problems considering control point smoothness, obstacle avoidance safety, and trajectory time, a safe and dynamically feasible trajectory is obtained. Then, the risk assessment results of the trajectory are incorporated into the trajectory time adjustment, enabling the autonomous mobile robot to adaptively adjust its travel speed according to environmental obstacles.

[0127] It should be noted that, in this document, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or terminal that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or terminal. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or terminal that includes that element.

[0128] Of course, those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided by this invention can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0129] It should be understood that the application of the present invention is not limited to the examples above. Those skilled in the art can make improvements or modifications based on the above description, and all such improvements and modifications should fall within the protection scope of the appended claims.

Claims

1. A robot motion planning method based on rolling search and optimization, characterized in that, The robot motion planning method based on rolling search and optimization comprises: acquiring a map and determining a target point, calculating a new planning speed according to a current speed and a yaw direction of an autonomous mobile robot, converting a current position and the planning speed into an initial node of a search process, performing a first search process according to the planning speed and the initial node to obtain a path, an end position and an end speed to the target point; performing a second search process according to the path, the end position and the end speed to obtain a first target path that minimizes a cost function, performing optimization processing on the first target path to obtain a second target path; performing parameterization processing on the second target path using a uniform B-spline to obtain a preliminary parameterized uniform B-spline trajectory, performing nonlinear optimization on the uniform B-spline trajectory to obtain a target trajectory; obtaining the risk of the trajectory according to the distance relationship between all control points on the target trajectory and obstacles in the map, and dynamically adjusting the trajectory time of the target trajectory according to the risk; using a rolling replanning strategy to obtain a new second target path and a new target trajectory after adjusting the trajectory time.

2. The robot motion planning method based on rolling search and optimization according to claim 1, characterized in that, The calculation of the new planning speed according to the current speed and the yaw direction of the autonomous mobile robot specifically comprises: Obtain the current speed of the autonomous mobile robot and yaw direction According to the formula Calculate the virtual speed in the yaw direction. ,in, It is the major axis radius of the autonomous mobile robot. It is the maximum scalar speed required for autonomous mobile robot navigation. It is a constant. yaw direction The angle with the horizontal direction; The virtual speed With the speed Horizontal component velocity Synthesize the data to obtain a new planned speed. ,like > Then make and keep The direction remains unchanged.

3. The method of claim 1, wherein, The conversion of the current position and the planning speed into the initial node of the search process, and the performance of the first search process according to the planning speed and the initial node to obtain the path, the end position and the end speed to the target point specifically comprises: the current position and the planned velocity is converted into an initial node of a search process , an open set O, a closed set I and a flag reach-horizon are initialized, and the initial node is added to the open set O, wherein the open set O is used to store expanded nodes, the closed set I is used to store all traversed nodes, and the flag reach-horizon is used to determine whether a search node exceeds a preset planning range H In the main loop, iterate from the open set O to output the minimum cost node and add to the closed set I, expand the minimum cost node by the Expand() method to get the expanded nodes , calculate the actual cost and heuristic cost of each expanded node by the EdgeCost() method and the Heuristic() method respectively, and backtrace the whole path nodes from the minimum cost node to the expanded nodes by the RetrievePath() method to get the path , the end position and the end velocity .

4. The method of claim 1, wherein, The performance of the second search process according to the path, the end position and the end speed to obtain the first target path that minimizes the cost function specifically comprises: According to the formula The end position The entire cost price to the current position is calculated, and a set of path points that makes the minimum value is obtained as the first target path; wherein, is the end position to the current position the overall cost penalty, is the path at any point, is the cost of path length, is the cost of position orientation, is the cost of safety distance, is the heuristic penalty of the diagonal distance from the current position to the target point, , , , and and are weights for , respectively. Cost of location The calculation process is as follows: for each position Assign a direction attribute to define the end position. arrive The direction is The direction of the terminal velocity direction and The angle between the directions Considering directional costs In the middle, Represented as ,in, terminal velocity direction and The angle between the directions, For position With end position The Euclidean distance; Cost of safety distance The calculation process is: according to position the shortest Euclidean distance to the obstacles in the map and position the safety distance to the obstacles in the map The cost of safety distance is calculated as .

5. The method of claim 1, wherein, The optimization processing on the first target path to obtain the second target path specifically comprises: Under the premise of ensuring path safety, the n+1 path points on the first target path are optimized by bending characteristics According to the path optimization formula The optimization variable As the second target path, wherein, The position The belt balancing force cost of The safety distance cost of the first target path, And The weight of the corresponding penalty term.

6. The method of claim 5, wherein, The parameterization processing on the second target path using the uniform B-spline to obtain the preliminary parameterized uniform B-spline trajectory, and the nonlinear optimization on the uniform B-spline trajectory to obtain the target trajectory specifically comprises: Obtaining the current position of the autonomous mobile robot, from the current position, obtaining M+1 continuous path points in the direction of the target point as the control points of the uniform B-spline trajectory, querying the maximum distance between two adjacent path points , and calculating the time interval , is the maximum scalar velocity that satisfies the navigation of the autonomous mobile robot; two adjacent path points and substitute the control points of the uniform B-spline and , calculate the velocity and acceleration of the initial position and the velocity and acceleration of the end position between two adjacent path points, and inversely calculate the control points by taking the velocity and acceleration of the initial position and the velocity and acceleration of the end position as boundary conditions , obtain the uniform B-spline trajectory by the formula wherein, is the control point at time , the node vector is composed of , =N+ +1, N represents the number of control points; M represents the number of time intervals required by the entire B-spline trajectory; is the order of the B-spline trajectory, is the corresponding base function;​ performing a non-linear optimization of the uniform B-spline trajectory according to an optimization formula to obtain a minimized trajectory time to obtain a target trajectory, wherein, is a smoothness cost penalty, is a safety distance cost penalty, is a constraint on velocity, is a constraint on acceleration, is a function to minimize the overall time of the trajectory, , , , are parameter factors to weigh the smoothness, safety, dynamics and time minimization, respectively, is a subset of control points excluding the first and last 6 control points.

7. The method of claim 6, wherein, The obtaining of the risk of the trajectory according to the distance relationship between all control points on the target trajectory and obstacles in the map, and the dynamic adjustment of the trajectory time of the target trajectory according to the risk specifically comprises: According to all control points on the target trajectory Distance to obstacles in the map , determine whether there is a condition of , if there is a risk of the target trajectory, the condition is met Summation to get the average distance , wherein is the radius of the bubble centered on any control point; According to and The risk of a trajectory is obtained by a risk assessment function and is denoted as where, is a tunable parameter; According to the minimized trajectory time and the riskiness of the trajectory readjusting the minimized trajectory time, denoted as ( ) wherein, is the trajectory time of the dynamically adjusted target trajectory.

8. A robot motion planning system based on rolling search and optimization, characterized by, The robot motion planning system based on rolling search and optimization comprises: a path planning module, configured to acquire a map and determine a target point, calculate a new planning speed according to a current speed and a yaw direction of an autonomous mobile robot, and convert a current position and the planning speed into an initial node of a search process, and perform a first search process according to the planning speed and the initial node to obtain a path, an end position and an end speed to the target point; a path optimization module, configured to perform a second search process according to the path, the end position and the end speed to obtain a first target path that minimizes a cost function, and perform optimization processing on the first target path to obtain a second target path. The trajectory generation module is configured to parameterize the second target path using a uniform B-spline to obtain a preliminary parameterized uniform B-spline trajectory, and to perform nonlinear optimization on the uniform B-spline trajectory to obtain a target trajectory. The trajectory time adjustment module is configured to obtain the risk of the target trajectory according to the distance relationship between all control points on the target trajectory and obstacles in the map, and to dynamically adjust the trajectory time of the target trajectory according to the risk. The re-planning module is configured to obtain a new second target path and a new target trajectory with adjusted trajectory time using a rolling re-planning strategy.

9. A terminal, characterized by comprising: The terminal comprises a memory, a processor, and a robot motion planning program based on rolling search and optimization stored on the memory and executable on the processor, and the robot motion planning program based on rolling search and optimization, when executed by the processor, implements the steps of the robot motion planning method based on rolling search and optimization according to any one of claims 1-7.

10. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a robot motion planning program based on rolling search and optimization, and the robot motion planning program based on rolling search and optimization, when executed by the processor, implements the steps of the robot motion planning method based on rolling search and optimization according to any one of claims 1-7.

Images